From 0475efae2d0d3cbd7571519fd7aa770aaa4c4312 Mon Sep 17 00:00:00 2001 From: skywhite1024 <129768272+skywhite1024@users.noreply.github.com> Date: Fri, 21 Aug 2026 15:40:19 +0800 Subject: [PATCH 01/29] feat(sim): add semantic skill IR and compiler --- agent_context/MAP.yaml | 36 + .../topics/atomic-actions/atomic-actions.md | 119 +- .../embodichain.lab.sim.skills.rst | 179 ++ docs/source/api_reference/public_api.rst | 81 + .../sim/atomic_actions/builtin_actions.md | 32 +- .../overview/sim/atomic_actions/index.md | 71 +- .../atomic_actions/robot_skill_profiles.md | 57 +- docs/source/overview/sim/index.rst | 21 +- docs/source/overview/sim/scene_registry.md | 101 +- docs/source/overview/sim/semantic_skills.md | 312 ++++ docs/source/tutorial/atomic_actions.rst | 14 +- docs/source/tutorial/index.rst | 14 +- docs/source/tutorial/semantic_skills.rst | 437 +++++ embodichain/lab/sim/atomic_actions/core.py | 23 + embodichain/lab/sim/atomic_actions/engine.py | 30 +- .../lab/sim/atomic_actions/execution.py | 22 +- embodichain/lab/sim/atomic_actions/goals.py | 13 + embodichain/lab/sim/atomic_actions/plans.py | 17 + .../atomic_actions/primitives/hand_over.py | 57 +- .../sim/atomic_actions/primitives/pick_up.py | 47 +- embodichain/lab/sim/objects/rigid_object.py | 17 +- embodichain/lab/sim/skills/__init__.py | 86 + embodichain/lab/sim/skills/calls.py | 806 +++++++++ embodichain/lab/sim/skills/compiler.py | 1333 +++++++++++++++ embodichain/lab/sim/skills/integration.py | 1063 ++++++++++++ embodichain/lab/sim/skills/profiles.py | 23 +- embodichain/lab/sim/skills/runtime.py | 1443 +++++++++++++++++ embodichain/lab/sim/skills/scene.py | 804 +++++++-- scripts/tutorials/semantic_skill/hand_over.py | 573 +++++++ scripts/tutorials/semantic_skill/place.py | 407 +++++ .../semantic_skill/tutorial_utils.py | 331 ++++ tests/sim/atomic_actions/test_actions.py | 81 + tests/sim/atomic_actions/test_core.py | 22 +- .../sim/atomic_actions/test_engine_per_env.py | 102 ++ tests/sim/skills/test_calls.py | 432 +++++ tests/sim/skills/test_compiler.py | 903 +++++++++++ tests/sim/skills/test_integration.py | 554 +++++++ tests/sim/skills/test_profiles.py | 38 + tests/sim/skills/test_runtime.py | 552 +++++++ tests/sim/skills/test_scene.py | 195 ++- .../skills/test_semantic_skill_tutorials.py | 480 ++++++ 41 files changed, 11731 insertions(+), 197 deletions(-) create mode 100644 docs/source/overview/sim/semantic_skills.md create mode 100644 docs/source/tutorial/semantic_skills.rst create mode 100644 embodichain/lab/sim/skills/calls.py create mode 100644 embodichain/lab/sim/skills/compiler.py create mode 100644 embodichain/lab/sim/skills/integration.py create mode 100644 embodichain/lab/sim/skills/runtime.py create mode 100644 scripts/tutorials/semantic_skill/hand_over.py create mode 100644 scripts/tutorials/semantic_skill/place.py create mode 100644 scripts/tutorials/semantic_skill/tutorial_utils.py create mode 100644 tests/sim/skills/test_calls.py create mode 100644 tests/sim/skills/test_compiler.py create mode 100644 tests/sim/skills/test_integration.py create mode 100644 tests/sim/skills/test_runtime.py create mode 100644 tests/sim/skills/test_semantic_skill_tutorials.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 562fc849c..8c0d3bc57 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -541,6 +541,10 @@ topics: - resource graph - resource DAG - semantic skill catalog + - semantic skill compiler + - semantic call + - semantic workflow + - semantic integration manifest - capability binding - AtomicAction - ActionInvocation @@ -627,6 +631,32 @@ topics: - ResolvedSkillBinding - SkillPolicyPreset - SkillPolicyPreset.required_planner + - SemanticCallSpec + - SemanticPose + - Pick + - Place + - HandOver + - RegisteredSemanticCall + - SemanticCallCatalog + - SceneManifest + - SemanticIntegrationManifest + - BoundSemanticIntegration + - SemanticSkillCompiler + - SemanticSkillRuntime + - SemanticTask + - SemanticExecution + - SemanticTaskResult + - SemanticWorkflow + - SemanticLowering + - GroundedSemanticCall + - RelationTargetGrounder + - HandOverPoseProvider + - SemanticDiagnostic + - SemanticValidationError + - affordance_capabilities + - default_affordances + - grounding_providers + - skill_catalog_revision - binding_contract - engine.skills - skill_profile @@ -686,6 +716,7 @@ topics: - engine.plan - engine.compile - engine.start + - eligible_mask paths: - topics/atomic-actions/atomic-actions.md source_of_truth: @@ -713,7 +744,12 @@ topics: - embodichain/lab/sim/atomic_actions/__init__.py - embodichain/lab/sim/skills/scene.py - embodichain/lab/sim/skills/profiles.py + - embodichain/lab/sim/skills/calls.py + - embodichain/lab/sim/skills/integration.py + - embodichain/lab/sim/skills/compiler.py + - embodichain/lab/sim/skills/runtime.py - embodichain/lab/sim/skills/__init__.py + - scripts/tutorials/semantic_skill/ related_topics: - simulation-system - motion-planning diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 1e77fb97e..ea7a3ec09 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -70,7 +70,7 @@ Choose the public engine entry point by lifecycle, not by skill type: |---|---|---| | `engine.plan(invocation, context)` | Inspect or plan one registered action | Returns one `ActionPlan`; does not project a context for another action | | `engine.compile(invocations, context)` | Plan an ordered sequence against a fixed scene | Returns a concatenated `CompiledTrajectory`; propagates hypothetical qpos and expected effects through `projected_context` | -| `engine.start(invocations, context)` | Execute incrementally from observations | Returns an `ExecutionSession`; `tick(latest_context)` emits commands and performs bounded recovery | +| `engine.start(invocations, context, *, eligible_mask=None)` | Execute incrementally from observations | Returns an `ExecutionSession`; the optional initial cohort is sticky, and `tick(latest_context)` emits commands and performs bounded recovery | None steps simulation directly. `compile()` never observes physical execution; split compilation at observation boundaries when later goals depend on measured @@ -279,6 +279,82 @@ installed as aliases, and unlisted simulation entities are never scanned. Collision participation defaults to `NONE`, and every static/dynamic collision registration requires a geometry provider. +`SceneEntityMetadata` is the single provider-free scene declaration model. +`SceneEntityManifest` specializes that value without redeclaring its fields, +and both `SceneManifest` and `SceneRegistry` use the same canonical ID, alias, +parent, native-member, and affordance index. A `SceneManifest` additionally +snapshots `collision_world_mode`; `SemanticIntegrationManifest.bind()` rejects +live metadata or collision-mode drift before installing a robot profile. + +## Semantic workflow compilation + +Semantic calls (`Pick`, `Place`, `HandOver`, and catalog-registered values) are +robot-independent declarations. `SemanticCallDescriptor` has one canonical +atomic `target_descriptor`; its `skill_id` and `binding_contract` are derived +views, not separately stored values. Curated call targets cannot be remapped. +Registered calls require an explicit agent-visible target plus an installed +`RegisteredSemanticLowerer` with a matching call ID and schema version. + +`SemanticSkillCompiler.analyze()` performs provider-free linking, resource and +affordance validation, held-object flow analysis, and first-release look-ahead. +A pick therefore owns zero or one downstream object target rather than an +arbitrary target tuple. Relation targets retain affordance payload type and +revision metadata and stay late-bound through an explicitly installed +`RelationTargetGrounder`; handover poses stay behind a named +`HandOverPoseProvider` selected by the robot profile. + +`SemanticSkillCompiler.ground()` lowers exactly one analyzed call from the +latest `PlanningContext` and returns a `GroundedSemanticCall`. Its +`eligible_mask` is an owned snapshot and must be handed to execution together +with the invocation: + +```python +grounded = compiler.ground(workflow, call_index, context, eligible_mask=cohort) +session = engine.start( + (grounded.invocation,), + context, + eligible_mask=grounded.eligible_mask, +) +``` + +The compiler identity prevents a workflow from crossing lowerer/grounder +registries. Engine/profile staleness is checked through the bound integration; +the workflow does not duplicate engine-owner or catalog-revision fields. + +## Semantic runtime facade + +`embodichain.lab.sim.skills.SemanticSkillRuntime` is the application-facing +orchestration layer. `bind()` connects an explicit manifest, registry, engine, +observation provider, command sink, and clock; `from_simulation()` assembles the +standard joint-position simulation ports while still requiring an explicit +registry, robot profile, and motion generator. Its optional `control_dt` +selects a command cadence independently of the simulation physics period. A +runtime-level `runner_cfg` overrides all calls; when omitted, each grounded +call uses the `ExecutionRunnerCfg` owned by its selected `SkillPolicyPreset`. +The runtime exposes only calls supported by both the semantic catalog and the +currently bound robot profile, and allows exactly one active `SemanticTask` +because no resource scheduler or lease manager exists. + +`SemanticSkillRuntime.run()` is the blocking one-segment convenience path and +requires a `SemanticEffectVerifier`. Use `start()` when effect verification is +asynchronous. A `SemanticTask` retains externally verified `TaskState`, stable +environment IDs, and the sticky eligible cohort across several independently +analyzed segments. `run_segment()` supports dynamic application decisions at +safe semantic-call boundaries; submit all known calls in one segment when Pick +look-ahead should account for a downstream Place or HandOver target. + +`SemanticExecution` always JIT-grounds and starts one invocation at a time. It +uses a fresh observation before each call, delegates local recovery and safe +stop to `ExecutionRunner`, commits only verified effects, then carries the +session's task state and eligibility into the next grounding boundary. Manual +execution reports `WAITING_FOR_EFFECT` and resumes through `step(effect_success=...)`; +compatible in-place call changes use `revise_current()`, which reanalyzes the +workflow and still inherits the runner's same-skill, same-invocation, and +same-runtime-address restrictions. Runtime failures remain terminal; automatic +task-level skill replacement or symbolic-state reconciliation is not provided. +A failed or cancelled segment closes its task and releases runtime ownership; +successful dynamic segments retain ownership until `finish()` or cancellation. + `registry.make_planning_scene_provider(motion_generator, batch_size=...)` returns a fresh `RegistrySceneProvider` with independent baselines and revision counters after eager registry/provider/planner validation. Snapshots expose @@ -341,7 +417,7 @@ Scene dependencies must match the poses each primitive actually consumes: |---|---| | `MoveEndEffector` | A `SceneEntityPose` in `xpos`. | | `MoveJoints` | None; its target is qpos or a named control-profile command. | -| `PickUp` | Always its semantic `entity_id`, when present, because the object pose is grounded once and reused; plus any goal-owned `SceneEntityPose`, such as `grasp_xpos`. | +| `PickUp` | Always its semantic `entity_id`, when present, because the object pose is grounded once and reused; plus any goal-owned `SceneEntityPose`, such as `grasp_xpos`. These dependencies are monitored only through the `approach` segment. | | `CoordinatedPickment` | Goal-owned target/initial `SceneEntityPose` values; the semantic `entity_id` only when `object_initial_pose` is omitted and semantic grounding supplies that pose. | | `Place` | A `SceneEntityPose` in ordinary `xpos`; for `AssembleGoal`, `base_pose` when supplied. Omitting `base_pose` uses the deprecated live `AssembleAffordance.base_object_entity` fallback with no dependency. | | `MoveHeldObject` | A `SceneEntityPose` in `object_target_pose`; current object orientation is derived from observed EEF pose plus verified `object_to_eef`, not a scene-object read. | @@ -349,12 +425,18 @@ Scene dependencies must match the poses each primitive actually consumes: | `Slide` | `SlideGoal.target_pose` when it is a `SceneEntityPose`; the local grasp mesh does not own the link. | | `Twist` | `TwistGoal.target_pose` when it is a `SceneEntityPose`; affordance data is entity-free. | | `CoordinatedPlacement` | `SceneEntityPose` values in the placing or support object target pose. | -| `HandOver` | No semantic-object scene dependency. It verifies stable attachment identity and derives current pose from held state; its middle/final option poses are tensors, and the reused `GraspGoal.grasp_xpos` field is ignored. | +| `HandOver` | `SceneEntityPose` values in `HandOverOptions.middle_object_pose` or `final_object_pose`. Its current held-object pose is derived from verified attachment state and observed EEF pose; the reused `GraspGoal.grasp_xpos` field is ignored. | `collect_scene_dependencies()` deliberately stops at `ObjectSemantics`. Therefore, a custom action that consumes a snapshot pose through semantic data must override `_scene_dependencies()`, union `super()` dependencies, and add the consumed semantic ID. Do not declare an ID merely because semantics are present. +`ActionPlan.scene_dependency_end_segment` can bound dynamic-goal monitoring to +the reversible part of a staged action. `PickUp` stops monitoring after its +approach is dispatched: target motion before contact still replans, while +contact-, grasp-, and lift-induced object motion is not misclassified as an +external target update. Collision-world and joint-tracking checks remain +independent of this boundary. ## Static compilation @@ -388,7 +470,11 @@ snapshot every time the action plans. Its entity ID is recorded in `ActionPlan.scene_dependencies`. ```python -session = engine.start(invocations, initial_context) +session = engine.start( + invocations, + initial_context, + eligible_mask=initial_eligible_mask, +) runner = ExecutionRunner( session, observation_provider, @@ -421,6 +507,12 @@ active targets so the caller can still hold them. The session monitors: - action-attempt timeout; - planner and semantic-effect failure. +The optional initial `eligible_mask` is copied onto the engine device and must +be a boolean tensor with one value per environment. Initially ineligible rows +never re-enter the cohort. They are excluded from every command, replan, effect +verification, and later invocation barrier. An all-false cohort creates a +failed session without invoking any action planner. + It replans from the latest observation within per-environment budgets. The budgets and eligibility masks are row-local, while the action waypoint cursor is batch-synchronized: one allowed replan regenerates the active cohort and @@ -539,6 +631,25 @@ Runnable closed-loop examples live under `scripts/tutorials/atomic_action/`: `dynamic_obstacle_recovery.py`. Each injects one disturbance, reports the structured invalidation/replan events, and requires terminal completion. +Semantic integration tutorials live under `scripts/tutorials/semantic_skill/`. +Both examples separate `create_*_application()` (scene/profile/runtime and +default verifier wiring), `create_*_task()` (robot-independent semantic calls), +and the application-facing `app.run(task, ...)` entry. `app` remains a +`SemanticSkillRuntime`; there is no tutorial-specific facade. `place.py` +executes `Pick -> Place`, verifying the observed lift, planned object-to-EEF +relation, release pose, and open hand. `hand_over.py` demonstrates disjoint +dual-arm resources plus an explicit `RegisteredSemanticLowerer`, then verifies +source release and receiver ownership at the final target. Both report +structured recovery events and use `--diagnose_plan` only for a separate +offline compile that projects hypothetical effects without executing them. +Release and ownership-transfer presets disable whole-action effect retries +because those physical changes are not safely repeatable without state +reconciliation. + +Human-facing architecture and lifecycle documentation lives in +`docs/source/overview/sim/semantic_skills.md`; the runnable walkthrough is +indexed at `docs/source/tutorial/semantic_skills.rst`. + The latest validated session context is retained for safe hold if the first live observation fails. Environment IDs must remain stable and ordered for the entire session; robot and scene timestamps and scene versions must be monotonic. diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst index 1b3022fe8..4a33cb46b 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -3,6 +3,54 @@ embodichain.lab.sim.skills .. automodule:: embodichain.lab.sim.skills + .. rubric:: Semantic calls and catalog + + .. autosummary:: + + SemanticPose + Pick + Place + HandOver + RegisteredSemanticCall + SemanticCallDescriptor + SemanticCallCatalog + builtin_semantic_call_catalog + + .. rubric:: Semantic compilation and grounding + + .. autosummary:: + + SemanticWorkflow + SemanticLowering + GroundedSemanticCall + SemanticObjectTarget + SemanticRelationTarget + RegisteredSemanticLowerer + RelationTargetGrounder + HandOverPoseTargets + HandOverPoseProvider + SemanticSkillCompiler + + .. rubric:: Semantic integration and execution + + .. autosummary:: + + SceneEntityManifest + SceneManifest + SemanticIntegrationManifest + SemanticDiagnostic + SemanticValidationError + SemanticEffectVerifier + SemanticSkillRuntime + SemanticTask + SemanticExecution + SemanticExecutionStatus + SemanticTaskStatus + SemanticExecutionStep + SemanticCallRecord + SemanticSegmentResult + SemanticTaskResult + .. rubric:: Scene integration contracts .. autosummary:: @@ -10,6 +58,7 @@ embodichain.lab.sim.skills SceneRegistry RegistrySceneProvider SceneEntityRegistration + SceneEntityMetadata SceneEntityRef SceneObjectRef SceneArticulationRef @@ -20,6 +69,11 @@ embodichain.lab.sim.skills SceneDynamics SceneCollisionRole SceneCollisionWorldMode + GRASP_AFFORDANCE_CAPABILITY + PLACE_ON_AFFORDANCE_CAPABILITY + PLACE_IN_AFFORDANCE_CAPABILITY + UnsupportedSceneAffordanceError + AmbiguousSceneAffordanceError .. rubric:: Robot skill profiles @@ -45,6 +99,115 @@ embodichain.lab.sim.skills .. currentmodule:: embodichain.lab.sim.skills +Semantic calls and catalog +-------------------------- + +.. autoclass:: SemanticPose + :members: + +.. autoclass:: Pick + :members: + +.. autoclass:: Place + :members: + +.. autoclass:: HandOver + :members: + +.. autoclass:: RegisteredSemanticCall + :members: + +.. autoclass:: SemanticCallDescriptor + :members: + +.. autoclass:: SemanticCallCatalog + :members: + +.. autofunction:: builtin_semantic_call_catalog + +Semantic compilation and grounding +----------------------------------- + +.. autoclass:: SemanticWorkflow + :members: + +.. autoclass:: SemanticLowering + :members: + +.. autoclass:: GroundedSemanticCall + :members: + +.. autoclass:: SemanticObjectTarget + :members: + +.. autoclass:: SemanticRelationTarget + :members: + +.. autoclass:: RegisteredSemanticLowerer + :members: + +.. autoclass:: RelationTargetGrounder + :members: + +.. autoclass:: HandOverPoseTargets + :members: + +.. autoclass:: HandOverPoseProvider + :members: + +.. autoclass:: SemanticSkillCompiler + :members: + +Semantic integration +-------------------- + +.. autoclass:: SceneEntityManifest + :members: + +.. autoclass:: SceneManifest + :members: + +.. autoclass:: SemanticIntegrationManifest + :members: + +.. autoclass:: SemanticDiagnostic + :members: + +.. autoclass:: SemanticValidationError + :members: + +Semantic runtime +---------------- + +.. autodata:: SemanticEffectVerifier + +.. autoclass:: SemanticSkillRuntime + :members: + +.. autoclass:: SemanticTask + :members: + +.. autoclass:: SemanticExecution + :members: + +.. autoclass:: SemanticExecutionStatus + :members: + +.. autoclass:: SemanticTaskStatus + :members: + +.. autoclass:: SemanticExecutionStep + :members: + +.. autoclass:: SemanticCallRecord + :members: + +.. autoclass:: SemanticSegmentResult + :members: + +.. autoclass:: SemanticTaskResult + :members: + Robot resources and profiles ---------------------------- @@ -114,6 +277,9 @@ Registration contracts .. autoclass:: SceneEntityRegistration :members: +.. autoclass:: SceneEntityMetadata + :members: + .. autoclass:: SceneEntityStateProvider :members: @@ -146,3 +312,16 @@ References and enums .. autoclass:: SceneCollisionWorldMode :members: + +Affordance capabilities and errors +---------------------------------- + +.. autodata:: GRASP_AFFORDANCE_CAPABILITY + +.. autodata:: PLACE_ON_AFFORDANCE_CAPABILITY + +.. autodata:: PLACE_IN_AFFORDANCE_CAPABILITY + +.. autoclass:: UnsupportedSceneAffordanceError + +.. autoclass:: AmbiguousSceneAffordanceError diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 11f2a70e0..4491b3454 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -856,6 +856,63 @@ embodichain.lab.sim.sim_manager CONVEX_DECOMP_DIR REACHABLE_XPOS_DIR +embodichain.lab.sim.skills.calls +-------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.calls + +.. autosummary:: + + DeclarativeValue + HandOver + Pick + Place + PlaceRelationTarget + RegisteredSemanticCall + SemanticCallCatalog + SemanticCallDescriptor + SemanticCallSpec + SemanticPose + builtin_semantic_call_catalog + +embodichain.lab.sim.skills.compiler +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.compiler + +.. autosummary:: + + AnalyzedSemanticCall + GroundedSemanticCall + HandOverPoseProvider + HandOverPoseTargets + RelationTargetGrounder + RegisteredSemanticLowerer + SemanticEffectDependency + SemanticEffectKind + SemanticHandOverTarget + SemanticLowering + SemanticObjectTarget + SemanticRelationTarget + SemanticSkillCompiler + SemanticWorkflow + +embodichain.lab.sim.skills.integration +-------------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.integration + +.. autosummary:: + + BoundSemanticCall + LinkedSemanticCall + PathPart + SceneEntityManifest + SceneManifest + SemanticDiagnostic + SemanticIntegrationManifest + SemanticValidationError + embodichain.lab.sim.skills.profiles ----------------------------------- @@ -881,6 +938,24 @@ embodichain.lab.sim.skills.profiles SkillPolicyPreset UnsupportedSkillError +embodichain.lab.sim.skills.runtime +---------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.runtime + +.. autosummary:: + + SemanticCallRecord + SemanticEffectVerifier + SemanticExecution + SemanticExecutionStatus + SemanticExecutionStep + SemanticSegmentResult + SemanticSkillRuntime + SemanticTask + SemanticTaskResult + SemanticTaskStatus + embodichain.lab.sim.skills.scene -------------------------------- @@ -895,12 +970,18 @@ embodichain.lab.sim.skills.scene SceneCollisionWorldMode SceneDynamics SceneEntityRef + SceneEntityMetadata SceneEntityRegistration SceneEntityStateProvider SceneGeometryProvider SceneLinkRef SceneObjectRef SceneRegistry + AmbiguousSceneAffordanceError + GRASP_AFFORDANCE_CAPABILITY + PLACE_IN_AFFORDANCE_CAPABILITY + PLACE_ON_AFFORDANCE_CAPABILITY + UnsupportedSceneAffordanceError embodichain.lab.sim.solvers.neural_ik_solver -------------------------------------------- diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 2a26f15c7..e766bd68a 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -225,12 +225,13 @@ entity as a recovery dependency. | `Place.xpos` | yes | yes | | `CoordinatedPickGoal.object_target_pose` / `object_initial_pose` | yes | yes | | `CoordinatedPlacementGoal` placing/support poses | yes | yes | -| `PickUp.grasp_xpos` | yes | yes | -| `PickUp` `ObjectSemantics.entity_id` grounding | implicit snapshot reference | yes; always consumed for the object pose | +| `PickUp.grasp_xpos` | yes | yes; monitored through `approach` only | +| `PickUp` `ObjectSemantics.entity_id` grounding | implicit snapshot reference | yes; monitored through `approach` only | | Coordinated pickup implicit initial pose via `ObjectSemantics.entity_id` | implicit snapshot reference | yes; only when `object_initial_pose` is omitted | | `AssembleGoal.base_pose` | yes | yes | | Deprecated `ObjectSemantics.entity` / `AssembleAffordance.base_object_entity` fallback | no | no | | `HandOver` current held-object pose | no scene lookup | no; derived from observed EEF pose and verified attachment state | +| `HandOverOptions.middle_object_pose` / `final_object_pose` | yes | yes | ### Object identity and grounding @@ -376,7 +377,11 @@ bound motion target. `grasp_xpos` may be `(4, 4)`, `(B, 4, 4)`, or a `SceneEntityPose`. A scene reference resolves the latest grasp pose and registers its entity as a recovery dependency, so material target motion invalidates and replans an executing -`PickUp`. When omitted, the action samples valid affordance grasps, evaluates +`PickUp` while its `approach` segment is active. Once approach has been +dispatched, dependency monitoring stops: contact-, close-, and lift-induced +object motion must not be misclassified as an external target update. Tracking +and collision-world checks remain active independently. When `grasp_xpos` is +omitted, the action samples valid affordance grasps, evaluates reachability, and stores the selected `object_to_eef` transform in the expected held-object state. Later object-centric skills reuse that transform. @@ -719,15 +724,18 @@ source/destination motion and grasp endpoints come exclusively from the corresponding generic `ActionBinding` slots. The destination attachment reuses the source relation's canonical `ObjectSemantics` instance. -The middle and final poses are currently option tensors rather than -`SceneEntityPose` goal fields. Consequently, handover supports tracking-error -and timeout recovery, but does not automatically invalidate a moving handover -point. An application can submit a newer invocation revision with updated -`HandOverOptions`. The action verifies that the goal and source attachment have -the same stable object identity, then derives the current object orientation -from the observed source EEF pose and verified `object_to_eef` relation. -The reused `GraspGoal.grasp_xpos` field is not consumed by `HandOver` and does -not create a scene dependency. +`middle_object_pose` and `final_object_pose` accept `(4, 4)`, `(B, 4, 4)`, or +`SceneEntityPose`. Scene-relative option values register their referenced +entities as recovery dependencies, so material movement of a handover or final +target can invalidate and replan the action. Tensor options remain fixed for the +invocation revision; an application can submit a newer compatible revision when +it intentionally changes them. + +The action verifies that the goal and source attachment have the same stable +object identity, then derives the current object orientation from the observed +source EEF pose and verified `object_to_eef` relation. The reused +`GraspGoal.grasp_xpos` field is not consumed by `HandOver` and does not create a +scene dependency; only the middle and final option poses do. As with the other coordinated primitive, cuRobo does not currently support its dual-arm `strategy="motion_gen"` path. diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index ebe26490e..e90d85289 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -36,8 +36,8 @@ payloads, and transports without adding fixed resource categories to the core. +---------------+----------------+ +---------------+----------------+ | | v | - semantic adapter: schema validation, | - SceneRegistry grounding, endpoint binding | + SemanticSkillCompiler / SemanticSkillRuntime: | + schema validation, SceneRegistry grounding, binding | | | +------------------+------------------+ | @@ -102,10 +102,13 @@ state. ### Caller entry points -The engine supports two first-class caller paths. An Action Agent emits a -semantic skill call that an adapter validates, grounds, and converts into an -`ActionInvocation`. A user can instead author the typed invocation directly in -Python or load it from an application-owned configuration layer: +The engine supports two first-class caller paths. An Action Agent or +configuration-driven application can emit a semantic call for +{class}`~embodichain.lab.sim.skills.SemanticSkillCompiler` and +{class}`~embodichain.lab.sim.skills.SemanticSkillRuntime` to validate, ground, +and convert into an `ActionInvocation`. A user can instead author the typed +invocation directly in Python or load it from an application-owned +configuration layer: ```python binding = engine.bind_control_parts( @@ -758,6 +761,12 @@ implicit-initial-pose path of coordinated pickup declare that dependency automatically. The deprecated live-entity fallback does not trigger scene-motion replanning. +An `ActionPlan.scene_dependency_end_segment` may bound monitoring to the +reversible portion of a staged action. `PickUp` stops monitoring its object and +grasp dependencies after `approach` is dispatched so contact-, close-, and +lift-induced movement does not trigger a false replan. Joint-tracking and +collision-world checks remain active. + Dynamic collision invalidation is provider-driven. Only registered, pose-updatable collision entities are supported; adding/removing obstacles or changing their geometry requires rebuilding the planner world. @@ -795,30 +804,39 @@ misreported as a successful grasp, release, or handover. The typed ## Action Agent integration An MLLM should not construct `ActionInvocation` by copying arbitrary JSON into -runtime objects. An adapter should expose stable `SkillDescriptor` metadata and -agent-facing goal schemas, validate the semantic call, resolve object references -and embodiment capabilities, then produce the typed invocation: +runtime objects. The `embodichain.lab.sim.skills` package provides the semantic +boundary: stable call descriptors, immutable call values, scene/profile +manifests, a compiler, and a runtime facade. The agent selects among +`SemanticSkillRuntime.available_calls` and supplies declarative object-centric +values; the compiler performs validation and grounding before the atomic engine +sees the request: ```text -MLLM SkillCallSpec - -> schema validation - -> object / scene grounding - -> participant and endpoint capability binding - -> safe skill-option selection - -> semantic command selection (never raw qpos) - -> ActionInvocation - -> AtomicActionEngine - -> ActionPlan / execution events +MLLM / application SemanticCallSpec + -> SemanticCallCatalog discovery + -> SemanticIntegrationManifest validation + -> SemanticSkillCompiler.analyze() + object / affordance / resource / effect-flow validation + -> SemanticSkillCompiler.ground(latest_context) + participant binding + safe options + ActionInvocation + -> SemanticSkillRuntime / AtomicActionEngine + -> verified task state + structured execution events ``` -The adapter may expose a curated subset of `OptionsType`, but engine-only -profiles, `JointPositionCommand` payloads, planner instances, and concrete joint -groups should remain outside the MLLM schema. If the agent needs an -object-specific grasp mode, it should choose a semantic command or capability; -the grounding layer turns that choice into `ActionControlOverrides`. Invocation -IDs and monotonic revisions correlate updated agent decisions with planner -diagnostics and execution events, providing structured feedback for the next -decision without mutating an in-flight request implicitly. +Engine-only profiles, `JointPositionCommand` payloads, planner instances, live +objects, and concrete joint groups remain outside semantic call payloads. A +registered extension accepts only declarative data and requires an explicitly +installed version-matched lowerer. Invocation IDs and monotonic revisions +correlate compatible in-flight updates with planner diagnostics and execution +events without mutating a request implicitly. + +The semantic runtime is also useful without an agent. `run()` executes one +known workflow, while `open_task()` and `run_segment()` retain verified state +across safe application decision boundaries. Call-local recovery remains owned +by `ExecutionRunner`; automatic skill replacement or symbolic-state +reconciliation after a terminal failure is intentionally not provided. See +{doc}`../semantic_skills` for the complete compiler/runtime and dynamic-task +contract. ## Extending the module @@ -847,6 +865,7 @@ See {doc}`builtin_actions` for the shipped skill catalog and visual demos, and ## Further reading - {doc}`../scene_registry` — canonical scene identity, snapshots, and collision integration +- {doc}`../semantic_skills` — semantic calls, compilation, runtime execution, and dynamic task boundaries - {doc}`../planners/motion_generator` — the motion generator owned by the engine - {doc}`../sim_robot` — robot control parts and kinematic configuration - {doc}`/tutorial/atomic_actions` — static, closed-loop, and recovery examples diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md index 18444e768..854d7bc6c 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -77,6 +77,7 @@ from embodichain.lab.sim.atomic_actions import ( FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, ControlPartCommandProfile, + ExecutionRunnerCfg, MotionPolicy, ) from embodichain.lab.sim.skills import ( @@ -138,6 +139,7 @@ profile = RobotSkillProfile( "default": SkillPolicyPreset( preset_id="default", motion_policy=MotionPolicy(strategy="ik_interp"), + runner_cfg=ExecutionRunnerCfg(command_timeout=2.0), ), }, default_preset="default", @@ -149,6 +151,51 @@ planner backend, typically because it carries backend-specific typed planning options. Profile binding checks that requirement against the engine's configured backend and fails early on a mismatch. Leave it as `None` for portable presets. +A {class}`SkillPolicyPreset` owns three independently snapshotted policy layers: +`motion_policy`, `recovery_policy`, and `runner_cfg`. Semantic integration +selects a preset in this order: an integration-wide `runtime_preset`, the +profile's `skill_presets[atomic_skill_id]`, then `default_preset`. At execution +time, an explicit `SemanticSkillRuntime.runner_cfg` overrides the selected +preset's runner configuration for every call; otherwise each call keeps its +selected preset's transport timeouts, minimum cycle time, and completion-hold +behavior. + +## Select semantic grounding providers + +Some semantic calls require embodiment knowledge that does not belong in the +agent-facing call or the atomic action. The built-in semantic HandOver is the +canonical example: the robot profile selects a named provider that supplies a +safe middle and default final object target for that embodiment. An explicit +semantic `HandOver.final_target` overrides the provider's final target. + +```python +profile = RobotSkillProfile( + profile_id="dual_arm_robot", + resources=dual_arm_resources, + command_profiles=hand_command_profiles, + defaults=dual_arm_skill_defaults, + presets={"default": default_preset}, + default_preset="default", + grounding_providers={"hand_over": "center_workspace_handover"}, +) + +runtime = SemanticSkillRuntime.from_simulation( + simulation=sim, + robot=robot, + motion_generator=motion_generator, + scene_registry=scene_registry, + robot_profile=profile, + handover_pose_providers=(CenterWorkspaceHandOverProvider(),), +) +``` + +`grounding_providers` maps a **semantic call ID** to a provider ID. The selected +ID must match one explicitly installed {class}`HandOverPoseProvider`; missing or +unknown providers fail during workflow analysis, before observation, planning, +or controller work. The provider is executable integration code and therefore +is passed to the runtime/compiler rather than stored inside the declarative +profile. + Every `ControlPartEndpoint.control_part` must be a key in `robot.control_parts`. A composite endpoint may reuse a member's control part, but all joints controlled directly by the composite must already be covered by @@ -212,6 +259,13 @@ and `bound.skills` is the profile-supported catalog. Registering or replacing an action invalidates the bound profile; bind it again before discovery or resolution. +{attr}`BoundRobotSkillProfile.source_profile` identifies the exact immutable +profile used for the binding. The bound view also snapshots the engine's +monotonic semantic skill-catalog revision. A later agent-visible action +registration or replacement makes discovery, preset selection, and resolution +fail until the profile and semantic integration are rebound; an equal public +descriptor does not make a different implementation owner safe to reuse. + ## Extend the graph beyond manipulation Resource and capability identifiers are open strings. A joint-driven mobile @@ -288,5 +342,6 @@ may retain a full-robot trajectory for feedback and offline compilation, but runtime dispatch is scoped to the endpoints in each command frame. ``` -See {doc}`index` for the direct atomic-action core and +See {doc}`index` for the direct atomic-action core, +{doc}`../semantic_skills` for compiler/runtime integration, and {doc}`../scene_registry` for canonical scene identity and snapshots. diff --git a/docs/source/overview/sim/index.rst b/docs/source/overview/sim/index.rst index 20d25c7a5..07aaa9eb9 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -9,7 +9,8 @@ designed around a small set of composable components: a :class:`SimulationManager` owns the simulation lifecycle, asset classes represent objects in the scene, sensors produce batched observations, solvers convert between joint space and task space, planners generate feasible -trajectories, and atomic actions package common manipulation skills. +trajectories, atomic actions package common manipulation primitives, and +semantic skills bind robot-independent task intent to those primitives. Like EmbodiChain's environment and learning modules, the simulation framework is configuration driven. Scene elements are declared through config classes, spawned @@ -47,8 +48,10 @@ The simulation stack can be read from the bottom up: | `-- canonical semantic identity, snapshots, and collision integration |-- robot skill profiles | `-- generic resource graphs, capabilities, commands, and policy presets - `-- atomic actions - `-- reusable manipulation primitives built from assets, solvers, and planners + |-- atomic actions + | `-- reusable manipulation primitives built from assets, solvers, and planners + `-- semantic skills + `-- manifests, call catalogs, workflow compilation, and task execution The :class:`SimulationManager` is the entry point for most workflows. It creates the physics world, configures rendering and time stepping, lays out multiple @@ -102,6 +105,9 @@ Submodule Relationships - Package complete manipulation primitives such as move, pick, and place. - Compose semantic targets, solvers, planners, and robot control into reusable higher-level skills. + * - Semantic skills + - Declare object-centric Pick, Place, HandOver, and registered calls without robot-specific bindings. + - Validate scene/profile manifests, JIT-ground calls to atomic actions, and retain verified state across dynamic task segments. Typical Data Flow ----------------- @@ -120,6 +126,11 @@ planner calls. An action engine receives semantic targets or poses, resolves the motion primitive sequence, and returns a trajectory that can be replayed in the simulation. +For robot-independent task code, the semantic-skill layer validates typed scene +references and robot capabilities before lowering one call at a time to that +same action engine. It adds task segmentation and structured results without +adding a separate planner or controller implementation. + Choosing Where to Start ----------------------- @@ -139,6 +150,9 @@ Choosing Where to Start - Use :doc:`atomic_actions/robot_skill_profiles` when semantic skills should resolve robot resources and policy presets from reusable embodiment configuration. +- Use :doc:`semantic_skills` when an application or agent should issue + robot-independent object-centric calls and retain verified state across + dynamic task segments. - Use :doc:`atomic actions ` when building scripted manipulation from reusable motion primitives. @@ -165,4 +179,5 @@ See Also solvers/index planners/index scene_registry.md + semantic_skills.md atomic_actions/index diff --git a/docs/source/overview/sim/scene_registry.md b/docs/source/overview/sim/scene_registry.md index 74f693c15..5e2804f56 100644 --- a/docs/source/overview/sim/scene_registry.md +++ b/docs/source/overview/sim/scene_registry.md @@ -90,6 +90,101 @@ For perception or hardware, construct registrations with an implementation of a {class}`SceneGeometryProvider`; the geometry belongs to the catalog even though the snapshot contains only its current pose and confidence. +## Semantic affordance capabilities + +An affordance is a registered direct child of an object, articulation, or link. +Its {class}`SceneAffordanceRef` has its own canonical ID, while `parent` and +`native_name` describe topology and the backend-local member. Semantic calls +select affordances by open, namespaced capabilities rather than by payload class +or declaration order. The built-in capabilities are: + +- {data}`GRASP_AFFORDANCE_CAPABILITY` (`affordance.grasp`); +- {data}`PLACE_ON_AFFORDANCE_CAPABILITY` (`affordance.place.on`); +- {data}`PLACE_IN_AFFORDANCE_CAPABILITY` (`affordance.place.in`). + +Register a capability-bearing grasp affordance and a scoped parent default as +follows. `antipodal_affordance` is an existing +`AntipodalAffordance` value, for example one produced by the grasp annotation +pipeline: + +```python +from dataclasses import replace + +import torch + +from embodichain.lab.sim.skills import ( + GRASP_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + +object_ref = SceneObjectRef("workpiece") +grasp_ref = SceneAffordanceRef("workpiece.grasp.antipodal") + +simulation_registry = SceneRegistry.from_simulation( + sim, + rigid_objects={"workpiece": "cube"}, +) +object_registration = replace( + simulation_registry.lookup(object_ref), + semantic_type="cube", + default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp_ref}, +) +registry = SceneRegistry( + ( + object_registration, + SceneEntityRegistration( + ref=grasp_ref, + parent=object_ref, + native_name="antipodal_grasp", + affordance=antipodal_affordance, + affordance_capabilities=frozenset( + {GRASP_AFFORDANCE_CAPABILITY} + ), + affordance_revision="antipodal-v1", + relative_pose=torch.eye(4), + ), + ) +) +``` + +The registry enforces these rules: + +- capabilities belong only to an affordance registration; +- a capability-bearing affordance declares an explicit + `affordance_revision`; +- `affordance.grasp` requires an `AntipodalAffordance` payload; +- `default_affordances` belongs to the parent and maps each capability to one + compatible direct child; +- an affordance has either a live `state_provider` or a parent-relative + `relative_pose`, never neither. + +{meth}`SceneRegistry.affordances` lists compatible direct children in canonical +ID order without selecting one. {meth}`SceneRegistry.resolve_affordance` applies +one strict selection rule: + +1. validate and use the explicit affordance, when supplied; +2. otherwise use the only compatible child; +3. otherwise use the parent's capability-scoped default; +4. otherwise raise {class}`AmbiguousSceneAffordanceError`. + +No compatible child, a parent mismatch, or a capability mismatch raises +{class}`UnsupportedSceneAffordanceError`. There is no declaration-order +fallback. Once selected, {meth}`SceneRegistry.object_semantics` creates an owned +atomic-action `ObjectSemantics` value using the canonical object ID and a copied +affordance payload. + +{attr}`SceneRegistry.entity_metadata` projects provider-free +{class}`SceneEntityMetadata` values. {class}`SceneManifest` uses the same value +model, so semantic integration can validate IDs, aliases, topology, capability +sets, payload types and revisions, relative affordance poses, and collision mode +before observing the scene. Changing any of that metadata requires rebuilding +and rebinding the semantic integration. Relation grounders dispatch by the exact +capability, payload type, and revision tuple; revisions therefore form part of +the integration contract rather than runtime pose state. + ## Publish canonical snapshots For an atomic-action planning runtime, create the provider through @@ -239,6 +334,6 @@ core by hand. The list form derives obstacle names from each object's `uid` (or an `obstacle_` fallback). It is not the registry-backed path and does not provide alias normalization or registry/provider/planner construction checks. -See {doc}`atomic_actions/index` for snapshot grounding and recovery semantics, -and {doc}`planners/curobo_planner` for cuRobo world representation and frame -details. +See {doc}`semantic_skills` for manifest and semantic-call integration, +{doc}`atomic_actions/index` for snapshot grounding and recovery semantics, and +{doc}`planners/curobo_planner` for cuRobo world representation and frame details. diff --git a/docs/source/overview/sim/semantic_skills.md b/docs/source/overview/sim/semantic_skills.md new file mode 100644 index 000000000..65ef61d29 --- /dev/null +++ b/docs/source/overview/sim/semantic_skills.md @@ -0,0 +1,312 @@ +(semantic-skills)= + +# Semantic skills + +```{currentmodule} embodichain.lab.sim.skills +``` + +Semantic skills are the application-facing layer above +{doc}`atomic actions `. A semantic call names an object, +relation, and optional robot participant without exposing joint groups, planner +instances, raw controller commands, or an `ActionBinding`. The semantic layer +validates that declaration against one scene and robot embodiment, then lowers +it to the same typed atomic-action runtime used by direct Python callers. + +This boundary is useful for MLLM agents, task planners, configuration-driven +applications, and users who want robot-independent task code. It is not an +agent loop: the application still owns task selection, perception policy, +physical-effect verification, and any task-level fallback strategy. + +```text +SemanticCallSpec values + | + v +SemanticIntegrationManifest + +-- SceneManifest canonical IDs and affordance metadata + +-- RobotSkillProfile resources, commands, presets, providers + `-- SemanticCallCatalog discoverable call schemas + | + v bind live registry + action engine +BoundSemanticIntegration + | + v +SemanticSkillCompiler + analyze() -> SemanticWorkflow provider-free validation and look-ahead + ground() -> GroundedSemanticCall latest observation -> ActionInvocation + | + v +SemanticSkillRuntime / SemanticTask + | + v +ExecutionRunner -> controller transports -> verified effects +``` + +## Semantic skills or direct atomic actions? + +Both paths use `AtomicActionEngine` and therefore share planning, controller +authorization, recovery, and effect semantics. + +| Choose | When it is the better boundary | +|---|---| +| Semantic skills | Task code should be robot-independent; an agent or planner emits object-centric calls; scene and resource validation should happen before controller work; dynamic task segments must retain verified state. | +| Direct atomic actions | A scripted application already knows exact goals, bindings, policies, and options; low-level tuning or a custom controller contract is part of the application. | + +For a user writing a small fixed robot script, direct atomic actions usually +have fewer integration objects. For an MLLM agent or an application targeting +multiple robot profiles, semantic skills provide the safer and more stable +interface. + +## Public semantic calls + +The built-in catalog returned by {func}`builtin_semantic_call_catalog` exposes +three curated call values: + +| Call | Intent | Main lowering behavior | +|---|---|---| +| {class}`Pick` | Acquire a registered object, optionally through an explicit grasp affordance. | Selects a capability-compatible grasp affordance and lowers to atomic `pick_up`. | +| {class}`Place` | Release a held object at an absolute pose, on a support, or inside a container. | Requires exactly one of `at`, `on`, or `inside`; relation targets use an explicitly installed typed grounder. | +| {class}`HandOver` | Transfer a held object to another robot resource. | Uses a robot-profile-selected provider for the middle and default final pose; an explicit `final_target` overrides the latter. | + +{class}`SemanticPose` expresses an absolute object-space pose with a position +and normalized WXYZ quaternion. Scene objects and affordances use typed +{class}`SceneObjectRef` and {class}`SceneAffordanceRef` values, so aliases are +resolved at the registry boundary instead of being propagated into execution. + +Extensions use {class}`RegisteredSemanticCall`. Its argument tree accepts only +declarative values; tensors, callables, classes, modules, and live simulator +objects are rejected. A registered descriptor must identify one exact +agent-visible atomic target, and the compiler must install a matching +{class}`RegisteredSemanticLowerer` with the same call ID and schema version. +Curated calls cannot be remapped through this extension mechanism. + +## Static integration + +Create the static declaration before execution: + +```python +from embodichain.lab.sim.skills import ( + SceneManifest, + SemanticIntegrationManifest, + builtin_semantic_call_catalog, +) + +manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(scene_registry), + robot_profile=robot_profile, + call_catalog=builtin_semantic_call_catalog(), +) +``` + +`SceneManifest` is provider-free: creating it does not observe simulation or +perception. It snapshots canonical identity, aliases, topology, affordance +capabilities and revisions, and collision-world mode. `manifest.bind(...)` +requires the live {class}`SceneRegistry` to match that snapshot and binds the +profile to the exact action engine. Replacing an installed agent-visible action, +changing the bound profile, or changing scene metadata invalidates the old +integration rather than silently reusing stale contracts. + +Policy preset selection is deterministic: + +1. `SemanticIntegrationManifest.runtime_preset`, when configured; +2. `RobotSkillProfile.skill_presets[atomic_skill_id]`; +3. `RobotSkillProfile.default_preset`. + +A missing or unknown preset is a validation error. The selected +{class}`SkillPolicyPreset` owns the motion policy, recovery policy, and +`ExecutionRunnerCfg` used by that call. + +## Analyze first, ground from fresh state + +{meth}`SemanticSkillCompiler.analyze` performs provider-free work: + +- catalog and schema discovery; +- canonical scene and affordance resolution; +- robot-resource and preset selection; +- verified-held-object flow analysis; +- first-release look-ahead for Pick grasp selection; +- validation that required lowerers and grounders are installed. + +It returns an immutable {class}`SemanticWorkflow`. No scene provider is read and +no planner is run at this stage. + +{meth}`SemanticSkillCompiler.ground` lowers exactly one analyzed call from the +latest {class}`~embodichain.lab.sim.atomic_actions.PlanningContext`. It resolves +late-bound relation or handover targets and returns a +{class}`GroundedSemanticCall` containing an `ActionInvocation` and an owned +per-environment `eligible_mask`: + +```python +workflow = compiler.analyze(calls, workflow_id="sort_workpiece") +grounded = compiler.ground( + workflow, + call_index=0, + context=latest_context, + eligible_mask=active_rows, +) +session = engine.start( + (grounded.invocation,), + latest_context, + eligible_mask=grounded.eligible_mask, +) +``` + +The runtime performs this JIT grounding automatically before every call. Known +calls should be submitted together when possible: a `Pick -> Place` or +`Pick -> HandOver` segment lets analysis pass the first downstream object target +into grasp selection. Splitting those calls into separate dynamic segments is +valid, but removes that look-ahead information from the earlier Pick. + +## Construct a runtime + +Use {meth}`SemanticSkillRuntime.from_simulation` for the standard simulation +path. It creates a registry-backed planning scene provider, a +`SimulationExecutionAdapter`, an `AtomicActionEngine` with built-ins, and the +semantic manifest/compiler: + +```python +runtime = SemanticSkillRuntime.from_simulation( + simulation=sim, + robot=robot, + motion_generator=motion_generator, + scene_registry=scene_registry, + robot_profile=robot_profile, + effect_verifier=verify_effect, + control_dt=4 * sim.sim_config.physics_dt, +) +``` + +Use {meth}`SemanticSkillRuntime.bind` when the application owns custom +observation, command, clock, endpoint-adapter, or hardware ports. Only one +{class}`SemanticTask` may own a runtime at a time; this layer does not implement +a resource scheduler or lease manager. + +`runtime.runner_cfg`, when supplied, overrides the runner configuration from +every selected skill preset. When omitted, each grounded call uses its own +preset's runner configuration. `control_dt` is the command cadence and is +independent of the simulation physics period. + +## Execute a fixed workflow + +The minimal robot-independent program names only the registered object and its +desired final pose: + +```python +from embodichain.lab.sim.skills import Pick, Place, SceneObjectRef, SemanticPose + +workpiece = SceneObjectRef("workpiece") +calls = ( + Pick(object=workpiece), + Place( + object=workpiece, + at=SemanticPose( + position=(-0.40, 0.48, 0.025), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ), + ), +) + +result = runtime.run( + calls, + task_id="pick_and_place", + effect_verifier=verify_effect, +) +result.require_all_succeeded() +``` + +{meth}`SemanticSkillRuntime.run` is blocking and requires a +`SemanticEffectVerifier`. Use {meth}`SemanticSkillRuntime.start` plus +{meth}`SemanticExecution.step` or +{meth}`SemanticExecution.run_until_blocked` when physical verification arrives +asynchronously. A pending effect produces +`SemanticExecutionStatus.WAITING_FOR_EFFECT`; resume it with a boolean +per-environment `effect_success` mask. + +## Dynamic tasks + +Dynamic task construction is supported at completed semantic-segment +boundaries. A {class}`SemanticTask` carries verified `TaskState`, the latest +observation, and a sticky eligible cohort across those decisions: + +```python +with runtime.open_task("clear_table") as task: + first = task.run_segment( + (Pick(object=workpiece),), + segment_id="acquire", + effect_verifier=verify_effect, + ) + + next_calls = decide_next_calls(first.task_state, task.latest_context) + task.run_segment( + next_calls, + segment_id="agent_decision_1", + effect_verifier=verify_effect, + ) + result = task.finish() +``` + +The following boundaries are intentional: + +- only one segment executes at a time; +- successful segments leave the task open until `finish()` or `cancel()`; +- failed or cancelled segments are terminal and release runtime ownership; +- environment rows that become ineligible remain excluded in later calls and + segments; +- `revise_current()` can update a compatible in-flight call, but it cannot + replace the semantic skill, logical invocation, or runtime endpoint addresses. + +The runtime does not automatically choose a replacement skill, re-run an agent, +or reconcile symbolic state after an uncertain physical effect. Implement those +task-level policies in the application at a safe segment boundary. + +## Recovery and physical success + +Each grounded call runs through the existing closed-loop `ExecutionRunner`. +Depending on its `RecoveryPolicy`, it can detect and recover from tracking +errors, supported scene-target motion, collision-world revisions, timeouts, and +per-environment planning failure. Recovery is bounded and emits structured +atomic-action events retained in {class}`SemanticCallRecord` and aggregated by +{class}`SemanticSegmentResult` and {class}`SemanticTaskResult`. + +Dynamic target recovery follows the atomic primitive's dependency contract. +For example, Pick monitors its object/grasp dependency only through the +`approach` segment; contact-, close-, and lift-induced object movement is not +treated as an external target update. Atomic HandOver can monitor +`SceneEntityPose` values supplied for its middle and final option poses. + +Planning success is not physical success. Attachment, release, and ownership +transfer are committed only after the application verifier accepts the pending +effect for each environment. A failed verification follows the configured +atomic recovery budget; if the runner terminates unsuccessfully, the semantic +segment and task fail. There is no implicit success assumption. + +Final task status is: + +- `SemanticTaskStatus.SUCCEEDED` when all initially eligible rows remain; +- `SemanticTaskStatus.PARTIAL_SUCCESS` when a non-empty subset remains; +- `SemanticTaskStatus.FAILED` when execution fails or no row remains; +- `SemanticTaskStatus.CANCELLED` after explicit cancellation. + +Call {meth}`SemanticTaskResult.require_all_succeeded` when partial batch success +is not acceptable. + +## Diagnostics and extension points + +{meth}`SemanticSkillRuntime.validate` exposes static analysis without observing, +planning, or executing. Static integration and grounding errors use +{class}`SemanticValidationError`, whose {class}`SemanticDiagnostic` contains a +stable code, a complete path, a human-readable message, and sorted candidates. +Agents should consume the structured fields rather than parse the exception +string. + +Three explicit extension points keep executable objects outside semantic calls: + +- {class}`RegisteredSemanticLowerer` lowers a catalog-registered call; +- {class}`RelationTargetGrounder` converts a capability-, payload-type-, and + revision-matched relation into an object pose; +- {class}`HandOverPoseProvider` supplies embodiment-appropriate middle and + default final object targets and is selected through + `RobotSkillProfile.grounding_providers["hand_over"]`. + +See {doc}`/tutorial/semantic_skills` for complete runnable Place and dual-arm +HandOver examples, {doc}`scene_registry` for affordance registration, and +{doc}`atomic_actions/robot_skill_profiles` for embodiment resource binding. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index f8c756cec..3fc278938 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -380,10 +380,12 @@ the robot, then applies a short horizontal force pulse so physics and friction slide it sideways during one ``PickUp`` invocation whose ``GraspGoal.grasp_xpos`` is a ``SceneEntityPose``. The session observes ``dynamic_goal_changed`` and ``replanned`` events, discards the entire stale -approach/close/lift plan, and rebuilds it from the cube's new location. The -replanned action closes the gripper, verifies the physical lift, and finishes -while holding the cube. The original and regenerated goal axes remain visible -for comparison: +approach/close/lift plan, and rebuilds it from the cube's new location while the +approach segment is active. After approach is dispatched, Pick stops monitoring +that object dependency so contact-, close-, and lift-induced movement does not +trigger a false dynamic-goal update. The replanned action closes the gripper, +verifies the physical lift, and finishes while holding the cube. The original +and regenerated goal axes remain visible for comparison: .. code-block:: bash @@ -460,7 +462,9 @@ dependencies. Object-centric skills may additionally declare an explicit ``ObjectSemantics.entity_id`` when they ground an object pose from the same scene snapshot; for example, ``PickUp`` automatically tracks that ID. The legacy ``ObjectSemantics.entity`` live-pose fallback is deprecated and does not -create a scene dependency. +create a scene dependency. An ``ActionPlan`` may bound dependency monitoring +with ``scene_dependency_end_segment``. ``PickUp`` uses ``approach`` as that +boundary; joint tracking and collision-world revision checks are unaffected. Task-state effects ------------------ diff --git a/docs/source/tutorial/index.rst b/docs/source/tutorial/index.rst index 10b76881f..c383e02f4 100644 --- a/docs/source/tutorial/index.rst +++ b/docs/source/tutorial/index.rst @@ -22,18 +22,19 @@ Follow the tutorials in this order for the best learning experience: 9. :doc:`motion_gen` — Generate smooth trajectories with motion planners. 10. :doc:`robot_articulation` — Plan contact-rich motion to open a passive drawer and push it halfway back. 11. :doc:`atomic_actions` — Use built-in action primitives (move, move joints, pick, move held object, place). -12. :doc:`gizmo` — Interactively control robots with on-screen gizmos. +12. :doc:`semantic_skills` — Build robot-independent Pick, Place, and dual-arm workflows on top of atomic actions. +13. :doc:`gizmo` — Interactively control robots with on-screen gizmos. **Phase 2: Environments** -13. :doc:`basic_env` — Create a simple Gymnasium environment with ``BaseEnv``. Prerequisite: Phase 1 basics. -14. :doc:`modular_env` — Build a config-driven environment with ``EmbodiedEnv``, managers, and randomization. Prerequisite: :doc:`basic_env`. -15. :doc:`data_generation` — Generate expert demonstration datasets for imitation learning. Prerequisite: :doc:`modular_env`. -16. :doc:`rl` — Train RL agents with PPO or GRPO. Prerequisite: :doc:`basic_env`. +14. :doc:`basic_env` — Create a simple Gymnasium environment with ``BaseEnv``. Prerequisite: Phase 1 basics. +15. :doc:`modular_env` — Build a config-driven environment with ``EmbodiedEnv``, managers, and randomization. Prerequisite: :doc:`basic_env`. +16. :doc:`data_generation` — Generate expert demonstration datasets for imitation learning. Prerequisite: :doc:`modular_env`. +17. :doc:`rl` — Train RL agents with PPO or GRPO. Prerequisite: :doc:`basic_env`. **Phase 3: Extending the Framework** -17. :doc:`/guides/add_robot` — Add a new robot model to EmbodiChain. +18. :doc:`/guides/add_robot` — Add a new robot model to EmbodiChain. .. toctree:: :maxdepth: 1 @@ -51,6 +52,7 @@ Follow the tutorials in this order for the best learning experience: motion_gen robot_articulation atomic_actions + semantic_skills gizmo basic_env modular_env diff --git a/docs/source/tutorial/semantic_skills.rst b/docs/source/tutorial/semantic_skills.rst new file mode 100644 index 000000000..4422ea7ae --- /dev/null +++ b/docs/source/tutorial/semantic_skills.rst @@ -0,0 +1,437 @@ +Semantic skills +=============== + +Semantic skills let task code describe object-centric intent while the scene +registry and robot profile own simulator- and embodiment-specific details. This +tutorial covers two complete examples: + +* ``Pick -> Place`` with one manipulator; +* ``Pick -> RegisteredSemanticCall`` lowered to a dual-arm HandOver. + +The runnable sources are: + +* ``scripts/tutorials/semantic_skill/place.py``; +* ``scripts/tutorials/semantic_skill/hand_over.py``; +* ``scripts/tutorials/semantic_skill/tutorial_utils.py`` for shared setup and + verification helpers. + +Both runnable examples use the same three-part structure: + +* ``create_*_application(...)`` assembles a fully bound + ``SemanticSkillRuntime`` and installs its default physical-effect verifier; +* ``create_*_task()`` declares only robot-independent semantic calls; +* ``app.run(task, ...)`` is the application-facing execution entry point. + +``app`` is still a ``SemanticSkillRuntime`` rather than another wrapper class. +The factory only keeps simulator, scene-registry, robot-profile, and verifier +construction out of the task declaration. + +Read :doc:`atomic_actions` first if you need the underlying planning, execution, +and effect-verification model. The complete semantic architecture is documented +in :doc:`/overview/sim/semantic_skills`; canonical scene registration is covered +by :doc:`/overview/sim/scene_registry`. + +Run the examples +---------------- + +The examples are interactive by default: + +.. code-block:: bash + + python scripts/tutorials/semantic_skill/place.py + python scripts/tutorials/semantic_skill/hand_over.py + +For an unattended simulation run, use: + +.. code-block:: bash + + python scripts/tutorials/semantic_skill/place.py --headless --auto_play --device cpu + python scripts/tutorials/semantic_skill/hand_over.py --headless --auto_play --device cpu + +Both examples accept the common simulation tutorial flags. Use ``--help`` for +the complete list. ``--diagnose_plan`` takes a separate offline path that +analyzes, grounds, and statically compiles the workflow without executing +controller commands: + +.. code-block:: bash + + python scripts/tutorials/semantic_skill/place.py --headless --device cpu --diagnose_plan + python scripts/tutorials/semantic_skill/hand_over.py --headless --device cpu --diagnose_plan + +.. attention:: + + Diagnostic compilation projects expected attachment changes hypothetically + between calls. It proves that the current workflow can be lowered and + planned; it does not prove that a physical grasp, release, or transfer + occurred. Normal execution uses ``SemanticSkillRuntime`` and an explicit + effect verifier. + +The application entry +--------------------- + +After constructing the simulator entities, normal task-facing code is compact: + +.. code-block:: python + + app = create_place_application( + sim, + robot, + workpiece, + hand_open=hand_open, + hand_grasp=hand_grasp, + n_sample=args.n_sample, + force_reannotate=args.force_reannotate, + ) + + result = app.run( + create_place_task(), + task_id="tutorial.semantic_pick_place", + on_step=observe_runtime_step, + ) + result.require_all_succeeded() + +The factory installs the live effect verifier on the runtime, so it does not +appear in every ``run`` call. ``on_step`` remains explicit because it is +optional tutorial observability rather than semantic task intent. + +Keep the whole known task in one tuple when possible. Use ``open_task`` and +multiple segments only when a later call genuinely depends on a new +observation or an application/agent decision. + +Example 1: semantic Pick and Place +---------------------------------- + +The Place example demonstrates the normal built-in path. Its workflow contains +no robot control-part names: + +.. code-block:: python + + from embodichain.lab.sim.skills import ( + Pick, + Place, + SceneObjectRef, + SemanticPose, + ) + + def create_place_task() -> tuple[Pick, Place]: + workpiece = SceneObjectRef("workpiece") + return ( + Pick(object=workpiece), + Place( + object=workpiece, + at=SemanticPose( + position=(-0.40, 0.48, 0.025), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ), + ), + ) + +The scene registry owns object identity +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The simulation object is registered under the canonical semantic ID +``workpiece``. The grasp affordance is a direct child with the open +``affordance.grasp`` capability and an explicit payload revision. The object +selects it as its capability-scoped default: + +.. code-block:: python + + object_ref = SceneObjectRef("workpiece") + grasp_ref = SceneAffordanceRef("workpiece.grasp.antipodal") + + object_registration = replace( + simulation_registry.lookup(object_ref), + semantic_type="cube", + default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp_ref}, + ) + registry = SceneRegistry( + ( + object_registration, + SceneEntityRegistration( + ref=grasp_ref, + parent=object_ref, + native_name="antipodal_grasp", + affordance=antipodal_affordance, + affordance_capabilities=frozenset( + {GRASP_AFFORDANCE_CAPABILITY} + ), + affordance_revision="antipodal-v1", + relative_pose=torch.eye(4), + ), + ) + ) + +``Pick(object=workpiece)`` can now omit an explicit affordance. Resolution is +deterministic because the parent owns one default for the required capability. + +The robot profile owns embodiment details +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The tutorial profile maps one logical ``primary_manipulator`` resource onto the +robot's ``arm`` motion endpoint and ``hand`` grasp endpoint. It also owns the +semantic ``open`` and ``grasp`` joint commands, resource default, and per-skill +policy presets: + +.. code-block:: python + + profile = RobotSkillProfile( + profile_id="tutorial.single_arm", + resources={ + "primary_manipulator": create_manipulator_resource( + "primary_manipulator", + motion_control_part="arm", + grasp_control_part="hand", + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=hand_open, + grasp=hand_grasp, + ) + }, + defaults={ + "pick_up": ResourceBinding( + resources={"primary": "primary_manipulator"} + ), + "place": ResourceBinding( + resources={"primary": "primary_manipulator"} + ), + }, + presets={...}, + default_preset="default", + ) + +The semantic calls remain unchanged if another robot profile can satisfy the +same atomic skill contracts. + +Assemble and run the application +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +``SemanticSkillRuntime.from_simulation`` assembles the standard planning scene, +simulation ports, action engine, manifest, and compiler: + +.. code-block:: python + + def create_place_application( + simulation, + robot, + workpiece, + *, + hand_open, + hand_grasp, + n_sample, + force_reannotate, + ) -> SemanticSkillRuntime: + registry = ... + profile = ... + verify_effect = ... + return SemanticSkillRuntime.from_simulation( + simulation=simulation, + robot=robot, + motion_generator=create_curobo_motion_generator(robot), + scene_registry=registry, + robot_profile=profile, + effect_verifier=verify_effect, + control_dt=4 * simulation.sim_config.physics_dt, + ) + + app = create_place_application(...) + result = app.run( + create_place_task(), + task_id="tutorial.semantic_pick_place", + on_step=observe_runtime_step, + ) + result.require_all_succeeded() + +The verifier checks the live lift, the planned object-to-EEF relation, final +object position, and open hand before accepting the symbolic effects. The +``on_step`` callback reports recovery events and does not decide whether an +effect succeeded. + +Submitting both calls in one segment is important for planning quality. Static +analysis can pass the Place target to Pick as a downstream reachability target. +The runtime still grounds and executes one call at a time from fresh +observations. + +Example 2: registered dual-arm HandOver +--------------------------------------- + +The dual-arm example demonstrates the extension path in addition to resource +disjointness. It registers ``tutorial.hand_over`` against the existing atomic +HandOver descriptor: + +.. code-block:: python + + call_catalog = builtin_semantic_call_catalog().with_descriptor( + SemanticCallDescriptor( + call_id="tutorial.hand_over", + spec_type=RegisteredSemanticCall, + target_descriptor=AtomicHandOver.descriptor(), + ) + ) + + def create_handover_task() -> tuple[Pick, RegisteredSemanticCall]: + return ( + Pick(object=workpiece), + RegisteredSemanticCall( + call_id="tutorial.hand_over", + arguments={"object": workpiece}, + ), + ) + +Why use a registered call here? +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The public semantic :class:`~embodichain.lab.sim.skills.HandOver` path delegates +middle and final object targets to a named ``HandOverPoseProvider`` selected by +the robot profile. This tutorial instead demonstrates how an application can +publish a separate versioned call schema and explicitly lower it to tuned +``HandOverOptions``. + +The lowerer is executable integration code, so it is installed on the compiler +rather than placed inside ``RegisteredSemanticCall.arguments``: + +.. code-block:: python + + class TutorialHandOverLowerer(RegisteredSemanticLowerer): + call_id = "tutorial.hand_over" + schema_version = 1 + + def lower(self, call, *, context, bound): + semantics = registry.object_semantics( + call.arguments["object"], + affordance=registry.resolve_affordance( + call.arguments["object"], + capability=GRASP_AFFORDANCE_CAPABILITY, + ), + ) + return SemanticLowering( + goal=GraspGoal(semantics), + skill_options=HandOverOptions( + middle_object_pose=middle_pose.to(context.robot.qpos.device), + final_object_pose=final_pose.to(context.robot.qpos.device), + receive_pick_object_part="bottom", + ), + ) + +The descriptor and lowerer call IDs and schema versions must match exactly. A +registered call cannot replace the curated ``pick``, ``place``, or +``hand_over`` meanings. + +The dual-arm profile declares two physically disjoint manipulator resources. +Its defaults map Pick's ``primary`` slot to the source and HandOver's ``source`` +and ``destination`` slots to different resources. Binding rejects overlapping +claims before execution. + +The application factory passes the extension objects and default verifier to +the runtime explicitly: + +.. code-block:: python + + def create_handover_application( + simulation, + robot, + workpiece, + *, + left_open, + left_grasp, + right_open, + right_grasp, + n_sample, + force_reannotate, + ) -> SemanticSkillRuntime: + registry = ... + profile = ... + verify_effect = ... + return SemanticSkillRuntime.from_simulation( + simulation=simulation, + robot=robot, + motion_generator=create_toppra_motion_generator(robot), + scene_registry=registry, + robot_profile=profile, + call_catalog=call_catalog, + effect_verifier=verify_effect, + registered_lowerers=(TutorialHandOverLowerer(registry),), + control_dt=4 * simulation.sim_config.physics_dt, + ) + + app = create_handover_application(...) + result = app.run( + create_handover_task(), + task_id="tutorial.semantic_pick_handover", + on_step=observe_runtime_step, + ) + +The effect verifier first accepts the source Pick only after observing the held +relation. At the transfer boundary it verifies source release, destination +grasp, destination ownership, and the final object target before committing the +new ``TaskState``. + +Dynamic decisions between segments +---------------------------------- + +Use ``open_task`` when an application or agent cannot know the whole task in +advance: + +.. code-block:: python + + with app.open_task("agent_task") as task: + acquire = task.run_segment( + (Pick(object=workpiece),), + segment_id="acquire", + ) + + # Decide only after the successful segment has committed verified state. + destination = choose_destination(acquire.task_state, task.latest_context) + task.run_segment( + (Place(object=workpiece, at=destination),), + segment_id="deliver", + ) + result = task.finish() + +Successful segments retain verified symbolic state and the per-environment +eligible mask. A failed or cancelled segment is terminal. Only one task and one +segment may own a runtime at a time; scheduling multiple independent tasks is an +application responsibility. + +For non-blocking integration, replace ``run_segment`` with ``start_segment`` and +advance the returned ``SemanticExecution`` through ``step``. When its status is +``WAITING_FOR_EFFECT``, inspect ``pending_effect`` and submit an +``effect_success`` boolean mask on a later step. + +Recovery boundaries +------------------- + +The semantic runtime delegates call-local recovery to the atomic +``ExecutionRunner``. Tracking errors, supported scene-target movement, +collision-world revisions, timeouts, and planning failure produce structured +events and consume the selected preset's bounded recovery budget. + +Keep these distinctions in mind: + +* Pick monitors its object and grasp target only through the approach segment; + contact- or lift-induced motion does not look like an external target update. +* Physical effects are never committed from planning success alone. +* Rows that exhaust recovery become ineligible and remain excluded from later + calls and dynamic segments. +* ``revise_current`` can change a compatible active call, but cannot switch to a + different skill or controller address. +* A terminal runtime failure does not automatically choose another skill or + reconcile uncertain physical state. Perform that task-level recovery at an + application-controlled segment boundary. + +Inspect ``SemanticTaskResult.status``, ``eligible_mask``, ``segments``, and +aggregated ``events`` for structured feedback. Call ``require_all_succeeded`` +when partial vectorized success should be treated as an application error. + +Further reading +--------------- + +* :doc:`/overview/sim/semantic_skills` — architecture, ownership, dynamic tasks, + and extension contracts; +* :doc:`/overview/sim/scene_registry` — canonical IDs, affordances, snapshots, + and collision integration; +* :doc:`/overview/sim/atomic_actions/robot_skill_profiles` — resource graphs, + policy presets, and grounding-provider selection; +* :doc:`/overview/sim/atomic_actions/builtin_actions` — behavior and recovery + contracts of the lowered atomic primitives. diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index fd13e617a..4006fa008 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -509,6 +509,7 @@ def build_plan( replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, + scene_dependency_end_segment: str | None = None, ) -> ActionPlan: """Build a validated action plan for a primitive implementation. @@ -522,6 +523,8 @@ def build_plan( diagnostics: Optional retained planner diagnostics. segment_lengths: Optional ordered mapping from semantic segment names to waypoint counts. Zero-length entries are omitted. + scene_dependency_end_segment: Optional last segment during which + scene motion may invalidate and replan the action. Returns: Side-effect-free action plan. @@ -559,6 +562,7 @@ def build_plan( replannable=replannable, diagnostics=diagnostics, segment_lengths=segment_lengths, + scene_dependency_end_segment=scene_dependency_end_segment, feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, joint_trajectory=timed, ) @@ -574,6 +578,7 @@ def build_command_plan( replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, + scene_dependency_end_segment: str | None = None, feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, joint_trajectory: TimedTrajectory | None = None, ) -> ActionPlan: @@ -582,6 +587,23 @@ def build_command_plan( Non-joint command sequences use timed completion unless a future endpoint-specific feedback evaluator is installed. Semantic effects remain externally verified through the execution session. + + Args: + request: Resolved invocation snapshot being planned. + context: Planning input used for the plan. + success: Per-environment planning success or scalar planner result. + commands: Transport-neutral timed command sequence. + expected_effects: Symbolic effects to verify after execution. + replannable: Whether the execution runtime may replan this action. + diagnostics: Optional retained planner diagnostics. + segment_lengths: Optional ordered semantic segment lengths. + scene_dependency_end_segment: Optional last segment during which + scene motion may invalidate and replan the action. + feedback_mode: Feedback contract used to detect command completion. + joint_trajectory: Optional trajectory paired with joint feedback. + + Returns: + Validated, endpoint-authorized action plan. """ if not isinstance(commands, TimedCommandSequence): raise TypeError("commands must be a TimedCommandSequence.") @@ -624,6 +646,7 @@ def build_command_plan( joint_trajectory=joint_trajectory, segments=segments, scene_dependencies=self._scene_dependencies(request), + scene_dependency_end_segment=scene_dependency_end_segment, collision_world_sensitive=self._uses_collision_world(request, context), replannable=replannable, expected_effects=expected_effects or StateDelta(), diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index a7083fcb4..ab0711c2f 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -91,6 +91,7 @@ def __init__( control_profiles=control_profiles, ) self._actions: dict[str, AtomicAction] = {} + self._skill_catalog_revision = 0 self._skill_profile: BoundRobotSkillProfile | None = None if load_builtins: self._load_builtin_actions() @@ -154,6 +155,16 @@ def skills(self) -> Mapping[str, SkillDescriptor]: } ) + @property + def skill_catalog_revision(self) -> int: + """Return the monotonic installed semantic-skill catalog revision. + + Replacing an agent-visible implementation advances the revision even + when its public descriptor is equal. Bound profiles and semantic + compilers can therefore reject stale implementation ownership. + """ + return self._skill_catalog_revision + @property def skill_profile(self) -> BoundRobotSkillProfile | None: """Return the currently bound semantic robot profile, when configured.""" @@ -330,6 +341,13 @@ def register(self, action: AtomicAction, *, replace: bool = False) -> None: ) action._bind(self._planning_services) self._actions[descriptor.skill_id] = action + existing_descriptor = None if existing is None else existing.descriptor() + if (descriptor.agent_visible and descriptor.binding_contract is not None) or ( + existing_descriptor is not None + and existing_descriptor.agent_visible + and existing_descriptor.binding_contract is not None + ): + self._skill_catalog_revision += 1 self._skill_profile = None def _load_builtin_actions(self) -> None: @@ -577,6 +595,8 @@ def start( self, invocations: Iterable[ActionInvocation], context: PlanningContext | None = None, + *, + eligible_mask: torch.Tensor | None = None, ) -> ExecutionSession: """Start closed-loop execution for a grounded invocation sequence. @@ -584,6 +604,9 @@ def start( invocations: Grounded action requests in execution order. context: Initial measured state and scene snapshot. The engine captures one when omitted. + eligible_mask: Optional per-environment cohort allowed to execute. + Ineligible rows remain excluded for the whole session. All rows + are eligible when omitted. Returns: Stateful execution session advanced by ``session.tick(...)``. @@ -591,7 +614,12 @@ def start( from .execution import ExecutionSession initial = self.initial_context() if context is None else context - return ExecutionSession(self, tuple(invocations), initial) + return ExecutionSession( + self, + tuple(invocations), + initial, + eligible_mask=eligible_mask, + ) def _validate_context(self, context: PlanningContext) -> None: """Validate an externally supplied planning context.""" diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 8511519a1..59a32a5a6 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -194,6 +194,8 @@ def __init__( engine: AtomicActionEngine, invocations: tuple[ActionInvocation, ...], context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, ) -> None: if not invocations: raise ValueError("ExecutionSession requires at least one invocation.") @@ -218,16 +220,23 @@ def __init__( self._last_command_mask = torch.zeros( context.batch_size, dtype=torch.bool, device=context.robot.qpos.device ) - self._eligible = torch.ones_like(self._last_command_mask) + self._eligible = ( + torch.ones_like(self._last_command_mask) + if eligible_mask is None + else self._normalize_mask(eligible_mask, "eligible_mask") + ) self._pending = self._eligible.clone() self._action_retries = torch.zeros( context.batch_size, dtype=torch.long, device=context.robot.qpos.device ) self._replans = torch.zeros_like(self._action_retries) self._pending_effect: EffectVerificationRequest | None = None - self._status = ExecutionStatus.RUNNING + self._status = ( + ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED + ) self._queued_events: list[ExecutionEvent] = [] - self._plan_current(context, ExecutionEventKind.ACTION_PLANNED) + if self._status is ExecutionStatus.RUNNING: + self._plan_current(context, ExecutionEventKind.ACTION_PLANNED) @property def status(self) -> ExecutionStatus: @@ -982,8 +991,13 @@ def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: """Detect material motion of entities referenced by the action goal.""" dependencies = plan.scene_dependencies changed = torch.zeros_like(self._eligible) + dependency_end = plan.scene_dependency_end_segment if ( not dependencies + or ( + dependency_end is not None + and self._waypoint_index >= plan.segment(dependency_end).stop + ) or self._context.scene.version == self._planned_scene.version ): return changed @@ -1042,6 +1056,8 @@ def _batched_entity_pose(self, state: EntityState) -> torch.Tensor: def _normalize_mask(self, value: torch.Tensor, name: str) -> torch.Tensor: """Validate and copy a per-environment boolean mask.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") if value.dtype != torch.bool or value.shape != (self._context.batch_size,): raise ValueError( f"{name} must be bool with shape ({self._context.batch_size},)." diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index bb6502567..9a887552c 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -57,9 +57,22 @@ def __post_init__(self) -> None: "relative_pose", allow_waypoints=False, ) + object.__setattr__(self, "relative_pose", self.relative_pose.clone()) if not 0.0 <= self.minimum_confidence <= 1.0: raise ValueError("minimum_confidence must be in [0, 1].") + def snapshot(self) -> SceneEntityPose: + """Return an independently owned late-bound pose value. + + Returns: + Exact scene reference with an owned relative-pose tensor. + """ + return SceneEntityPose( + self.entity_id, + relative_pose=self.relative_pose, + minimum_confidence=self.minimum_confidence, + ) + PoseGoalValue = torch.Tensor | SceneEntityPose """Explicit pose tensor or a pose resolved from the latest scene snapshot.""" diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index 25cf83c79..66f224516 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -430,6 +430,8 @@ class ActionPlan: joint_trajectory: TimedTrajectory | None = None segments: tuple[TrajectorySegment, ...] = () scene_dependencies: tuple[str, ...] = () + scene_dependency_end_segment: str | None = None + """Stop dynamic-goal monitoring after this segment is dispatched.""" collision_world_sensitive: bool = False replannable: bool = True expected_effects: StateDelta = field(default_factory=StateDelta) @@ -670,6 +672,21 @@ def __post_init__(self) -> None: "ActionPlan segments must cover the command sequence exactly without " "gaps or overlaps." ) + dependency_end = self.scene_dependency_end_segment + if dependency_end is not None: + if not isinstance(dependency_end, str) or not dependency_end: + raise ValueError( + "scene_dependency_end_segment must be a non-empty segment " + "name or None." + ) + if dependency_end not in names: + raise ValueError( + "scene_dependency_end_segment must name an ActionPlan segment." + ) + if not dependencies: + raise ValueError( + "scene_dependency_end_segment requires scene_dependencies." + ) object.__setattr__(self, "plan_success", self.plan_success.clone()) object.__setattr__(self, "commands", self.commands.snapshot()) object.__setattr__( diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 6e7fa9269..ff97a27b9 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -38,6 +38,12 @@ _same_object_identity, ) from embodichain.lab.sim.atomic_actions.effects import StateDelta +from embodichain.lab.sim.atomic_actions.goals import ( + PoseGoalValue, + collect_scene_dependencies, + resolve_pose_goal, + validate_pose_goal, +) from embodichain.lab.sim.atomic_actions.invocation import ( ActionOptions, ResolvedActionRequest, @@ -78,13 +84,15 @@ class HandOverOptions(ActionOptions): """Object part the receiving arm grasps during the handover (see :meth:`AntipodalAffordance.get_valid_grasp_poses`).""" - middle_object_pose: torch.Tensor | None = None + middle_object_pose: PoseGoalValue | None = None """Object pose at the handover point where the receiving arm grasps it, - shape ``(4, 4)`` or ``(num_envs, 4, 4)``. Must be set by the caller.""" + either a scene-relative pose or a tensor with shape ``(4, 4)`` or + ``(n_envs, 4, 4)``. Must be set by the caller.""" - final_object_pose: torch.Tensor | None = None - """Object pose the receiving arm delivers the object to, shape ``(4, 4)`` - or ``(num_envs, 4, 4)``. Must be set by the caller.""" + final_object_pose: PoseGoalValue | None = None + """Object pose the receiving arm delivers the object to, either a + scene-relative pose or a tensor with shape ``(4, 4)`` or + ``(n_envs, 4, 4)``. Must be set by the caller.""" receive_approach_direction: torch.Tensor = torch.tensor( [0.0, 0.0, -1.0], dtype=torch.float32 @@ -134,7 +142,16 @@ def __post_init__(self) -> None: for name in ("middle_object_pose", "final_object_pose"): value = getattr(self, name) if value is not None: - object.__setattr__(self, name, value.clone()) + validate_pose_goal(value, name, allow_waypoints=False) + object.__setattr__( + self, + name, + ( + value.clone() + if isinstance(value, torch.Tensor) + else value.snapshot() + ), + ) @dataclass(frozen=True, slots=True, eq=False) @@ -195,9 +212,17 @@ def _scene_dependencies( self, request: ResolvedActionRequest[GraspGoal, HandOverOptions], ) -> tuple[str, ...]: - """Return no goal-pose dependency because handover ignores grasp_xpos.""" - del request - return () + """Return scene entities referenced by late-bound handover targets.""" + return collect_scene_dependencies( + tuple( + target + for target in ( + request.skill_options.middle_object_pose, + request.skill_options.final_object_pose, + ) + if target is not None + ) + ) def _resolve_resources( self, @@ -303,10 +328,20 @@ def _plan( assert options.middle_object_pose is not None assert options.final_object_pose is not None middle_object_pose = self._resolve_matrix( - options.middle_object_pose, "middle_object_pose" + resolve_pose_goal( + options.middle_object_pose, + context, + name="middle_object_pose", + ), + "middle_object_pose", ) final_object_pose = self._resolve_matrix( - options.final_object_pose, "final_object_pose" + resolve_pose_goal( + options.final_object_pose, + context, + name="final_object_pose", + ), + "final_object_pose", ) receive_approach_direction = options.receive_approach_direction.to( device=self.device, dtype=torch.float32 diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index 13437db44..a1a381f32 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -19,7 +19,7 @@ from __future__ import annotations import math -from dataclasses import dataclass +from dataclasses import dataclass, replace from typing import ClassVar import torch @@ -46,6 +46,7 @@ ObjectActionGoal, PoseGoalValue, _resolve_object_pose, + collect_scene_dependencies, resolve_pose_goal, validate_pose_goal, ) @@ -120,7 +121,7 @@ class PickUpOptions(ActionOptions): approach_alignment_max_angle: float | None = None """Optional maximum TCP z-axis deviation from the approach direction.""" - downstream_object_target_poses: tuple[torch.Tensor, ...] = () + downstream_object_target_poses: tuple[PoseGoalValue, ...] = () """Future object poses that must be reachable with the selected grasp.""" obj_upright_direction: torch.Tensor | None = None @@ -154,10 +155,20 @@ def __post_init__(self) -> None: ): raise ValueError("obj_upright_direction must be a finite (3,) tensor.") object.__setattr__(self, "approach_direction", self.approach_direction.clone()) + downstream_targets: list[PoseGoalValue] = [] + for index, value in enumerate(self.downstream_object_target_poses): + validate_pose_goal( + value, + f"downstream_object_target_poses[{index}]", + allow_waypoints=False, + ) + downstream_targets.append( + value.clone() if isinstance(value, torch.Tensor) else value.snapshot() + ) object.__setattr__( self, "downstream_object_target_poses", - tuple(value.clone() for value in self.downstream_object_target_poses), + tuple(downstream_targets), ) if self.obj_upright_direction is not None: object.__setattr__( @@ -199,6 +210,11 @@ def _scene_dependencies( entity_id = request.goal.semantics.entity_id if entity_id is not None: dependencies.add(entity_id) + dependencies.update( + collect_scene_dependencies( + request.skill_options.downstream_object_target_poses + ) + ) return tuple(sorted(dependencies)) def _get_full_pickup_trajectory( @@ -305,8 +321,20 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan approach, close, and lift segments without committing attachment.""" - target = request.goal - options = request.skill_options + target = self.require_goal(request) + options = replace( + request.skill_options, + downstream_object_target_poses=tuple( + resolve_pose_goal( + downstream_target, + context, + name=f"downstream_object_target_poses[{index}]", + ) + for index, downstream_target in enumerate( + request.skill_options.downstream_object_target_poses + ) + ), + ) approach_direction = options.approach_direction.to( device=self.device, dtype=torch.float32 ) @@ -417,6 +445,15 @@ def _plan( ), expected_effects=StateDelta(held_object_updates={control_part: held}), segment_lengths=segment_lengths, + # Once the approach is dispatched the object can move because of + # contact or grasping. That self-induced motion must not look like + # an external dynamic-goal update. + scene_dependency_end_segment=( + "approach" + if segment_lengths.get("approach", 0) > 0 + and self._scene_dependencies(request) + else None + ), ) def _resolve_grasp_pose( diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index 5a4bd80a1..39185a8a0 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -502,12 +502,17 @@ def get_local_pose_cpu( """Helper function to get local pose on CPU.""" if to_matrix: pose = torch.as_tensor( - [entity.get_local_pose() for entity in entities], + np.array([entity.get_local_pose() for entity in entities]), + dtype=torch.float32, ) else: - xyzs = torch.as_tensor([entity.get_location() for entity in entities]) + xyzs = torch.as_tensor( + np.array([entity.get_location() for entity in entities]), + dtype=torch.float32, + ) quats = torch.as_tensor( - [entity.get_rotation_quat() for entity in entities] + np.array([entity.get_rotation_quat() for entity in entities]), + dtype=torch.float32, ) quats = convert_quat(quats, to="wxyz") pose = torch.cat((xyzs, quats), dim=-1) @@ -856,7 +861,9 @@ def get_inertia(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: ) inertias.append(inertia) - return torch.as_tensor(inertias, dtype=torch.float32, device=self.device) + return torch.as_tensor( + np.array(inertias), dtype=torch.float32, device=self.device + ) def set_visual_material( self, @@ -1074,7 +1081,7 @@ def get_body_scale(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """ ids = env_ids if env_ids is not None else range(self.num_instances) return torch.as_tensor( - [self._entities[id].get_body_scale() for id in ids], + np.array([self._entities[id].get_body_scale() for id in ids]), dtype=torch.float32, device=self.device, ) diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 7a990fb28..b9ae2951f 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -18,6 +18,35 @@ from __future__ import annotations +from .calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticPose, + builtin_semantic_call_catalog, +) +from .compiler import ( + GroundedSemanticCall, + HandOverPoseProvider, + HandOverPoseTargets, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticLowering, + SemanticObjectTarget, + SemanticRelationTarget, + SemanticSkillCompiler, + SemanticWorkflow, +) +from .integration import ( + SceneEntityManifest, + SceneManifest, + SemanticDiagnostic, + SemanticIntegrationManifest, + SemanticValidationError, +) from .profiles import ( AmbiguousSkillBindingError, BoundRobotSkillProfile, @@ -37,7 +66,23 @@ SkillPolicyPreset, UnsupportedSkillError, ) +from .runtime import ( + SemanticCallRecord, + SemanticEffectVerifier, + SemanticExecution, + SemanticExecutionStatus, + SemanticExecutionStep, + SemanticSegmentResult, + SemanticSkillRuntime, + SemanticTask, + SemanticTaskResult, + SemanticTaskStatus, +) from .scene import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, RegistrySceneProvider, SceneAffordanceRef, SceneArticulationRef, @@ -45,20 +90,32 @@ SceneCollisionWorldMode, SceneDynamics, SceneEntityRef, + SceneEntityMetadata, SceneEntityRegistration, SceneEntityStateProvider, SceneGeometryProvider, SceneLinkRef, SceneObjectRef, SceneRegistry, + UnsupportedSceneAffordanceError, ) __all__ = [ + "AmbiguousSceneAffordanceError", "AmbiguousSkillBindingError", "BoundRobotSkillProfile", "ControlPartEndpoint", "ControlPartEndpointAdapter", "EndpointResolution", + "GRASP_AFFORDANCE_CAPABILITY", + "GroundedSemanticCall", + "HandOver", + "HandOverPoseProvider", + "HandOverPoseTargets", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", + "Pick", + "Place", "ProfileValidationError", "RegistrySceneProvider", "ResolvedRobotResource", @@ -68,6 +125,9 @@ "ResourceClaim", "ResourceEndpoint", "ResourceEndpointAdapter", + "RegisteredSemanticCall", + "RegisteredSemanticLowerer", + "RelationTargetGrounder", "RobotResource", "RobotSkillProfile", "SceneAffordanceRef", @@ -76,12 +136,38 @@ "SceneCollisionWorldMode", "SceneDynamics", "SceneEntityRef", + "SceneEntityMetadata", "SceneEntityRegistration", + "SceneEntityManifest", "SceneEntityStateProvider", "SceneGeometryProvider", "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "SceneManifest", + "SemanticCallCatalog", + "SemanticCallDescriptor", + "SemanticCallRecord", + "SemanticDiagnostic", + "SemanticEffectVerifier", + "SemanticExecution", + "SemanticExecutionStatus", + "SemanticExecutionStep", + "SemanticIntegrationManifest", + "SemanticLowering", + "SemanticObjectTarget", + "SemanticPose", + "SemanticRelationTarget", + "SemanticSegmentResult", + "SemanticSkillCompiler", + "SemanticSkillRuntime", + "SemanticTask", + "SemanticTaskResult", + "SemanticTaskStatus", + "SemanticValidationError", + "SemanticWorkflow", "SkillPolicyPreset", "UnsupportedSkillError", + "UnsupportedSceneAffordanceError", + "builtin_semantic_call_catalog", ] diff --git a/embodichain/lab/sim/skills/calls.py b/embodichain/lab/sim/skills/calls.py new file mode 100644 index 000000000..f0e359a88 --- /dev/null +++ b/embodichain/lab/sim/skills/calls.py @@ -0,0 +1,806 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Immutable, robot-independent semantic call specifications.""" + +from __future__ import annotations + +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, field +import math +import re +from types import MappingProxyType +from typing import ClassVar, TypeAlias + +import torch + +from embodichain.utils.math import matrix_from_quat +from embodichain.lab.sim.atomic_actions import ( + DisjointResourceSlots, + DisjointSlotEndpoints, + SkillBindingContract, + SkillDescriptor, + SkillEndpointRequirement, + SkillResourceSlot, +) + +from .scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact, non-empty identifier. + + Args: + value: Candidate identifier. + field_name: Diagnostic field name. + + Returns: + The validated input value. + + Raises: + ValueError: If the value is empty or has outer whitespace. + """ + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _validate_registered_call_id(value: str) -> str: + """Validate one lowercase, multi-segment extension identifier.""" + _validate_identifier(value, field_name="registered semantic call ID") + if re.fullmatch(r"[a-z][a-z0-9_]*(?:\.[a-z][a-z0-9_]*)+", value) is None: + raise ValueError( + "Registered semantic call IDs must contain two or more lowercase " + "identifier segments separated by single dots." + ) + return value + + +def _snapshot_resources(values: Mapping[str, str]) -> Mapping[str, str]: + """Validate and own a generic slot-to-resource mapping.""" + if not isinstance(values, Mapping): + raise TypeError("resources must be a mapping from slot IDs to resource IDs.") + resources: dict[str, str] = {} + for slot_id, resource_id in values.items(): + _validate_identifier(slot_id, field_name="resource slot IDs") + _validate_identifier(resource_id, field_name="resource IDs") + resources[slot_id] = resource_id + return MappingProxyType(resources) + + +def _validate_static_binding_contract( + contract: SkillBindingContract, + *, + field_name: str, +) -> None: + """Reject runtime-bearing subclasses anywhere in a binding contract.""" + if type(contract) is not SkillBindingContract: + raise TypeError(f"{field_name} must be exactly SkillBindingContract.") + if type(contract.slots) is not tuple or type(contract.constraints) is not tuple: + raise TypeError(f"{field_name} must contain exact immutable tuples.") + for slot in contract.slots: + if type(slot) is not SkillResourceSlot: + raise TypeError( + f"{field_name}.slots must contain exact SkillResourceSlot values." + ) + _validate_identifier(slot.slot_id, field_name=f"{field_name} slot IDs") + if type(slot.endpoints) is not tuple or type(slot.constraints) is not tuple: + raise TypeError(f"{field_name}.slots must contain exact immutable tuples.") + for endpoint in slot.endpoints: + if type(endpoint) is not SkillEndpointRequirement: + raise TypeError( + f"{field_name}.slots.endpoints must contain exact " + "SkillEndpointRequirement values." + ) + _validate_identifier( + endpoint.endpoint_id, + field_name=f"{field_name} endpoint IDs", + ) + if type(endpoint.capabilities) is not frozenset: + raise TypeError( + f"{field_name} endpoint capabilities must be exact frozensets." + ) + for capability in endpoint.capabilities: + _validate_identifier( + capability, + field_name=f"{field_name} endpoint capabilities", + ) + if type(endpoint.required_commands) is not MappingProxyType: + raise TypeError( + f"{field_name} required commands must be an immutable snapshot." + ) + for command_name, command_type in endpoint.required_commands.items(): + _validate_identifier( + command_name, + field_name=f"{field_name} required command names", + ) + if not isinstance(command_type, type): + raise TypeError( + f"{field_name} required command contracts must be class " + "objects." + ) + for constraint in slot.constraints: + if type(constraint) is not DisjointSlotEndpoints: + raise TypeError( + f"{field_name}.slots.constraints must contain exact " + "DisjointSlotEndpoints values." + ) + if type(constraint.endpoint_ids) is not tuple: + raise TypeError( + f"{field_name} endpoint constraints must contain exact tuples." + ) + for endpoint_id in constraint.endpoint_ids: + _validate_identifier( + endpoint_id, + field_name=f"{field_name} constrained endpoint IDs", + ) + for constraint in contract.constraints: + if type(constraint) is not DisjointResourceSlots: + raise TypeError( + f"{field_name}.constraints must contain exact " + "DisjointResourceSlots values." + ) + if type(constraint.slots) is not tuple: + raise TypeError( + f"{field_name} resource constraints must contain exact tuples." + ) + for slot_id in constraint.slots: + _validate_identifier( + slot_id, + field_name=f"{field_name} constrained slot IDs", + ) + + +def _validate_static_skill_descriptor( + descriptor: SkillDescriptor, + *, + field_name: str, +) -> None: + """Validate one exact, provider-free atomic target descriptor.""" + if type(descriptor) is not SkillDescriptor: + raise TypeError(f"{field_name} must be exactly SkillDescriptor.") + _validate_identifier(descriptor.skill_id, field_name=f"{field_name}.skill_id") + if type(descriptor.agent_visible) is not bool: + raise TypeError(f"{field_name}.agent_visible must be exactly bool.") + if type(descriptor.goal_type) is tuple: + if not descriptor.goal_type or not all( + type(goal_type) is type for goal_type in descriptor.goal_type + ): + raise TypeError(f"{field_name}.goal_type must contain exact class objects.") + elif type(descriptor.goal_type) is not type: + raise TypeError( + f"{field_name}.goal_type must be an exact class or tuple of classes." + ) + if type(descriptor.options_type) is not type: + raise TypeError(f"{field_name}.options_type must be an exact class object.") + if descriptor.binding_contract is None: + raise TypeError(f"{field_name}.binding_contract must be declared.") + _validate_static_binding_contract( + descriptor.binding_contract, + field_name=f"{field_name}.binding_contract", + ) + + +@dataclass(frozen=True, slots=True, init=False, eq=False) +class SemanticPose: + """Object-space pose expressed as position and a WXYZ quaternion. + + The value owns normalized tensor snapshots and never exposes its internal + tensors directly. A single pose or an environment batch is accepted. + + Args: + position: Shape ``(3,)`` or ``(B, 3)``. + quaternion_wxyz: Shape ``(4,)`` or ``(B, 4)``. Finite, non-zero + quaternions are normalized at construction. + """ + + _position: torch.Tensor = field(repr=False) + _quaternion_wxyz: torch.Tensor = field(repr=False) + + def __init__( + self, + position: torch.Tensor | tuple[float, float, float] | list[float], + quaternion_wxyz: torch.Tensor | tuple[float, float, float, float] | list[float], + ) -> None: + position_tensor = torch.as_tensor(position, dtype=torch.float32) + quaternion_tensor = torch.as_tensor(quaternion_wxyz, dtype=torch.float32) + if position_tensor.dim() not in (1, 2) or position_tensor.shape[-1] != 3: + raise ValueError("position must have shape (3,) or (B, 3).") + if quaternion_tensor.dim() not in (1, 2) or quaternion_tensor.shape[-1] != 4: + raise ValueError("quaternion_wxyz must have shape (4,) or (B, 4).") + if position_tensor.dim() != quaternion_tensor.dim(): + raise ValueError( + "position and quaternion_wxyz must both be unbatched or batched." + ) + if position_tensor.dim() == 2 and ( + position_tensor.shape[0] != quaternion_tensor.shape[0] + ): + raise ValueError("position and quaternion_wxyz batch sizes must match.") + if position_tensor.dim() == 2 and position_tensor.shape[0] == 0: + raise ValueError("SemanticPose batches must contain at least one pose.") + if not torch.isfinite(position_tensor).all(): + raise ValueError("position must contain only finite values.") + if not torch.isfinite(quaternion_tensor).all(): + raise ValueError("quaternion_wxyz must contain only finite values.") + norms = torch.linalg.vector_norm(quaternion_tensor, dim=-1, keepdim=True) + if torch.any(norms <= torch.finfo(torch.float32).eps): + raise ValueError("quaternion_wxyz must be non-zero.") + object.__setattr__(self, "_position", position_tensor.clone()) + object.__setattr__( + self, + "_quaternion_wxyz", + (quaternion_tensor / norms).clone(), + ) + + @property + def position(self) -> torch.Tensor: + """Return an independent position tensor.""" + return self._position.clone() + + @property + def quaternion_wxyz(self) -> torch.Tensor: + """Return an independent normalized quaternion tensor.""" + return self._quaternion_wxyz.clone() + + @property + def batch_size(self) -> int | None: + """Return the explicit batch size, or ``None`` for one broadcast pose.""" + return None if self._position.dim() == 1 else self._position.shape[0] + + def snapshot(self) -> SemanticPose: + """Return an independently owned pose value.""" + return SemanticPose(self._position, self._quaternion_wxyz) + + def to_matrix(self) -> torch.Tensor: + """Convert the semantic pose to a homogeneous transform. + + Returns: + Shape ``(4, 4)`` for an unbatched pose or ``(B, 4, 4)`` for a + batched pose. + """ + quaternion = self._quaternion_wxyz + was_unbatched = quaternion.dim() == 1 + if was_unbatched: + quaternion = quaternion.unsqueeze(0) + position = self._position.unsqueeze(0) + else: + position = self._position + output = torch.eye( + 4, + dtype=quaternion.dtype, + device=quaternion.device, + ).repeat(quaternion.shape[0], 1, 1) + output[:, :3, :3] = matrix_from_quat(quaternion) + output[:, :3, 3] = position + return output[0] if was_unbatched else output + + +@dataclass(frozen=True, slots=True, kw_only=True, eq=False) +class SemanticCallSpec: + """Base value contract shared by every declarative semantic call. + + Args: + resources: Optional skill-local slot to robot-resource overrides. + """ + + call_kind: ClassVar[str] = "semantic" + + resources: Mapping[str, str] = field(default_factory=dict) + + def __post_init__(self) -> None: + object.__setattr__(self, "resources", _snapshot_resources(self.resources)) + + @property + def semantic_id(self) -> str: + """Return the stable catalog identifier for this call.""" + return self.call_kind + + +@dataclass(frozen=True, slots=True, eq=False) +class Pick(SemanticCallSpec): + """Pick one registered object using an optional explicit grasp affordance. + + Args: + object: Authoritative semantic object reference. + grasp: Optional explicit grasp affordance. Omission requests deterministic + registry selection. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "pick" + + object: SceneObjectRef + grasp: SceneAffordanceRef | None = None + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + if type(self.object) is not SceneObjectRef: + raise TypeError("Pick.object must be a SceneObjectRef.") + if self.grasp is not None and type(self.grasp) is not SceneAffordanceRef: + raise TypeError("Pick.grasp must be a SceneAffordanceRef or None.") + + +PlaceRelationTarget: TypeAlias = SceneObjectRef | SceneAffordanceRef + + +@dataclass(frozen=True, slots=True, eq=False) +class Place(SemanticCallSpec): + """Place a held object at exactly one semantic destination. + + Args: + object: Authoritative held-object reference. + at: Absolute object-space pose. + on: Object or affordance supporting an ``on`` relation. + inside: Object or affordance supporting an ``inside`` relation. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "place" + + object: SceneObjectRef + at: SemanticPose | None = None + on: PlaceRelationTarget | None = None + inside: PlaceRelationTarget | None = None + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + if type(self.object) is not SceneObjectRef: + raise TypeError("Place.object must be a SceneObjectRef.") + destinations = { + "at": self.at, + "on": self.on, + "inside": self.inside, + } + selected = [name for name, value in destinations.items() if value is not None] + if len(selected) != 1: + raise ValueError( + "Place requires exactly one of at, on, or inside; selected " + f"{selected}." + ) + if self.at is not None: + if type(self.at) is not SemanticPose: + raise TypeError("Place.at must be a SemanticPose or None.") + object.__setattr__(self, "at", self.at.snapshot()) + for field_name in ("on", "inside"): + target = getattr(self, field_name) + if target is not None and type(target) not in ( + SceneObjectRef, + SceneAffordanceRef, + ): + raise TypeError( + f"Place.{field_name} must be a SceneObjectRef, " + "SceneAffordanceRef, or None." + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class HandOver(SemanticCallSpec): + """Transfer a held object to another robot resource. + + Args: + object: Authoritative held-object reference. + receiver: Optional destination resource ID. It is equivalent to the + ``destination`` resource slot and must agree with an explicit map. + final_target: Optional final object-space delivery pose. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "hand_over" + + object: SceneObjectRef + receiver: str | None = None + final_target: SemanticPose | None = None + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + if type(self.object) is not SceneObjectRef: + raise TypeError("HandOver.object must be a SceneObjectRef.") + resources = dict(self.resources) + if self.receiver is not None: + _validate_identifier(self.receiver, field_name="HandOver.receiver") + selected = resources.get("destination") + if selected is not None and selected != self.receiver: + raise ValueError( + "HandOver.receiver conflicts with resources['destination']." + ) + resources["destination"] = self.receiver + object.__setattr__(self, "resources", _snapshot_resources(resources)) + if self.final_target is not None: + if type(self.final_target) is not SemanticPose: + raise TypeError("HandOver.final_target must be a SemanticPose or None.") + object.__setattr__( + self, + "final_target", + self.final_target.snapshot(), + ) + + +DeclarativeValue: TypeAlias = ( + None + | bool + | int + | float + | str + | SceneEntityRef + | SemanticPose + | tuple["DeclarativeValue", ...] + | Mapping[str, "DeclarativeValue"] +) + + +def _snapshot_declarative_value( + value: object, + *, + path: str, + _active: set[int] | None = None, + _budget: list[int] | None = None, + _depth: int = 0, +) -> DeclarativeValue: + """Recursively own a bounded, acyclic, non-executable payload.""" + if _active is None: + _active = set() + if _budget is None: + _budget = [4096] + if _depth > 32: + raise ValueError(f"{path} exceeds the maximum declarative depth of 32.") + _budget[0] -= 1 + if _budget[0] < 0: + raise ValueError(f"{path} exceeds the maximum declarative node count.") + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{path} must be finite.") + return value + if type(value) in ( + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + ): + return value + if type(value) is SemanticPose: + snapshot = value.snapshot() + if type(snapshot) is not SemanticPose or snapshot is value: + raise TypeError( + f"{path}.snapshot() must return an independent SemanticPose." + ) + return snapshot + if type(value) in (dict, MappingProxyType): + container_id = id(value) + if container_id in _active: + raise ValueError(f"{path} contains a cyclic declarative mapping.") + _active.add(container_id) + try: + snapshot: dict[str, DeclarativeValue] = {} + for key, nested in value.items(): + _validate_identifier(key, field_name=f"{path} keys") + snapshot[key] = _snapshot_declarative_value( + nested, + path=f"{path}.{key}", + _active=_active, + _budget=_budget, + _depth=_depth + 1, + ) + return MappingProxyType(snapshot) + finally: + _active.remove(container_id) + if type(value) in (tuple, list): + container_id = id(value) + if container_id in _active: + raise ValueError(f"{path} contains a cyclic declarative sequence.") + _active.add(container_id) + try: + return tuple( + _snapshot_declarative_value( + nested, + path=f"{path}[{index}]", + _active=_active, + _budget=_budget, + _depth=_depth + 1, + ) + for index, nested in enumerate(value) + ) + finally: + _active.remove(container_id) + raise TypeError( + f"{path} contains non-declarative {type(value).__name__}; callables, " + "classes, modules, tensors, and live objects are not allowed." + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class RegisteredSemanticCall(SemanticCallSpec): + """Safe value payload for a catalog-registered semantic extension. + + Args: + call_id: Stable extension identifier discovered in a semantic catalog. + arguments: Nested declarative data. Executable or live values are + rejected at construction. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "registered" + + call_id: str + arguments: Mapping[str, DeclarativeValue] = field(default_factory=dict) + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + _validate_registered_call_id(self.call_id) + if type(self.arguments) not in (dict, MappingProxyType): + raise TypeError( + "RegisteredSemanticCall.arguments must be an exact dict or " + "immutable mapping proxy." + ) + object.__setattr__( + self, + "arguments", + _snapshot_declarative_value( + self.arguments, + path="RegisteredSemanticCall.arguments", + ), + ) + + @property + def semantic_id(self) -> str: + """Return the registered extension identifier.""" + return self.call_id + + +@dataclass(frozen=True, slots=True) +class SemanticCallDescriptor: + """Static catalog metadata for one semantic call kind. + + Args: + call_id: Stable semantic call identifier. + spec_type: Exact public call value type. + schema_version: Explicit configuration payload schema version. + target_descriptor: Exact atomic goal/options/resource contract. It is + inferred and non-overridable for curated calls and required for + registered extensions. + """ + + call_id: str + spec_type: type[SemanticCallSpec] + schema_version: int = 1 + target_descriptor: SkillDescriptor | None = None + + def __post_init__(self) -> None: + _validate_identifier(self.call_id, field_name="SemanticCallDescriptor.call_id") + if self.spec_type not in (Pick, Place, HandOver, RegisteredSemanticCall): + raise TypeError( + "spec_type must be exactly Pick, Place, HandOver, or " + "RegisteredSemanticCall; extensions use the registered payload " + "contract rather than executable call subclasses." + ) + if not isinstance(self.schema_version, int) or isinstance( + self.schema_version, bool + ): + raise TypeError("schema_version must be an integer.") + if self.schema_version != 1: + raise ValueError( + "Unsupported semantic call schema_version " + f"{self.schema_version}; supported versions are [1]." + ) + if self.spec_type is not RegisteredSemanticCall and ( + self.call_id != self.spec_type.call_kind + ): + raise ValueError( + f"Descriptor ID {self.call_id!r} must match " + f"{self.spec_type.__name__}.call_kind " + f"{self.spec_type.call_kind!r}." + ) + if self.spec_type is not RegisteredSemanticCall: + expected = _builtin_call_target(self.spec_type) + if ( + self.target_descriptor is not None + and self.target_descriptor != expected + ): + raise ValueError( + f"Built-in semantic call {self.call_id!r} must target skill " + f"{expected.skill_id!r} with its exact curated descriptor. " + "Use RegisteredSemanticCall for extensions." + ) + object.__setattr__(self, "target_descriptor", expected) + else: + if self.target_descriptor is None: + raise TypeError( + "Registered semantic descriptors require target_descriptor." + ) + _validate_static_skill_descriptor( + self.target_descriptor, + field_name="SemanticCallDescriptor.target_descriptor", + ) + if ( + not self.target_descriptor.agent_visible + or self.target_descriptor.binding_contract is None + ): + raise ValueError( + "Registered target_descriptor must be agent-visible and " + "declare a binding contract." + ) + if self.spec_type is RegisteredSemanticCall and self.call_id in { + Pick.call_kind, + Place.call_kind, + HandOver.call_kind, + RegisteredSemanticCall.call_kind, + }: + raise ValueError( + f"Registered semantic call ID {self.call_id!r} is reserved." + ) + if self.spec_type is RegisteredSemanticCall: + _validate_registered_call_id(self.call_id) + + @property + def skill_id(self) -> str: + """Return the atomic skill ID from the canonical target descriptor.""" + assert self.target_descriptor is not None + return self.target_descriptor.skill_id + + @property + def binding_contract(self) -> SkillBindingContract: + """Return the resource contract from the canonical target descriptor.""" + assert self.target_descriptor is not None + contract = self.target_descriptor.binding_contract + assert contract is not None + return contract + + +@dataclass(frozen=True, slots=True, init=False) +class SemanticCallCatalog: + """Immutable discovery catalog separated from engine installation.""" + + _descriptors: Mapping[str, SemanticCallDescriptor] + + def __init__( + self, + descriptors: Iterable[SemanticCallDescriptor], + ) -> None: + if isinstance(descriptors, (str, bytes)): + raise TypeError("descriptors must be an iterable of descriptors.") + try: + supplied = tuple(descriptors) + except TypeError as exc: + raise TypeError("descriptors must be an iterable of descriptors.") from exc + normalized: dict[str, SemanticCallDescriptor] = {} + for descriptor in supplied: + if type(descriptor) is not SemanticCallDescriptor: + raise TypeError( + "descriptors must contain exact SemanticCallDescriptor values." + ) + if descriptor.call_id in normalized: + raise ValueError(f"Duplicate semantic call ID {descriptor.call_id!r}.") + normalized[descriptor.call_id] = descriptor + object.__setattr__( + self, + "_descriptors", + MappingProxyType(normalized), + ) + + @property + def descriptors(self) -> Mapping[str, SemanticCallDescriptor]: + """Return immutable descriptors keyed by exact semantic ID.""" + return self._descriptors + + def discover( + self, + call: str | SemanticCallSpec, + ) -> SemanticCallDescriptor: + """Discover metadata without installing or executing an implementation. + + Args: + call: Exact semantic ID or a call value. + + Returns: + Matching immutable descriptor. + + Raises: + KeyError: If the exact call ID is unknown. + TypeError: If the call type disagrees with its descriptor. + """ + if type(call) is str: + call_id = _validate_identifier(call, field_name="semantic call ID") + call_value = None + elif type(call) in (Pick, Place, HandOver, RegisteredSemanticCall): + call_id = call.semantic_id + call_value = call + else: + raise TypeError( + "call must be an exact semantic call ID or supported call value." + ) + descriptor = self._descriptors.get(call_id) + if descriptor is None: + raise KeyError( + f"Unknown semantic call {call_id!r}; available calls are " + f"{sorted(self._descriptors)}." + ) + if call_value is not None and type(call_value) is not descriptor.spec_type: + raise TypeError( + f"Semantic call {call_id!r} expects " + f"{descriptor.spec_type.__name__}, got " + f"{type(call_value).__name__}." + ) + return descriptor + + def with_descriptor( + self, + descriptor: SemanticCallDescriptor, + ) -> SemanticCallCatalog: + """Return a new catalog containing one additional descriptor.""" + return SemanticCallCatalog((*self._descriptors.values(), descriptor)) + + +def _builtin_call_target( + spec_type: type[SemanticCallSpec], +) -> SkillDescriptor: + """Return the non-overridable atomic target for one curated call type.""" + from embodichain.lab.sim.atomic_actions.primitives.hand_over import ( + HandOver as HandOverAction, + ) + from embodichain.lab.sim.atomic_actions.primitives.pick_up import PickUp + from embodichain.lab.sim.atomic_actions.primitives.place import Place as PlaceAction + + targets = { + Pick: PickUp.descriptor(), + Place: PlaceAction.descriptor(), + HandOver: HandOverAction.descriptor(), + } + try: + return targets[spec_type] + except KeyError as exc: + raise TypeError(f"Unsupported curated call type {spec_type!r}.") from exc + + +def builtin_semantic_call_catalog() -> SemanticCallCatalog: + """Build the curated catalog for installed manipulation primitives. + + Returns: + A fresh immutable catalog. Atomic implementations remain uninstalled; + callers bind them to an engine through the separate runtime path. + """ + descriptors = tuple( + SemanticCallDescriptor( + call_id=spec_type.call_kind, + spec_type=spec_type, + ) + for spec_type in (Pick, Place, HandOver) + ) + return SemanticCallCatalog(descriptors) + + +__all__ = [ + "DeclarativeValue", + "HandOver", + "Pick", + "Place", + "PlaceRelationTarget", + "RegisteredSemanticCall", + "SemanticCallCatalog", + "SemanticCallDescriptor", + "SemanticCallSpec", + "SemanticPose", + "builtin_semantic_call_catalog", +] diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py new file mode 100644 index 000000000..04093b7f2 --- /dev/null +++ b/embodichain/lab/sim/skills/compiler.py @@ -0,0 +1,1333 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Static workflow analysis and JIT semantic-call lowering.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, field +from enum import Enum +from types import MappingProxyType +from typing import ClassVar +from uuid import uuid4 + +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionControlOverrides, + ActionInvocation, + ActionOptions, + Affordance, + GraspGoal, + HandOverOptions, + JointPositionTarget, + HeldObjectState, + PickUpOptions, + PlaceGoal, + PlaceOptions, + PlanningContext, + PoseGoalValue, + SceneEntityPose, +) +from .calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, + SemanticPose, +) +from .integration import ( + BoundSemanticCall, + BoundSemanticIntegration, + PathPart, + SemanticDiagnostic, + SemanticValidationError, +) +from .scene import ( + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneObjectRef, +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _diagnostic( + code: str, + path: tuple[PathPart, ...], + message: str, + candidates: tuple[str, ...] = (), +) -> SemanticValidationError: + """Build one pathful semantic compiler error.""" + return SemanticValidationError(SemanticDiagnostic(code, path, message, candidates)) + + +class SemanticEffectKind(str, Enum): + """Symbolic effect boundary inferred for a semantic call.""" + + ATTACH = "attach" + RELEASE = "release" + TRANSFER = "transfer" + REGISTERED = "registered" + + +@dataclass(frozen=True, slots=True) +class SemanticRelationTarget: + """Statically selected relation affordance awaiting typed grounding.""" + + capability: str + affordance: SceneAffordanceRef + payload_type: type[Affordance] + payload_revision: str + + def __post_init__(self) -> None: + _validate_identifier(self.capability, field_name="relation capability") + if type(self.affordance) is not SceneAffordanceRef: + raise TypeError("affordance must be exactly SceneAffordanceRef.") + if not isinstance(self.payload_type, type) or not issubclass( + self.payload_type, Affordance + ): + raise TypeError("payload_type must be an Affordance subclass.") + _validate_identifier( + self.payload_revision, + field_name="relation payload_revision", + ) + + @property + def grounder_key(self) -> tuple[str, type[Affordance], str]: + """Return the exact typed/versioned grounder lookup key.""" + return self.capability, self.payload_type, self.payload_revision + + +class RelationTargetGrounder(ABC): + """Shared implementation that converts one relation into object pose.""" + + capability: ClassVar[str] + affordance_type: ClassVar[type[Affordance]] + affordance_revision: ClassVar[str] + + @abstractmethod + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> PoseGoalValue: + """Return an object-space target from current state and typed payload. + + Args: + relation: Statically selected relation metadata. + affordance: Owned exact-type affordance payload. + context: Latest immutable planning observation. + + Returns: + Direct or scene-relative desired object pose. + """ + + +@dataclass(frozen=True, slots=True) +class SemanticObjectTarget: + """One object-space look-ahead target. + + Relation targets remain late-bound and require an explicitly installed + typed/versioned grounder. Handover targets defer to the + embodiment-selected provider and are used only for workflow look-ahead. + """ + + value: ( + SemanticPose | SceneEntityPose | SemanticRelationTarget | SemanticHandOverTarget + ) + + def __post_init__(self) -> None: + if type(self.value) in (SemanticPose, SceneEntityPose): + object.__setattr__(self, "value", self.value.snapshot()) + elif type(self.value) not in ( + SemanticRelationTarget, + SemanticHandOverTarget, + ): + raise TypeError( + "value must be exactly SemanticPose, SceneEntityPose, " + "SemanticRelationTarget, or SemanticHandOverTarget." + ) + + @property + def pose(self) -> SemanticPose | SceneEntityPose | None: + """Return the direct pose variant, when selected.""" + if type(self.value) in (SemanticPose, SceneEntityPose): + return self.value # type: ignore[return-value] + return None + + @property + def relation(self) -> SemanticRelationTarget | None: + """Return the relation variant, when selected.""" + return self.value if type(self.value) is SemanticRelationTarget else None + + @property + def handover(self) -> SemanticHandOverTarget | None: + """Return the deferred handover variant, when selected.""" + return self.value if type(self.value) is SemanticHandOverTarget else None + + +@dataclass(frozen=True, slots=True) +class SemanticHandOverTarget: + """Deferred middle pose selected by one named embodiment provider.""" + + provider_id: str + bound: BoundSemanticCall + + def __post_init__(self) -> None: + _validate_identifier(self.provider_id, field_name="handover provider_id") + if type(self.bound) is not BoundSemanticCall: + raise TypeError("bound must be exactly BoundSemanticCall.") + if type(self.bound.linked.call) is not HandOver: + raise TypeError("bound must contain an exact HandOver call.") + + +@dataclass(frozen=True, slots=True) +class SemanticEffectDependency: + """A consumer's verified-held-state dependency on an earlier call.""" + + producer_index: int | None + consumer_index: int + object: SceneObjectRef + + def __post_init__(self) -> None: + if self.producer_index is not None and ( + type(self.producer_index) is not int or self.producer_index < 0 + ): + raise ValueError("producer_index must be non-negative or None.") + if type(self.consumer_index) is not int or self.consumer_index < 0: + raise ValueError("consumer_index must be non-negative.") + if self.producer_index is not None and ( + self.producer_index >= self.consumer_index + ): + raise ValueError("producer_index must precede consumer_index.") + if type(self.object) is not SceneObjectRef: + raise TypeError("object must be exactly SceneObjectRef.") + + +@dataclass(frozen=True, slots=True) +class AnalyzedSemanticCall: + """One statically linked call plus workflow-derived lowering metadata.""" + + index: int + bound: BoundSemanticCall + effect_kind: SemanticEffectKind + downstream_object_target: SemanticObjectTarget | None = None + + def __post_init__(self) -> None: + if type(self.index) is not int or self.index < 0: + raise ValueError("index must be a non-negative integer.") + if type(self.bound) is not BoundSemanticCall: + raise TypeError("bound must be exactly BoundSemanticCall.") + if not isinstance(self.effect_kind, SemanticEffectKind): + raise TypeError("effect_kind must be a SemanticEffectKind.") + if self.downstream_object_target is not None and ( + type(self.downstream_object_target) is not SemanticObjectTarget + ): + raise TypeError( + "downstream_object_target must be exactly SemanticObjectTarget " + "or None." + ) + + @property + def call(self) -> SemanticCallSpec: + """Return the canonical linked semantic call.""" + return self.bound.linked.call + + +@dataclass(frozen=True, slots=True) +class SemanticWorkflow: + """Immutable result of static workflow analysis.""" + + workflow_id: str + calls: tuple[AnalyzedSemanticCall, ...] + effect_dependencies: tuple[SemanticEffectDependency, ...] = () + _compiler_id: str = field(repr=False, compare=False, default="") + + def __post_init__(self) -> None: + _validate_identifier(self.workflow_id, field_name="workflow_id") + calls = tuple(self.calls) + if not calls: + raise ValueError("SemanticWorkflow requires at least one call.") + if not all(type(call) is AnalyzedSemanticCall for call in calls): + raise TypeError("calls must contain exact AnalyzedSemanticCall values.") + if tuple(call.index for call in calls) != tuple(range(len(calls))): + raise ValueError("SemanticWorkflow call indices must be contiguous.") + dependencies = tuple(self.effect_dependencies) + if not all( + type(dependency) is SemanticEffectDependency for dependency in dependencies + ): + raise TypeError( + "effect_dependencies must contain exact " + "SemanticEffectDependency values." + ) + _validate_identifier(self._compiler_id, field_name="compiler_id") + object.__setattr__(self, "calls", calls) + object.__setattr__(self, "effect_dependencies", dependencies) + + +@dataclass(frozen=True, slots=True) +class SemanticLowering: + """Registered-lowerer output wrapped by compiler-owned invocation policy.""" + + goal: object + skill_options: ActionOptions | None = None + control_overrides: ActionControlOverrides = field( + default_factory=ActionControlOverrides + ) + + def __post_init__(self) -> None: + if self.skill_options is not None and not isinstance( + self.skill_options, ActionOptions + ): + raise TypeError("skill_options must be an ActionOptions or None.") + if type(self.control_overrides) is not ActionControlOverrides: + raise TypeError("control_overrides must be exactly ActionControlOverrides.") + + +class RegisteredSemanticLowerer(ABC): + """Explicitly installed implementation for one registered call ID.""" + + call_id: ClassVar[str] + schema_version: ClassVar[int] + + @abstractmethod + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> SemanticLowering: + """Lower one registered value to goal/options without changing policy.""" + + +@dataclass(frozen=True, slots=True) +class HandOverPoseTargets: + """Embodiment-owned object-space poses needed by the core handover skill.""" + + middle: SemanticObjectTarget + final: SemanticObjectTarget + + def __post_init__(self) -> None: + if type(self.middle) is not SemanticObjectTarget: + raise TypeError("middle must be exactly SemanticObjectTarget.") + if type(self.final) is not SemanticObjectTarget: + raise TypeError("final must be exactly SemanticObjectTarget.") + + +class HandOverPoseProvider(ABC): + """Integration extension that selects robot-appropriate handover poses.""" + + provider_id: ClassVar[str] + + @abstractmethod + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Return middle and final object-space targets for one handover. + + Args: + call: Canonical handover semantic value. + context: Latest immutable planning observation. + bound: Engine/profile-bound handover call. + + Returns: + Embodiment-appropriate middle and final object targets. + """ + + +@dataclass(frozen=True, slots=True) +class GroundedSemanticCall: + """Call lowered from the latest observed context.""" + + analyzed: AnalyzedSemanticCall + invocation: ActionInvocation + _eligible_mask: torch.Tensor = field(repr=False, compare=False) + + def __post_init__(self) -> None: + if type(self.analyzed) is not AnalyzedSemanticCall: + raise TypeError("analyzed must be exactly AnalyzedSemanticCall.") + if type(self.invocation) is not ActionInvocation: + raise TypeError("invocation must be exactly ActionInvocation.") + if self.invocation.skill_id != self.analyzed.bound.linked.descriptor.skill_id: + raise ValueError("invocation skill_id must match the analyzed call.") + 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: + raise ValueError("eligible_mask must be a one-dimensional bool tensor.") + if self._eligible_mask.numel() == 0: + raise ValueError("eligible_mask must contain at least one environment.") + object.__setattr__(self, "_eligible_mask", self._eligible_mask.clone()) + + @property + def eligible_mask(self) -> torch.Tensor: + """Return an owned mask that the execution session must preserve.""" + return self._eligible_mask.clone() + + +class SemanticSkillCompiler: + """Analyze semantic workflows and JIT-lower exactly one call at a time.""" + + def __init__( + self, + integration: BoundSemanticIntegration, + *, + registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), + relation_grounders: Iterable[RelationTargetGrounder] = (), + handover_pose_providers: Iterable[HandOverPoseProvider] = (), + ) -> None: + """Install immutable semantic lowering and grounding registries. + + Args: + integration: Exact live scene, engine, and robot-profile binding. + registered_lowerers: Explicit implementations for registered calls. + relation_grounders: Exact capability/payload/revision dispatch entries. + handover_pose_providers: Named embodiment-owned handover providers. + """ + if type(integration) is not BoundSemanticIntegration: + raise TypeError("integration must be exactly BoundSemanticIntegration.") + if isinstance(registered_lowerers, (str, bytes)): + raise TypeError("registered_lowerers must be an iterable of lowerers.") + try: + supplied_lowerers = tuple(registered_lowerers) + except TypeError as exc: + raise TypeError( + "registered_lowerers must be an iterable of lowerers." + ) from exc + lowerers: dict[str, RegisteredSemanticLowerer] = {} + for lowerer in supplied_lowerers: + if not isinstance(lowerer, RegisteredSemanticLowerer): + raise TypeError( + "registered_lowerers must contain RegisteredSemanticLowerer " + "instances." + ) + call_id = _validate_identifier( + getattr(type(lowerer), "call_id", None), + field_name="RegisteredSemanticLowerer.call_id", + ) + if call_id in lowerers: + raise ValueError(f"Duplicate registered lowerer {call_id!r}.") + try: + descriptor = integration.manifest.call_catalog.discover(call_id) + except KeyError as exc: + raise ValueError( + f"Lowerer {call_id!r} has no registered semantic descriptor." + ) from exc + if descriptor.spec_type is not RegisteredSemanticCall: + raise ValueError( + f"Lowerer {call_id!r} cannot replace curated call semantics." + ) + schema_version = getattr(type(lowerer), "schema_version", None) + if type(schema_version) is not int or ( + schema_version != descriptor.schema_version + ): + raise ValueError( + f"Lowerer {call_id!r} schema_version must exactly match " + f"descriptor version {descriptor.schema_version}." + ) + lowerers[call_id] = lowerer + if isinstance(relation_grounders, (str, bytes)): + raise TypeError("relation_grounders must be an iterable of grounders.") + try: + supplied_grounders = tuple(relation_grounders) + except TypeError as exc: + raise TypeError( + "relation_grounders must be an iterable of grounders." + ) from exc + normalized_grounders: dict[ + tuple[str, type[Affordance], str], RelationTargetGrounder + ] = {} + for grounder in supplied_grounders: + if not isinstance(grounder, RelationTargetGrounder): + raise TypeError( + "relation_grounders must contain RelationTargetGrounder " + "instances." + ) + grounder_type = type(grounder) + capability = _validate_identifier( + getattr(grounder_type, "capability", None), + field_name="RelationTargetGrounder.capability", + ) + affordance_type = getattr(grounder_type, "affordance_type", None) + if not isinstance(affordance_type, type) or not issubclass( + affordance_type, Affordance + ): + raise TypeError( + "RelationTargetGrounder.affordance_type must be an " + "Affordance subclass." + ) + revision = _validate_identifier( + getattr(grounder_type, "affordance_revision", None), + field_name="RelationTargetGrounder.affordance_revision", + ) + key = (capability, affordance_type, revision) + if key in normalized_grounders: + raise ValueError(f"Duplicate relation grounder key {key!r}.") + normalized_grounders[key] = grounder + if isinstance(handover_pose_providers, (str, bytes)): + raise TypeError("handover_pose_providers must be an iterable of providers.") + try: + supplied_handover_providers = tuple(handover_pose_providers) + except TypeError as exc: + raise TypeError( + "handover_pose_providers must be an iterable of providers." + ) from exc + normalized_handover_providers: dict[str, HandOverPoseProvider] = {} + for provider in supplied_handover_providers: + if not isinstance(provider, HandOverPoseProvider): + raise TypeError( + "handover_pose_providers must contain " + "HandOverPoseProvider instances." + ) + provider_id = _validate_identifier( + getattr(type(provider), "provider_id", None), + field_name="HandOverPoseProvider.provider_id", + ) + if provider_id in normalized_handover_providers: + raise ValueError(f"Duplicate handover pose provider {provider_id!r}.") + normalized_handover_providers[provider_id] = provider + self._integration = integration + self._compiler_id = uuid4().hex + self._registered_lowerers = MappingProxyType(lowerers) + self._relation_grounders = MappingProxyType(normalized_grounders) + self._handover_pose_providers = MappingProxyType(normalized_handover_providers) + + @property + def integration(self) -> BoundSemanticIntegration: + """Return the exact live integration used for linking and grounding.""" + return self._integration + + @property + def registered_lowerers(self) -> Mapping[str, RegisteredSemanticLowerer]: + """Return installed registered-call lowerers by stable call ID.""" + return self._registered_lowerers + + @property + def relation_grounders( + self, + ) -> Mapping[tuple[str, type[Affordance], str], RelationTargetGrounder]: + """Return exact typed/versioned relation grounders.""" + return self._relation_grounders + + @property + def handover_pose_providers(self) -> Mapping[str, HandOverPoseProvider]: + """Return installed handover pose providers by stable provider ID.""" + return self._handover_pose_providers + + def analyze( + self, + calls: Iterable[SemanticCallSpec], + *, + workflow_id: str = "semantic_workflow", + path: tuple[PathPart, ...] = ("workflow",), + ) -> SemanticWorkflow: + """Statically link calls and infer look-ahead/effect dependencies. + + Args: + calls: Ordered exact semantic call values. + workflow_id: Stable caller-selected workflow identifier. + path: Root diagnostic path. + + Returns: + Factory-owned provider-free workflow analysis. + + Raises: + SemanticValidationError: If linking, grounding capabilities, or + object-state flow are invalid. + """ + _validate_identifier(workflow_id, field_name="workflow_id") + self._assert_current(path=("integration", "robot_profile")) + if isinstance(calls, (str, bytes)): + raise TypeError("calls must be an iterable of semantic call values.") + try: + supplied = tuple(calls) + except TypeError as exc: + raise TypeError( + "calls must be an iterable of semantic call values." + ) from exc + if not supplied: + raise ValueError("Semantic workflow requires at least one call.") + allowed_types = (Pick, Place, HandOver, RegisteredSemanticCall) + if not all(type(call) in allowed_types for call in supplied): + raise TypeError("calls must contain exact supported semantic call values.") + + bound_calls: list[BoundSemanticCall] = [] + for index, call in enumerate(supplied): + if type(call) is RegisteredSemanticCall and ( + call.call_id not in self._registered_lowerers + ): + raise _diagnostic( + "semantic_lowerer_not_installed", + (*path, index, "kind"), + f"Registered semantic call {call.call_id!r} has no explicitly " + "installed compiler lowerer.", + tuple(self._registered_lowerers), + ) + bound_calls.append( + self._integration.link_call( + call, + path=(*path, index, "call"), + ) + ) + for index, bound in enumerate(bound_calls): + call = bound.linked.call + if type(call) is HandOver: + self._require_handover_pose_provider( + call, + path=(*path, index, "call"), + ) + if type(call) is Place and call.at is None: + target = self._relation_target(bound) + assert target.relation is not None + destination_metadata = self._integration.manifest.scene.lookup( + target.relation.affordance, + expected_type=SceneAffordanceRef, + ) + if destination_metadata.parent == call.object: + raise _diagnostic( + "place_self_reference", + (*path, index, "call", "destination"), + f"Object {call.object.entity_id!r} cannot be placed in a " + "relation to its own affordance.", + ) + self._require_relation_grounder( + target.relation, + path=(*path, index, "call", "destination"), + ) + + dependencies: list[SemanticEffectDependency] = [] + latest_holder: dict[str, tuple[int, str]] = {} + analyzed: list[AnalyzedSemanticCall] = [] + for index, bound in enumerate(bound_calls): + call = bound.linked.call + if type(call) is Pick: + previous = latest_holder.get(call.object.entity_id) + if previous is not None: + raise _diagnostic( + "invalid_object_state_flow", + (*path, index, "call", "object"), + f"Object {call.object.entity_id!r} is already acquired by " + f"call {previous[0]} without an intervening release.", + ) + effect_kind = SemanticEffectKind.ATTACH + latest_holder[call.object.entity_id] = ( + index, + bound.binding.resource_ids["primary"], + ) + elif type(call) is Place: + producer = latest_holder.get(call.object.entity_id) + selected_resource = bound.binding.resource_ids["primary"] + if producer is not None and producer[1] != selected_resource: + raise _diagnostic( + "held_resource_mismatch", + (*path, index, "call", "resources", "primary"), + f"Place selects resource {selected_resource!r}, but the " + f"verified producer selects {producer[1]!r}.", + (producer[1],), + ) + effect_kind = SemanticEffectKind.RELEASE + dependencies.append( + SemanticEffectDependency( + producer_index=None if producer is None else producer[0], + consumer_index=index, + object=call.object, + ) + ) + latest_holder.pop(call.object.entity_id, None) + elif type(call) is HandOver: + producer = latest_holder.get(call.object.entity_id) + source_resource = bound.binding.resource_ids["source"] + if producer is not None and producer[1] != source_resource: + raise _diagnostic( + "held_resource_mismatch", + (*path, index, "call", "resources", "source"), + f"HandOver selects source {source_resource!r}, but the " + f"verified producer selects {producer[1]!r}.", + (producer[1],), + ) + effect_kind = SemanticEffectKind.TRANSFER + dependencies.append( + SemanticEffectDependency( + producer_index=None if producer is None else producer[0], + consumer_index=index, + object=call.object, + ) + ) + latest_holder[call.object.entity_id] = ( + index, + bound.binding.resource_ids["destination"], + ) + else: + effect_kind = SemanticEffectKind.REGISTERED + # A registered extension has no declarative state-flow contract + # in Version 1. Treat it as an opaque effect boundary. + latest_holder.clear() + downstream_target = ( + self._downstream_target(index, bound_calls) + if type(call) is Pick + else None + ) + analyzed.append( + AnalyzedSemanticCall( + index=index, + bound=bound, + effect_kind=effect_kind, + downstream_object_target=downstream_target, + ) + ) + return SemanticWorkflow( + workflow_id=workflow_id, + calls=tuple(analyzed), + effect_dependencies=tuple(dependencies), + _compiler_id=self._compiler_id, + ) + + def ground( + self, + workflow: SemanticWorkflow, + call_index: int, + context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, + revision: int = 0, + path: tuple[PathPart, ...] = ("workflow",), + ) -> GroundedSemanticCall: + """Lower one analyzed call from the latest immutable observation. + + Args: + workflow: Workflow created by this compiler. + call_index: Zero-based call index to lower. + context: Latest immutable planning observation. + eligible_mask: Rows still eligible to execute this call. + revision: Monotonic revision for re-grounding the same invocation. + path: Root diagnostic path. + + Returns: + Invocation and execution eligibility. + + Raises: + SemanticValidationError: If workflow ownership, live integration, + grounding, or verified state is invalid. + """ + if type(workflow) is not SemanticWorkflow: + raise TypeError("workflow must be exactly SemanticWorkflow.") + if type(call_index) is not int or not 0 <= call_index < len(workflow.calls): + raise IndexError(f"call_index {call_index!r} is outside the workflow.") + if type(context) is not PlanningContext: + raise TypeError("context must be exactly PlanningContext.") + if type(revision) is not int or revision < 0: + raise ValueError("revision must be a non-negative integer.") + self._assert_workflow_current(workflow, path=path) + self._integration.engine._validate_context(context) + eligible = self._normalize_eligible_mask(eligible_mask, context) + analyzed = workflow.calls[call_index] + call = analyzed.call + if type(call) is Pick: + lowering = self._lower_pick(analyzed, context) + elif type(call) is Place: + lowering = self._lower_place(analyzed, context, eligible, path=path) + elif type(call) is HandOver: + lowering = self._lower_handover(analyzed, context, eligible, path=path) + elif type(call) is RegisteredSemanticCall: + lowering = self._lower_registered(analyzed, context, path=path) + else: # pragma: no cover - exact workflow construction prevents this + raise AssertionError(f"Unsupported analyzed call {type(call).__name__}.") + + bound = analyzed.bound + invocation = ActionInvocation( + skill_id=bound.linked.descriptor.skill_id, + goal=lowering.goal, + binding=bound.binding.action_binding, + motion_policy=bound.preset.motion_policy, + recovery_policy=bound.preset.recovery_policy, + skill_options=lowering.skill_options, + control_overrides=lowering.control_overrides, + invocation_id=f"{workflow.workflow_id}:{call_index}", + revision=revision, + ) + return GroundedSemanticCall( + analyzed=analyzed, + invocation=invocation, + _eligible_mask=eligible, + ) + + def _assert_current(self, *, path: tuple[PathPart, ...]) -> None: + """Reject a compiler after engine profile/catalog ownership changes.""" + engine = self._integration.engine + if engine.skill_profile is not self._integration.robot_profile: + raise _diagnostic( + "semantic_profile_stale", + path, + "The engine's canonical robot profile changed after compiler " + "construction.", + ) + try: + _ = self._integration.robot_profile.skills + except RuntimeError as exc: + raise _diagnostic( + "semantic_catalog_stale", + path, + str(exc), + ) from exc + + def _assert_workflow_current( + self, + workflow: SemanticWorkflow, + *, + path: tuple[PathPart, ...], + ) -> None: + """Ensure a workflow belongs to this still-current engine revision.""" + self._assert_current(path=("integration", "robot_profile")) + if workflow._compiler_id != self._compiler_id: + raise _diagnostic( + "semantic_program_stale", + path, + "The workflow belongs to a different compiler/grounder registry.", + ) + + @staticmethod + def _normalize_eligible_mask( + eligible_mask: torch.Tensor | None, + context: PlanningContext, + ) -> torch.Tensor: + """Return one owned per-row eligibility mask.""" + if eligible_mask is None: + return torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ) + if not isinstance(eligible_mask, torch.Tensor): + raise TypeError("eligible_mask must be a torch.Tensor or None.") + if eligible_mask.dtype != torch.bool or eligible_mask.shape != ( + context.batch_size, + ): + raise ValueError( + "eligible_mask must be a bool tensor matching the context batch." + ) + if eligible_mask.device != context.robot.qpos.device: + raise ValueError("eligible_mask must use the context device.") + return eligible_mask.clone() + + def _downstream_target( + self, + pick_index: int, + bound_calls: list[BoundSemanticCall], + ) -> SemanticObjectTarget | None: + """Return the first target at which the picked object is released.""" + pick = bound_calls[pick_index].linked.call + assert type(pick) is Pick + object_id = pick.object.entity_id + for call_index, bound in enumerate( + bound_calls[pick_index + 1 :], + start=pick_index + 1, + ): + call = bound.linked.call + if type(call) is RegisteredSemanticCall: + break + call_object = getattr(call, "object", None) + if type(call_object) is not SceneObjectRef or ( + call_object.entity_id != object_id + ): + continue + if type(call) is Pick: + break + if type(call) is HandOver: + provider_id, _ = self._require_handover_pose_provider( + call, + path=("workflow", call_index, "call"), + ) + return SemanticObjectTarget( + SemanticHandOverTarget( + provider_id=provider_id, + bound=bound, + ) + ) + if type(call) is Place: + if call.at is not None: + return SemanticObjectTarget(call.at) + return self._relation_target(bound) + return None + + def _lower_pick( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + ) -> SemanticLowering: + """Lower object-centric pickup and its downstream look-ahead.""" + call = analyzed.call + assert type(call) is Pick + grasp_ref = analyzed.bound.linked.affordances.get("grasp") + if grasp_ref is None: + raise AssertionError("Linked pick call lacks a grasp affordance.") + semantics = self._integration.scene_registry.object_semantics( + call.object, + affordance=grasp_ref, + ) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + skill_options=PickUpOptions( + downstream_object_target_poses=( + () + if analyzed.downstream_object_target is None + else ( + self._ground_object_target( + analyzed.downstream_object_target, + context, + ), + ) + ), + ), + ) + + def _lower_place( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + eligible: torch.Tensor, + *, + path: tuple[PathPart, ...], + ) -> SemanticLowering: + """Convert an object-space place target using verified held state.""" + call = analyzed.call + assert type(call) is Place + control_part, held = self._require_held_object( + analyzed, + context, + eligible, + slot_id="primary", + path=(*path, analyzed.index, "call"), + ) + del control_part + if call.at is not None: + object_target = self._broadcast_pose( + call.at.to_matrix(), + context, + name="Place.at", + ) + xpos: PoseGoalValue = torch.bmm(object_target, held.object_to_eef) + else: + object_target = self._ground_object_target( + self._relation_target(analyzed.bound), + context, + ) + xpos = self._compose_object_to_eef( + object_target, held.object_to_eef, context + ) + return SemanticLowering(goal=PlaceGoal(xpos=xpos), skill_options=PlaceOptions()) + + def _lower_handover( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + eligible: torch.Tensor, + *, + path: tuple[PathPart, ...], + ) -> SemanticLowering: + """Lower handover through an explicitly installed embodiment provider.""" + call = analyzed.call + assert type(call) is HandOver + self._require_held_object( + analyzed, + context, + eligible, + slot_id="source", + path=(*path, analyzed.index, "call"), + ) + _, provider = self._require_handover_pose_provider( + call, + path=(*path, analyzed.index, "call"), + ) + targets = self._resolve_handover_targets( + provider, + call, + context=context, + bound=analyzed.bound, + ) + grasp_ref = analyzed.bound.linked.affordances.get("receiver_grasp") + if grasp_ref is None: + raise AssertionError("Linked handover lacks receiver grasp affordance.") + semantics = self._integration.scene_registry.object_semantics( + call.object, + affordance=grasp_ref, + ) + middle = self._ground_object_target(targets.middle, context) + final_target = ( + SemanticObjectTarget(call.final_target) + if call.final_target is not None + else targets.final + ) + final = self._ground_object_target(final_target, context) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + skill_options=HandOverOptions( + middle_object_pose=middle, + final_object_pose=final, + ), + ) + + def _lower_registered( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> SemanticLowering: + """Invoke one explicitly installed registered-call lowerer.""" + call = analyzed.call + assert type(call) is RegisteredSemanticCall + lowerer = self._registered_lowerers.get(call.call_id) + if lowerer is None: + raise _diagnostic( + "semantic_lowerer_not_installed", + (*path, analyzed.index, "call", "kind"), + f"No lowerer is installed for {call.call_id!r}.", + tuple(self._registered_lowerers), + ) + lowering = lowerer.lower( + call, + context=context, + bound=analyzed.bound, + ) + if type(lowering) is not SemanticLowering: + raise TypeError( + "RegisteredSemanticLowerer.lower() must return exactly " + "SemanticLowering." + ) + descriptor = analyzed.bound.linked.descriptor + target = descriptor.target_descriptor + assert target is not None + expected_goal_types = ( + target.goal_type + if isinstance(target.goal_type, tuple) + else (target.goal_type,) + ) + if type(lowering.goal) not in expected_goal_types: + raise TypeError( + f"Lowerer {call.call_id!r} produced {type(lowering.goal).__name__}; " + f"target skill {target.skill_id!r} expects {target.goal_type!r}." + ) + if lowering.skill_options is not None and ( + type(lowering.skill_options) is not target.options_type + ): + raise TypeError( + f"Lowerer {call.call_id!r} produced incompatible skill options." + ) + return lowering + + def _relation_target( + self, + bound: BoundSemanticCall, + ) -> SemanticObjectTarget: + """Describe a linked placement relation without observing providers.""" + call = bound.linked.call + assert type(call) is Place and call.at is None + capability = ( + PLACE_ON_AFFORDANCE_CAPABILITY + if call.on is not None + else PLACE_IN_AFFORDANCE_CAPABILITY + ) + affordance_ref = bound.linked.affordances.get("destination") + if affordance_ref is None: + raise AssertionError("Linked relation place lacks destination affordance.") + metadata = self._integration.manifest.scene.lookup( + affordance_ref, + expected_type=SceneAffordanceRef, + ) + if ( + metadata.affordance_payload_type is None + or metadata.affordance_revision is None + ): + raise AssertionError( + "Capability-bearing relation affordance lacks payload metadata." + ) + return SemanticObjectTarget( + SemanticRelationTarget( + capability=capability, + affordance=affordance_ref, + payload_type=metadata.affordance_payload_type, + payload_revision=metadata.affordance_revision, + ) + ) + + def _require_relation_grounder( + self, + relation: SemanticRelationTarget | None, + *, + path: tuple[PathPart, ...], + ) -> RelationTargetGrounder: + """Resolve one exact relation grounder or fail during static analysis.""" + assert relation is not None + grounder = self._relation_grounders.get(relation.grounder_key) + if grounder is None: + candidates = tuple( + f"{capability}:{payload_type.__name__}:{revision}" + for capability, payload_type, revision in self._relation_grounders + ) + raise _diagnostic( + "relation_grounder_not_installed", + path, + "No relation target grounder is installed for " + f"{relation.capability!r}, {relation.payload_type.__name__}, " + f"revision {relation.payload_revision!r}.", + candidates, + ) + return grounder + + def _ground_object_target( + self, + target: SemanticObjectTarget, + context: PlanningContext, + ) -> PoseGoalValue: + """Ground a direct pose or dispatch one typed relation grounder.""" + if type(target.pose) is SemanticPose: + return target.pose.to_matrix() + if type(target.pose) is SceneEntityPose: + return target.pose + deferred_handover = target.handover + if deferred_handover is not None: + call = deferred_handover.bound.linked.call + assert type(call) is HandOver + provider_id, provider = self._require_handover_pose_provider( + call, + path=("handover", "provider"), + ) + if provider_id != deferred_handover.provider_id: + raise _diagnostic( + "semantic_program_stale", + ("handover", "provider"), + "The profile-selected handover provider changed after " + "workflow analysis.", + ) + targets = self._resolve_handover_targets( + provider, + call, + context=context, + bound=deferred_handover.bound, + ) + return self._ground_object_target(targets.middle, context) + relation = target.relation + assert relation is not None + grounder = self._require_relation_grounder( + relation, + path=("relation", relation.affordance.entity_id), + ) + registration = self._integration.scene_registry.lookup( + relation.affordance, + expected_type=SceneAffordanceRef, + ) + affordance = registration.affordance + assert affordance is not None + if ( + type(affordance) is not relation.payload_type + or registration.affordance_revision != relation.payload_revision + or relation.capability not in registration.affordance_capabilities + ): + raise TypeError( + "Semantic relation target does not match the exact live " + "affordance type, capability, and revision." + ) + pose_goal = grounder.ground( + relation, + affordance=affordance, + context=context, + ) + if type(pose_goal) is not SceneEntityPose and not isinstance( + pose_goal, torch.Tensor + ): + raise TypeError( + "RelationTargetGrounder.ground() must return a torch.Tensor or " + "exact SceneEntityPose." + ) + return pose_goal + + def _require_handover_pose_provider( + self, + call: HandOver, + *, + path: tuple[PathPart, ...], + ) -> tuple[str, HandOverPoseProvider]: + """Resolve the profile-selected named handover grounding provider.""" + provider_id = ( + self._integration.robot_profile.source_profile.grounding_providers.get( + call.semantic_id + ) + ) + if provider_id is None: + raise _diagnostic( + "handover_grounding_unconfigured", + path, + "The robot profile must select a named grounding provider for " + f"semantic call {call.semantic_id!r}.", + tuple(self._handover_pose_providers), + ) + provider = self._handover_pose_providers.get(provider_id) + if provider is None: + raise _diagnostic( + "handover_grounding_provider_not_installed", + path, + f"Robot profile selects handover provider {provider_id!r}, but " + "the compiler did not install it.", + tuple(self._handover_pose_providers), + ) + return provider_id, provider + + @staticmethod + def _resolve_handover_targets( + provider: HandOverPoseProvider, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Run one provider and reject recursive deferred target values.""" + targets = provider.resolve(call, context=context, bound=bound) + if type(targets) is not HandOverPoseTargets: + raise TypeError( + "HandOverPoseProvider.resolve() must return exactly " + "HandOverPoseTargets." + ) + if targets.middle.handover is not None or targets.final.handover is not None: + raise TypeError( + "HandOverPoseProvider targets cannot recursively defer to another " + "handover provider." + ) + return targets + + def _compose_object_to_eef( + self, + object_target: PoseGoalValue, + object_to_eef: torch.Tensor, + context: PlanningContext, + ) -> PoseGoalValue: + """Compose a relation-grounded object target with verified held state.""" + if isinstance(object_target, torch.Tensor): + return torch.bmm( + self._broadcast_pose(object_target, context, name="relation target"), + object_to_eef, + ) + relative = object_target.relative_pose + if relative is None: + composed = object_to_eef.clone() + else: + composed = torch.bmm( + self._broadcast_pose(relative, context, name="relation offset"), + object_to_eef, + ) + return SceneEntityPose( + object_target.entity_id, + relative_pose=composed, + minimum_confidence=object_target.minimum_confidence, + ) + + def _require_held_object( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + eligible: torch.Tensor, + *, + slot_id: str, + path: tuple[PathPart, ...], + ) -> tuple[str, HeldObjectState]: + """Resolve the motion control part and verify its held-object identity.""" + endpoint = analyzed.bound.binding.action_binding.endpoint(slot_id, "motion") + try: + target = endpoint.require_target(JointPositionTarget) + except TypeError as exc: + raise _diagnostic( + "unsupported_builtin_endpoint", + (*path, "resources", slot_id, "motion"), + "The current built-in semantic lowerer requires a joint-position " + "motion endpoint.", + ) from exc + held = context.task.get_held_object(target.control_part) + call_object = getattr(analyzed.call, "object", None) + assert type(call_object) is SceneObjectRef + if held is None or held.semantics.entity_id != call_object.entity_id: + raise _diagnostic( + "verified_held_object_required", + (*path, "object"), + f"Call requires verified object {call_object.entity_id!r} held by " + f"{target.control_part!r}.", + ) + assert held.env_mask is not None + missing = eligible & ~held.env_mask + if missing.any(): + missing_env_ids = tuple( + str(value) + for value in context.env_ids[missing].detach().to("cpu").tolist() + ) + raise _diagnostic( + "verified_held_object_required", + (*path, "object"), + f"Object {call_object.entity_id!r} is not verified as held in " + "every eligible environment.", + missing_env_ids, + ) + return target.control_part, held + + @staticmethod + def _broadcast_pose( + pose: torch.Tensor, + context: PlanningContext, + *, + name: str, + ) -> torch.Tensor: + """Move and broadcast one object-space pose to the planning batch.""" + pose = pose.to(device=context.robot.qpos.device, dtype=torch.float32) + if pose.shape == (4, 4): + return pose.unsqueeze(0).expand(context.batch_size, -1, -1).clone() + if pose.shape != (context.batch_size, 4, 4): + raise ValueError( + f"{name} must have shape (4, 4) or " f"({context.batch_size}, 4, 4)." + ) + return pose.clone() + + +__all__ = [ + "AnalyzedSemanticCall", + "GroundedSemanticCall", + "HandOverPoseProvider", + "HandOverPoseTargets", + "RelationTargetGrounder", + "RegisteredSemanticLowerer", + "SemanticEffectDependency", + "SemanticEffectKind", + "SemanticHandOverTarget", + "SemanticLowering", + "SemanticObjectTarget", + "SemanticRelationTarget", + "SemanticSkillCompiler", + "SemanticWorkflow", +] diff --git a/embodichain/lab/sim/skills/integration.py b/embodichain/lab/sim/skills/integration.py new file mode 100644 index 000000000..68e0134cf --- /dev/null +++ b/embodichain/lab/sim/skills/integration.py @@ -0,0 +1,1063 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Two-phase static and live semantic integration validation.""" + +from __future__ import annotations + +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, field, replace +from types import MappingProxyType +from typing import TypeVar + +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + DisjointResourceSlots, + DisjointSlotEndpoints, + SkillResourceSlot, +) + +from .calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticCallSpec, +) +from .profiles import ( + BoundRobotSkillProfile, + ControlPartEndpoint, + ResolvedSkillBinding, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from .scene import ( + GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionWorldMode, + SceneEntityMetadata, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, + _SceneMetadataIndex, +) + +PathPart = str | int +RefT = TypeVar("RefT", bound=SceneEntityRef) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact, non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _render_path(path: tuple[PathPart, ...]) -> str: + """Render tuple path components in configuration notation.""" + output = "" + for part in path: + if isinstance(part, int): + output += f"[{part}]" + elif not output: + output = part + else: + output += f".{part}" + return output or "" + + +@dataclass(frozen=True, slots=True) +class SemanticDiagnostic: + """Structured deterministic semantic-integration diagnostic. + + Args: + code: Stable machine-readable failure code. + path: Complete configuration or program path. + message: Human-readable explanation. + candidates: Canonical candidate IDs, sorted when applicable. + """ + + code: str + path: tuple[PathPart, ...] + message: str + candidates: tuple[str, ...] = () + + def __post_init__(self) -> None: + _validate_identifier(self.code, field_name="SemanticDiagnostic.code") + if isinstance(self.path, (str, bytes)): + raise TypeError("SemanticDiagnostic.path must be a tuple of components.") + path = tuple(self.path) + if not all( + (isinstance(part, str) and part) + or (isinstance(part, int) and not isinstance(part, bool)) + for part in path + ): + raise ValueError("SemanticDiagnostic.path contains an invalid component.") + object.__setattr__(self, "path", path) + if not isinstance(self.message, str) or not self.message: + raise ValueError("SemanticDiagnostic.message must be non-empty.") + candidates = tuple(self.candidates) + if not all(isinstance(candidate, str) for candidate in candidates): + raise TypeError("SemanticDiagnostic.candidates must contain strings.") + object.__setattr__(self, "candidates", tuple(sorted(candidates))) + + @property + def rendered_path(self) -> str: + """Return the path in dotted/indexed notation.""" + return _render_path(self.path) + + +class SemanticValidationError(ValueError): + """Raise one structured error at a static or live integration boundary.""" + + def __init__(self, diagnostic: SemanticDiagnostic) -> None: + if not isinstance(diagnostic, SemanticDiagnostic): + raise TypeError("diagnostic must be a SemanticDiagnostic.") + self.diagnostic = diagnostic + super().__init__(f"{diagnostic.rendered_path}: {diagnostic.message}") + + +@dataclass(frozen=True, slots=True) +class SceneEntityManifest(SceneEntityMetadata): + """Static scene declaration sharing the canonical metadata value model.""" + + @classmethod + def from_metadata(cls, metadata: SceneEntityMetadata) -> SceneEntityManifest: + """Copy one canonical provider-free metadata value.""" + if not isinstance(metadata, SceneEntityMetadata): + raise TypeError("metadata must be a SceneEntityMetadata.") + return cls( + ref=metadata.ref, + aliases=metadata.aliases, + parent=metadata.parent, + native_name=metadata.native_name, + dynamics=metadata.dynamics, + collision_role=metadata.collision_role, + semantic_type=metadata.semantic_type, + affordance_capabilities=metadata.affordance_capabilities, + default_affordances=metadata.default_affordances, + affordance_payload_type=metadata.affordance_payload_type, + affordance_revision=metadata.affordance_revision, + relative_pose=metadata.relative_pose, + ) + + +@dataclass(frozen=True, slots=True, init=False) +class SceneManifest: + """Immutable provider-free scene catalog used before simulation starts.""" + + _entries: tuple[SceneEntityManifest, ...] + _index: _SceneMetadataIndex + collision_world_mode: SceneCollisionWorldMode | None + + def __init__( + self, + entries: Iterable[SceneEntityManifest] = (), + *, + collision_world_mode: SceneCollisionWorldMode | None = None, + ) -> None: + if isinstance(entries, (str, bytes)): + raise TypeError("entries must be an iterable of scene manifests.") + try: + supplied = tuple(entries) + except TypeError as exc: + raise TypeError("entries must be an iterable of scene manifests.") from exc + if not all(type(entry) is SceneEntityManifest for entry in supplied): + raise TypeError("entries must contain exact SceneEntityManifest values.") + if collision_world_mode is not None and not isinstance( + collision_world_mode, + SceneCollisionWorldMode, + ): + raise TypeError( + "collision_world_mode must be a SceneCollisionWorldMode or None." + ) + object.__setattr__(self, "_entries", supplied) + object.__setattr__(self, "_index", _SceneMetadataIndex(supplied)) + object.__setattr__(self, "collision_world_mode", collision_world_mode) + + @property + def entries(self) -> tuple[SceneEntityManifest, ...]: + """Return immutable provider-free entries in declaration order.""" + return self._entries + + @classmethod + def from_registry(cls, registry: SceneRegistry) -> SceneManifest: + """Project a live registry without observing any dynamic provider.""" + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + return cls( + ( + SceneEntityManifest.from_metadata(metadata) + for metadata in registry.entity_metadata + ), + collision_world_mode=registry.collision_world_mode, + ) + + def resolve( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + path: tuple[PathPart, ...] = (), + ) -> RefT: + """Resolve one canonical or alias reference with pathful diagnostics.""" + if isinstance(identifier, SceneEntityRef): + candidate_id = identifier.entity_id + supplied_type: type[SceneEntityRef] | None = type(identifier) + elif isinstance(identifier, str): + _validate_identifier(identifier, field_name="scene identifier") + candidate_id = self._index.aliases.get(identifier, identifier) + supplied_type = None + else: + raise SemanticValidationError( + SemanticDiagnostic( + "invalid_entity_reference", + path, + "Expected a scene identifier or typed scene reference.", + ) + ) + entry = self._index.by_id.get(candidate_id) + if entry is None: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_entity", + path, + f"Unknown scene entity {candidate_id!r}.", + tuple(self._index.by_id), + ) + ) + if supplied_type is not None and supplied_type is not type(entry.ref): + raise SemanticValidationError( + SemanticDiagnostic( + "entity_type_mismatch", + path, + f"Scene entity {candidate_id!r} is " + f"{type(entry.ref).__name__}, not {supplied_type.__name__}.", + ) + ) + if not isinstance(entry.ref, expected_type): + raise SemanticValidationError( + SemanticDiagnostic( + "entity_type_mismatch", + path, + f"Scene entity {candidate_id!r} is " + f"{type(entry.ref).__name__}, not {expected_type.__name__}.", + ) + ) + return entry.ref # type: ignore[return-value] + + def lookup( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + path: tuple[PathPart, ...] = (), + ) -> SceneEntityManifest: + """Return one static entry after canonical typed resolution.""" + ref = self.resolve(identifier, expected_type=expected_type, path=path) + return self._index.by_id[ref.entity_id] # type: ignore[return-value] + + def resolve_affordance( + self, + parent: str | SceneEntityRef, + *, + capability: str, + explicit: str | SceneAffordanceRef | None = None, + path: tuple[PathPart, ...] = (), + ) -> SceneAffordanceRef: + """Resolve one affordance using the same strict rule as SceneRegistry.""" + parent_ref = self.resolve(parent, path=path) + _validate_identifier(capability, field_name="affordance capability") + candidates = self._index.affordances_by_parent_capability.get( + (parent_ref.entity_id, capability), + (), + ) + if explicit is not None: + selected = self.resolve( + explicit, + expected_type=SceneAffordanceRef, + path=path, + ) + entry = self._index.by_id[selected.entity_id] + if entry.parent != parent_ref: + raise SemanticValidationError( + SemanticDiagnostic( + "affordance_parent_mismatch", + path, + f"Affordance {selected.entity_id!r} is not a direct child " + f"of {parent_ref.entity_id!r}.", + tuple(candidate.entity_id for candidate in candidates), + ) + ) + if capability not in entry.affordance_capabilities: + raise SemanticValidationError( + SemanticDiagnostic( + "unsupported_affordance", + path, + f"Affordance {selected.entity_id!r} does not support " + f"{capability!r}.", + tuple(candidate.entity_id for candidate in candidates), + ) + ) + return selected + if not candidates: + raise SemanticValidationError( + SemanticDiagnostic( + "missing_affordance", + path, + f"Scene entity {parent_ref.entity_id!r} has no affordance for " + f"{capability!r}.", + ) + ) + if len(candidates) == 1: + return candidates[0] + parent_entry = self._index.by_id[parent_ref.entity_id] + default = parent_entry.default_affordances.get(capability) + if default is not None: + return default + raise SemanticValidationError( + SemanticDiagnostic( + "ambiguous_affordance", + path, + f"Multiple affordances support {capability!r}; configure a " + "scoped default or select one explicitly.", + tuple(candidate.entity_id for candidate in candidates), + ) + ) + + def validate_registry( + self, + registry: SceneRegistry, + *, + path: tuple[PathPart, ...] = ("integration", "scene_registry"), + ) -> None: + """Require a live registry to match this provider-free declaration.""" + try: + live = SceneManifest.from_registry(registry) + except Exception as exc: # noqa: BLE001 - normalize integration failures + raise SemanticValidationError( + SemanticDiagnostic( + "invalid_scene_registry", + path, + f"Could not project the live scene registry: {exc}", + ) + ) from exc + static_ids = set(self._index.by_id) + live_ids = set(live._index.by_id) + if static_ids != live_ids: + raise SemanticValidationError( + SemanticDiagnostic( + "scene_manifest_mismatch", + path, + "Live scene IDs differ from the static manifest; " + f"missing={sorted(static_ids - live_ids)}, " + f"extra={sorted(live_ids - static_ids)}.", + ) + ) + for entity_id in sorted(static_ids): + if self._index.by_id[entity_id] != live._index.by_id[entity_id]: + raise SemanticValidationError( + SemanticDiagnostic( + "scene_manifest_mismatch", + (*path, entity_id), + "Live scene metadata differs from the static manifest.", + ) + ) + if self.collision_world_mode is not live.collision_world_mode: + raise SemanticValidationError( + SemanticDiagnostic( + "scene_manifest_mismatch", + (*path, "collision_world_mode"), + "Live collision-world mode differs from the static manifest.", + ) + ) + + +@dataclass(frozen=True, slots=True) +class LinkedSemanticCall: + """Provider-free static link result for one semantic call.""" + + call: SemanticCallSpec + descriptor: SemanticCallDescriptor + preset_id: str + affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) + + def __post_init__(self) -> None: + if type(self.call) not in (Pick, Place, HandOver, RegisteredSemanticCall): + raise TypeError("call must be an exact supported semantic call value.") + if type(self.descriptor) is not SemanticCallDescriptor: + raise TypeError("descriptor must be exactly SemanticCallDescriptor.") + if type(self.call) is not self.descriptor.spec_type or ( + self.call.semantic_id != self.descriptor.call_id + ): + raise ValueError( + "call type and semantic ID must match the linked descriptor." + ) + _validate_identifier(self.preset_id, field_name="LinkedSemanticCall.preset_id") + if not isinstance(self.affordances, Mapping): + raise TypeError("affordances must be a mapping.") + normalized: dict[str, SceneAffordanceRef] = {} + for role, affordance in self.affordances.items(): + _validate_identifier(role, field_name="affordance roles") + if type(affordance) is not SceneAffordanceRef: + raise TypeError("affordances values must be SceneAffordanceRef values.") + normalized[role] = affordance + object.__setattr__(self, "affordances", MappingProxyType(normalized)) + + +@dataclass(frozen=True, slots=True) +class BoundSemanticCall: + """Call linked to one installed engine/profile combination.""" + + linked: LinkedSemanticCall + binding: ResolvedSkillBinding + preset: SkillPolicyPreset + _robot_profile: BoundRobotSkillProfile = field(repr=False, compare=False) + + def __post_init__(self) -> None: + """Validate the static and live ownership links.""" + if not isinstance(self.linked, LinkedSemanticCall): + raise TypeError("linked must be a LinkedSemanticCall.") + if not isinstance(self.binding, ResolvedSkillBinding): + raise TypeError("binding must be a ResolvedSkillBinding.") + if not isinstance(self.preset, SkillPolicyPreset): + raise TypeError("preset must be a SkillPolicyPreset.") + if self.binding.skill_id != self.linked.descriptor.skill_id: + raise ValueError( + "binding skill_id must match the linked semantic descriptor." + ) + if self.preset.preset_id != self.linked.preset_id: + raise ValueError("preset ID must match the statically linked preset.") + if not isinstance(self._robot_profile, BoundRobotSkillProfile): + raise TypeError("robot_profile must be a BoundRobotSkillProfile.") + if ( + self.binding.action_binding.owner_id + != self._robot_profile.engine.binding_owner_id + ): + raise ValueError("binding belongs to a different action engine.") + + @property + def robot_profile(self) -> BoundRobotSkillProfile: + """Return the exact bound profile that produced this call.""" + return self._robot_profile + + +@dataclass(frozen=True, slots=True) +class SemanticIntegrationManifest: + """Static scene/profile/catalog declaration validated before execution. + + Args: + scene: Provider-free scene manifest. + robot_profile: Declarative robot resource/profile snapshot. + call_catalog: Discoverable semantic call descriptors. + runtime_preset: Optional integration-wide policy preset override. + """ + + scene: SceneManifest + robot_profile: RobotSkillProfile + call_catalog: SemanticCallCatalog + runtime_preset: str | None = None + + def __post_init__(self) -> None: + if type(self.scene) is not SceneManifest: + raise TypeError("scene must be exactly SceneManifest.") + if type(self.robot_profile) is not RobotSkillProfile: + raise TypeError("robot_profile must be exactly RobotSkillProfile.") + if type(self.call_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + if self.runtime_preset is not None: + _validate_identifier( + self.runtime_preset, + field_name="runtime_preset", + ) + if self.runtime_preset not in self.robot_profile.presets: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_preset", + ("integration", "runtime_preset"), + f"Unknown runtime preset {self.runtime_preset!r}.", + tuple(self.robot_profile.presets), + ) + ) + + def link_call( + self, + call: SemanticCallSpec, + *, + path: tuple[PathPart, ...] = ("call",), + ) -> LinkedSemanticCall: + """Resolve static refs, affordances, and declared resource structure. + + This method never observes scene providers, constructs an engine, + samples a grasp, or runs a planner. + """ + if not isinstance(call, SemanticCallSpec): + raise TypeError("call must be a SemanticCallSpec.") + try: + descriptor = self.call_catalog.discover(call) + except (KeyError, TypeError, ValueError) as exc: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_call", + (*path, "kind"), + str(exc), + tuple(self.call_catalog.descriptors), + ) + ) from exc + + affordances: dict[str, SceneAffordanceRef] = {} + if isinstance(call, Pick): + object_ref = self.scene.resolve( + call.object, + expected_type=SceneObjectRef, + path=(*path, "object"), + ) + grasp = self.scene.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + explicit=call.grasp, + path=(*path, "grasp"), + ) + normalized_call: SemanticCallSpec = replace( + call, + object=object_ref, + grasp=grasp, + ) + affordances["grasp"] = grasp + elif isinstance(call, Place): + object_ref = self.scene.resolve( + call.object, + expected_type=SceneObjectRef, + path=(*path, "object"), + ) + replacements: dict[str, object] = {"object": object_ref} + if call.on is not None: + destination, affordance = self._link_relation( + call.on, + capability=PLACE_ON_AFFORDANCE_CAPABILITY, + path=(*path, "on"), + ) + replacements["on"] = destination + affordances["destination"] = affordance + elif call.inside is not None: + destination, affordance = self._link_relation( + call.inside, + capability=PLACE_IN_AFFORDANCE_CAPABILITY, + path=(*path, "inside"), + ) + replacements["inside"] = destination + affordances["destination"] = affordance + normalized_call = replace(call, **replacements) + elif isinstance(call, HandOver): + object_ref = self.scene.resolve( + call.object, + expected_type=SceneObjectRef, + path=(*path, "object"), + ) + grasp = self.scene.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + path=(*path, "object", "handover_grasp"), + ) + normalized_call = replace(call, object=object_ref) + affordances["receiver_grasp"] = grasp + elif isinstance(call, RegisteredSemanticCall): + normalized_call = replace( + call, + arguments=self._normalize_registered_arguments( + call.arguments, + path=(*path, "arguments"), + ), + ) + else: # defensive for future subclasses not represented by the catalog + raise SemanticValidationError( + SemanticDiagnostic( + "unsupported_call_type", + path, + f"No static linker exists for {type(call).__name__}.", + ) + ) + self._validate_declared_resources( + descriptor, + normalized_call.resources, + path=(*path, "resources"), + ) + preset_id = self._resolve_declared_preset( + descriptor, + path=(*path, "preset"), + ) + return LinkedSemanticCall( + call=normalized_call, + descriptor=descriptor, + preset_id=preset_id, + affordances=affordances, + ) + + def _resolve_declared_preset( + self, + descriptor: SemanticCallDescriptor, + *, + path: tuple[PathPart, ...], + ) -> str: + """Resolve the static integration/per-skill/profile preset ID.""" + preset_id = self.runtime_preset + if preset_id is None: + preset_id = self.robot_profile.skill_presets.get(descriptor.skill_id) + if preset_id is None: + preset_id = self.robot_profile.default_preset + if preset_id is None: + raise SemanticValidationError( + SemanticDiagnostic( + "missing_preset", + path, + f"No policy preset is configured for skill " + f"{descriptor.skill_id!r}.", + tuple(self.robot_profile.presets), + ) + ) + if preset_id not in self.robot_profile.presets: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_preset", + path, + f"Unknown policy preset {preset_id!r}.", + tuple(self.robot_profile.presets), + ) + ) + return preset_id + + def _normalize_registered_arguments( + self, + value: object, + *, + path: tuple[PathPart, ...], + ) -> object: + """Canonicalize every typed scene ref in a registered payload.""" + if type(value) in ( + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + ): + return self.scene.resolve( + value, + expected_type=type(value), + path=path, + ) + # Other exact scene-ref variants are admitted by the call value + # contract and resolved through their exact runtime type here. + if isinstance(value, SceneEntityRef): + return self.scene.resolve( + value, + expected_type=type(value), + path=path, + ) + if isinstance(value, Mapping): + return MappingProxyType( + { + key: self._normalize_registered_arguments( + nested, + path=(*path, key), + ) + for key, nested in value.items() + } + ) + if isinstance(value, tuple): + return tuple( + self._normalize_registered_arguments( + nested, + path=(*path, index), + ) + for index, nested in enumerate(value) + ) + return value + + def _link_relation( + self, + target: SceneObjectRef | SceneAffordanceRef, + *, + capability: str, + path: tuple[PathPart, ...], + ) -> tuple[SceneObjectRef | SceneAffordanceRef, SceneAffordanceRef]: + """Normalize one placement relation and select its affordance.""" + if isinstance(target, SceneObjectRef): + parent = self.scene.resolve( + target, + expected_type=SceneObjectRef, + path=path, + ) + affordance = self.scene.resolve_affordance( + parent, + capability=capability, + path=path, + ) + return parent, affordance + explicit = self.scene.resolve( + target, + expected_type=SceneAffordanceRef, + path=path, + ) + entry = self.scene.lookup(explicit, path=path) + assert entry.parent is not None + affordance = self.scene.resolve_affordance( + entry.parent, + capability=capability, + explicit=explicit, + path=path, + ) + return explicit, affordance + + def _validate_declared_resources( + self, + descriptor: SemanticCallDescriptor, + selections: Mapping[str, str], + *, + path: tuple[PathPart, ...], + ) -> None: + """Validate resource IDs and obvious capability mismatches statically.""" + contract = descriptor.binding_contract + default = self.robot_profile.defaults.get(descriptor.skill_id) + if default is not None: + expected_slots = set(contract.slot_ids) + default_slots = set(default.resources) + unknown_default_resources = sorted( + set(default.resources.values()) - set(self.robot_profile.resources) + ) + if default_slots != expected_slots or unknown_default_resources: + raise SemanticValidationError( + SemanticDiagnostic( + "invalid_default_binding", + ( + "integration", + "robot_profile", + "defaults", + descriptor.skill_id, + ), + "Default resource binding must cover the exact skill slots " + "and reference known resources.", + contract.slot_ids, + ) + ) + unknown_slots = sorted(set(selections) - set(contract.slot_ids)) + if unknown_slots: + slot = unknown_slots[0] + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_resource_slot", + (*path, slot), + f"Skill {descriptor.skill_id!r} has no resource slot {slot!r}.", + contract.slot_ids, + ) + ) + unknown_resources = sorted( + set(selections.values()) - set(self.robot_profile.resources) + ) + if unknown_resources: + unknown = unknown_resources[0] + slot = next(key for key, value in selections.items() if value == unknown) + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_resource", + (*path, slot), + f"Unknown robot resource {unknown!r}.", + tuple(self.robot_profile.resources), + ) + ) + for slot in contract.slots: + selected = selections.get(slot.slot_id) + if default is not None: + default_resource = self.robot_profile.resources[ + default.resources[slot.slot_id] + ] + if not self._resource_declares_requirements(default_resource, slot): + raise SemanticValidationError( + SemanticDiagnostic( + "invalid_default_binding", + ( + "integration", + "robot_profile", + "defaults", + descriptor.skill_id, + slot.slot_id, + ), + f"Default resource {default_resource.resource_id!r} " + f"does not satisfy slot {slot.slot_id!r}.", + ) + ) + candidates = tuple( + resource + for resource in self.robot_profile.resources.values() + if (selected is None or resource.resource_id == selected) + and self._resource_declares_requirements(resource, slot) + ) + if not candidates: + code = ( + "unsupported_resource" + if selected is not None + else "unsupported_skill" + ) + raise SemanticValidationError( + SemanticDiagnostic( + code, + (*path, slot.slot_id), + f"No declared robot resource satisfies slot " + f"{slot.slot_id!r} for skill {descriptor.skill_id!r}.", + tuple(self.robot_profile.resources), + ) + ) + effective_selections: dict[str, str] = {} + if default is not None: + effective_selections.update(default.resources) + effective_selections.update(selections) + for constraint in contract.constraints: + if not isinstance(constraint, DisjointResourceSlots) or not all( + slot_id in effective_selections for slot_id in constraint.slots + ): + continue + resources = [ + self.robot_profile.resources[effective_selections[slot_id]] + for slot_id in constraint.slots + ] + leaf_sets = [ + self._declared_resource_leaves(resource) for resource in resources + ] + for index, left in enumerate(leaf_sets): + if any(left & right for right in leaf_sets[index + 1 :]): + raise SemanticValidationError( + SemanticDiagnostic( + "resource_claim_conflict", + path, + f"Selected resources for slots {list(constraint.slots)} " + "share declared physical leaves.", + tuple(resource.resource_id for resource in resources), + ) + ) + + def _declared_resource_leaves(self, resource: RobotResource) -> frozenset[str]: + """Return transitive leaves from the static profile resource DAG.""" + if not resource.members: + return frozenset({resource.resource_id}) + leaves: set[str] = set() + for member_id in resource.members: + leaves.update( + self._declared_resource_leaves(self.robot_profile.resources[member_id]) + ) + return frozenset(leaves) + + def _resource_declares_requirements( + self, + resource: RobotResource, + slot: SkillResourceSlot, + ) -> bool: + """Check provider-free endpoint declarations without physical binding.""" + endpoints: dict[str, ResourceEndpoint] = {} + for requirement in slot.endpoints: + endpoint = resource.endpoints.get(requirement.endpoint_id) + if endpoint is None or not requirement.capabilities.issubset( + endpoint.capabilities + ): + return False + if requirement.required_commands and isinstance( + endpoint, ControlPartEndpoint + ): + profile_id = endpoint.command_profile or endpoint.control_part + command_profile = self.robot_profile.command_profiles.get(profile_id) + if command_profile is None: + return False + if any( + not isinstance(command_profile.commands.get(name), command_type) + for name, command_type in requirement.required_commands.items() + ): + return False + endpoints[requirement.endpoint_id] = endpoint + # Adapter claims are unavailable before live binding. For the built-in + # endpoint, equal control parts are an exact static conflict. + for constraint in slot.constraints: + if not isinstance(constraint, DisjointSlotEndpoints): + continue + constrained = [endpoints[name] for name in constraint.endpoint_ids] + for index, left in enumerate(constrained): + if not isinstance(left, ControlPartEndpoint): + continue + if any( + isinstance(right, ControlPartEndpoint) + and left.control_part == right.control_part + for right in constrained[index + 1 :] + ): + return False + return True + + def bind( + self, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + *, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + ) -> BoundSemanticIntegration: + """Validate live scene and robot bindings without observing or planning.""" + self.scene.validate_registry(scene_registry) + try: + bound_profile = engine.bind_skill_profile( + self.robot_profile, + endpoint_adapters=endpoint_adapters, + ) + except Exception as exc: # noqa: BLE001 - add semantic integration path + raise SemanticValidationError( + SemanticDiagnostic( + "robot_profile_binding_failed", + ("integration", "robot_profile"), + str(exc), + ) + ) from exc + return BoundSemanticIntegration( + manifest=self, + scene_registry=scene_registry, + robot_profile=bound_profile, + engine=engine, + ) + + +class BoundSemanticIntegration: + """Live-installed, still side-effect-free semantic integration link.""" + + def __init__( + self, + *, + manifest: SemanticIntegrationManifest, + scene_registry: SceneRegistry, + robot_profile: BoundRobotSkillProfile, + engine: AtomicActionEngine, + ) -> None: + if type(manifest) is not SemanticIntegrationManifest: + raise TypeError("manifest must be exactly SemanticIntegrationManifest.") + if not isinstance(scene_registry, SceneRegistry): + raise TypeError("scene_registry must be a SceneRegistry.") + if type(robot_profile) is not BoundRobotSkillProfile: + raise TypeError("robot_profile must be exactly BoundRobotSkillProfile.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if robot_profile.engine is not engine: + raise ValueError("robot_profile belongs to a different engine.") + if engine.skill_profile is not robot_profile: + raise ValueError( + "robot_profile must be the canonical profile installed on engine." + ) + if robot_profile.source_profile is not manifest.robot_profile: + raise ValueError( + "robot_profile does not match the semantic integration manifest." + ) + self._manifest = manifest + self._scene_registry = scene_registry + self._robot_profile = robot_profile + self._engine = engine + + @property + def manifest(self) -> SemanticIntegrationManifest: + """Return the static integration declaration.""" + return self._manifest + + @property + def scene_registry(self) -> SceneRegistry: + """Return the validated live scene registry.""" + return self._scene_registry + + @property + def robot_profile(self) -> BoundRobotSkillProfile: + """Return the validated live robot profile.""" + return self._robot_profile + + @property + def engine(self) -> AtomicActionEngine: + """Return the engine whose used call targets are validated at link time.""" + return self._engine + + def link_call( + self, + call: SemanticCallSpec, + *, + path: tuple[PathPart, ...] = ("call",), + ) -> BoundSemanticCall: + """Resolve one call against exact installed skills, resources, and preset.""" + if self._engine.skill_profile is not self._robot_profile: + raise SemanticValidationError( + SemanticDiagnostic( + "semantic_profile_stale", + ("integration", "robot_profile"), + "The engine's canonical robot profile changed after this " + "semantic integration was bound.", + ) + ) + linked = self._manifest.link_call(call, path=path) + installed = self._engine.skills.get(linked.descriptor.skill_id) + if installed is None or installed != linked.descriptor.target_descriptor: + raise SemanticValidationError( + SemanticDiagnostic( + "semantic_skill_not_installed", + (*path, "kind"), + f"Installed engine skill {linked.descriptor.skill_id!r} is " + "missing or has a different goal/options/resource contract.", + tuple(self._engine.skills), + ) + ) + try: + binding = self._robot_profile.resolve( + linked.descriptor.skill_id, + linked.call.resources, + ) + preset = self._robot_profile.preset( + linked.preset_id, + skill_id=linked.descriptor.skill_id, + ) + except Exception as exc: # noqa: BLE001 - add complete call path + raise SemanticValidationError( + SemanticDiagnostic( + "semantic_binding_failed", + (*path, "resources"), + str(exc), + ) + ) from exc + return BoundSemanticCall( + linked=linked, + binding=binding, + preset=preset, + _robot_profile=self._robot_profile, + ) + + +__all__ = [ + "BoundSemanticCall", + "LinkedSemanticCall", + "PathPart", + "SceneEntityManifest", + "SceneManifest", + "SemanticDiagnostic", + "SemanticIntegrationManifest", + "SemanticValidationError", +] diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 71fa2c4e0..ec924db8b 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -989,6 +989,8 @@ class RobotSkillProfile: presets: Mapping[str, SkillPolicyPreset] = field(default_factory=dict) default_preset: str | None = None skill_presets: Mapping[str, str] = field(default_factory=dict) + grounding_providers: Mapping[str, str] = field(default_factory=dict) + """Semantic call ID to embodiment-owned named grounding provider ID.""" def __post_init__(self) -> None: _validate_identifier(self.profile_id, field_name="RobotSkillProfile.profile_id") @@ -1022,6 +1024,14 @@ def __post_init__(self) -> None: f"skill_presets references unknown presets {unknown_presets}." ) object.__setattr__(self, "skill_presets", skill_presets) + object.__setattr__( + self, + "grounding_providers", + _normalize_named_mapping( + self.grounding_providers, + field_name="grounding_providers", + ), + ) self._validate_resource_graph(resources) self.action_control_profiles() @@ -1150,6 +1160,7 @@ def __init__( self._resources = self._resolve_resources() self._validate_engine_control_profiles() self._validate_leaf_ownership() + self._skill_catalog_revision = engine.skill_catalog_revision self._installed_skills = MappingProxyType(dict(engine.skills)) self._validate_named_skill_configuration() self._validate_defaults() @@ -1166,6 +1177,16 @@ def profile_id(self) -> str: """Return the stable profile identifier.""" return self._profile.profile_id + @property + def engine(self) -> AtomicActionEngine: + """Return the exact action engine that owns this bound profile.""" + return self._engine + + @property + def source_profile(self) -> RobotSkillProfile: + """Return the immutable profile object used to create this binding.""" + return self._profile + @property def resources(self) -> Mapping[str, ResolvedRobotResource]: """Return resolved generic robot resources keyed by logical ID.""" @@ -1293,7 +1314,7 @@ def _require_installed_skill(self, skill_id: str) -> SkillDescriptor: def _assert_catalog_current(self) -> None: """Prevent stale contracts after engine registration or replacement.""" - if dict(self._engine.skills) != dict(self._installed_skills): + if self._engine.skill_catalog_revision != self._skill_catalog_revision: raise RuntimeError( "AtomicActionEngine semantic skills changed after the robot skill " "profile was bound; bind the profile again before discovery or " diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py new file mode 100644 index 000000000..0f5113386 --- /dev/null +++ b/embodichain/lab/sim/skills/runtime.py @@ -0,0 +1,1443 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""High-level orchestration for semantic skill workflows.""" + +from __future__ import annotations + +from collections.abc import Callable, Iterable, Mapping +from copy import deepcopy +from dataclasses import dataclass +from enum import Enum +from types import MappingProxyType, TracebackType +from typing import TYPE_CHECKING + +import torch + +from ..atomic_actions.engine import AtomicActionEngine +from ..atomic_actions.execution import ( + EffectVerificationRequest, + ExecutionEvent, + ExecutionTick, +) +from ..atomic_actions.runner import ( + CommandSink, + ExecutionClock, + ExecutionRunner, + ExecutionRunnerCfg, + MonotonicExecutionClock, + ObservationProvider, + RunnerStatus, + RunnerStep, + RunnerStepCallback, +) +from ..atomic_actions.sim_adapter import SimulationExecutionAdapter +from ..atomic_actions.state import PlanningContext, TaskState +from .calls import ( + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticCallSpec, + builtin_semantic_call_catalog, +) +from .compiler import ( + GroundedSemanticCall, + HandOverPoseProvider, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticSkillCompiler, + SemanticWorkflow, +) +from .integration import SceneManifest, SemanticIntegrationManifest +from .profiles import ( + ResourceEndpoint, + ResourceEndpointAdapter, + RobotSkillProfile, +) +from .scene import SceneRegistry + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.planners import MotionGenerator + from embodichain.lab.sim.sim_manager import SimulationManager + + +SemanticEffectVerifier = Callable[ + [SemanticCallSpec, EffectVerificationRequest, PlanningContext], + torch.Tensor, +] +"""Verify one semantic effect and return a per-environment success mask.""" + + +class SemanticExecutionStatus(str, Enum): + """Lifecycle status of one semantic workflow segment.""" + + RUNNING = "running" + WAITING_FOR_EFFECT = "waiting_for_effect" + COMPLETED = "completed" + FAILED = "failed" + CANCELLED = "cancelled" + + +class SemanticTaskStatus(str, Enum): + """Terminal status of one semantic task.""" + + SUCCEEDED = "succeeded" + PARTIAL_SUCCESS = "partial_success" + FAILED = "failed" + CANCELLED = "cancelled" + + +@dataclass(frozen=True, slots=True, eq=False) +class SemanticCallRecord: + """Terminal execution record for one grounded semantic call.""" + + call_index: int + semantic_id: str + skill_id: str + invocation_id: str | None + invocation_revision: int + status: RunnerStatus + eligible_mask: torch.Tensor + events: tuple[ExecutionEvent, ...] + command_count: int + message: str | None = None + + def __post_init__(self) -> None: + if self.call_index < 0: + raise ValueError("call_index must be non-negative.") + for name in ("semantic_id", "skill_id"): + value = getattr(self, name) + if not isinstance(value, str) or not value: + raise ValueError(f"{name} must be a non-empty string.") + if self.invocation_revision < 0: + raise ValueError("invocation_revision must be non-negative.") + if self.invocation_id is not None and ( + not isinstance(self.invocation_id, str) or not self.invocation_id + ): + raise ValueError("invocation_id must be a non-empty string or None.") + if not isinstance(self.status, RunnerStatus): + raise TypeError("status must be a RunnerStatus.") + if ( + not isinstance(self.eligible_mask, torch.Tensor) + or self.eligible_mask.dtype != torch.bool + or self.eligible_mask.dim() != 1 + ): + raise ValueError("eligible_mask must be a one-dimensional bool tensor.") + if not all(isinstance(event, ExecutionEvent) for event in self.events): + raise TypeError("events must contain ExecutionEvent values.") + if self.command_count < 0: + raise ValueError("command_count must be non-negative.") + if self.message is not None and not isinstance(self.message, str): + raise TypeError("message must be a string or None.") + object.__setattr__(self, "eligible_mask", self.eligible_mask.clone()) + object.__setattr__(self, "events", tuple(self.events)) + + +@dataclass(frozen=True, slots=True, eq=False) +class SemanticSegmentResult: + """Terminal result of one statically analyzed workflow segment.""" + + segment_id: str + workflow_id: str + status: SemanticExecutionStatus + eligible_mask: torch.Tensor + task_state: TaskState + calls: tuple[SemanticCallRecord, ...] + message: str | None = None + + def __post_init__(self) -> None: + for name in ("segment_id", "workflow_id"): + value = getattr(self, name) + if not isinstance(value, str) or not value: + raise ValueError(f"{name} must be a non-empty string.") + if not isinstance(self.status, SemanticExecutionStatus) or self.status not in { + SemanticExecutionStatus.COMPLETED, + SemanticExecutionStatus.FAILED, + SemanticExecutionStatus.CANCELLED, + }: + raise ValueError("SemanticSegmentResult status must be terminal.") + if ( + not isinstance(self.eligible_mask, torch.Tensor) + or self.eligible_mask.dtype != torch.bool + or self.eligible_mask.dim() != 1 + ): + raise ValueError("eligible_mask must be a one-dimensional bool tensor.") + if not isinstance(self.task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if not all(isinstance(call, SemanticCallRecord) for call in self.calls): + raise TypeError("calls must contain SemanticCallRecord values.") + if self.message is not None and not isinstance(self.message, str): + raise TypeError("message must be a string or None.") + object.__setattr__(self, "eligible_mask", self.eligible_mask.clone()) + object.__setattr__(self, "calls", tuple(self.calls)) + + @property + def events(self) -> tuple[ExecutionEvent, ...]: + """Return all structured execution events in call order.""" + return tuple(event for call in self.calls for event in call.events) + + +@dataclass(frozen=True, slots=True, eq=False) +class SemanticTaskResult: + """Terminal result of a complete static or dynamically segmented task.""" + + task_id: str + status: SemanticTaskStatus + initial_eligible_mask: torch.Tensor + eligible_mask: torch.Tensor + task_state: TaskState + latest_context: PlanningContext + segments: tuple[SemanticSegmentResult, ...] + message: str | None = None + + def __post_init__(self) -> None: + if not isinstance(self.task_id, str) or not self.task_id: + raise ValueError("task_id must be a non-empty string.") + if not isinstance(self.status, SemanticTaskStatus): + raise TypeError("status must be a SemanticTaskStatus.") + for name in ("initial_eligible_mask", "eligible_mask"): + value = getattr(self, name) + if ( + not isinstance(value, torch.Tensor) + or value.dtype != torch.bool + or value.dim() != 1 + ): + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + object.__setattr__(self, name, value.clone()) + if self.initial_eligible_mask.shape != self.eligible_mask.shape: + raise ValueError("Task eligibility masks must share a shape.") + if not isinstance(self.task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if not isinstance(self.latest_context, PlanningContext): + raise TypeError("latest_context must be a PlanningContext.") + if not all(isinstance(item, SemanticSegmentResult) for item in self.segments): + raise TypeError("segments must contain SemanticSegmentResult values.") + if self.message is not None and not isinstance(self.message, str): + raise TypeError("message must be a string or None.") + object.__setattr__(self, "segments", tuple(self.segments)) + + @property + def events(self) -> tuple[ExecutionEvent, ...]: + """Return all structured execution events in segment order.""" + return tuple(event for segment in self.segments for event in segment.events) + + def require_all_succeeded(self) -> None: + """Require every initially eligible environment to have succeeded. + + Raises: + RuntimeError: If the task failed, was cancelled, or retained only a + subset of its initial environment cohort. + """ + if self.status is not SemanticTaskStatus.SUCCEEDED: + detail = "" if self.message is None else f" {self.message}" + raise RuntimeError( + f"Semantic task {self.task_id!r} finished with " + f"{self.status.value!r}.{detail}" + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class SemanticExecutionStep: + """Latest high-level state after advancing a semantic segment.""" + + status: SemanticExecutionStatus + task_id: str + segment_id: str + call_index: int + eligible_mask: torch.Tensor + runner_step: RunnerStep | None = None + pending_effect: EffectVerificationRequest | None = None + message: str | None = None + + def __post_init__(self) -> None: + if not isinstance(self.status, SemanticExecutionStatus): + raise TypeError("status must be a SemanticExecutionStatus.") + if not isinstance(self.task_id, str) or not self.task_id: + raise ValueError("task_id must be a non-empty string.") + if not isinstance(self.segment_id, str) or not self.segment_id: + raise ValueError("segment_id must be a non-empty string.") + if self.call_index < 0: + raise ValueError("call_index must be non-negative.") + if ( + not isinstance(self.eligible_mask, torch.Tensor) + or self.eligible_mask.dtype != torch.bool + or self.eligible_mask.dim() != 1 + ): + raise ValueError("eligible_mask must be a one-dimensional bool tensor.") + if self.message is not None and not isinstance(self.message, str): + raise TypeError("message must be a string or None.") + object.__setattr__(self, "eligible_mask", self.eligible_mask.clone()) + + +class SemanticSkillRuntime: + """Bind semantic declarations to closed-loop planning and execution ports. + + The runtime is intentionally thin. It owns one compiler and the controller + ports used to construct existing :class:`ExecutionRunner` instances. A + :class:`SemanticTask` owns verified symbolic state across workflow segments. + + Args: + compiler: Bound compiler used for static analysis and JIT grounding. + observation_provider: Source of fresh robot and scene observations. + command_sink: Destination for controller commands and safe-stop requests. + clock: Optional execution clock. Wall-clock time is used when omitted. + effect_verifier: Optional default callback for physical effect checks. + runner_cfg: Optional transport and scheduling policy overriding each + call's skill preset. + """ + + def __init__( + self, + compiler: SemanticSkillCompiler, + observation_provider: ObservationProvider, + command_sink: CommandSink, + *, + clock: ExecutionClock | None = None, + effect_verifier: SemanticEffectVerifier | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> None: + if not isinstance(compiler, SemanticSkillCompiler): + raise TypeError("compiler must be a SemanticSkillCompiler.") + if not isinstance(observation_provider, ObservationProvider): + raise TypeError("observation_provider must implement ObservationProvider.") + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + if clock is not None and not isinstance(clock, ExecutionClock): + raise TypeError("clock must implement ExecutionClock.") + if effect_verifier is not None and not callable(effect_verifier): + raise TypeError("effect_verifier must be callable or None.") + if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") + self.compiler = compiler + self.observation_provider = observation_provider + self.command_sink = command_sink + self.clock = MonotonicExecutionClock() if clock is None else clock + self.effect_verifier = effect_verifier + self.runner_cfg: ExecutionRunnerCfg | None = ( + None if runner_cfg is None else deepcopy(runner_cfg) + ) + self._active_task: SemanticTask | None = None + + @classmethod + def bind( + cls, + *, + manifest: SemanticIntegrationManifest, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: ObservationProvider, + command_sink: CommandSink, + clock: ExecutionClock | None = None, + effect_verifier: SemanticEffectVerifier | None = None, + registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), + relation_grounders: Iterable[RelationTargetGrounder] = (), + handover_pose_providers: Iterable[HandOverPoseProvider] = (), + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> SemanticSkillRuntime: + """Bind an explicit integration to generic execution ports. + + Args: + manifest: Provider-free scene, robot, and semantic-call declaration. + scene_registry: Live registry matching ``manifest.scene``. + engine: Atomic-action engine owning the target robot and planners. + observation_provider: Source of fresh execution observations. + command_sink: Destination for runtime commands and safe stops. + clock: Optional scheduler clock shared by every per-call runner. + effect_verifier: Optional default semantic effect verifier. + registered_lowerers: Lowerers for registered extension calls. + relation_grounders: Providers for late-bound relation targets. + handover_pose_providers: Named handover-pose providers. + endpoint_adapters: Optional adapters for custom resource endpoints. + runner_cfg: Optional runner policy overriding per-skill presets. + + Returns: + A validated runtime ready to analyze or execute semantic calls. + + Raises: + TypeError: If an integration object or execution port is invalid. + SemanticValidationError: If the live registry, engine, and manifest + do not describe the same scene and robot capabilities. + """ + if type(manifest) is not SemanticIntegrationManifest: + raise TypeError("manifest must be exactly SemanticIntegrationManifest.") + if not isinstance(scene_registry, SceneRegistry): + raise TypeError("scene_registry must be a SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + integration = manifest.bind( + scene_registry, + engine, + endpoint_adapters=endpoint_adapters, + ) + compiler = SemanticSkillCompiler( + integration, + registered_lowerers=registered_lowerers, + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + ) + return cls( + compiler, + observation_provider, + command_sink, + clock=clock, + effect_verifier=effect_verifier, + runner_cfg=runner_cfg, + ) + + @classmethod + def from_simulation( + cls, + *, + simulation: SimulationManager, + robot: Robot, + motion_generator: MotionGenerator, + scene_registry: SceneRegistry, + robot_profile: RobotSkillProfile, + call_catalog: SemanticCallCatalog | None = None, + effect_verifier: SemanticEffectVerifier | None = None, + registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), + relation_grounders: Iterable[RelationTargetGrounder] = (), + handover_pose_providers: Iterable[HandOverPoseProvider] = (), + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + runner_cfg: ExecutionRunnerCfg | None = None, + control_dt: float | None = None, + scene_translation_threshold: float = 1.0e-4, + scene_rotation_threshold: float = 1.0e-3, + ) -> SemanticSkillRuntime: + """Build the standard joint-position semantic runtime for simulation. + + This factory validates the registry against the motion generator, + constructs a :class:`SimulationExecutionAdapter`, installs built-in + atomic actions, and delegates the remaining binding to :meth:`bind`. + + Args: + simulation: Simulation advanced by the runtime execution clock. + robot: Robot observed and controlled through joint-position ports. + motion_generator: Planner used by built-in atomic actions. + scene_registry: Canonical live scene and affordance registry. + robot_profile: Embodiment-specific skill and resource declaration. + call_catalog: Optional semantic-call catalog. Built-ins are used by + default. + effect_verifier: Optional default semantic effect verifier. + registered_lowerers: Lowerers for registered extension calls. + relation_grounders: Providers for late-bound relation targets. + handover_pose_providers: Named handover-pose providers. + endpoint_adapters: Optional adapters for custom resource endpoints. + runner_cfg: Optional runner policy overriding per-skill presets. + control_dt: Optional semantic command period. The simulation physics + period is used when omitted. + scene_translation_threshold: Translation needed to advance the + registry-backed scene version. + scene_rotation_threshold: Rotation needed to advance the + registry-backed scene version. + + Returns: + A runtime using one simulation adapter for observation, commands, + and deterministic simulated time. + + Raises: + TypeError: If a registry, profile, catalog, or port is invalid. + ValueError: If robot state or collision integration is inconsistent. + """ + if not isinstance(scene_registry, SceneRegistry): + raise TypeError("scene_registry must be a SceneRegistry.") + if type(robot_profile) is not RobotSkillProfile: + raise TypeError("robot_profile must be exactly RobotSkillProfile.") + if call_catalog is not None and type(call_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog or None.") + qpos = robot.get_qpos() + if not isinstance(qpos, torch.Tensor) or qpos.dim() != 2: + raise ValueError("robot.get_qpos() must return shape (B, robot_dof).") + batch_size = int(qpos.shape[0]) + scene_provider = scene_registry.make_planning_scene_provider( + motion_generator, + batch_size=batch_size, + translation_threshold=scene_translation_threshold, + rotation_threshold=scene_rotation_threshold, + ) + adapter = SimulationExecutionAdapter( + simulation, + robot, + control_dt=control_dt, + scene_provider=scene_provider, + ) + engine = AtomicActionEngine( + motion_generator, + control_profiles=robot_profile.action_control_profiles(), + ) + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(scene_registry), + robot_profile=robot_profile, + call_catalog=( + builtin_semantic_call_catalog() + if call_catalog is None + else call_catalog + ), + ) + return cls.bind( + manifest=manifest, + scene_registry=scene_registry, + engine=engine, + observation_provider=adapter, + command_sink=adapter, + clock=adapter, + effect_verifier=effect_verifier, + registered_lowerers=registered_lowerers, + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + endpoint_adapters=endpoint_adapters, + runner_cfg=runner_cfg, + ) + + @property + def engine(self) -> AtomicActionEngine: + """Return the bound atomic-action engine.""" + return self.compiler.integration.engine + + @property + def scene_registry(self) -> SceneRegistry: + """Return the bound canonical scene registry.""" + return self.compiler.integration.scene_registry + + @property + def available_calls(self) -> Mapping[str, SemanticCallDescriptor]: + """Return semantic calls executable by the currently bound profile.""" + supported_skills = set(self.compiler.integration.robot_profile.skills) + return MappingProxyType( + { + call_id: descriptor + for call_id, descriptor in self.compiler.integration.manifest.call_catalog.descriptors.items() + if descriptor.skill_id in supported_skills + } + ) + + @property + def active_task(self) -> SemanticTask | None: + """Return the task currently owning this runtime.""" + return self._active_task + + def validate( + self, + calls: Iterable[SemanticCallSpec], + *, + workflow_id: str = "semantic_workflow", + ) -> SemanticWorkflow: + """Analyze one workflow without observing, planning, or executing it. + + Args: + calls: Ordered robot-independent semantic calls. + workflow_id: Stable identifier used for diagnostics and revisions. + + Returns: + Provider-free linked workflow accepted by the current integration. + """ + return self.compiler.analyze(calls, workflow_id=workflow_id) + + def open_task( + self, + task_id: str, + *, + initial_task_state: TaskState | None = None, + eligible_mask: torch.Tensor | None = None, + ) -> SemanticTask: + """Open one exclusive task that may execute several workflow segments. + + Args: + task_id: Stable task identifier without outer whitespace. + initial_task_state: Optional previously verified symbolic state. + eligible_mask: Optional initial per-environment execution cohort. + + Returns: + A task retaining verified state across dynamic segment boundaries. + + Raises: + RuntimeError: If another task already owns this runtime. + ValueError: If the identifier or eligibility mask is invalid. + """ + _validate_identifier(task_id, name="task_id") + if self._active_task is not None: + raise RuntimeError( + f"Semantic task {self._active_task.task_id!r} already owns this runtime." + ) + task = SemanticTask( + self, + task_id, + initial_task_state=initial_task_state, + eligible_mask=eligible_mask, + ) + self._active_task = task + return task + + def start( + self, + calls: Iterable[SemanticCallSpec], + *, + task_id: str = "semantic_task", + segment_id: str = "main", + initial_task_state: TaskState | None = None, + eligible_mask: torch.Tensor | None = None, + ) -> SemanticExecution: + """Start a one-segment task and return a non-blocking execution handle. + + Args: + calls: Ordered semantic calls analyzed before controller work starts. + task_id: Stable identifier for the one-shot task. + segment_id: Stable identifier for its only workflow segment. + initial_task_state: Optional previously verified symbolic state. + eligible_mask: Optional initial per-environment execution cohort. + + Returns: + A handle advanced through :meth:`SemanticExecution.step` or + :meth:`SemanticExecution.run_until_blocked`. + """ + task = self.open_task( + task_id, + initial_task_state=initial_task_state, + eligible_mask=eligible_mask, + ) + try: + return task._start_segment( + calls, + segment_id=segment_id, + finish_task_on_completion=True, + ) + except Exception: + task.cancel("Semantic task could not be started.") + raise + + def run( + self, + calls: Iterable[SemanticCallSpec], + *, + task_id: str = "semantic_task", + segment_id: str = "main", + initial_task_state: TaskState | None = None, + eligible_mask: torch.Tensor | None = None, + effect_verifier: SemanticEffectVerifier | None = None, + on_step: RunnerStepCallback | None = None, + max_steps_per_call: int = 100_000, + ) -> SemanticTaskResult: + """Run one semantic workflow to a terminal task result. + + Args: + calls: Ordered semantic calls analyzed before execution. + task_id: Stable identifier for the one-shot task. + segment_id: Stable identifier for its only workflow segment. + initial_task_state: Optional previously verified symbolic state. + eligible_mask: Optional initial per-environment execution cohort. + effect_verifier: Per-call effect verifier overriding the runtime + default. + on_step: Optional observer for each low-level runner step. + max_steps_per_call: Hard loop bound applied separately to each call. + + Returns: + Terminal result containing verified state, eligibility, and events. + + Raises: + ValueError: If no effect verifier is available or a bound is invalid. + """ + verifier = self.effect_verifier if effect_verifier is None else effect_verifier + if verifier is None: + raise ValueError( + "run() requires an effect_verifier; use start() for manual " + "effect submission." + ) + execution = self.start( + calls, + task_id=task_id, + segment_id=segment_id, + initial_task_state=initial_task_state, + eligible_mask=eligible_mask, + ) + execution.run_until_blocked( + effect_verifier=verifier, + on_step=on_step, + max_steps_per_call=max_steps_per_call, + ) + result = execution.task_result + if result is None: + execution.cancel("Blocking semantic execution did not terminate.") + result = execution.task_result + assert result is not None + return result + + def _release_task(self, task: SemanticTask) -> None: + """Release task ownership after an exact active-task match.""" + if self._active_task is task: + self._active_task = None + + +class SemanticTask: + """Own verified state across one or more semantic workflow segments. + + Tasks are created by :meth:`SemanticSkillRuntime.open_task`. Successful + segments leave the task open for a later application or agent decision. + Failed or cancelled segments are terminal and release runtime ownership. + + Args: + runtime: Runtime exclusively owned until this task terminates. + task_id: Stable task identifier. + initial_task_state: Optional externally verified symbolic state. + eligible_mask: Optional initial per-environment execution cohort. + """ + + def __init__( + self, + runtime: SemanticSkillRuntime, + task_id: str, + *, + initial_task_state: TaskState | None, + eligible_mask: torch.Tensor | None, + ) -> None: + self.runtime = runtime + self.task_id = _validate_identifier(task_id, name="task_id") + if initial_task_state is not None and not isinstance( + initial_task_state, TaskState + ): + raise TypeError("initial_task_state must be a TaskState or None.") + self._task_state = ( + runtime.engine.initial_context().task + if initial_task_state is None + else initial_task_state + ) + self._latest_context: PlanningContext | None = None + self._latest_context = self._observe() + self._initial_eligible_mask = _normalize_eligible_mask( + eligible_mask, + self._latest_context, + ) + self._eligible_mask = self._initial_eligible_mask.clone() + self._segments: list[SemanticSegmentResult] = [] + self._segment_ids: set[str] = set() + self._active_execution: SemanticExecution | None = None + self._failed = False + self._cancelled = False + self._message: str | None = None + self._result: SemanticTaskResult | None = None + + def __enter__(self) -> SemanticTask: + """Return this task for scoped dynamic execution.""" + return self + + def __exit__( + self, + exc_type: type[BaseException] | None, + exc: BaseException | None, + traceback: TracebackType | None, + ) -> None: + """Cancel unfinished work on scope exit and release runtime ownership.""" + del exc_type, traceback + if self._result is not None: + return + if exc is not None or self._active_execution is not None or not self._segments: + self.cancel( + "Semantic task scope exited before normal completion." + if exc is None + else f"Semantic task scope exited with {type(exc).__name__}." + ) + else: + self.finish() + + @property + def task_state(self) -> TaskState: + """Return the latest externally verified symbolic state.""" + return self._task_state + + @property + def latest_context(self) -> PlanningContext: + """Return the latest observation carrying verified task state.""" + assert self._latest_context is not None + return self._latest_context + + @property + def eligible_mask(self) -> torch.Tensor: + """Return environments still eligible to finish this task.""" + return self._eligible_mask.clone() + + @property + def active_execution(self) -> SemanticExecution | None: + """Return the currently active workflow segment, if any.""" + return self._active_execution + + @property + def segments(self) -> tuple[SemanticSegmentResult, ...]: + """Return terminal segment results in execution order.""" + return tuple(self._segments) + + @property + def result(self) -> SemanticTaskResult | None: + """Return the terminal task result, when finished.""" + return self._result + + def start_segment( + self, + calls: Iterable[SemanticCallSpec], + *, + segment_id: str | None = None, + ) -> SemanticExecution: + """Analyze and start one non-blocking workflow segment. + + Args: + calls: Semantic calls known at the current decision boundary. + segment_id: Optional stable segment identifier. A deterministic + task-local identifier is generated when omitted. + + Returns: + A non-blocking execution handle for this segment. + + Raises: + RuntimeError: If the task is terminal or another segment is active. + ValueError: If the segment identifier is invalid or already used. + """ + return self._start_segment( + calls, + segment_id=segment_id, + finish_task_on_completion=False, + ) + + def _start_segment( + self, + calls: Iterable[SemanticCallSpec], + *, + segment_id: str | None, + finish_task_on_completion: bool, + ) -> SemanticExecution: + if self._result is not None: + raise RuntimeError("A finished semantic task cannot start another segment.") + if self._failed or self._cancelled: + raise RuntimeError("A failed or cancelled task cannot start a segment.") + if self._active_execution is not None: + raise RuntimeError("Only one semantic segment may execute at a time.") + selected_segment_id = ( + f"segment_{len(self._segments)}" if segment_id is None else segment_id + ) + _validate_identifier(selected_segment_id, name="segment_id") + if selected_segment_id in self._segment_ids: + raise ValueError(f"Duplicate segment_id {selected_segment_id!r}.") + workflow = self.runtime.compiler.analyze( + calls, + workflow_id=f"{self.task_id}.{selected_segment_id}", + ) + execution = SemanticExecution( + self, + workflow, + selected_segment_id, + finish_task_on_completion=finish_task_on_completion, + ) + self._segment_ids.add(selected_segment_id) + self._active_execution = execution + return execution + + def run_segment( + self, + calls: Iterable[SemanticCallSpec], + *, + segment_id: str | None = None, + effect_verifier: SemanticEffectVerifier | None = None, + on_step: RunnerStepCallback | None = None, + max_steps_per_call: int = 100_000, + ) -> SemanticSegmentResult: + """Run one segment while retaining successful task state for more work. + + Args: + calls: Semantic calls known at the current decision boundary. + segment_id: Optional stable segment identifier. + effect_verifier: Verifier overriding the runtime default. + on_step: Optional observer for low-level runner steps. + max_steps_per_call: Hard loop bound applied separately to each call. + + Returns: + Terminal segment result. A successful task remains open; a failed + or cancelled task closes automatically. + + Raises: + ValueError: If no effect verifier is available or a bound is invalid. + RuntimeError: If this task cannot start another segment. + """ + verifier = ( + self.runtime.effect_verifier if effect_verifier is None else effect_verifier + ) + if verifier is None: + raise ValueError( + "run_segment() requires an effect_verifier; use start_segment() " + "for manual effect submission." + ) + execution = self.start_segment(calls, segment_id=segment_id) + execution.run_until_blocked( + effect_verifier=verifier, + on_step=on_step, + max_steps_per_call=max_steps_per_call, + ) + result = execution.segment_result + if result is None: + execution.cancel("Blocking semantic segment did not terminate.") + result = execution.segment_result + assert result is not None + return result + + def cancel( + self, reason: str = "Semantic task cancelled by caller." + ) -> SemanticTaskResult: + """Cancel active controller work and release runtime ownership. + + Args: + reason: Non-empty cancellation diagnostic. + + Returns: + Idempotent terminal task result after best-effort safe stop. + """ + _validate_identifier(reason, name="reason") + if self._result is not None: + return self._result + if self._active_execution is not None: + self._active_execution.cancel(reason) + else: + self._cancelled = True + self._message = reason + return self.finish() + + def finish(self) -> SemanticTaskResult: + """Finalize this task and release its exclusive runtime ownership. + + Returns: + Idempotent terminal result derived from sticky eligibility and + segment outcomes. + + Raises: + RuntimeError: If a segment is active or no segment has run. + """ + if self._result is not None: + return self._result + if self._active_execution is not None: + raise RuntimeError("Cannot finish while a semantic segment is running.") + if not self._segments and not self._cancelled and not self._failed: + raise RuntimeError("Cannot finish a semantic task with no segments.") + if self._cancelled: + status = SemanticTaskStatus.CANCELLED + elif self._failed or not self._eligible_mask.any(): + status = SemanticTaskStatus.FAILED + else: + initial = self._initial_eligible_mask + retained = self._eligible_mask & initial + status = ( + SemanticTaskStatus.SUCCEEDED + if torch.equal(retained, initial) + else SemanticTaskStatus.PARTIAL_SUCCESS + ) + result = SemanticTaskResult( + task_id=self.task_id, + status=status, + initial_eligible_mask=self._initial_eligible_mask, + eligible_mask=self._eligible_mask, + task_state=self._task_state, + latest_context=self.latest_context, + segments=tuple(self._segments), + message=self._message, + ) + self._result = result + self.runtime._release_task(self) + return result + + def _observe(self) -> PlanningContext: + """Capture and validate a fresh context carrying verified task state.""" + observed = self.runtime.observation_provider.observe(self._task_state) + if type(observed) is not PlanningContext: + raise TypeError( + "ObservationProvider.observe() must return PlanningContext." + ) + context = PlanningContext( + robot=observed.robot, + task=self._task_state, + scene=observed.scene, + env_ids=observed.env_ids, + control_dt=observed.control_dt, + ) + self.runtime.engine._validate_context(context) + previous = self._latest_context + if previous is not None: + _validate_context_progress(previous, context) + self._latest_context = context + return context + + def _adopt_runner(self, runner: ExecutionRunner) -> None: + """Carry verified state and sticky eligibility across call boundaries.""" + session = runner.session + self._task_state = session.task_state + self._eligible_mask = session.eligible_mask + self._latest_context = session.latest_context + + def _accept_segment( + self, + execution: SemanticExecution, + result: SemanticSegmentResult, + ) -> None: + """Install one exact active segment result.""" + if self._active_execution is not execution: + raise RuntimeError("Semantic segment no longer owns this task.") + self._segments.append(result) + self._active_execution = None + if result.status is SemanticExecutionStatus.FAILED: + self._failed = True + self._message = result.message + elif result.status is SemanticExecutionStatus.CANCELLED: + self._cancelled = True + self._message = result.message + + +class SemanticExecution: + """Drive one analyzed workflow through one JIT-grounded call at a time. + + Instances are created by :meth:`SemanticSkillRuntime.start` or + :meth:`SemanticTask.start_segment`; direct construction is not required. + + Args: + task: Task retaining verified state and sticky eligibility. + workflow: Statically analyzed semantic workflow. + segment_id: Stable identifier of the owning segment. + finish_task_on_completion: Whether a successful segment also finalizes + its task. Failures and cancellations always finalize the task. + """ + + def __init__( + self, + task: SemanticTask, + workflow: SemanticWorkflow, + segment_id: str, + *, + finish_task_on_completion: bool, + ) -> None: + self.task = task + self.workflow = workflow + self.segment_id = segment_id + self._finish_task_on_completion = finish_task_on_completion + self._call_index = 0 + self._runner: ExecutionRunner | None = None + self._grounded: GroundedSemanticCall | None = None + self._current_events: list[ExecutionEvent] = [] + self._current_event_ids: set[int] = set() + self._call_records: list[SemanticCallRecord] = [] + self._status = SemanticExecutionStatus.RUNNING + self._message: str | None = None + self._segment_result: SemanticSegmentResult | None = None + self._task_result: SemanticTaskResult | None = None + self._last_step: SemanticExecutionStep | None = None + self._start_current_call() + + @property + def status(self) -> SemanticExecutionStatus: + """Return the current segment lifecycle status.""" + return self._status + + @property + def call_index(self) -> int: + """Return the currently active or last call index.""" + return self._call_index + + @property + def segment_result(self) -> SemanticSegmentResult | None: + """Return the terminal segment result, when available.""" + return self._segment_result + + @property + def task_result(self) -> SemanticTaskResult | None: + """Return the terminal task result for one-shot runtime execution.""" + return self._task_result + + @property + def pending_effect(self) -> EffectVerificationRequest | None: + """Return the effect currently awaiting external verification.""" + runner = self._runner + if runner is None or not runner.effect_verification_pending: + return None + step = self._last_step + return None if step is None else step.pending_effect + + def step( + self, + *, + effect_success: torch.Tensor | None = None, + ) -> SemanticExecutionStep: + """Advance the active call without sleeping. + + Args: + effect_success: Optional per-environment result for a currently + pending effect request. Premature submissions are rejected. + + Returns: + Latest semantic status and its underlying runner step. + + Raises: + RuntimeError: If an effect result is submitted before the explicit + verification boundary. + """ + if self._segment_result is not None: + assert self._last_step is not None + return self._last_step + assert self._runner is not None + if effect_success is not None and not self._runner.effect_verification_pending: + raise RuntimeError( + "effect_success may only be submitted for pending effect " + "verification." + ) + runner_step = self._runner.step(effect_success=effect_success) + self._record_runner_step(runner_step) + return self._consume_runner_step(runner_step) + + def run_until_blocked( + self, + *, + effect_verifier: SemanticEffectVerifier | None = None, + on_step: RunnerStepCallback | None = None, + max_steps_per_call: int = 100_000, + ) -> SemanticExecutionStep: + """Run until the segment terminates or external verification is needed. + + Args: + effect_verifier: Optional callback used at every physical effect + boundary. Without one, execution returns ``WAITING_FOR_EFFECT``. + on_step: Optional observer for each low-level runner step. + max_steps_per_call: Hard loop bound reset for every semantic call. + + Returns: + Terminal segment step or an external-verification boundary. + + Raises: + ValueError: If ``max_steps_per_call`` is not positive. + """ + if max_steps_per_call <= 0: + raise ValueError("max_steps_per_call must be greater than zero.") + if self._segment_result is not None: + assert self._last_step is not None + return self._last_step + while True: + assert self._runner is not None + runner = self._runner + callback_step: RunnerStep | None = None + + def record_step(step: RunnerStep) -> None: + nonlocal callback_step + callback_step = step + self._record_runner_step(step) + if on_step is not None: + on_step(step) + + runner_step = runner.run_until_blocked( + effect_verifier=( + None + if effect_verifier is None + else self._adapt_effect_verifier(effect_verifier) + ), + on_step=record_step, + max_steps=max_steps_per_call, + ) + if callback_step is not runner_step: + self._record_runner_step(runner_step) + result = self._consume_runner_step(runner_step) + if result.status is not SemanticExecutionStatus.RUNNING: + return result + + def revise_current(self, replacement: SemanticCallSpec) -> None: + """Reanalyze and stage a compatible revision of the active call. + + The low-level runner still enforces the same semantic skill, logical + invocation ID, and runtime endpoint addresses. This method is for a + newer target or policy revision, not task-level skill replacement. + + Args: + replacement: Replacement semantic call for the active workflow slot. + + Raises: + TypeError: If ``replacement`` is not a semantic call. + RuntimeError: If the segment is not running or awaits verification. + ValueError: If the replacement violates compiler or runner revision + invariants. + """ + if not isinstance(replacement, SemanticCallSpec): + raise TypeError("replacement must be a SemanticCallSpec.") + if self._status is not SemanticExecutionStatus.RUNNING: + raise RuntimeError("Only a running semantic call can be revised.") + assert self._runner is not None and self._grounded is not None + calls = [item.call for item in self.workflow.calls] + calls[self._call_index] = replacement + revised_workflow = self.task.runtime.compiler.analyze( + calls, + workflow_id=self.workflow.workflow_id, + ) + context = self.task._observe() + grounded = self.task.runtime.compiler.ground( + revised_workflow, + self._call_index, + context, + eligible_mask=self.task.eligible_mask, + revision=self._grounded.invocation.revision + 1, + ) + self._runner.revise_current(grounded.invocation) + self.workflow = revised_workflow + self._grounded = grounded + + def cancel( + self, + reason: str = "Semantic execution cancelled by caller.", + ) -> SemanticExecutionStep: + """Cancel the active low-level runner and finalize this segment. + + Args: + reason: Non-empty cancellation diagnostic. + + Returns: + Terminal semantic step after best-effort cancel and safe hold. + """ + _validate_identifier(reason, name="reason") + if self._segment_result is not None: + assert self._last_step is not None + return self._last_step + assert self._runner is not None + runner_step = self._runner.cancel(reason) + self._record_runner_step(runner_step) + return self._consume_runner_step(runner_step) + + def _start_current_call(self) -> None: + """Observe, ground, plan, and install one semantic call runner.""" + context = self.task._observe() + grounded = self.task.runtime.compiler.ground( + self.workflow, + self._call_index, + context, + eligible_mask=self.task.eligible_mask, + ) + session = self.task.runtime.engine.start( + (grounded.invocation,), + context, + eligible_mask=grounded.eligible_mask, + ) + self._grounded = grounded + self._runner = ExecutionRunner( + session, + self.task.runtime.observation_provider, + self.task.runtime.command_sink, + clock=self.task.runtime.clock, + cfg=deepcopy( + self._grounded.analyzed.bound.preset.runner_cfg + if self.task.runtime.runner_cfg is None + else self.task.runtime.runner_cfg + ), + ) + self._current_events = [] + self._current_event_ids = set() + + def _adapt_effect_verifier( + self, + verifier: SemanticEffectVerifier, + ) -> Callable[[PlanningContext, ExecutionTick], torch.Tensor]: + """Adapt the semantic verifier to the low-level runner callback.""" + + def verify(context: PlanningContext, tick: ExecutionTick) -> torch.Tensor: + pending = tick.pending_effect + if not isinstance(pending, EffectVerificationRequest): + raise RuntimeError("Effect verifier was called without a request.") + call = self.workflow.calls[self._call_index].call + result = verifier(call, pending, context) + if not isinstance(result, torch.Tensor): + raise TypeError("SemanticEffectVerifier must return a torch.Tensor.") + return result + + return verify + + def _record_runner_step(self, step: RunnerStep) -> None: + """Retain each structured event exactly once.""" + if step.tick is None: + return + for event in step.tick.events: + event_id = id(event) + if event_id in self._current_event_ids: + continue + self._current_event_ids.add(event_id) + self._current_events.append(event) + + def _consume_runner_step(self, runner_step: RunnerStep) -> SemanticExecutionStep: + """Advance the semantic call barrier from one low-level result.""" + assert self._runner is not None + pending = None if runner_step.tick is None else runner_step.tick.pending_effect + if runner_step.status is RunnerStatus.RUNNING: + self._status = ( + SemanticExecutionStatus.WAITING_FOR_EFFECT + if pending is not None + else SemanticExecutionStatus.RUNNING + ) + return self._make_step(runner_step, pending_effect=pending) + + self.task._adopt_runner(self._runner) + self._record_call(runner_step) + if runner_step.status is RunnerStatus.COMPLETED: + if self._call_index + 1 < len(self.workflow.calls): + self._call_index += 1 + try: + self._start_current_call() + except Exception as exc: # noqa: BLE001 - normalize call boundary + self._message = ( + f"Could not start semantic call {self._call_index}: " + f"{type(exc).__name__}: {exc}" + ) + self._finish_segment(SemanticExecutionStatus.FAILED) + return self._make_step(None, message=self._message) + self._status = SemanticExecutionStatus.RUNNING + return self._make_step(runner_step) + self._finish_segment(SemanticExecutionStatus.COMPLETED) + return self._make_step(runner_step) + + terminal_status = ( + SemanticExecutionStatus.CANCELLED + if runner_step.status is RunnerStatus.CANCELLED + else SemanticExecutionStatus.FAILED + ) + self._message = runner_step.message + self._finish_segment(terminal_status) + return self._make_step(runner_step, message=self._message) + + def _record_call(self, runner_step: RunnerStep) -> None: + """Snapshot the terminal state of the current grounded call.""" + assert self._runner is not None and self._grounded is not None + invocation = self._grounded.invocation + call = self.workflow.calls[self._call_index].call + self._call_records.append( + SemanticCallRecord( + call_index=self._call_index, + semantic_id=call.semantic_id, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + status=runner_step.status, + eligible_mask=self.task.eligible_mask, + events=tuple(self._current_events), + command_count=self._runner.command_count, + message=runner_step.message, + ) + ) + + def _finish_segment(self, status: SemanticExecutionStatus) -> None: + """Install one terminal segment and optionally finalize its task.""" + self._status = status + result = SemanticSegmentResult( + segment_id=self.segment_id, + workflow_id=self.workflow.workflow_id, + status=status, + eligible_mask=self.task.eligible_mask, + task_state=self.task.task_state, + calls=tuple(self._call_records), + message=self._message, + ) + self._segment_result = result + self.task._accept_segment(self, result) + if ( + self._finish_task_on_completion + or status is not SemanticExecutionStatus.COMPLETED + ): + self._task_result = self.task.finish() + + def _make_step( + self, + runner_step: RunnerStep | None, + *, + pending_effect: EffectVerificationRequest | None = None, + message: str | None = None, + ) -> SemanticExecutionStep: + """Build and retain the latest high-level execution step.""" + step = SemanticExecutionStep( + status=self._status, + task_id=self.task.task_id, + segment_id=self.segment_id, + call_index=self._call_index, + eligible_mask=self.task.eligible_mask, + runner_step=runner_step, + pending_effect=pending_effect, + message=message, + ) + self._last_step = step + return step + + +def _validate_identifier(value: str, *, name: str) -> str: + """Return one exact non-empty identifier.""" + 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.") + return value + + +def _normalize_eligible_mask( + eligible_mask: torch.Tensor | None, + context: PlanningContext, +) -> torch.Tensor: + """Return one owned eligibility mask matching an observed context.""" + if eligible_mask is None: + return torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ) + if not isinstance(eligible_mask, torch.Tensor): + raise TypeError("eligible_mask must be a torch.Tensor or None.") + if eligible_mask.dtype != torch.bool or eligible_mask.shape != ( + context.batch_size, + ): + raise ValueError("eligible_mask must be bool with one value per environment.") + if eligible_mask.device != context.robot.qpos.device: + raise ValueError("eligible_mask and the planning context must share a device.") + return eligible_mask.clone() + + +def _validate_context_progress( + previous: PlanningContext, + current: PlanningContext, +) -> None: + """Validate monotonic observations across semantic call sessions.""" + if not torch.equal(previous.env_ids, current.env_ids): + raise ValueError("Semantic task env_ids must remain stable and ordered.") + if current.robot.timestamp < previous.robot.timestamp: + raise ValueError("Semantic task robot timestamps must be monotonic.") + if current.scene.timestamp < previous.scene.timestamp: + raise ValueError("Semantic task scene timestamps must be monotonic.") + if current.scene.version < previous.scene.version: + raise ValueError("Semantic task scene versions must be monotonic.") + previous_revisions = previous.scene.collision_world_revisions(previous.batch_size) + current_revisions = current.scene.collision_world_revisions(current.batch_size) + if any( + current_revision < previous_revision + for previous_revision, current_revision in zip( + previous_revisions, + current_revisions, + strict=True, + ) + ): + raise ValueError("Semantic task collision-world revisions must be monotonic.") + + +__all__ = [ + "SemanticCallRecord", + "SemanticEffectVerifier", + "SemanticExecution", + "SemanticExecutionStatus", + "SemanticExecutionStep", + "SemanticSegmentResult", + "SemanticSkillRuntime", + "SemanticTask", + "SemanticTaskResult", + "SemanticTaskStatus", +] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py index 93076a03a..325e126c7 100644 --- a/embodichain/lab/sim/skills/scene.py +++ b/embodichain/lab/sim/skills/scene.py @@ -31,7 +31,9 @@ from embodichain.lab.sim.common import BatchEntity from embodichain.lab.sim.atomic_actions import ( Affordance, + AntipodalAffordance, EntityState, + ObjectSemantics, SceneProvider, SceneSnapshot, ) @@ -43,13 +45,66 @@ RefT = TypeVar("RefT", bound="SceneEntityRef") +GRASP_AFFORDANCE_CAPABILITY = "affordance.grasp" +"""Capability for an affordance usable by object pickup or handover.""" + +PLACE_ON_AFFORDANCE_CAPABILITY = "affordance.place.on" +"""Capability for an affordance that defines an ``on`` placement relation.""" + +PLACE_IN_AFFORDANCE_CAPABILITY = "affordance.place.in" +"""Capability for an affordance that defines an ``inside`` placement relation.""" + + +class UnsupportedSceneAffordanceError(ValueError): + """Raised when a parent has no affordance for a required capability.""" + + +class AmbiguousSceneAffordanceError(ValueError): + """Raised when compatible affordances lack one explicitly scoped default.""" + def _validate_identifier(value: str, name: str) -> None: """Validate an exact, non-empty identifier without normalizing it.""" - if not isinstance(value, str) or not value or value != value.strip(): + 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 _normalize_affordance_capabilities( + values: Iterable[str], +) -> frozenset[str]: + """Validate one open set of namespaced affordance capabilities.""" + if isinstance(values, (str, bytes)): + raise TypeError( + "affordance_capabilities must be an iterable of strings, not a string." + ) + try: + capabilities = frozenset(values) + except TypeError as exc: + raise TypeError( + "affordance_capabilities must be an iterable of strings." + ) from exc + for capability in capabilities: + _validate_identifier(capability, "affordance capability") + return capabilities + + +def _normalize_default_affordances( + values: Mapping[str, SceneAffordanceRef], +) -> Mapping[str, SceneAffordanceRef]: + """Validate and own a capability-scoped default-affordance mapping.""" + if not isinstance(values, Mapping): + raise TypeError("default_affordances must be a mapping.") + defaults: dict[str, SceneAffordanceRef] = {} + for capability, affordance_ref in values.items(): + _validate_identifier(capability, "default affordance capability") + if type(affordance_ref) is not SceneAffordanceRef: + raise TypeError( + "default_affordances values must be SceneAffordanceRef instances." + ) + defaults[capability] = affordance_ref + return MappingProxyType(defaults) + + @dataclass(frozen=True, slots=True) class SceneEntityRef: """Typed reference to one authoritative scene-registry entity. @@ -109,6 +164,458 @@ class SceneCollisionWorldMode(str, Enum): PER_ENV = "per_env" +@dataclass(frozen=True, slots=True) +class SceneEntityMetadata: + """Provider-free semantic metadata projected from one registration. + + Args: + ref: Canonical typed entity reference. + aliases: Boundary aliases, compared as an order-independent set. + parent: Canonical parent for links and affordances. + native_name: Backend-local child name. + dynamics: Physical mobility classification. + collision_role: Planner collision classification. + semantic_type: Optional application semantic type. + affordance_capabilities: Open capabilities of an affordance. + default_affordances: Capability-scoped direct-child defaults. + affordance_payload_type: Exact registered affordance value type. + affordance_revision: Integrator-owned payload revision or fingerprint. + relative_pose: Flattened parent-relative 4x4 pose, when declared. + """ + + ref: SceneEntityRef + aliases: tuple[str, ...] = () + parent: SceneEntityRef | None = None + native_name: str | None = None + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + semantic_type: str | None = None + affordance_capabilities: frozenset[str] = frozenset() + default_affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) + affordance_payload_type: type[Affordance] | None = None + affordance_revision: str | None = None + relative_pose: tuple[float, ...] | None = None + + def __post_init__(self) -> None: + allowed_ref_types = { + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + } + if type(self.ref) not in allowed_ref_types: + raise TypeError("SceneEntityMetadata.ref must be a SceneEntityRef.") + if isinstance(self.aliases, (str, bytes)): + raise TypeError("SceneEntityMetadata.aliases must be an iterable.") + aliases = tuple(sorted(set(self.aliases))) + for alias in aliases: + _validate_identifier(alias, "scene alias") + object.__setattr__(self, "aliases", aliases) + if self.parent is not None and type(self.parent) not in allowed_ref_types: + raise TypeError("SceneEntityMetadata.parent must be a SceneEntityRef.") + if self.native_name is not None: + _validate_identifier(self.native_name, "native_name") + if not isinstance(self.dynamics, SceneDynamics): + raise TypeError("SceneEntityMetadata.dynamics must be SceneDynamics.") + if not isinstance(self.collision_role, SceneCollisionRole): + raise TypeError( + "SceneEntityMetadata.collision_role must be SceneCollisionRole." + ) + if self.semantic_type is not None: + _validate_identifier(self.semantic_type, "semantic_type") + object.__setattr__( + self, + "affordance_capabilities", + _normalize_affordance_capabilities(self.affordance_capabilities), + ) + object.__setattr__( + self, + "default_affordances", + _normalize_default_affordances(self.default_affordances), + ) + if self.affordance_payload_type is not None and ( + not isinstance(self.affordance_payload_type, type) + or not issubclass(self.affordance_payload_type, Affordance) + ): + raise TypeError( + "affordance_payload_type must be an Affordance subclass or None." + ) + if self.affordance_revision is not None: + _validate_identifier(self.affordance_revision, "affordance_revision") + if self.relative_pose is not None: + relative_pose = tuple(float(value) for value in self.relative_pose) + if len(relative_pose) != 16 or not all( + math.isfinite(value) for value in relative_pose + ): + raise ValueError( + "SceneEntityMetadata.relative_pose must contain 16 finite values." + ) + object.__setattr__(self, "relative_pose", relative_pose) + self._validate_topology() + + def _validate_topology(self) -> None: + """Apply the typed topology contract without requiring live providers.""" + if isinstance(self.ref, (SceneObjectRef, SceneArticulationRef)): + if self.parent is not None or self.native_name is not None: + raise ValueError( + "Object and articulation metadata cannot declare a parent " + "or native_name." + ) + if self.affordance_capabilities or self.affordance_payload_type is not None: + raise ValueError( + "Object and articulation metadata cannot declare affordance " + "payload capabilities." + ) + if self.affordance_revision is not None or self.relative_pose is not None: + raise ValueError( + "Object and articulation metadata cannot declare affordance " + "revision or relative_pose." + ) + return + if isinstance(self.ref, SceneLinkRef): + if not isinstance(self.parent, SceneArticulationRef) or ( + self.native_name is None + ): + raise ValueError( + "Link metadata requires an articulation parent and native_name." + ) + if self.affordance_capabilities or self.affordance_payload_type is not None: + raise ValueError( + "Link metadata cannot declare affordance payload capabilities." + ) + if self.affordance_revision is not None or self.relative_pose is not None: + raise ValueError( + "Link metadata cannot declare affordance revision or relative_pose." + ) + return + if isinstance(self.ref, SceneAffordanceRef): + if ( + not isinstance( + self.parent, + (SceneObjectRef, SceneArticulationRef, SceneLinkRef), + ) + or self.native_name is None + ): + raise ValueError( + "Affordance metadata requires an object, articulation, or link " + "parent and native_name." + ) + if self.affordance_payload_type is None: + raise ValueError( + "Affordance metadata requires affordance_payload_type." + ) + if self.default_affordances: + raise ValueError( + "Affordance metadata cannot declare default_affordances." + ) + if self.affordance_capabilities and self.affordance_revision is None: + raise ValueError( + "Capability-bearing affordance metadata requires an explicit " + "affordance_revision." + ) + if ( + GRASP_AFFORDANCE_CAPABILITY in self.affordance_capabilities + and not issubclass(self.affordance_payload_type, AntipodalAffordance) + ): + raise TypeError( + f"{GRASP_AFFORDANCE_CAPABILITY!r} requires an " + "AntipodalAffordance payload." + ) + return + if self.parent is not None or self.native_name is not None: + raise ValueError("Generic scene metadata cannot declare a parent.") + if self.affordance_capabilities or self.affordance_payload_type is not None: + raise ValueError( + "Generic scene metadata cannot declare affordance capabilities." + ) + if self.default_affordances: + raise ValueError( + "Generic scene metadata cannot declare default_affordances." + ) + if self.affordance_revision is not None or self.relative_pose is not None: + raise ValueError( + "Generic scene metadata cannot declare affordance revision or pose." + ) + + @classmethod + def from_registration( + cls, + registration: SceneEntityRegistration, + ) -> SceneEntityMetadata: + """Project semantic metadata without copying a live payload/provider.""" + relative_pose = registration.relative_pose + return cls( + ref=registration.ref, + aliases=registration.aliases, + parent=registration.parent, + native_name=registration.native_name, + dynamics=registration.dynamics, + collision_role=registration.collision_role, + semantic_type=registration.semantic_type, + affordance_capabilities=registration.affordance_capabilities, + default_affordances=registration.default_affordances, + affordance_payload_type=( + None + if registration.affordance is None + else type(registration.affordance) + ), + affordance_revision=registration.affordance_revision, + relative_pose=( + None + if relative_pose is None + else tuple(relative_pose.detach().cpu().reshape(-1).tolist()) + ), + ) + + +@dataclass(frozen=True, slots=True, init=False) +class _SceneMetadataIndex: + """Shared identity and affordance index for static and live scene catalogs.""" + + entries: tuple[SceneEntityMetadata, ...] + by_id: Mapping[str, SceneEntityMetadata] + aliases: Mapping[str, str] + affordances_by_parent_capability: Mapping[ + tuple[str, str], tuple[SceneAffordanceRef, ...] + ] + + def __init__(self, entries: Iterable[SceneEntityMetadata] = ()) -> None: + try: + supplied = tuple(entries) + except TypeError as exc: + raise TypeError("entries must be an iterable of scene metadata.") from exc + if not all(isinstance(entry, SceneEntityMetadata) for entry in supplied): + raise TypeError("entries must contain SceneEntityMetadata values.") + + by_id: dict[str, SceneEntityMetadata] = {} + for entry in supplied: + entity_id = entry.ref.entity_id + if entity_id in by_id: + raise ValueError(f"Duplicate canonical scene entity ID {entity_id!r}.") + by_id[entity_id] = entry + + aliases: dict[str, str] = {} + canonical_ids = set(by_id) + for entry in supplied: + canonical_id = entry.ref.entity_id + for alias in entry.aliases: + if alias in canonical_ids: + raise ValueError( + f"Scene alias {alias!r} collides with canonical entity ID " + f"{alias!r}." + ) + previous = aliases.get(alias) + if previous is not None: + raise ValueError( + f"Scene alias {alias!r} is ambiguous between canonical " + f"IDs {previous!r} and {canonical_id!r}." + ) + aliases[alias] = canonical_id + + self._validate_relationships(supplied, by_id) + affordances = self._index_affordances(supplied) + object.__setattr__(self, "entries", supplied) + object.__setattr__(self, "by_id", MappingProxyType(by_id)) + object.__setattr__(self, "aliases", MappingProxyType(aliases)) + object.__setattr__( + self, + "affordances_by_parent_capability", + MappingProxyType(affordances), + ) + + @staticmethod + def _validate_relationships( + entries: tuple[SceneEntityMetadata, ...], + by_id: Mapping[str, SceneEntityMetadata], + ) -> None: + """Validate canonical parents, native members, and scoped defaults.""" + native_members: dict[tuple[type[SceneEntityRef], str, str], str] = {} + for entry in entries: + parent = entry.parent + if parent is None: + continue + if parent.entity_id == entry.ref.entity_id: + raise ValueError( + f"Scene entity {entry.ref.entity_id!r} cannot parent itself." + ) + parent_entry = by_id.get(parent.entity_id) + if parent_entry is None: + raise ValueError( + f"Scene entity {entry.ref.entity_id!r} references " + f"unregistered parent {parent.entity_id!r}." + ) + if type(parent_entry.ref) is not type(parent): + raise TypeError( + f"Parent {parent.entity_id!r} is registered as " + f"{type(parent_entry.ref).__name__}, not " + f"{type(parent).__name__}." + ) + if isinstance(entry.ref, (SceneLinkRef, SceneAffordanceRef)): + assert entry.native_name is not None + member_key = ( + type(entry.ref), + parent.entity_id, + entry.native_name, + ) + previous = native_members.get(member_key) + if previous is not None: + raise ValueError( + f"{type(entry.ref).__name__} parent {parent.entity_id!r} " + f"and native_name {entry.native_name!r} are already " + f"registered as canonical ID {previous!r}." + ) + native_members[member_key] = entry.ref.entity_id + + for entry in entries: + for capability, default_ref in entry.default_affordances.items(): + default_entry = by_id.get(default_ref.entity_id) + if default_entry is None: + raise ValueError( + f"Scene entity {entry.ref.entity_id!r} declares unknown " + f"default affordance {default_ref.entity_id!r} for " + f"capability {capability!r}." + ) + if not isinstance(default_entry.ref, SceneAffordanceRef): + raise TypeError( + f"Default affordance {default_ref.entity_id!r} is " + f"registered as {type(default_entry.ref).__name__}, not " + "SceneAffordanceRef." + ) + if default_entry.parent != entry.ref: + actual_parent = default_entry.parent + raise ValueError( + f"Default affordance {default_ref.entity_id!r} is not a " + f"direct child of {entry.ref.entity_id!r}; its parent is " + f"{None if actual_parent is None else actual_parent.entity_id!r}." + ) + if capability not in default_entry.affordance_capabilities: + raise ValueError( + f"Default affordance {default_ref.entity_id!r} does not " + f"declare capability {capability!r}." + ) + + @staticmethod + def _index_affordances( + entries: tuple[SceneEntityMetadata, ...], + ) -> dict[tuple[str, str], tuple[SceneAffordanceRef, ...]]: + """Build deterministic parent/capability reverse lookup entries.""" + mutable: dict[tuple[str, str], list[SceneAffordanceRef]] = {} + for entry in entries: + if not isinstance(entry.ref, SceneAffordanceRef): + continue + assert entry.parent is not None + for capability in entry.affordance_capabilities: + mutable.setdefault((entry.parent.entity_id, capability), []).append( + entry.ref + ) + return { + key: tuple(sorted(refs, key=lambda ref: ref.entity_id)) + for key, refs in mutable.items() + } + + def resolve( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + ) -> RefT: + """Resolve one canonical ID, alias, or typed reference.""" + if not isinstance(expected_type, type) or not issubclass( + expected_type, + SceneEntityRef, + ): + raise TypeError("expected_type must be a SceneEntityRef subclass.") + if isinstance(identifier, SceneEntityRef): + canonical_id = identifier.entity_id + supplied_ref: SceneEntityRef | None = identifier + elif isinstance(identifier, str): + _validate_identifier(identifier, "identifier") + canonical_id = self.aliases.get(identifier, identifier) + supplied_ref = None + else: + raise TypeError("identifier must be a string or SceneEntityRef.") + + entry = self.by_id.get(canonical_id) + if entry is None: + raise KeyError(f"Unknown scene entity {identifier!r}.") + canonical_ref = entry.ref + if supplied_ref is not None and type(supplied_ref) is not type(canonical_ref): + raise TypeError( + f"Scene entity {canonical_id!r} is registered as " + f"{type(canonical_ref).__name__}, not " + f"{type(supplied_ref).__name__}." + ) + if not isinstance(canonical_ref, expected_type): + raise TypeError( + f"Scene entity {canonical_id!r} is " + f"{type(canonical_ref).__name__}, not {expected_type.__name__}." + ) + return canonical_ref # type: ignore[return-value] + + def affordances( + self, + parent: str | SceneEntityRef, + *, + capability: str, + ) -> tuple[SceneAffordanceRef, ...]: + """Return compatible direct-child affordances without selecting one.""" + parent_ref = self.resolve(parent) + _validate_identifier(capability, "affordance capability") + return self.affordances_by_parent_capability.get( + (parent_ref.entity_id, capability), + (), + ) + + def resolve_affordance( + self, + parent: str | SceneEntityRef, + *, + capability: str, + explicit: str | SceneAffordanceRef | None = None, + ) -> SceneAffordanceRef: + """Select one compatible affordance using strict scoped defaults.""" + parent_ref = self.resolve(parent) + candidates = self.affordances(parent_ref, capability=capability) + if explicit is not None: + try: + selected = self.resolve(explicit, expected_type=SceneAffordanceRef) + except (KeyError, TypeError, ValueError) as exc: + raise UnsupportedSceneAffordanceError( + f"Explicit affordance {explicit!r} is not a registered " + "SceneAffordanceRef." + ) from exc + entry = self.by_id[selected.entity_id] + if entry.parent != parent_ref: + raise UnsupportedSceneAffordanceError( + f"Affordance {selected.entity_id!r} is not a direct child of " + f"{parent_ref.entity_id!r}." + ) + if capability not in entry.affordance_capabilities: + raise UnsupportedSceneAffordanceError( + f"Affordance {selected.entity_id!r} does not support " + f"capability {capability!r}." + ) + return selected + if not candidates: + raise UnsupportedSceneAffordanceError( + f"Scene entity {parent_ref.entity_id!r} has no affordance for " + f"capability {capability!r}." + ) + if len(candidates) == 1: + return candidates[0] + default = self.by_id[parent_ref.entity_id].default_affordances.get(capability) + if default is not None: + return self.resolve(default, expected_type=SceneAffordanceRef) + raise AmbiguousSceneAffordanceError( + f"Scene entity {parent_ref.entity_id!r} has multiple affordances for " + f"capability {capability!r}: " + f"{[candidate.entity_id for candidate in candidates]}. Configure " + "default_affordances for this parent and capability or select one " + "explicitly." + ) + + @runtime_checkable class SceneEntityStateProvider(Protocol): """Observe one registered entity for an ordered environment batch.""" @@ -161,6 +668,12 @@ class SceneEntityRegistration: collision_role: Static, dynamic, or no planner collision role. semantic_type: Optional application semantic type. affordance: Affordance value for an affordance registration. + affordance_capabilities: Open semantic operations supported by an + affordance registration. + default_affordances: Capability-to-child mapping owned by a parent + object, articulation, or link registration. + affordance_revision: Stable integrator-owned revision or fingerprint for + capability-bearing affordance payload data. relative_pose: Optional parent-relative affordance transform. """ @@ -194,11 +707,26 @@ class SceneEntityRegistration: affordance: Affordance | None = None """Affordance value owned by a :class:`SceneAffordanceRef` registration.""" + affordance_capabilities: frozenset[str] = frozenset() + """Open semantic capabilities declared by an affordance registration.""" + + default_affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) + """Capability-scoped child affordances selected when multiple are valid.""" + + affordance_revision: str | None = None + """Stable payload revision required by capability-bearing affordances.""" + relative_pose: torch.Tensor | None = None """Optional parent-relative pose when no explicit state provider exists.""" def __post_init__(self) -> None: - if not isinstance(self.ref, SceneEntityRef): + if type(self.ref) not in { + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + }: raise TypeError("ref must be a SceneEntityRef.") if self.state_provider is not None and not isinstance( self.state_provider, @@ -219,7 +747,13 @@ def __post_init__(self) -> None: raise ValueError("aliases must be unique.") object.__setattr__(self, "aliases", aliases) - if self.parent is not None and not isinstance(self.parent, SceneEntityRef): + if self.parent is not None and type(self.parent) not in { + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + }: raise TypeError("parent must be a SceneEntityRef or None.") if self.native_name is not None: _validate_identifier(self.native_name, "native_name") @@ -236,6 +770,18 @@ def __post_init__(self) -> None: _validate_identifier(self.semantic_type, "semantic_type") if self.affordance is not None and not isinstance(self.affordance, Affordance): raise TypeError("affordance must be an Affordance or None.") + object.__setattr__( + self, + "affordance_capabilities", + _normalize_affordance_capabilities(self.affordance_capabilities), + ) + object.__setattr__( + self, + "default_affordances", + _normalize_default_affordances(self.default_affordances), + ) + if self.affordance_revision is not None: + _validate_identifier(self.affordance_revision, "affordance_revision") if self.relative_pose is not None: if not isinstance(self.relative_pose, torch.Tensor): raise TypeError("relative_pose must be a torch.Tensor or None.") @@ -248,6 +794,7 @@ def __post_init__(self) -> None: ) self._validate_reference_contract() + SceneEntityMetadata.from_registration(self) if ( self.collision_role is not SceneCollisionRole.NONE and self.geometry_provider is None @@ -279,6 +826,11 @@ def _validate_reference_contract(self) -> None: raise ValueError( "Affordance values require a SceneAffordanceRef registration." ) + if self.affordance_capabilities: + raise ValueError( + "affordance_capabilities require a SceneAffordanceRef " + "registration." + ) return if isinstance(self.ref, SceneLinkRef): @@ -295,6 +847,11 @@ def _validate_reference_contract(self) -> None: raise ValueError( "Affordance values require a SceneAffordanceRef registration." ) + if self.affordance_capabilities: + raise ValueError( + "affordance_capabilities require a SceneAffordanceRef " + "registration." + ) return if isinstance(self.ref, SceneAffordanceRef): @@ -314,12 +871,25 @@ def _validate_reference_contract(self) -> None: raise ValueError( "Affordance registrations require state_provider or relative_pose." ) + if self.default_affordances: + raise ValueError( + "An affordance registration cannot declare default_affordances." + ) return if self.parent is not None or self.native_name is not None: raise ValueError("Generic entity registrations cannot declare a parent.") if self.state_provider is None: raise ValueError("Generic entity registrations require state_provider.") + if self.affordance_capabilities: + raise ValueError( + "affordance_capabilities require a SceneAffordanceRef registration." + ) + if self.default_affordances: + raise ValueError( + "Only object, articulation, or link registrations may declare " + "default_affordances." + ) def _copy_registration( @@ -368,12 +938,18 @@ def visit(value: object) -> None: visit(affordance) try: - return deepcopy(affordance, memo) + copied = deepcopy(affordance, memo) except Exception as exc: # noqa: BLE001 - normalize opaque metadata failures raise TypeError( f"Affordance {type(affordance).__name__} must contain copyable " "registry metadata." ) from exc + if copied is affordance or type(copied) is not type(affordance): + raise TypeError( + f"Affordance {type(affordance).__name__} must deepcopy to a distinct " + "value of the exact same type." + ) + return copied @dataclass(frozen=True, slots=True, eq=False, init=False) @@ -394,7 +970,7 @@ class SceneRegistry: _registrations: tuple[SceneEntityRegistration, ...] = field(repr=False) _registrations_by_id: Mapping[str, SceneEntityRegistration] = field(repr=False) - _aliases: Mapping[str, str] = field(repr=False) + _metadata_index: _SceneMetadataIndex = field(repr=False) _collision_world_entity_ids: tuple[str, ...] = field(repr=False) _dynamic_collision_entity_ids: tuple[str, ...] = field(repr=False) _static_collision_entity_ids: tuple[str, ...] = field(repr=False) @@ -422,39 +998,18 @@ def __init__( "registrations must contain SceneEntityRegistration values." ) owned = tuple(_copy_registration(item) for item in supplied) - by_id: dict[str, SceneEntityRegistration] = {} - for registration in owned: - entity_id = registration.ref.entity_id - if entity_id in by_id: - raise ValueError(f"Duplicate canonical scene entity ID {entity_id!r}.") - by_id[entity_id] = registration - - aliases: dict[str, str] = {} - canonical_ids = set(by_id) - for registration in owned: - canonical_id = registration.ref.entity_id - for alias in registration.aliases: - if alias in canonical_ids: - raise ValueError( - f"Scene alias {alias!r} collides with canonical entity ID " - f"{alias!r}." - ) - previous = aliases.get(alias) - if previous is not None: - raise ValueError( - f"Scene alias {alias!r} is ambiguous between canonical " - f"IDs {previous!r} and {canonical_id!r}." - ) - aliases[alias] = canonical_id - - self._validate_relationships(owned, by_id) + entity_metadata = tuple( + SceneEntityMetadata.from_registration(item) for item in owned + ) + metadata_index = _SceneMetadataIndex(entity_metadata) + by_id = {registration.ref.entity_id: registration for registration in owned} object.__setattr__(self, "_registrations", owned) object.__setattr__( self, "_registrations_by_id", MappingProxyType(by_id), ) - object.__setattr__(self, "_aliases", MappingProxyType(aliases)) + object.__setattr__(self, "_metadata_index", metadata_index) object.__setattr__( self, "_collision_world_entity_ids", @@ -484,55 +1039,16 @@ def __init__( ) object.__setattr__(self, "collision_world_mode", collision_world_mode) - @staticmethod - def _validate_relationships( - registrations: tuple[SceneEntityRegistration, ...], - by_id: Mapping[str, SceneEntityRegistration], - ) -> None: - """Require every parent to be a canonical, correctly typed ref.""" - native_members: dict[tuple[type[SceneEntityRef], str, str], str] = {} - for registration in registrations: - parent = registration.parent - if parent is None: - continue - if parent.entity_id == registration.ref.entity_id: - raise ValueError( - f"Scene entity {registration.ref.entity_id!r} cannot parent itself." - ) - parent_registration = by_id.get(parent.entity_id) - if parent_registration is None: - raise ValueError( - f"Scene entity {registration.ref.entity_id!r} references " - f"unregistered parent {parent.entity_id!r}." - ) - if type(parent_registration.ref) is not type(parent): - raise TypeError( - f"Parent {parent.entity_id!r} is registered as " - f"{type(parent_registration.ref).__name__}, not " - f"{type(parent).__name__}." - ) - if isinstance(registration.ref, (SceneLinkRef, SceneAffordanceRef)): - assert registration.native_name is not None - member_key = ( - type(registration.ref), - parent.entity_id, - registration.native_name, - ) - previous = native_members.get(member_key) - if previous is not None: - raise ValueError( - f"{type(registration.ref).__name__} parent " - f"{parent.entity_id!r} and native_name " - f"{registration.native_name!r} are already registered as " - f"canonical ID {previous!r}." - ) - native_members[member_key] = registration.ref.entity_id - @property def registrations(self) -> tuple[SceneEntityRegistration, ...]: """Return structurally independent registration values.""" return tuple(_copy_registration(item) for item in self._registrations) + @property + def entity_metadata(self) -> tuple[SceneEntityMetadata, ...]: + """Return provider-free metadata without copying affordance payloads.""" + return self._metadata_index.entries + @property def entity_refs(self) -> tuple[SceneEntityRef, ...]: """Return canonical typed references in registration order.""" @@ -541,7 +1057,7 @@ def entity_refs(self) -> tuple[SceneEntityRef, ...]: @property def aliases(self) -> Mapping[str, str]: """Return the immutable alias-to-canonical-ID index.""" - return self._aliases + return self._metadata_index.aliases @property def collision_world_entity_ids(self) -> tuple[str, ...]: @@ -589,38 +1105,10 @@ def resolve( KeyError: If the canonical ID or alias is unknown. TypeError: If the supplied or resolved reference has the wrong type. """ - if not isinstance(expected_type, type) or not issubclass( - expected_type, - SceneEntityRef, - ): - raise TypeError("expected_type must be a SceneEntityRef subclass.") - supplied_ref: SceneEntityRef | None - if isinstance(identifier, SceneEntityRef): - canonical_id = identifier.entity_id - supplied_ref = identifier - elif isinstance(identifier, str): - _validate_identifier(identifier, "identifier") - canonical_id = self._aliases.get(identifier, identifier) - supplied_ref = None - else: - raise TypeError("identifier must be a string or SceneEntityRef.") - - registration = self._registrations_by_id.get(canonical_id) - if registration is None: - raise KeyError(f"Unknown scene entity {identifier!r}.") - canonical_ref = registration.ref - if supplied_ref is not None and type(supplied_ref) is not type(canonical_ref): - raise TypeError( - f"Scene entity {canonical_id!r} is registered as " - f"{type(canonical_ref).__name__}, not " - f"{type(supplied_ref).__name__}." - ) - if not isinstance(canonical_ref, expected_type): - raise TypeError( - f"Scene entity {canonical_id!r} is " - f"{type(canonical_ref).__name__}, not {expected_type.__name__}." - ) - return canonical_ref # type: ignore[return-value] + return self._metadata_index.resolve( + identifier, + expected_type=expected_type, + ) def lookup( self, @@ -640,6 +1128,98 @@ def lookup( ref = self.resolve(identifier, expected_type=expected_type) return _copy_registration(self._registrations_by_id[ref.entity_id]) + def affordances( + self, + parent: str | SceneEntityRef, + *, + capability: str, + ) -> tuple[SceneAffordanceRef, ...]: + """Return compatible direct-child affordances without selecting one. + + Args: + parent: Canonical ID, alias, or typed parent reference. + capability: Required open affordance capability. + + Returns: + Compatible canonical references sorted by canonical ID. + """ + return self._metadata_index.affordances( + parent, + capability=capability, + ) + + def resolve_affordance( + self, + parent: str | SceneEntityRef, + *, + capability: str, + explicit: str | SceneAffordanceRef | None = None, + ) -> SceneAffordanceRef: + """Select one compatible affordance with strict scoped-default rules. + + Args: + parent: Entity that directly owns the affordance. + capability: Required semantic affordance capability. + explicit: Optional explicit affordance ID or typed reference. + + Returns: + One canonical compatible affordance reference. + + Raises: + UnsupportedSceneAffordanceError: If no compatible affordance exists + or an explicit affordance has the wrong parent/capability. + AmbiguousSceneAffordanceError: If multiple candidates exist without + a scoped default. + """ + return self._metadata_index.resolve_affordance( + parent, + capability=capability, + explicit=explicit, + ) + + def object_semantics( + self, + object_ref: str | SceneObjectRef, + *, + affordance: str | SceneAffordanceRef, + ) -> ObjectSemantics: + """Build one owned atomic-action semantic snapshot. + + Args: + object_ref: Canonical object ID, alias, or typed reference. + affordance: Registered direct-child affordance for the object. + + Returns: + Object semantics with an owned affordance payload and canonical ID. + + Raises: + ValueError: If the affordance does not belong to the object. + """ + canonical_object = self.resolve( + object_ref, + expected_type=SceneObjectRef, + ) + object_registration = self._registrations_by_id[canonical_object.entity_id] + affordance_registration = self.lookup( + affordance, + expected_type=SceneAffordanceRef, + ) + if affordance_registration.parent != canonical_object: + raise ValueError( + f"Affordance {affordance_registration.ref.entity_id!r} is not a " + f"direct child of object {canonical_object.entity_id!r}." + ) + payload = affordance_registration.affordance + if payload is None: + raise AssertionError("Affordance registration lost its payload.") + return ObjectSemantics( + affordance=payload, + geometry={}, + properties={}, + label=object_registration.semantic_type or "none", + entity_id=canonical_object.entity_id, + ) + def make_scene_provider( self, *, @@ -1333,6 +1913,10 @@ def _pose_change_mask( __all__ = [ + "AmbiguousSceneAffordanceError", + "GRASP_AFFORDANCE_CAPABILITY", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", "RegistrySceneProvider", "SceneAffordanceRef", "SceneArticulationRef", @@ -1340,10 +1924,12 @@ def _pose_change_mask( "SceneCollisionWorldMode", "SceneDynamics", "SceneEntityRef", + "SceneEntityMetadata", "SceneEntityRegistration", "SceneEntityStateProvider", "SceneGeometryProvider", "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "UnsupportedSceneAffordanceError", ] diff --git a/scripts/tutorials/semantic_skill/hand_over.py b/scripts/tutorials/semantic_skill/hand_over.py new file mode 100644 index 000000000..e646b592c --- /dev/null +++ b/scripts/tutorials/semantic_skill/hand_over.py @@ -0,0 +1,573 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Use semantic skills to pick with one arm and hand over to the other. + +The workflow contains an object-centric ``Pick`` followed by a registered +dual-arm transfer call. The robot profile chooses the left and right resources; +an explicit lowerer supplies the atomic HandOver goal and embodiment-specific +receive behavior at grounding time. :class:`SemanticSkillRuntime` executes each +call from fresh observations and commits transfer state only after physical +verification. +""" + +from __future__ import annotations + +import argparse +from collections.abc import Mapping +import sys +from pathlib import Path +from typing import ClassVar, TYPE_CHECKING + +_REPO_ROOT = Path(__file__).resolve().parents[3] +if str(_REPO_ROOT) not in sys.path: + sys.path.insert(0, str(_REPO_ROOT)) + +import torch + +from embodichain.lab.sim import SimulationManager +from embodichain.lab.sim.atomic_actions import ( + ControlPartCommandProfile, + EffectVerificationRequest, + GraspGoal, + HandOver as AtomicHandOver, + HandOverOptions, + MotionPolicy, + PlanningContext, + RecoveryPolicy, +) +from embodichain.lab.sim.objects import RigidObject, Robot +from embodichain.lab.sim.skills import ( + GRASP_AFFORDANCE_CAPABILITY, + Pick, + RegisteredSemanticCall, + RegisteredSemanticLowerer, + ResourceBinding, + RobotSkillProfile, + SceneObjectRef, + SceneRegistry, + SemanticCallDescriptor, + SemanticEffectVerifier, + SemanticLowering, + SemanticPose, + SemanticSkillRuntime, + SkillPolicyPreset, + builtin_semantic_call_catalog, +) +from embodichain.utils import logger +from scripts.tutorials.atomic_action.hand_over import ( + HANDOVER_RECORD_LOOK_AT, + HAND_CLOSE_QPOS, + TRAJECTORY_SIM_STEPS, + create_dual_robot, + create_handover_object, + create_support_surface, +) +from scripts.tutorials.atomic_action.scenario_utils import settle_object +from scripts.tutorials.atomic_action.tutorial_utils import ( + clone_local_pose_from_first_env, + create_antipodal_semantics, + create_toppra_motion_generator, + create_tutorial_argument_parser, + create_tutorial_simulation, + get_hand_open_close_qpos, + prepare_tutorial_scene, + publish_tutorial_scene, + run_tutorial, + serve_tutorial_scene, + start_auto_play_recording, + stop_auto_play_recording, +) +from scripts.tutorials.semantic_skill.tutorial_utils import ( + compile_semantic_workflow_for_diagnostics, + create_graspable_object_registry, + create_manipulator_resource, + create_runtime_step_observer, + joint_target_error, + object_to_eef_translation_error, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.skills.integration import BoundSemanticCall + +OBJECT_ID = "workpiece" +OBJECT_SIMULATION_UID = "handover_object" +PICK_SAMPLE_COUNT = 80 +HANDOVER_SAMPLE_COUNT = 140 +MIDDLE_OBJECT_POSITION = (0.0, 0.0, 0.70) +FINAL_OBJECT_POSITION = (0.0, -0.20, 0.70) +OBJECT_QUATERNION_WXYZ = (0.70710678, 0.70710678, 0.0, 0.0) +HANDOVER_CALL_ID = "tutorial.hand_over" +HANDOVER_PRE_GRASP_DISTANCE = 0.08 +HANDOVER_LIFT_HEIGHT = 0.08 +HANDOVER_HAND_INTERP_STEPS = 10 +HANDOVER_HOLD_STEPS = 4 +HANDOVER_RETREAT_STEPS = 28 +HANDOVER_RECEIVE_APPROACH_DIRECTION = (0.0, 0.70710678, -0.70710678) +TRACKING_ERROR_THRESHOLD = 1.0 +MINIMUM_PICK_LIFT = 0.05 +MAXIMUM_HELD_RELATION_ERROR = 0.06 +MAXIMUM_FINAL_POSITION_ERROR = 0.10 +MAXIMUM_HAND_ERROR = 0.03 +POST_EXECUTION_UPDATES = 120 + + +class TutorialHandOverLowerer(RegisteredSemanticLowerer): + """Lower the tutorial's registered transfer call to atomic HandOver.""" + + call_id: ClassVar[str] = HANDOVER_CALL_ID + schema_version: ClassVar[int] = 1 + + def __init__(self, registry: SceneRegistry) -> None: + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + self._registry = registry + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> SemanticLowering: + """Build tuned HandOver options from the latest planning device.""" + del bound + if not isinstance(call.arguments, Mapping): + raise TypeError("tutorial.hand_over arguments must be a mapping.") + object_ref = call.arguments.get("object") + if type(object_ref) is not SceneObjectRef: + raise TypeError("tutorial.hand_over requires a SceneObjectRef object.") + grasp_ref = self._registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + semantics = self._registry.object_semantics( + object_ref, + affordance=grasp_ref, + ) + device = context.robot.qpos.device + return SemanticLowering( + goal=GraspGoal(semantics), + skill_options=HandOverOptions( + receive_pick_object_part="bottom", + middle_object_pose=SemanticPose( + MIDDLE_OBJECT_POSITION, + OBJECT_QUATERNION_WXYZ, + ) + .to_matrix() + .to(device), + final_object_pose=SemanticPose( + FINAL_OBJECT_POSITION, + OBJECT_QUATERNION_WXYZ, + ) + .to_matrix() + .to(device), + pre_grasp_distance=HANDOVER_PRE_GRASP_DISTANCE, + lift_height=HANDOVER_LIFT_HEIGHT, + hand_interp_steps=HANDOVER_HAND_INTERP_STEPS, + hold_steps=HANDOVER_HOLD_STEPS, + retreat_steps=HANDOVER_RETREAT_STEPS, + receive_approach_direction=torch.tensor( + HANDOVER_RECEIVE_APPROACH_DIRECTION, + dtype=torch.float32, + device=device, + ), + ), + ) + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the semantic HandOver tutorial.""" + parser = create_tutorial_argument_parser( + "Execute and verify semantic Pick -> HandOver through the skill runtime.", + features=("diagnose_plan", "grasp_sampling", "headless_play"), + default_device="cpu", + default_renderer="hybrid", + ) + return parser.parse_args() + + +def create_robot_profile( + left_open: torch.Tensor, + left_grasp: torch.Tensor, + right_open: torch.Tensor, + right_grasp: torch.Tensor, +) -> RobotSkillProfile: + """Declare two disjoint manipulators and their semantic skill defaults. + + Args: + left_open: Left-hand joint positions for ``open``. + left_grasp: Left-hand joint positions for ``grasp``. + right_open: Right-hand joint positions for ``open``. + right_grasp: Right-hand joint positions for ``grasp``. + + Returns: + A dual-arm profile with deterministic Pick and HandOver assignments. + """ + return RobotSkillProfile( + profile_id="tutorial.dual_arm", + resources={ + "left": create_manipulator_resource( + "left", + motion_control_part="left_arm", + grasp_control_part="left_hand", + ), + "right": create_manipulator_resource( + "right", + motion_control_part="right_arm", + grasp_control_part="right_hand", + ), + }, + command_profiles={ + "left_hand": ControlPartCommandProfile.joint_positions( + open=left_open, + grasp=left_grasp, + ), + "right_hand": ControlPartCommandProfile.joint_positions( + open=right_open, + grasp=right_grasp, + ), + }, + defaults={ + "pick_up": ResourceBinding({"primary": "left"}), + "hand_over": ResourceBinding({"source": "left", "destination": "right"}), + }, + presets={ + "pick": SkillPolicyPreset( + "pick", + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PICK_SAMPLE_COUNT, + ), + recovery_policy=RecoveryPolicy( + tracking_error_threshold=TRACKING_ERROR_THRESHOLD, + ), + ), + "hand_over": SkillPolicyPreset( + "hand_over", + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=HANDOVER_SAMPLE_COUNT, + ), + # Retrying after either gripper has changed ownership is not + # safe without reconciling the physical attachment first. + recovery_policy=RecoveryPolicy( + max_action_retries=0, + tracking_error_threshold=TRACKING_ERROR_THRESHOLD, + ), + ), + }, + default_preset="pick", + skill_presets={"pick_up": "pick", "hand_over": "hand_over"}, + ) + + +def create_handover_task() -> tuple[Pick, RegisteredSemanticCall]: + """Declare the robot-independent calls submitted at the application entry.""" + object_ref = SceneObjectRef(OBJECT_ID) + return ( + Pick(object=object_ref), + RegisteredSemanticCall( + call_id=HANDOVER_CALL_ID, + arguments={"object": object_ref}, + ), + ) + + +def create_handover_effect_verifier( + obj: RigidObject, + robot: Robot, + *, + left_open: torch.Tensor, + right_grasp: torch.Tensor, +) -> SemanticEffectVerifier: + """Create physical Pick and HandOver verification for the tutorial scene. + + Args: + obj: Object transferred between manipulators. + robot: Dual-arm robot executing the workflow. + left_open: Source-hand release target. + right_grasp: Destination-hand grasp target. + + Returns: + Runtime callback producing a boolean result per environment. + """ + initial_pose = obj.get_local_pose(to_matrix=True) + if not isinstance(initial_pose, torch.Tensor) or initial_pose.dim() != 3: + raise ValueError("The tutorial object pose must have shape (B, 4, 4).") + initial_height = initial_pose[:, 2, 3].clone() + final_position = torch.tensor(FINAL_OBJECT_POSITION, dtype=torch.float32) + + def verify( + call: object, + request: EffectVerificationRequest, + context: PlanningContext, + ) -> torch.Tensor: + object_position = obj.get_local_pose(to_matrix=True)[:, :3, 3] + if type(call) is Pick and request.skill_id == "pick_up": + lift = object_position[:, 2] - initial_height.to(object_position.device) + held = request.expected_effects.held_object_updates.get("left_arm") + if held is None: + raise RuntimeError("Pick verification requires a left-arm attachment.") + held_error = object_to_eef_translation_error( + obj, + robot, + motion_control_part="left_arm", + expected_object_to_eef=held.object_to_eef, + ) + success = (lift >= MINIMUM_PICK_LIFT) & ( + held_error <= MAXIMUM_HELD_RELATION_ERROR + ) + logger.log_info( + "Semantic Pick verification: " + f"lift={lift.detach().cpu().tolist()} m, " + "object-to-left-EEF translation error=" + f"{held_error.detach().cpu().tolist()} m, " + f"success={success.detach().cpu().tolist()}." + ) + elif ( + type(call) is RegisteredSemanticCall + and call.call_id == HANDOVER_CALL_ID + and request.skill_id == "hand_over" + ): + final_error = torch.linalg.vector_norm( + object_position + - final_position.to( + device=object_position.device, + dtype=object_position.dtype, + ), + dim=1, + ) + held = request.expected_effects.held_object_updates.get("right_arm") + if held is None: + raise RuntimeError( + "HandOver verification requires a right-arm attachment." + ) + receiver_error = object_to_eef_translation_error( + obj, + robot, + motion_control_part="right_arm", + expected_object_to_eef=held.object_to_eef, + ) + source_error = joint_target_error( + robot, + control_part="left_hand", + target=left_open, + ) + receiver_hand_error = joint_target_error( + robot, + control_part="right_hand", + target=right_grasp, + ) + success = ( + (final_error <= MAXIMUM_FINAL_POSITION_ERROR) + & (receiver_error <= MAXIMUM_HELD_RELATION_ERROR) + & (source_error <= MAXIMUM_HAND_ERROR) + & (receiver_hand_error <= MAXIMUM_HAND_ERROR) + ) + logger.log_info( + "Semantic HandOver verification: " + f"final_error={final_error.detach().cpu().tolist()} m, " + "object-to-right-EEF translation error=" + f"{receiver_error.detach().cpu().tolist()} m, " + f"source_open_error={source_error.detach().cpu().tolist()} rad, " + "receiver_grasp_error=" + f"{receiver_hand_error.detach().cpu().tolist()} rad, " + f"success={success.detach().cpu().tolist()}." + ) + else: + raise TypeError( + f"Unexpected effect request {request.skill_id!r} for " + f"{type(call).__name__}." + ) + return success.to(context.robot.qpos.device) + + return verify + + +def create_handover_application( + simulation: SimulationManager, + robot: Robot, + obj: RigidObject, + *, + left_open: torch.Tensor, + left_grasp: torch.Tensor, + right_open: torch.Tensor, + right_grasp: torch.Tensor, + n_sample: int, + force_reannotate: bool, +) -> SemanticSkillRuntime: + """Assemble the application-facing runtime for the HandOver tutorial. + + The returned runtime owns the registered call extension, robot binding, + scene catalog, and default physical-effect verifier. Task code only needs + to submit semantic calls through :meth:`SemanticSkillRuntime.run`. + + Args: + simulation: Simulation containing the robot and workpiece. + robot: Dual-arm robot executing the semantic calls. + obj: Workpiece registered under :data:`OBJECT_ID`. + left_open: Left-hand target for the semantic ``open`` command. + left_grasp: Left-hand target for the semantic ``grasp`` command. + right_open: Right-hand target for the semantic ``open`` command. + right_grasp: Right-hand target for the semantic ``grasp`` command. + n_sample: Number of grasp candidates generated during annotation. + force_reannotate: Whether to regenerate cached grasp annotations. + + Returns: + A fully bound semantic runtime with a default effect verifier. + """ + object_semantics = create_antipodal_semantics( + obj, + label="handover object", + n_sample=n_sample, + force_reannotate=force_reannotate, + ) + registry, _ = create_graspable_object_registry( + simulation, + object_id=OBJECT_ID, + simulation_uid=OBJECT_SIMULATION_UID, + semantic_type="handover object", + affordance=object_semantics.affordance, + ) + profile = create_robot_profile( + left_open, + left_grasp, + right_open, + right_grasp, + ) + call_catalog = builtin_semantic_call_catalog().with_descriptor( + SemanticCallDescriptor( + call_id=HANDOVER_CALL_ID, + spec_type=RegisteredSemanticCall, + target_descriptor=AtomicHandOver.descriptor(), + ) + ) + return SemanticSkillRuntime.from_simulation( + simulation=simulation, + robot=robot, + motion_generator=create_toppra_motion_generator(robot), + scene_registry=registry, + robot_profile=profile, + call_catalog=call_catalog, + effect_verifier=create_handover_effect_verifier( + obj, + robot, + left_open=left_open, + right_grasp=right_grasp, + ), + registered_lowerers=(TutorialHandOverLowerer(registry),), + control_dt=TRAJECTORY_SIM_STEPS * simulation.sim_config.physics_dt, + ) + + +def main() -> None: + """Execute and physically verify the semantic dual-arm workflow.""" + args = parse_arguments() + sim = create_tutorial_simulation( + args, + arena_space=3.0, + light_pos=(0.0, -0.4, 3.0), + ) + robot = create_dual_robot(sim, args.robot) + create_support_surface(sim) + obj = create_handover_object(sim) + settle_object(sim, obj, step=0) + clone_local_pose_from_first_env(obj) + obj.clear_dynamics() + publish_tutorial_scene(sim, args) + left_open, left_grasp = get_hand_open_close_qpos( + robot, + hand_control_part="left_hand", + close_qpos=HAND_CLOSE_QPOS, + ) + right_open, right_grasp = get_hand_open_close_qpos( + robot, + hand_control_part="right_hand", + close_qpos=HAND_CLOSE_QPOS, + ) + app = create_handover_application( + sim, + robot=robot, + obj=obj, + left_open=left_open, + left_grasp=left_grasp, + right_open=right_open, + right_grasp=right_grasp, + n_sample=args.n_sample, + force_reannotate=args.force_reannotate, + ) + calls = create_handover_task() + + wait_for_user = prepare_tutorial_scene( + sim, + args, + "Inspect the scene, then press Enter to execute Pick -> HandOver...", + ) + for _ in range(20): + sim.update(step=10) + + if args.diagnose_plan: + try: + trajectory, skill_ids = compile_semantic_workflow_for_diagnostics( + app, + calls, + workflow_id="tutorial.semantic_pick_handover", + ) + except RuntimeError as exc: + logger.log_warning(str(exc)) + return + logger.log_info( + f"Diagnostic compile lowered {' -> '.join(skill_ids)} with " + f"{trajectory.waypoint_count} waypoints." + ) + return + + recording_started = start_auto_play_recording( + sim, + args, + video_prefix="semantic_handover_auto_play", + look_at=HANDOVER_RECORD_LOOK_AT, + ) + try: + result = app.run( + calls, + task_id="tutorial.semantic_pick_handover", + on_step=create_runtime_step_observer( + obj, + robot, + grasp_control_part="left_hand", + grasp_target=left_grasp, + ), + ) + result.require_all_succeeded() + for _ in range(POST_EXECUTION_UPDATES): + app.clock.sleep(sim.sim_config.physics_dt) + finally: + stop_auto_play_recording(sim, recording_started) + logger.log_info( + "Closed-loop semantic Pick -> HandOver completed with " + f"{sum(call.command_count for call in result.segments[0].calls)} " + "accepted commands.", + color="green", + ) + if wait_for_user: + input("Press Enter to exit the simulation...") + serve_tutorial_scene(sim, args) + + +if __name__ == "__main__": + run_tutorial(main) diff --git a/scripts/tutorials/semantic_skill/place.py b/scripts/tutorials/semantic_skill/place.py new file mode 100644 index 000000000..417dbcea6 --- /dev/null +++ b/scripts/tutorials/semantic_skill/place.py @@ -0,0 +1,407 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Use semantic skills to pick a registered cube and place it at an object pose. + +Unlike the direct atomic-action tutorial, this example never names ``arm`` or +``hand`` in the workflow. The scene registry owns object identity, the robot +profile owns embodiment-specific resources, and :class:`SemanticSkillRuntime` +lowers each call from fresh observations, executes it, and commits only +verified effects. +""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +_REPO_ROOT = Path(__file__).resolve().parents[3] +if str(_REPO_ROOT) not in sys.path: + sys.path.insert(0, str(_REPO_ROOT)) + +import torch + +from embodichain.lab.sim import SimulationManager +from embodichain.lab.sim.atomic_actions import ( + ControlPartCommandProfile, + EffectVerificationRequest, + MotionPolicy, + PlanningContext, + RecoveryPolicy, +) +from embodichain.lab.sim.objects import RigidObject, Robot +from embodichain.lab.sim.skills import ( + Pick, + Place, + ResourceBinding, + RobotSkillProfile, + SceneObjectRef, + SemanticEffectVerifier, + SemanticPose, + SemanticSkillRuntime, + SkillPolicyPreset, +) +from embodichain.utils import logger +from scripts.tutorials.atomic_action.place import create_pick_object +from scripts.tutorials.atomic_action.tutorial_utils import ( + add_tutorial_robot, + broadcast_pose_batch, + create_antipodal_semantics, + create_curobo_motion_generator, + create_tutorial_argument_parser, + create_tutorial_simulation, + draw_axis_marker, + get_hand_open_close_qpos, + initialize_pre_pick_robot_pose, + prepare_tutorial_scene, + run_tutorial, + serve_tutorial_scene, + start_auto_play_recording, + stop_auto_play_recording, +) +from scripts.tutorials.semantic_skill.tutorial_utils import ( + compile_semantic_workflow_for_diagnostics, + create_graspable_object_registry, + create_manipulator_resource, + create_runtime_step_observer, + joint_target_error, + object_to_eef_translation_error, +) + +OBJECT_ID = "workpiece" +OBJECT_SIMULATION_UID = "cube" +TARGET_OBJECT_POSITION = (-0.40, 0.48, 0.025) +TARGET_OBJECT_QUATERNION_WXYZ = (1.0, 0.0, 0.0, 0.0) +PICK_SAMPLE_COUNT = 120 +PLACE_SAMPLE_COUNT = 120 +TRAJECTORY_SIM_STEPS = 4 +TRACKING_ERROR_THRESHOLD = 0.25 +MINIMUM_PICK_LIFT = 0.08 +MAXIMUM_HELD_RELATION_ERROR = 0.05 +MAXIMUM_PLACE_POSITION_ERROR = 0.05 +MAXIMUM_OPEN_HAND_ERROR = 0.02 +POST_EXECUTION_UPDATES = 240 + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the semantic Place tutorial.""" + parser = create_tutorial_argument_parser( + "Execute and verify semantic Pick -> Place through the skill runtime.", + features=("diagnose_plan", "grasp_sampling", "visualize_axes"), + ) + return parser.parse_args() + + +def create_robot_profile( + hand_open: torch.Tensor, + hand_grasp: torch.Tensor, +) -> RobotSkillProfile: + """Declare how semantic manipulation maps onto the tutorial robot. + + Args: + hand_open: Joint positions for the semantic ``open`` command. + hand_grasp: Joint positions for the semantic ``grasp`` command. + + Returns: + A profile with one manipulation resource and per-skill policies. + """ + manipulator_id = "primary_manipulator" + return RobotSkillProfile( + profile_id="tutorial.single_arm", + resources={ + manipulator_id: create_manipulator_resource( + manipulator_id, + motion_control_part="arm", + grasp_control_part="hand", + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=hand_open, + grasp=hand_grasp, + ) + }, + defaults={ + "pick_up": ResourceBinding({"primary": manipulator_id}), + "place": ResourceBinding({"primary": manipulator_id}), + }, + presets={ + "pick": SkillPolicyPreset( + "pick", + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PICK_SAMPLE_COUNT, + ), + # The PGI gripper closes in five interpolated commands. Its + # position controller can legitimately trail one command by + # more than the generic 0.05-rad threshold. + recovery_policy=RecoveryPolicy( + tracking_error_threshold=TRACKING_ERROR_THRESHOLD, + ), + ), + "place": SkillPolicyPreset( + "place", + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PLACE_SAMPLE_COUNT, + ), + # A failed release is not safely repeatable without first + # reconciling the physical object state. + recovery_policy=RecoveryPolicy( + max_action_retries=0, + tracking_error_threshold=TRACKING_ERROR_THRESHOLD, + ), + ), + }, + default_preset="pick", + skill_presets={"pick_up": "pick", "place": "place"}, + ) + + +def create_place_task() -> tuple[Pick, Place]: + """Declare the robot-independent calls submitted at the application entry.""" + object_ref = SceneObjectRef(OBJECT_ID) + return ( + Pick(object=object_ref), + Place( + object=object_ref, + at=SemanticPose( + TARGET_OBJECT_POSITION, + TARGET_OBJECT_QUATERNION_WXYZ, + ), + ), + ) + + +def create_place_effect_verifier( + obj: RigidObject, + robot: Robot, + hand_open: torch.Tensor, +) -> SemanticEffectVerifier: + """Create physical Pick and Place verification for the live tutorial scene. + + Args: + obj: Cube manipulated by the workflow. + robot: Robot executing the semantic calls. + hand_open: Joint target representing a released object. + + Returns: + Runtime callback producing a boolean result per environment. + """ + initial_pose = obj.get_local_pose(to_matrix=True) + if not isinstance(initial_pose, torch.Tensor) or initial_pose.dim() != 3: + raise ValueError("The tutorial object pose must have shape (B, 4, 4).") + initial_height = initial_pose[:, 2, 3].clone() + target_position = torch.tensor(TARGET_OBJECT_POSITION, dtype=torch.float32) + + def verify( + call: object, + request: EffectVerificationRequest, + context: PlanningContext, + ) -> torch.Tensor: + object_position = obj.get_local_pose(to_matrix=True)[:, :3, 3] + if type(call) is Pick and request.skill_id == "pick_up": + lift = object_position[:, 2] - initial_height.to(object_position.device) + held = request.expected_effects.held_object_updates.get("arm") + if held is None: + raise RuntimeError("Pick verification requires an arm attachment.") + held_error = object_to_eef_translation_error( + obj, + robot, + motion_control_part="arm", + expected_object_to_eef=held.object_to_eef, + ) + success = (lift >= MINIMUM_PICK_LIFT) & ( + held_error <= MAXIMUM_HELD_RELATION_ERROR + ) + logger.log_info( + "Semantic Pick verification: " + f"lift={lift.detach().cpu().tolist()} m, " + "object-to-EEF translation error=" + f"{held_error.detach().cpu().tolist()} m, " + f"success={success.detach().cpu().tolist()}." + ) + elif type(call) is Place and request.skill_id == "place": + position_error = torch.linalg.vector_norm( + object_position + - target_position.to( + device=object_position.device, + dtype=object_position.dtype, + ), + dim=1, + ) + hand_error = joint_target_error( + robot, + control_part="hand", + target=hand_open, + ) + success = (position_error <= MAXIMUM_PLACE_POSITION_ERROR) & ( + hand_error <= MAXIMUM_OPEN_HAND_ERROR + ) + logger.log_info( + "Semantic Place verification: " + f"position_error={position_error.detach().cpu().tolist()} m, " + f"open_hand_error={hand_error.detach().cpu().tolist()} rad, " + f"success={success.detach().cpu().tolist()}." + ) + else: + raise TypeError( + f"Unexpected effect request {request.skill_id!r} for " + f"{type(call).__name__}." + ) + return success.to(context.robot.qpos.device) + + return verify + + +def create_place_application( + simulation: SimulationManager, + robot: Robot, + obj: RigidObject, + *, + hand_open: torch.Tensor, + hand_grasp: torch.Tensor, + n_sample: int, + force_reannotate: bool, +) -> SemanticSkillRuntime: + """Assemble the application-facing runtime for the Place tutorial. + + The returned runtime owns the scene/profile/compiler binding and the + default physical-effect verifier. Task code only needs to submit semantic + calls through :meth:`SemanticSkillRuntime.run`. + + Args: + simulation: Simulation containing the robot and workpiece. + robot: Robot executing the semantic calls. + obj: Workpiece registered under :data:`OBJECT_ID`. + hand_open: Joint target for the semantic ``open`` command. + hand_grasp: Joint target for the semantic ``grasp`` command. + n_sample: Number of grasp candidates generated during annotation. + force_reannotate: Whether to regenerate cached grasp annotations. + + Returns: + A fully bound semantic runtime with a default effect verifier. + """ + object_semantics = create_antipodal_semantics( + obj, + label="cube", + n_sample=n_sample, + force_reannotate=force_reannotate, + ) + registry, _ = create_graspable_object_registry( + simulation, + object_id=OBJECT_ID, + simulation_uid=OBJECT_SIMULATION_UID, + semantic_type="cube", + affordance=object_semantics.affordance, + ) + return SemanticSkillRuntime.from_simulation( + simulation=simulation, + robot=robot, + motion_generator=create_curobo_motion_generator(robot), + scene_registry=registry, + robot_profile=create_robot_profile(hand_open, hand_grasp), + effect_verifier=create_place_effect_verifier(obj, robot, hand_open), + control_dt=TRAJECTORY_SIM_STEPS * simulation.sim_config.physics_dt, + ) + + +def main() -> None: + """Execute and physically verify the semantic Pick-to-Place workflow.""" + args = parse_arguments() + sim = create_tutorial_simulation(args) + robot = add_tutorial_robot(sim, args.robot) + obj = create_pick_object(sim) + hand_open, hand_grasp = get_hand_open_close_qpos(robot) + initialize_pre_pick_robot_pose(robot, obj, hand_open) + app = create_place_application( + sim, + robot, + obj, + hand_open=hand_open, + hand_grasp=hand_grasp, + n_sample=args.n_sample, + force_reannotate=args.force_reannotate, + ) + calls = create_place_task() + + target_pose = calls[1].at + assert target_pose is not None + if not args.no_vis_eef_axis: + draw_axis_marker( + sim, + "semantic_place_object_target", + broadcast_pose_batch( + target_pose.to_matrix().to(sim.device), + robot.get_qpos().shape[0], + ), + ) + wait_for_user = prepare_tutorial_scene( + sim, + args, + "Inspect the scene, then press Enter to execute Pick -> Place...", + ) + if args.diagnose_plan: + try: + trajectory, skill_ids = compile_semantic_workflow_for_diagnostics( + app, + calls, + workflow_id="tutorial.semantic_pick_place", + ) + except RuntimeError as exc: + logger.log_warning(str(exc)) + return + logger.log_info( + f"Diagnostic compile lowered {' -> '.join(skill_ids)} with " + f"{trajectory.waypoint_count} waypoints." + ) + return + + recording_started = start_auto_play_recording( + sim, + args, + video_prefix="semantic_place_auto_play", + ) + try: + result = app.run( + calls, + task_id="tutorial.semantic_pick_place", + on_step=create_runtime_step_observer( + obj, + robot, + grasp_control_part="hand", + grasp_target=hand_grasp, + ), + ) + result.require_all_succeeded() + for _ in range(POST_EXECUTION_UPDATES): + app.clock.sleep(sim.sim_config.physics_dt) + finally: + stop_auto_play_recording(sim, recording_started) + logger.log_info( + "Closed-loop semantic Pick -> Place completed with " + f"{sum(call.command_count for call in result.segments[0].calls)} " + "accepted commands.", + color="green", + ) + if wait_for_user: + input("Press Enter to exit the simulation...") + serve_tutorial_scene(sim, args) + + +if __name__ == "__main__": + run_tutorial(main) diff --git a/scripts/tutorials/semantic_skill/tutorial_utils.py b/scripts/tutorials/semantic_skill/tutorial_utils.py new file mode 100644 index 000000000..afdf8dc95 --- /dev/null +++ b/scripts/tutorials/semantic_skill/tutorial_utils.py @@ -0,0 +1,331 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Shared construction helpers for semantic-skill tutorials.""" + +from __future__ import annotations + +from collections.abc import Iterable +from dataclasses import replace + +import torch + +from embodichain.lab.sim import SimulationManager +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + ExecutionEventKind, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + RunnerStep, + RunnerStepCallback, + TimedTrajectory, +) +from embodichain.lab.sim.objects import RigidObject, Robot +from embodichain.lab.sim.skills import ( + GRASP_AFFORDANCE_CAPABILITY, + ControlPartEndpoint, + RobotResource, + SceneAffordanceRef, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, + SemanticSkillRuntime, +) +from embodichain.lab.sim.skills.calls import SemanticCallSpec +from embodichain.utils import logger + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } +) + + +def create_graspable_object_registry( + simulation: SimulationManager, + *, + object_id: str, + simulation_uid: str, + semantic_type: str, + affordance: AntipodalAffordance, +) -> tuple[SceneRegistry, SceneObjectRef]: + """Register one live object and its default antipodal grasp affordance. + + Args: + simulation: Simulation containing the selected rigid object. + object_id: Canonical semantic object identifier. + simulation_uid: Backend-local rigid-object identifier. + semantic_type: Human-readable object category. + affordance: Target-local grasp metadata copied into the registry. + + Returns: + The immutable registry and its canonical object reference. + """ + object_ref = SceneObjectRef(object_id) + grasp_ref = SceneAffordanceRef(f"{object_id}.grasp.antipodal") + simulation_registry = SceneRegistry.from_simulation( + simulation, + rigid_objects={object_id: simulation_uid}, + ) + object_registration = replace( + simulation_registry.lookup(object_ref, expected_type=SceneObjectRef), + semantic_type=semantic_type, + default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp_ref}, + ) + registry = SceneRegistry( + ( + object_registration, + SceneEntityRegistration( + ref=grasp_ref, + parent=object_ref, + native_name="antipodal_grasp", + affordance=affordance, + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="antipodal-v1", + relative_pose=torch.eye(4, dtype=torch.float32), + ), + ) + ) + return registry, object_ref + + +def create_manipulator_resource( + resource_id: str, + *, + motion_control_part: str, + grasp_control_part: str, +) -> RobotResource: + """Declare one arm-and-gripper resource for semantic skill binding. + + Args: + resource_id: Stable embodiment-level resource identifier. + motion_control_part: Robot control part used for Cartesian motion. + grasp_control_part: Robot control part used for open/grasp commands. + + Returns: + A resource satisfying the built-in manipulation-skill contracts. + """ + return RobotResource( + resource_id=resource_id, + endpoints={ + "motion": ControlPartEndpoint( + control_part=motion_control_part, + capabilities=_MOTION_CAPABILITIES, + ), + "grasp": ControlPartEndpoint( + control_part=grasp_control_part, + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + + +def compile_semantic_workflow_for_diagnostics( + runtime: SemanticSkillRuntime, + calls: Iterable[SemanticCallSpec], + *, + workflow_id: str, +) -> tuple[TimedTrajectory, tuple[str, ...]]: + """Compile a semantic workflow without physically executing it. + + This helper exists only for the tutorials' ``--diagnose-plan`` path. + Expected effects are projected hypothetically between calls; normal runs + must use :class:`SemanticSkillRuntime` and verify physical effects. + + Args: + runtime: Fully bound semantic runtime. + calls: Ordered semantic calls to analyze and compile. + workflow_id: Stable workflow identifier used in diagnostics. + + Returns: + Concatenated diagnostic trajectory and lowered atomic skill IDs. + + Raises: + RuntimeError: If any call fails to plan for any environment. + """ + workflow = runtime.validate(tuple(calls), workflow_id=workflow_id) + empty_task_state = runtime.engine.initial_context().task + context = runtime.observation_provider.observe(empty_task_state) + trajectories: list[TimedTrajectory] = [] + skill_ids: list[str] = [] + for call_index in range(len(workflow.calls)): + grounded = runtime.compiler.ground(workflow, call_index, context) + compiled = runtime.engine.compile((grounded.invocation,), context) + if not compiled.plan_success.all(): + failed_rows = ( + (~compiled.plan_success) + .nonzero(as_tuple=False) + .flatten() + .detach() + .cpu() + .tolist() + ) + raise RuntimeError( + f"Semantic call {call_index} ({grounded.invocation.skill_id!r}) " + f"failed to plan for environment rows {failed_rows}." + ) + trajectories.append(compiled.trajectory) + skill_ids.append(grounded.invocation.skill_id) + context = compiled.projected_context + return TimedTrajectory.concatenate(trajectories), tuple(skill_ids) + + +def object_to_eef_translation_error( + obj: RigidObject, + robot: Robot, + *, + motion_control_part: str, + expected_object_to_eef: torch.Tensor, +) -> torch.Tensor: + """Compare the observed and expected object-to-EEF translations. + + Args: + obj: Live object whose relative transform is measured. + robot: Robot providing current joint state and forward kinematics. + motion_control_part: Control part identifying the target end effector. + expected_object_to_eef: Relation declared by the pending symbolic effect. + + Returns: + Translation error in metres for every environment. + """ + object_pose = obj.get_local_pose(to_matrix=True) + eef_pose = robot.compute_fk( + qpos=robot.get_qpos(name=motion_control_part), + name=motion_control_part, + to_matrix=True, + ) + if ( + not isinstance(object_pose, torch.Tensor) + or not isinstance(eef_pose, torch.Tensor) + or object_pose.dim() != 3 + or eef_pose.shape != object_pose.shape + or object_pose.shape[-2:] != (4, 4) + ): + raise ValueError("Object and end-effector poses must share shape (B, 4, 4).") + expected = torch.as_tensor( + expected_object_to_eef, + dtype=object_pose.dtype, + device=object_pose.device, + ) + if expected.shape == (4, 4): + expected = expected.unsqueeze(0).expand(object_pose.shape[0], -1, -1) + if expected.shape != object_pose.shape: + raise ValueError( + "Expected object-to-EEF pose must have shape (4, 4) or (B, 4, 4)." + ) + observed = torch.bmm(torch.linalg.inv(object_pose), eef_pose) + return torch.linalg.vector_norm( + observed[:, :3, 3] - expected[:, :3, 3], + dim=1, + ) + + +def joint_target_error( + robot: Robot, + *, + control_part: str, + target: torch.Tensor, +) -> torch.Tensor: + """Measure maximum absolute joint error per environment. + + Args: + robot: Robot providing current control-part positions. + control_part: Control part whose joints are compared. + target: One-dimensional target or a full ``(B, D)`` batch. + + Returns: + Maximum absolute joint error for every environment. + """ + current = robot.get_qpos(name=control_part) + if not isinstance(current, torch.Tensor) or current.dim() != 2: + raise ValueError("Control-part qpos must have shape (B, D).") + expected = torch.as_tensor(target, dtype=current.dtype, device=current.device) + if expected.dim() == 1: + expected = expected.unsqueeze(0).expand(current.shape[0], -1) + if expected.shape != current.shape: + raise ValueError("Joint target must have shape (D,) or match qpos (B, D).") + return torch.amax(torch.abs(current - expected), dim=1) + + +def create_runtime_step_observer( + obj: RigidObject, + robot: Robot, + *, + grasp_control_part: str, + grasp_target: torch.Tensor, + grasp_tolerance: float = 1.0e-2, +) -> RunnerStepCallback: + """Create a runner observer that logs recovery and stabilizes one grasp. + + Args: + obj: Physical object stabilized once the grasp target is reached. + robot: Robot whose gripper state is observed. + grasp_control_part: Control part executing the initial grasp. + grasp_target: Joint target representing a closed grasp. + grasp_tolerance: Maximum joint error before dynamics are cleared once. + + Returns: + Callback accepted by ``SemanticSkillRuntime.run(on_step=...)``. + """ + if grasp_tolerance <= 0.0: + raise ValueError("grasp_tolerance must be greater than zero.") + target = grasp_target.clone() + dynamics_cleared = False + reported_events = { + ExecutionEventKind.REPLANNED, + ExecutionEventKind.TRACKING_ERROR, + ExecutionEventKind.DYNAMIC_GOAL_CHANGED, + ExecutionEventKind.COLLISION_WORLD_CHANGED, + ExecutionEventKind.ACTION_RETRY, + ExecutionEventKind.RECOVERY_EXHAUSTED, + } + + def observe(step: RunnerStep) -> None: + nonlocal dynamics_cleared + if step.tick is not None: + for event in step.tick.events: + if event.kind in reported_events: + env_rows = event.env_mask.nonzero(as_tuple=False).flatten().tolist() + logger.log_info( + f"Runtime event {event.kind.value}: rows={env_rows}; " + f"{event.message}" + ) + if dynamics_cleared: + return + error = joint_target_error( + robot, + control_part=grasp_control_part, + target=target, + ) + if torch.all(error <= grasp_tolerance): + obj.clear_dynamics() + dynamics_cleared = True + + return observe + + +__all__ = [ + "compile_semantic_workflow_for_diagnostics", + "create_graspable_object_registry", + "create_manipulator_resource", + "create_runtime_step_observer", + "joint_target_error", + "object_to_eef_translation_error", +] diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 89118dda4..57b22ca22 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -537,6 +537,27 @@ def test_action_options_do_not_contain_embodiment_resources(options: object) -> assert not any(name.endswith("_qpos") for name in field_names) +def test_pose_options_own_late_bound_relative_transforms() -> None: + relative_pose = torch.eye(4) + target = SceneEntityPose("target", relative_pose=relative_pose) + pick_options = PickUpOptions(downstream_object_target_poses=(target,)) + handover_options = HandOverOptions( + middle_object_pose=target, + final_object_pose=target, + ) + + assert target.relative_pose is not None + target.relative_pose[0, 3] = 9.0 + + pick_target = pick_options.downstream_object_target_poses[0] + assert type(pick_target) is SceneEntityPose + assert pick_target.relative_pose is not None + assert pick_target.relative_pose[0, 3].item() == 0.0 + assert type(handover_options.middle_object_pose) is SceneEntityPose + assert handover_options.middle_object_pose.relative_pose is not None + assert handover_options.middle_object_pose.relative_pose[0, 3].item() == 0.0 + + def test_joint_position_goal_rejects_unsupported_target_type() -> None: with pytest.raises(TypeError, match="torch.Tensor or str"): JointPositionGoal(target=1.0) # type: ignore[arg-type] @@ -1084,6 +1105,7 @@ def test_pick_resolves_late_bound_scene_grasp_and_declares_dependency() -> None: assert held is not None assert torch.allclose(held.grasp_xpos, expected_grasp) assert plan.scene_dependencies == ("target",) + assert plan.scene_dependency_end_segment == "approach" def test_pick_session_replans_when_late_bound_target_moves() -> None: @@ -2089,6 +2111,65 @@ def plan_from_start( ] +def test_handover_replan_resolves_named_targets_from_latest_snapshot() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + HandOver( + default_options=HandOverOptions( + middle_object_pose=SceneEntityPose("target"), + final_object_pose=SceneEntityPose("target"), + ) + ), + ) + semantics = _semantics(entity_id="handover_object") + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"left_arm": _held(semantics)}, + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=GraspGoal(semantics=semantics), + binding=_dual_binding(action, "source", "destination"), + ) + request = action.resolve_request(invocation) + assert action._scene_dependencies(request) == ("target",) + captured: list[torch.Tensor] = [] + original_resolve_matrix = action._resolve_matrix + + def capture_middle(matrix: torch.Tensor, name: str) -> torch.Tensor: + if name == "middle_object_pose": + captured.append(matrix.clone()) + raise RuntimeError("captured target") + return original_resolve_matrix(matrix, name) + + action._resolve_matrix = capture_middle # type: ignore[method-assign] + first_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + first_pose[:, 0, 3] = 0.3 + with pytest.raises(RuntimeError, match="captured target"): + action.plan( + request, + _dual_context( + task, + scene=_target_scene(first_pose, timestamp=0.0, version=0), + ), + ) + second_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + second_pose[:, 0, 3] = 0.7 + with pytest.raises(RuntimeError, match="captured target"): + action.plan( + request, + _dual_context( + task, + scene=_target_scene(second_pose, timestamp=0.0, version=1), + ), + ) + + torch.testing.assert_close(captured[0], first_pose) + torch.testing.assert_close(captured[1], second_pose) + + def test_handover_holds_only_environment_with_ik_failure() -> None: generator = _dual_motion_generator() original_compute_ik = generator.robot.compute_ik.side_effect diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 512f0295a..45351344e 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -18,7 +18,7 @@ from __future__ import annotations -from dataclasses import dataclass, FrozenInstanceError +from dataclasses import dataclass, FrozenInstanceError, replace from unittest.mock import Mock import pytest @@ -598,6 +598,7 @@ def test_scene_entity_pose_is_resolved_late_from_snapshot() -> None: offset = torch.eye(4) offset[2, 3] = 0.1 reference = SceneEntityPose("cup", relative_pose=offset) + offset[2, 3] = 9.0 context = _context( SceneSnapshot( timestamp=1.0, @@ -864,6 +865,25 @@ def test_command_target_authorization_rejects_custom_claim_conflicts() -> None: ) +def test_action_plan_rejects_unknown_scene_dependency_end_segment() -> None: + plan = _action_plan( + _command_sequence( + env_ids=torch.tensor([0, 1], dtype=torch.long), + frame_count=2, + ) + ) + + with pytest.raises( + ValueError, + match="scene_dependency_end_segment must name an ActionPlan segment", + ): + replace( + plan, + scene_dependencies=("target",), + scene_dependency_end_segment="approach", + ) + + def test_action_plan_owns_commands_and_optional_joint_trajectory() -> None: env_ids = torch.tensor([4, 7], dtype=torch.long) commands = _command_sequence(env_ids=env_ids, frame_count=2) diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 7929a98ee..e9b8b5c5f 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -61,6 +61,7 @@ TaskState, TimedCommandSequence, TimedTrajectory, + TrajectorySegment, ) from embodichain.lab.sim.common import BatchEntity from embodichain.lab.sim.atomic_actions.goals import resolve_pose_goal @@ -145,6 +146,28 @@ def _plan( ) +class StagedDynamicAction(DynamicAction): + """Monitor its scene target only during a pre-contact segment.""" + + skill_id: ClassVar[str] = "staged_dynamic" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + return replace( + plan, + segments=( + TrajectorySegment("approach", 0, 1), + TrajectorySegment("manipulate", 1, 2), + ), + scene_dependency_end_segment="approach", + ) + + class FailedEffectAction(EffectAction): """Effect-declaring action whose planner fails for every environment.""" @@ -480,6 +503,64 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +def test_initial_eligibility_is_owned_and_masks_commands() -> None: + engine, _ = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + eligible_mask = torch.tensor([True, False]) + + session = engine.start( + (_invocation(engine),), + initial, + eligible_mask=eligible_mask, + ) + eligible_mask.fill_(False) + tick = session.tick(initial) + + assert session.eligible_mask.tolist() == [True, False] + assert tick.command is not None + assert tick.command.active_mask.tolist() == [True, False] + + +def test_empty_initial_eligibility_fails_without_planning() -> None: + engine, action = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + + session = engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([False, False]), + ) + tick = session.tick(initial) + + assert action.plan_count == 0 + assert tick.status is ExecutionStatus.FAILED + assert tick.command is None + assert tick.eligible_mask.tolist() == [False, False] + + +@pytest.mark.parametrize( + ("eligible_mask", "exception", "message"), + [ + ([True], TypeError, "torch.Tensor"), + (torch.tensor([1, 0]), ValueError, "bool with shape"), + (torch.tensor([True]), ValueError, "bool with shape"), + ], +) +def test_initial_eligibility_is_validated( + eligible_mask: object, + exception: type[Exception], + message: str, +) -> None: + engine, _ = _engine(batch_size=2) + + with pytest.raises(exception, match=message): + engine.start( + (_invocation(engine),), + _context(0.0, (0.0, 0.0), (0.2, 0.4), 0), + eligible_mask=eligible_mask, # type: ignore[arg-type] + ) + + def test_session_commands_schedule_arrivals_and_final_settling() -> None: engine, _ = _engine() engine.register(NonuniformTimingAction()) @@ -555,6 +636,27 @@ def test_scene_motion_replans_late_bound_goal() -> None: assert tick.command is not None +def test_scene_motion_is_ignored_after_dependency_segment_is_dispatched() -> None: + engine, _ = _engine() + action = StagedDynamicAction() + engine.register(action) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start( + (_invocation(engine, skill_id=StagedDynamicAction.skill_id),), + initial, + ) + + approach = session.tick(initial) + after_contact = session.tick(_context(0.1, 0.0, 0.4, 1)) + + assert approach.command is not None + assert after_contact.command is not None + assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED not in { + event.kind for event in after_contact.events + } + assert action.plan_count == 1 + + def test_recovery_replan_rejects_runtime_destination_change() -> None: engine, action = _destination_engine(("first", "second")) invocation = _destination_invocation(engine) diff --git a/tests/sim/skills/test_calls.py b/tests/sim/skills/test_calls.py new file mode 100644 index 000000000..d20ba087d --- /dev/null +++ b/tests/sim/skills/test_calls.py @@ -0,0 +1,432 @@ +# ---------------------------------------------------------------------------- +# 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 immutable, declarative semantic call values.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping +import math + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + SkillBindingContract, + SkillDescriptor, + SkillEndpointRequirement, + SkillResourceSlot, +) +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticCallSpec, + SemanticPose, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneEntityRef, + SceneObjectRef, +) + + +def _identity_pose() -> SemanticPose: + return SemanticPose((0.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0)) + + +def _call_descriptor( + call_id: str, + spec_type: type[SemanticCallSpec], +) -> SemanticCallDescriptor: + if spec_type is not RegisteredSemanticCall: + return builtin_semantic_call_catalog().discover(call_id) + target = builtin_semantic_call_catalog().discover("pick").target_descriptor + assert target is not None + assert target.binding_contract is not None + return SemanticCallDescriptor( + call_id=call_id, + spec_type=spec_type, + target_descriptor=target, + ) + + +def test_semantic_pose_owns_inputs_and_returns_independent_tensors() -> None: + position = torch.tensor([1.0, 2.0, 3.0]) + quaternion = torch.tensor([1.0, 0.0, 0.0, 0.0]) + pose = SemanticPose(position, quaternion) + + position.zero_() + quaternion.zero_() + returned_position = pose.position + returned_quaternion = pose.quaternion_wxyz + returned_position.fill_(9.0) + returned_quaternion.fill_(9.0) + + torch.testing.assert_close(pose.position, torch.tensor([1.0, 2.0, 3.0])) + torch.testing.assert_close( + pose.quaternion_wxyz, + torch.tensor([1.0, 0.0, 0.0, 0.0]), + ) + + +def test_semantic_pose_normalizes_wxyz_quaternion() -> None: + pose = SemanticPose((0.0, 0.0, 0.0), (2.0, 0.0, 0.0, 2.0)) + + expected = torch.tensor( + [math.sqrt(0.5), 0.0, 0.0, math.sqrt(0.5)], + dtype=torch.float32, + ) + torch.testing.assert_close(pose.quaternion_wxyz, expected) + + +def test_semantic_pose_converts_to_homogeneous_matrix() -> None: + pose = SemanticPose((1.0, 2.0, 3.0), (2.0, 0.0, 0.0, 2.0)) + + expected = torch.tensor( + [ + [0.0, -1.0, 0.0, 1.0], + [1.0, 0.0, 0.0, 2.0], + [0.0, 0.0, 1.0, 3.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) + torch.testing.assert_close(pose.to_matrix(), expected, atol=1.0e-6, rtol=1.0e-6) + + +@pytest.mark.parametrize( + "factory", + ( + pytest.param( + lambda resources: Pick( + object=SceneObjectRef("cube"), + resources=resources, + ), + id="pick", + ), + pytest.param( + lambda resources: Place( + object=SceneObjectRef("cube"), + at=_identity_pose(), + resources=resources, + ), + id="place", + ), + pytest.param( + lambda resources: HandOver( + object=SceneObjectRef("cube"), + resources=resources, + ), + id="hand-over", + ), + pytest.param( + lambda resources: RegisteredSemanticCall( + call_id="vendor.navigate", + resources=resources, + ), + id="registered", + ), + ), +) +def test_semantic_calls_snapshot_and_freeze_resources( + factory: Callable[[Mapping[str, str]], SemanticCallSpec], +) -> None: + source = {"actor": "left_arm"} + call = factory(source) + + source["actor"] = "right_arm" + + assert call.resources == {"actor": "left_arm"} + with pytest.raises(TypeError): + call.resources["actor"] = "right_arm" # type: ignore[index] + + +def test_pick_requires_typed_object_and_affordance_references() -> None: + with pytest.raises(TypeError, match="Pick.object"): + Pick(object=SceneEntityRef("cube")) # type: ignore[arg-type] + + with pytest.raises(TypeError, match="Pick.grasp"): + Pick( + object=SceneObjectRef("cube"), + grasp=SceneObjectRef("cube.grasp"), # type: ignore[arg-type] + ) + + +def test_place_requires_exactly_one_destination() -> None: + object_ref = SceneObjectRef("cube") + + with pytest.raises(ValueError, match="exactly one"): + Place(object=object_ref) + with pytest.raises(ValueError, match="exactly one"): + Place( + object=object_ref, + at=_identity_pose(), + on=SceneObjectRef("table"), + ) + + +def test_place_snapshots_absolute_destination_pose() -> None: + destination = _identity_pose() + + call = Place(object=SceneObjectRef("cube"), at=destination) + + assert call.at is not destination + assert call.at is not None + torch.testing.assert_close(call.at.to_matrix(), destination.to_matrix()) + + +def test_handover_normalizes_receiver_as_destination_resource() -> None: + call = HandOver(object=SceneObjectRef("cube"), receiver="right_actor") + + assert call.receiver == "right_actor" + assert call.resources == {"destination": "right_actor"} + + +def test_handover_rejects_conflicting_receiver_resource() -> None: + with pytest.raises(ValueError, match="conflicts"): + HandOver( + object=SceneObjectRef("cube"), + receiver="right_actor", + resources={"destination": "left_actor"}, + ) + + +def test_handover_snapshots_optional_final_target() -> None: + final_target = _identity_pose() + + call = HandOver( + object=SceneObjectRef("cube"), + final_target=final_target, + ) + + assert call.final_target is not final_target + assert call.final_target is not None + torch.testing.assert_close(call.final_target.to_matrix(), final_target.to_matrix()) + + +def test_registered_call_recursively_snapshots_declarative_arguments() -> None: + step = {"object": SceneObjectRef("cube")} + steps = [step] + pose = _identity_pose() + arguments = {"steps": steps, "target": pose} + + call = RegisteredSemanticCall( + call_id="vendor.navigate", + arguments=arguments, + ) + step["object"] = SceneObjectRef("changed") + steps.append({"object": SceneObjectRef("extra")}) + + saved_steps = call.arguments["steps"] + assert isinstance(saved_steps, tuple) + assert len(saved_steps) == 1 + assert saved_steps[0] == {"object": SceneObjectRef("cube")} + saved_target = call.arguments["target"] + assert isinstance(saved_target, SemanticPose) + assert saved_target is not pose + with pytest.raises(TypeError): + call.arguments["new"] = 1 # type: ignore[index] + + +@pytest.mark.parametrize( + "unsafe_value", + ( + pytest.param(lambda: None, id="callable"), + pytest.param(torch.tensor([1.0]), id="tensor"), + pytest.param(object(), id="live-object"), + ), +) +def test_registered_call_rejects_executable_or_live_payloads( + unsafe_value: object, +) -> None: + with pytest.raises(TypeError, match="non-declarative"): + RegisteredSemanticCall( + call_id="vendor.navigate", + arguments={"unsafe": unsafe_value}, + ) + + +def test_registered_call_rejects_non_finite_payload_numbers() -> None: + with pytest.raises(ValueError, match="finite"): + RegisteredSemanticCall( + call_id="vendor.navigate", + arguments={"speed": float("nan")}, + ) + + +@pytest.mark.parametrize( + "call_id", + (".", "vendor.", ".inspect", "vendor..inspect", "Vendor.inspect"), +) +def test_registered_call_rejects_malformed_namespace(call_id: str) -> None: + with pytest.raises(ValueError, match="segments"): + RegisteredSemanticCall(call_id=call_id) + + +def test_registered_call_rejects_cyclic_payload() -> None: + payload: dict[str, object] = {} + payload["self"] = payload + + with pytest.raises(ValueError, match="cyclic"): + RegisteredSemanticCall( + call_id="vendor.inspect", + arguments=payload, + ) + + +def test_registered_call_rejects_string_subclass_identifier() -> None: + class LiveString(str): + live_handle = object() + + with pytest.raises(ValueError, match="non-empty string"): + RegisteredSemanticCall(call_id=LiveString("vendor.inspect")) + + +def test_semantic_call_catalog_discovers_without_mutable_runtime_state() -> None: + pick_descriptor = _call_descriptor(Pick.call_kind, Pick) + catalog = SemanticCallCatalog([pick_descriptor]) + + assert catalog.discover("pick") is pick_descriptor + assert catalog.discover(Pick(object=SceneObjectRef("cube"))) is pick_descriptor + with pytest.raises(TypeError): + catalog.descriptors["other"] = pick_descriptor # type: ignore[index] + + +def test_semantic_call_catalog_extension_does_not_mutate_original() -> None: + pick_descriptor = _call_descriptor(Pick.call_kind, Pick) + extension = _call_descriptor("vendor.navigate", RegisteredSemanticCall) + original = SemanticCallCatalog([pick_descriptor]) + + extended = original.with_descriptor(extension) + + with pytest.raises(KeyError, match="Unknown semantic call"): + original.discover("vendor.navigate") + assert ( + extended.discover(RegisteredSemanticCall(call_id="vendor.navigate")) + is extension + ) + + +def test_semantic_call_catalog_rejects_duplicate_ids() -> None: + descriptor = _call_descriptor(Pick.call_kind, Pick) + + with pytest.raises(ValueError, match="Duplicate semantic call ID"): + SemanticCallCatalog([descriptor, descriptor]) + + +def test_catalog_rejects_executable_call_subclasses() -> None: + class UnsafeRegisteredCall(RegisteredSemanticCall): + pass + + with pytest.raises(TypeError, match="exactly"): + SemanticCallDescriptor( + call_id="vendor.unsafe", + spec_type=UnsafeRegisteredCall, + ) + + +def test_registered_payload_rejects_value_subclasses() -> None: + class LiveInteger(int): + live_handle = object() + + with pytest.raises(TypeError, match="non-declarative"): + RegisteredSemanticCall( + call_id="vendor.unsafe", + arguments={"value": LiveInteger(1)}, + ) + + +def test_builtin_descriptor_target_cannot_be_remapped() -> None: + target = builtin_semantic_call_catalog().discover("pick").target_descriptor + assert target is not None + remapped = SkillDescriptor( + skill_id="move_joints", + goal_type=target.goal_type, + options_type=target.options_type, + binding_contract=SkillBindingContract(), + ) + + with pytest.raises(ValueError, match="exact curated"): + SemanticCallDescriptor( + call_id=Pick.call_kind, + spec_type=Pick, + target_descriptor=remapped, + ) + + +def test_catalog_rejects_descriptor_subclass_with_live_state() -> None: + class LiveDescriptor(SemanticCallDescriptor): + live_handle = object() + + source = _call_descriptor("vendor.inspect", RegisteredSemanticCall) + descriptor = LiveDescriptor( + call_id=source.call_id, + spec_type=source.spec_type, + target_descriptor=source.target_descriptor, + ) + + with pytest.raises(TypeError, match="exact SemanticCallDescriptor"): + SemanticCallCatalog((descriptor,)) + + +def test_descriptor_rejects_runtime_bearing_binding_contract_subclasses() -> None: + class LiveSlot(SkillResourceSlot): + live_handle = object() + + target = builtin_semantic_call_catalog().discover("pick").target_descriptor + assert target is not None + endpoint = SkillEndpointRequirement("motion") + contract = SkillBindingContract(slots=(LiveSlot("primary", (endpoint,)),)) + remapped_target = SkillDescriptor( + skill_id=target.skill_id, + goal_type=target.goal_type, + options_type=target.options_type, + binding_contract=contract, + ) + + with pytest.raises(TypeError, match="exact SkillResourceSlot"): + SemanticCallDescriptor( + call_id="vendor.inspect", + spec_type=RegisteredSemanticCall, + target_descriptor=remapped_target, + ) + + +def test_descriptor_rejects_target_descriptor_subclass() -> None: + class LiveTarget(SkillDescriptor): + live_handle = object() + + target = builtin_semantic_call_catalog().discover("pick").target_descriptor + assert target is not None + live_target = LiveTarget( + skill_id=target.skill_id, + goal_type=target.goal_type, + options_type=target.options_type, + agent_visible=target.agent_visible, + binding_contract=target.binding_contract, + ) + assert target.binding_contract is not None + + with pytest.raises(TypeError, match="exactly SkillDescriptor"): + SemanticCallDescriptor( + call_id="vendor.inspect", + spec_type=RegisteredSemanticCall, + target_descriptor=live_target, + ) diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py new file mode 100644 index 000000000..5a1e8aca7 --- /dev/null +++ b/tests/sim/skills/test_compiler.py @@ -0,0 +1,903 @@ +# ---------------------------------------------------------------------------- +# 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 static semantic analysis and JIT invocation lowering.""" + +from __future__ import annotations + +from types import MethodType +from typing import ClassVar +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AntipodalAffordance, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + EntityState, + ExecutionStatus, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + GraspGoal, + HandOverOptions, + HeldObjectState, + ObjectSemantics, + PickUp, + PickUpOptions, + PlaceGoal, + PlanningContext, + RobotObservation, + SceneEntityPose, + TaskState, +) +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallDescriptor, + SemanticPose, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.compiler import ( + HandOverPoseProvider, + HandOverPoseTargets, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticLowering, + SemanticObjectTarget, + SemanticRelationTarget, + SemanticSkillCompiler, +) +from embodichain.lab.sim.skills.integration import ( + BoundSemanticCall, + SceneManifest, + SemanticIntegrationManifest, + SemanticValidationError, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + ResourceBinding, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import ( + GRASP_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } +) +_PICK_TARGET = PickUp.descriptor() + + +class _PoseProvider: + """Return a fixed pose while exposing observation call count.""" + + def __init__(self, pose: torch.Tensor) -> None: + self.pose = pose + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + return EntityState(self.pose) + + +class _FrameRelationGrounder(RelationTargetGrounder): + """Explicit test contract: relation frame equals target object frame.""" + + capability: ClassVar[str] = PLACE_ON_AFFORDANCE_CAPABILITY + affordance_type: ClassVar[type[Affordance]] = Affordance + affordance_revision: ClassVar[str] = "relation-v1" + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> SceneEntityPose: + del affordance, context + return SceneEntityPose(relation.affordance.entity_id) + + +class _InspectLowerer(RegisteredSemanticLowerer): + """Test extension proving a lowerer cannot replace compiler ownership.""" + + call_id: ClassVar[str] = "vendor.inspect" + schema_version: ClassVar[int] = 1 + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: object, + ) -> SemanticLowering: + del call, context, bound + return SemanticLowering( + goal=GraspGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + entity_id="cube", + ) + ), + skill_options=PickUpOptions(), + ) + + +class _DerivedGraspGoal(GraspGoal): + """Executable subclass that an extension must not smuggle into the core.""" + + +class _DerivedPickUpOptions(PickUpOptions): + """Options subclass that must fail the registered target contract.""" + + +class _SubclassOutputLowerer(RegisteredSemanticLowerer): + """Try to bypass exact target contracts with executable subclasses.""" + + call_id: ClassVar[str] = "vendor.inspect" + schema_version: ClassVar[int] = 1 + + def __init__(self, output: str) -> None: + self.output = output + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> SemanticLowering: + del call, context, bound + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + entity_id="cube", + ) + if self.output == "goal": + return SemanticLowering( + goal=_DerivedGraspGoal(semantics=semantics), + skill_options=PickUpOptions(), + ) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + skill_options=_DerivedPickUpOptions(), + ) + + +class _DualCenterHandOverProvider(HandOverPoseProvider): + """Resolve named dual-arm handover poses without observing during analysis.""" + + provider_id: ClassVar[str] = "dual_center" + + def __init__(self) -> None: + self.calls = 0 + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + del call, context, bound + self.calls += 1 + return HandOverPoseTargets( + middle=SemanticObjectTarget(SceneEntityPose("table_top")), + final=SemanticObjectTarget( + SemanticPose( + (0.5, 0.0, 0.4), + (1.0, 0.0, 0.0, 0.0), + ) + ), + ) + + +def _scene_registry() -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider]]: + cube_provider = _PoseProvider(torch.eye(4).repeat(2, 1, 1)) + table_pose = torch.eye(4).repeat(2, 1, 1) + table_pose[:, 0, 3] = 0.6 + table_provider = _PoseProvider(table_pose) + cube = SceneObjectRef("cube") + table = SceneObjectRef("table") + grasp = SceneAffordanceRef("cube_grasp") + table_top = SceneAffordanceRef("table_top") + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=cube, + state_provider=cube_provider, + semantic_type="cube", + default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp}, + ), + SceneEntityRegistration( + ref=grasp, + parent=cube, + native_name="grasp", + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="grasp-v1", + relative_pose=torch.eye(4), + ), + SceneEntityRegistration( + ref=table, + state_provider=table_provider, + semantic_type="table", + default_affordances={PLACE_ON_AFFORDANCE_CAPABILITY: table_top}, + ), + SceneEntityRegistration( + ref=table_top, + parent=table, + native_name="top", + affordance=Affordance(), + affordance_capabilities=frozenset({PLACE_ON_AFFORDANCE_CAPABILITY}), + affordance_revision="relation-v1", + relative_pose=torch.eye(4), + ), + ) + ) + return registry, (cube_provider, table_provider) + + +def _profile() -> RobotSkillProfile: + return RobotSkillProfile( + profile_id="test_robot", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=_MOTION_CAPABILITIES, + ), + "grasp": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), + grasp=torch.tensor([1.0]), + ) + }, + presets={"safe": SkillPolicyPreset("safe")}, + default_preset="safe", + ) + + +def _dual_profile(*, provider_id: str | None = "dual_center") -> RobotSkillProfile: + resources = { + side: RobotResource( + resource_id=side, + endpoints={ + "motion": ControlPartEndpoint( + control_part=f"{side}_arm", + capabilities=_MOTION_CAPABILITIES, + ), + "grasp": ControlPartEndpoint( + control_part=f"{side}_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + for side in ("left", "right") + } + return RobotSkillProfile( + profile_id="dual_robot", + resources=resources, + command_profiles={ + f"{side}_hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), + grasp=torch.tensor([1.0]), + ) + for side in ("left", "right") + }, + defaults={ + "pick_up": ResourceBinding({"primary": "left"}), + "hand_over": ResourceBinding({"source": "left", "destination": "right"}), + }, + presets={"safe": SkillPolicyPreset("safe")}, + default_preset="safe", + grounding_providers=({} if provider_id is None else {"hand_over": provider_id}), + ) + + +def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: + robot = Mock() + robot.device = torch.device("cpu") + control_parts = tuple( + sorted( + { + endpoint.control_part + for resource in profile.resources.values() + for endpoint in resource.endpoints.values() + if type(endpoint) is ControlPartEndpoint + } + ) + ) + joint_ids = {name: [index] for index, name in enumerate(control_parts)} + robot.dof = len(control_parts) + robot.control_parts = {name: object() for name in control_parts} + robot.get_qpos.return_value = torch.zeros(2, robot.dof) + robot.get_qvel.return_value = torch.zeros(2, robot.dof) + robot.get_joint_ids.side_effect = lambda name: joint_ids[name] + robot.get_solver.return_value = object() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + return AtomicActionEngine(generator, skill_profile=profile) + + +def _integration( + registry: SceneRegistry, + *, + registered: bool = False, +) -> tuple[SemanticIntegrationManifest, AtomicActionEngine]: + profile = _profile() + catalog = builtin_semantic_call_catalog() + if registered: + assert _PICK_TARGET.binding_contract is not None + catalog = catalog.with_descriptor( + SemanticCallDescriptor( + call_id="vendor.inspect", + spec_type=RegisteredSemanticCall, + target_descriptor=_PICK_TARGET, + ) + ) + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=catalog, + ) + return manifest, _engine(profile) + + +def _compiler( + registry: SceneRegistry, + *, + registered: bool = False, + relation_grounders: tuple[RelationTargetGrounder, ...] = ( + _FrameRelationGrounder(), + ), + registered_lowerers: tuple[RegisteredSemanticLowerer, ...] = (), +) -> tuple[SemanticSkillCompiler, AtomicActionEngine]: + manifest, engine = _integration(registry, registered=registered) + bound = manifest.bind(registry, engine) + return ( + SemanticSkillCompiler( + bound, + relation_grounders=relation_grounders, + registered_lowerers=registered_lowerers, + ), + engine, + ) + + +def _context( + registry: SceneRegistry, + *, + task: TaskState | None = None, + timestamp: float = 0.0, + robot_dof: int = 2, +) -> PlanningContext: + env_ids = torch.tensor([0, 1], dtype=torch.long) + scene = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ).snapshot(timestamp=timestamp, env_ids=env_ids) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=torch.zeros(2, robot_dof), + qvel=torch.zeros(2, robot_dof), + ), + task=TaskState.empty(2, "cpu") if task is None else task, + scene=scene, + env_ids=env_ids, + ) + + +def _held_context( + registry: SceneRegistry, + semantics: ObjectSemantics, + object_to_eef: torch.Tensor, + *, + env_mask: torch.Tensor | None = None, + control_part: str = "arm", + robot_dof: int = 2, +) -> PlanningContext: + held = HeldObjectState( + semantics=semantics, + object_to_eef=object_to_eef, + grasp_xpos=torch.eye(4).repeat(2, 1, 1), + env_mask=env_mask, + ) + return _context( + registry, + task=TaskState( + batch_size=2, + device="cpu", + held_objects={control_part: held}, + ), + robot_dof=robot_dof, + ) + + +def test_analysis_is_provider_free_and_propagates_object_target() -> None: + registry, providers = _scene_registry() + compiler, engine = _compiler(registry) + drop = SemanticPose((0.4, 0.2, 0.3), (1.0, 0.0, 0.0, 0.0)) + + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + Place(object=SceneObjectRef("cube"), at=drop), + ), + workflow_id="pick_place", + ) + + assert [provider.calls for provider in providers] == [0, 0] + assert workflow.calls[0].downstream_object_target is not None + assert workflow.calls[0].downstream_object_target.pose is not drop + assert workflow.effect_dependencies[0].producer_index == 0 + context = _context(registry) + grounded = compiler.ground(workflow, 0, context) + assert type(grounded.invocation.goal) is GraspGoal + assert grounded.invocation.goal.semantics.entity_id == "cube" + options = grounded.invocation.skill_options + assert type(options) is PickUpOptions + torch.testing.assert_close( + options.downstream_object_target_poses[0], + drop.to_matrix(), + ) + engine.resolve(grounded.invocation) + + +def test_grounded_eligibility_hands_off_to_execution_session() -> None: + registry, _ = _scene_registry() + compiler, engine = _compiler(registry) + context = _context(registry) + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + eligible_mask = torch.tensor([False, False]) + + grounded = compiler.ground( + workflow, + 0, + context, + eligible_mask=eligible_mask, + ) + eligible_mask.fill_(True) + session = engine.start( + (grounded.invocation,), + context, + eligible_mask=grounded.eligible_mask, + ) + + assert grounded.eligible_mask.tolist() == [False, False] + assert session.status is ExecutionStatus.FAILED + assert session.eligible_mask.tolist() == [False, False] + + +def test_pick_relation_lookahead_stays_late_bound_scene_dependency() -> None: + registry, _ = _scene_registry() + compiler, engine = _compiler(registry) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + ), + ) + ) + + grounded = compiler.ground(workflow, 0, _context(registry)) + + options = grounded.invocation.skill_options + assert type(options) is PickUpOptions + assert len(options.downstream_object_target_poses) == 1 + downstream = options.downstream_object_target_poses[0] + assert type(downstream) is SceneEntityPose + assert downstream.entity_id == "table_top" + request = engine.resolve(grounded.invocation) + action = engine.actions["pick_up"] + assert "table_top" in action._scene_dependencies(request) + + +def test_pick_replan_resolves_downstream_target_from_latest_snapshot() -> None: + registry, providers = _scene_registry() + compiler, engine = _compiler(registry) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + ), + ) + ) + first_context = _context(registry, timestamp=0.0) + invocation = compiler.ground(workflow, 0, first_context).invocation + action = engine.actions["pick_up"] + captured: list[torch.Tensor] = [] + + def fail_after_capture( + self: object, + semantics: object, + object_pose: torch.Tensor, + start_qpos: torch.Tensor, + manipulator: object, + options: PickUpOptions, + approach_direction: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + del self, semantics, start_qpos, manipulator, approach_direction + target = options.downstream_object_target_poses[0] + assert isinstance(target, torch.Tensor) + captured.append(target.clone()) + return ( + torch.zeros(2, dtype=torch.bool), + object_pose.clone(), + ) + + action._resolve_grasp_pose = MethodType( # type: ignore[method-assign] + fail_after_capture, + action, + ) + request = engine.resolve(invocation) + engine.plan_request(request, first_context) + moved_table_pose = torch.eye(4).repeat(2, 1, 1) + moved_table_pose[:, 0, 3] = 0.9 + providers[1].pose = moved_table_pose + second_context = _context(registry, timestamp=1.0) + engine.plan_request(request, second_context) + + assert captured[0][0, 0, 3].item() == pytest.approx(0.6) + assert captured[1][0, 0, 3].item() == pytest.approx(0.9) + + +def test_handover_uses_profile_selected_named_provider_and_stops_lookahead() -> None: + registry, providers = _scene_registry() + profile = _dual_profile() + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=builtin_semantic_call_catalog(), + ) + engine = _engine(profile) + provider = _DualCenterHandOverProvider() + compiler = SemanticSkillCompiler( + manifest.bind(registry, engine), + relation_grounders=(_FrameRelationGrounder(),), + handover_pose_providers=(provider,), + ) + final_target = SemanticPose( + (0.8, 0.0, 0.4), + (1.0, 0.0, 0.0, 0.0), + ) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + HandOver( + object=SceneObjectRef("cube"), + final_target=final_target, + ), + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + resources={"primary": "right"}, + ), + ) + ) + + assert provider.calls == 0 + assert [scene_provider.calls for scene_provider in providers] == [0, 0] + assert workflow.calls[0].downstream_object_target is not None + pick = compiler.ground(workflow, 0, _context(registry, robot_dof=4)) + assert provider.calls == 1 + pick_options = pick.invocation.skill_options + assert type(pick_options) is PickUpOptions + assert type(pick_options.downstream_object_target_poses[0]) is SceneEntityPose + assert pick_options.downstream_object_target_poses[0].entity_id == "table_top" + + held_context = _held_context( + registry, + pick.invocation.goal.semantics, + torch.eye(4).repeat(2, 1, 1), + control_part="left_arm", + robot_dof=4, + ) + handover = compiler.ground(workflow, 1, held_context) + assert provider.calls == 2 + options = handover.invocation.skill_options + assert type(options) is HandOverOptions + assert type(options.middle_object_pose) is SceneEntityPose + assert options.middle_object_pose.entity_id == "table_top" + assert options.final_object_pose[0, 3].item() == pytest.approx(0.8) + request = engine.resolve(handover.invocation) + action = engine.actions["hand_over"] + assert action._scene_dependencies(request) == ("table_top",) + action._resolve_start_qpos = Mock( # type: ignore[method-assign] + return_value=(torch.zeros(2, 1), torch.zeros(2, 1)) + ) + captured: list[torch.Tensor] = [] + original_resolve_matrix = action._resolve_matrix + + def capture_middle(matrix: torch.Tensor, name: str) -> torch.Tensor: + if name == "middle_object_pose": + captured.append(matrix.clone()) + raise RuntimeError("captured target") + return original_resolve_matrix(matrix, name) + + action._resolve_matrix = capture_middle # type: ignore[method-assign] + with pytest.raises(RuntimeError, match="captured target"): + engine.plan_request(request, held_context) + moved_table_pose = torch.eye(4).repeat(2, 1, 1) + moved_table_pose[:, 0, 3] = 0.9 + providers[1].pose = moved_table_pose + moved_context = _held_context( + registry, + pick.invocation.goal.semantics, + torch.eye(4).repeat(2, 1, 1), + control_part="left_arm", + robot_dof=4, + ) + with pytest.raises(RuntimeError, match="captured target"): + engine.plan_request(request, moved_context) + + assert captured[0][0, 0, 3].item() == pytest.approx(0.6) + assert captured[1][0, 0, 3].item() == pytest.approx(0.9) + + +def test_handover_requires_profile_selection_and_installed_provider() -> None: + registry, _ = _scene_registry() + call = HandOver(object=SceneObjectRef("cube")) + + unconfigured_profile = _dual_profile(provider_id=None) + unconfigured_manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=unconfigured_profile, + call_catalog=builtin_semantic_call_catalog(), + ) + unconfigured_engine = _engine(unconfigured_profile) + unconfigured = SemanticSkillCompiler( + unconfigured_manifest.bind(registry, unconfigured_engine) + ) + with pytest.raises(SemanticValidationError) as unconfigured_error: + unconfigured.analyze((call,)) + assert unconfigured_error.value.diagnostic.code == "handover_grounding_unconfigured" + + missing_profile = _dual_profile(provider_id="not_installed") + missing_manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=missing_profile, + call_catalog=builtin_semantic_call_catalog(), + ) + missing_engine = _engine(missing_profile) + missing = SemanticSkillCompiler(missing_manifest.bind(registry, missing_engine)) + with pytest.raises(SemanticValidationError) as missing_error: + missing.analyze((call,)) + assert ( + missing_error.value.diagnostic.code + == "handover_grounding_provider_not_installed" + ) + + +def test_relation_call_requires_exact_typed_versioned_grounder() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry, relation_grounders=()) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze( + ( + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + ), + ) + ) + + assert error.value.diagnostic.code == "relation_grounder_not_installed" + + +def test_place_uses_verified_object_to_eef_transform() -> None: + registry, _ = _scene_registry() + compiler, engine = _compiler(registry) + drop = SemanticPose((0.5, -0.2, 0.4), (1.0, 0.0, 0.0, 0.0)) + workflow = compiler.analyze((Place(object=SceneObjectRef("cube"), at=drop),)) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = compiler.ground( + pick_workflow, + 0, + _context(registry), + ).invocation.goal.semantics + object_to_eef = torch.eye(4).repeat(2, 1, 1) + object_to_eef[:, 2, 3] = 0.12 + context = _held_context(registry, semantics, object_to_eef) + + grounded = compiler.ground(workflow, 0, context) + + assert type(grounded.invocation.goal) is PlaceGoal + expected = torch.bmm(drop.to_matrix().repeat(2, 1, 1), object_to_eef) + torch.testing.assert_close(grounded.invocation.goal.xpos, expected) + engine.resolve(grounded.invocation) + + +def test_relation_place_composes_late_target_with_verified_transform() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = compiler.ground( + pick_workflow, + 0, + _context(registry), + ).invocation.goal.semantics + object_to_eef = torch.eye(4).repeat(2, 1, 1) + object_to_eef[:, 0, 3] = 0.08 + context = _held_context(registry, semantics, object_to_eef) + workflow = compiler.analyze( + ( + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + ), + ) + ) + + grounded = compiler.ground(workflow, 0, context) + + goal = grounded.invocation.goal + assert type(goal) is PlaceGoal + assert type(goal.xpos) is SceneEntityPose + assert goal.xpos.entity_id == "table_top" + torch.testing.assert_close(goal.xpos.relative_pose, object_to_eef) + + +def test_place_rejects_wrong_or_inactive_verified_holder() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + workflow = compiler.analyze( + ( + Place( + object=SceneObjectRef("cube"), + at=SemanticPose((0.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0)), + ), + ) + ) + wrong = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + entity_id="other", + ) + wrong_context = _held_context( + registry, + wrong, + torch.eye(4).repeat(2, 1, 1), + ) + + with pytest.raises(SemanticValidationError) as wrong_error: + compiler.ground(workflow, 0, wrong_context) + assert wrong_error.value.diagnostic.code == "verified_held_object_required" + assert wrong_error.value.diagnostic.rendered_path == "workflow[0].call.object" + + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = compiler.ground( + pick_workflow, + 0, + _context(registry), + ).invocation.goal.semantics + partial_context = _held_context( + registry, + semantics, + torch.eye(4).repeat(2, 1, 1), + env_mask=torch.tensor([True, False]), + ) + with pytest.raises(SemanticValidationError) as inactive_error: + compiler.ground(workflow, 0, partial_context) + assert inactive_error.value.diagnostic.code == "verified_held_object_required" + assert inactive_error.value.diagnostic.rendered_path == "workflow[0].call.object" + + grounded = compiler.ground( + workflow, + 0, + partial_context, + eligible_mask=torch.tensor([True, False]), + ) + assert grounded.eligible_mask.tolist() == [True, False] + + +def test_registered_lowerer_is_explicit_and_opaque_to_lookahead() -> None: + registry, _ = _scene_registry() + without_lowerer, _ = _compiler(registry, registered=True) + registered = RegisteredSemanticCall(call_id="vendor.inspect") + + with pytest.raises(SemanticValidationError) as error: + without_lowerer.analyze((registered,)) + assert error.value.diagnostic.code == "semantic_lowerer_not_installed" + + compiler, engine = _compiler( + registry, + registered=True, + registered_lowerers=(_InspectLowerer(),), + ) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + registered, + Place( + object=SceneObjectRef("cube"), + at=SemanticPose((0.3, 0.0, 0.2), (1.0, 0.0, 0.0, 0.0)), + ), + ) + ) + + assert workflow.calls[0].downstream_object_target is None + assert workflow.effect_dependencies[0].producer_index is None + grounded = compiler.ground(workflow, 1, _context(registry)) + assert grounded.invocation.skill_id == "pick_up" + engine.resolve(grounded.invocation) + + +@pytest.mark.parametrize("output", ["goal", "options"]) +def test_registered_lowerer_cannot_return_target_subclasses(output: str) -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_SubclassOutputLowerer(output),), + ) + workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + + with pytest.raises(TypeError, match="produced|incompatible"): + compiler.ground(workflow, 0, _context(registry)) + + +def test_workflow_cannot_cross_compilers() -> None: + registry, _ = _scene_registry() + first, _ = _compiler(registry) + second, _ = _compiler(registry) + workflow = first.analyze((Pick(object=SceneObjectRef("cube")),)) + + with pytest.raises(SemanticValidationError) as error: + second.ground(workflow, 0, _context(registry)) + assert error.value.diagnostic.code == "semantic_program_stale" diff --git a/tests/sim/skills/test_integration.py b/tests/sim/skills/test_integration.py new file mode 100644 index 000000000..028cdd3f3 --- /dev/null +++ b/tests/sim/skills/test_integration.py @@ -0,0 +1,554 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Pure-Python tests for static semantic integration.""" + +from __future__ import annotations + +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AntipodalAffordance, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + EntityState, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, +) +from embodichain.lab.sim.skills.calls import ( + Pick, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.integration import ( + SceneEntityManifest, + SceneManifest, + SemanticIntegrationManifest, + SemanticValidationError, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneCollisionWorldMode, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, + UnsupportedSceneAffordanceError, +) + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } +) + + +class _NeverObservedStateProvider: + """Fail if provider-backed state leaks into static validation.""" + + def __init__(self) -> None: + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + raise AssertionError("static semantic validation must not observe providers") + + +class _CopyTrackedAffordance(AntipodalAffordance): + """Count payload copies so metadata projection can prove it performs none.""" + + copies = 0 + + def __deepcopy__(self, memo: dict[int, object]) -> _CopyTrackedAffordance: + del memo + type(self).copies += 1 + return _CopyTrackedAffordance() + + +def _scene_registry( + *, + with_default: bool, +) -> tuple[SceneRegistry, _NeverObservedStateProvider]: + provider = _NeverObservedStateProvider() + object_ref = SceneObjectRef("cube") + side_grasp = SceneAffordanceRef("cube.grasp.side") + top_grasp = SceneAffordanceRef("cube.grasp.top") + defaults = {GRASP_AFFORDANCE_CAPABILITY: top_grasp} if with_default else {} + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=object_ref, + state_provider=provider, + aliases=("sim_cube",), + default_affordances=defaults, + ), + SceneEntityRegistration( + ref=side_grasp, + parent=object_ref, + native_name="side_grasp", + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="grasp-v1", + relative_pose=torch.eye(4), + ), + SceneEntityRegistration( + ref=top_grasp, + parent=object_ref, + native_name="top_grasp", + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset( + { + GRASP_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + } + ), + affordance_revision="grasp-v1", + relative_pose=torch.eye(4), + ), + ) + ) + return registry, provider + + +def _semantic_integration( + registry: SceneRegistry, +) -> SemanticIntegrationManifest: + robot_profile = RobotSkillProfile( + profile_id="test_robot", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=_MOTION_CAPABILITIES, + ), + "grasp": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), + grasp=torch.tensor([1.0]), + ) + }, + presets={"safe": SkillPolicyPreset("safe")}, + default_preset="safe", + ) + return SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=robot_profile, + call_catalog=builtin_semantic_call_catalog(), + ) + + +def _engine_for_integration( + integration: SemanticIntegrationManifest, +) -> AtomicActionEngine: + """Build a minimal live engine whose resource graph matches the manifest.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 2 + robot.control_parts = {"arm": object(), "hand": object()} + robot.get_qpos.return_value = torch.zeros(1, 2) + robot.get_qvel.return_value = torch.zeros(1, 2) + robot.get_joint_ids.side_effect = lambda name: { + "arm": [0], + "hand": [1], + }[name] + robot.get_solver.side_effect = lambda name=None: ( + object() if name == "arm" else None + ) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + return AtomicActionEngine( + generator, + skill_profile=integration.robot_profile, + ) + + +def test_scene_registry_filters_capabilities_and_uses_scoped_default() -> None: + registry, _ = _scene_registry(with_default=True) + + assert registry.affordances( + "sim_cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + ) == ( + SceneAffordanceRef("cube.grasp.side"), + SceneAffordanceRef("cube.grasp.top"), + ) + assert registry.affordances( + "cube", + capability=PLACE_ON_AFFORDANCE_CAPABILITY, + ) == (SceneAffordanceRef("cube.grasp.top"),) + assert registry.resolve_affordance( + "cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + ) == SceneAffordanceRef("cube.grasp.top") + assert registry.resolve_affordance( + "cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + explicit="cube.grasp.side", + ) == SceneAffordanceRef("cube.grasp.side") + + +def test_scene_registry_rejects_ambiguous_or_unsupported_affordance() -> None: + registry, _ = _scene_registry(with_default=False) + + with pytest.raises(AmbiguousSceneAffordanceError, match="multiple affordances"): + registry.resolve_affordance( + "cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + with pytest.raises(UnsupportedSceneAffordanceError, match="no affordance"): + registry.resolve_affordance( + "cube", + capability="affordance.place.inside", + ) + + +def test_scene_registry_rejects_untyped_or_unversioned_grasp_capability() -> None: + object_ref = SceneObjectRef("cube") + base = dict( + ref=SceneAffordanceRef("cube.grasp"), + parent=object_ref, + native_name="grasp", + relative_pose=torch.eye(4), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + ) + + with pytest.raises(TypeError, match="AntipodalAffordance"): + SceneEntityRegistration( + **base, + affordance=Affordance(), + affordance_revision="v1", + ) + with pytest.raises(ValueError, match="affordance_revision"): + SceneEntityRegistration( + **base, + affordance=AntipodalAffordance(), + ) + + +def test_scene_registry_rejects_default_reference_subclass() -> None: + class SpecialAffordanceRef(SceneAffordanceRef): + pass + + with pytest.raises(TypeError, match="SceneAffordanceRef"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_NeverObservedStateProvider(), + default_affordances={ + GRASP_AFFORDANCE_CAPABILITY: SpecialAffordanceRef("cube.grasp") + }, + ) + + +def test_scene_manifest_projection_does_not_copy_affordance_payload() -> None: + provider = _NeverObservedStateProvider() + object_ref = SceneObjectRef("cube") + _CopyTrackedAffordance.copies = 0 + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=object_ref, state_provider=provider), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube.grasp"), + parent=object_ref, + native_name="grasp", + relative_pose=torch.eye(4), + affordance=_CopyTrackedAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="v1", + ), + ) + ) + _CopyTrackedAffordance.copies = 0 + + SceneManifest.from_registry(registry) + + assert _CopyTrackedAffordance.copies == 0 + assert provider.calls == 0 + + +def test_scene_manifest_detects_grounding_metadata_drift() -> None: + provider = _NeverObservedStateProvider() + object_ref = SceneObjectRef("cube") + + def registry(native_name: str, revision: str) -> SceneRegistry: + return SceneRegistry( + ( + SceneEntityRegistration(ref=object_ref, state_provider=provider), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube.grasp"), + parent=object_ref, + native_name=native_name, + relative_pose=torch.eye(4), + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision=revision, + ), + ) + ) + + manifest = SceneManifest.from_registry(registry("grasp", "v1")) + + with pytest.raises(SemanticValidationError) as error: + manifest.validate_registry(registry("changed", "v2")) + + assert error.value.diagnostic.code == "scene_manifest_mismatch" + + +def test_scene_manifest_detects_collision_world_mode_drift() -> None: + manifest = SceneManifest.from_registry( + SceneRegistry((), collision_world_mode=SceneCollisionWorldMode.SHARED) + ) + + with pytest.raises(SemanticValidationError) as error: + manifest.validate_registry( + SceneRegistry((), collision_world_mode=SceneCollisionWorldMode.PER_ENV) + ) + + assert error.value.diagnostic.code == "scene_manifest_mismatch" + assert error.value.diagnostic.rendered_path.endswith("collision_world_mode") + + +def test_scene_manifest_rejects_impossible_typed_topology() -> None: + affordance = SceneAffordanceRef("self") + + with pytest.raises(ValueError, match="object, articulation, or link"): + SceneEntityManifest( + ref=affordance, + parent=affordance, + native_name="self", + affordance_payload_type=AntipodalAffordance, + affordance_revision="v1", + ) + + +def test_scene_manifest_rejects_entry_subclass_with_live_state() -> None: + class LiveManifest(SceneEntityManifest): + live_handle = object() + + with pytest.raises(TypeError, match="exact SceneEntityManifest"): + SceneManifest((LiveManifest(ref=SceneObjectRef("cube")),)) + + +def test_semantic_integration_rejects_catalog_subclass_with_behavior() -> None: + class LiveCatalog(SemanticCallCatalog): + live_handle = object() + + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + live_catalog = LiveCatalog(integration.call_catalog.descriptors.values()) + + with pytest.raises(TypeError, match="exactly SemanticCallCatalog"): + SemanticIntegrationManifest( + scene=integration.scene, + robot_profile=integration.robot_profile, + call_catalog=live_catalog, + ) + + +def test_scene_manifest_reports_structured_pathful_diagnostic() -> None: + manifest = SceneManifest((SceneEntityManifest(ref=SceneObjectRef("cube")),)) + + with pytest.raises(SemanticValidationError) as error: + manifest.resolve( + "missing", + expected_type=SceneObjectRef, + path=("program", 2, "object"), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_entity" + assert diagnostic.path == ("program", 2, "object") + assert diagnostic.rendered_path == "program[2].object" + assert diagnostic.candidates == ("cube",) + assert str(error.value).startswith("program[2].object:") + + +def test_static_integration_links_resources_and_affordances_without_observation() -> ( + None +): + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + + linked = integration.link_call( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "manipulator"}, + ), + path=("program", 0), + ) + integration.scene.validate_registry(registry) + + assert linked.descriptor.skill_id == "pick_up" + assert linked.preset_id == "safe" + assert linked.call.resources == {"primary": "manipulator"} + assert isinstance(linked.call, Pick) + assert linked.call.grasp == SceneAffordanceRef("cube.grasp.top") + assert linked.affordances == {"grasp": SceneAffordanceRef("cube.grasp.top")} + assert provider.calls == 0 + + +def test_static_integration_rejects_unknown_resource_with_complete_path() -> None: + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + + with pytest.raises(SemanticValidationError) as error: + integration.link_call( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "missing"}, + ), + path=("program", 3), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_resource" + assert diagnostic.path == ("program", 3, "resources", "primary") + assert diagnostic.rendered_path == "program[3].resources.primary" + assert diagnostic.candidates == ("manipulator",) + assert provider.calls == 0 + + +def test_static_integration_preserves_scene_path_without_observing_provider() -> None: + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + + with pytest.raises(SemanticValidationError) as error: + integration.link_call( + Pick( + object=SceneObjectRef("missing"), + resources={"primary": "manipulator"}, + ), + path=("program", 4), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_entity" + assert diagnostic.rendered_path == "program[4].object" + assert provider.calls == 0 + + +def test_registered_payload_scene_refs_are_statically_resolved() -> None: + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + pick = integration.call_catalog.discover("pick") + extension = SemanticCallDescriptor( + call_id="vendor.inspect", + spec_type=RegisteredSemanticCall, + target_descriptor=pick.target_descriptor, + ) + integration = SemanticIntegrationManifest( + scene=integration.scene, + robot_profile=integration.robot_profile, + call_catalog=integration.call_catalog.with_descriptor(extension), + ) + + with pytest.raises(SemanticValidationError) as error: + integration.link_call( + RegisteredSemanticCall( + call_id="vendor.inspect", + arguments={"object": SceneObjectRef("missing")}, + resources={"primary": "manipulator"}, + ), + path=("program", 5, "call"), + ) + + assert error.value.diagnostic.code == "unknown_entity" + assert error.value.diagnostic.rendered_path == ("program[5].call.arguments.object") + assert provider.calls == 0 + + +def test_bound_semantic_call_retains_installed_profile_ownership() -> None: + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + engine = _engine_for_integration(integration) + bound_integration = integration.bind(registry, engine) + + result = bound_integration.link_call(Pick(object=SceneObjectRef("cube"))) + + assert result.robot_profile is bound_integration.robot_profile + assert result.binding.action_binding.owner_id == engine.binding_owner_id + + +def test_bound_semantic_integration_rejects_engine_profile_rebind() -> None: + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + engine = _engine_for_integration(integration) + stale = integration.bind(registry, engine) + + engine.bind_skill_profile(integration.robot_profile) + + with pytest.raises(SemanticValidationError) as error: + stale.link_call(Pick(object=SceneObjectRef("cube"))) + + assert error.value.diagnostic.code == "semantic_profile_stale" + + +def test_bound_semantic_integration_rejects_manifest_subclass_behavior() -> None: + class LiveManifest(SemanticIntegrationManifest): + live_handle = object() + + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + engine = _engine_for_integration(integration) + bound_profile = engine.skill_profile + assert bound_profile is not None + live_manifest = LiveManifest( + scene=integration.scene, + robot_profile=integration.robot_profile, + call_catalog=integration.call_catalog, + ) + + with pytest.raises(TypeError, match="exactly SemanticIntegrationManifest"): + type(integration.bind(registry, engine))( + manifest=live_manifest, + scene_registry=registry, + robot_profile=bound_profile, + engine=engine, + ) diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index 5ca64cdbe..68087a82d 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -1350,6 +1350,28 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: incompatible.bind(_engine(control_profiles=_command_profiles())) +def test_profile_owns_named_grounding_provider_selections() -> None: + selections = {"hand_over": "dual_center"} + profile = RobotSkillProfile( + "grounding", + resources=_resources(), + command_profiles=_command_profiles(), + grounding_providers=selections, + ) + + selections["hand_over"] = "source_mutation" + + assert profile.grounding_providers == {"hand_over": "dual_center"} + with pytest.raises(TypeError): + profile.grounding_providers["pick"] = "invalid" # type: ignore[index] + with pytest.raises(ValueError, match="grounding_providers"): + RobotSkillProfile( + "invalid_grounding", + resources=_resources(), + grounding_providers={"hand_over": " provider"}, + ) + + def test_profile_rejects_default_for_uninstalled_skill() -> None: with pytest.raises(ProfileValidationError, match="not installed"): _profile(defaults={"missing": ResourceBinding({"primary": "left_actor"})}).bind( @@ -1408,4 +1430,20 @@ class Replacement(action_type): _ = bound.skills +def test_bound_profile_rejects_equal_descriptor_implementation_replacement() -> None: + engine = _engine(control_profiles=_command_profiles()) + bound = _profile().bind(engine) + action_type = BUILTIN_ACTION_TYPES[0] + + class EquivalentReplacement(action_type): + binding_contract: ClassVar[SkillBindingContract] = action_type.binding_contract + + assert EquivalentReplacement.descriptor() == action_type.descriptor() + + engine.register(EquivalentReplacement(), replace=True) + + with pytest.raises(RuntimeError, match="changed after"): + _ = bound.skills + + __all__ = [] diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py new file mode 100644 index 000000000..9c93189d1 --- /dev/null +++ b/tests/sim/skills/test_runtime.py @@ -0,0 +1,552 @@ +# ---------------------------------------------------------------------------- +# 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 + +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + AssembleGoal, + AtomicAction, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + CommandAcknowledgement, + ControlPartCommandProfile, + EntityState, + ExecutionRunnerCfg, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + GraspGoal, + HeldObjectState, + JointPositionTarget, + PickUp, + PickUpOptions, + Place as AtomicPlace, + PlaceGoal, + PlaceOptions, + PlanningContext, + RobotObservation, + RuntimeCommandFrame, + RuntimeEndpointTarget, + StateDelta, + TaskState, + TimedCommandSequence, +) +from embodichain.lab.sim.skills import ( + ControlPartEndpoint, + GRASP_AFFORDANCE_CAPABILITY, + Pick, + Place, + ResourceBinding, + RobotResource, + RobotSkillProfile, + SceneAffordanceRef, + SceneEntityRegistration, + SceneManifest, + SceneObjectRef, + SceneRegistry, + SemanticExecutionStatus, + SemanticEffectVerifier, + SemanticIntegrationManifest, + SemanticPose, + SemanticSkillRuntime, + SemanticTaskStatus, + SkillPolicyPreset, + builtin_semantic_call_catalog, +) + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } +) + + +class _PoseProvider: + def __init__(self, pose: torch.Tensor) -> None: + self.pose = pose + + def observe(self, *, timestamp: float, env_ids: torch.Tensor) -> EntityState: + del timestamp, env_ids + return EntityState(self.pose) + + +class _InstantPick(AtomicAction): + skill_id = PickUp.skill_id + GoalType = GraspGoal + OptionsType = PickUpOptions + binding_contract = PickUp.binding_contract + + def _plan(self, request, context): + goal = self.require_goal(request) + target = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + held = HeldObjectState( + semantics=goal.semantics, + object_to_eef=torch.eye(4).repeat(context.batch_size, 1, 1), + grasp_xpos=torch.eye(4).repeat(context.batch_size, 1, 1), + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence((), context.env_ids), + expected_effects=StateDelta( + held_object_updates={target.control_part: held} + ), + ) + + +class _InstantPlace(AtomicAction): + skill_id = AtomicPlace.skill_id + GoalType = (PlaceGoal, AssembleGoal) + OptionsType = PlaceOptions + binding_contract = AtomicPlace.binding_contract + + def _plan(self, request, context): + self.require_goal(request) + target = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence((), context.env_ids), + expected_effects=StateDelta( + held_object_updates={target.control_part: None} + ), + ) + + +class _ExecutionPorts: + def __init__(self, registry: SceneRegistry, robot: Mock) -> None: + self.registry = registry + self.robot = robot + self.env_ids = torch.tensor([0, 1], dtype=torch.long) + self.scene_provider = registry.make_scene_provider(batch_size=2) + self.time = 0.0 + self.hold_calls = 0 + self.cancel_calls = 0 + + def observe(self, task_state: TaskState) -> PlanningContext: + qpos = self.robot.get_qpos() + return PlanningContext( + robot=RobotObservation( + timestamp=self.time, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=task_state, + scene=self.scene_provider.snapshot( + timestamp=self.time, + env_ids=self.env_ids, + ), + env_ids=self.env_ids, + control_dt=0.01, + ) + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + del command, timeout + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, context, timeout + self.hold_calls += 1 + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, timeout + self.cancel_calls += 1 + return CommandAcknowledgement.accepted_ack() + + def now(self) -> float: + return self.time + + def sleep(self, duration: float) -> None: + self.time += duration + + +def _scene_registry() -> SceneRegistry: + cube = SceneObjectRef("cube") + grasp = SceneAffordanceRef("cube_grasp") + return SceneRegistry( + ( + SceneEntityRegistration( + ref=cube, + state_provider=_PoseProvider(torch.eye(4).repeat(2, 1, 1)), + semantic_type="cube", + default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp}, + ), + SceneEntityRegistration( + ref=grasp, + parent=cube, + native_name="grasp", + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="grasp-v1", + relative_pose=torch.eye(4), + ), + ) + ) + + +def _profile( + *, + runner_cfg: ExecutionRunnerCfg | None = None, +) -> RobotSkillProfile: + return RobotSkillProfile( + profile_id="runtime_test_robot", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=_MOTION_CAPABILITIES, + ), + "grasp": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), + grasp=torch.tensor([1.0]), + ) + }, + defaults={ + "pick_up": ResourceBinding({"primary": "manipulator"}), + "place": ResourceBinding({"primary": "manipulator"}), + }, + presets={"safe": SkillPolicyPreset("safe", runner_cfg=runner_cfg)}, + default_preset="safe", + ) + + +def _robot() -> Mock: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 2 + robot.control_parts = {"arm": object(), "hand": object()} + robot.get_qpos.return_value = torch.zeros(2, 2) + robot.get_qvel.return_value = torch.zeros(2, 2) + robot.get_joint_ids.side_effect = lambda name: {"arm": [0], "hand": [1]}[name] + robot.get_solver.return_value = object() + return robot + + +def _runtime( + *, + verifier: SemanticEffectVerifier | None = None, + profile_runner_cfg: ExecutionRunnerCfg | None = None, + runtime_runner_cfg: ExecutionRunnerCfg | None = None, +) -> tuple[SemanticSkillRuntime, _ExecutionPorts]: + registry = _scene_registry() + profile = _profile(runner_cfg=profile_runner_cfg) + robot = _robot() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + engine = AtomicActionEngine(generator, skill_profile=profile) + engine.register(_InstantPick(), replace=True) + engine.register(_InstantPlace(), replace=True) + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=builtin_semantic_call_catalog(), + ) + ports = _ExecutionPorts(registry, robot) + runtime = SemanticSkillRuntime.bind( + manifest=manifest, + scene_registry=registry, + engine=engine, + observation_provider=ports, + command_sink=ports, + clock=ports, + effect_verifier=verifier, + runner_cfg=runtime_runner_cfg, + ) + return runtime, ports + + +def _successful_verifier(call, request, context) -> torch.Tensor: + del call, request + return torch.ones(context.batch_size, dtype=torch.bool) + + +def _pick_place_calls() -> tuple[Pick, Place]: + cube = SceneObjectRef("cube") + return ( + Pick(object=cube), + Place( + object=cube, + at=SemanticPose((0.4, 0.0, 0.2), (1.0, 0.0, 0.0, 0.0)), + ), + ) + + +def test_runtime_runs_jit_grounded_workflow_to_verified_completion() -> None: + observed_calls: list[str] = [] + + def verifier(call, request, context) -> torch.Tensor: + del request + observed_calls.append(call.semantic_id) + return torch.ones(context.batch_size, dtype=torch.bool) + + runtime, ports = _runtime(verifier=verifier) + + result = runtime.run(_pick_place_calls(), task_id="pick_place") + + assert result.status is SemanticTaskStatus.SUCCEEDED + assert result.eligible_mask.tolist() == [True, True] + assert result.task_state.held_objects == {} + assert observed_calls == ["pick", "place"] + assert [record.skill_id for record in result.segments[0].calls] == [ + "pick_up", + "place", + ] + assert ports.hold_calls == 2 + assert runtime.active_task is None + + +def test_task_preserves_verified_state_across_dynamic_segments() -> None: + runtime, _ = _runtime(verifier=_successful_verifier) + cube = SceneObjectRef("cube") + task = runtime.open_task("dynamic_delivery") + + pick_result = task.run_segment((Pick(object=cube),), segment_id="acquire") + assert pick_result.status is SemanticExecutionStatus.COMPLETED + assert task.task_state.held_object_mask("arm").tolist() == [True, True] + + place_result = task.run_segment( + ( + Place( + object=cube, + at=SemanticPose((0.5, 0.0, 0.2), (1.0, 0.0, 0.0, 0.0)), + ), + ), + segment_id="deliver", + ) + assert place_result.status is SemanticExecutionStatus.COMPLETED + assert task.task_state.held_objects == {} + + result = task.finish() + assert result.status is SemanticTaskStatus.SUCCEEDED + assert [segment.segment_id for segment in result.segments] == [ + "acquire", + "deliver", + ] + + +def test_manual_execution_blocks_until_effect_mask_is_submitted() -> None: + runtime, _ = _runtime() + execution = runtime.start( + (Pick(object=SceneObjectRef("cube")),), + task_id="manual_pick", + ) + + blocked = execution.run_until_blocked() + assert blocked.status is SemanticExecutionStatus.WAITING_FOR_EFFECT + assert blocked.pending_effect is not None + assert execution.task_result is None + + completed = execution.step(effect_success=torch.tensor([True, True])) + assert completed.status is SemanticExecutionStatus.COMPLETED + assert execution.task_result is not None + assert execution.task_result.status is SemanticTaskStatus.SUCCEEDED + assert runtime.active_task is None + + +def test_manual_execution_rejects_effect_before_verification_boundary() -> None: + runtime, _ = _runtime() + execution = runtime.start( + (Pick(object=SceneObjectRef("cube")),), + task_id="premature_effect", + ) + + with pytest.raises(RuntimeError, match="pending effect verification"): + execution.step(effect_success=torch.tensor([True, True])) + + execution.cancel() + assert runtime.active_task is None + + +def test_execution_stages_same_call_revision_through_runner_boundary() -> None: + runtime, _ = _runtime(verifier=_successful_verifier) + cube = SceneObjectRef("cube") + execution = runtime.start((Pick(object=cube),), task_id="revised_pick") + + execution.revise_current(Pick(object=cube)) + completed = execution.run_until_blocked( + effect_verifier=_successful_verifier, + ) + + assert completed.status is SemanticExecutionStatus.COMPLETED + assert execution.task_result is not None + assert execution.task_result.segments[0].calls[0].invocation_revision == 1 + + +def test_effect_failures_produce_partial_task_success_after_bounded_retries() -> None: + runtime, _ = _runtime( + verifier=lambda call, request, context: torch.tensor([True, False]) + ) + + result = runtime.run( + (Pick(object=SceneObjectRef("cube")),), + task_id="partial_pick", + ) + + assert result.status is SemanticTaskStatus.PARTIAL_SUCCESS + assert result.eligible_mask.tolist() == [True, False] + assert result.task_state.held_object_mask("arm").tolist() == [True, False] + + +def test_runtime_rejects_concurrent_tasks() -> None: + runtime, _ = _runtime() + task = runtime.open_task("first") + + with pytest.raises(RuntimeError, match="already owns this runtime"): + runtime.open_task("second") + + result = task.cancel() + assert result.status is SemanticTaskStatus.CANCELLED + assert runtime.active_task is None + + +def test_blocking_run_requires_effect_verifier_before_owning_runtime() -> None: + runtime, _ = _runtime() + + with pytest.raises(ValueError, match="requires an effect_verifier"): + runtime.run((Pick(object=SceneObjectRef("cube")),)) + + assert runtime.active_task is None + + +def test_verifier_exception_fails_safely_and_releases_runtime() -> None: + def failing_verifier(call, request, context) -> torch.Tensor: + del call, request, context + raise RuntimeError("camera unavailable") + + runtime, ports = _runtime(verifier=failing_verifier) + + result = runtime.run( + (Pick(object=SceneObjectRef("cube")),), + task_id="failed_verification", + ) + + assert result.status is SemanticTaskStatus.FAILED + assert result.segments[0].status is SemanticExecutionStatus.FAILED + assert "camera unavailable" in (result.message or "") + assert ports.cancel_calls == 1 + assert runtime.active_task is None + + +def test_failed_dynamic_segment_closes_terminal_task_ownership() -> None: + runtime, _ = _runtime( + verifier=lambda call, request, context: torch.zeros( + context.batch_size, + dtype=torch.bool, + ) + ) + task = runtime.open_task("terminal_dynamic_failure") + + segment = task.run_segment((Pick(object=SceneObjectRef("cube")),)) + + assert segment.status is SemanticExecutionStatus.FAILED + assert task.result is not None + assert task.result.status is SemanticTaskStatus.FAILED + assert runtime.active_task is None + + +def test_from_simulation_builds_ports_and_filters_agent_visible_calls() -> None: + registry = _scene_registry() + profile = _profile() + robot = _robot() + simulation = Mock() + simulation.sim_config.physics_dt = 0.01 + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + generator.collision_world_info = None + + runtime = SemanticSkillRuntime.from_simulation( + simulation=simulation, + robot=robot, + motion_generator=generator, + scene_registry=registry, + robot_profile=profile, + control_dt=0.04, + ) + + assert set(runtime.available_calls) == {"pick", "place"} + assert runtime.observation_provider is runtime.command_sink + assert runtime.clock is runtime.observation_provider + assert runtime.observation_provider.control_dt == pytest.approx(0.04) + + +def test_runtime_uses_skill_preset_runner_cfg_without_global_override() -> None: + runtime, ports = _runtime( + verifier=_successful_verifier, + profile_runner_cfg=ExecutionRunnerCfg(hold_on_completion=False), + ) + + result = runtime.run((Pick(object=SceneObjectRef("cube")),)) + + assert result.status is SemanticTaskStatus.SUCCEEDED + assert ports.hold_calls == 0 + + +def test_runtime_runner_cfg_overrides_skill_preset() -> None: + runtime, ports = _runtime( + verifier=_successful_verifier, + profile_runner_cfg=ExecutionRunnerCfg(hold_on_completion=False), + runtime_runner_cfg=ExecutionRunnerCfg(hold_on_completion=True), + ) + + result = runtime.run((Pick(object=SceneObjectRef("cube")),)) + + assert result.status is SemanticTaskStatus.SUCCEEDED + assert ports.hold_calls == 1 diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py index 904c749a8..903cb3ef1 100644 --- a/tests/sim/skills/test_scene.py +++ b/tests/sim/skills/test_scene.py @@ -24,9 +24,16 @@ import pytest import torch -from embodichain.lab.sim.atomic_actions import Affordance, EntityState, SceneSnapshot +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AntipodalAffordance, + EntityState, + SceneSnapshot, +) from embodichain.lab.sim.planners.base_planner import CollisionWorldInfo from embodichain.lab.sim.skills import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneArticulationRef, SceneCollisionRole, @@ -35,6 +42,7 @@ SceneLinkRef, SceneObjectRef, SceneRegistry, + UnsupportedSceneAffordanceError, ) @@ -148,6 +156,25 @@ def get_articulation(self, uid: str) -> _SimulationEntity | None: return self.articulations.get(uid) +class _CopyTrackedAffordance(AntipodalAffordance): + """Count payload copies so metadata projection can prove it performs none.""" + + copies = 0 + + def __deepcopy__(self, memo: dict[int, object]) -> _CopyTrackedAffordance: + del memo + type(self).copies += 1 + return _CopyTrackedAffordance() + + +class _SelfCopyAffordance(AntipodalAffordance): + """Malicious payload that violates deepcopy ownership.""" + + def __deepcopy__(self, memo: dict[int, object]) -> _SelfCopyAffordance: + del memo + return self + + @pytest.mark.parametrize("entity_id", ["", " cube", "cube "]) def test_scene_entity_ref_rejects_non_exact_identifier(entity_id: str) -> None: with pytest.raises(ValueError, match="entity_id"): @@ -230,6 +257,172 @@ def test_affordance_registration_rejects_two_pose_sources() -> None: ) +def test_grasp_capability_requires_typed_versioned_payload() -> None: + object_ref = SceneObjectRef("cube") + common = { + "ref": SceneAffordanceRef("cube_grasp"), + "parent": object_ref, + "native_name": "grasp", + "relative_pose": torch.eye(4), + "affordance_capabilities": frozenset({GRASP_AFFORDANCE_CAPABILITY}), + } + + with pytest.raises(TypeError, match="AntipodalAffordance"): + SceneEntityRegistration( + **common, + affordance=Affordance(), + affordance_revision="v1", + ) + with pytest.raises(ValueError, match="affordance_revision"): + SceneEntityRegistration( + **common, + affordance=AntipodalAffordance(), + ) + + +def test_registry_selects_only_explicit_scoped_affordance_default() -> None: + object_ref = SceneObjectRef("cube") + first = SceneAffordanceRef("first_grasp") + second = SceneAffordanceRef("second_grasp") + + def registrations(*, with_default: bool) -> tuple[SceneEntityRegistration, ...]: + return ( + SceneEntityRegistration( + ref=object_ref, + state_provider=_StateProvider(), + default_affordances=( + {GRASP_AFFORDANCE_CAPABILITY: second} if with_default else {} + ), + ), + *tuple( + SceneEntityRegistration( + ref=ref, + parent=object_ref, + native_name=ref.entity_id, + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="v1", + relative_pose=torch.eye(4), + ) + for ref in (first, second) + ), + ) + + ambiguous = SceneRegistry(registrations(with_default=False)) + with pytest.raises(AmbiguousSceneAffordanceError, match="multiple"): + ambiguous.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + with pytest.raises(UnsupportedSceneAffordanceError, match="no affordance"): + ambiguous.resolve_affordance( + object_ref, + capability="affordance.unknown", + ) + + registry = SceneRegistry(registrations(with_default=True)) + assert ( + registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + == second + ) + assert ( + registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + explicit=first, + ) + == first + ) + + +def test_registry_metadata_projection_does_not_copy_affordance_payload() -> None: + object_ref = SceneObjectRef("cube") + _CopyTrackedAffordance.copies = 0 + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=object_ref, state_provider=_StateProvider()), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube_grasp"), + parent=object_ref, + native_name="grasp", + affordance=_CopyTrackedAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="v1", + relative_pose=torch.eye(4), + ), + ) + ) + _CopyTrackedAffordance.copies = 0 + + metadata = registry.entity_metadata + + assert metadata[1].affordance_payload_type is _CopyTrackedAffordance + assert _CopyTrackedAffordance.copies == 0 + + +def test_registry_rejects_affordance_that_cannot_produce_owned_copy() -> None: + cube = SceneObjectRef("cube") + + with pytest.raises(TypeError, match="distinct value"): + SceneRegistry( + ( + SceneEntityRegistration(ref=cube, state_provider=_StateProvider()), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube_grasp"), + parent=cube, + native_name="grasp", + affordance=_SelfCopyAffordance(), + relative_pose=torch.eye(4), + ), + ) + ) + + +def test_registry_builds_owned_object_semantics_from_direct_child() -> None: + cube = SceneObjectRef("cube") + table = SceneObjectRef("table") + cube_grasp = SceneAffordanceRef("cube_grasp") + table_grasp = SceneAffordanceRef("table_grasp") + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=cube, + state_provider=_StateProvider(), + semantic_type="cube", + ), + SceneEntityRegistration(ref=table, state_provider=_StateProvider()), + SceneEntityRegistration( + ref=cube_grasp, + parent=cube, + native_name="grasp", + affordance=AntipodalAffordance(), + relative_pose=torch.eye(4), + ), + SceneEntityRegistration( + ref=table_grasp, + parent=table, + native_name="grasp", + affordance=AntipodalAffordance(), + relative_pose=torch.eye(4), + ), + ) + ) + + first = registry.object_semantics(cube, affordance=cube_grasp) + second = registry.object_semantics("cube", affordance="cube_grasp") + + assert first.entity_id == "cube" + assert first.label == "cube" + assert first.affordance is not second.affordance + first.affordance.custom_config["mutated"] = True + assert "mutated" not in second.affordance.custom_config + with pytest.raises(ValueError, match="not a direct child"): + registry.object_semantics(cube, affordance=table_grasp) + + def test_collision_registration_requires_geometry_provider() -> None: with pytest.raises(ValueError, match="geometry_provider"): SceneEntityRegistration( diff --git a/tests/sim/skills/test_semantic_skill_tutorials.py b/tests/sim/skills/test_semantic_skill_tutorials.py new file mode 100644 index 000000000..5a6bbf5cc --- /dev/null +++ b/tests/sim/skills/test_semantic_skill_tutorials.py @@ -0,0 +1,480 @@ +# ---------------------------------------------------------------------------- +# 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 semantic-skill tutorial declarations.""" + +from __future__ import annotations + +from typing import cast, TYPE_CHECKING +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim import SimulationManager +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + EffectVerificationRequest, + GraspGoal, + HandOverOptions, + PlanningContext, +) +from embodichain.lab.sim.skills import ( + GRASP_AFFORDANCE_CAPABILITY, + Pick, + Place, + RegisteredSemanticCall, + SceneRegistry, +) +import scripts.tutorials.semantic_skill.hand_over as handover_tutorial +import scripts.tutorials.semantic_skill.place as place_tutorial +from scripts.tutorials.semantic_skill.hand_over import ( + FINAL_OBJECT_POSITION, + HANDOVER_CALL_ID, + MIDDLE_OBJECT_POSITION, + TRACKING_ERROR_THRESHOLD as HANDOVER_TRACKING_ERROR_THRESHOLD, + TutorialHandOverLowerer, + create_handover_effect_verifier, + create_handover_task, + create_robot_profile as create_dual_arm_profile, +) +from scripts.tutorials.semantic_skill.place import ( + MINIMUM_PICK_LIFT, + TARGET_OBJECT_POSITION, + TRACKING_ERROR_THRESHOLD as PLACE_TRACKING_ERROR_THRESHOLD, + create_place_effect_verifier, + create_place_task, + create_robot_profile as create_single_arm_profile, +) +from scripts.tutorials.semantic_skill.tutorial_utils import ( + create_graspable_object_registry, + create_runtime_step_observer, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import RigidObject, Robot + from embodichain.lab.sim.skills.integration import BoundSemanticCall + +_TEST_PHYSICS_DT = 0.01 +_TEST_GRASP_SAMPLE_COUNT = 8 + + +class _PhysicalObject: + """Small mutable pose source used by verifier tests.""" + + def __init__(self, pose: torch.Tensor) -> None: + self.pose = pose + self.clear_count = 0 + + def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: + assert to_matrix is True + return self.pose.clone() + + def clear_dynamics(self) -> None: + self.clear_count += 1 + + +class _PhysicalRobot: + """Expose only the joint and FK observations used by tutorial verifiers.""" + + def __init__(self) -> None: + self.qpos: dict[str, torch.Tensor] = {} + self.eef_pose: dict[str, torch.Tensor] = {} + + def get_qpos(self, name: str) -> torch.Tensor: + return self.qpos[name].clone() + + def compute_fk( + self, + *, + qpos: torch.Tensor, + name: str, + to_matrix: bool, + ) -> torch.Tensor: + del qpos + assert to_matrix is True + return self.eef_pose[name].clone() + + +def _pose_at(position: tuple[float, float, float]) -> torch.Tensor: + pose = torch.eye(4).unsqueeze(0) + pose[:, :3, 3] = torch.tensor(position) + return pose + + +def _request( + skill_id: str, + *, + held_control_part: str | None = None, +) -> EffectVerificationRequest: + request = Mock() + request.skill_id = skill_id + held = Mock() + held.object_to_eef = torch.eye(4).unsqueeze(0) + request.expected_effects.held_object_updates = ( + {} if held_control_part is None else {held_control_part: held} + ) + return cast(EffectVerificationRequest, request) + + +def _verification_context() -> PlanningContext: + context = Mock() + context.robot.qpos = torch.zeros(1, 1) + return cast(PlanningContext, context) + + +def _graspable_registry() -> SceneRegistry: + entity = Mock() + entity.get_local_pose.return_value = torch.eye(4) + simulation = Mock() + simulation.get_rigid_object.return_value = entity + + registry, _ = create_graspable_object_registry( + cast(SimulationManager, simulation), + object_id="workpiece", + simulation_uid="sim_cube", + semantic_type="cube", + affordance=AntipodalAffordance(), + ) + return registry + + +def test_graspable_registry_maps_simulation_identity_to_semantic_identity() -> None: + registry = _graspable_registry() + object_ref = registry.resolve("workpiece") + + assert registry.resolve("sim_cube") == object_ref + grasp_ref = registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + semantics = registry.object_semantics(object_ref, affordance=grasp_ref) + assert semantics.entity_id == "workpiece" + assert type(semantics.affordance) is AntipodalAffordance + + +def test_place_tutorial_task_contains_no_robot_resource_names() -> None: + calls = create_place_task() + + assert tuple(type(call) for call in calls) == (Pick, Place) + assert calls[0].object == calls[1].object + assert dict(calls[0].resources) == {} + assert dict(calls[1].resources) == {} + assert calls[1].at is not None + torch.testing.assert_close( + calls[1].at.position, + torch.tensor(TARGET_OBJECT_POSITION), + ) + + +def test_place_tutorial_profile_owns_single_arm_binding_and_policies() -> None: + profile = create_single_arm_profile( + torch.tensor([0.0, 0.0]), + torch.tensor([0.5, 0.5]), + ) + + resource = profile.resources["primary_manipulator"] + assert resource.endpoints["motion"].control_part == "arm" + assert resource.endpoints["grasp"].control_part == "hand" + assert dict(profile.defaults["pick_up"].resources) == { + "primary": "primary_manipulator" + } + assert dict(profile.defaults["place"].resources) == { + "primary": "primary_manipulator" + } + assert dict(profile.skill_presets) == {"pick_up": "pick", "place": "place"} + assert ( + profile.presets["pick"].recovery_policy.tracking_error_threshold + == PLACE_TRACKING_ERROR_THRESHOLD + ) + assert profile.presets["place"].recovery_policy.max_action_retries == 0 + assert ( + profile.presets["place"].recovery_policy.tracking_error_threshold + == PLACE_TRACKING_ERROR_THRESHOLD + ) + + +def test_place_application_installs_default_effect_verifier( + monkeypatch: pytest.MonkeyPatch, +) -> None: + simulation = Mock() + simulation.sim_config.physics_dt = _TEST_PHYSICS_DT + simulation.get_rigid_object.return_value = Mock() + robot = Mock() + obj = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) + semantics = Mock(affordance=AntipodalAffordance()) + motion_generator = Mock() + runtime = Mock() + runtime_factory = Mock(return_value=runtime) + + monkeypatch.setattr( + place_tutorial, + "create_antipodal_semantics", + Mock(return_value=semantics), + ) + monkeypatch.setattr( + place_tutorial, + "create_curobo_motion_generator", + Mock(return_value=motion_generator), + ) + monkeypatch.setattr( + place_tutorial.SemanticSkillRuntime, + "from_simulation", + runtime_factory, + ) + + result = place_tutorial.create_place_application( + cast(SimulationManager, simulation), + cast("Robot", robot), + cast("RigidObject", obj), + hand_open=torch.zeros(1), + hand_grasp=torch.ones(1), + n_sample=_TEST_GRASP_SAMPLE_COUNT, + force_reannotate=False, + ) + + assert result is runtime + assert type(runtime_factory.call_args.kwargs["scene_registry"]) is SceneRegistry + assert ( + runtime_factory.call_args.kwargs["robot_profile"].profile_id + == "tutorial.single_arm" + ) + assert runtime_factory.call_args.kwargs["motion_generator"] is motion_generator + assert callable(runtime_factory.call_args.kwargs["effect_verifier"]) + + +def test_place_tutorial_verifies_observed_pick_lift_and_eef_proximity() -> None: + physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) + physical_robot = _PhysicalRobot() + physical_robot.qpos["arm"] = torch.zeros(1, 1) + physical_robot.qpos["hand"] = torch.zeros(1, 1) + verifier = create_place_effect_verifier( + cast("RigidObject", physical_object), + cast("Robot", physical_robot), + torch.zeros(1), + ) + lifted_pose = _pose_at((0.0, 0.0, MINIMUM_PICK_LIFT + 0.01)) + physical_object.pose = lifted_pose + physical_robot.eef_pose["arm"] = lifted_pose + + success = verifier( + create_place_task()[0], + _request("pick_up", held_control_part="arm"), + _verification_context(), + ) + + assert success.tolist() == [True] + + +def test_place_tutorial_rejects_lift_with_wrong_grasp_relation() -> None: + physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) + physical_robot = _PhysicalRobot() + physical_robot.qpos["arm"] = torch.zeros(1, 1) + physical_robot.qpos["hand"] = torch.zeros(1, 1) + verifier = create_place_effect_verifier( + cast("RigidObject", physical_object), + cast("Robot", physical_robot), + torch.zeros(1), + ) + physical_object.pose = _pose_at((0.0, 0.0, MINIMUM_PICK_LIFT + 0.01)) + physical_robot.eef_pose["arm"] = _pose_at((0.2, 0.0, MINIMUM_PICK_LIFT + 0.01)) + + success = verifier( + create_place_task()[0], + _request("pick_up", held_control_part="arm"), + _verification_context(), + ) + + assert success.tolist() == [False] + + +def test_place_tutorial_verifies_release_at_requested_position() -> None: + physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) + physical_robot = _PhysicalRobot() + hand_open = torch.tensor([0.0, 0.0]) + physical_robot.qpos["hand"] = hand_open.unsqueeze(0) + verifier = create_place_effect_verifier( + cast("RigidObject", physical_object), + cast("Robot", physical_robot), + hand_open, + ) + physical_object.pose = _pose_at(TARGET_OBJECT_POSITION) + + success = verifier( + create_place_task()[1], + _request("place"), + _verification_context(), + ) + + assert success.tolist() == [True] + + +def test_handover_tutorial_registers_tuned_atomic_lowering() -> None: + calls = create_handover_task() + context = Mock() + context.robot.qpos = torch.zeros(1, 1) + lowerer = TutorialHandOverLowerer(_graspable_registry()) + + assert tuple(type(call) for call in calls) == (Pick, RegisteredSemanticCall) + assert calls[1].call_id == HANDOVER_CALL_ID + assert dict(calls[0].resources) == {} + assert calls[1].arguments["object"] == calls[0].object + lowering = lowerer.lower( + calls[1], + context=cast(PlanningContext, context), + bound=cast("BoundSemanticCall", object()), + ) + assert type(lowering.goal) is GraspGoal + assert type(lowering.skill_options) is HandOverOptions + assert lowering.goal.semantics.entity_id == "workpiece" + torch.testing.assert_close( + lowering.skill_options.middle_object_pose[:3, 3], + torch.tensor(MIDDLE_OBJECT_POSITION), + ) + torch.testing.assert_close( + lowering.skill_options.final_object_pose[:3, 3], + torch.tensor(FINAL_OBJECT_POSITION), + ) + + +def test_handover_tutorial_profile_binds_disjoint_arms() -> None: + profile = create_dual_arm_profile( + torch.tensor([0.0]), + torch.tensor([0.5]), + torch.tensor([0.0]), + torch.tensor([0.5]), + ) + + assert profile.resources["left"].endpoints["motion"].control_part == "left_arm" + assert profile.resources["right"].endpoints["motion"].control_part == "right_arm" + assert dict(profile.defaults["pick_up"].resources) == {"primary": "left"} + assert dict(profile.defaults["hand_over"].resources) == { + "source": "left", + "destination": "right", + } + assert ( + profile.presets["pick"].recovery_policy.tracking_error_threshold + == HANDOVER_TRACKING_ERROR_THRESHOLD + ) + assert profile.presets["hand_over"].recovery_policy.max_action_retries == 0 + assert ( + profile.presets["hand_over"].recovery_policy.tracking_error_threshold + == HANDOVER_TRACKING_ERROR_THRESHOLD + ) + + +def test_handover_application_installs_extension_and_default_verifier( + monkeypatch: pytest.MonkeyPatch, +) -> None: + simulation = Mock() + simulation.sim_config.physics_dt = _TEST_PHYSICS_DT + simulation.get_rigid_object.return_value = Mock() + robot = Mock() + obj = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) + semantics = Mock(affordance=AntipodalAffordance()) + motion_generator = Mock() + runtime = Mock() + runtime_factory = Mock(return_value=runtime) + + monkeypatch.setattr( + handover_tutorial, + "create_antipodal_semantics", + Mock(return_value=semantics), + ) + monkeypatch.setattr( + handover_tutorial, + "create_toppra_motion_generator", + Mock(return_value=motion_generator), + ) + monkeypatch.setattr( + handover_tutorial.SemanticSkillRuntime, + "from_simulation", + runtime_factory, + ) + + result = handover_tutorial.create_handover_application( + cast(SimulationManager, simulation), + cast("Robot", robot), + cast("RigidObject", obj), + left_open=torch.zeros(1), + left_grasp=torch.ones(1), + right_open=torch.zeros(1), + right_grasp=torch.ones(1), + n_sample=_TEST_GRASP_SAMPLE_COUNT, + force_reannotate=False, + ) + + assert result is runtime + assert type(runtime_factory.call_args.kwargs["scene_registry"]) is SceneRegistry + assert ( + runtime_factory.call_args.kwargs["robot_profile"].profile_id + == "tutorial.dual_arm" + ) + assert runtime_factory.call_args.kwargs["motion_generator"] is motion_generator + assert ( + HANDOVER_CALL_ID in runtime_factory.call_args.kwargs["call_catalog"].descriptors + ) + assert callable(runtime_factory.call_args.kwargs["effect_verifier"]) + assert type(runtime_factory.call_args.kwargs["registered_lowerers"][0]) is ( + TutorialHandOverLowerer + ) + + +def test_handover_tutorial_verifies_receiver_ownership_at_final_target() -> None: + physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) + physical_robot = _PhysicalRobot() + left_open = torch.tensor([0.0]) + right_grasp = torch.tensor([0.5]) + physical_robot.qpos["left_hand"] = left_open.unsqueeze(0) + physical_robot.qpos["right_hand"] = right_grasp.unsqueeze(0) + verifier = create_handover_effect_verifier( + cast("RigidObject", physical_object), + cast("Robot", physical_robot), + left_open=left_open, + right_grasp=right_grasp, + ) + final_pose = _pose_at(FINAL_OBJECT_POSITION) + physical_object.pose = final_pose + physical_robot.qpos["right_arm"] = torch.zeros(1, 1) + physical_robot.eef_pose["right_arm"] = final_pose + + success = verifier( + create_handover_task()[1], + _request("hand_over", held_control_part="right_arm"), + _verification_context(), + ) + + assert success.tolist() == [True] + + +def test_runtime_step_observer_stabilizes_initial_grasp_once() -> None: + physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) + physical_robot = _PhysicalRobot() + physical_robot.qpos["hand"] = torch.zeros(1, 1) + observer = create_runtime_step_observer( + cast("RigidObject", physical_object), + cast("Robot", physical_robot), + grasp_control_part="hand", + grasp_target=torch.ones(1), + ) + runner_step = Mock(tick=None) + + observer(runner_step) + physical_robot.qpos["hand"] = torch.ones(1, 1) + observer(runner_step) + observer(runner_step) + + assert physical_object.clear_count == 1 From 7b8424c3375c7d1933dfdcaf961ae43773b45e02 Mon Sep 17 00:00:00 2001 From: skywhite1024 <129768272+skywhite1024@users.noreply.github.com> Date: Fri, 21 Aug 2026 17:24:01 +0800 Subject: [PATCH 02/29] fix(skills): inherit held resource selections --- .../topics/atomic-actions/atomic-actions.md | 7 + .../design/declarative_expert_program_plan.md | 7 +- ...gen_sim_semantic_skill_integration_plan.md | 877 ++++++++++++++++++ embodichain/lab/sim/skills/calls.py | 15 +- embodichain/lab/sim/skills/compiler.py | 48 +- tests/sim/skills/test_calls.py | 17 +- tests/sim/skills/test_compiler.py | 107 +++ 7 files changed, 1037 insertions(+), 41 deletions(-) create mode 100644 docs/design/gen_sim_semantic_skill_integration_plan.md diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index ea7a3ec09..1e46b14c2 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -297,6 +297,13 @@ Registered calls require an explicit agent-visible target plus an installed `SemanticSkillCompiler.analyze()` performs provider-free linking, resource and affordance validation, held-object flow analysis, and first-release look-ahead. +A `Place` with no explicit `primary` resource inherits the workflow's known +holder resource, and a `HandOver` with no explicit `source` does the same. The +inferred selection is snapshotted onto the canonical linked call before +binding; an explicit conflicting selection still fails with +`held_resource_mismatch`. Inference never crosses a registered-call boundary. +`HandOver` selects participants only through the `source` and `destination` +resource slots; there is no separate receiver alias. A pick therefore owns zero or one downstream object target rather than an arbitrary target tuple. Relation targets retain affordance payload type and revision metadata and stay late-bound through an explicitly installed diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 4c22c8899..5f536c1b6 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -481,14 +481,17 @@ Version 1 should provide first-class calls for: - `Pick(object, grasp?, resources?)`; - `Place(object, pose?|on?|in?, resources?)`; -- `HandOver(object, receiver?, final_target?, resources?)`; +- `HandOver(object, final_target?, resources?)`; - a registered semantic call for shared extensions. `resources`, when present, is a mapping from the selected skill's local slot IDs to profile resource IDs (for example, `{"primary": "left_actor"}` or `{"body": "mobile_base"}`). It is an explicit ambiguity override, not a fixed arm/tool field. Ordinary calls omit it and use unique or profile-default -resolution. +resolution. Within one analyzed workflow, an omitted `Place.primary` or +`HandOver.source` inherits the known resource holding that object. Explicit +consumer selections remain authoritative constraints and fail if they conflict +with the known holder; inference never crosses a registered-call boundary. `Place` consumes verified held-object state. The compiler computes the release EEF pose from the requested object-space target and the verified diff --git a/docs/design/gen_sim_semantic_skill_integration_plan.md b/docs/design/gen_sim_semantic_skill_integration_plan.md new file mode 100644 index 000000000..34b23db75 --- /dev/null +++ b/docs/design/gen_sim_semantic_skill_integration_plan.md @@ -0,0 +1,877 @@ +# GenSim PR #532-#538 and Semantic Skill Integration Plan + +- Status: proposed +- Last updated: 2026-08-21 +- Guiding principle: **Semantic Skill is the only unified lower-level execution architecture** +- Related Semantic Skill PR: [#492](https://github.com/DexForce/EmbodiChain/pull/492) +- Related GenSim PRs: + [#532](https://github.com/DexForce/EmbodiChain/pull/532), + [#533](https://github.com/DexForce/EmbodiChain/pull/533), + [#534](https://github.com/DexForce/EmbodiChain/pull/534), + [#535](https://github.com/DexForce/EmbodiChain/pull/535), + [#536](https://github.com/DexForce/EmbodiChain/pull/536), + [#537](https://github.com/DexForce/EmbodiChain/pull/537), and + [#538](https://github.com/DexForce/EmbodiChain/pull/538) +- Related Semantic Skill stack: + [#495](https://github.com/DexForce/EmbodiChain/pull/495), + [#496](https://github.com/DexForce/EmbodiChain/pull/496), + [#497](https://github.com/DexForce/EmbodiChain/pull/497), + [#498](https://github.com/DexForce/EmbodiChain/pull/498), + [#500](https://github.com/DexForce/EmbodiChain/pull/500), + [#501](https://github.com/DexForce/EmbodiChain/pull/501), + [#504](https://github.com/DexForce/EmbodiChain/pull/504), and + [#480-#483](https://github.com/DexForce/EmbodiChain/pull/480) + +## 1. Executive summary + +PR #532-#538 should not introduce a second Action Engine beside the existing +Semantic Skill and Atomic Action stack. The adjusted design keeps the parts of +GenSim that provide real additional value: + +- task interpretation and typed `TaskSpec` values; +- offline and online task-plan candidates; +- an immutable task DAG and revision log; +- candidate selection and TaskGroup-level fusion; +- unfinished-suffix replanning; +- task-level recovery, audit artifacts, and A/B evaluation; +- cross-engine orchestration and final task inspection. + +The following duplicate implementations should be removed from GenSim: + +- an independent atomic capability registry; +- action-specific motion policies and robot-part routing; +- a second live action grounder; +- an adapter that directly materializes `ActionInvocation` values; +- a second physical execution loop and trajectory scheduler; +- independent held-object, effect, and recovery truth. + +The final layering is: + +```text +Task Engine + owns TaskSpec, SemanticTaskGraph, candidates, task scheduling, + task-level recovery, and final success + | + v +Semantic Skill + owns semantic calls, scene/profile binding, JIT grounding, + physical effects, workflow recovery, and the application runtime + | + v +Atomic Actions + owns typed goals, planning, command execution, tracking recovery, + transport acknowledgement, and safe stop +``` + +This is not a compatibility exercise for the current GenSim runtime. PR +#533-#538 should be structurally rewritten around the canonical Semantic Skill +interfaces instead of accumulating adapters around the current implementation. + +## 2. Scope and assumptions + +### 2.1 In scope + +- Reassign responsibilities across PR #532-#538. +- Define the target task graph and execution boundary. +- Identify GenSim components to keep, transform, move, or remove. +- Define dynamic-task and error-recovery behavior. +- Define the landing order and review gates. +- Define architecture and end-to-end acceptance tests. + +### 2.2 Out of scope + +- Replacing `AtomicActionEngine`, `ExecutionSession`, or `ExecutionRunner`. +- Replacing the canonical `SceneRegistry` or `RobotSkillProfile`. +- Implementing a second controller or simulation stepping loop in GenSim. +- Persisting robot trajectories, qpos, grasp poses, or controller routing in a + task-planning artifact. +- Allowing task-graph rewrites during an unverified physical-effect boundary. +- Claiming arbitrary per-environment task-program divergence in the first + adjusted version. + +### 2.3 Canonical runtime naming + +The current #492 head contains `SemanticSkillRuntime`, while downstream #496 +defines the intended canonical `SkillRuntime`. Before adapting #536 and #537, +these implementations must be consolidated into one public runtime. This plan +uses `SkillRuntime` as the conceptual name; the exact final class name is less +important than having exactly one implementation and one ownership boundary. + +## 3. Target architecture + +```text +Scene Engine + GeneratedSceneGraph / SceneAuthoringGraph + | + v +Task Engine SceneAdapter + SceneManifest + SceneRegistry registration plan + evidence sidecar + | + +------------------------------------------+ + | +TaskSpec | + | | + v v +Offline / Online Semantic Task Planner Semantic Integration Catalog + | - SceneManifest + | - RobotSkillProfile + | - SemanticCallCatalog + | - provider declarations + | - integration fingerprint + v +SemanticTaskGraph candidates + | + v +Candidate selection / TaskGroup fusion / preflight + | + v +TaskGraphScheduler + | + | selected route and semantic look-ahead suffix + v +SkillRuntime / ParallelSkillRuntime + | + v +SemanticSkillCompiler + | + v +ActionInvocation + | + v +AtomicActionEngine -> ExecutionRunner -> controller or simulation +``` + +The main runtime resolution path is: + +```text +SemanticTaskGraph node + -> canonical SemanticCallCfg decoder + -> SemanticCallSpec + -> SemanticSkillCompiler.analyze() + -> fresh observation + -> SemanticSkillCompiler.ground() + -> ActionInvocation + -> AtomicActionEngine / ExecutionRunner + -> verified SkillResult and TaskState + -> TaskGraphScheduler transition +``` + +## 4. Ownership matrix + +| Concern | Canonical owner | Allowed GenSim responsibility | +|---|---|---| +| Generated scene authoring | Scene Engine | Generate, edit, import, export, and preserve provenance | +| Provider-free scene identity | `SceneManifest` | Adapt generated scene data once into canonical IDs | +| Live scene identity and metadata | `SceneRegistry` | Hold only references to the canonical integration | +| Dynamic scene state | `SceneSnapshot` and `SceneProvider` | Consume snapshots through Semantic Skill ports | +| Semantic call discovery | `SemanticCallCatalog` | Consume a JSON-safe planner projection | +| Robot capabilities and resources | `RobotSkillProfile` | Express semantic participant constraints only | +| Exact physical resource claims | Bound `RobotSkillProfile` | Use claims returned by canonical preflight/runtime | +| Semantic grounding | `SemanticSkillCompiler` and registered grounders | Bind language roles to canonical scene references | +| Motion and action lowering | `SemanticSkillCompiler` and Atomic Actions | Do not materialize goals, options, or invocations | +| Physical execution | `SkillRuntime` and `ExecutionRunner` | Invoke a narrow semantic execution port | +| Effect verification | Semantic effect monitors and evidence collectors | Consume verified results and evidence summaries | +| Tracking and transport recovery | `ExecutionSession` and `ExecutionRunner` | Observe terminal typed failures only | +| Reacquisition and workflow recovery | `SkillRuntime` | Wait until workflow recovery is exhausted | +| DAG scheduling and route selection | `TaskGraphScheduler` | Own node readiness, route selection, and graph revisions | +| Task-level recovery | `TaskGraphScheduler` | Replace a TaskGroup or unfinished suffix at a safe boundary | +| Task or scene regeneration | Task Engine orchestration | Start a new safe execution transaction | +| Final task success | `SuccessSpec` and final inspection | Inspect final observed state without duplicating call effects | + +## 5. SemanticTaskGraph contract + +### 5.1 Purpose + +`SemanticTaskGraph` replaces the current direct-AtomicAction `SeedGraph`. It is +an immutable, JSON-safe task-planning artifact. It describes semantic intent, +dependencies, candidate routes, and task-level recovery, but contains no +grounded execution data. + +Each node contains exactly one canonical semantic-call payload. A scheduler +may give the selected future suffix to `SkillRuntime` for static look-ahead +while executing only the current node. + +### 5.2 Conceptual schema + +```json +{ + "schema_version": "semantic_task_graph/v1", + "task_id": "place_cube", + "instruction": "Place the cube on the tray", + "planner_route": "selected", + "integration_fingerprint": "", + "nodes": [ + { + "id": "pick_cube", + "call": { + "kind": "pick", + "object": "cube" + }, + "depends_on": [], + "task_instance_id": "e1_0", + "task_type": "E1", + "role": "primary" + }, + { + "id": "place_cube", + "call": { + "kind": "place", + "object": "cube", + "on": "tray" + }, + "depends_on": ["pick_cube"], + "task_instance_id": "e1_0", + "task_type": "E1", + "role": "primary" + } + ], + "task_groups": [ + { + "id": "e1_0", + "task_type": "E1", + "node_ids": ["pick_cube", "place_cube"], + "depends_on": [], + "success": { + "kind": "object_supported_by", + "object": "cube", + "support": "tray" + } + } + ], + "success": { + "kind": "all_task_groups" + } +} +``` + +The concrete schema should reuse the exact `SemanticCallCfg` and decoder from +the Expert Program stack. GenSim must not define another semantic-call JSON +format. + +### 5.3 Allowed node data + +- Stable node and TaskGroup IDs. +- A versioned `SemanticCallCfg` payload. +- Canonical scene-reference IDs. +- Task dependencies and TaskGroup membership. +- Task-level route guards and completion importance. +- Bounded task-level failure routes. +- Planner provenance and confidence. +- Optional semantic resource selections when explicitly bound to the exact + integration fingerprint. + +### 5.4 Forbidden node data + +- Atomic action class or implementation name. +- `ActionInvocation`, typed atomic goal, or action options. +- Arm, hand, control-part, solver, or controller route. +- `MotionPolicy`, planner backend, or recovery thresholds. +- qpos, EEF pose, grasp pose, waypoint, trajectory, or command frame. +- Runtime `ResourceClaim` snapshots. +- Held-object state or speculative physical effects. +- Action-level preconditions and postconditions already owned by the semantic + compiler or effect monitor. + +Task-level object pose constraints may be stored when they are part of the +task intent. Grounded robot or motion coordinates may not be stored. + +### 5.5 Look-ahead without premature execution + +For a selected `Pick -> Place` route, the scheduler should pass both calls to +the canonical runtime analysis window but execute only the first call: + +```python +runtime.start( + pick_call, + place_call, + execution_prefix_length=1, +) +``` + +This lets the compiler use the Place target during grasp selection while +retaining a verified physical boundary after Pick. If the route changes after +Pick, the scheduler can replace only the unfinished suffix. + +If the final canonical runtime does not expose an analysis-window/execution- +prefix boundary, that capability belongs in Semantic Skill, not in GenSim. + +## 6. PR-by-PR adjustment plan + +### 6.1 PR #532: Scene Authoring only + +Recommended title: + +> `feat(scene-engine): add generated-scene authoring and editing` + +Keep: + +- scene generation and editing pipelines; +- layout, gravity settling, and asset preparation; +- image and geometry service boundaries; +- scene import/export; +- semantic evidence and provenance. + +Adjust: + +- Rename the public `SceneGraph` concept to `GeneratedSceneGraph` or + `SceneAuthoringGraph`, or make its authoring-only status explicit. +- Emit stable canonical IDs, parent relationships, affordance evidence, + geometry metadata, and physics provenance. +- Do not construct or own a live `SceneRegistry`. +- Keep richer generation evidence in an audit-only sidecar rather than a + second execution manifest. +- Put conversion to `SceneManifest` in #538's cross-engine adapter. + +Exit criteria: + +- The generated artifact converts deterministically to one canonical + `SceneManifest`. +- Import/export preserves canonical identity and ancestry. +- No authoring artifact contains live simulator handles or pose readers. +- Scene Engine types cannot become a second runtime scene authority. + +### 6.2 PR #533: TaskSpec and SemanticTaskGraph contracts + +Recommended title: + +> `feat(task-engine): add TaskSpec and semantic task-graph contracts` + +Keep: + +- `TaskSpec`, E1-E9 ontology, and `SuccessSpec`; +- strict JSON validation and stable hashing; +- DAG validation and TaskGroup metadata; +- planner provenance and bounded failure policy. + +Transform: + +- Replace direct-AtomicAction `SeedGraph` with `SemanticTaskGraph`. +- Make every graph node contain one canonical `SemanticCallCfg`. +- Replace `capability_catalog_hash` with the canonical semantic integration + fingerprint. +- Validate TaskGroup semantic coverage using calls and success conditions, + rather than hard-coded action sequences. + +Remove: + +- `AtomicCapabilityRegistry`; +- the parallel generic `CapabilityRegistry` when it represents the same + executable surface; +- `build_atomic_capability_registry()`; +- GenSim-owned runtime policy and motion-policy defaults; +- node fields for actor, control, atomic action, and motion policy. + +Unsupported tasks: + +- A TaskSpec may still express a task such as Pour. +- If the semantic catalog has no executable call, planning returns a structured + `unsupported_semantic_capability` result. +- The planner must not emit planning-only fake AtomicAction nodes. + +Exit criteria: + +- Graph JSON round-trips through the same Expert Program semantic-call decoder. +- Forbidden grounded or atomic fields are rejected recursively. +- Graph hashes are deterministic. +- There is one capability/fingerprint source. + +### 6.3 PR #534: Task interpretation and canonical scene binding + +Recommended title: + +> `feat(task-engine): add task interpretation and canonical scene binding` + +Keep: + +- typed instruction interpretation; +- multiple TaskDraft candidates; +- explicit role grounding and ambiguity diagnostics; +- `SceneRequirements` and task-level success conditions; +- strict structured-model boundaries where an MLLM is used. + +Adjust: + +- Bind scene roles only to canonical `SceneObjectRef`, + `SceneArticulationRef`, `SceneLinkRef`, and `SceneAffordanceRef` identities. +- Resolve references through the canonical `SceneManifest`. +- Consume a planner projection generated from `RobotSkillProfile` and + `SemanticCallCatalog`. +- Keep language grounding separate from physical goal grounding. + +Remove: + +- robot/action configuration construction from + `generation/config_builder.py`; +- hard-coded `left_arm`, `right_arm`, gripper states, and packaged robot-action + policy profiles; +- keyword-based affordance or object inference; +- knowledge of planner backends, solvers, controllers, or atomic goal types. + +Exit criteria: + +- Interpretation produces only TaskSpec, role bindings, and semantic call + candidates. +- Canonical identity is resolved once and ambiguity fails explicitly. +- MLLM output cannot bypass the same local schema and catalog validation used + by deterministic callers. + +### 6.4 PR #535: Semantic graph planning and bundle generation + +Recommended title: + +> `feat(task-engine): add semantic task-graph planning and bundles` + +Keep: + +- deterministic offline recipes; +- online semantic task planning; +- candidate scoring and selection; +- conservative TaskGroup-level fusion; +- stable graph loader, hashing, visualization, and bundle artifacts; +- candidate-local preparation failures. + +Adjust: + +- Make all recipes produce Semantic Calls rather than AtomicActions. +- Compile and validate only `SemanticTaskGraph` topology and task semantics. +- Preflight calls through the canonical Expert Program and Semantic Skill + catalogs. +- Derive exact resource conflicts from bound `ResourceClaim` values instead of + persisting claims in the graph. +- Move robot, sensor, and light templates to their owning scene/robot + integration packages. + +Recommended bundle: + +```text +task_spec.json +scene_requirements.json +semantic_task_graph.json +semantic_task_graph.png +integration_fingerprint.json +planner_report.json +``` + +Exit criteria: + +- Offline and online candidates use the same graph schema. +- Candidate fusion occurs only at complete TaskGroup boundaries. +- Every executable candidate passes Semantic Skill provider-free preflight. +- No graph compiler imports or constructs atomic-action implementation types. + +### 6.5 PR #536: Thin Semantic Skill runtime binding + +Recommended title: + +> `feat(task-engine): bind semantic task graphs to SkillRuntime` + +Remove: + +- `ActionGrounder`; +- `AtomicActionAdapter`; +- `atomic_compat.py`; +- `robot_parts.py`; +- `solver_compat.py`; +- GenSim-owned frame-to-action goal lowering; +- GenSim motion-policy materialization; +- duplicate qpos and held-object execution state. + +Replace with a narrow semantic execution boundary: + +1. Decode one graph call through the canonical semantic-call decoder. +2. Validate it against the exact semantic integration fingerprint. +3. Build the selected route's semantic analysis window. +4. Call `SkillRuntime` with an execution prefix. +5. Return the canonical `SkillResult` without reconstructing physical truth. + +Move remaining responsibilities: + +| Current responsibility | New owner | +|---|---| +| camera/depth target materialization | registered Semantic Skill target provider or lowerer | +| live object and articulation goal grounding | `SemanticSkillCompiler` | +| robot arm and endpoint selection | `RobotSkillProfile` | +| grasp collision cache | Atomic Action/graspkit planning service | +| action effect predicate | semantic effect monitor/evidence collector | +| task completion predicate | Task Engine final inspection | + +Exit criteria: + +- One graph node creates one canonical semantic runtime execution boundary. +- Every executed node captures a fresh observation. +- Pick look-ahead can inspect the selected suffix without executing it. +- GenSim does not construct `ActionInvocation` values. +- GenSim does not read qpos or held-object state as an independent authority. + +### 6.6 PR #537: TaskGraphScheduler, graph recovery, and reporting + +Recommended title: + +> `feat(task-engine): add graph scheduling, recovery, and reporting` + +Keep: + +- immutable original graph; +- a detached runtime graph and ordered revision log; +- bounded route and transition budgets; +- unfinished-suffix replanning; +- execution recording and tensor-free reporting; +- shared-state A/B evaluation contracts. + +Replace `ProgramExecutor` with `TaskGraphScheduler`. + +`TaskGraphScheduler` owns only: + +- DAG readiness and deterministic tie-breaking; +- selected-route and TaskGroup transitions; +- a shared task-node barrier with row-local masks; +- calls to `SkillRuntime` or `ParallelSkillRuntime`; +- consumption of verified `SkillResult` and `TaskState` values; +- task-level route substitution after runtime recovery is exhausted; +- graph revision, transition, and task-recovery records. + +Remove: + +- merged-trajectory and per-arm command scheduling; +- controller dispatch and simulation stepping; +- action retry and grasp retry; +- physical effect verification; +- raw qpos and held-object execution state; +- failure classification from exception strings; +- command-level timeout and safe-stop logic. + +Parallel behavior: + +- Delegate physical concurrency to the canonical `ParallelSkillRuntime` and its + required safety validator. +- If those contracts are unavailable, serialize independent ready nodes + deterministically. +- Never merge command frames or trajectories in `TaskGraphScheduler`. + +Naming: + +- Rename `ActionAgent` to `SemanticTaskPlanner` or `TaskPlanAgent`. +- Avoid exposing `gen_sim.action_engine` as a second public execution system. +- Prefer moving planning/runtime-graph code under + `embodichain.gen_sim.task_engine` while the PRs remain unmerged. + +Exit criteria: + +- Graph recovery begins only after a canonical runtime terminal result. +- Verified `TaskState` is the only physical-state transition input. +- Scheduler cancellation delegates safe stop to the active runtime. +- Parallel paths use the canonical parallel safety boundary. +- Reports retain original Semantic Skill events and failures without replacing + them with string-derived classifications. + +### 6.7 PR #538: End-to-end orchestration + +Recommended title: + +> `feat(task-engine): add semantic orchestration and end-to-end integration` + +Keep: + +- Scene/Task/Planner orchestration; +- run-directory isolation; +- feasibility reports; +- final task inspection; +- CLI and artifact publication; +- task or scene regeneration; +- end-to-end benchmarks. + +Adjust: + +- Make `SceneAdapter` produce one canonical `SceneManifest`, a + SceneRegistry-registration plan, and optional audit evidence. +- Make `FeasibilityBroker` consume the Semantic Integration Catalog rather than + an atomic capability registry. +- Make the coordinator invoke `TaskGraphScheduler` rather than + `ProgramExecutor` or `AtomicActionAdapter`. +- Keep final inspection at TaskSpec success level; do not reimplement grasp, + release, handover, or articulation effect checks. +- Treat `StaticSceneManifest`, redacted manifests, and + `ConservativeSceneGraph` as derived evidence or reports, not execution + identity sources. + +Split out unrelated changes: + +- upright-grasp ranking changes in `pick_up.py`; +- yaw-equivalent downstream-reachability changes; +- graspkit implementation changes; +- corresponding Atomic Action tests. + +These are reusable lower-layer enhancements and should land as a focused +Atomic Action prerequisite before the adjusted task-planning stack. + +Exit criteria: + +- One end-to-end path runs from generated scene data through canonical scene + integration, SemanticTaskGraph, SkillRuntime, and final inspection. +- No orchestration component sends controller commands directly. +- Infeasible tasks publish structured failure artifacts without stale bundles. +- Scene/task regeneration starts only after the active runtime reaches a safe + terminal boundary. + +## 7. Dynamic-task support + +Dynamic behavior has several different meanings and must be assigned to the +correct layer. + +### 7.1 Supported behavior + +- A moving target can invalidate and replan the current action through + `SceneEntityPose`, scene dependencies, and `ExecutionSession`. +- Collision-world pose revisions can trigger row-local replanning through the + canonical scene/planner integration. +- Every Semantic Call is observed and JIT-grounded again before execution. +- Pick can use the selected downstream suffix for grasp look-ahead. +- The TaskGraphScheduler can replace an unfinished suffix after a verified + semantic-call boundary. +- Offline and online candidates can be selected or fused at TaskGroup + boundaries. +- The immutable source graph can retain an auditable sequence of runtime + revisions. +- Final task inspection can trigger a bounded repair TaskGroup or new route. + +### 7.2 Explicit limitations + +- An active call cannot be replaced by an unrelated skill in place. +- A graph cannot be rewritten while a physical effect is pending verification. +- Changing runtime controller destinations requires a new invocation and safe + ownership transition. +- Dynamic obstacle pose updates are supported only where the registered scene + provider and planner support them. +- Entity add/remove and geometry changes require a new scene integration and + runtime session; they are not an in-place pose revision. +- The first adjusted version should retain a shared task-node barrier. It may + use row-local success, failure, eligibility, and recovery masks, but should + not claim arbitrary divergent graph program counters per environment. + +## 8. Error-recovery ownership + +| Failure class | Owner | Behavior | +|---|---|---| +| tracking error | `ExecutionSession` / `ExecutionRunner` | bounded row-local replan | +| moving semantic target | `ExecutionSession` | re-ground the same invocation revision within policy | +| collision-world revision | `ExecutionSession` | invalidate and replan affected rows | +| planner failure | Atomic Action runtime | retry within the action policy and emit typed failure | +| controller rejection or timeout | `ExecutionRunner` | cancel addressed targets, then hold safely | +| semantic effect not achieved | `SkillRuntime` | verify evidence and apply the semantic workflow policy | +| held relation lost | `SkillRuntime` | perform bounded physical reacquisition where configured | +| current TaskGroup route exhausted | `TaskGraphScheduler` | choose a different TaskGroup or unfinished suffix | +| final task predicate failed | Task Engine | add a bounded repair route or regenerate the plan | +| task or scene infeasible | Task Engine orchestration | publish failure or create a new transaction | + +Important constraints: + +- The graph scheduler must not insert its own Pick merely because it infers + that an object was dropped. Canonical workflow recovery owns real + reacquisition. +- Task-level recovery begins only after `SkillRuntime` exhausts its own action + and workflow budgets. +- Original typed failures and evidence remain in reports. A task-level category + may summarize them but must not replace them. +- Every layer owns a separate bounded budget so nested recovery cannot form an + unbounded loop. + +Recommended budget hierarchy: + +```text +Atomic recovery budget + inside one ActionInvocation + +Semantic workflow recovery budget + retry or physical reacquisition of one semantic call + +Task graph revision budget + alternate TaskGroup or unfinished suffix + +Task orchestration regeneration budget + new task or scene transaction +``` + +## 9. Components to keep, transform, move, and remove + +### 9.1 Keep + +- `TaskSpec`, E1-E9 ontology, and `SuccessSpec`. +- Offline and online candidate generation. +- TaskGroup grouping and conservative fusion. +- Immutable graph hash, loader, visualization, and artifacts. +- Runtime graph revision log. +- Task-level route recovery and suffix replanning. +- Final task inspection. +- Run directories, recording, reporting, and A/B comparison. + +### 9.2 Transform + +| Current concept | Target concept | +|---|---| +| direct AtomicAction SeedGraph | `SemanticTaskGraph` | +| ActionAgent | `SemanticTaskPlanner` or `TaskPlanAgent` | +| ProgramExecutor | `TaskGraphScheduler` | +| capability registry | planner projection of Semantic Integration Catalog | +| runtime graph node action | canonical `SemanticCallCfg` | +| action postcondition | semantic effect result or task success predicate | +| static scene execution manifest | canonical `SceneManifest` plus evidence sidecar | + +### 9.3 Move + +- Visual target providers to registered Semantic Skill grounders/providers. +- Robot and endpoint selection to `RobotSkillProfile`. +- Grasp collision preparation to Atomic Action/graspkit planning services. +- Atomic grasp-quality improvements to a focused lower-layer prerequisite PR. +- Task success predicates to Task Engine final inspection. +- CLI-only execution assembly to the final orchestration layer. + +### 9.4 Remove + +- `AtomicCapabilityRegistry` and repeated registry builders. +- `ActionGrounder`. +- `AtomicActionAdapter`. +- `ProgramExecutor` as a physical executor. +- GenSim qpos and held-object execution truth. +- GenSim motion policy and robot-part compatibility layers. +- Independent command scheduling, effect verification, and safe-stop logic. +- Hard-coded arm names and robot action templates. +- Planning-only fake actions in executable graph artifacts. + +## 10. Dependency and landing plan + +### Gate 0: consolidate the Semantic Skill stack + +Before adapting the GenSim runtime layers: + +1. Decide whether runtime remains in #492 or is owned by #496. +2. Produce exactly one canonical runtime API. +3. Restack #495-#504 and #480-#483 on the current #492 head. +4. Preserve or land the following shared contracts: + - canonical Semantic Call config/decoder; + - Semantic Integration Catalog and fingerprint; + - physical effect/evidence runtime; + - workflow recovery and reacquisition; + - parallel runtime and safety validator if physical parallelism is required. + +### Recommended merge order + +```text +Semantic Skill runtime consolidation + | + +------ focused Atomic Action grasp enhancement + | + +------ adjusted PR #532 Scene Authoring + | + v +PR #533 SemanticTaskGraph contracts + | + v +PR #534 task interpretation and scene binding + | + v +PR #535 semantic graph planning and bundles + | + v +PR #536 thin SkillRuntime binding + | + v +PR #537 graph scheduling and task-level recovery + | + v +PR #538 end-to-end orchestration +``` + +Operationally, #533-#538 should be marked Draft while their contracts and +branches are rewritten. Their PR bodies should be updated with the new +dependency chain, ownership rules, and explicit removal of the duplicate +execution architecture. + +## 11. Validation plan + +### 11.1 Architecture guards + +- `embodichain/gen_sim/task_engine` must not directly import + `embodichain.lab.sim.atomic_actions`. +- GenSim must not define `AtomicCapabilityRegistry`, `ActionGrounder`, + `AtomicActionAdapter`, or a physical `ProgramExecutor`. +- An architecture test must reject direct controller, simulator-step, or + command-frame ownership in Task Engine. +- A graph-schema test must recursively reject atomic action, motion policy, + qpos, grasp pose, and trajectory fields. +- Capability and integration fingerprints must come from one canonical + Semantic Integration Catalog. + +### 11.2 Contract tests + +- Semantic graph calls round-trip through the Expert Program decoder. +- Canonical scene aliases normalize exactly once. +- Unknown or ambiguous references fail with pathful diagnostics. +- Unsupported semantic capabilities reject a candidate before bundle + publication. +- Integration fingerprint drift fails before execution. +- Offline and online candidates use the same graph and call schema. + +### 11.3 Runtime-boundary tests + +- One graph node creates one semantic runtime execution boundary. +- A fresh observation is captured before every call. +- A selected future suffix participates in compiler look-ahead without + premature execution. +- Verified `TaskState` is the only state adopted by the graph scheduler. +- A controller rejection invokes canonical cancel-then-hold before graph + recovery. +- Workflow reacquisition completes or exhausts before TaskGraphScheduler route + substitution. +- Parallel nodes either use canonical `ParallelSkillRuntime` with a safety + validator or execute serially. + +### 11.4 End-to-end tests + +At minimum, cover: + +1. deterministic Pick -> Place through a generated scene; +2. dual-resource HandOver through one SemanticTaskGraph; +3. moving target recovery during one semantic call; +4. real held-object loss followed by canonical SkillRuntime reacquisition; +5. exhausted semantic recovery followed by TaskGroup route replacement; +6. unsupported task capability producing a preparation failure artifact; +7. offline/online A/B candidates starting from identical state and using the + same Semantic Integration Catalog; +8. final task inspection failing and producing a bounded repair or terminal + report. + +## 12. Stack-wide acceptance criteria + +The adjusted stack is ready only when all of the following are true: + +- Semantic Skill is the only path from semantic intent to `ActionInvocation`. +- Atomic Action and `ExecutionRunner` are the only owners of command execution + and safe stop. +- GenSim artifacts contain semantic calls and task dependencies, not physical + action configuration. +- Scene Engine authoring data converts once into the canonical scene + integration. +- Robot-specific behavior comes from `RobotSkillProfile`, not task recipes. +- Each graph node is JIT-grounded from a fresh observation. +- Effects are committed only from verified Semantic Skill results. +- Graph recovery occurs only at a safe semantic boundary. +- Task-level dynamic replanning preserves completed physical effects and + revises only unfinished work. +- Reports preserve call-, effect-, recovery-, graph-, and final-task evidence + without duplicating physical truth. +- The end-to-end tutorials and benchmarks use the same runtime path as Python, + Expert Program, and model-generated callers. + +## 13. Expected result + +After this adjustment, the two systems become complementary: + +- Semantic Skill supplies one reliable, robot-generic, effect-aware, and + recoverable physical execution architecture. +- GenSim supplies task interpretation, candidate planning, immutable task + graphs, dynamic suffix replanning, task-level recovery, audit artifacts, and + A/B evaluation. + +The final system has one physical truth, one capability source, one grounding +path, and one execution path, while retaining GenSim's task-level planning and +dynamic orchestration strengths. diff --git a/embodichain/lab/sim/skills/calls.py b/embodichain/lab/sim/skills/calls.py index f0e359a88..12207be04 100644 --- a/embodichain/lab/sim/skills/calls.py +++ b/embodichain/lab/sim/skills/calls.py @@ -400,32 +400,19 @@ class HandOver(SemanticCallSpec): Args: object: Authoritative held-object reference. - receiver: Optional destination resource ID. It is equivalent to the - ``destination`` resource slot and must agree with an explicit map. final_target: Optional final object-space delivery pose. - resources: Optional skill-local resource overrides. + resources: Optional ``source`` and ``destination`` resource overrides. """ call_kind: ClassVar[str] = "hand_over" object: SceneObjectRef - receiver: str | None = None final_target: SemanticPose | None = None def __post_init__(self) -> None: SemanticCallSpec.__post_init__(self) if type(self.object) is not SceneObjectRef: raise TypeError("HandOver.object must be a SceneObjectRef.") - resources = dict(self.resources) - if self.receiver is not None: - _validate_identifier(self.receiver, field_name="HandOver.receiver") - selected = resources.get("destination") - if selected is not None and selected != self.receiver: - raise ValueError( - "HandOver.receiver conflicts with resources['destination']." - ) - resources["destination"] = self.receiver - object.__setattr__(self, "resources", _snapshot_resources(resources)) if self.final_target is not None: if type(self.final_target) is not SemanticPose: raise TypeError("HandOver.final_target must be a SemanticPose or None.") diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 04093b7f2..1872f27d2 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -20,7 +20,7 @@ from abc import ABC, abstractmethod from collections.abc import Iterable, Mapping -from dataclasses import dataclass, field +from dataclasses import dataclass, field, replace from enum import Enum from types import MappingProxyType from typing import ClassVar @@ -583,6 +583,9 @@ def analyze( raise TypeError("calls must contain exact supported semantic call values.") bound_calls: list[BoundSemanticCall] = [] + effect_kinds: list[SemanticEffectKind] = [] + dependencies: list[SemanticEffectDependency] = [] + latest_holder: dict[str, tuple[int, str]] = {} for index, call in enumerate(supplied): if type(call) is RegisteredSemanticCall and ( call.call_id not in self._registered_lowerers @@ -594,13 +597,12 @@ def analyze( "installed compiler lowerer.", tuple(self._registered_lowerers), ) - bound_calls.append( - self._integration.link_call( - call, - path=(*path, index, "call"), - ) + call = self._inherit_held_resource(call, latest_holder) + bound = self._integration.link_call( + call, + path=(*path, index, "call"), ) - for index, bound in enumerate(bound_calls): + bound_calls.append(bound) call = bound.linked.call if type(call) is HandOver: self._require_handover_pose_provider( @@ -625,12 +627,6 @@ def analyze( target.relation, path=(*path, index, "call", "destination"), ) - - dependencies: list[SemanticEffectDependency] = [] - latest_holder: dict[str, tuple[int, str]] = {} - analyzed: list[AnalyzedSemanticCall] = [] - for index, bound in enumerate(bound_calls): - call = bound.linked.call if type(call) is Pick: previous = latest_holder.get(call.object.entity_id) if previous is not None: @@ -693,6 +689,13 @@ def analyze( # A registered extension has no declarative state-flow contract # in Version 1. Treat it as an opaque effect boundary. latest_holder.clear() + effect_kinds.append(effect_kind) + + analyzed: list[AnalyzedSemanticCall] = [] + for index, (bound, effect_kind) in enumerate( + zip(bound_calls, effect_kinds, strict=True) + ): + call = bound.linked.call downstream_target = ( self._downstream_target(index, bound_calls) if type(call) is Pick @@ -713,6 +716,25 @@ def analyze( _compiler_id=self._compiler_id, ) + @staticmethod + def _inherit_held_resource( + call: SemanticCallSpec, + latest_holder: Mapping[str, tuple[int, str]], + ) -> SemanticCallSpec: + """Fill an omitted consumer slot from the workflow's known holder.""" + if type(call) is Place: + slot_id = "primary" + elif type(call) is HandOver: + slot_id = "source" + else: + return call + holder = latest_holder.get(call.object.entity_id) + if holder is None or slot_id in call.resources: + return call + resources = dict(call.resources) + resources[slot_id] = holder[1] + return replace(call, resources=resources) + def ground( self, workflow: SemanticWorkflow, diff --git a/tests/sim/skills/test_calls.py b/tests/sim/skills/test_calls.py index d20ba087d..ec9d91272 100644 --- a/tests/sim/skills/test_calls.py +++ b/tests/sim/skills/test_calls.py @@ -192,22 +192,15 @@ def test_place_snapshots_absolute_destination_pose() -> None: torch.testing.assert_close(call.at.to_matrix(), destination.to_matrix()) -def test_handover_normalizes_receiver_as_destination_resource() -> None: - call = HandOver(object=SceneObjectRef("cube"), receiver="right_actor") +def test_handover_uses_destination_resource_selection() -> None: + call = HandOver( + object=SceneObjectRef("cube"), + resources={"destination": "right_actor"}, + ) - assert call.receiver == "right_actor" assert call.resources == {"destination": "right_actor"} -def test_handover_rejects_conflicting_receiver_resource() -> None: - with pytest.raises(ValueError, match="conflicts"): - HandOver( - object=SceneObjectRef("cube"), - receiver="right_actor", - resources={"destination": "left_actor"}, - ) - - def test_handover_snapshots_optional_final_target() -> None: final_target = _identity_pose() diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index 5a1e8aca7..d3df21288 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -414,6 +414,32 @@ def _compiler( ) +def _dual_compiler( + registry: SceneRegistry, +) -> tuple[ + SemanticSkillCompiler, + AtomicActionEngine, + _DualCenterHandOverProvider, +]: + profile = _dual_profile() + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=builtin_semantic_call_catalog(), + ) + engine = _engine(profile) + provider = _DualCenterHandOverProvider() + return ( + SemanticSkillCompiler( + manifest.bind(registry, engine), + relation_grounders=(_FrameRelationGrounder(),), + handover_pose_providers=(provider,), + ), + engine, + provider, + ) + + def _context( registry: SceneRegistry, *, @@ -519,6 +545,87 @@ def test_grounded_eligibility_hands_off_to_execution_session() -> None: assert session.eligible_mask.tolist() == [False, False] +def test_place_inherits_known_pick_resource_when_primary_is_omitted() -> None: + registry, _ = _scene_registry() + compiler, _, _ = _dual_compiler(registry) + place = Place( + object=SceneObjectRef("cube"), + at=SemanticPose((0.4, 0.2, 0.3), (1.0, 0.0, 0.0, 0.0)), + ) + + workflow = compiler.analyze( + ( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "right"}, + ), + place, + ) + ) + + assert place.resources == {} + assert workflow.calls[1].call.resources == {"primary": "right"} + assert workflow.calls[1].bound.binding.resource_ids == {"primary": "right"} + + +def test_handover_source_inherits_known_pick_resource() -> None: + registry, _ = _scene_registry() + compiler, _, provider = _dual_compiler(registry) + handover = HandOver( + object=SceneObjectRef("cube"), + resources={"destination": "left"}, + ) + + workflow = compiler.analyze( + ( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "right"}, + ), + handover, + ) + ) + + assert handover.resources == {"destination": "left"} + assert workflow.calls[1].call.resources == { + "source": "right", + "destination": "left", + } + assert workflow.calls[1].bound.binding.resource_ids == { + "source": "right", + "destination": "left", + } + assert provider.calls == 0 + + +def test_explicit_consumer_resource_must_match_known_holder() -> None: + registry, _ = _scene_registry() + compiler, _, _ = _dual_compiler(registry) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze( + ( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "right"}, + ), + Place( + object=SceneObjectRef("cube"), + at=SemanticPose( + (0.4, 0.2, 0.3), + (1.0, 0.0, 0.0, 0.0), + ), + resources={"primary": "left"}, + ), + ) + ) + + assert error.value.diagnostic.code == "held_resource_mismatch" + assert error.value.diagnostic.rendered_path == ( + "workflow[1].call.resources.primary" + ) + + def test_pick_relation_lookahead_stays_late_bound_scene_dependency() -> None: registry, _ = _scene_registry() compiler, engine = _compiler(registry) From 867d7a22d7e974d3e2400d997a772768a9145c13 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 03:33:12 +0800 Subject: [PATCH 03/29] refactor(atomic-actions): preserve per-environment runtime lifecycle --- agent_context/MAP.yaml | 4 + .../topics/atomic-actions/atomic-actions.md | 54 +- .../overview/sim/atomic_actions/index.md | 43 +- docs/source/tutorial/atomic_actions.rst | 76 +- .../lab/sim/atomic_actions/__init__.py | 2 + embodichain/lab/sim/atomic_actions/effects.py | 117 ++- embodichain/lab/sim/atomic_actions/engine.py | 5 +- .../lab/sim/atomic_actions/execution.py | 500 +++++++++++- .../lab/sim/atomic_actions/policies.py | 2 +- embodichain/lab/sim/atomic_actions/runner.py | 104 ++- .../sim/atomic_actions/test_engine_per_env.py | 729 +++++++++++++++++- tests/sim/atomic_actions/test_runner.py | 216 +++++- 12 files changed, 1688 insertions(+), 164 deletions(-) diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 8c0d3bc57..aebc0a6d2 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -563,6 +563,10 @@ topics: - PlanningContext - ExecutionSession - EffectVerificationRequest + - EffectVerificationResult + - eligible_mask + - deactivate_rows + - effect verification deadline - ExecutionRunner - ObservationProvider - CommandSink diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 1e46b14c2..2335a8d06 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -488,7 +488,7 @@ runner = ExecutionRunner( command_sink, clock=execution_clock, ) -result = runner.step(effect_success=None) +result = runner.step(effect_result=None) ``` `ExecutionSession` owns deterministic planning progress and recovery state. It @@ -514,21 +514,43 @@ active targets so the caller can still hold them. The session monitors: - action-attempt timeout; - planner and semantic-effect failure. -The optional initial `eligible_mask` is copied onto the engine device and must -be a boolean tensor with one value per environment. Initially ineligible rows -never re-enter the cohort. They are excluded from every command, replan, effect -verification, and later invocation barrier. An all-false cohort creates a -failed session without invoking any action planner. - -It replans from the latest observation within per-environment budgets. The -budgets and eligibility masks are row-local, while the action waypoint cursor -is batch-synchronized: one allowed replan regenerates the active cohort and -restarts its action trajectory without charging unaffected rows. Unknown -or exhausted failures are reported as structured `ExecutionEvent` objects. A -non-empty `StateDelta` is not committed until the caller supplies an external -`effect_success` mask. While verification is outstanding, -`ExecutionTick.pending_effect` retains a typed `EffectVerificationRequest` on -every tick; `EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. +It replans from the latest observation within per-environment budgets. Pass an +owned boolean `eligible_mask` to `engine.start()` when a previous semantic call +has already deactivated rows. Eligibility can only shrink; use +`runner.deactivate_rows(mask, reason=...)` while a runner owns scheduling so its +cached effect request stays correlated. The budgets, verified task state, and +eligibility masks are row-local, while the action waypoint cursor and call +barrier are batch-synchronized. One allowed replan regenerates the still-pending +cohort without charging unaffected rows. Exhausted rows hold and never become +eligible again. + +A non-empty `StateDelta` is not committed until the caller supplies a +correlated `EffectVerificationResult`. Its disjoint `success_mask` and +`failure_mask` must be subsets of the current request mask; requested rows in +neither mask remain unresolved. Partial successes commit immediately while +unresolved rows keep the barrier pending. `EffectVerificationRequest` carries a +monotonic `verification_id`, stable `requested_at`/`deadline` values in the +robot-observation timestamp domain, and an owned effect snapshot. Mask shrinkage +creates a new ID without extending the deadline; whole-action retry creates a +new attempt. Results for an old ID are rejected. `RecoveryPolicy.action_timeout` +covers the trajectory and terminal effect wait together, and only timestamps +strictly greater than the deadline time out. While verification is outstanding, +`ExecutionTick.pending_effect` retains the request on every tick; +`EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. + +```python +request = tick.pending_effect +effect_result = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=observed_success, + failure_mask=observed_failure, +) +result = runner.step(effect_result=effect_result) +``` + +Cause events (`ACTION_PLANNING_FAILED`, `EFFECT_VERIFICATION_FAILED`, and +`EFFECT_VERIFICATION_TIMEOUT`) are distinct from the `ACTION_RETRY` recovery +event. `SESSION_COMPLETED` and `SESSION_FAILED` are distinct terminal events. Recovery replans reuse the current immutable `ResolvedActionRequest`, including its owned goal snapshot. Mutable goal values are copied, while simulator-backed diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index e90d85289..8b117c846 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -440,9 +440,11 @@ an older custom action by renaming its implementation to `_plan()`. | `AtomicAction.plan(request, context)` | `AtomicActionEngine` | Binds the current collision scene into a copied policy, then delegates to `_plan()` | | `AtomicAction._plan(request, context)` | Atomic-action implementer | Consumes the prepared immutable `ResolvedActionRequest` and returns an `ActionPlan` | | `engine.plan_action(action, invocation, context)` | Extension or isolated test | Temporarily binds and plans an unregistered action instance; built-in parameter variants should use invocation `skill_options` instead | +| `engine.start(invocations, context, eligible_mask=...)` | Runtime orchestrator | Starts a session whose owned row cohort can only shrink across action barriers and recovery | | `session.revise_current(invocation)` | Manually ticked runtime orchestrator | Replaces the active logical call with a newer same-destination revision and replans from the latest observed context | | `runner.revise_current(invocation)` | Runner-driven runtime orchestrator or Action Agent | Snapshots a revision, preserves the current frame deadline, then replans from a fresh due-time observation | -| `runner.step(effect_success=...)` | Non-blocking controller integration | Observes and routes a `RuntimeCommandFrame` only when it is due | +| `runner.deactivate_rows(mask, reason=...)` | Runner-driven runtime orchestrator | Permanently removes rows and refreshes the runner's cached effect request; prefer it over direct session mutation | +| `runner.step(effect_result=...)` | Non-blocking controller integration | Observes and routes a `RuntimeCommandFrame` only when it is due | | `runner.run_until_blocked(...)` | Simple blocking application or tutorial | Advances the injected clock until terminal or external effect verification is required | | `runner.cancel(reason)` | Explicit safe stop | Requests controller cancellation followed by an observed-position hold | @@ -623,6 +625,16 @@ unknown or incompatible transport cannot cause partial dispatch. Cancellation, observation/session exceptions, and negative acknowledgements enter a best-effort cancel-then-hold path for every armed runtime target. +Pass an owned `eligible_mask` to `engine.start()` when only a subset of rows may +enter the invocation sequence. This cohort is sticky: eligibility can only +shrink across action barriers and replans. Later failures outside the atomic +runtime should call `runner.deactivate_rows(mask, reason=...)`; the operation is +idempotent, the next command neutralizes changed rows, and removing the final +eligible row fails and terminates the session. When effect verification is +pending, deactivation narrows the request and assigns a new +`verification_id`. Do not mutate `session` directly while its runner owns +scheduling, because the runner must refresh its cached effect boundary. + The engine authorizes every emitted command against the immutable target and physical claims in the resolved binding. A command cannot address an unbound destination, substitute target metadata, or overlap another endpoint's joints @@ -786,20 +798,39 @@ Consumers query per-environment active and exclusive-hold masks from that one map. A single-arm transport, release, or handover row fails safely while a second manipulator still holds the same semantic object or live entity. -At the terminal waypoint, an `ExecutionSession` requests an external -per-environment verification mask before committing a non-empty effect: +At the terminal waypoint, an `ExecutionSession` requests an external, +correlated per-environment result before committing a non-empty effect: ```python +from embodichain.lab.sim.atomic_actions import EffectVerificationResult + tick = session.tick(latest_context) if tick.pending_effect is not None: - effect_success = verify_grasp_or_release() - tick = session.tick(latest_context, effect_success=effect_success) + request = tick.pending_effect + success_mask, failure_mask = verify_grasp_or_release(request.env_mask) + effect_result = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + ) + tick = session.tick(latest_context, effect_result=effect_result) ``` This prevents a collision-free or well-tracked command plan from being misreported as a successful grasp, release, or handover. The typed `EffectVerificationRequest` persists on subsequent ticks while waiting; -`EFFECT_VERIFICATION_REQUIRED` remains a one-time observability event. +`EFFECT_VERIFICATION_REQUIRED` remains a one-time observability event. Success +and failure masks are disjoint subsets of the request mask; omitted request rows +remain unresolved. Request IDs change after mask shrinkage or whole-action +retry, so a delayed result cannot commit a newer attempt. + +`request.deadline` is expressed in the robot-observation timestamp domain. +`RecoveryPolicy.action_timeout` covers both trajectory execution and the +terminal effect wait; a retry invalidates the old request ID. With +`ExecutionRunner.step()`, a call made before the next due cycle does not consume +its `effect_result`: schedule another call using `wait_duration`, re-read the +current request, and submit a result for that current ID. Partial resolution and +row deactivation can also replace the request before the delayed result arrives. ## Action Agent integration diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 3fc278938..03cfd6705 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -348,7 +348,12 @@ must be resolved from the latest scene snapshot: ) task = TaskState.empty(robot.get_qpos().shape[0], robot.device) initial_context = adapter.observe(task) - session = engine.start((invocation,), initial_context) + initial_eligible = determine_ready_rows(initial_context) + session = engine.start( + (invocation,), + initial_context, + eligible_mask=initial_eligible, + ) router = EndpointCommandRouter((adapter,)) runner = ExecutionRunner(session, adapter, router, clock=adapter) result = runner.run_until_blocked() @@ -375,6 +380,25 @@ For an application that already owns its event loop, call the non-blocking with ``is_waiting`` set has not consumed a new observation or effect result; use its ``wait_duration`` to schedule the next call. +``eligible_mask`` is an owned initial cohort, not a one-tick filter. Eligibility +can only shrink for the lifetime of the session and remains inactive across +action barriers and replans. If an application later loses a row, deactivate it +through the runner that owns scheduling: + +.. code-block:: python + + changed = runner.deactivate_rows( + lost_tracking_mask, + reason="object tracking was lost", + ) + +The operation is idempotent and the next command actively neutralizes changed +rows. Deactivating rows while an effect is pending narrows the request and +changes its ``verification_id``. Deactivating the last eligible row fails and +terminates the session. Do not call ``session.deactivate_rows()`` directly while +an ``ExecutionRunner`` owns the session because the runner must refresh its +cached effect boundary. + The complete simulation example starts with a visible cube directly in front of the robot, then applies a short horizontal force pulse so physics and friction slide it sideways during one ``PickUp`` invocation whose @@ -471,24 +495,58 @@ Task-state effects Pick, place, handover, and coordinated skills declare attachment changes as a :class:`~embodichain.lab.sim.atomic_actions.StateDelta`. Planning does not commit -those changes. During closed-loop execution, a non-empty effect requires an -external per-environment verification mask: +those changes. During closed-loop execution, a non-empty effect requires a +correlated per-environment verification result: .. code-block:: python + from embodichain.lab.sim.atomic_actions import EffectVerificationResult + def verify_effect(context, tick): - return verify_grasp_or_release(context) + request = tick.pending_effect + assert request is not None + success_mask, failure_mask = verify_grasp_or_release(context, request.env_mask) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + ) result = runner.run_until_blocked(effect_verifier=verify_effect) This prevents a successful trajectory plan from being mistaken for a successful physical grasp or release. If verification is asynchronous, omit the callback; ``run_until_blocked`` returns at the verification boundary and the application -can later resume with ``runner.step(effect_success=verified)`` when the next -cycle is due, or call ``run_until_blocked(effect_verifier=...)`` again. The -runner remembers the pending boundary even though the session emits its event -only once. The durable state is ``tick.pending_effect`` (an -``EffectVerificationRequest``), not the presence of that one-time event. +can later resume from the *current* pending request: + +.. code-block:: python + + request = runner.session.pending_effect + assert request is not None + success_mask, failure_mask = await_effect_observation(request.env_mask) + verified = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + ) + resumed = runner.step(effect_result=verified) + if resumed.is_waiting: + schedule_after(resumed.wait_duration) + # This call did not consume ``verified``. Re-read the current request + # and submit a result for that ID again at the due cycle. + +Alternatively, call ``run_until_blocked(effect_verifier=...)`` again. Success +and failure masks must be disjoint subsets of the request mask; rows in neither +mask remain unresolved. A result must reuse the current request's +``verification_id``. Deactivation, partial resolution, or retry can replace the +request, so re-read it before delayed submission and re-verify if its ID or mask +changed. ``request.deadline`` uses the robot-observation timestamp domain; +``RecoveryPolicy.action_timeout`` covers both trajectory execution and the +terminal effect wait. A result submitted after timeout cannot satisfy the new +retry attempt because its old ID is invalid. The runner remembers the pending +boundary even though the session emits its event only once. The durable state is +``tick.pending_effect`` (an ``EffectVerificationRequest``), not the presence of +that one-time event. Adding an action ---------------- diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index ab6a5f760..9b28f7beb 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -56,6 +56,7 @@ from .engine import AtomicActionEngine from .execution import ( EffectVerificationRequest, + EffectVerificationResult, ExecutionEvent, ExecutionEventKind, ExecutionSession, @@ -203,6 +204,7 @@ "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", + "EffectVerificationResult", "EffectVerifier", "ExecutionClock", "ExecutionFeedbackMode", diff --git a/embodichain/lab/sim/atomic_actions/effects.py b/embodichain/lab/sim/atomic_actions/effects.py index 324260297..802175a8f 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -18,13 +18,104 @@ from __future__ import annotations -from dataclasses import dataclass, field +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass from types import MappingProxyType -from typing import Mapping +from typing import TYPE_CHECKING import torch -from .state import HeldObjectState, TaskState, _normalize_held, _normalize_mask +from embodichain.lab.sim.common import BatchEntity + +from .state import ( + CoordinatedHeldObjectState, + HeldObjectState, + TaskState, + _normalize_coordinated_held, + _normalize_held, + _normalize_mask, +) + +if TYPE_CHECKING: + from .core import ObjectSemantics + + +def _effect_snapshot_memo(value: object) -> dict[int, object]: + """Preserve live entities and private runtime caches during effect copies.""" + memo: dict[int, object] = {} + visited: set[int] = set() + + def visit(nested: object) -> None: + nested_id = id(nested) + if nested_id in visited: + return + visited.add(nested_id) + if isinstance(nested, BatchEntity): + memo[nested_id] = nested + return + if is_dataclass(nested) and not isinstance(nested, type): + for data_field in fields(nested): + child = getattr(nested, data_field.name) + if data_field.name == "_generator" and child is not None: + memo[id(child)] = None + elif not data_field.init and child is not None: + memo[id(child)] = child + else: + visit(child) + return + if isinstance(nested, Mapping): + for key, child in nested.items(): + visit(key) + visit(child) + return + if isinstance(nested, (list, tuple, set, frozenset)): + for child in nested: + visit(child) + + visit(value) + return memo + + +def _snapshot_semantics(value: ObjectSemantics) -> ObjectSemantics: + """Copy semantic data while retaining live simulation-entity identity.""" + try: + copied = deepcopy(value, _effect_snapshot_memo(value)) + except Exception as exc: + raise TypeError( + "ObjectSemantics effect metadata must be copyable without cloning " + "live simulation entities." + ) from exc + if type(copied) is not type(value) or copied is value: + raise TypeError( + "ObjectSemantics effect snapshots must produce a distinct value " + "of the same exact type." + ) + return copied + + +def _snapshot_held(value: HeldObjectState) -> HeldObjectState: + """Return an independently owned held-object effect value.""" + return HeldObjectState( + semantics=_snapshot_semantics(value.semantics), + object_to_eef=value.object_to_eef.clone(), + grasp_xpos=value.grasp_xpos.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + + +def _snapshot_coordinated( + value: CoordinatedHeldObjectState, +) -> CoordinatedHeldObjectState: + """Return an independently owned coordinated held-object effect value.""" + return CoordinatedHeldObjectState( + semantics=_snapshot_semantics(value.semantics), + left_object_to_eef=value.left_object_to_eef.clone(), + right_object_to_eef=value.right_object_to_eef.clone(), + left_grasp_xpos=value.left_grasp_xpos.clone(), + right_grasp_xpos=value.right_grasp_xpos.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) def _with_held_mask( @@ -117,6 +208,26 @@ def is_empty(self) -> bool: """Whether this delta declares no symbolic state changes.""" return not self.held_object_updates + def snapshot(self) -> StateDelta: + """Return an independently owned symbolic-effect snapshot. + + Live simulation entities retain identity, while semantic metadata, + affordance data, and every attachment tensor are copied. + + Returns: + Independently owned state delta. + """ + return StateDelta( + held_object_updates={ + resource: None if value is None else _snapshot_held(value) + for resource, value in self.held_object_updates.items() + }, + coordinated_held_object_updates={ + resources: (None if value is None else _snapshot_coordinated(value)) + for resources, value in self.coordinated_held_object_updates.items() + }, + ) + def apply( self, state: TaskState, diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index ab0711c2f..fc8cdda38 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -604,9 +604,8 @@ def start( invocations: Grounded action requests in execution order. context: Initial measured state and scene snapshot. The engine captures one when omitted. - eligible_mask: Optional per-environment cohort allowed to execute. - Ineligible rows remain excluded for the whole session. All rows - are eligible when omitted. + eligible_mask: Optional rows allowed to enter this session. Inactive + rows remain inactive across every invocation in the sequence. Returns: Stateful execution session advanced by ``session.tick(...)``. diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 59a32a5a6..bde4d07b4 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -20,6 +20,7 @@ from dataclasses import dataclass from enum import Enum +import math from typing import TYPE_CHECKING import torch @@ -60,13 +61,18 @@ class ExecutionEventKind(str, Enum): TRACKING_ERROR = "tracking_error" DYNAMIC_GOAL_CHANGED = "dynamic_goal_changed" COLLISION_WORLD_CHANGED = "collision_world_changed" + ACTION_PLANNING_FAILED = "action_planning_failed" ACTION_TIMEOUT = "action_timeout" TRAJECTORY_COMPLETED = "trajectory_completed" EFFECT_VERIFICATION_REQUIRED = "effect_verification_required" + EFFECT_VERIFICATION_FAILED = "effect_verification_failed" + EFFECT_VERIFICATION_TIMEOUT = "effect_verification_timeout" ACTION_RETRY = "action_retry" ACTION_COMPLETED = "action_completed" RECOVERY_EXHAUSTED = "recovery_exhausted" + ROWS_DEACTIVATED = "rows_deactivated" SESSION_COMPLETED = "session_completed" + SESSION_FAILED = "session_failed" @dataclass(frozen=True, slots=True, eq=False) @@ -89,6 +95,8 @@ def __post_init__(self) -> None: raise ValueError("invocation_index must be non-negative.") if self.invocation_revision < 0: raise ValueError("invocation_revision must be non-negative.") + if not isinstance(self.env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") if self.env_mask.dtype != torch.bool or self.env_mask.dim() != 1: raise ValueError("ExecutionEvent.env_mask must be a 1D bool tensor.") object.__setattr__(self, "env_mask", self.env_mask.clone()) @@ -96,17 +104,27 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True, eq=False) class EffectVerificationRequest: - """Typed boundary describing a semantic effect awaiting verification.""" + """Typed boundary describing a semantic effect awaiting verification. + ``requested_at`` and ``deadline`` use the same timestamp domain as + :class:`RobotObservation`. Request-mask shrinkage retains both values; + only a whole-action retry starts a new attempt deadline. + """ + + verification_id: int skill_id: str invocation_id: str | None invocation_revision: int invocation_index: int terminal_segment: str | None + requested_at: float + deadline: float env_mask: torch.Tensor expected_effects: StateDelta def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") if not isinstance(self.skill_id, str) or not self.skill_id: raise ValueError("skill_id must be a non-empty string.") if self.invocation_id is not None and ( @@ -121,13 +139,71 @@ def __post_init__(self) -> None: not isinstance(self.terminal_segment, str) or not self.terminal_segment ): raise ValueError("terminal_segment must be a non-empty string or None.") + if not math.isfinite(self.requested_at) or self.requested_at < 0.0: + raise ValueError("requested_at must be finite and non-negative.") + if not math.isfinite(self.deadline) or self.deadline < self.requested_at: + raise ValueError( + "deadline must be finite and no earlier than requested_at." + ) + if not isinstance(self.env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") if self.env_mask.dtype != torch.bool or self.env_mask.dim() != 1: raise ValueError("env_mask must be a 1D bool tensor.") + if not self.env_mask.any(): + raise ValueError("env_mask must contain at least one requested row.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") if self.expected_effects.is_empty: raise ValueError("Effect verification requires a non-empty StateDelta.") object.__setattr__(self, "env_mask", self.env_mask.clone()) + object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + + def snapshot(self) -> EffectVerificationRequest: + """Return a request snapshot with an independently owned row mask.""" + return EffectVerificationRequest( + verification_id=self.verification_id, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + invocation_index=self.invocation_index, + terminal_segment=self.terminal_segment, + requested_at=self.requested_at, + deadline=self.deadline, + env_mask=self.env_mask, + expected_effects=self.expected_effects, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectVerificationResult: + """Correlated per-environment update for one effect boundary. + + Rows absent from both masks remain unresolved. This lets one shared batch + barrier commit verified rows while other rows continue observing the same + physical effect. + """ + + verification_id: int + success_mask: torch.Tensor + failure_mask: torch.Tensor + + def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") + for name in ("success_mask", "failure_mask"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if self.success_mask.shape != self.failure_mask.shape: + raise ValueError("success_mask and failure_mask must have equal shapes.") + if self.success_mask.device != self.failure_mask.device: + raise ValueError("success_mask and failure_mask must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("success_mask and failure_mask must not overlap.") + object.__setattr__(self, "success_mask", self.success_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) @dataclass(frozen=True, slots=True, eq=False) @@ -162,6 +238,16 @@ def __post_init__(self) -> None: raise TypeError("hold_targets must contain RuntimeEndpointTarget values.") if self.command is not None and self.hold_targets: raise ValueError("A tick cannot send commands and request a hold together.") + if self.pending_effect is not None: + if not isinstance(self.pending_effect, EffectVerificationRequest): + raise TypeError( + "pending_effect must be an EffectVerificationRequest or None." + ) + object.__setattr__( + self, + "pending_effect", + self.pending_effect.snapshot(), + ) hold_targets: list[RuntimeEndpointTarget] = [] for target in self.hold_targets: snapshot = target.snapshot() @@ -182,11 +268,14 @@ class ExecutionSession: The session never steps a simulator itself. Each :meth:`tick` consumes the latest observation and scene snapshot and emits at most one synchronized endpoint-command frame. Expected symbolic effects are committed only after the caller - supplies ``effect_success`` for a non-empty :class:`StateDelta`. + supplies a correlated :class:`EffectVerificationResult` for a non-empty + :class:`StateDelta`. Environment eligibility and recovery budgets are tracked per row. The waypoint cursor is batch-synchronized: a recoverable row replans the active cohort from the latest observation and restarts the action trajectory. + Calls that mutate the session must be serialized by its owner; the session + does not provide thread synchronization. """ def __init__( @@ -231,12 +320,23 @@ def __init__( ) self._replans = torch.zeros_like(self._action_retries) self._pending_effect: EffectVerificationRequest | None = None + self._effect_failures = torch.zeros_like(self._eligible) + self._effect_requested_at: float | None = None + self._next_effect_verification_id = 0 self._status = ( ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED ) self._queued_events: list[ExecutionEvent] = [] if self._status is ExecutionStatus.RUNNING: self._plan_current(context, ExecutionEventKind.ACTION_PLANNED) + else: + self._queued_events.append( + self._event( + ExecutionEventKind.SESSION_FAILED, + self._eligible, + "No environment was initially eligible for execution.", + ) + ) @property def status(self) -> ExecutionStatus: @@ -263,6 +363,68 @@ def effect_verification_pending(self) -> bool: """Whether the current physical effect still requires verification.""" return self._pending_effect is not None + @property + def pending_effect(self) -> EffectVerificationRequest | None: + """Owned snapshot of the current effect boundary, when present.""" + return None if self._pending_effect is None else self._pending_effect.snapshot() + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> torch.Tensor: + """Permanently remove selected rows from this invocation sequence. + + Deactivation is sticky across action barriers and recovery replans. + The next emitted command frame marks those rows inactive so the command + sink can apply target-specific safe hold behavior. + + Args: + env_mask: Rows requested for deactivation. + reason: Human-readable event message. + + Returns: + Owned mask of rows that changed from eligible to inactive. + + Raises: + RuntimeError: If the session is already terminal. + ValueError: If ``reason`` is empty or the mask shape is invalid. + """ + if self._status is not ExecutionStatus.RUNNING: + raise RuntimeError("Only a running execution session can deactivate rows.") + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + requested = self._normalize_mask(env_mask, "env_mask") + changed = requested & self._eligible + if not changed.any(): + return changed + self._eligible &= ~changed + self._pending &= ~changed + self._effect_failures &= ~changed + self._last_command_mask &= ~changed + self._queued_events.append( + self._event(ExecutionEventKind.ROWS_DEACTIVATED, changed, reason) + ) + if self._pending_effect is not None: + assert self._plan is not None + previous_effect = self._pending_effect + remaining_effect = ( + previous_effect.env_mask & self._pending & self._plan.plan_success + ) + if torch.equal(remaining_effect, previous_effect.env_mask): + self._pending_effect = previous_effect + elif remaining_effect.any(): + self._pending_effect = self._effect_verification_request( + remaining_effect + ) + else: + self._pending_effect = None + terminal_event = self._update_terminal_status() + if terminal_event is not None: + self._queued_events.append(terminal_event) + return changed.clone() + def revise_current( self, invocation: ActionInvocation, @@ -311,10 +473,10 @@ def _prepare_revision( raise TypeError("invocation must be an ActionInvocation.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - if self._pending_effect is not None: + if self._pending_effect is not None or self._effect_failures.any(): raise RuntimeError( - "Cannot revise while a physical effect is awaiting verification; " - "verify it or cancel and start a new invocation." + "Cannot revise while physical-effect resolution is pending; " + "resolve it or cancel and start a new invocation." ) self._validate_revision_identity( skill_id=invocation.skill_id, @@ -333,10 +495,10 @@ def _install_prepared_revision( raise TypeError("replacement must be a ResolvedActionRequest.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - if self._pending_effect is not None: + if self._pending_effect is not None or self._effect_failures.any(): raise RuntimeError( - "Cannot revise while a physical effect is awaiting verification; " - "verify it or cancel and start a new invocation." + "Cannot revise while physical-effect resolution is pending; " + "resolve it or cancel and start a new invocation." ) self._validate_revision_identity( skill_id=replacement.skill_id, @@ -417,32 +579,41 @@ def tick( self, context: PlanningContext, *, - effect_success: torch.Tensor | None = None, + effect_result: EffectVerificationResult | None = None, ) -> ExecutionTick: """Advance execution by one observation/command cycle. Args: context: Latest measured robot and versioned scene state. Its task state is replaced by the session's verified task state. - effect_success: Optional per-environment semantic-effect verification - for an action waiting at its terminal waypoint. + effect_result: Optional correlated semantic-effect result for an + action waiting at its terminal waypoint. Returns: Status, optional command, events, and current verified task state. """ self._context = self._validated_context(context) events = self._drain_events() + if effect_result is not None: + if type(effect_result) is not EffectVerificationResult: + raise TypeError( + "effect_result must be exactly EffectVerificationResult or None." + ) + if self._pending_effect is None: + raise ValueError("No semantic effect is awaiting verification.") + if effect_result.verification_id != self._pending_effect.verification_id: + raise ValueError( + "effect_result verification_id does not match the pending " + "effect boundary." + ) if self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) assert self._plan is not None - if self._pending_effect is not None: - execution_mask = ( - self._pending_effect.env_mask & self._pending & self._plan.plan_success - ) + if not self._pending.any(): command, hold_targets, completion_events = self._finish_action( - execution_mask, - effect_success, + self._pending, + None, ) events.extend(completion_events) return self._tick_result( @@ -451,6 +622,104 @@ def tick( events=events, ) + if self._pending_effect is not None: + execution_mask = ( + self._pending_effect.env_mask & self._pending & self._plan.plan_success + ) + if self._action_timed_out(self._plan, execution_mask): + timed_out = execution_mask.clone() + known_failures = self._effect_failures.clone() + planning_failed = self._pending & ~self._plan.plan_success + retry_mask = timed_out | known_failures | planning_failed + self._pending_effect = None + self._effect_failures.zero_() + if known_failures.any(): + events.append( + self._event( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + known_failures, + "Expected semantic effects were not observed.", + ) + ) + if planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT, + "Effect verification exceeded the action attempt timeout.", + reason_mask=timed_out, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + assert self._plan is not None + effect_result = None + else: + command, hold_targets, completion_events = self._finish_action( + execution_mask, + effect_result, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + + if self._effect_failures.any(): + failed_effect = self._effect_failures.clone() + planning_failed = self._pending & ~self._plan.plan_success + retry_mask = failed_effect | planning_failed + self._effect_failures.zero_() + if planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + "Expected semantic effects were not observed.", + reason_mask=failed_effect, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + assert self._plan is not None + plan = self._plan execution_mask = self._pending & plan.plan_success recovery_events = self._recover_if_needed(plan, execution_mask) @@ -468,6 +737,17 @@ def tick( assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) commands = plan.commands if self._waypoint_index < commands.frame_count: @@ -492,6 +772,17 @@ def tick( assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) if plan.commands.frame_count > 0: command = self._command_at(plan, 0, execution_mask) self._waypoint_index = 1 @@ -505,7 +796,7 @@ def tick( ) command, hold_targets, completion_events = self._finish_action( execution_mask, - effect_success, + effect_result, ) events.extend(completion_events) return self._tick_result( @@ -524,7 +815,7 @@ def tick( command, hold_targets, completion_events = self._finish_action( execution_mask, - effect_success, + effect_result, ) events.extend(completion_events) return self._tick_result( @@ -607,6 +898,8 @@ def _install_plan( self._last_joint_ids = () self._last_command_mask.zero_() self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None planned_mask = self._pending & plan.plan_success self._queued_events.append( self._event(event_kind, planned_mask, "Planned from the latest context.") @@ -682,10 +975,7 @@ def _recover_if_needed( events: list[ExecutionEvent] = [] if not execution_mask.any(): return events - if ( - self._context.robot.timestamp - self._action_started_at - > plan.recovery_policy.action_timeout - ): + if self._action_timed_out(plan, execution_mask): return self._attempt_action_retry( execution_mask, ExecutionEventKind.ACTION_TIMEOUT, @@ -731,6 +1021,18 @@ def _recover_if_needed( ) return events + def _action_timed_out( + self, + plan: ActionPlan, + execution_mask: torch.Tensor, + ) -> bool: + """Return whether an active action attempt exceeded its deadline.""" + return bool( + execution_mask.any() + and self._context.robot.timestamp - self._action_started_at + > plan.recovery_policy.action_timeout + ) + def _attempt_replan( self, trigger_mask: torch.Tensor, @@ -761,7 +1063,9 @@ def _attempt_replan( self._replans[allowed] += 1 self._plan_current(self._context, ExecutionEventKind.REPLANNED) events.extend(self._drain_events()) - self._update_terminal_status() + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) return events def _attempt_action_retry( @@ -769,11 +1073,16 @@ def _attempt_action_retry( trigger_mask: torch.Tensor, reason: ExecutionEventKind, message: str, + *, + reason_mask: torch.Tensor | None = None, ) -> list[ExecutionEvent]: """Retry the current action or permanently fail exhausted rows.""" assert self._plan is not None policy = self._plan.recovery_policy - events = [self._event(reason, trigger_mask, message)] + cause_mask = trigger_mask if reason_mask is None else reason_mask + events = [self._event(reason, cause_mask, message)] + self._pending_effect = None + self._effect_failures &= ~trigger_mask allowed = trigger_mask & (self._action_retries < policy.max_action_retries) exhausted = trigger_mask & ~allowed if exhausted.any(): @@ -798,13 +1107,15 @@ def _attempt_action_retry( ) self._plan_current(self._context, ExecutionEventKind.REPLANNED) events.extend(self._drain_events()) - self._update_terminal_status() + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) return events def _finish_action( self, execution_mask: torch.Tensor, - effect_success: torch.Tensor | None, + effect_result: EffectVerificationResult | None, ) -> tuple[ RuntimeCommandFrame | None, tuple[RuntimeEndpointTarget, ...], @@ -820,22 +1131,39 @@ def _finish_action( ) orphaned_targets = bool(active_targets) and not plan_targets events: list[ExecutionEvent] = [] + if not self._pending.any(): + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + return None, hold_targets, barrier_events + planning_failed = self._pending & ~self._plan.plan_success if not execution_mask.any() and planning_failed.any(): events.extend( self._attempt_action_retry( planning_failed, - ExecutionEventKind.ACTION_RETRY, + ExecutionEventKind.ACTION_PLANNING_FAILED, "Planning failed for every pending environment.", ) ) if self._status is not ExecutionStatus.RUNNING: return None, active_targets, events + if not self._pending.any(): + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + events.extend(barrier_events) + return None, hold_targets, events return None, active_targets, events + failed_effect = torch.zeros_like(execution_mask) + unresolved = torch.zeros_like(execution_mask) + made_progress = False if self._plan.expected_effects.is_empty: verified = execution_mask - elif effect_success is None: + elif effect_result is None: if self._pending_effect is None: self._pending_effect = self._effect_verification_request(execution_mask) events.append( @@ -847,9 +1175,27 @@ def _finish_action( ) return None, active_targets, events else: - verified_input = self._normalize_mask(effect_success, "effect_success") - verified = execution_mask & verified_input - self._pending_effect = None + success_input = self._normalize_mask( + effect_result.success_mask, + "effect_result.success_mask", + ) + failure_input = self._normalize_mask( + effect_result.failure_mask, + "effect_result.failure_mask", + ) + reported = success_input | failure_input + if (reported & ~execution_mask).any(): + raise ValueError( + "Effect verification masks must be subsets of the pending " + "effect request env_mask." + ) + verified = execution_mask & success_input + failed_effect = execution_mask & failure_input + unresolved = execution_mask & ~reported + made_progress = bool(reported.any().item()) + self._effect_failures |= failed_effect + if not unresolved.any(): + self._pending_effect = None if verified.any(): self._task_state = self._plan.expected_effects.apply( @@ -863,22 +1209,67 @@ def _finish_action( control_dt=self._context.control_dt, ) self._pending &= ~verified - failed_effect = execution_mask & ~verified - retry_mask = failed_effect | planning_failed + if unresolved.any(): + if made_progress: + self._pending_effect = self._effect_verification_request(unresolved) + return None, active_targets, events + retry_mask = self._effect_failures | planning_failed if retry_mask.any(): + effect_failure_mask = self._effect_failures.clone() + self._effect_failures.zero_() + reason = ( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED + if effect_failure_mask.any() + else ExecutionEventKind.ACTION_PLANNING_FAILED + ) + reason_mask = ( + effect_failure_mask if effect_failure_mask.any() else retry_mask + ) + if effect_failure_mask.any() and planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) events.extend( self._attempt_action_retry( retry_mask, - ExecutionEventKind.ACTION_RETRY, + reason, "Planning or expected-effect verification failed.", + reason_mask=reason_mask, ) ) if self._status is not ExecutionStatus.RUNNING: return None, active_targets, events - return None, active_targets, events + if self._pending.any(): + return None, active_targets, events if self._pending.any(): return None, active_targets, events + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + events.extend(barrier_events) + return None, hold_targets, events + + def _advance_action_barrier( + self, + active_targets: tuple[RuntimeEndpointTarget, ...], + *, + orphaned_targets: bool, + ) -> tuple[tuple[RuntimeEndpointTarget, ...], list[ExecutionEvent]]: + """Complete an empty action cohort and install the next invocation.""" + if self._status is not ExecutionStatus.RUNNING or self._plan is None: + raise RuntimeError("Only a running planned action can cross its barrier.") + if self._pending.any(): + raise RuntimeError("The action barrier cannot advance with pending rows.") + self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None + events: list[ExecutionEvent] = [] events.append( self._event( ExecutionEventKind.ACTION_COMPLETED, @@ -893,22 +1284,28 @@ def _finish_action( if self._eligible.any() else ExecutionStatus.FAILED ) + terminal_kind = ( + ExecutionEventKind.SESSION_COMPLETED + if self._status is ExecutionStatus.COMPLETED + else ExecutionEventKind.SESSION_FAILED + ) events.append( self._event( - ExecutionEventKind.SESSION_COMPLETED, + terminal_kind, self._eligible, "Invocation sequence completed.", ) ) - return None, (active_targets if orphaned_targets else ()), events + return (active_targets if orphaned_targets else ()), events self._pending = self._eligible.clone() self._pending_effect = None + self._effect_failures.zero_() self._action_retries.zero_() self._replans.zero_() self._plan_current(self._context, ExecutionEventKind.ACTION_PLANNED) events.extend(self._drain_events()) - return None, active_targets, events + return active_targets, events def _command_at( self, @@ -1071,7 +1468,12 @@ def _effect_verification_request( """Describe the current action's pending semantic-effect boundary.""" assert self._plan is not None request = self._requests[self._invocation_index] + verification_id = self._next_effect_verification_id + self._next_effect_verification_id += 1 + if self._effect_requested_at is None: + self._effect_requested_at = self._context.robot.timestamp return EffectVerificationRequest( + verification_id=verification_id, skill_id=request.skill_id, invocation_id=request.invocation_id, invocation_revision=request.revision, @@ -1079,6 +1481,10 @@ def _effect_verification_request( terminal_segment=( self._plan.segments[-1].name if self._plan.segments else None ), + requested_at=self._effect_requested_at, + deadline=( + self._action_started_at + self._plan.recovery_policy.action_timeout + ), env_mask=env_mask, expected_effects=self._plan.expected_effects, ) @@ -1122,10 +1528,19 @@ def _drain_events(self) -> list[ExecutionEvent]: self._queued_events = [] return events - def _update_terminal_status(self) -> None: - """Mark the session failed when no environment can continue.""" - if not self._eligible.any(): + def _update_terminal_status(self) -> ExecutionEvent | None: + """Mark and report failure when no environment can continue.""" + if not self._eligible.any() and self._status is ExecutionStatus.RUNNING: self._status = ExecutionStatus.FAILED + self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None + return self._event( + ExecutionEventKind.SESSION_FAILED, + self._eligible, + "No environment remains eligible for execution.", + ) + return None def _tick_result( self, @@ -1148,6 +1563,7 @@ def _tick_result( __all__ = [ "EffectVerificationRequest", + "EffectVerificationResult", "ExecutionEvent", "ExecutionEventKind", "ExecutionSession", diff --git a/embodichain/lab/sim/atomic_actions/policies.py b/embodichain/lab/sim/atomic_actions/policies.py index bf75cf01c..66757ce50 100644 --- a/embodichain/lab/sim/atomic_actions/policies.py +++ b/embodichain/lab/sim/atomic_actions/policies.py @@ -151,7 +151,7 @@ class RecoveryPolicy: """Dynamic-goal rotation threshold in radians (five degrees by default).""" action_timeout: float = 30.0 - """Maximum execution time for one action attempt in seconds.""" + """Maximum time for one action attempt, including terminal effect verification.""" def __post_init__(self) -> None: if self.max_replans < 0: diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 063bd1fa5..65043e374 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -19,7 +19,7 @@ from __future__ import annotations from collections.abc import Callable -from dataclasses import dataclass +from dataclasses import dataclass, replace from enum import Enum import math import time @@ -31,6 +31,7 @@ from .bindings import RuntimeEndpointTarget from .execution import ( + EffectVerificationResult, ExecutionSession, ExecutionStatus, ExecutionTick, @@ -291,7 +292,10 @@ def is_waiting(self) -> bool: ) -EffectVerifier = Callable[[PlanningContext, ExecutionTick], torch.Tensor | None] +EffectVerifier = Callable[ + [PlanningContext, ExecutionTick], + EffectVerificationResult | None, +] """Callback that verifies a pending semantic effect for each environment.""" RunnerStepCallback = Callable[[RunnerStep], None] @@ -307,6 +311,8 @@ class ExecutionRunner: :meth:`run_until_blocked` supplies the blocking loop for tutorials and simple applications. Controller rejection, timeout, observation failure, and session exceptions all trigger a best-effort cancel-then-hold sequence. + Runner methods are designed for serialized event-loop use and are not + thread-safe. Args: session: Stateful atomic-action execution session. @@ -354,8 +360,9 @@ def __init__( def session(self) -> ExecutionSession: """Execution session advanced by this runner. - Call :meth:`revise_current` on the runner, rather than mutating the - session directly, while this runner owns scheduling. + Call :meth:`revise_current` or :meth:`deactivate_rows` on the runner, + rather than mutating the session directly, while this runner owns + scheduling. """ return self._session @@ -410,16 +417,59 @@ def revise_current(self, invocation: ActionInvocation) -> None: ) self._pending_revision = prepared + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> torch.Tensor: + """Permanently deactivate environment rows owned by this runner. + + The runner refreshes its cached effect boundary so a verifier cannot + submit a result correlated with a request that deactivation replaced. + In-flight controller work is neutralized for those rows by the next + due command frame according to the :class:`CommandSink` contract. + + Args: + env_mask: Rows requested for deactivation. + reason: Human-readable event message. + + Returns: + Owned mask of rows that changed from eligible to inactive. + + Raises: + RuntimeError: If the runner is already terminal. + TypeError: If ``env_mask`` is not a tensor. + ValueError: If the mask or reason is invalid. + """ + if self._status is not RunnerStatus.RUNNING: + raise RuntimeError("Only a running execution runner can deactivate rows.") + changed = self._session.deactivate_rows(env_mask, reason=reason) + if self._session.status is not ExecutionStatus.RUNNING: + self._pending_revision = None + pending_effect = self._session.pending_effect + if pending_effect is None: + self._clear_effect_boundary() + elif self._effect_tick is not None: + self._effect_tick = replace( + self._effect_tick, + status=self._session.status, + eligible_mask=self._session.eligible_mask, + task_state=self._session.task_state, + pending_effect=pending_effect, + ) + return changed + def step( self, *, - effect_success: torch.Tensor | None = None, + effect_result: EffectVerificationResult | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. Args: - effect_success: Optional per-environment verification mask. If this - call occurs before the next cycle is due, it is not consumed and + effect_result: Optional correlated effect result. If this call + occurs before the next cycle is due, it is not consumed and must be supplied again on a later call. Returns: @@ -456,7 +506,9 @@ def step( context, ) self._pending_revision = None - tick = self._session.tick(context, effect_success=effect_success) + tick = self._session.tick(context, effect_result=effect_result) + context = self._session.latest_context + self._last_context = context except Exception as exc: return self._fail( f"Execution session failed: {type(exc).__name__}: {exc}", @@ -520,6 +572,8 @@ def step( dispatches=dispatches, ) self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time + elif tick.pending_effect is not None: + self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time else: self._next_step_at = self._clock_now() @@ -549,7 +603,7 @@ def step( self._next_step_at = self._clock_now() elif tick.status is ExecutionStatus.FAILED: return self._fail( - "Execution session exhausted its recovery budget.", + "Execution session failed; inspect its terminal events for the cause.", context=context, tick=tick, dispatches=dispatches, @@ -616,7 +670,7 @@ def run_until_blocked( """ if max_steps <= 0: raise ValueError("max_steps must be greater than zero.") - effect_success: torch.Tensor | None = None + effect_result: EffectVerificationResult | None = None now = self._clock_now() last_result = self._result( timestamp=now, @@ -624,30 +678,12 @@ def run_until_blocked( context=self._effect_context, tick=self._effect_tick, ) - if self.effect_verification_pending: - if ( - effect_verifier is None - or self._effect_context is None - or self._effect_tick is None - ): - return last_result - try: - effect_success = effect_verifier( - self._effect_context, - self._effect_tick, - ) - except Exception as exc: - return self._fail( - f"Effect verifier failed: {type(exc).__name__}: {exc}", - context=self._effect_context, - tick=self._effect_tick, - ) - if effect_success is None: - return last_result + if self.effect_verification_pending and effect_verifier is None: + return last_result for _ in range(max_steps): - result = self.step(effect_success=effect_success) + result = self.step(effect_result=effect_result) if result.tick is not None: - effect_success = None + effect_result = None if on_step is not None: try: on_step(result) @@ -668,7 +704,7 @@ def run_until_blocked( if effect_verifier is None or result.context is None: return result try: - effect_success = effect_verifier(result.context, result.tick) + effect_result = effect_verifier(result.context, result.tick) except Exception as exc: return self._fail( f"Effect verifier failed: {type(exc).__name__}: {exc}", @@ -676,7 +712,7 @@ def run_until_blocked( tick=result.tick, dispatches=list(result.dispatches), ) - if effect_success is None: + if effect_result is None: return result if result.wait_duration > 0.0: try: diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index e9b8b5c5f..82a572dcb 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -40,7 +40,10 @@ EndpointCommand, EntityState, ExecutionEventKind, + ExecutionSession, ExecutionStatus, + ExecutionTick, + EffectVerificationResult, GraspGoal, HeldObjectState, JointPositionPayload, @@ -183,6 +186,23 @@ def _plan( return replace(plan, plan_success=torch.zeros_like(plan.plan_success)) +class MixedEffectAction(EffectAction): + """Effect action whose final environment row always fails planning.""" + + skill_id: ClassVar[str] = "mixed_effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + plan_success = torch.ones_like(plan.plan_success) + plan_success[-1] = False + return replace(plan, plan_success=plan_success) + + class NonuniformTimingAction(DynamicAction): """Test action with explicit nonuniform waypoint arrival intervals.""" @@ -479,6 +499,44 @@ def _destination_invocation( ) +def _effect_session( + *, + batch_size: int = 1, + max_action_retries: int = 2, + action_timeout: float = 30.0, + eligible_mask: torch.Tensor | None = None, + action: EffectAction | None = None, +) -> tuple[ExecutionSession, ExecutionTick]: + """Advance a test effect action to its verification boundary.""" + engine, _ = _engine(batch_size=batch_size) + selected_action = EffectAction() if action is None else action + engine.register(selected_action) + base = _invocation( + engine, + max_action_retries=max_action_retries, + action_timeout=action_timeout, + ) + invocation = ActionInvocation( + skill_id=selected_action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + qpos = tuple(0.0 for _ in range(batch_size)) + target = tuple(0.2 for _ in range(batch_size)) + session = engine.start( + (invocation,), + _context(0.0, qpos, target, 0), + eligible_mask=eligible_mask, + ) + session.tick(_context(0.0, qpos, target, 0)) + session.tick(_context(0.1, qpos, target, 0)) + waiting = session.tick(_context(0.2, target, target, 0)) + assert waiting.pending_effect is not None + return session, waiting + + def _joint_positions(command: RuntimeCommandFrame | None) -> torch.Tensor: """Return the only joint-position payload emitted by the test action.""" assert command is not None @@ -503,63 +561,110 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] -def test_initial_eligibility_is_owned_and_masks_commands() -> None: +def test_initial_eligibility_is_sticky_across_invocation_barriers() -> None: engine, _ = _engine(batch_size=2) - initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) - eligible_mask = torch.tensor([True, False]) - + invocation = _invocation(engine) + supplied_mask = torch.tensor([True, False]) session = engine.start( - (_invocation(engine),), - initial, - eligible_mask=eligible_mask, + (invocation, invocation), + _context(0.0, (0.0, 0.0), (0.2, 0.2), 0), + eligible_mask=supplied_mask, ) - eligible_mask.fill_(False) - tick = session.tick(initial) + supplied_mask.fill_(True) - assert session.eligible_mask.tolist() == [True, False] - assert tick.command is not None - assert tick.command.active_mask.tolist() == [True, False] + first = session.tick(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + barrier = session.tick(_context(0.2, (0.2, 7.0), (0.2, 0.2), 0)) + second_action = session.tick(_context(0.3, (0.2, 7.0), (0.2, 0.2), 0)) + + assert first.command is not None + assert first.command.active_mask.tolist() == [True, False] + assert barrier.status is ExecutionStatus.RUNNING + assert second_action.command is not None + assert second_action.command.active_mask.tolist() == [True, False] + assert second_action.eligible_mask.tolist() == [True, False] def test_empty_initial_eligibility_fails_without_planning() -> None: engine, action = _engine(batch_size=2) - initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) session = engine.start( (_invocation(engine),), initial, eligible_mask=torch.tensor([False, False]), ) - tick = session.tick(initial) + terminal = session.tick(initial) assert action.plan_count == 0 - assert tick.status is ExecutionStatus.FAILED - assert tick.command is None - assert tick.eligible_mask.tolist() == [False, False] + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False] + assert any( + event.kind is ExecutionEventKind.SESSION_FAILED for event in terminal.events + ) -@pytest.mark.parametrize( - ("eligible_mask", "exception", "message"), - [ - ([True], TypeError, "torch.Tensor"), - (torch.tensor([1, 0]), ValueError, "bool with shape"), - (torch.tensor([True]), ValueError, "bool with shape"), - ], -) -def test_initial_eligibility_is_validated( - eligible_mask: object, - exception: type[Exception], - message: str, -) -> None: +def test_initial_eligibility_is_owned_and_validated() -> None: engine, _ = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) - with pytest.raises(exception, match=message): + with pytest.raises(TypeError, match="eligible_mask must be a torch.Tensor"): + engine.start((_invocation(engine),), initial, eligible_mask=[True, False]) + with pytest.raises(ValueError, match="bool with shape"): engine.start( (_invocation(engine),), - _context(0.0, (0.0, 0.0), (0.2, 0.4), 0), - eligible_mask=eligible_mask, # type: ignore[arg-type] + initial, + eligible_mask=torch.tensor([1, 0]), + ) + with pytest.raises(ValueError, match="bool with shape"): + engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([True]), ) + supplied = torch.tensor([True, False]) + session = engine.start( + (_invocation(engine),), + initial, + eligible_mask=supplied, + ) + supplied.fill_(False) + observed = session.eligible_mask + observed.fill_(False) + + assert session.eligible_mask.tolist() == [True, False] + + +def test_deactivate_rows_is_sticky_and_masks_the_next_command() -> None: + engine, _ = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + session = engine.start((_invocation(engine),), initial) + session.tick(initial) + + changed = session.deactivate_rows( + torch.tensor([False, True]), + reason="environment terminated", + ) + unchanged = session.deactivate_rows( + torch.tensor([False, True]), + reason="duplicate termination", + ) + tick = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + + assert changed.tolist() == [False, True] + assert unchanged.tolist() == [False, False] + assert tick.command is not None + assert tick.command.active_mask.tolist() == [True, False] + assert tick.eligible_mask.tolist() == [True, False] + deactivated = [ + event + for event in tick.events + if event.kind is ExecutionEventKind.ROWS_DEACTIVATED + ] + assert len(deactivated) == 1 + assert deactivated[0].env_mask.tolist() == [False, True] + assert deactivated[0].message == "environment terminated" def test_session_commands_schedule_arrivals_and_final_settling() -> None: engine, _ = _engine() @@ -1262,7 +1367,11 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None still_waiting = session.tick(_context(0.25, 0.2, 0.2, 0)) completed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_success=torch.tensor([True]), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), ) assert waiting.status is ExecutionStatus.RUNNING @@ -1290,6 +1399,347 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None assert completed.task_state.get_held_object("arm") is not None +def test_initially_ineligible_rows_never_receive_effects() -> None: + session, waiting = _effect_session( + batch_size=2, + eligible_mask=torch.tensor([True, False]), + ) + request = waiting.pending_effect + assert request is not None + assert request.env_mask.tolist() == [True, False] + + completed = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = completed.task_state.get_held_object("arm") + assert completed.status is ExecutionStatus.COMPLETED + assert completed.eligible_mask.tolist() == [True, False] + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + + +def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> None: + session, waiting = _effect_session(batch_size=2) + first_request = waiting.pending_effect + assert first_request is not None + + no_progress = session.tick( + _context(0.205, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([False, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert no_progress.pending_effect is not None + assert no_progress.pending_effect.verification_id == first_request.verification_id + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = partial.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True] + assert partial.pending_effect.verification_id != first_request.verification_id + assert partial.pending_effect.requested_at == first_request.requested_at + assert partial.pending_effect.deadline == first_request.deadline + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in partial.events + ) + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + current_request = partial.pending_effect + completed = session.tick( + _context(0.23, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + current_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + completed_held = completed.task_state.get_held_object("arm") + assert completed.status is ExecutionStatus.COMPLETED + assert completed_held is not None and completed_held.env_mask is not None + assert completed_held.env_mask.tolist() == [True, True] + + +def test_effect_result_masks_are_owned_disjoint_and_request_scoped() -> None: + success = torch.tensor([True, False]) + failure = torch.tensor([False, True]) + result = EffectVerificationResult(0, success, failure) + success.fill_(False) + failure.fill_(False) + assert result.success_mask.tolist() == [True, False] + assert result.failure_mask.tolist() == [False, True] + + with pytest.raises(ValueError, match="must not overlap"): + EffectVerificationResult( + 0, + torch.tensor([True, False]), + torch.tensor([True, False]), + ) + + session, waiting = _effect_session(batch_size=2) + request = waiting.pending_effect + assert request is not None + request.env_mask.fill_(False) + published_effect = request.expected_effects.held_object_updates["arm"] + assert published_effect is not None + published_effect.object_to_eef.fill_(9.0) + published_effect.grasp_xpos.fill_(8.0) + published_effect.semantics.affordance.set_custom_config("mutated", True) + preserved = session.pending_effect + assert preserved is not None + assert preserved.env_mask.tolist() == [True, True] + preserved_effect = preserved.expected_effects.held_object_updates["arm"] + assert preserved_effect is not None + assert torch.equal(preserved_effect.object_to_eef, torch.eye(4)) + assert torch.equal(preserved_effect.grasp_xpos, torch.eye(4)) + assert preserved_effect.semantics.affordance.get_custom_config("mutated") is None + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + preserved.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + current = partial.pending_effect + assert current is not None + held = partial.task_state.get_held_object("arm") + assert held is not None + assert torch.equal(held.object_to_eef[0], torch.eye(4)) + + with pytest.raises(ValueError, match="subsets"): + session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + current.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + +def test_state_delta_snapshot_owns_effect_data_and_preserves_live_entity() -> None: + entity = UncopyableEntity() + semantics = ObjectSemantics( + affordance=Affordance(custom_config={"threshold": [1.0]}), + geometry={"size": torch.ones(3)}, + properties={"mass": torch.tensor(1.0)}, + label="snapshot-object", + entity=entity, + ) + held = HeldObjectState( + semantics=semantics, + object_to_eef=torch.eye(4), + grasp_xpos=torch.eye(4), + ) + delta = StateDelta(held_object_updates={"arm": held}) + + snapshot = delta.snapshot() + copied = snapshot.held_object_updates["arm"] + assert copied is not None + assert copied is not held + assert copied.semantics is not semantics + assert copied.semantics.entity is entity + assert copied.semantics.affordance is not semantics.affordance + assert copied.object_to_eef.data_ptr() != held.object_to_eef.data_ptr() + assert copied.grasp_xpos.data_ptr() != held.grasp_xpos.data_ptr() + + copied.object_to_eef.fill_(7.0) + copied.semantics.affordance.custom_config["threshold"].append(2.0) + copied.semantics.geometry["size"].zero_() + assert torch.equal(held.object_to_eef, torch.eye(4)) + assert semantics.affordance.custom_config["threshold"] == [1.0] + assert torch.equal(semantics.geometry["size"], torch.ones(3)) + + +def test_partial_effect_failure_waits_for_unresolved_rows_then_retries_failure() -> ( + None +): + session, waiting = _effect_session(batch_size=2, max_action_retries=1) + request = waiting.pending_effect + assert request is not None + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([False, False]), + failure_mask=torch.tensor([True, False]), + ), + ) + + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True] + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in partial.events + ) + + unresolved_request = partial.pending_effect + resolved = session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + unresolved_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = resolved.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [False, True] + failed_event = next( + event + for event in resolved.events + if event.kind is ExecutionEventKind.EFFECT_VERIFICATION_FAILED + ) + retry_event = next( + event + for event in resolved.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + assert failed_event.env_mask.tolist() == [True, False] + assert retry_event.env_mask.tolist() == [True, False] + + retry_command = session.tick(_context(0.23, (0.2, 0.2), (0.2, 0.2), 0)) + assert retry_command.command is not None + assert retry_command.command.active_mask.tolist() == [True, False] + + +def test_effect_failure_exhaustion_advances_completed_rows_without_empty_request() -> ( + None +): + session, waiting = _effect_session(batch_size=2, max_action_retries=0) + request = waiting.pending_effect + assert request is not None + + terminal = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, True]), + ), + ) + + held = terminal.task_state.get_held_object("arm") + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert terminal.command is None + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert any( + event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + and event.env_mask.tolist() == [False, True] + for event in terminal.events + ) + assert any( + event.kind is ExecutionEventKind.SESSION_COMPLETED for event in terminal.events + ) + + +def test_deactivating_last_unresolved_effect_row_advances_barrier() -> None: + session, waiting = _effect_session(batch_size=2) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert partial.pending_effect is not None + + session.deactivate_rows( + torch.tensor([False, True]), + reason="effect observation terminated", + ) + terminal = session.tick(_context(0.22, (0.2, 0.2), (0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert not any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in terminal.events + ) + + +def test_deactivating_all_effect_rows_is_terminal_and_clears_request() -> None: + session, _ = _effect_session(batch_size=2) + + changed = session.deactivate_rows( + torch.tensor([True, True]), + reason="all environments terminated", + ) + terminal = session.tick(_context(0.21, (0.2, 0.2), (0.2, 0.2), 0)) + + assert changed.tolist() == [True, True] + assert terminal.status is ExecutionStatus.FAILED + assert terminal.pending_effect is None + assert any( + event.kind is ExecutionEventKind.SESSION_FAILED for event in terminal.events + ) + assert not any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in terminal.events + ) + + +def test_effect_request_deadline_is_stable_and_accepts_result_at_boundary() -> None: + session, waiting = _effect_session(action_timeout=0.25) + request = waiting.pending_effect + assert request is not None + assert request.requested_at == pytest.approx(0.2) + assert request.deadline == pytest.approx(0.25) + + polled = session.tick(_context(0.24, 0.2, 0.2, 0)) + assert polled.pending_effect is not None + assert polled.pending_effect.verification_id == request.verification_id + assert polled.pending_effect.requested_at == request.requested_at + assert polled.pending_effect.deadline == request.deadline + + completed = session.tick( + _context(0.25, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + assert completed.status is ExecutionStatus.COMPLETED + + def test_session_revision_cannot_abandon_pending_effect_verification() -> None: engine, _ = _engine() engine.register(EffectAction()) @@ -1307,13 +1757,17 @@ def test_session_revision_cannot_abandon_pending_effect_verification() -> None: waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) assert waiting.pending_effect is not None - with pytest.raises(RuntimeError, match="awaiting verification"): + with pytest.raises(RuntimeError, match="physical-effect resolution"): session.revise_current(replace(invocation, revision=1)) assert session.effect_verification_pending is True completed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_success=torch.tensor([True]), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), ) assert completed.status is ExecutionStatus.COMPLETED assert completed.task_state.get_held_object("arm") is not None @@ -1334,9 +1788,15 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: session.tick(_context(0.0, 0.0, 0.2, 0)) session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None failed = session.tick( - _context(0.2, 0.2, 0.2, 0), - effect_success=torch.tensor([False]), + _context(0.3, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([False]), + torch.tensor([True]), + ), ) assert failed.status is ExecutionStatus.FAILED @@ -1346,6 +1806,199 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: ) +def test_pending_effect_timeout_exhausts_without_committing_late_result() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation( + engine, + max_action_retries=0, + action_timeout=0.25, + ) + invocation = ActionInvocation( + skill_id="effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None + + timed_out = session.tick( + _context(0.3, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), + ) + + kinds = {event.kind for event in timed_out.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.RECOVERY_EXHAUSTED in kinds + assert timed_out.status is ExecutionStatus.FAILED + assert timed_out.pending_effect is None + assert timed_out.task_state.get_held_object("arm") is None + + +def test_effect_timeout_exhaustion_advances_rows_already_verified() -> None: + session, waiting = _effect_session( + batch_size=2, + max_action_retries=0, + action_timeout=0.25, + ) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert partial.pending_effect is not None + + terminal = session.tick(_context(0.3, (0.2, 0.2), (0.2, 0.2), 0)) + + held = terminal.task_state.get_held_object("arm") + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + timeout_event = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT + ) + assert timeout_event.env_mask.tolist() == [False, True] + + +def test_effect_timeout_charges_concurrent_planning_failures() -> None: + session, _ = _effect_session( + batch_size=2, + max_action_retries=1, + action_timeout=0.25, + action=MixedEffectAction(), + ) + + first_retry = session.tick(_context(0.3, (0.2, 0.2), (0.2, 0.2), 0)) + retry_event = next( + event + for event in first_retry.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + planning_event = next( + event + for event in first_retry.events + if event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + ) + assert retry_event.env_mask.tolist() == [True, True] + assert planning_event.env_mask.tolist() == [False, True] + + session.tick(_context(0.4, (0.2, 0.2), (0.2, 0.2), 0)) + second_wait = session.tick(_context(0.5, (0.2, 0.2), (0.2, 0.2), 0)) + assert second_wait.pending_effect is not None + terminal = session.tick(_context(0.6, (0.2, 0.2), (0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False] + exhausted = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert exhausted.env_mask.tolist() == [True, True] + + +def test_deferred_effect_failure_charges_concurrent_planning_failures() -> None: + session, waiting = _effect_session( + batch_size=3, + max_action_retries=0, + action=MixedEffectAction(), + ) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2, 0.2), (0.2, 0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([False, False, False]), + failure_mask=torch.tensor([True, False, False]), + ), + ) + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True, False] + + session.deactivate_rows( + torch.tensor([False, True, False]), + reason="unresolved effect row terminated", + ) + terminal = session.tick(_context(0.22, (0.2, 0.2, 0.2), (0.2, 0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False, False] + planning_event = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + ) + exhausted = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert planning_event.env_mask.tolist() == [False, False, True] + assert exhausted.env_mask.tolist() == [True, False, True] + + +def test_effect_retry_invalidates_previous_verification_id() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation( + engine, + max_action_retries=1, + action_timeout=0.25, + ) + invocation = ActionInvocation( + skill_id="effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + first_wait = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert first_wait.pending_effect is not None + old_id = first_wait.pending_effect.verification_id + old_deadline = first_wait.pending_effect.deadline + + retry = session.tick(_context(0.3, 0.2, 0.2, 0)) + assert retry.command is not None + assert any(event.kind is ExecutionEventKind.ACTION_RETRY for event in retry.events) + session.tick(_context(0.4, 0.2, 0.2, 0)) + second_wait = session.tick(_context(0.5, 0.2, 0.2, 0)) + assert second_wait.pending_effect is not None + assert second_wait.pending_effect.verification_id != old_id + assert second_wait.pending_effect.deadline > old_deadline + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.55, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + old_id, + torch.tensor([True]), + torch.tensor([False]), + ), + ) + + def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: engine, _ = _engine() engine.register(FailedEffectAction()) diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index 296623a88..f3476a49b 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -37,9 +37,11 @@ CommandAckStatus, CommandOperation, EndEffectorPoseGoal, + EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, + ExecutionTick, HeldObjectState, JOINT_POSITION_CAPABILITY, JointPositionPayload, @@ -264,6 +266,8 @@ def _make_runner( with_effect: bool = False, batch_size: int = BATCH_SIZE, control_joint_ids: tuple[int, ...] | None = None, + max_action_retries: int = 2, + action_timeout: float = 10.0, ) -> tuple[ ExecutionRunner, FakeClock, @@ -300,8 +304,9 @@ def _make_runner( motion_policy=MotionPolicy(sample_count=3), recovery_policy=RecoveryPolicy( max_replans=2, + max_action_retries=max_action_retries, tracking_error_threshold=0.05, - action_timeout=10.0, + action_timeout=action_timeout, ), ) session = engine.start((invocation,), initial_context) @@ -315,6 +320,28 @@ def _make_runner( return runner, clock, provider, sink, action +def _successful_effect_result( + context: PlanningContext, + tick: ExecutionTick, +) -> EffectVerificationResult: + """Correlate a successful result with the pending effect boundary.""" + pending_effect = tick.pending_effect + assert pending_effect is not None + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + failure_mask=torch.zeros( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + ) + + def test_joint_feedback_ignores_motion_outside_bound_endpoint() -> None: runner, clock, provider, sink, action = _make_runner(control_joint_ids=(0,)) @@ -553,15 +580,12 @@ def test_runner_revision_rejects_pending_effect_verification() -> None: revision=1, ) - with pytest.raises(RuntimeError, match="awaiting verification"): + with pytest.raises(RuntimeError, match="physical-effect resolution"): runner.revise_current(revised) assert runner.effect_verification_pending is True completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, - dtype=torch.bool, - ) + effect_verifier=_successful_effect_result, ) assert completed.status is RunnerStatus.COMPLETED @@ -619,9 +643,7 @@ def test_blocking_runner_verifies_effect_before_committing_task_state() -> None: runner, _, _, _, _ = _make_runner(with_effect=True) completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, dtype=torch.bool - ) + effect_verifier=_successful_effect_result, ) assert completed.status is RunnerStatus.COMPLETED @@ -640,12 +662,182 @@ def test_blocking_runner_resumes_a_stored_effect_verification_boundary() -> None assert runner.effect_verification_pending is True completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, dtype=torch.bool - ) + effect_verifier=_successful_effect_result, ) assert runner.effect_verification_pending is False assert completed.status is RunnerStatus.COMPLETED assert completed.tick is not None assert completed.tick.task_state.get_held_object("arm") is not None + + +def test_resumed_effect_verifier_uses_a_fresh_observation() -> None: + runner, clock, _, _, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.context is not None + blocked_at = blocked.context.robot.timestamp + clock.advance(0.5) + resumed_at = clock.now() + observed_at: list[float] = [] + + def record_fresh_context( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, tick) + + completed = runner.run_until_blocked(effect_verifier=record_fresh_context) + + assert completed.status is RunnerStatus.COMPLETED + assert observed_at and observed_at[0] >= resumed_at + assert observed_at[0] > blocked_at + + +def test_partial_effect_verifier_receives_the_committed_task_state() -> None: + runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) + observations: list[list[bool] | None] = [] + + def verify_in_two_updates( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + pending_effect = tick.pending_effect + assert pending_effect is not None + held = context.task.get_held_object("arm") + observations.append( + None if held is None or held.env_mask is None else held.env_mask.tolist() + ) + if pending_effect.env_mask.tolist() == [True, True]: + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ) + assert pending_effect.env_mask.tolist() == [False, True] + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert torch.equal(context.task.held_objects["arm"].env_mask, held.env_mask) + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ) + + completed = runner.run_until_blocked(effect_verifier=verify_in_two_updates) + + assert completed.status is RunnerStatus.COMPLETED + assert observations == [None, [True, False]] + assert completed.context is not None and completed.tick is not None + assert completed.context.task is completed.tick.task_state + + +def test_runner_effect_timeout_replans_and_invalidates_cached_request() -> None: + runner, clock, _, _, action = _make_runner( + with_effect=True, + max_action_retries=1, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + plan_count = action.plan_count + clock.advance(request.deadline - clock.now() + 0.01) + + retry = runner.step() + + assert retry.status is RunnerStatus.RUNNING + assert retry.tick is not None and retry.tick.command is not None + assert runner.effect_verification_pending is False + assert action.plan_count == plan_count + 1 + kinds = {event.kind for event in retry.tick.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + assert ExecutionEventKind.REPLANNED in kinds + + +def test_runner_effect_timeout_exhaustion_cancels_and_holds() -> None: + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + max_action_retries=0, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + clock.advance(request.deadline - clock.now() + 0.01) + + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert runner.effect_verification_pending is False + assert failed.tick is not None and failed.tick.pending_effect is None + assert failed.tick.task_state.get_held_object("arm") is None + assert [item.operation for item in failed.dispatches[-2:]] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + + +def test_runner_deactivation_refreshes_cached_effect_request() -> None: + runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + old_id = blocked.tick.pending_effect.verification_id + + changed = runner.deactivate_rows( + torch.tensor([False, True]), + reason="environment terminated", + ) + refreshed = runner.run_until_blocked() + + assert changed.tolist() == [False, True] + assert refreshed.tick is not None and refreshed.tick.pending_effect is not None + assert refreshed.tick.pending_effect.env_mask.tolist() == [True, False] + assert refreshed.tick.pending_effect.verification_id != old_id + + def verify_remaining( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + pending_effect = tick.pending_effect + assert pending_effect is not None + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ) + + completed = runner.run_until_blocked(effect_verifier=verify_remaining) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None + assert completed.tick.eligible_mask.tolist() == [True, False] + + +def test_blocking_runner_fails_safely_for_a_mismatched_effect_result() -> None: + runner, _, _, sink, _ = _make_runner(with_effect=True) + + def mismatched_effect_result( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + pending_effect = tick.pending_effect + assert pending_effect is not None + return EffectVerificationResult( + verification_id=pending_effect.verification_id + 1, + success_mask=torch.ones(context.batch_size, dtype=torch.bool), + failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), + ) + + failed = runner.run_until_blocked(effect_verifier=mismatched_effect_result) + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches[-2:]] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "verification_id does not match" in failed.message From 84928a0528ec99e62dcd6758ce5be24e52db97d7 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 03:50:13 +0800 Subject: [PATCH 04/29] refactor(atomic-actions): verify effects on due observations --- .../topics/atomic-actions/atomic-actions.md | 16 +- .../overview/sim/atomic_actions/index.md | 2 +- docs/source/tutorial/atomic_actions.rst | 9 +- .../lab/sim/atomic_actions/execution.py | 11 +- embodichain/lab/sim/atomic_actions/runner.py | 66 +++--- .../atomic_action/moving_target_recovery.py | 14 +- .../sim/atomic_actions/test_engine_per_env.py | 27 +++ tests/sim/atomic_actions/test_runner.py | 199 ++++++++++++++++-- 8 files changed, 285 insertions(+), 59 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 2335a8d06..0665907cf 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -530,14 +530,24 @@ correlated `EffectVerificationResult`. Its disjoint `success_mask` and neither mask remain unresolved. Partial successes commit immediately while unresolved rows keep the barrier pending. `EffectVerificationRequest` carries a monotonic `verification_id`, stable `requested_at`/`deadline` values in the -robot-observation timestamp domain, and an owned effect snapshot. Mask shrinkage -creates a new ID without extending the deadline; whole-action retry creates a -new attempt. Results for an old ID are rejected. `RecoveryPolicy.action_timeout` +robot-observation timestamp domain, a session-local `attempt_generation`, and +an owned effect snapshot. Mask shrinkage creates a new ID without extending the +deadline or changing the generation; installing a replacement plan increments +the generation. Results for an old ID are rejected. `RecoveryPolicy.action_timeout` covers the trajectory and terminal effect wait together, and only timestamps strictly greater than the deadline time out. While verification is outstanding, `ExecutionTick.pending_effect` retains the request on every tick; `EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. +For synchronous verification, pass `effect_verifier(context, request)` to +`runner.step()` or `run_until_blocked()`. The runner calls it after the fresh +due-cycle observation and supplies its result to `session.tick()` in that same +cycle. It does not call the verifier when the observation timestamp is already +past the request deadline. A verifier must return an exact +`EffectVerificationResult`; all-false masks mean unresolved. External +asynchronous integrations instead pass `effect_result` explicitly on a due +`step()` call. + ```python request = tick.pending_effect effect_result = EffectVerificationResult( diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 8b117c846..2508cf93f 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -444,7 +444,7 @@ an older custom action by renaming its implementation to `_plan()`. | `session.revise_current(invocation)` | Manually ticked runtime orchestrator | Replaces the active logical call with a newer same-destination revision and replans from the latest observed context | | `runner.revise_current(invocation)` | Runner-driven runtime orchestrator or Action Agent | Snapshots a revision, preserves the current frame deadline, then replans from a fresh due-time observation | | `runner.deactivate_rows(mask, reason=...)` | Runner-driven runtime orchestrator | Permanently removes rows and refreshes the runner's cached effect request; prefer it over direct session mutation | -| `runner.step(effect_result=...)` | Non-blocking controller integration | Observes and routes a `RuntimeCommandFrame` only when it is due | +| `runner.step(effect_result=..., effect_verifier=...)` | Non-blocking controller integration | Observes only when due; accepts either an asynchronous correlated result or a synchronous verifier, never both | | `runner.run_until_blocked(...)` | Simple blocking application or tutorial | Advances the injected clock until terminal or external effect verification is required | | `runner.cancel(reason)` | Explicit safe stop | Requests controller cancellation followed by an observed-position hold | diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 03cfd6705..b46909766 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -502,9 +502,7 @@ correlated per-environment verification result: from embodichain.lab.sim.atomic_actions import EffectVerificationResult - def verify_effect(context, tick): - request = tick.pending_effect - assert request is not None + def verify_effect(context, request): success_mask, failure_mask = verify_grasp_or_release(context, request.env_mask) return EffectVerificationResult( verification_id=request.verification_id, @@ -515,7 +513,10 @@ correlated per-environment verification result: result = runner.run_until_blocked(effect_verifier=verify_effect) This prevents a successful trajectory plan from being mistaken for a successful -physical grasp or release. If verification is asynchronous, omit the callback; +physical grasp or release. The runner invokes this synchronous callback after a +fresh due-cycle observation and feeds its result to the session in that same +cycle. Returning all-false masks keeps the remaining rows unresolved. If +verification is asynchronous, omit the callback; ``run_until_blocked`` returns at the verification boundary and the application can later resume from the *current* pending request: diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index bde4d07b4..1721d43c6 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -108,7 +108,9 @@ class EffectVerificationRequest: ``requested_at`` and ``deadline`` use the same timestamp domain as :class:`RobotObservation`. Request-mask shrinkage retains both values; - only a whole-action retry starts a new attempt deadline. + only a newly installed plan starts a new attempt deadline. + ``attempt_generation`` is session-local and remains stable when partial + resolution or row deactivation replaces only the request ID. """ verification_id: int @@ -116,6 +118,7 @@ class EffectVerificationRequest: invocation_id: str | None invocation_revision: int invocation_index: int + attempt_generation: int terminal_segment: str | None requested_at: float deadline: float @@ -135,6 +138,8 @@ def __post_init__(self) -> None: raise ValueError("invocation_revision must be non-negative.") if self.invocation_index < 0: raise ValueError("invocation_index must be non-negative.") + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be a non-negative integer.") if self.terminal_segment is not None and ( not isinstance(self.terminal_segment, str) or not self.terminal_segment ): @@ -166,6 +171,7 @@ def snapshot(self) -> EffectVerificationRequest: invocation_id=self.invocation_id, invocation_revision=self.invocation_revision, invocation_index=self.invocation_index, + attempt_generation=self.attempt_generation, terminal_segment=self.terminal_segment, requested_at=self.requested_at, deadline=self.deadline, @@ -304,6 +310,7 @@ def __init__( ] = {} self._planned_scene = context.scene self._action_started_at = context.robot.timestamp + self._attempt_generation = -1 self._last_joint_command: torch.Tensor | None = None self._last_joint_ids: tuple[int, ...] = () self._last_command_mask = torch.zeros( @@ -891,6 +898,7 @@ def _install_plan( ): self._active_targets = replacement_targets self._plan = plan + self._attempt_generation += 1 self._waypoint_index = 0 self._planned_scene = context.scene self._action_started_at = context.robot.timestamp @@ -1478,6 +1486,7 @@ def _effect_verification_request( invocation_id=request.invocation_id, invocation_revision=request.revision, invocation_index=self._invocation_index, + attempt_generation=self._attempt_generation, terminal_segment=( self._plan.segments[-1].name if self._plan.segments else None ), diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 65043e374..8dac661a6 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -31,6 +31,7 @@ from .bindings import RuntimeEndpointTarget from .execution import ( + EffectVerificationRequest, EffectVerificationResult, ExecutionSession, ExecutionStatus, @@ -293,10 +294,10 @@ def is_waiting(self) -> bool: EffectVerifier = Callable[ - [PlanningContext, ExecutionTick], - EffectVerificationResult | None, + [PlanningContext, EffectVerificationRequest], + EffectVerificationResult, ] -"""Callback that verifies a pending semantic effect for each environment.""" +"""Synchronous verifier called on a fresh due-cycle observation.""" RunnerStepCallback = Callable[[RunnerStep], None] """Optional observer called after every blocking runner-loop iteration.""" @@ -464,6 +465,7 @@ def step( self, *, effect_result: EffectVerificationResult | None = None, + effect_verifier: EffectVerifier | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. @@ -471,11 +473,22 @@ def step( effect_result: Optional correlated effect result. If this call occurs before the next cycle is due, it is not consumed and must be supplied again on a later call. + effect_verifier: Optional synchronous verifier for the current + pending request. It runs after a fresh due-cycle observation + and before the session consumes the result. It is not called + after the request deadline. Mutually exclusive with + ``effect_result``. Returns: Runner status, optional session tick, controller acknowledgements, and time remaining before another update is due. """ + if effect_result is not None and effect_verifier is not None: + raise ValueError( + "effect_result and effect_verifier are mutually exclusive." + ) + if effect_verifier is not None and not callable(effect_verifier): + raise TypeError("effect_verifier must be callable or None.") now = self._clock_now() if self._status is not RunnerStatus.RUNNING: return self._result(timestamp=now) @@ -499,6 +512,25 @@ def step( ) self._last_context = context + pending_effect = self._session.pending_effect + if ( + effect_verifier is not None + and pending_effect is not None + and context.robot.timestamp <= pending_effect.deadline + ): + try: + effect_result = effect_verifier(context, pending_effect) + if type(effect_result) is not EffectVerificationResult: + raise TypeError( + "EffectVerifier must return exactly " + "EffectVerificationResult." + ) + except Exception as exc: + return self._fail( + f"Effect verifier failed: {type(exc).__name__}: {exc}", + context=context, + ) + try: if self._pending_revision is not None: self._session._install_prepared_revision( @@ -659,9 +691,10 @@ def run_until_blocked( """Run with clock-driven waiting until terminal or effect verification blocks. Args: - effect_verifier: Optional callback used after an - ``effect_verification_required`` event. Without one, the method - returns the running step so the caller can verify externally. + effect_verifier: Optional synchronous callback used on fresh + due-cycle observations while effect verification is pending. + Without one, the method returns the running boundary so the + caller can verify externally. on_step: Optional callback for tracing or tutorial visualization. max_steps: Hard bound on loop iterations. @@ -670,7 +703,6 @@ def run_until_blocked( """ if max_steps <= 0: raise ValueError("max_steps must be greater than zero.") - effect_result: EffectVerificationResult | None = None now = self._clock_now() last_result = self._result( timestamp=now, @@ -681,9 +713,7 @@ def run_until_blocked( if self.effect_verification_pending and effect_verifier is None: return last_result for _ in range(max_steps): - result = self.step(effect_result=effect_result) - if result.tick is not None: - effect_result = None + result = self.step(effect_verifier=effect_verifier) if on_step is not None: try: on_step(result) @@ -700,20 +730,8 @@ def run_until_blocked( verification_required = ( result.tick is not None and result.tick.pending_effect is not None ) - if verification_required: - if effect_verifier is None or result.context is None: - return result - try: - effect_result = effect_verifier(result.context, result.tick) - except Exception as exc: - return self._fail( - f"Effect verifier failed: {type(exc).__name__}: {exc}", - context=result.context, - tick=result.tick, - dispatches=list(result.dispatches), - ) - if effect_result is None: - return result + if verification_required and effect_verifier is None: + return result if result.wait_duration > 0.0: try: self._clock.sleep(result.wait_duration) diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index a31ee0c66..3738d3a2c 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -35,10 +35,11 @@ AtomicActionEngine, ControlPartCommandProfile, EntityState, + EffectVerificationRequest, + EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, - ExecutionTick, GraspGoal, MotionPolicy, ObjectSemantics, @@ -402,8 +403,8 @@ def on_step(step: RunnerStep) -> None: def verify_pickup_effect( _context: PlanningContext, - _: ExecutionTick, - ) -> torch.Tensor: + request: EffectVerificationRequest, + ) -> EffectVerificationResult: """Verify that the cube rose with, and remains near, the end effector.""" cube_position = target.get_local_pose(to_matrix=True)[:, :3, 3] eef_position = robot.compute_fk( @@ -422,7 +423,12 @@ def verify_pickup_effect( f"cube-to-EEF={held_distance.detach().cpu().tolist()} m, " f"success={success.detach().cpu().tolist()}." ) - return success + verified_success = request.env_mask & success + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=verified_success, + failure_mask=request.env_mask & ~success, + ) recording_started = start_auto_play_recording( sim, diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 82a572dcb..d4f833d42 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -1455,6 +1455,7 @@ def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> N assert partial.pending_effect is not None assert partial.pending_effect.env_mask.tolist() == [False, True] assert partial.pending_effect.verification_id != first_request.verification_id + assert partial.pending_effect.attempt_generation == first_request.attempt_generation assert partial.pending_effect.requested_at == first_request.requested_at assert partial.pending_effect.deadline == first_request.deadline assert not any( @@ -1978,6 +1979,7 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: assert first_wait.pending_effect is not None old_id = first_wait.pending_effect.verification_id old_deadline = first_wait.pending_effect.deadline + old_generation = first_wait.pending_effect.attempt_generation retry = session.tick(_context(0.3, 0.2, 0.2, 0)) assert retry.command is not None @@ -1986,6 +1988,7 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: second_wait = session.tick(_context(0.5, 0.2, 0.2, 0)) assert second_wait.pending_effect is not None assert second_wait.pending_effect.verification_id != old_id + assert second_wait.pending_effect.attempt_generation == old_generation + 1 assert second_wait.pending_effect.deadline > old_deadline with pytest.raises(ValueError, match="verification_id"): @@ -1999,6 +2002,30 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: ) +def test_effect_request_generation_advances_after_tracking_replan() -> None: + engine, _ = _engine() + effect = EffectAction() + engine.register(effect) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=effect.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + + replanned = session.tick(_context(0.1, 1.0, 0.2, 0)) + session.tick(_context(0.2, 1.0, 0.2, 0)) + waiting = session.tick(_context(0.3, 0.2, 0.2, 0)) + + assert any(event.kind is ExecutionEventKind.REPLANNED for event in replanned.events) + assert waiting.pending_effect is not None + assert waiting.pending_effect.attempt_generation == 1 + + def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: engine, _ = _engine() engine.register(FailedEffectAction()) diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index f3476a49b..9b3bd3477 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -37,11 +37,11 @@ CommandAckStatus, CommandOperation, EndEffectorPoseGoal, + EffectVerificationRequest, EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, - ExecutionTick, HeldObjectState, JOINT_POSITION_CAPABILITY, JointPositionPayload, @@ -322,13 +322,11 @@ def _make_runner( def _successful_effect_result( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: """Correlate a successful result with the pending effect boundary.""" - pending_effect = tick.pending_effect - assert pending_effect is not None return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.ones( context.batch_size, dtype=torch.bool, @@ -682,10 +680,10 @@ def test_resumed_effect_verifier_uses_a_fresh_observation() -> None: def record_fresh_context( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: observed_at.append(context.robot.timestamp) - return _successful_effect_result(context, tick) + return _successful_effect_result(context, request) completed = runner.run_until_blocked(effect_verifier=record_fresh_context) @@ -694,32 +692,191 @@ def record_fresh_context( assert observed_at[0] > blocked_at +def test_due_effect_verifier_consumes_fresh_observation_in_the_same_step() -> None: + runner, clock, _, _, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.context is not None + assert blocked.tick is not None and blocked.tick.pending_effect is not None + blocked_at = blocked.context.robot.timestamp + clock.advance(MINIMUM_CYCLE_TIME) + observed_at: list[float] = [] + + def verify_fresh_observation( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, request) + + completed = runner.step(effect_verifier=verify_fresh_observation) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None and completed.tick.pending_effect is None + assert completed.tick.task_state.get_held_object("arm") is not None + assert completed.context is not None + assert observed_at == [completed.context.robot.timestamp] + assert observed_at[0] > blocked_at + + +def test_effect_verifier_runs_and_succeeds_at_the_request_deadline() -> None: + runner, clock, _, _, _ = _make_runner( + with_effect=True, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + clock.advance(request.deadline - clock.now()) + observed_at: list[float] = [] + + def verify_at_deadline( + context: PlanningContext, + current_request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, current_request) + + completed = runner.step(effect_verifier=verify_at_deadline) + + assert completed.status is RunnerStatus.COMPLETED + assert observed_at == pytest.approx([request.deadline]) + + +def test_effect_verifier_is_not_called_after_deadline_and_session_retries() -> None: + runner, clock, _, _, action = _make_runner( + with_effect=True, + max_action_retries=1, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + plan_count = action.plan_count + clock.advance(request.deadline - clock.now() + MINIMUM_CYCLE_TIME) + verifier = Mock() + + retry = runner.step(effect_verifier=verifier) + + verifier.assert_not_called() + assert retry.status is RunnerStatus.RUNNING + assert retry.tick is not None and retry.tick.command is not None + assert action.plan_count == plan_count + 1 + assert { + ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT, + ExecutionEventKind.ACTION_RETRY, + ExecutionEventKind.REPLANNED, + }.issubset({event.kind for event in retry.tick.events}) + + +def test_effect_result_and_effect_verifier_are_mutually_exclusive() -> None: + runner, _, _, sink, action = _make_runner(with_effect=True) + result = EffectVerificationResult( + verification_id=0, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ) + + with pytest.raises(ValueError, match="mutually exclusive"): + runner.step( + effect_result=result, + effect_verifier=_successful_effect_result, + ) + + assert action.plan_count == 1 + assert sink.sent == [] + + +@pytest.mark.parametrize( + "invalid_result", + [None, True], + ids=["none", "wrong-type"], +) +def test_effect_verifier_invalid_result_fails_with_cancel_then_hold( + invalid_result: object | None, +) -> None: + runner, clock, _, sink, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + clock.advance(MINIMUM_CYCLE_TIME) + + def invalid_verifier( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> object | None: + del context, request + return invalid_result + + failed = runner.step(effect_verifier=invalid_verifier) + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "must return exactly EffectVerificationResult" in failed.message + + +def test_all_false_effect_updates_keep_polling_the_same_request() -> None: + runner, clock, _, sink, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + initial_request = blocked.tick.pending_effect + observed_requests: list[tuple[int, int]] = [] + + def report_no_progress( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_requests.append((request.verification_id, request.attempt_generation)) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=torch.zeros(context.batch_size, dtype=torch.bool), + failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), + ) + + clock.advance(MINIMUM_CYCLE_TIME) + first_poll = runner.step(effect_verifier=report_no_progress) + clock.advance(MINIMUM_CYCLE_TIME) + second_poll = runner.step(effect_verifier=report_no_progress) + + assert first_poll.status is RunnerStatus.RUNNING + assert second_poll.status is RunnerStatus.RUNNING + assert first_poll.tick is not None and first_poll.tick.pending_effect is not None + assert second_poll.tick is not None and second_poll.tick.pending_effect is not None + assert observed_requests == [ + (initial_request.verification_id, initial_request.attempt_generation), + (initial_request.verification_id, initial_request.attempt_generation), + ] + assert sink.cancel_count == 0 + assert second_poll.tick.task_state.get_held_object("arm") is None + + def test_partial_effect_verifier_receives_the_committed_task_state() -> None: runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) observations: list[list[bool] | None] = [] def verify_in_two_updates( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: - pending_effect = tick.pending_effect - assert pending_effect is not None held = context.task.get_held_object("arm") observations.append( None if held is None or held.env_mask is None else held.env_mask.tolist() ) - if pending_effect.env_mask.tolist() == [True, True]: + if request.env_mask.tolist() == [True, True]: return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), ) - assert pending_effect.env_mask.tolist() == [False, True] + assert request.env_mask.tolist() == [False, True] assert held is not None and held.env_mask is not None assert held.env_mask.tolist() == [True, False] assert torch.equal(context.task.held_objects["arm"].env_mask, held.env_mask) return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.tensor([False, True]), failure_mask=torch.tensor([False, False]), ) @@ -785,6 +942,7 @@ def test_runner_deactivation_refreshes_cached_effect_request() -> None: blocked = runner.run_until_blocked() assert blocked.tick is not None and blocked.tick.pending_effect is not None old_id = blocked.tick.pending_effect.verification_id + old_generation = blocked.tick.pending_effect.attempt_generation changed = runner.deactivate_rows( torch.tensor([False, True]), @@ -796,15 +954,14 @@ def test_runner_deactivation_refreshes_cached_effect_request() -> None: assert refreshed.tick is not None and refreshed.tick.pending_effect is not None assert refreshed.tick.pending_effect.env_mask.tolist() == [True, False] assert refreshed.tick.pending_effect.verification_id != old_id + assert refreshed.tick.pending_effect.attempt_generation == old_generation def verify_remaining( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: - pending_effect = tick.pending_effect - assert pending_effect is not None return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), ) @@ -821,12 +978,10 @@ def test_blocking_runner_fails_safely_for_a_mismatched_effect_result() -> None: def mismatched_effect_result( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: - pending_effect = tick.pending_effect - assert pending_effect is not None return EffectVerificationResult( - verification_id=pending_effect.verification_id + 1, + verification_id=request.verification_id + 1, success_mask=torch.ones(context.batch_size, dtype=torch.bool), failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), ) From 09686846cf48ed43d7dcc974a27f259c82a516e2 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:17:03 +0800 Subject: [PATCH 05/29] feat(atomic-actions): complete verified action runtime --- .../topics/atomic-actions/atomic-actions.md | 4 +- docs/source/api_reference/public_api.rst | 35 + .../overview/sim/atomic_actions/index.md | 10 +- docs/source/tutorial/atomic_actions.rst | 7 +- .../lab/sim/atomic_actions/__init__.py | 28 +- .../lab/sim/atomic_actions/affordance.py | 306 +++- .../lab/sim/atomic_actions/bindings.py | 13 + embodichain/lab/sim/atomic_actions/core.py | 49 +- embodichain/lab/sim/atomic_actions/effects.py | 233 ++- embodichain/lab/sim/atomic_actions/engine.py | 12 +- .../lab/sim/atomic_actions/execution.py | 344 +++- embodichain/lab/sim/atomic_actions/goals.py | 131 +- .../lab/sim/atomic_actions/invocation.py | 13 + embodichain/lab/sim/atomic_actions/plans.py | 164 +- .../sim/atomic_actions/primitives/__init__.py | 9 + .../sim/atomic_actions/primitives/_helpers.py | 39 +- .../primitives/coordinated_pickment.py | 56 +- .../primitives/coordinated_placement.py | 97 +- .../atomic_actions/primitives/hand_over.py | 79 +- .../primitives/move_held_object.py | 54 +- .../primitives/operate_articulation.py | 478 +++++ .../sim/atomic_actions/primitives/pick_up.py | 45 +- .../sim/atomic_actions/primitives/place.py | 61 +- embodichain/lab/sim/atomic_actions/runtime.py | 65 + embodichain/lab/sim/atomic_actions/state.py | 448 ++++- embodichain/lab/sim/objects/articulation.py | 9 + embodichain/lab/sim/skills/compiler.py | 19 +- embodichain/lab/sim/skills/profiles.py | 1 + embodichain/lab/sim/skills/runtime.py | 52 +- tests/sim/atomic_actions/test_actions.py | 1604 +++++++---------- .../test_articulation_effects.py | 120 ++ tests/sim/atomic_actions/test_control.py | 58 + tests/sim/atomic_actions/test_core.py | 194 +- tests/sim/atomic_actions/test_engine.py | 5 +- .../sim/atomic_actions/test_engine_per_env.py | 501 ++++- tests/sim/objects/test_articulation.py | 14 +- tests/sim/objects/test_robot.py | 20 +- tests/sim/skills/test_compiler.py | 6 +- tests/sim/skills/test_runtime.py | 32 +- 39 files changed, 4138 insertions(+), 1277 deletions(-) create mode 100644 embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py create mode 100644 tests/sim/atomic_actions/test_articulation_effects.py diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 0665907cf..c6743b397 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -438,8 +438,8 @@ Scene dependencies must match the poses each primitive actually consumes: Therefore, a custom action that consumes a snapshot pose through semantic data must override `_scene_dependencies()`, union `super()` dependencies, and add the consumed semantic ID. Do not declare an ID merely because semantics are present. -`ActionPlan.scene_dependency_end_segment` can bound dynamic-goal monitoring to -the reversible part of a staged action. `PickUp` stops monitoring after its +`ActionPlan.scene_dependency_monitor_until` can bound each dynamic dependency +to an exclusive command-frame index. `PickUp` stops monitoring after its approach is dispatched: target motion before contact still replans, while contact-, grasp-, and lift-induced object motion is not misclassified as an external target update. Collision-world and joint-tracking checks remain diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 4491b3454..779327edc 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -278,13 +278,19 @@ embodichain.lab.sim.atomic_actions ActionPlanningServices Affordance AntipodalAffordance + ArticulationJointState + ArticulationOperationAffordance + ArticulationOperationTarget AssembleAffordance BUILTIN_ACTION_TYPES CoordinatedPickmentOptions CoordinatedPlacementOptions DynamicCollisionMode EntityState + EffectVerificationRequirement + EffectVerificationResult EffectVerifier + ExecutionPlanAttempt GRASP_COMMAND HandOverOptions InteractionPoints @@ -292,7 +298,11 @@ embodichain.lab.sim.atomic_actions MoveHeldObjectOptions MoveJointsOptions ObjectActionGoal + ObservedArticulationJointState OPEN_COMMAND + OperateArticulation + OperateArticulationGoal + OperateArticulationOptions PickUpOptions PlaceOptions PoseGoalValue @@ -300,6 +310,7 @@ embodichain.lab.sim.atomic_actions RigidObjectSceneProviderCfg RunnerStepCallback SceneProvider + SceneArticulationOperationGeometry SceneSnapshotSupplier embodichain.lab.sim.atomic_actions.affordance @@ -311,6 +322,8 @@ embodichain.lab.sim.atomic_actions.affordance Affordance AntipodalAffordance + ArticulationOperationAffordance + ArticulationOperationTarget SlideAffordance PressAffordance TwistAffordance @@ -381,8 +394,10 @@ embodichain.lab.sim.atomic_actions.execution .. autosummary:: EffectVerificationRequest + EffectVerificationResult ExecutionEvent ExecutionEventKind + ExecutionPlanAttempt ExecutionSession ExecutionStatus ExecutionTick @@ -394,8 +409,10 @@ embodichain.lab.sim.atomic_actions.goals .. autosummary:: + ActionGoal ObjectActionGoal PoseGoalValue + SceneArticulationOperationGeometry SceneEntityPose collect_scene_dependencies resolve_pose_goal @@ -424,6 +441,7 @@ embodichain.lab.sim.atomic_actions.plans ActionPlan CompiledTrajectory + EffectVerificationRequirement ExecutionFeedbackMode PlannerDiagnostics TimedTrajectory @@ -449,6 +467,20 @@ embodichain.lab.sim.atomic_actions.primitives .. autosummary:: BUILTIN_ACTION_TYPES + OperateArticulation + OperateArticulationGoal + OperateArticulationOptions + +embodichain.lab.sim.atomic_actions.primitives.operate_articulation +----------------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.primitives.operate_articulation + +.. autosummary:: + + OperateArticulation + OperateArticulationGoal + OperateArticulationOptions embodichain.lab.sim.atomic_actions.requirements ----------------------------------------------- @@ -542,7 +574,10 @@ embodichain.lab.sim.atomic_actions.state .. autosummary:: EntityState + ArticulationJointState + CoordinatedHeldObjectState HeldObjectState + ObservedArticulationJointState PlanningContext RobotObservation SceneSnapshot diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 2508cf93f..c66f5656e 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -773,11 +773,11 @@ implicit-initial-pose path of coordinated pickup declare that dependency automatically. The deprecated live-entity fallback does not trigger scene-motion replanning. -An `ActionPlan.scene_dependency_end_segment` may bound monitoring to the -reversible portion of a staged action. `PickUp` stops monitoring its object and -grasp dependencies after `approach` is dispatched so contact-, close-, and -lift-induced movement does not trigger a false replan. Joint-tracking and -collision-world checks remain active. +`ActionPlan.scene_dependency_monitor_until` may bound each dependency to an +exclusive command-frame index. `PickUp` stops monitoring its object after the +`approach` segment is dispatched so contact-, close-, and lift-induced movement +does not trigger a false replan. Joint-tracking and collision-world checks +remain active. Dynamic collision invalidation is provider-driven. Only registered, pose-updatable collision entities are supported; adding/removing obstacles or diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index b46909766..e96c1e910 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -486,9 +486,10 @@ dependencies. Object-centric skills may additionally declare an explicit ``ObjectSemantics.entity_id`` when they ground an object pose from the same scene snapshot; for example, ``PickUp`` automatically tracks that ID. The legacy ``ObjectSemantics.entity`` live-pose fallback is deprecated and does not -create a scene dependency. An ``ActionPlan`` may bound dependency monitoring -with ``scene_dependency_end_segment``. ``PickUp`` uses ``approach`` as that -boundary; joint tracking and collision-world revision checks are unaffected. +create a scene dependency. An ``ActionPlan`` may bound each dependency with +``scene_dependency_monitor_until``. ``PickUp`` uses the exclusive end of +``approach`` as that boundary; joint tracking and collision-world revision +checks are unaffected. Task-state effects ------------------ diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 9b28f7beb..c2d38d545 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -31,6 +31,8 @@ from .affordance import ( Affordance, AntipodalAffordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, AssembleAffordance, InteractionPoints, PressAffordance, @@ -59,15 +61,23 @@ EffectVerificationResult, ExecutionEvent, ExecutionEventKind, + ExecutionPlanAttempt, ExecutionSession, ExecutionStatus, ExecutionTick, ) -from .goals import ObjectActionGoal, PoseGoalValue, SceneEntityPose +from .goals import ( + ActionGoal, + ObjectActionGoal, + PoseGoalValue, + SceneArticulationOperationGeometry, + SceneEntityPose, +) from .invocation import ActionInvocation, ActionOptions, ResolvedActionRequest from .plans import ( ActionPlan, CompiledTrajectory, + EffectVerificationRequirement, ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, @@ -117,6 +127,9 @@ MoveHeldObjectOptions, MoveJoints, MoveJointsOptions, + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, PickUp, PickUpOptions, Place, @@ -156,8 +169,11 @@ SimulationExecutionAdapter, ) from .state import ( + ArticulationJointState, + CoordinatedHeldObjectState, EntityState, HeldObjectState, + ObservedArticulationJointState, PlanningContext, RobotObservation, SceneSnapshot, @@ -173,6 +189,9 @@ "ActionPlanningServices", "Affordance", "AntipodalAffordance", + "ArticulationOperationAffordance", + "ArticulationOperationTarget", + "ArticulationJointState", "AssembleAffordance", "AssembleGoal", "AtomicAction", @@ -204,12 +223,14 @@ "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", + "EffectVerificationRequirement", "EffectVerificationResult", "EffectVerifier", "ExecutionClock", "ExecutionFeedbackMode", "ExecutionEvent", "ExecutionEventKind", + "ExecutionPlanAttempt", "ExecutionRunner", "ExecutionRunnerCfg", "ExecutionSession", @@ -242,6 +263,10 @@ "ObjectSemantics", "OPEN_COMMAND", "ObservationProvider", + "ObservedArticulationJointState", + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", "PickUp", "PickUpOptions", "Place", @@ -273,6 +298,7 @@ "RunnerStep", "RunnerStepCallback", "SceneProvider", + "SceneArticulationOperationGeometry", "SceneSnapshot", "SceneSnapshotSupplier", "SceneEntityPose", diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index ad6b7c8af..3b0c7f458 100644 --- a/embodichain/lab/sim/atomic_actions/affordance.py +++ b/embodichain/lab/sim/atomic_actions/affordance.py @@ -17,7 +17,11 @@ from __future__ import annotations import torch +from collections.abc import Mapping +from copy import deepcopy from dataclasses import dataclass, field +import math +from types import MappingProxyType from typing import Any, TYPE_CHECKING from embodichain.toolkits.graspkit.pg_grasp import ( @@ -537,6 +541,303 @@ def get_approach_direction(self, point_idx: int) -> torch.Tensor: ) +def _owned_se3_offset(value: torch.Tensor, *, field_name: str) -> torch.Tensor: + """Validate and own one affordance-local homogeneous transform.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.shape != (4, 4): + raise ValueError(f"{field_name} must have shape (4, 4).") + if not value.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + if not torch.isfinite(value).all(): + raise ValueError(f"{field_name} must contain only finite values.") + checked = value.to(dtype=torch.float64) + bottom = checked.new_tensor((0.0, 0.0, 0.0, 1.0)) + if not torch.allclose(checked[3], bottom, atol=1.0e-6, rtol=0.0): + raise ValueError(f"{field_name} must have bottom row [0, 0, 0, 1].") + rotation = checked[:3, :3] + if not torch.allclose( + rotation.T @ rotation, + torch.eye(3, dtype=checked.dtype, device=checked.device), + atol=1.0e-6, + rtol=0.0, + ) or not torch.isclose( + torch.linalg.det(rotation), + checked.new_tensor(1.0), + atol=1.0e-6, + rtol=0.0, + ): + raise ValueError(f"{field_name} must contain a proper SE(3) rotation.") + return value.clone() + + +def _finite_scalar(value: float, *, field_name: str) -> float: + """Return one finite non-boolean scalar as a float.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a finite scalar.") + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + return normalized + + +@dataclass(frozen=True, slots=True) +class ArticulationOperationTarget: + """Named joint target and handle-relative operation displacement. + + ``displacement`` is deliberately explicit: it is the full signed handle + stroke from the live source joint position captured during semantic + grounding to ``target_position``. Recovery replans scale this stroke by + the remaining live joint progress. + """ + + target_position: float + """Absolute desired articulation joint position.""" + + displacement: float + """Signed operation displacement from the currently observed handle pose.""" + + def __post_init__(self) -> None: + object.__setattr__( + self, + "target_position", + _finite_scalar( + self.target_position, + field_name="ArticulationOperationTarget.target_position", + ), + ) + object.__setattr__( + self, + "displacement", + _finite_scalar( + self.displacement, + field_name="ArticulationOperationTarget.displacement", + ), + ) + + def snapshot(self) -> ArticulationOperationTarget: + """Return an independently constructed immutable target.""" + return ArticulationOperationTarget(self.target_position, self.displacement) + + +@dataclass(eq=False) +class ArticulationOperationAffordance(Affordance): + """Declarative handle geometry for one articulated joint operation. + + The four offsets are expressed in the live handle frame. During semantic + grounding the approach and contact poses are ``handle @ offset``. The + operation and retract poses additionally insert a local translation of + ``operation_axis * displacement * position_scale`` before their offsets. + This keeps task code free of pose-matrix construction; the semantic + compiler copies the geometry into a late-bound atomic goal. + """ + + joint_id: str = "" + """Canonical joint identifier written to the atomic goal and effect.""" + + approach_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + contact_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + operation_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + retract_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + operation_axis: torch.Tensor = field( + default_factory=lambda: torch.tensor((1.0, 0.0, 0.0), dtype=torch.float32) + ) + """Unit operation direction expressed in the observed handle frame.""" + + position_scale: float = 1.0 + """Positive conversion from declared displacement units to pose metres.""" + + semantic_targets: Mapping[str, ArticulationOperationTarget] = field( + default_factory=dict + ) + """Optional stable target names mapped to position/displacement pairs.""" + + def __post_init__(self) -> None: + if ( + type(self.joint_id) is not str + or not self.joint_id + or self.joint_id != self.joint_id.strip() + ): + raise ValueError( + "ArticulationOperationAffordance.joint_id must be a non-empty " + "canonical identifier." + ) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + setattr( + self, + field_name, + _owned_se3_offset( + getattr(self, field_name), + field_name=f"ArticulationOperationAffordance.{field_name}", + ), + ) + axis = self.operation_axis + if not isinstance(axis, torch.Tensor): + raise TypeError( + "ArticulationOperationAffordance.operation_axis must be a tensor." + ) + if axis.shape != (3,) or not axis.is_floating_point(): + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be a " + "floating tensor with shape (3,)." + ) + if not torch.isfinite(axis).all(): + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be finite." + ) + norm = torch.linalg.vector_norm(axis) + if float(norm) <= torch.finfo(axis.dtype).eps: + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be non-zero." + ) + self.operation_axis = (axis / norm).clone() + self.position_scale = _finite_scalar( + self.position_scale, + field_name="ArticulationOperationAffordance.position_scale", + ) + if self.position_scale <= 0.0: + raise ValueError( + "ArticulationOperationAffordance.position_scale must be positive." + ) + if not isinstance(self.semantic_targets, Mapping): + raise TypeError( + "ArticulationOperationAffordance.semantic_targets must be a mapping." + ) + targets: dict[str, ArticulationOperationTarget] = {} + for target_id, target in self.semantic_targets.items(): + if ( + type(target_id) is not str + or not target_id + or target_id != target_id.strip() + ): + raise ValueError( + "Articulation operation target IDs must be non-empty canonical " + "identifiers." + ) + if type(target) is not ArticulationOperationTarget: + raise TypeError( + "semantic_targets values must be exact " + "ArticulationOperationTarget values." + ) + targets[target_id] = target.snapshot() + self.semantic_targets = MappingProxyType(targets) + + def resolve_target(self, target_id: str) -> ArticulationOperationTarget: + """Return an owned named target or raise with deterministic candidates.""" + if type(target_id) is not str or not target_id: + raise ValueError("target_id must be a non-empty string.") + try: + target = self.semantic_targets[target_id] + except KeyError as exc: + raise KeyError( + f"Unknown articulation target {target_id!r}; available targets are " + f"{sorted(self.semantic_targets)}." + ) from exc + return target.snapshot() + + def __deepcopy__(self, memo: dict[int, object]) -> ArticulationOperationAffordance: + """Copy immutable configuration despite ``MappingProxyType`` storage.""" + existing = memo.get(id(self)) + if existing is not None: + assert isinstance(existing, ArticulationOperationAffordance) + return existing + copied = ArticulationOperationAffordance( + object_label=self.object_label, + custom_config=deepcopy(self.custom_config, memo), + joint_id=self.joint_id, + approach_offset=self.approach_offset, + contact_offset=self.contact_offset, + operation_offset=self.operation_offset, + retract_offset=self.retract_offset, + operation_axis=self.operation_axis, + position_scale=self.position_scale, + semantic_targets={ + target_id: target.snapshot() + for target_id, target in self.semantic_targets.items() + }, + ) + memo[id(self)] = copied + return copied + + def ground_poses( + self, + handle_pose: torch.Tensor, + *, + displacement: float, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Ground four end-effector poses from a fresh handle observation. + + Args: + handle_pose: Live handle pose with shape ``(4, 4)`` or ``(B, 4, 4)``. + displacement: Signed displacement from this observed handle pose. + + Returns: + Approach, contact, operation, and retract pose batches. + """ + if not isinstance(handle_pose, torch.Tensor): + raise TypeError("handle_pose must be a torch.Tensor.") + if handle_pose.shape == (4, 4): + handles = handle_pose.unsqueeze(0) + elif ( + handle_pose.dim() == 3 + and handle_pose.shape[0] > 0 + and handle_pose.shape[-2:] == (4, 4) + ): + handles = handle_pose + else: + raise ValueError("handle_pose must have shape (4, 4) or (B, 4, 4).") + if not handle_pose.is_floating_point() or not torch.isfinite(handle_pose).all(): + raise ValueError("handle_pose must be a finite floating tensor.") + displacement = _finite_scalar(displacement, field_name="displacement") + offsets = tuple( + getattr(self, field_name) + .to( + device=handles.device, + dtype=handles.dtype, + ) + .unsqueeze(0) + .expand(handles.shape[0], -1, -1) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ) + ) + translation = ( + torch.eye( + 4, + dtype=handles.dtype, + device=handles.device, + ) + .unsqueeze(0) + .repeat(handles.shape[0], 1, 1) + ) + translation[:, :3, 3] = self.operation_axis.to( + device=handles.device, + dtype=handles.dtype, + ) * (displacement * self.position_scale) + approach = torch.bmm(handles, offsets[0]) + contact = torch.bmm(handles, offsets[1]) + moved_handle = torch.bmm(handles, translation) + operation = torch.bmm(moved_handle, offsets[2]) + retract = torch.bmm(moved_handle, offsets[3]) + return tuple(pose.clone() for pose in (approach, contact, operation, retract)) + + @dataclass class AssembleAffordance(Affordance): """Affordance describing how an assemble object fits onto a base object. @@ -617,9 +918,8 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: __all__ = [ "Affordance", "AntipodalAffordance", - "SlideAffordance", - "PressAffordance", - "TwistAffordance", + "ArticulationOperationAffordance", + "ArticulationOperationTarget", "InteractionPoints", "AssembleAffordance", ] diff --git a/embodichain/lab/sim/atomic_actions/bindings.py b/embodichain/lab/sim/atomic_actions/bindings.py index badfb26b5..a43bbaf71 100644 --- a/embodichain/lab/sim/atomic_actions/bindings.py +++ b/embodichain/lab/sim/atomic_actions/bindings.py @@ -189,6 +189,9 @@ class EndpointBinding: resource_id: str adapter_id: str target: RuntimeEndpointTarget + task_state_key: str | None = None + """Symbolic task-state key; direct-core defaults to ``target.target_id``.""" + capabilities: frozenset[str] = frozenset() commands: Mapping[str, ControlCommand] = field(default_factory=dict) claim_tokens: frozenset[str] = frozenset() @@ -235,6 +238,14 @@ def __post_init__(self) -> None: "fingerprint." ) object.__setattr__(self, "target", target) + task_state_key = ( + target.target_id if self.task_state_key is None else self.task_state_key + ) + _validate_identifier( + task_state_key, + field_name="EndpointBinding.task_state_key", + ) + object.__setattr__(self, "task_state_key", task_state_key) object.__setattr__( self, "capabilities", @@ -344,6 +355,7 @@ def with_commands( resource_id=self.resource_id, adapter_id=self.adapter_id, target=self.target, + task_state_key=self.task_state_key, capabilities=self.capabilities, commands=merged, claim_tokens=self.claim_tokens, @@ -358,6 +370,7 @@ def snapshot(self) -> EndpointBinding: resource_id=self.resource_id, adapter_id=self.adapter_id, target=self.target, + task_state_key=self.task_state_key, capabilities=self.capabilities, commands=self.commands, claim_tokens=self.claim_tokens, diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index 4006fa008..fe099461c 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -42,6 +42,7 @@ ) from .plans import ( ActionPlan, + EffectVerificationRequirement, ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, @@ -506,10 +507,11 @@ def build_plan( success: bool | torch.Tensor, trajectory: TimedTrajectory, expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, - scene_dependency_end_segment: str | None = None, + scene_dependency_monitor_until: Mapping[str, int] | None = None, ) -> ActionPlan: """Build a validated action plan for a primitive implementation. @@ -519,12 +521,19 @@ def build_plan( success: Per-environment planning success or scalar planner result. trajectory: Full-robot trajectory with explicit timing. expected_effects: Symbolic effects to verify after execution. + effect_verification: Optional explicit physical-effect boundary. + Use this when verification is required without a symbolic task- + state delta. replannable: Whether the execution runtime may replan this action. diagnostics: Optional retained planner diagnostics. segment_lengths: Optional ordered mapping from semantic segment names to waypoint counts. Zero-length entries are omitted. - scene_dependency_end_segment: Optional last segment during which - scene motion may invalidate and replan the action. + scene_dependency_monitor_until: Optional per-entity exclusive + waypoint-index upper bound for scene-motion invalidation. An + entity is monitored while the current waypoint index is smaller + than its bound. ``0`` disables monitoring immediately; omitted + dependencies remain monitored for the full action. Once the bound + is reached, all pose changes for that entity are ignored. Returns: Side-effect-free action plan. @@ -559,10 +568,11 @@ def build_plan( success=success_mask, commands=commands, expected_effects=expected_effects, + effect_verification=effect_verification, replannable=replannable, diagnostics=diagnostics, segment_lengths=segment_lengths, - scene_dependency_end_segment=scene_dependency_end_segment, + scene_dependency_monitor_until=scene_dependency_monitor_until, feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, joint_trajectory=timed, ) @@ -575,10 +585,11 @@ def build_command_plan( success: bool | torch.Tensor, commands: TimedCommandSequence, expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, - scene_dependency_end_segment: str | None = None, + scene_dependency_monitor_until: Mapping[str, int] | None = None, feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, joint_trajectory: TimedTrajectory | None = None, ) -> ActionPlan: @@ -592,18 +603,25 @@ def build_command_plan( request: Resolved invocation snapshot being planned. context: Planning input used for the plan. success: Per-environment planning success or scalar planner result. - commands: Transport-neutral timed command sequence. + commands: Transport-neutral command sequence for the action. expected_effects: Symbolic effects to verify after execution. + effect_verification: Optional explicit physical-effect boundary. replannable: Whether the execution runtime may replan this action. diagnostics: Optional retained planner diagnostics. - segment_lengths: Optional ordered semantic segment lengths. - scene_dependency_end_segment: Optional last segment during which - scene motion may invalidate and replan the action. - feedback_mode: Feedback contract used to detect command completion. - joint_trajectory: Optional trajectory paired with joint feedback. + segment_lengths: Optional ordered mapping from semantic segment names + to command-frame counts. Zero-length entries are omitted. + scene_dependency_monitor_until: Optional per-entity exclusive + command-frame-index upper bound for scene-motion invalidation. An + entity is monitored while the current frame index is smaller than + its bound. ``0`` disables monitoring immediately; omitted + dependencies remain monitored for the full action. Once the bound + is reached, all pose changes for that entity are ignored. + feedback_mode: Feedback contract used to determine target completion. + joint_trajectory: Optional joint trajectory retained for joint-position + feedback and inspection. Returns: - Validated, endpoint-authorized action plan. + Side-effect-free action plan. """ if not isinstance(commands, TimedCommandSequence): raise TypeError("commands must be a TimedCommandSequence.") @@ -646,10 +664,15 @@ def build_command_plan( joint_trajectory=joint_trajectory, segments=segments, scene_dependencies=self._scene_dependencies(request), - scene_dependency_end_segment=scene_dependency_end_segment, + scene_dependency_monitor_until=( + {} + if scene_dependency_monitor_until is None + else scene_dependency_monitor_until + ), collision_world_sensitive=self._uses_collision_world(request, context), replannable=replannable, expected_effects=expected_effects or StateDelta(), + effect_verification=effect_verification, invocation_id=request.invocation_id, invocation_revision=request.revision, ) diff --git a/embodichain/lab/sim/atomic_actions/effects.py b/embodichain/lab/sim/atomic_actions/effects.py index 802175a8f..740cf4887 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -29,9 +29,11 @@ from embodichain.lab.sim.common import BatchEntity from .state import ( + ArticulationJointState, CoordinatedHeldObjectState, HeldObjectState, TaskState, + _normalize_articulation_joint, _normalize_coordinated_held, _normalize_held, _normalize_mask, @@ -118,6 +120,16 @@ def _snapshot_coordinated( ) +def _snapshot_articulation_joint( + value: ArticulationJointState, +) -> ArticulationJointState: + """Return an independently owned articulation-joint effect value.""" + return ArticulationJointState( + position=value.position.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + + def _with_held_mask( value: HeldObjectState, env_mask: torch.Tensor, @@ -131,6 +143,29 @@ def _with_held_mask( ) +def _with_coordinated_mask( + value: CoordinatedHeldObjectState, + env_mask: torch.Tensor, +) -> CoordinatedHeldObjectState: + """Copy a coordinated held-object relation with a replacement mask.""" + return CoordinatedHeldObjectState( + semantics=value.semantics, + left_object_to_eef=value.left_object_to_eef, + right_object_to_eef=value.right_object_to_eef, + left_grasp_xpos=value.left_grasp_xpos, + right_grasp_xpos=value.right_grasp_xpos, + env_mask=env_mask, + ) + + +def _with_articulation_joint_mask( + value: ArticulationJointState, + env_mask: torch.Tensor, +) -> ArticulationJointState: + """Copy an articulation-joint state with a replacement mask.""" + return ArticulationJointState(position=value.position, env_mask=env_mask) + + def _merge_held( previous: HeldObjectState | None, candidate: HeldObjectState | None, @@ -176,6 +211,101 @@ def _merge_held( ) +def _merge_coordinated( + previous: CoordinatedHeldObjectState | None, + candidate: CoordinatedHeldObjectState | None, + update_mask: torch.Tensor, +) -> CoordinatedHeldObjectState | None: + """Apply one optional coordinated relation update per environment.""" + from .core import _same_object_identity + + if previous is None and candidate is None: + return None + if previous is None: + assert candidate is not None and candidate.env_mask is not None + env_mask = candidate.env_mask & update_mask + return _with_coordinated_mask(candidate, env_mask) if env_mask.any() else None + assert previous.env_mask is not None + if candidate is None: + env_mask = previous.env_mask & ~update_mask + return _with_coordinated_mask(previous, env_mask) if env_mask.any() else None + assert candidate.env_mask is not None + + previous_retained = bool((previous.env_mask & ~update_mask).any().item()) + candidate_applied = bool((candidate.env_mask & update_mask).any().item()) + if ( + previous_retained + and candidate_applied + and not _same_object_identity(previous.semantics, candidate.semantics) + ): + raise ValueError( + "Cannot merge different coordinated held-object semantics for one " + "resource pair across environments." + ) + env_mask = torch.where(update_mask, candidate.env_mask, previous.env_mask) + if not env_mask.any(): + return None + selector = update_mask[:, None, None] + return CoordinatedHeldObjectState( + semantics=(previous.semantics if previous_retained else candidate.semantics), + left_object_to_eef=torch.where( + selector, candidate.left_object_to_eef, previous.left_object_to_eef + ), + right_object_to_eef=torch.where( + selector, candidate.right_object_to_eef, previous.right_object_to_eef + ), + left_grasp_xpos=torch.where( + selector, candidate.left_grasp_xpos, previous.left_grasp_xpos + ), + right_grasp_xpos=torch.where( + selector, candidate.right_grasp_xpos, previous.right_grasp_xpos + ), + env_mask=env_mask, + ) + + +def _merge_articulation_joint( + previous: ArticulationJointState | None, + candidate: ArticulationJointState | None, + update_mask: torch.Tensor, +) -> ArticulationJointState | None: + """Apply one optional articulation-joint update per environment.""" + if previous is None and candidate is None: + return None + if previous is None: + assert candidate is not None and candidate.env_mask is not None + env_mask = candidate.env_mask & update_mask + return ( + _with_articulation_joint_mask(candidate, env_mask) + if env_mask.any() + else None + ) + assert previous.env_mask is not None + if candidate is None: + env_mask = previous.env_mask & ~update_mask + return ( + _with_articulation_joint_mask(previous, env_mask) + if env_mask.any() + else None + ) + assert candidate.env_mask is not None + if candidate.position.shape != previous.position.shape: + raise ValueError( + "Cannot merge articulation-joint states with different joint widths." + ) + env_mask = torch.where(update_mask, candidate.env_mask, previous.env_mask) + if not env_mask.any(): + return None + return ArticulationJointState( + position=torch.where( + update_mask[:, None], + candidate.position, + previous.position, + ), + env_mask=env_mask, + ) + + @dataclass(frozen=True, slots=True, eq=False) class StateDelta: """Expected task-state changes that require post-execution verification. @@ -190,8 +320,20 @@ class StateDelta: ) """Per-resource attachment replacements or removals.""" + coordinated_held_object_updates: Mapping[ + tuple[str, str], CoordinatedHeldObjectState | None + ] = field(default_factory=dict) + """Per-resource-pair coordinated attachment replacements or removals.""" + + articulation_joint_updates: Mapping[ + tuple[str, str], ArticulationJointState | None + ] = field(default_factory=dict) + """Per-articulation/joint verified state replacements or removals.""" + def __post_init__(self) -> None: held = dict(self.held_object_updates) + coordinated = dict(self.coordinated_held_object_updates) + articulation = dict(self.articulation_joint_updates) for resource, value in held.items(): if not isinstance(resource, str) or not resource: raise ValueError( @@ -201,12 +343,57 @@ def __post_init__(self) -> None: raise TypeError( "held_object_updates values must be HeldObjectState or None." ) + for resources, value in coordinated.items(): + if ( + not isinstance(resources, tuple) + or len(resources) != 2 + or not all(isinstance(item, str) and item for item in resources) + ): + raise ValueError( + "coordinated_held_object_updates keys must be resource pairs." + ) + if value is not None and not isinstance(value, CoordinatedHeldObjectState): + raise TypeError( + "coordinated_held_object_updates values must be " + "CoordinatedHeldObjectState or None." + ) + for key, value in articulation.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(item) is str and item and item == item.strip() for item in key + ) + ): + raise ValueError( + "articulation_joint_updates keys must be canonical " + "articulation/joint pairs." + ) + if value is not None and not isinstance(value, ArticulationJointState): + raise TypeError( + "articulation_joint_updates values must be " + "ArticulationJointState or None." + ) object.__setattr__(self, "held_object_updates", MappingProxyType(held)) + object.__setattr__( + self, + "coordinated_held_object_updates", + MappingProxyType(coordinated), + ) + object.__setattr__( + self, + "articulation_joint_updates", + MappingProxyType(articulation), + ) @property def is_empty(self) -> bool: """Whether this delta declares no symbolic state changes.""" - return not self.held_object_updates + return ( + not self.held_object_updates + and not self.coordinated_held_object_updates + and not self.articulation_joint_updates + ) def snapshot(self) -> StateDelta: """Return an independently owned symbolic-effect snapshot. @@ -226,6 +413,10 @@ def snapshot(self) -> StateDelta: resources: (None if value is None else _snapshot_coordinated(value)) for resources, value in self.coordinated_held_object_updates.items() }, + articulation_joint_updates={ + key: (None if value is None else _snapshot_articulation_joint(value)) + for key, value in self.articulation_joint_updates.items() + }, ) def apply( @@ -271,10 +462,50 @@ def apply( else: held[resource] = merged + coordinated = dict(state.coordinated_held_objects) + for resources, candidate in self.coordinated_held_object_updates.items(): + normalized = ( + None + if candidate is None + else _normalize_coordinated_held( + candidate, + batch_size=state.batch_size, + device=state.device, + ) + ) + merged = _merge_coordinated(coordinated.get(resources), normalized, mask) + if merged is None: + coordinated.pop(resources, None) + else: + coordinated[resources] = merged + + articulation = dict(state.articulation_joints) + for key, candidate in self.articulation_joint_updates.items(): + normalized = ( + None + if candidate is None + else _normalize_articulation_joint( + candidate, + batch_size=state.batch_size, + device=state.device, + ) + ) + merged = _merge_articulation_joint( + articulation.get(key), + normalized, + mask, + ) + if merged is None: + articulation.pop(key, None) + else: + articulation[key] = merged + return TaskState( batch_size=state.batch_size, device=state.device, held_objects=held, + coordinated_held_objects=coordinated, + articulation_joints=articulation, ) diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index fc8cdda38..c4214f382 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -206,6 +206,8 @@ def bind_control_parts( self, skill: str | AtomicAction, endpoints: Mapping[str, Mapping[str, str]], + *, + task_state_keys: Mapping[str, str] | None = None, ) -> ActionBinding: """Build an advanced direct-core binding from control-part names. @@ -213,6 +215,10 @@ def bind_control_parts( skill: Installed skill ID or an explicit action passed later to :meth:`plan_action`. endpoints: Nested ``slot_id -> endpoint_id -> control_part`` mapping. + task_state_keys: Optional explicit stable task-state key for each + resource slot. See + :meth:`ActionPlanningServices.bind_control_parts` for inference + rules when omitted. Returns: Engine-owned generic endpoint binding. @@ -237,7 +243,11 @@ def bind_control_parts( raise ValueError( f"Skill {action.skill_id!r} has no explicit SkillBindingContract." ) - return self._planning_services.bind_control_parts(contract, endpoints) + return self._planning_services.bind_control_parts( + contract, + endpoints, + task_state_keys=task_state_keys, + ) def make_invocation( self, diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 1721d43c6..61a9e9e2a 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -30,9 +30,11 @@ from .bindings import JointPositionTarget, RuntimeEndpointTarget from .plans import ( ActionPlan, + EffectVerificationRequirement, ExecutionFeedbackMode, TrajectorySegment, ) +from .policies import RecoveryPolicy from .runtime_commands import ( JointPositionPayload, RuntimeCommandFrame, @@ -102,9 +104,118 @@ def __post_init__(self) -> None: object.__setattr__(self, "env_mask", self.env_mask.clone()) +@dataclass(frozen=True, slots=True, eq=False) +class ExecutionPlanAttempt: + """Owned inspection snapshot for one installed action plan. + + Recovery can install several plans for one logical invocation. This value + preserves the exact scene/collision revisions and trajectory structure of + every installation, correlated with the session-local attempt generation + and row-local recovery counters. + """ + + attempt_generation: int + event_kind: ExecutionEventKind + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + request: ResolvedActionRequest + plan: ActionPlan + + def __post_init__(self) -> None: + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be a non-negative integer.") + if self.event_kind not in { + ExecutionEventKind.ACTION_PLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ExecutionEventKind.REPLANNED, + }: + raise ValueError("event_kind must describe an installed action plan.") + if not math.isfinite(self.planned_at) or self.planned_at < 0.0: + raise ValueError("planned_at must be finite and non-negative.") + if type(self.invocation_index) is not int or self.invocation_index < 0: + raise ValueError("invocation_index must be a non-negative integer.") + if ( + not isinstance(self.planned_mask, torch.Tensor) + or self.planned_mask.dtype != torch.bool + or self.planned_mask.dim() != 1 + ): + raise ValueError("planned_mask must be a one-dimensional bool tensor.") + retries = tuple(self.action_retry_counts) + replans = tuple(self.replan_counts) + batch_size = int(self.planned_mask.numel()) + if len(retries) != batch_size or len(replans) != batch_size: + raise ValueError("Recovery counters must contain one value per row.") + if any(type(value) is not int or value < 0 for value in (*retries, *replans)): + raise ValueError("Recovery counters must be non-negative integers.") + if not isinstance(self.request, ResolvedActionRequest): + raise TypeError("request must be a ResolvedActionRequest.") + if not isinstance(self.plan, ActionPlan): + raise TypeError("plan must be an ActionPlan.") + if ( + self.request.skill_id != self.plan.skill_id + or self.request.invocation_id != self.plan.invocation_id + or self.request.revision != self.plan.invocation_revision + ): + raise ValueError("request identity must match the installed plan.") + if self.plan.plan_success.shape != self.planned_mask.shape: + raise ValueError("plan and planned_mask batch shapes must match.") + if self.plan.plan_success.device != self.planned_mask.device: + raise ValueError("plan and planned_mask must share a device.") + object.__setattr__(self, "planned_mask", self.planned_mask.clone()) + object.__setattr__(self, "action_retry_counts", retries) + object.__setattr__(self, "replan_counts", replans) + object.__setattr__(self, "request", self.request.snapshot()) + object.__setattr__(self, "plan", self.plan.snapshot()) + + def snapshot(self) -> ExecutionPlanAttempt: + """Return an independently owned plan-attempt trace.""" + return ExecutionPlanAttempt( + attempt_generation=self.attempt_generation, + event_kind=self.event_kind, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + request=self.request, + plan=self.plan, + ) + + +@dataclass(frozen=True, slots=True) +class _ExecutionPlanAttemptRecord: + """Session-private plan reference converted to an owned public snapshot.""" + + attempt_generation: int + event_kind: ExecutionEventKind + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + request: ResolvedActionRequest + plan: ActionPlan + + def snapshot(self) -> ExecutionPlanAttempt: + return ExecutionPlanAttempt( + attempt_generation=self.attempt_generation, + event_kind=self.event_kind, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + request=self.request, + plan=self.plan, + ) + + @dataclass(frozen=True, slots=True, eq=False) class EffectVerificationRequest: - """Typed boundary describing a semantic effect awaiting verification. + """Typed boundary describing a physical effect awaiting verification. ``requested_at`` and ``deadline`` use the same timestamp domain as :class:`RobotObservation`. Request-mask shrinkage retains both values; @@ -124,6 +235,7 @@ class EffectVerificationRequest: deadline: float env_mask: torch.Tensor expected_effects: StateDelta + effect_verification: EffectVerificationRequirement | None = None def __post_init__(self) -> None: if type(self.verification_id) is not int or self.verification_id < 0: @@ -158,10 +270,30 @@ def __post_init__(self) -> None: raise ValueError("env_mask must contain at least one requested row.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") - if self.expected_effects.is_empty: - raise ValueError("Effect verification requires a non-empty StateDelta.") + if ( + self.effect_verification is not None + and type(self.effect_verification) is not EffectVerificationRequirement + ): + raise TypeError( + "effect_verification must be exactly " + "EffectVerificationRequirement or None." + ) + if self.expected_effects.is_empty and self.effect_verification is None: + raise ValueError( + "Effect verification requires expected symbolic effects or an " + "explicit physical-effect requirement." + ) object.__setattr__(self, "env_mask", self.env_mask.clone()) object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + object.__setattr__( + self, + "effect_verification", + ( + None + if self.effect_verification is None + else self.effect_verification.snapshot() + ), + ) def snapshot(self) -> EffectVerificationRequest: """Return a request snapshot with an independently owned row mask.""" @@ -177,6 +309,7 @@ def snapshot(self) -> EffectVerificationRequest: deadline=self.deadline, env_mask=self.env_mask, expected_effects=self.expected_effects, + effect_verification=self.effect_verification, ) @@ -273,9 +406,9 @@ class ExecutionSession: The session never steps a simulator itself. Each :meth:`tick` consumes the latest observation and scene snapshot and emits at most one synchronized - endpoint-command frame. Expected symbolic effects are committed only after the caller - supplies a correlated :class:`EffectVerificationResult` for a non-empty - :class:`StateDelta`. + endpoint-command frame. A declared physical-effect boundary resolves only + after the caller supplies a correlated :class:`EffectVerificationResult`. + Non-empty expected symbolic effects are committed for verified rows only. Environment eligibility and recovery budgets are tracked per row. The waypoint cursor is batch-synchronized: a recoverable row replans the active @@ -330,6 +463,7 @@ def __init__( self._effect_failures = torch.zeros_like(self._eligible) self._effect_requested_at: float | None = None self._next_effect_verification_id = 0 + self._plan_attempt_records: list[_ExecutionPlanAttemptRecord] = [] self._status = ( ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED ) @@ -573,6 +707,27 @@ def active_commands(self) -> TimedCommandSequence: assert self._plan is not None return self._plan.commands.snapshot() + @property + def active_plan(self) -> ActionPlan: + """Return an independently owned snapshot of the active action plan. + + This is a read-only diagnostics boundary for runtime metadata, + visualization, and tests. Planning and recovery remain session-owned; + mutating any tensor in the returned value cannot affect execution. + """ + assert self._plan is not None + return self._plan.snapshot() + + @property + def plan_attempts(self) -> tuple[ExecutionPlanAttempt, ...]: + """Return every installed plan in deterministic recovery order. + + The initial plan has generation zero. Each invocation revision, + recovery replan, or whole-action retry appends a new generation instead + of replacing earlier scene/collision evidence. + """ + return tuple(record.snapshot() for record in self._plan_attempt_records) + def trajectory_segment(self, name: str) -> TrajectorySegment: """Return named segment metadata for the active action plan. @@ -607,7 +762,7 @@ def tick( "effect_result must be exactly EffectVerificationResult or None." ) if self._pending_effect is None: - raise ValueError("No semantic effect is awaiting verification.") + raise ValueError("No physical effect is awaiting verification.") if effect_result.verification_id != self._pending_effect.verification_id: raise ValueError( "effect_result verification_id does not match the pending " @@ -645,7 +800,7 @@ def tick( self._event( ExecutionEventKind.EFFECT_VERIFICATION_FAILED, known_failures, - "Expected semantic effects were not observed.", + "Required physical effects were not observed.", ) ) if planning_failed.any(): @@ -708,7 +863,7 @@ def tick( self._attempt_action_retry( retry_mask, ExecutionEventKind.EFFECT_VERIFICATION_FAILED, - "Expected semantic effects were not observed.", + "Required physical effects were not observed.", reason_mask=failed_effect, ) ) @@ -756,6 +911,18 @@ def tick( events=events, ) + if not execution_mask.any(): + command, hold_targets, completion_events = self._finish_action( + execution_mask, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + commands = plan.commands if self._waypoint_index < commands.frame_count: command = self._command_at(plan, self._waypoint_index, execution_mask) @@ -767,11 +934,15 @@ def tick( terminal_error > plan.recovery_policy.tracking_error_threshold ) if not_reached.any(): + max_terminal_error = float(terminal_error[not_reached].amax().item()) events.extend( self._attempt_replan( not_reached, ExecutionEventKind.TRACKING_ERROR, - "Terminal command has not been reached.", + "Terminal command has not been reached " + f"(max_error={max_terminal_error:.6f}, " + "threshold=" + f"{plan.recovery_policy.tracking_error_threshold:.6f}).", ) ) if self._status is not ExecutionStatus.RUNNING: @@ -909,6 +1080,23 @@ def _install_plan( self._effect_failures.zero_() self._effect_requested_at = None planned_mask = self._pending & plan.plan_success + self._plan_attempt_records.append( + _ExecutionPlanAttemptRecord( + attempt_generation=self._attempt_generation, + event_kind=event_kind, + planned_at=context.robot.timestamp, + invocation_index=self._invocation_index, + planned_mask=planned_mask.clone(), + action_retry_counts=tuple( + int(value) for value in self._action_retries.detach().cpu().tolist() + ), + replan_counts=tuple( + int(value) for value in self._replans.detach().cpu().tolist() + ), + request=self._requests[self._invocation_index].snapshot(), + plan=plan.snapshot(), + ) + ) self._queued_events.append( self._event(event_kind, planned_mask, "Planned from the latest context.") ) @@ -1015,17 +1203,25 @@ def _recover_if_needed( & (tracking_error > plan.recovery_policy.tracking_error_threshold) ) if tracking_mask.any(): + max_tracking_error = float(tracking_error[tracking_mask].amax().item()) return self._attempt_replan( tracking_mask, ExecutionEventKind.TRACKING_ERROR, - "Observed joint tracking error exceeded the policy threshold.", + "Observed joint tracking error exceeded the policy threshold " + f"(max_error={max_tracking_error:.6f}, " + "threshold=" + f"{plan.recovery_policy.tracking_error_threshold:.6f}).", ) - scene_mask = self._dynamic_scene_change_mask(plan) - if (execution_mask & scene_mask).any(): + scene_mask, scene_message = self._dynamic_scene_change( + plan, + execution_mask, + ) + if scene_mask.any(): + assert scene_message is not None return self._attempt_replan( - execution_mask & scene_mask, + scene_mask, ExecutionEventKind.DYNAMIC_GOAL_CHANGED, - "A referenced scene entity moved beyond the policy threshold.", + scene_message, ) return events @@ -1169,7 +1365,7 @@ def _finish_action( failed_effect = torch.zeros_like(execution_mask) unresolved = torch.zeros_like(execution_mask) made_progress = False - if self._plan.expected_effects.is_empty: + if not self._plan.requires_effect_verification: verified = execution_mask elif effect_result is None: if self._pending_effect is None: @@ -1178,7 +1374,7 @@ def _finish_action( self._event( ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED, execution_mask, - "Expected symbolic effects require external verification.", + "The action requires external physical-effect verification.", ) ) return None, active_targets, events @@ -1206,16 +1402,16 @@ def _finish_action( self._pending_effect = None if verified.any(): - self._task_state = self._plan.expected_effects.apply( - self._task_state, verified - ) - self._context = PlanningContext( - robot=self._context.robot, - task=self._task_state, - scene=self._context.scene, - env_ids=self._context.env_ids, - control_dt=self._context.control_dt, - ) + if not self._plan.expected_effects.is_empty: + self._task_state = self._plan.expected_effects.apply( + self._task_state, verified + ) + self._context = PlanningContext( + robot=self._context.robot, + task=self._task_state, + scene=self._context.scene, + env_ids=self._context.env_ids, + ) self._pending &= ~verified if unresolved.any(): if made_progress: @@ -1392,26 +1588,47 @@ def _terminal_error(self, plan: ActionPlan) -> torch.Tensor: ) return torch.amax(torch.cat(errors, dim=1), dim=1) - def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: - """Detect material motion of entities referenced by the action goal.""" + def _dynamic_scene_change( + self, + plan: ActionPlan, + execution_mask: torch.Tensor, + ) -> tuple[torch.Tensor, str | None]: + """Detect and describe material scene-dependency invalidation.""" dependencies = plan.scene_dependencies changed = torch.zeros_like(self._eligible) - dependency_end = plan.scene_dependency_end_segment if ( not dependencies - or ( - dependency_end is not None - and self._waypoint_index >= plan.segment(dependency_end).stop - ) or self._context.scene.version == self._planned_scene.version ): - return changed + return changed, None policy = plan.recovery_policy - for entity_id in dependencies: + details: list[str] = [] + for entity_id in sorted(dependencies): + monitor_until = plan.scene_dependency_monitor_until.get(entity_id) + if monitor_until is not None and self._waypoint_index >= monitor_until: + continue previous = self._planned_scene.entities.get(entity_id) current = self._context.scene.entities.get(entity_id) if previous is None or current is None: - changed |= self._eligible + entity_changed = execution_mask.clone() + if not entity_changed.any(): + continue + changed |= entity_changed + missing = [] + if previous is None: + missing.append("planned_scene") + if current is None: + missing.append("current_scene") + details.append( + self._scene_dependency_change_detail( + entity_id=entity_id, + monitor_until=monitor_until, + policy=policy, + max_translation=None, + max_rotation=None, + missing=",".join(missing), + ) + ) continue previous_pose = self._batched_entity_pose(previous) current_pose = self._batched_entity_pose(current) @@ -1426,10 +1643,55 @@ def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: (relative_rotation.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) / 2.0 ).clamp(-1.0, 1.0) rotation = torch.acos(cosine) - changed |= (translation > policy.goal_translation_threshold) | ( - rotation > policy.goal_rotation_threshold + entity_changed = execution_mask & ( + (translation > policy.goal_translation_threshold) + | (rotation > policy.goal_rotation_threshold) + ) + if not entity_changed.any(): + continue + changed |= entity_changed + details.append( + self._scene_dependency_change_detail( + entity_id=entity_id, + monitor_until=monitor_until, + policy=policy, + max_translation=float(translation[entity_changed].amax().item()), + max_rotation=float(rotation[entity_changed].amax().item()), + missing=None, + ) ) - return changed + if not details: + return changed, None + return ( + changed, + "Scene dependency invalidated the active plan at " + f"waypoint_index={self._waypoint_index}: " + " | ".join(details) + ".", + ) + + @staticmethod + def _scene_dependency_change_detail( + *, + entity_id: str, + monitor_until: int | None, + policy: RecoveryPolicy, + max_translation: float | None, + max_rotation: float | None, + missing: str | None, + ) -> str: + """Return one stable scene-dependency diagnostic fragment.""" + cutoff = "none" if monitor_until is None else str(monitor_until) + translation = ( + "unavailable" if max_translation is None else f"{max_translation:.6f}" + ) + rotation = "unavailable" if max_rotation is None else f"{max_rotation:.6f}" + missing_detail = "" if missing is None else f", missing={missing}" + return ( + f"entity_id={entity_id!r}, monitor_cutoff={cutoff}{missing_detail}, " + f"max_translation={translation}, " + f"translation_threshold={policy.goal_translation_threshold:.6f}, " + f"max_rotation={rotation}, " + f"rotation_threshold={policy.goal_rotation_threshold:.6f}" + ) def _collision_world_change_mask(self, plan: ActionPlan) -> torch.Tensor: """Detect collision revisions newer than the active action plan.""" @@ -1496,6 +1758,7 @@ def _effect_verification_request( ), env_mask=env_mask, expected_effects=self._plan.expected_effects, + effect_verification=self._plan.effect_verification, ) def _event( @@ -1575,6 +1838,7 @@ def _tick_result( "EffectVerificationResult", "ExecutionEvent", "ExecutionEventKind", + "ExecutionPlanAttempt", "ExecutionSession", "ExecutionStatus", "ExecutionTick", diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index 9a887552c..3ee3a651d 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -21,7 +21,8 @@ import warnings from collections.abc import Mapping, Sequence from dataclasses import dataclass, fields, is_dataclass -from typing import Any, TYPE_CHECKING +import math +from typing import Any, ClassVar, Protocol, TYPE_CHECKING import torch @@ -30,6 +31,12 @@ from .state import PlanningContext +class ActionGoal(Protocol): + """Structural protocol implemented by atomic-action goal value objects.""" + + goal_kind: ClassVar[str] + + @dataclass(frozen=True, slots=True, eq=False) class SceneEntityPose: """Late-bound pose derived from a versioned scene entity. @@ -74,6 +81,126 @@ def snapshot(self) -> SceneEntityPose: ) +@dataclass(frozen=True, slots=True, eq=False) +class SceneArticulationOperationGeometry: + """Late-bound handle geometry for an articulation operation. + + The offsets and operation axis are immutable grounded affordance data. The + handle itself remains a :class:`SceneEntityPose`, so every atomic plan or + recovery replan resolves it from the latest :class:`SceneSnapshot`. + """ + + handle_pose: SceneEntityPose + approach_offset: torch.Tensor + contact_offset: torch.Tensor + operation_offset: torch.Tensor + retract_offset: torch.Tensor + operation_axis: torch.Tensor + position_scale: float = 1.0 + + def __post_init__(self) -> None: + if not isinstance(self.handle_pose, SceneEntityPose): + raise TypeError("handle_pose must be a SceneEntityPose.") + object.__setattr__(self, "handle_pose", self.handle_pose.snapshot()) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + offset = getattr(self, field_name) + validate_pose_tensor(offset, field_name, allow_waypoints=False) + if offset.shape != (4, 4): + raise ValueError(f"{field_name} must have shape (4, 4).") + if not offset.is_floating_point() or not torch.isfinite(offset).all(): + raise ValueError(f"{field_name} must be a finite floating tensor.") + object.__setattr__(self, field_name, offset.clone()) + axis = self.operation_axis + if ( + not isinstance(axis, torch.Tensor) + or axis.shape != (3,) + or not axis.is_floating_point() + ): + raise ValueError("operation_axis must be a floating tensor of shape (3,).") + if not torch.isfinite(axis).all(): + raise ValueError("operation_axis must contain only finite values.") + norm = torch.linalg.vector_norm(axis) + if float(norm) <= torch.finfo(axis.dtype).eps: + raise ValueError("operation_axis must be non-zero.") + object.__setattr__(self, "operation_axis", (axis / norm).clone()) + scale = self.position_scale + if isinstance(scale, bool) or not isinstance(scale, (int, float)): + raise TypeError("position_scale must be a finite positive scalar.") + scale = float(scale) + if not math.isfinite(scale) or scale <= 0.0: + raise ValueError("position_scale must be a finite positive scalar.") + object.__setattr__(self, "position_scale", scale) + + def resolve( + self, + context: PlanningContext, + *, + displacement: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Resolve four poses using a fresh handle and row-local displacement. + + Args: + context: Latest immutable planning observation. + displacement: Remaining signed handle displacement, shape ``(B,)``. + + Returns: + Approach, contact, operation, and retract pose batches. + """ + if not isinstance(displacement, torch.Tensor): + raise TypeError("displacement must be a torch.Tensor.") + if displacement.shape != (context.batch_size,): + raise ValueError("displacement must have one scalar for each planning row.") + if ( + not displacement.is_floating_point() + or not torch.isfinite(displacement).all() + ): + raise ValueError("displacement must be a finite floating tensor.") + handle = resolve_pose_goal( + self.handle_pose, + context, + name="handle_pose", + ) + offsets = tuple( + getattr(self, field_name) + .to(device=handle.device, dtype=handle.dtype) + .unsqueeze(0) + .expand(context.batch_size, -1, -1) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ) + ) + translation = ( + torch.eye( + 4, + dtype=handle.dtype, + device=handle.device, + ) + .unsqueeze(0) + .repeat(context.batch_size, 1, 1) + ) + axis = self.operation_axis.to(device=handle.device, dtype=handle.dtype) + translation[:, :3, 3] = ( + axis.unsqueeze(0) + * displacement.to(device=handle.device, dtype=handle.dtype).unsqueeze(1) + * self.position_scale + ) + moved_handle = torch.bmm(handle, translation) + return ( + torch.bmm(handle, offsets[0]), + torch.bmm(handle, offsets[1]), + torch.bmm(moved_handle, offsets[2]), + torch.bmm(moved_handle, offsets[3]), + ) + + PoseGoalValue = torch.Tensor | SceneEntityPose """Explicit pose tensor or a pose resolved from the latest scene snapshot.""" @@ -246,8 +373,10 @@ def __post_init__(self) -> None: __all__ = [ + "ActionGoal", "ObjectActionGoal", "PoseGoalValue", + "SceneArticulationOperationGeometry", "SceneEntityPose", "collect_scene_dependencies", "resolve_pose_goal", diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index f5fde60f9..00bcfa72a 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -193,6 +193,19 @@ def __post_init__(self) -> None: object.__setattr__(self, "recovery_policy", deepcopy(self.recovery_policy)) object.__setattr__(self, "skill_options", deepcopy(self.skill_options)) + def snapshot(self) -> ResolvedActionRequest[GoalT, OptionsT]: + """Return an independently owned resolved-request snapshot.""" + return ResolvedActionRequest( + skill_id=self.skill_id, + goal=self.goal, + binding=self.binding, + motion_policy=self.motion_policy, + recovery_policy=self.recovery_policy, + skill_options=self.skill_options, + invocation_id=self.invocation_id, + revision=self.revision, + ) + __all__ = [ "ActionInvocation", diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index 66f224516..753b016cf 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -18,9 +18,10 @@ from __future__ import annotations -import math +from copy import deepcopy from dataclasses import dataclass, field from enum import Enum +import math from types import MappingProxyType from typing import Any, Mapping, Sequence @@ -364,8 +365,17 @@ class PlannerDiagnostics: def __post_init__(self) -> None: if not isinstance(self.backend, str) or not self.backend: raise ValueError("PlannerDiagnostics.backend must be non-empty.") - object.__setattr__(self, "messages", tuple(self.messages)) - object.__setattr__(self, "metadata", MappingProxyType(dict(self.metadata))) + if not isinstance(self.metadata, Mapping): + raise TypeError("PlannerDiagnostics.metadata must be a mapping.") + messages = tuple(self.messages) + if not all(type(message) is str for message in messages): + raise TypeError("PlannerDiagnostics.messages must contain strings.") + object.__setattr__(self, "messages", messages) + object.__setattr__( + self, + "metadata", + MappingProxyType(deepcopy(dict(self.metadata))), + ) class ExecutionFeedbackMode(str, Enum): @@ -375,6 +385,36 @@ class ExecutionFeedbackMode(str, Enum): TIMED = "timed" +@dataclass(frozen=True, slots=True) +class EffectVerificationRequirement: + """Explicit physical-effect verification independent of symbolic state. + + Presence of this value on an :class:`ActionPlan` forces a terminal effect + boundary even when the plan declares no :class:`StateDelta`. The open + ``kind`` identifier lets an external runtime select an appropriate + verifier without placing backend-specific callbacks in the core plan. + + Args: + kind: Stable, non-empty discriminator for the physical effect. + """ + + kind: str + + def __post_init__(self) -> None: + if ( + type(self.kind) is not str + or not self.kind + or self.kind != self.kind.strip() + ): + raise ValueError( + "kind must be a non-empty string without outer whitespace." + ) + + def snapshot(self) -> EffectVerificationRequirement: + """Return an independently owned requirement value.""" + return EffectVerificationRequirement(kind=self.kind) + + @dataclass(frozen=True, slots=True) class TrajectorySegment: """Named half-open waypoint range inside an action trajectory. @@ -417,6 +457,15 @@ class ActionPlan: An action owns one timed command sequence and one recovery boundary. Named :class:`TrajectorySegment` values describe semantic structure within that sequence without implying independent planning or recovery boundaries. + + Attributes: + scene_dependency_monitor_until: Optional exclusive waypoint-index upper + bounds for individual ``scene_dependencies``. An entity is monitored + while the current waypoint index is smaller than its bound; ``0`` + disables monitoring immediately, while an omitted entity remains + monitored for the action's full execution. Once the bound is reached, + all pose changes for that entity are ignored, regardless of whether + they were caused by the action or by an external disturbance. """ skill_id: str @@ -430,11 +479,11 @@ class ActionPlan: joint_trajectory: TimedTrajectory | None = None segments: tuple[TrajectorySegment, ...] = () scene_dependencies: tuple[str, ...] = () - scene_dependency_end_segment: str | None = None - """Stop dynamic-goal monitoring after this segment is dispatched.""" + scene_dependency_monitor_until: Mapping[str, int] = field(default_factory=dict) collision_world_sensitive: bool = False replannable: bool = True expected_effects: StateDelta = field(default_factory=StateDelta) + effect_verification: EffectVerificationRequirement | None = None invocation_id: str | None = None invocation_revision: int = 0 @@ -641,13 +690,37 @@ def __post_init__(self) -> None: raise ValueError( "scene_dependencies must contain unique non-empty entity ids." ) + waypoint_count = self.commands.frame_count + monitor_until = dict(self.scene_dependency_monitor_until) + if not set(monitor_until).issubset(dependencies): + raise ValueError( + "scene_dependency_monitor_until keys must be scene dependencies." + ) + for entity_id, waypoint_index in monitor_until.items(): + if ( + type(entity_id) is not str + or not entity_id + or type(waypoint_index) is not int + or not 0 <= waypoint_index <= waypoint_count + ): + raise ValueError( + "scene_dependency_monitor_until must map non-empty entity IDs " + "to waypoint indices within the command sequence." + ) if not isinstance(self.collision_world_sensitive, bool): raise TypeError("collision_world_sensitive must be a bool.") if not isinstance(self.replannable, bool): raise TypeError("replannable must be a bool.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") - waypoint_count = self.commands.frame_count + if ( + self.effect_verification is not None + and type(self.effect_verification) is not EffectVerificationRequirement + ): + raise TypeError( + "effect_verification must be exactly " + "EffectVerificationRequirement or None." + ) segments = tuple(self.segments) if not all(isinstance(segment, TrajectorySegment) for segment in segments): raise TypeError("segments must contain only TrajectorySegment values.") @@ -672,21 +745,6 @@ def __post_init__(self) -> None: "ActionPlan segments must cover the command sequence exactly without " "gaps or overlaps." ) - dependency_end = self.scene_dependency_end_segment - if dependency_end is not None: - if not isinstance(dependency_end, str) or not dependency_end: - raise ValueError( - "scene_dependency_end_segment must be a non-empty segment " - "name or None." - ) - if dependency_end not in names: - raise ValueError( - "scene_dependency_end_segment must name an ActionPlan segment." - ) - if not dependencies: - raise ValueError( - "scene_dependency_end_segment requires scene_dependencies." - ) object.__setattr__(self, "plan_success", self.plan_success.clone()) object.__setattr__(self, "commands", self.commands.snapshot()) object.__setattr__( @@ -699,14 +757,77 @@ def __post_init__(self) -> None: ), ) object.__setattr__(self, "planned_collision_world_revision", revisions) + object.__setattr__( + self, + "diagnostics", + PlannerDiagnostics( + backend=self.diagnostics.backend, + messages=self.diagnostics.messages, + metadata=self.diagnostics.metadata, + ), + ) object.__setattr__(self, "scene_dependencies", dependencies) + object.__setattr__( + self, + "scene_dependency_monitor_until", + MappingProxyType(monitor_until), + ) object.__setattr__(self, "segments", segments) + object.__setattr__( + self, + "effect_verification", + ( + None + if self.effect_verification is None + else self.effect_verification.snapshot() + ), + ) @property def success_all(self) -> bool: """Whether every environment row planned successfully.""" return bool(self.plan_success.all().item()) + def snapshot(self) -> ActionPlan: + """Return an independently owned inspection snapshot of this plan. + + Runtime tracing and visualization need access to the exact plan that + reached an execution boundary without being able to mutate the live + session. Reconstructing the value through the public constructor also + re-applies every plan invariant and snapshots all tensor-owning nested + contracts. + + Returns: + A validated plan with independently owned tensor storage. + """ + return ActionPlan( + skill_id=self.skill_id, + plan_success=self.plan_success, + commands=self.commands, + recovery_policy=self.recovery_policy, + planned_scene_version=self.planned_scene_version, + planned_collision_world_revision=self.planned_collision_world_revision, + diagnostics=self.diagnostics, + feedback_mode=self.feedback_mode, + joint_trajectory=self.joint_trajectory, + segments=self.segments, + scene_dependencies=self.scene_dependencies, + scene_dependency_monitor_until=self.scene_dependency_monitor_until, + collision_world_sensitive=self.collision_world_sensitive, + replannable=self.replannable, + expected_effects=self.expected_effects, + effect_verification=self.effect_verification, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + ) + + @property + def requires_effect_verification(self) -> bool: + """Whether execution must verify a terminal physical effect.""" + return ( + self.effect_verification is not None or not self.expected_effects.is_empty + ) + def segment(self, name: str) -> TrajectorySegment: """Return a named trajectory segment. @@ -787,6 +908,7 @@ def segment(self, action_index: int, name: str) -> TrajectorySegment: __all__ = [ "ActionPlan", "CompiledTrajectory", + "EffectVerificationRequirement", "ExecutionFeedbackMode", "PlannerDiagnostics", "TimedTrajectory", diff --git a/embodichain/lab/sim/atomic_actions/primitives/__init__.py b/embodichain/lab/sim/atomic_actions/primitives/__init__.py index 718e6780b..ac82134d3 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_actions/primitives/__init__.py @@ -41,6 +41,11 @@ MoveHeldObjectOptions, ) from .move_joints import JointPositionGoal, MoveJoints, MoveJointsOptions +from .operate_articulation import ( + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, +) from .pick_up import GraspGoal, PickUp, PickUpOptions from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions from .press import Press, PressGoal, PressOptions @@ -63,6 +68,7 @@ CoordinatedPickment, CoordinatedPlacement, HandOver, + OperateArticulation, ) """Built-in action implementations instantiated once per action engine.""" @@ -87,6 +93,9 @@ "MoveHeldObjectOptions", "MoveJoints", "MoveJointsOptions", + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", "PickUp", "PickUpOptions", "Place", diff --git a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py index cbb8ae7e0..84839a878 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py +++ b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py @@ -23,6 +23,9 @@ import torch +from embodichain.utils import logger + +from ..bindings import EndpointBinding from ..state import PlanningContext from ..trajectory_ops import build_pose_plan_states @@ -51,6 +54,37 @@ def resolve_batched_pose( return pose +def require_shared_task_state_key( + motion: EndpointBinding, + grasp: EndpointBinding, + *, + participant: str, +) -> str: + """Return the stable task-state key shared by one participant's endpoints. + + Args: + motion: Participant endpoint used for motion control. + grasp: Participant endpoint used for grasp control. + participant: Human-readable participant name used in validation errors. + + Returns: + Stable logical key used to address held-object task state. + + Raises: + ValueError: If the participant endpoints use different task-state keys. + """ + motion_key = motion.task_state_key + grasp_key = grasp.task_state_key + if motion_key != grasp_key: + raise ValueError( + f"{participant} motion and grasp endpoints must share one " + f"task_state_key, but got {motion_key!r} and {grasp_key!r}." + ) + if not isinstance(motion_key, str) or not motion_key: + raise ValueError(f"{participant} task_state_key must be a non-empty string.") + return motion_key + + def resolve_object_target( target: torch.Tensor, *, @@ -128,9 +162,6 @@ def arm_qpos_from_state( __all__ = [ "arm_qpos_from_state", - "assemble_full_robot_trajectory", - "plan_named_arm_trajectory", - "repeat_qpos", - "resolve_batched_pose", + "require_shared_task_state_key", "resolve_object_target", ] diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 935452aaf..75dac8f21 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -56,19 +56,15 @@ INVERSE_KINEMATICS_CAPABILITY, SkillBindingContract, ) -from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext -from embodichain.lab.sim.atomic_actions.trajectory_ops import ( - interpolate_joint_trajectory, - translate_pose_world, -) -from embodichain.lab.sim.atomic_actions.primitives._helpers import ( +from ..state import CoordinatedHeldObjectState, HeldObjectState, PlanningContext +from ..trajectory_ops import interpolate_joint_trajectory, translate_pose_world +from ._binding_contracts import make_manipulation_slot +from ._helpers import ( assemble_full_robot_trajectory, repeat_qpos, + require_shared_task_state_key, resolve_batched_pose, ) -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, -) @dataclass(frozen=True, slots=True, eq=False) @@ -174,6 +170,8 @@ def __post_init__(self) -> None: class _CoordinatedPickResources: """Invocation-bound control parts and compatible hand commands.""" + left_task_state_key: str + right_task_state_key: str left_arm: JointPositionTarget right_arm: JointPositionTarget left_hand: JointPositionTarget @@ -407,6 +405,21 @@ def _resolve_resources( right_arm = right_motion.require_target(JointPositionTarget) left_hand = left_grasp.require_target(JointPositionTarget) right_hand = right_grasp.require_target(JointPositionTarget) + left_task_state_key = require_shared_task_state_key( + left_motion, + left_grasp, + participant="CoordinatedPickment left participant", + ) + right_task_state_key = require_shared_task_state_key( + right_motion, + right_grasp, + participant="CoordinatedPickment right participant", + ) + if left_task_state_key == right_task_state_key: + raise ValueError( + "CoordinatedPickment left and right participants must use " + "different task_state_key values." + ) if left_arm.control_part == right_arm.control_part: raise ValueError( "CoordinatedPickment left and right roles must use different " @@ -418,6 +431,8 @@ def _resolve_resources( "end-effector control parts." ) return _CoordinatedPickResources( + left_task_state_key=left_task_state_key, + right_task_state_key=right_task_state_key, left_arm=left_arm, right_arm=right_arm, left_hand=left_hand, @@ -999,15 +1014,12 @@ def _plan( ], dim=1, ) - left_held_object = HeldObjectState( + coordinated_held_object = CoordinatedHeldObjectState( semantics=left_held_state.semantics, - object_to_eef=left_held_state.object_to_eef, - grasp_xpos=left_target_xpos, - ) - right_held_object = HeldObjectState( - semantics=right_held_state.semantics, - object_to_eef=right_held_state.object_to_eef, - grasp_xpos=right_target_xpos, + left_object_to_eef=left_held_state.object_to_eef, + right_object_to_eef=right_held_state.object_to_eef, + left_grasp_xpos=left_target_xpos, + right_grasp_xpos=right_target_xpos, ) return self.build_plan( request, @@ -1020,8 +1032,14 @@ def _plan( ), expected_effects=StateDelta( held_object_updates={ - resources.left_arm.control_part: left_held_object, - resources.right_arm.control_part: right_held_object, + resources.left_task_state_key: None, + resources.right_task_state_key: None, + }, + coordinated_held_object_updates={ + ( + resources.left_task_state_key, + resources.right_task_state_key, + ): coordinated_held_object, }, ), segment_lengths={ diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index e6e2cc4c0..20812641f 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -25,29 +25,15 @@ from embodichain.utils import logger -from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget -from embodichain.lab.sim.atomic_actions.control import ( - GRASP_COMMAND, - OPEN_COMMAND, - JointPositionCommand, -) -from embodichain.lab.sim.atomic_actions.core import AtomicAction -from embodichain.lab.sim.atomic_actions.effects import StateDelta -from embodichain.lab.sim.atomic_actions.goals import ( - PoseGoalValue, - resolve_pose_goal, - validate_pose_goal, -) -from embodichain.lab.sim.atomic_actions.invocation import ( - ActionOptions, - ResolvedActionRequest, -) -from embodichain.lab.sim.atomic_actions.plans import ( - ActionPlan, - TimedTrajectory, - normalize_success_mask, -) -from embodichain.lab.sim.atomic_actions.requirements import ( +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand +from ..core import AtomicAction +from ..effects import StateDelta +from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal +from ..invocation import ActionOptions, ResolvedActionRequest +from ..plans import ActionPlan, TimedTrajectory, normalize_success_mask +from ..policies import MotionPolicy +from ..requirements import ( CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, SkillBindingContract, @@ -57,16 +43,15 @@ interpolate_hand_qpos, translate_pose_world, ) -from embodichain.lab.sim.atomic_actions.primitives._helpers import ( +from ._binding_contracts import make_manipulation_slot +from ._helpers import ( assemble_full_robot_trajectory, plan_named_arm_trajectory, repeat_qpos, + require_shared_task_state_key, resolve_batched_pose, resolve_object_target, ) -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, -) @dataclass(frozen=True, slots=True, eq=False) @@ -138,6 +123,8 @@ def __post_init__(self) -> None: class _CoordinatedPlacementResources: """Invocation-bound control parts and compatible hand commands.""" + placing_task_state_key: str + support_task_state_key: str placing_arm: JointPositionTarget support_arm: JointPositionTarget placing_hand: JointPositionTarget @@ -191,6 +178,21 @@ def _resolve_resources( support_arm = support_motion.require_target(JointPositionTarget) placing_hand = placing_grasp.require_target(JointPositionTarget) support_hand = support_grasp.require_target(JointPositionTarget) + placing_task_state_key = require_shared_task_state_key( + placing_motion, + placing_grasp, + participant="CoordinatedPlacement placing participant", + ) + support_task_state_key = require_shared_task_state_key( + support_motion, + support_grasp, + participant="CoordinatedPlacement support participant", + ) + if placing_task_state_key == support_task_state_key: + raise ValueError( + "CoordinatedPlacement placing and support participants must " + "use different task_state_key values." + ) if placing_arm.control_part == support_arm.control_part: raise ValueError( "CoordinatedPlacement placing and support roles must use " @@ -202,6 +204,8 @@ def _resolve_resources( "different end-effector control parts." ) return _CoordinatedPlacementResources( + placing_task_state_key=placing_task_state_key, + support_task_state_key=support_task_state_key, placing_arm=placing_arm, support_arm=support_arm, placing_hand=placing_hand, @@ -253,8 +257,8 @@ def _plan( support_held_object, ) = self._resolve_target(target, state, resources, options) eligible = context.task.exclusive_held_object_mask( - resources.placing_arm.control_part - ) & context.task.exclusive_held_object_mask(resources.support_arm.control_part) + resources.placing_task_state_key + ) & context.task.exclusive_held_object_mask(resources.support_task_state_key) if not eligible.any(): logger.log_warning( "CoordinatedPlacement requires two exclusively held objects." @@ -404,6 +408,15 @@ def _plan( ], dim=1, ) + involved_task_state_keys = { + resources.placing_task_state_key, + resources.support_task_state_key, + } + coordinated_removals = { + key: None + for key in state.task.coordinated_held_objects + if not involved_task_state_keys.isdisjoint(key) + } return self.build_plan( request, context, @@ -415,10 +428,10 @@ def _plan( ), expected_effects=StateDelta( held_object_updates={ - resources.placing_arm.control_part: ( + resources.placing_task_state_key: ( None if release else placing_held_object ), - resources.support_arm.control_part: support_held_object, + resources.support_task_state_key: support_held_object, }, ), segment_lengths={ @@ -497,19 +510,21 @@ def _resolve_target( HeldObjectState, HeldObjectState, ]: - placing_control_part = resources.placing_arm.control_part - support_control_part = resources.support_arm.control_part - placing_held_object = state.get_held_object(placing_control_part) + placing_task_state_key = resources.placing_task_state_key + support_task_state_key = resources.support_task_state_key + placing_held_object = state.get_held_object(placing_task_state_key) if placing_held_object is None: - raise ValueError( - "CoordinatedPlacement requires an object held by placing control " - f"part {placing_control_part!r}." + logger.log_error( + "CoordinatedPlacement requires an object held by placing " + f"task-state resource {placing_task_state_key!r}.", + ValueError, ) - support_held_object = state.get_held_object(support_control_part) + support_held_object = state.get_held_object(support_task_state_key) if support_held_object is None: - raise ValueError( - "CoordinatedPlacement requires an object held by support control " - f"part {support_control_part!r}." + logger.log_error( + "CoordinatedPlacement requires an object held by support " + f"task-state resource {support_task_state_key!r}.", + ValueError, ) placing_height_offset = ( options.placing_height_offset diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index ff97a27b9..3a06dce1b 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -64,16 +64,15 @@ interpolate_hand_qpos, translate_pose_world, ) -from embodichain.lab.sim.atomic_actions.primitives._helpers import ( +from ._binding_contracts import make_manipulation_slot +from ._helpers import ( assemble_full_robot_trajectory, plan_named_arm_trajectory, repeat_qpos, + require_shared_task_state_key, resolve_batched_pose, ) -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, -) -from embodichain.lab.sim.atomic_actions.primitives.pick_up import GraspGoal +from .pick_up import GraspGoal @dataclass(frozen=True, slots=True, eq=False) @@ -158,6 +157,8 @@ def __post_init__(self) -> None: class _HandOverResources: """Invocation-bound control parts and compatible hand commands.""" + transfer_task_state_key: str + receive_task_state_key: str transfer_arm: JointPositionTarget receive_arm: JointPositionTarget transfer_hand: JointPositionTarget @@ -238,6 +239,21 @@ def _resolve_resources( receive_arm = receive_motion.require_target(JointPositionTarget) transfer_hand = transfer_grasp.require_target(JointPositionTarget) receive_hand = receive_grasp.require_target(JointPositionTarget) + transfer_task_state_key = require_shared_task_state_key( + transfer_motion, + transfer_grasp, + participant="HandOver source participant", + ) + receive_task_state_key = require_shared_task_state_key( + receive_motion, + receive_grasp, + participant="HandOver destination participant", + ) + if transfer_task_state_key == receive_task_state_key: + raise ValueError( + "HandOver source and destination must use different " + "task_state_key values." + ) if transfer_arm.control_part == receive_arm.control_part: raise ValueError( "HandOver source and destination must use different manipulator " @@ -249,6 +265,8 @@ def _resolve_resources( "control parts." ) return _HandOverResources( + transfer_task_state_key=transfer_task_state_key, + receive_task_state_key=receive_task_state_key, transfer_arm=transfer_arm, receive_arm=receive_arm, transfer_hand=transfer_hand, @@ -301,26 +319,21 @@ def _plan( "Coordinated dual-arm planning is not supported by the cuRobo backend." ) state = context - transfer_control_part = resources.transfer_arm.control_part - transfer_held_object = self._resolve_transfer_held_object( - state, transfer_control_part + semantics = target.semantics + transfer_object_to_eef = self._resolve_transfer_object_to_eef( + state, + resources.transfer_task_state_key, + semantics, ) - self._validate_requested_object( - target.semantics, transfer_held_object.semantics + eligible = context.task.exclusive_held_object_mask( + resources.transfer_task_state_key ) - semantics = transfer_held_object.semantics - eligible = context.task.exclusive_held_object_mask(transfer_control_part) if not eligible.any(): - logger.log_warning("HandOver requires an exclusively held source object.") return self.failed_plan( request, context, message="Source object must be held exclusively.", ) - transfer_object_to_eef = self._resolve_matrix( - transfer_held_object.object_to_eef, - "held_object.object_to_eef", - ) transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( state, resources, @@ -615,8 +628,8 @@ def _plan( ), expected_effects=StateDelta( held_object_updates={ - resources.transfer_arm.control_part: None, - resources.receive_arm.control_part: held_object, + resources.transfer_task_state_key: None, + resources.receive_task_state_key: held_object, } ), segment_lengths=segment_lengths, @@ -640,30 +653,24 @@ def _resolve_matrix(self, matrix: torch.Tensor, name: str) -> torch.Tensor: name=name, ) - def _resolve_transfer_held_object( + def _resolve_transfer_object_to_eef( self, state: PlanningContext, - transfer_control_part: str, - ) -> HeldObjectState: - held = state.get_held_object(transfer_control_part) + transfer_task_state_key: str, + target_semantics: ObjectSemantics, + ) -> torch.Tensor: + held = state.get_held_object(transfer_task_state_key) if held is None: raise ValueError( - "HandOver requires an object held by transfer control part " - f"{transfer_control_part!r} (run PickUp first)." + "HandOver requires an object held by source task-state resource " + f"{transfer_task_state_key!r} (run PickUp first)." ) - return held - - @staticmethod - def _validate_requested_object( - requested: ObjectSemantics, - held: ObjectSemantics, - ) -> None: - """Reject a request that names a different grounded object.""" - if not _same_object_identity(requested, held): + if not _same_object_identity(target_semantics, held.semantics): raise ValueError( - "HandOver goal semantics must identify the object held by the " - "source control part." + "HandOver target semantics must identify the object held by " + f"source task-state resource {transfer_task_state_key!r}." ) + return self._resolve_matrix(held.object_to_eef, "held_object.object_to_eef") def _resolve_receive_grasp( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py index b087951e5..44e388357 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -29,36 +29,29 @@ pose_inv, ) -from embodichain.lab.sim.atomic_actions.primitives._helpers import ( +from ._helpers import ( arm_qpos_from_state, + require_shared_task_state_key, resolve_object_target, ) -from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget -from embodichain.lab.sim.atomic_actions.control import ( - GRASP_COMMAND, - JointPositionCommand, -) -from embodichain.lab.sim.atomic_actions.core import AtomicAction -from embodichain.lab.sim.atomic_actions.goals import ( - PoseGoalValue, - resolve_pose_goal, - validate_pose_goal, -) -from embodichain.lab.sim.atomic_actions.invocation import ( - ActionOptions, - ResolvedActionRequest, -) -from embodichain.lab.sim.atomic_actions.plans import ActionPlan, TimedTrajectory -from embodichain.lab.sim.atomic_actions.requirements import ( +from ._binding_contracts import make_manipulation_slot +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, JointPositionCommand +from ..core import AtomicAction +from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal +from ..invocation import ActionOptions, ResolvedActionRequest +from ..plans import ActionPlan, TimedTrajectory +from ..requirements import ( CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, ) -from embodichain.lab.sim.atomic_actions.state import PlanningContext -from embodichain.lab.sim.atomic_actions.trajectory_ops import build_pose_plan_states -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, -) +from ..state import PlanningContext +from ..trajectory_ops import build_pose_plan_states @dataclass(frozen=True, slots=True, eq=False) @@ -134,6 +127,11 @@ def _plan( grasp = binding.endpoint("primary", "grasp") motion_target = motion.require_target(JointPositionTarget) grasp_target = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="MoveHeldObject primary participant", + ) control_part = motion_target.control_part arm_joint_ids = list(motion_target.joint_ids) hand_joint_ids = list(grasp_target.joint_ids) @@ -144,18 +142,18 @@ def _plan( dtype=context.robot.qpos.dtype, ) state = context - held_object = state.get_held_object(control_part) + held_object = state.get_held_object(task_state_key) if held_object is None: raise ValueError( - "MoveHeldObject requires an object held by control part " - f"{control_part!r} - run PickUp first." + "MoveHeldObject requires an object held by task-state resource " + f"{task_state_key!r} - run PickUp first." ) - eligible = context.task.exclusive_held_object_mask(control_part) + eligible = context.task.exclusive_held_object_mask(task_state_key) if not eligible.any(): return self.failed_plan( request, context, - message="Held object is not exclusive to the control part.", + message="Held object is not exclusive to the task-state resource.", ) object_target_pose = resolve_object_target( resolve_pose_goal( diff --git a/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py b/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py new file mode 100644 index 000000000..ec00958f7 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py @@ -0,0 +1,478 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Reusable contact-and-drag operation for articulated mechanisms.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import ClassVar + +import torch + +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand +from ..core import AtomicAction +from ..effects import StateDelta +from ..goals import SceneArticulationOperationGeometry +from ..invocation import ActionOptions, ResolvedActionRequest +from ..plans import ( + ActionPlan, + EffectVerificationRequirement, + PlannerDiagnostics, + TimedTrajectory, +) +from ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) +from ..state import ArticulationJointState, PlanningContext +from ..trajectory_ops import ( + build_pose_plan_states, + interpolate_hand_qpos, + resolve_pose_target, +) + + +def _validate_identifier(value: str, *, field_name: str) -> None: + """Validate one canonical articulation identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty canonical identifier.") + + +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulationGoal: + """Grounded interaction path and desired state for one articulation joint. + + The semantic compiler copies immutable affordance geometry and records the + live source joint position. The atomic planner combines those values with + the latest handle and joint observation, so the same resolved request can + safely replan drawers, doors, sliders, and similar interactions. + """ + + goal_kind: ClassVar[str] = "operate_articulation" + + articulation_id: str + """Canonical scene-registry articulation identifier.""" + + joint_id: str + """Canonical joint identifier within the articulation.""" + + geometry: SceneArticulationOperationGeometry + """Handle-relative geometry resolved again for every plan and replan.""" + + source_position: torch.Tensor + """Live joint position at semantic grounding, shape ``(1,)`` or ``(B, 1)``.""" + + target_position: torch.Tensor + """Absolute desired joint position, shape ``(1,)`` or ``(B, 1)``.""" + + target_displacement: float + """Signed handle displacement from source position to target position.""" + + def __post_init__(self) -> None: + _validate_identifier(self.articulation_id, field_name="articulation_id") + _validate_identifier(self.joint_id, field_name="joint_id") + if not isinstance(self.geometry, SceneArticulationOperationGeometry): + raise TypeError("geometry must be a SceneArticulationOperationGeometry.") + for field_name in ("source_position", "target_position"): + position = getattr(self, field_name) + if not isinstance(position, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if position.dim() not in (1, 2) or position.shape[-1:] != (1,): + raise ValueError(f"{field_name} must have shape (1,) or (B, 1).") + if not position.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + if not torch.isfinite(position).all(): + raise ValueError(f"{field_name} must contain only finite values.") + object.__setattr__(self, field_name, position.clone()) + displacement = self.target_displacement + if isinstance(displacement, bool) or not isinstance(displacement, (int, float)): + raise TypeError("target_displacement must be a finite scalar.") + displacement = float(displacement) + if not math.isfinite(displacement): + raise ValueError("target_displacement must be finite.") + object.__setattr__(self, "target_displacement", displacement) + + +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulationOptions(ActionOptions): + """Per-invocation contact sequencing for articulation operations.""" + + engage_steps: int = 5 + """Number of gripper-closing waypoints at the contact pose.""" + + release_steps: int = 5 + """Number of gripper-opening waypoints before retracting.""" + + def __post_init__(self) -> None: + for field_name in ("engage_steps", "release_steps"): + value = getattr(self, field_name) + if type(value) is not int or value <= 0: + raise ValueError(f"{field_name} must be a positive integer.") + + +class OperateArticulation( + AtomicAction[OperateArticulationGoal, OperateArticulationOptions] +): + """Approach, engage, move, release, and retract an articulated affordance.""" + + skill_id: ClassVar[str] = "operate_articulation" + GoalType: ClassVar[type] = OperateArticulationGoal + OptionsType: ClassVar[type] = OperateArticulationOptions + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + SkillEndpointRequirement( + endpoint_id="interaction", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + GRASP_COMMAND: JointPositionCommand, + OPEN_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "interaction")),), + ), + ), + ) + + def __init__( + self, + default_options: OperateArticulationOptions | None = None, + ) -> None: + super().__init__(default_options) + + def _on_bind(self) -> None: + """Capture immutable robot dimensions from engine-owned services.""" + self.n_envs = int(self.robot.get_qpos().shape[0]) + self.robot_dof = int(self.robot.dof) + + def _plan( + self, + request: ResolvedActionRequest[ + OperateArticulationGoal, + OperateArticulationOptions, + ], + context: PlanningContext, + ) -> ActionPlan: + """Plan the complete contact interaction from one observed context.""" + goal = self.require_goal(request) + options = request.skill_options + motion = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + interaction = request.binding.endpoint("primary", "interaction") + interaction_target = interaction.require_target(JointPositionTarget) + arm_joint_ids = list(motion.joint_ids) + interaction_joint_ids = list(interaction_target.joint_ids) + + remaining_displacement = self._remaining_displacement(goal, context) + poses = tuple( + resolve_pose_target( + pose, + num_envs=context.batch_size, + device=self.device, + ) + for pose in goal.geometry.resolve( + context, + displacement=remaining_displacement, + ) + ) + motion_counts = self._motion_sample_counts( + request.motion_policy.sample_count, + options, + ) + arm_segments: list[torch.Tensor] = [] + phase_diagnostics: dict[str, dict[str, object]] = {} + arm_start = context.robot.qpos[:, arm_joint_ids] + success = torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ) + phase_names = ("approach", "contact", "operate", "retract") + for phase_name, pose, sample_count in zip( + phase_names, + poses, + motion_counts, + strict=True, + ): + result = self.motion_generator.generate( + build_pose_plan_states(pose), + options=request.motion_policy.to_motion_gen_options( + start_qpos=arm_start, + control_part=motion.control_part, + sample_count=sample_count, + interpolation_dt=context.require_control_dt(), + ), + ) + if result.positions is None or not isinstance(result.success, torch.Tensor): + return self.failed_plan( + request, + context, + message=( + "The articulation motion planner returned no trajectory for " + f"phase {phase_name!r}." + ), + ) + phase_success = result.success.to( + device=success.device, + dtype=torch.bool, + ) + failed_rows = ( + (~phase_success).nonzero(as_tuple=False).flatten().detach().cpu() + ) + phase_diagnostics[phase_name] = { + "success": phase_success.detach().cpu().tolist(), + "failed_rows": failed_rows.tolist(), + "waypoint_count": int(result.positions.shape[1]), + } + arm_segments.append(result.positions) + arm_start = result.positions[:, -1] + success &= phase_success + + grasp_qpos = interaction.joint_positions( + GRASP_COMMAND, + num_envs=self.num_envs, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + open_qpos = interaction.joint_positions( + OPEN_COMMAND, + num_envs=self.num_envs, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + initial_interaction = context.robot.qpos[:, interaction_joint_ids] + engage_path = interpolate_hand_qpos( + initial_interaction, + grasp_qpos, + n_waypoints=options.engage_steps, + ) + release_path = interpolate_hand_qpos( + grasp_qpos, + open_qpos, + n_waypoints=options.release_steps, + ) + + approach_arm, contact_arm, operation_arm, retract_arm = arm_segments + lengths = { + "approach": int(approach_arm.shape[1]), + "engage": int(contact_arm.shape[1] + engage_path.shape[1]), + "operate": int(operation_arm.shape[1]), + "release": int(release_path.shape[1]), + "retract": int(retract_arm.shape[1]), + } + full = torch.empty( + ( + context.batch_size, + sum(lengths.values()), + self.robot_dof, + ), + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + full[:] = context.robot.qpos.unsqueeze(1) + cursor = 0 + + def append_motion( + segment: torch.Tensor, + interaction_qpos: torch.Tensor, + ) -> None: + nonlocal cursor + count = int(segment.shape[1]) + full[:, cursor : cursor + count, arm_joint_ids] = segment + full[:, cursor : cursor + count, interaction_joint_ids] = ( + interaction_qpos.unsqueeze(1) + ) + cursor += count + + append_motion(approach_arm, initial_interaction) + append_motion(contact_arm, initial_interaction) + full[:, cursor : cursor + options.engage_steps, arm_joint_ids] = contact_arm[ + :, -1 + ].unsqueeze(1) + full[:, cursor : cursor + options.engage_steps, interaction_joint_ids] = ( + engage_path + ) + cursor += options.engage_steps + append_motion(operation_arm, grasp_qpos) + full[:, cursor : cursor + options.release_steps, arm_joint_ids] = operation_arm[ + :, -1 + ].unsqueeze(1) + full[:, cursor : cursor + options.release_steps, interaction_joint_ids] = ( + release_path + ) + cursor += options.release_steps + append_motion(retract_arm, open_qpos) + assert cursor == full.shape[1] + + target_position = goal.target_position.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + expected = StateDelta( + articulation_joint_updates={ + (goal.articulation_id, goal.joint_id): ArticulationJointState( + target_position + ) + } + ) + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + expected_effects=expected, + effect_verification=EffectVerificationRequirement( + kind="articulation.joint_progress" + ), + segment_lengths=lengths, + scene_dependency_monitor_until={ + goal.geometry.handle_pose.entity_id: lengths["approach"] + + lengths["engage"] + }, + diagnostics=PlannerDiagnostics( + backend=self.planning_services.planner_name, + messages=tuple( + f"Articulation motion phase {phase_name!r} failed for rows " + f"{details['failed_rows']}." + for phase_name, details in phase_diagnostics.items() + if details["failed_rows"] + ), + metadata={"motion_phases": phase_diagnostics}, + ), + ) + + @staticmethod + def _position_batch( + value: torch.Tensor, + context: PlanningContext, + *, + field_name: str, + ) -> torch.Tensor: + """Broadcast one scalar joint position to the planning batch.""" + position = value.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if position.shape == (1,): + return position.unsqueeze(0).expand(context.batch_size, -1).clone() + if position.shape != (context.batch_size, 1): + raise ValueError( + f"{field_name} must have shape (1,) or " f"({context.batch_size}, 1)." + ) + return position.clone() + + @classmethod + def _remaining_displacement( + cls, + goal: OperateArticulationGoal, + context: PlanningContext, + ) -> torch.Tensor: + """Map remaining joint stroke to a bounded signed handle displacement. + + For each row, ``remaining = target_displacement * clamp( + (target - current) / (target - source), 0, 1)``. A zero-length source + stroke, a reached target, and an overshot target all resolve to zero. + """ + observed = context.scene.get_articulation_joint_state( + goal.articulation_id, + goal.joint_id, + ) + address = (goal.articulation_id, goal.joint_id) + if observed is None: + raise ValueError( + "OperateArticulation recovery-safe planning requires a live " + f"ObservedArticulationJointState for {address!r}." + ) + current = cls._position_batch( + observed.position, + context, + field_name=f"observed articulation joint {address!r}", + ) + if observed.valid_mask is not None: + valid = observed.valid_mask.to(device=context.robot.qpos.device) + if not bool(valid.all()): + invalid_rows = (~valid).nonzero(as_tuple=False).flatten().tolist() + raise ValueError( + f"Live articulation joint {address!r} is invalid for planning " + f"rows {invalid_rows}." + ) + source = cls._position_batch( + goal.source_position, + context, + field_name="source_position", + ) + target = cls._position_batch( + goal.target_position, + context, + field_name="target_position", + ) + total = target - source + tolerance = torch.finfo(total.dtype).eps * 16.0 + nonzero_stroke = total.abs() > tolerance + fraction = torch.zeros_like(total) + fraction[nonzero_stroke] = ( + (target - current)[nonzero_stroke] / total[nonzero_stroke] + ).clamp(0.0, 1.0) + return fraction[:, 0] * goal.target_displacement + + @staticmethod + def _motion_sample_counts( + sample_count: int, + options: OperateArticulationOptions, + ) -> tuple[int, int, int, int]: + """Allocate the preset sample budget across four motion phases.""" + remaining = sample_count - options.engage_steps - options.release_steps + if remaining < 8: + raise ValueError( + "MotionPolicy.sample_count must leave at least two waypoints for " + "each articulation motion phase." + ) + base, remainder = divmod(remaining, 4) + counts = tuple(base + (1 if index < remainder else 0) for index in range(4)) + assert len(counts) == 4 and all(value >= 2 for value in counts) + return counts + + +__all__ = [ + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", +] diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index a1a381f32..93f90c683 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -32,17 +32,13 @@ quat_from_matrix, ) -from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state -from embodichain.lab.sim.atomic_actions.affordance import AntipodalAffordance -from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget -from embodichain.lab.sim.atomic_actions.control import ( - GRASP_COMMAND, - OPEN_COMMAND, - JointPositionCommand, -) -from embodichain.lab.sim.atomic_actions.core import AtomicAction, ObjectSemantics -from embodichain.lab.sim.atomic_actions.effects import StateDelta -from embodichain.lab.sim.atomic_actions.goals import ( +from ._helpers import arm_qpos_from_state, require_shared_task_state_key +from ..affordance import AntipodalAffordance +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand +from ..core import AtomicAction, ObjectSemantics +from ..effects import StateDelta +from ..goals import ( ObjectActionGoal, PoseGoalValue, _resolve_object_pose, @@ -346,6 +342,11 @@ def _plan( grasp = binding.endpoint("primary", "grasp") manipulator = motion.require_target(JointPositionTarget) end_effector = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="PickUp primary participant", + ) hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, num_envs=context.batch_size, @@ -434,6 +435,11 @@ def _plan( held = HeldObjectState( semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos ) + coordinated_updates = { + key: None + for key in state.task.coordinated_held_objects + if task_state_key in key + } return self.build_plan( request, context, @@ -443,16 +449,15 @@ def _plan( env_ids=context.env_ids, step_dt=context.require_control_dt(), ), - expected_effects=StateDelta(held_object_updates={control_part: held}), + expected_effects=StateDelta( + held_object_updates={task_state_key: held}, + coordinated_held_object_updates=coordinated_updates, + ), segment_lengths=segment_lengths, - # Once the approach is dispatched the object can move because of - # contact or grasping. That self-induced motion must not look like - # an external dynamic-goal update. - scene_dependency_end_segment=( - "approach" - if segment_lengths.get("approach", 0) > 0 - and self._scene_dependencies(request) - else None + scene_dependency_monitor_until=( + {} + if sem.entity_id is None + else {sem.entity_id: segment_lengths["approach"]} ), ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index 8d744afe0..7ba5c0d63 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -26,20 +26,17 @@ from embodichain.utils.math import quat_error_magnitude, quat_from_matrix -from embodichain.lab.sim.atomic_actions.primitives._helpers import ( +from ._helpers import ( arm_qpos_from_state, + require_shared_task_state_key, resolve_object_target, ) -from embodichain.lab.sim.atomic_actions.affordance import AssembleAffordance -from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget -from embodichain.lab.sim.atomic_actions.control import ( - GRASP_COMMAND, - OPEN_COMMAND, - JointPositionCommand, -) -from embodichain.lab.sim.atomic_actions.core import AtomicAction -from embodichain.lab.sim.atomic_actions.effects import StateDelta -from embodichain.lab.sim.atomic_actions.goals import ( +from ..affordance import AssembleAffordance +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand +from ..core import AtomicAction +from ..effects import StateDelta +from ..goals import ( PoseGoalValue, SceneEntityPose, resolve_pose_goal, @@ -212,11 +209,15 @@ def _plan( target = request.goal options = request.skill_options binding = request.binding - motion_target = binding.endpoint("primary", "motion").require_target( - JointPositionTarget - ) + motion = binding.endpoint("primary", "motion") grasp = binding.endpoint("primary", "grasp") + motion_target = motion.require_target(JointPositionTarget) grasp_target = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="Place primary participant", + ) control_part = motion_target.control_part arm_joint_ids = list(motion_target.joint_ids) hand_joint_ids = list(grasp_target.joint_ids) @@ -233,19 +234,19 @@ def _plan( dtype=context.robot.qpos.dtype, ) state = context - held_mask = context.task.held_object_mask(control_part) - exclusive_mask = context.task.exclusive_held_object_mask(control_part) + held_mask = context.task.held_object_mask(task_state_key) + exclusive_mask = context.task.exclusive_held_object_mask(task_state_key) eligible = ( exclusive_mask if isinstance(target, AssembleGoal) else ~held_mask | exclusive_mask ) - place_xpos = self._resolve_place_xpos(target, state, control_part) + place_xpos = self._resolve_place_xpos(target, state, task_state_key) if not eligible.any(): return self.failed_plan( request, context, - message="Held object is shared with another control part.", + message="Place requires an exclusive held-object relation.", ) if place_xpos.dim() == 3: place_xpos = place_xpos.unsqueeze(1) @@ -329,6 +330,11 @@ def _plan( full[:, n_down_actual + n_open :, arm_joint_ids] = back_arm full[:, n_down_actual + n_open :, hand_joint_ids] = hand_open_qpos.unsqueeze(1) + coordinated_updates = { + key: None + for key in state.task.coordinated_held_objects + if task_state_key in key + } return self.build_plan( request, context, @@ -338,7 +344,10 @@ def _plan( env_ids=context.env_ids, step_dt=context.require_control_dt(), ), - expected_effects=StateDelta(held_object_updates={control_part: None}), + expected_effects=StateDelta( + held_object_updates={task_state_key: None}, + coordinated_held_object_updates=coordinated_updates, + ), segment_lengths={ "approach": n_down_actual, "release": n_open, @@ -350,13 +359,14 @@ def _resolve_place_xpos( self, target: PlaceGoal | AssembleGoal, state: PlanningContext, - control_part: str, + task_state_key: str, ) -> torch.Tensor: """Resolve the place EEF poses from a typed target. Args: target: Either an explicit EEF pose target or an assembly target. state: World state carrying the held-object transform. + task_state_key: Stable logical resource used for held-object state. Returns: Place EEF poses with shape ``(num_envs, 4, 4)`` or @@ -368,13 +378,13 @@ def _resolve_place_xpos( num_envs=self.num_envs, device=self.device, ) - return self._resolve_assemble_place_xpos(target, state, control_part) + return self._resolve_assemble_place_xpos(target, state, task_state_key) def _resolve_assemble_place_xpos( self, target: AssembleGoal, state: PlanningContext, - control_part: str, + task_state_key: str, ) -> torch.Tensor: """Derive the place EEF pose from an assembly affordance. @@ -385,6 +395,7 @@ def _resolve_assemble_place_xpos( Args: target: Assembly target carrying the base/assemble affordance. state: World state carrying the held-object transform. + task_state_key: Stable logical resource used for held-object state. Returns: Place EEF poses with shape ``(num_envs, 4, 4)``. @@ -392,11 +403,11 @@ def _resolve_assemble_place_xpos( Raises: ValueError: If no held object or base-pose source is available. """ - held = state.get_held_object(control_part) + held = state.get_held_object(task_state_key) if held is None: raise ValueError( - "Place with AssembleGoal requires an object held by control " - f"part {control_part!r} (run PickUp first)." + "Place with AssembleGoal requires an object held by task-state " + f"resource {task_state_key!r} (run PickUp first)." ) affordance = target.affordance if target.base_pose is not None: diff --git a/embodichain/lab/sim/atomic_actions/runtime.py b/embodichain/lab/sim/atomic_actions/runtime.py index c0530db40..13228ff37 100644 --- a/embodichain/lab/sim/atomic_actions/runtime.py +++ b/embodichain/lab/sim/atomic_actions/runtime.py @@ -98,12 +98,25 @@ def bind_control_parts( self, contract: SkillBindingContract, endpoints: Mapping[str, Mapping[str, str]], + *, + task_state_keys: Mapping[str, str] | None = None, ) -> ActionBinding: """Build a generic binding from explicit robot control-part names. This is the advanced direct-core construction path. Profile-backed callers obtain the same :class:`ActionBinding` from ``BoundRobotSkillProfile.resolve()``. + + Args: + contract: Typed endpoint contract for the bound skill. + endpoints: Nested ``slot_id -> endpoint_id -> control_part`` mapping. + task_state_keys: Optional explicit stable task-state key for each + resource slot. When omitted, a slot inherits its ``motion`` + endpoint's control part. A slot without ``motion`` can be + inferred only when all of its endpoints use one control part. + + Returns: + Engine-owned generic endpoint binding. """ if not isinstance(contract, SkillBindingContract): raise TypeError("contract must be a SkillBindingContract.") @@ -138,6 +151,36 @@ def bind_control_parts( "Direct binding must cover the skill contract exactly: " f"missing={missing}, extra={extra}." ) + slot_ids = {slot.slot_id for slot in contract.slots} + if task_state_keys is not None: + if not isinstance(task_state_keys, Mapping): + raise TypeError("task_state_keys must be a slot-to-key mapping.") + for slot_id, task_state_key in task_state_keys.items(): + if ( + not isinstance(slot_id, str) + or not slot_id + or slot_id != slot_id.strip() + ): + raise ValueError( + "task_state_keys slot IDs must be non-empty strings " + "without outer whitespace." + ) + if not isinstance(task_state_key, str) or not task_state_key.strip(): + raise ValueError( + "task_state_keys values must be non-empty strings." + ) + if task_state_key != task_state_key.strip(): + raise ValueError( + "task_state_keys values must not contain outer whitespace." + ) + supplied_task_slots = set(task_state_keys) + if supplied_task_slots != slot_ids: + missing = sorted(slot_ids - supplied_task_slots) + extra = sorted(supplied_task_slots - slot_ids) + raise ValueError( + "task_state_keys must cover the binding slots exactly: " + f"missing={missing}, extra={extra}." + ) if not expected: binding = ActionBinding(owner_id=self.binding_owner_id) self.validate_binding(binding, contract) @@ -147,6 +190,27 @@ def bind_control_parts( if not isinstance(control_parts, Mapping): raise TypeError("Direct control-part binding requires Robot.control_parts.") available = sorted(str(name) for name in control_parts) + resolved_task_state_keys: dict[str, str] + if task_state_keys is not None: + resolved_task_state_keys = dict(task_state_keys) + else: + resolved_task_state_keys = {} + for slot in contract.slots: + motion_key = (slot.slot_id, "motion") + if motion_key in supplied: + resolved_task_state_keys[slot.slot_id] = supplied[motion_key] + continue + slot_control_parts = { + supplied[(slot.slot_id, endpoint.endpoint_id)] + for endpoint in slot.endpoints + } + if len(slot_control_parts) != 1: + raise ValueError( + f"Direct binding slot {slot.slot_id!r} has no 'motion' " + "endpoint and spans multiple control parts; provide an " + "explicit task_state_keys entry for this slot." + ) + resolved_task_state_keys[slot.slot_id] = next(iter(slot_control_parts)) resolved: list[EndpointBinding] = [] for key, requirement in expected.items(): slot_id, endpoint_id = key @@ -175,6 +239,7 @@ def bind_control_parts( resource_id=f"direct.{slot_id}", adapter_id="control_part", target=JointPositionTarget(control_part, joint_ids), + task_state_key=resolved_task_state_keys[slot_id], capabilities=requirement.capabilities, commands=commands, claim_tokens=frozenset({f"robot.control_part:{control_part}"}), diff --git a/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index 1cfd4af45..6b10d3a7a 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -103,6 +103,80 @@ def _broadcast_pose( return value.clone() +def _broadcast_joint_position( + value: torch.Tensor, + *, + batch_size: int, + device: torch.device, + name: str, +) -> torch.Tensor: + """Resolve an optionally batched joint-position value to a task batch.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dim() == 1: + if value.numel() == 0: + raise ValueError(f"{name} must contain at least one joint value.") + value = value.unsqueeze(0).expand(batch_size, -1) + elif value.dim() != 2 or value.shape[0] != batch_size or value.shape[1] == 0: + raise ValueError( + f"{name} must have shape (n_joints,) or " f"({batch_size}, n_joints)." + ) + if not value.is_floating_point(): + raise TypeError(f"{name} must use a floating-point dtype.") + if value.device != device: + raise ValueError(f"{name} must use task-state device {device}.") + if not torch.isfinite(value).all(): + raise ValueError(f"{name} must contain only finite values.") + return value.clone() + + +@dataclass(frozen=True, slots=True, eq=False) +class ArticulationJointState: + """Verified symbolic state for one named articulation joint. + + ``position`` may describe one scalar joint or a multi-DoF joint. The + surrounding :class:`TaskState` supplies the stable articulation/joint key; + this value only owns row-local verified measurements and activity. + """ + + position: torch.Tensor + """Joint positions with shape ``(J,)`` or ``(B, J)``.""" + + env_mask: torch.Tensor | None = None + """Rows for which the verified state is present.""" + + def __post_init__(self) -> None: + if not isinstance(self.position, torch.Tensor): + raise TypeError("ArticulationJointState.position must be a tensor.") + if self.position.dim() not in (1, 2) or self.position.numel() == 0: + raise ValueError( + "ArticulationJointState.position must have shape (J,) or (B, J)." + ) + if not self.position.is_floating_point(): + raise TypeError("ArticulationJointState.position must be floating point.") + if not torch.isfinite(self.position).all(): + raise ValueError("ArticulationJointState.position must be finite.") + object.__setattr__(self, "position", self.position.clone()) + if self.env_mask is not None: + batch_size = int(self.position.shape[0]) if self.position.dim() == 2 else -1 + if batch_size <= 0: + if self.env_mask.dim() != 1 or self.env_mask.numel() == 0: + raise ValueError( + "ArticulationJointState.env_mask must be a non-empty vector." + ) + batch_size = int(self.env_mask.shape[0]) + object.__setattr__( + self, + "env_mask", + _normalize_mask( + self.env_mask, + batch_size=batch_size, + device=self.position.device, + name="ArticulationJointState.env_mask", + ), + ) + + @dataclass(frozen=True, slots=True, eq=False) class HeldObjectState: """Observed or projected relation between an object and one manipulator.""" @@ -148,6 +222,49 @@ def __post_init__(self) -> None: ) +@dataclass(frozen=True, slots=True, eq=False) +class CoordinatedHeldObjectState: + """Observed or projected relation for an object held by two manipulators.""" + + semantics: ObjectSemantics + left_object_to_eef: torch.Tensor + right_object_to_eef: torch.Tensor + left_grasp_xpos: torch.Tensor + right_grasp_xpos: torch.Tensor + env_mask: torch.Tensor | None = None + + def __post_init__(self) -> None: + from .core import ObjectSemantics + + if not isinstance(self.semantics, ObjectSemantics): + raise TypeError("semantics must be an ObjectSemantics instance.") + poses = { + "left_object_to_eef": self.left_object_to_eef, + "right_object_to_eef": self.right_object_to_eef, + "left_grasp_xpos": self.left_grasp_xpos, + "right_grasp_xpos": self.right_grasp_xpos, + } + batches = {_validate_pose(value, name) for name, value in poses.items()} + batches.discard(None) + if len(batches) > 1: + raise ValueError("Coordinated held-object poses must share a batch size.") + if len({value.device for value in poses.values()}) != 1: + raise ValueError("Coordinated held-object poses must share a device.") + if self.env_mask is not None: + mask_batch = int(self.env_mask.shape[0]) if self.env_mask.dim() == 1 else -1 + batch_size = next(iter(batches), mask_batch) + object.__setattr__( + self, + "env_mask", + _normalize_mask( + self.env_mask, + batch_size=batch_size, + device=self.left_object_to_eef.device, + name="env_mask", + ), + ) + + def _normalize_held( value: HeldObjectState, *, @@ -178,6 +295,71 @@ def _normalize_held( ) +def _normalize_coordinated_held( + value: CoordinatedHeldObjectState, + *, + batch_size: int, + device: torch.device, +) -> CoordinatedHeldObjectState: + """Normalize a coordinated relation to one task-state batch.""" + return CoordinatedHeldObjectState( + semantics=value.semantics, + left_object_to_eef=_broadcast_pose( + value.left_object_to_eef, + batch_size=batch_size, + device=device, + name="CoordinatedHeldObjectState.left_object_to_eef", + ), + right_object_to_eef=_broadcast_pose( + value.right_object_to_eef, + batch_size=batch_size, + device=device, + name="CoordinatedHeldObjectState.right_object_to_eef", + ), + left_grasp_xpos=_broadcast_pose( + value.left_grasp_xpos, + batch_size=batch_size, + device=device, + name="CoordinatedHeldObjectState.left_grasp_xpos", + ), + right_grasp_xpos=_broadcast_pose( + value.right_grasp_xpos, + batch_size=batch_size, + device=device, + name="CoordinatedHeldObjectState.right_grasp_xpos", + ), + env_mask=_normalize_mask( + value.env_mask, + batch_size=batch_size, + device=device, + name="CoordinatedHeldObjectState.env_mask", + ), + ) + + +def _normalize_articulation_joint( + value: ArticulationJointState, + *, + batch_size: int, + device: torch.device, +) -> ArticulationJointState: + """Normalize one articulation-joint state to a task-state batch.""" + return ArticulationJointState( + position=_broadcast_joint_position( + value.position, + batch_size=batch_size, + device=device, + name="ArticulationJointState.position", + ), + env_mask=_normalize_mask( + value.env_mask, + batch_size=batch_size, + device=device, + name="ArticulationJointState.env_mask", + ), + ) + + @dataclass(frozen=True, slots=True, eq=False) class TaskState: """Symbolic task state, separate from measured robot state.""" @@ -189,7 +371,17 @@ class TaskState: """Device used by per-environment masks and relation tensors.""" held_objects: Mapping[str, HeldObjectState] = field(default_factory=dict) - """Single-manipulator held-object relations keyed by control resource.""" + """Held-object relations keyed by stable logical task-state resource.""" + + coordinated_held_objects: Mapping[tuple[str, str], CoordinatedHeldObjectState] = ( + field(default_factory=dict) + ) + """Coordinated relations keyed by ordered logical task-state resource pairs.""" + + articulation_joints: Mapping[tuple[str, str], ArticulationJointState] = field( + default_factory=dict + ) + """Verified articulation states keyed by canonical articulation and joint IDs.""" def __post_init__(self) -> None: if self.batch_size <= 0: @@ -205,8 +397,61 @@ def __post_init__(self) -> None: value, batch_size=self.batch_size, device=device ) + normalized_coordinated: dict[tuple[str, str], CoordinatedHeldObjectState] = {} + for resources, value in self.coordinated_held_objects.items(): + if ( + not isinstance(resources, tuple) + or len(resources) != 2 + or not all(isinstance(item, str) and item for item in resources) + ): + raise TypeError( + "coordinated_held_objects keys must be pairs of non-empty strings." + ) + if not isinstance(value, CoordinatedHeldObjectState): + raise TypeError( + "coordinated_held_objects values must be " + "CoordinatedHeldObjectState objects." + ) + normalized_coordinated[resources] = _normalize_coordinated_held( + value, batch_size=self.batch_size, device=device + ) + + normalized_articulation: dict[tuple[str, str], ArticulationJointState] = {} + for key, value in self.articulation_joints.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(item) is str and item and item == item.strip() for item in key + ) + ): + raise TypeError( + "articulation_joints keys must be pairs of non-empty " + "canonical identifiers." + ) + if not isinstance(value, ArticulationJointState): + raise TypeError( + "articulation_joints values must be ArticulationJointState " + "objects." + ) + normalized_articulation[key] = _normalize_articulation_joint( + value, + batch_size=self.batch_size, + device=device, + ) + object.__setattr__(self, "device", device) object.__setattr__(self, "held_objects", MappingProxyType(normalized_held)) + object.__setattr__( + self, + "coordinated_held_objects", + MappingProxyType(normalized_coordinated), + ) + object.__setattr__( + self, + "articulation_joints", + MappingProxyType(normalized_articulation), + ) @classmethod def empty( @@ -276,6 +521,22 @@ def exclusive_held_object_mask(self, resource: str) -> torch.Tensor: exclusive &= ~other.env_mask return exclusive + def get_coordinated_held_object( + self, + first_resource: str, + second_resource: str, + ) -> CoordinatedHeldObjectState | None: + """Return the relation for an ordered resource pair, if any.""" + return self.coordinated_held_objects.get((first_resource, second_resource)) + + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ArticulationJointState | None: + """Return verified state for one canonical articulation joint.""" + return self.articulation_joints.get((articulation_id, joint_id)) + @dataclass(frozen=True, slots=True, eq=False) class RobotObservation: @@ -360,6 +621,70 @@ def __post_init__(self) -> None: object.__setattr__(self, "pose", self.pose.clone()) +@dataclass(frozen=True, slots=True, eq=False) +class ObservedArticulationJointState: + """Live measured state for one scene articulation joint. + + This value belongs to :class:`SceneSnapshot`, not :class:`TaskState`. + ``ArticulationJointState`` records a verified symbolic effect after an + operation, while this class records the physical position used by online + grounding and recovery replans. + """ + + position: torch.Tensor + """Measured joint position with shape ``(J,)`` or ``(B, J)``.""" + + valid_mask: torch.Tensor | None = None + """Optional row-validity mask for a batched observation.""" + + def __post_init__(self) -> None: + position = self.position + if not isinstance(position, torch.Tensor): + raise TypeError("ObservedArticulationJointState.position must be a tensor.") + if position.dim() not in (1, 2) or position.numel() == 0: + raise ValueError( + "ObservedArticulationJointState.position must have shape (J,) " + "or (B, J)." + ) + if not position.is_floating_point(): + raise TypeError( + "ObservedArticulationJointState.position must be floating point." + ) + if not torch.isfinite(position).all(): + raise ValueError( + "ObservedArticulationJointState.position must contain only " + "finite values." + ) + object.__setattr__(self, "position", position.clone()) + if self.valid_mask is None: + return + valid_mask = self.valid_mask + if not isinstance(valid_mask, torch.Tensor): + raise TypeError( + "ObservedArticulationJointState.valid_mask must be a tensor or None." + ) + if position.dim() != 2: + raise ValueError( + "ObservedArticulationJointState.valid_mask requires a batched " + "position." + ) + if valid_mask.dtype != torch.bool or valid_mask.shape != (position.shape[0],): + raise ValueError( + "ObservedArticulationJointState.valid_mask must have shape (B,) " + "and dtype torch.bool." + ) + if valid_mask.device != position.device: + raise ValueError( + "ObservedArticulationJointState position and valid_mask must " + "share a device." + ) + object.__setattr__(self, "valid_mask", valid_mask.clone()) + + def snapshot(self) -> ObservedArticulationJointState: + """Return an independently owned observation value.""" + return ObservedArticulationJointState(self.position, self.valid_mask) + + class _ImmutableEntityMapping(Mapping[str, EntityState]): """Own entity states and return defensive copies on every public read.""" @@ -384,6 +709,34 @@ def __len__(self) -> int: return len(self._states) +class _ImmutableObservedArticulationJointMapping( + Mapping[tuple[str, str], ObservedArticulationJointState] +): + """Own live joint observations and copy values on every public read.""" + + __slots__ = ("_states",) + + def __init__( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> None: + self._states = MappingProxyType( + {key: state.snapshot() for key, state in states.items()} + ) + + def __getitem__( + self, + key: tuple[str, str], + ) -> ObservedArticulationJointState: + return self._states[key].snapshot() + + def __iter__(self) -> Iterator[tuple[str, str]]: + return iter(self._states) + + def __len__(self) -> int: + return len(self._states) + + @dataclass(frozen=True, slots=True, eq=False) class SceneSnapshot: """Versioned scene state used to ground dynamic goals and obstacles.""" @@ -397,6 +750,11 @@ class SceneSnapshot: collision_entity_ids: tuple[str, ...] = () """Entity IDs whose poses update a planner's dynamic collision world.""" + articulation_joints: Mapping[tuple[str, str], ObservedArticulationJointState] = ( + field(default_factory=dict) + ) + """Live physical joint observations keyed by articulation and joint ID.""" + def __post_init__(self) -> None: if self.timestamp < 0.0: raise ValueError("SceneSnapshot.timestamp must be non-negative.") @@ -434,6 +792,28 @@ def __post_init__(self) -> None: "SceneSnapshot entities must contain EntityState values." ) normalized[entity_id] = state + normalized_joints: dict[tuple[str, str], ObservedArticulationJointState] = {} + for key, state in self.articulation_joints.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(identifier) is str + and identifier + and identifier == identifier.strip() + for identifier in key + ) + ): + raise TypeError( + "SceneSnapshot articulation_joints keys must be canonical " + "(articulation_id, joint_id) pairs." + ) + if not isinstance(state, ObservedArticulationJointState): + raise TypeError( + "SceneSnapshot articulation_joints values must be " + "ObservedArticulationJointState objects." + ) + normalized_joints[key] = state collision_entity_ids = tuple(self.collision_entity_ids) if len(set(collision_entity_ids)) != len(collision_entity_ids) or not all( isinstance(entity_id, str) and entity_id @@ -449,8 +829,29 @@ def __post_init__(self) -> None: f"{sorted(missing)}." ) object.__setattr__(self, "entities", _ImmutableEntityMapping(normalized)) + object.__setattr__( + self, + "articulation_joints", + _ImmutableObservedArticulationJointMapping(normalized_joints), + ) object.__setattr__(self, "collision_entity_ids", collision_entity_ids) + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ObservedArticulationJointState | None: + """Return an owned live joint observation for a canonical address.""" + for value, field_name in ( + (articulation_id, "articulation_id"), + (joint_id, "joint_id"), + ): + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty canonical identifier." + ) + return self.articulation_joints.get((articulation_id, joint_id)) + def collision_world_revisions(self, batch_size: int) -> tuple[int, ...]: """Expand the collision revision to one value per environment. @@ -542,6 +943,18 @@ def __post_init__(self) -> None: f"Scene entity {entity_id!r} pose batch must match the " "planning context." ) + for ( + articulation_id, + joint_id, + ), state in self.scene.articulation_joints.items(): + if ( + state.position.dim() == 2 + and state.position.shape[0] != self.robot.batch_size + ): + raise ValueError( + f"Scene articulation joint ({articulation_id!r}, {joint_id!r}) " + "position batch must match the planning context." + ) if not isinstance(self.env_ids, torch.Tensor): raise TypeError("env_ids must be a torch.Tensor.") if self.env_ids.dtype != torch.long: @@ -584,6 +997,21 @@ def get_held_object(self, resource: str) -> HeldObjectState | None: """Return the object held by ``resource``, if any.""" return self.task.get_held_object(resource) + @property + def coordinated_held_objects( + self, + ) -> Mapping[tuple[str, str], CoordinatedHeldObjectState]: + """Coordinated held-object relations.""" + return self.task.coordinated_held_objects + + def get_coordinated_held_object( + self, + first_resource: str, + second_resource: str, + ) -> CoordinatedHeldObjectState | None: + """Return a coordinated held-object relation, if any.""" + return self.task.get_coordinated_held_object(first_resource, second_resource) + def require_control_dt(self) -> float: """Return the explicit command period required for interpolation. @@ -597,6 +1025,21 @@ def require_control_dt(self) -> float: ) return self.control_dt + @property + def articulation_joints( + self, + ) -> Mapping[tuple[str, str], ArticulationJointState]: + """Verified articulation-joint states.""" + return self.task.articulation_joints + + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ArticulationJointState | None: + """Return verified state for one canonical articulation joint.""" + return self.task.get_articulation_joint_state(articulation_id, joint_id) + def project( self, *, @@ -622,8 +1065,11 @@ def project( __all__ = [ + "ArticulationJointState", + "CoordinatedHeldObjectState", "EntityState", "HeldObjectState", + "ObservedArticulationJointState", "PlanningContext", "RobotObservation", "SceneSnapshot", diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 239258b13..d9128a149 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -1488,6 +1488,15 @@ def set_qf( data_type=ArticulationGPUAPIWriteType.JOINT_FORCE, ) + def get_qf(self) -> torch.Tensor: + """Get the current generalized efforts (qf) of the articulation. + + Returns: + torch.Tensor: Joint efforts with shape (N, dof), where N is the + number of environments. + """ + return self.body_data.qf + def get_qf_limits( self, joint_ids: Sequence[int] | torch.Tensor | None = None, diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 1872f27d2..3e1f0c21b 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -1282,18 +1282,11 @@ def _require_held_object( slot_id: str, path: tuple[PathPart, ...], ) -> tuple[str, HeldObjectState]: - """Resolve the motion control part and verify its held-object identity.""" + """Resolve the logical participant key and verify held-object identity.""" endpoint = analyzed.bound.binding.action_binding.endpoint(slot_id, "motion") - try: - target = endpoint.require_target(JointPositionTarget) - except TypeError as exc: - raise _diagnostic( - "unsupported_builtin_endpoint", - (*path, "resources", slot_id, "motion"), - "The current built-in semantic lowerer requires a joint-position " - "motion endpoint.", - ) from exc - held = context.task.get_held_object(target.control_part) + task_state_key = endpoint.task_state_key + assert isinstance(task_state_key, str) + held = context.task.get_held_object(task_state_key) call_object = getattr(analyzed.call, "object", None) assert type(call_object) is SceneObjectRef if held is None or held.semantics.entity_id != call_object.entity_id: @@ -1301,7 +1294,7 @@ def _require_held_object( "verified_held_object_required", (*path, "object"), f"Call requires verified object {call_object.entity_id!r} held by " - f"{target.control_part!r}.", + f"logical state key {task_state_key!r}.", ) assert held.env_mask is not None missing = eligible & ~held.env_mask @@ -1317,7 +1310,7 @@ def _require_held_object( "every eligible environment.", missing_env_ids, ) - return target.control_part, held + return task_state_key, held @staticmethod def _broadcast_pose( diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index ec924db8b..f3715f20b 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -1841,6 +1841,7 @@ def _lower_binding( resource_id=resource.resource_id, adapter_id=endpoint.adapter_id, target=endpoint.runtime_target, + task_state_key=resource.resource_id, capabilities=endpoint.capabilities, commands=endpoint.commands, claim_tokens=endpoint.claim_tokens, diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index 0f5113386..69d4f8306 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -30,6 +30,7 @@ from ..atomic_actions.engine import AtomicActionEngine from ..atomic_actions.execution import ( EffectVerificationRequest, + EffectVerificationResult, ExecutionEvent, ExecutionTick, ) @@ -1097,7 +1098,24 @@ def step( "effect_success may only be submitted for pending effect " "verification." ) - runner_step = self._runner.step(effect_success=effect_success) + effect_result = None + if effect_success is not None: + pending = self.pending_effect + if pending is None: + raise RuntimeError("No effect request is currently pending.") + success = ( + effect_success.to( + device=pending.env_mask.device, + dtype=torch.bool, + ) + & pending.env_mask + ) + effect_result = EffectVerificationResult( + verification_id=pending.verification_id, + success_mask=success, + failure_mask=pending.env_mask & ~success, + ) + runner_step = self._runner.step(effect_result=effect_result) self._record_runner_step(runner_step) return self._consume_runner_step(runner_step) @@ -1246,18 +1264,31 @@ def _start_current_call(self) -> None: def _adapt_effect_verifier( self, verifier: SemanticEffectVerifier, - ) -> Callable[[PlanningContext, ExecutionTick], torch.Tensor]: + ) -> Callable[ + [PlanningContext, EffectVerificationRequest], + EffectVerificationResult, + ]: """Adapt the semantic verifier to the low-level runner callback.""" - def verify(context: PlanningContext, tick: ExecutionTick) -> torch.Tensor: - pending = tick.pending_effect - if not isinstance(pending, EffectVerificationRequest): - raise RuntimeError("Effect verifier was called without a request.") + def verify( + context: PlanningContext, + pending: EffectVerificationRequest, + ) -> EffectVerificationResult: call = self.workflow.calls[self._call_index].call result = verifier(call, pending, context) if not isinstance(result, torch.Tensor): raise TypeError("SemanticEffectVerifier must return a torch.Tensor.") - return result + success = result.to(device=pending.env_mask.device, dtype=torch.bool) + if success.shape != pending.env_mask.shape: + raise ValueError( + "SemanticEffectVerifier must return one boolean per environment." + ) + success &= pending.env_mask + return EffectVerificationResult( + verification_id=pending.verification_id, + success_mask=success, + failure_mask=pending.env_mask & ~success, + ) return verify @@ -1275,7 +1306,14 @@ def _record_runner_step(self, step: RunnerStep) -> None: def _consume_runner_step(self, runner_step: RunnerStep) -> SemanticExecutionStep: """Advance the semantic call barrier from one low-level result.""" assert self._runner is not None + previous_pending = self.pending_effect pending = None if runner_step.tick is None else runner_step.tick.pending_effect + if ( + pending is None + and self._runner.effect_verification_pending + and previous_pending is not None + ): + pending = previous_pending if runner_step.status is RunnerStatus.RUNNING: self._status = ( SemanticExecutionStatus.WAITING_FOR_EFFECT diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 57b22ca22..ed40079c1 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -18,8 +18,8 @@ from __future__ import annotations -import math -from typing import Literal, TypeVar +from dataclasses import replace +from typing import TypeVar from unittest.mock import Mock import pytest @@ -37,6 +37,7 @@ AtomicActionEngine, ControlPartCommandProfile, CoordinatedPickGoal, + CoordinatedHeldObjectState, CoordinatedPickment, CoordinatedPickmentOptions, CoordinatedPlacement, @@ -61,6 +62,10 @@ MoveJoints, MoveJointsOptions, ObjectSemantics, + ObservedArticulationJointState, + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, PickUp, PickUpOptions, Place, @@ -76,6 +81,7 @@ SlideGoal, SlideOptions, RobotObservation, + SceneArticulationOperationGeometry, SceneEntityPose, SceneSnapshot, TaskState, @@ -270,16 +276,54 @@ def _target_scene( ) +def _articulation_geometry() -> SceneArticulationOperationGeometry: + """Build late-bound identity handle geometry for atomic tests.""" + identity = torch.eye(4) + return SceneArticulationOperationGeometry( + handle_pose=SceneEntityPose("drawer_handle"), + approach_offset=identity, + contact_offset=identity, + operation_offset=identity, + retract_offset=identity, + operation_axis=torch.tensor((1.0, 0.0, 0.0)), + ) + + +def _articulation_scene( + position: torch.Tensor, + *, + handle_x: float = 0.0, + timestamp: float = 0.0, + version: int = 0, +) -> SceneSnapshot: + """Build one live handle and articulation-joint snapshot.""" + handle = torch.eye(4).repeat(NUM_ENVS, 1, 1) + handle[:, 0, 3] = handle_x + return SceneSnapshot( + timestamp=timestamp, + version=version, + entities={"drawer_handle": EntityState(handle)}, + articulation_joints={ + ("drawer", "slide"): ObservedArticulationJointState(position) + }, + ) + + def _binding( action: AtomicAction, *, motion: str = "arm", grasp: str = "hand", + task_state_key: str | None = None, ) -> ActionBinding: """Bind one single-participant action through its owning engine.""" contract = type(action).__dict__.get("binding_contract") assert contract is not None - endpoint_parts = {"motion": motion, "grasp": grasp} + endpoint_parts = { + "motion": motion, + "grasp": grasp, + "interaction": grasp, + } return _ACTION_ENGINES[id(action)].bind_control_parts( action.skill_id, { @@ -289,6 +333,11 @@ def _binding( } for slot in contract.slots }, + task_state_keys=( + None + if task_state_key is None + else {slot.slot_id: task_state_key for slot in contract.slots} + ), ) @@ -462,6 +511,8 @@ def _dual_binding( action: AtomicAction, first_slot: str, second_slot: str, + *, + task_state_keys: dict[str, str] | None = None, ) -> ActionBinding: return _ACTION_ENGINES[id(action)].bind_control_parts( action.skill_id, @@ -475,6 +526,7 @@ def _dual_binding( "grasp": "right_hand", }, }, + task_state_keys=task_state_keys, ) @@ -493,6 +545,93 @@ def _sample(obj_poses: torch.Tensor, **_kwargs: object) -> list[dict]: affordance.get_dual_arm_valid_grasp_poses = Mock(side_effect=_sample) +def _plan_segment_contract_case(case_id: str) -> ActionPlan: + """Plan one built-in used by the Version 1 trajectory-segment contract.""" + generator = _motion_generator() + sample_count = 20 + + if case_id == "move_joints": + action = _bind_action(generator, MoveJoints()) + goal = JointPositionGoal(torch.zeros(ARM_DOF)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + if case_id == "move_end_effector": + action = _bind_action(generator, MoveEndEffector()) + goal = EndEffectorPoseGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + held_task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"arm": _held()}, + ) + if case_id == "move_held_object": + action = _bind_action(generator, MoveHeldObject()) + goal = HeldObjectPoseGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task), + ) + + if case_id == "place": + action = _bind_action(generator, Place()) + goal = PlaceGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task), + ) + + if case_id == "assemble": + action = _bind_action(generator, Place()) + goal = AssembleGoal( + affordance=AssembleAffordance( + base_object_entity=Mock(), + assemble_to_base_pose=torch.eye(4), + ), + base_pose=SceneEntityPose("base"), + ) + base_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"base": EntityState(base_pose)}, + ) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task, scene=scene), + ) + + if case_id == "press": + action = _bind_action(generator, Press()) + semantics = ObjectSemantics( + affordance=PressAffordance( + press_axis=torch.tensor([1.0, 0.0, 0.0]), + press_position=(0.0, 0.0, 0.0), + ), + geometry={}, + label="button", + ) + goal = PressGoal(semantics, torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + raise AssertionError(f"Unknown trajectory-segment contract case {case_id!r}.") + + def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: assert MoveEndEffector.GoalType is EndEffectorPoseGoal assert MoveJoints.GoalType is JointPositionGoal @@ -505,6 +644,34 @@ def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: assert CoordinatedPickment.GoalType is CoordinatedPickGoal assert CoordinatedPlacement.GoalType is CoordinatedPlacementGoal assert HandOver.GoalType is GraspGoal + assert OperateArticulation.GoalType is OperateArticulationGoal + + +@pytest.mark.parametrize( + ("case_id", "expected_names"), + ( + ("move_joints", ("move_joints",)), + ("move_end_effector", ("move_end_effector",)), + ("move_held_object", ("transport",)), + ("place", ("approach", "release", "retract")), + ("assemble", ("approach", "release", "retract")), + ("press", ("close", "approach", "contact", "press", "retract")), + ), +) +def test_builtin_trajectory_segment_names_and_ranges_are_stable( + case_id: str, + expected_names: tuple[str, ...], +) -> None: + plan = _plan_segment_contract_case(case_id) + + assert plan.success_all + assert tuple(segment.name for segment in plan.segments) == expected_names + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_interaction_primitives_use_motion_centric_skill_ids() -> None: @@ -527,6 +694,7 @@ def test_interaction_primitives_use_motion_centric_skill_ids() -> None: CoordinatedPickmentOptions(), CoordinatedPlacementOptions(), HandOverOptions(), + OperateArticulationOptions(), ), ) def test_action_options_do_not_contain_embodiment_resources(options: object) -> None: @@ -646,13 +814,18 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: pick_plan = _plan_action( pick, - _invocation(pick, GraspGoal(semantics=semantics, grasp_xpos=grasp)), + ActionInvocation( + skill_id="pick_up", + goal=GraspGoal(semantics=semantics, grasp_xpos=grasp), + binding=_binding(pick, task_state_key="logical_arm"), + ), initial, ) picked_task = pick_plan.expected_effects.apply(initial.task, pick_plan.plan_success) - assert initial.task.get_held_object("arm") is None - assert picked_task.get_held_object("arm") is not None + assert initial.task.get_held_object("logical_arm") is None + assert picked_task.get_held_object("logical_arm") is not None + assert picked_task.get_held_object("arm") is None place = _bind_action(generator, Place()) picked_context = PlanningContext( @@ -664,15 +837,19 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: ) place_plan = _plan_action( place, - _invocation(place, PlaceGoal(torch.eye(4))), + ActionInvocation( + skill_id="place", + goal=PlaceGoal(torch.eye(4)), + binding=_binding(place, task_state_key="logical_arm"), + ), picked_context, ) placed_task = place_plan.expected_effects.apply( picked_task, place_plan.plan_success ) - assert picked_task.get_held_object("arm") is not None - assert placed_task.get_held_object("arm") is None + assert picked_task.get_held_object("logical_arm") is not None + assert placed_task.get_held_object("logical_arm") is None def test_place_releases_only_exclusively_held_rows() -> None: @@ -729,6 +906,10 @@ def test_move_held_object_requires_projected_attachment() -> None: HeldObjectPoseGoal(torch.eye(4)), sample_count=10, ) + invocation = replace( + invocation, + binding=_binding(action, task_state_key="logical_arm"), + ) with pytest.raises(ValueError, match="requires an object held"): _plan_action(action, invocation, _context()) @@ -742,7 +923,7 @@ def test_move_held_object_requires_projected_attachment() -> None: task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"arm": held}, + held_objects={"logical_arm": held}, ) eef_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) eef_pose[:, :3, :3] = torch.tensor( @@ -755,7 +936,7 @@ def test_move_held_object_requires_projected_attachment() -> None: configured_invocation = ActionInvocation( skill_id="move_held_object", goal=HeldObjectPoseGoal(torch.eye(4)), - binding=_binding(action), + binding=_binding(action, task_state_key="logical_arm"), motion_policy=MotionPolicy(sample_count=10), skill_options=MoveHeldObjectOptions(pick_rotate_upright=0.25), ) @@ -764,6 +945,7 @@ def test_move_held_object_requires_projected_attachment() -> None: assert plan.plan_success.all() assert plan.expected_effects.is_empty + assert generator.robot.compute_fk.call_args.kwargs["name"] == "arm" current_object_pose = action._apply_configured_upright_rotation.call_args.args[2] assert torch.allclose( current_object_pose, @@ -814,6 +996,192 @@ def move_ik( assert not torch.allclose(trajectory.positions[1], context.robot.qpos[1]) +def test_operate_articulation_builds_named_verified_interaction() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + plan = _plan_action( + action, + _invocation(action, goal, sample_count=16), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + assert plan.plan_success.tolist() == [True, True] + assert plan.commands.frame_count == 16 + assert tuple(segment.name for segment in plan.segments) == ( + "approach", + "engage", + "operate", + "release", + "retract", + ) + assert plan.requires_effect_verification + assert plan.scene_dependency_monitor_until == { + "drawer_handle": plan.segment("operate").start + } + assert plan.effect_verification is not None + assert plan.effect_verification.kind == "articulation.joint_progress" + update = plan.expected_effects.articulation_joint_updates[("drawer", "slide")] + assert update is not None + assert torch.equal(update.position, torch.tensor([0.4])) + interaction = _joint_command_positions(plan, "hand") + assert torch.all(interaction[:, -1] == 0.0) + assert torch.any(interaction == 1.0) + + +def test_operate_articulation_reports_per_phase_planning_failures() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + phase_results = [] + for index in range(4): + phase_results.append( + PlanResult( + success=torch.tensor([index != 2, True]), + positions=torch.zeros(NUM_ENVS, 3, ARM_DOF), + dt=torch.tensor([[0.0, 0.1, 0.2]]).repeat(NUM_ENVS, 1), + ) + ) + generator.generate = Mock(side_effect=phase_results) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + plan = _plan_action( + action, + _invocation(action, goal, sample_count=16), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + assert plan.plan_success.tolist() == [False, True] + assert plan.diagnostics.messages == ( + "Articulation motion phase 'operate' failed for rows [0].", + ) + phases = plan.diagnostics.metadata["motion_phases"] + assert phases["operate"] == { + "success": [False, True], + "failed_rows": [0], + "waypoint_count": 3, + } + + +def test_operate_articulation_replan_uses_fresh_handle_and_remaining_stroke() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([[0.0], [0.0]]), + target_position=torch.tensor([[0.4], [0.4]]), + target_displacement=0.4, + ) + invocation = _invocation(action, goal, sample_count=16) + initial_context = _context( + scene=_articulation_scene( + torch.tensor([[0.0], [0.0]]), + handle_x=0.3, + ) + ) + session = _ACTION_ENGINES[id(action)].start((invocation,), initial_context) + first_operation = generator.robot.compute_ik.call_args_list[2].kwargs["pose"] + assert torch.allclose(first_operation[:, 0, 3], torch.tensor([0.7, 0.7])) + session.tick(initial_context) + + generator.robot.compute_ik.reset_mock() + recovered = session.tick( + _context( + scene=_articulation_scene( + torch.tensor([[0.2], [0.4]]), + handle_x=0.55, + timestamp=1.0, + version=1, + ), + timestamp=1.0, + ), + ) + recovered_operation = generator.robot.compute_ik.call_args_list[2].kwargs["pose"] + + assert torch.allclose(recovered_operation[:, 0, 3], torch.tensor([0.75, 0.55])) + event_kinds = {event.kind for event in recovered.events} + assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in event_kinds + assert ExecutionEventKind.REPLANNED in event_kinds + assert session.trajectory_segment("operate").name == "operate" + + +def test_operate_articulation_requires_live_joint_observation() -> None: + generator = _motion_generator() + action = _bind_action(generator, OperateArticulation()) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + handle = torch.eye(4).repeat(NUM_ENVS, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer_handle": EntityState(handle)}, + ) + + with pytest.raises(ValueError, match="ObservedArticulationJointState"): + _plan_action( + action, + _invocation(action, goal, sample_count=20), + _context(scene=scene), + ) + + +def test_operate_articulation_rejects_insufficient_motion_budget() -> None: + generator = _motion_generator() + action = _bind_action(generator, OperateArticulation()) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + with pytest.raises(ValueError, match="at least two waypoints"): + _plan_action( + action, + _invocation(action, goal, sample_count=17), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + def test_strategy_and_sample_count_are_not_action_config_fields() -> None: with pytest.raises(TypeError): MoveEndEffectorOptions(strategy="motion_gen") # type: ignore[call-arg] @@ -1014,6 +1382,9 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: "lift", ] assert plan.segment("close").stop == plan.segment("lift").start + assert plan.scene_dependency_monitor_until == { + "target": plan.segment("close").start + } def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: @@ -1105,7 +1476,9 @@ def test_pick_resolves_late_bound_scene_grasp_and_declares_dependency() -> None: assert held is not None assert torch.allclose(held.grasp_xpos, expected_grasp) assert plan.scene_dependencies == ("target",) - assert plan.scene_dependency_end_segment == "approach" + assert plan.scene_dependency_monitor_until == { + "target": plan.segment("approach").stop + } def test_pick_session_replans_when_late_bound_target_moves() -> None: @@ -1162,7 +1535,92 @@ def test_pick_session_replans_when_late_bound_target_moves() -> None: assert ExecutionEventKind.REPLANNED in event_kinds -def test_pick_uses_binding_control_part_as_effect_resource() -> None: +@pytest.mark.parametrize( + ("waypoint_offset", "expects_replan"), + ((-1, True), (0, False)), +) +def test_pick_scene_monitoring_window_is_exclusive_at_close_boundary( + waypoint_offset: int, + expects_replan: bool, +) -> None: + """External motion replans before close, while grasp-induced motion does not.""" + generator = _motion_generator() + action = _bind_action(generator, PickUp()) + engine = _ACTION_ENGINES[id(action)] + initial_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + moved_pose = initial_pose.clone() + moved_pose[:, 1, 3] = 0.3 + invocation = _invocation( + action, + GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), + sample_count=20, + ) + task_state = TaskState.empty(batch_size=NUM_ENVS, device="cpu") + qpos = torch.zeros(NUM_ENVS, ROBOT_DOF) + + def context_at( + pose: torch.Tensor, + *, + timestamp: float, + version: int, + ) -> PlanningContext: + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=task_state, + scene=_target_scene(pose, timestamp=timestamp, version=version), + env_ids=torch.arange(NUM_ENVS), + control_dt=1.0 / 60.0, + ) + + session = engine.start( + (invocation,), context_at(initial_pose, timestamp=0.0, version=0) + ) + tick = session.tick(context_at(initial_pose, timestamp=0.0, version=0)) + close_start = session.plan_attempts[0].plan.segment("close").start + commands_to_issue = close_start + waypoint_offset + issued = 1 + while issued < commands_to_issue: + assert tick.command is not None + for command in tick.command.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + qpos[:, list(command.target.joint_ids)] = command.payload.positions + tick = session.tick( + context_at( + initial_pose, + timestamp=0.04 * issued, + version=0, + ) + ) + issued += 1 + + assert tick.command is not None + for command in tick.command.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + qpos[:, list(command.target.joint_ids)] = command.payload.positions + moved = session.tick( + context_at( + moved_pose, + timestamp=0.04 * commands_to_issue, + version=1, + ) + ) + + event_kinds = {event.kind for event in moved.events} + assert (ExecutionEventKind.DYNAMIC_GOAL_CHANGED in event_kinds) is expects_replan + assert (ExecutionEventKind.REPLANNED in event_kinds) is expects_replan + assert len(session.plan_attempts) == (2 if expects_replan else 1) + + +def test_pick_uses_logical_task_state_key_and_physical_control_target() -> None: generator = _motion_generator() action = _bind_action(generator, PickUp()) invocation = ActionInvocation( @@ -1175,6 +1633,7 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: action, motion="alternate_arm", grasp="alternate_hand", + task_state_key="logical_picker", ), motion_policy=MotionPolicy(sample_count=20), ) @@ -1190,8 +1649,74 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) - assert projected.get_held_object("alternate_arm") is not None + assert projected.get_held_object("logical_picker") is not None + assert projected.get_held_object("alternate_arm") is None assert projected.get_held_object("arm") is None + assert {target.target_id for target in plan.commands.targets} == { + "alternate_arm", + "alternate_hand", + } + + +def test_participant_motion_and_grasp_must_share_task_state_key() -> None: + generator = _motion_generator() + action = _bind_action(generator, PickUp()) + binding = _binding(action) + mismatched = ActionBinding( + owner_id=binding.owner_id, + endpoints=tuple( + ( + replace(endpoint, task_state_key="other_participant") + if endpoint.endpoint_id == "grasp" + else endpoint + ) + for endpoint in binding.endpoints + ), + ) + invocation = ActionInvocation( + skill_id="pick_up", + goal=GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), + binding=mismatched, + ) + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) + + with pytest.raises(ValueError, match="must share one task_state_key"): + _plan_action(action, invocation, context) + + +def test_handover_participants_must_use_distinct_task_state_keys() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + HandOver( + default_options=HandOverOptions( + middle_object_pose=torch.eye(4), + final_object_pose=torch.eye(4), + ) + ), + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=GraspGoal(semantics=_semantics()), + binding=_dual_binding( + action, + "source", + "destination", + task_state_keys={"source": "same", "destination": "same"}, + ), + ) + + with pytest.raises(ValueError, match="different task_state_key"): + _plan_action(action, invocation, _dual_context()) def test_press_closes_hand_without_changing_projected_attachment() -> None: @@ -1232,807 +1757,32 @@ def test_press_closes_hand_without_changing_projected_attachment() -> None: assert torch.equal(projected_held.object_to_eef, held.object_to_eef) -def test_twist_plans_six_segments_from_articulation_link() -> None: - affordance = TwistAffordance( - grasp_position=(0.0, 0.0, 0.0), - axis_origin=(0.0, 0.0, 0.0), - twist_axis=torch.tensor([0.0, 1.0, 0.0]), - ) - semantics = ObjectSemantics( - affordance=affordance, - geometry={}, - label="knob", - ) - generator = _motion_generator() - action = _bind_action(generator, Twist()) - - plan = _plan_action( - action, - ActionInvocation( - skill_id="twist", - goal=TwistGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=24), - skill_options=TwistOptions(hand_interp_steps=3), +@pytest.mark.parametrize( + ("hold_steps", "expected_segments"), + ( + (0, ("transfer", "approach", "close", "release", "deliver")), + ( + 2, + ("transfer", "approach", "close", "hold", "release", "deliver"), ), - _context(), - ) - - assert plan.plan_success.tolist() == [True, True] - trajectory = _joint_trajectory(plan) - assert trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) - assert [segment.name for segment in plan.segments] == [ - "approach", - "reach", - "close", - "twist", - "open", - "retract", - ] - assert torch.all( - trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 - ) - assert torch.all( - trajectory.positions[:, plan.segment("open").stop - 1, ARM_DOF:] == 0.0 + ), +) +def test_handover_does_not_mutate_cached_final_pose_and_omits_empty_hold( + hold_steps: int, + expected_segments: tuple[str, ...], + monkeypatch: pytest.MonkeyPatch, +) -> None: + generator = _dual_motion_generator() + handover_options = HandOverOptions( + middle_object_pose=torch.eye(4), + final_object_pose=torch.eye(4), + hand_interp_steps=4, + hold_steps=hold_steps, + retreat_steps=5, ) - first_target = generator.robot.compute_ik.call_args_list[0].kwargs["pose"] - grasp_pose = affordance.get_grasp_pose(torch.eye(4).repeat(NUM_ENVS, 1, 1)) - expected_pre_grasp_position = ( - grasp_pose[:, :3, 3] - grasp_pose[:, :3, 2] * TwistOptions().pre_grasp_distance - ) - assert torch.allclose(first_target[:, :3, 3], expected_pre_grasp_position) - - -def test_twist_plans_from_explicit_rigid_object_pose_snapshot() -> None: - semantics = ObjectSemantics( - affordance=TwistAffordance( - grasp_position=(0.0, 0.0, 0.0), - axis_origin=(0.0, 0.0, 0.0), - twist_axis=torch.tensor([0.0, 1.0, 0.0]), - ), - geometry={}, - label="rigid-knob", - ) - - action = _bind_action(_motion_generator(), Twist()) - plan = _plan_action( - action, - ActionInvocation( - skill_id="twist", - goal=TwistGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=24), - skill_options=TwistOptions(hand_interp_steps=3), - ), - _context(), - ) - - assert plan.plan_success.tolist() == [True, True] - - -def test_twist_rotates_grasp_about_explicit_axis_origin() -> None: - action = _bind_action(_motion_generator(), Twist()) - target_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) - grasp_pose = target_pose.clone() - grasp_pose[:, 0, 3] = 2.0 - - twisted = action._twisted_grasp_poses( - target_pose, - grasp_pose, - torch.tensor([0.0, 0.0, 1.0]), - (1.0, 0.0, 0.0), - math.pi / 2, - 4, - ) - - assert torch.allclose( - twisted[:, -1, :3, 3], - torch.tensor([1.0, 1.0, 0.0]).expand(NUM_ENVS, -1), - atol=1.0e-6, - ) - - -@pytest.mark.parametrize( - ("goal_factory", "affordance"), - ( - ( - PressGoal, - PressAffordance( - press_axis=torch.tensor([1.0, 0.0, 0.0]), - press_position=(0.0, 0.0, 0.0), - ), - ), - ( - SlideGoal, - SlideAffordance( - mesh_vertices=torch.zeros(3, 3), - mesh_triangles=torch.tensor([[0, 1, 2]]), - ), - ), - ( - TwistGoal, - TwistAffordance( - grasp_position=(0.0, 0.0, 0.0), - axis_origin=(0.0, 0.0, 0.0), - ), - ), - ), -) -def test_interaction_goal_collects_target_scene_dependency( - goal_factory, - affordance, -) -> None: - semantics = ObjectSemantics(affordance=affordance, geometry={}, label="target") - goal = goal_factory(semantics, SceneEntityPose("target-link")) - - assert collect_scene_dependencies(goal) == ("target-link",) - - -def test_open_loop_interaction_primitives_are_explicitly_described() -> None: - assert Press.descriptor().open_loop is True - assert Slide.descriptor().open_loop is True - assert Twist.descriptor().open_loop is True - - -def test_twist_session_replans_when_scene_target_moves() -> None: - generator = _motion_generator() - engine = AtomicActionEngine( - generator, - control_profiles={ - "hand": ControlPartCommandProfile.joint_positions( - open=torch.zeros(HAND_DOF), - grasp=torch.ones(HAND_DOF), - ) - }, - load_builtins=False, - ) - action = Twist() - engine.register(action) - semantics = ObjectSemantics( - affordance=TwistAffordance( - grasp_position=(0.0, 0.0, 0.0), - axis_origin=(0.0, 0.0, 0.0), - ), - geometry={}, - label="moving-knob", - ) - invocation = ActionInvocation( - skill_id="twist", - goal=TwistGoal(semantics, SceneEntityPose("target")), - binding=engine.bind_control_parts( - "twist", - {"primary": {"motion": "arm", "grasp": "hand"}}, - ), - motion_policy=MotionPolicy(sample_count=24), - skill_options=TwistOptions(hand_interp_steps=3), - ) - initial_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) - initial_context = _context( - scene=_target_scene(initial_pose, timestamp=0.0, version=0) - ) - session = engine.start((invocation,), initial_context) - session.tick(initial_context) - moved_pose = initial_pose.clone() - moved_pose[:, 1, 3] = 0.3 - - recovered = session.tick( - _context( - scene=_target_scene(moved_pose, timestamp=0.1, version=1), - timestamp=0.1, - ) - ) - - event_kinds = {event.kind for event in recovered.events} - assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in event_kinds - assert ExecutionEventKind.REPLANNED in event_kinds - - -@pytest.mark.parametrize( - ("direction", "expected_segments", "translation_sign"), - ( - ("pull", ["approach", "reach", "close", "pull", "open"], -1.0), - ( - "push", - ["approach", "reach", "close", "push", "open", "return"], - 1.0, - ), - ), -) -def test_slide_plans_expected_segments( - direction: Literal["pull", "push"], - expected_segments: list[str], - translation_sign: float, - monkeypatch: pytest.MonkeyPatch, -) -> None: - vertices = torch.tensor( - [ - [-0.1, 0.0, 0.0], - [0.1, 0.0, 0.0], - [0.0, 0.0, 0.0], - ] - ) - link_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) - affordance = SlideAffordance( - mesh_vertices=vertices, - mesh_triangles=torch.tensor([[0, 1, 2]]), - translation_axis=torch.tensor([0.0, -1.0, 0.0]), - ) - grasp_calls: list[tuple[torch.Tensor, torch.Tensor]] = [] - - def sample_grasp( - self: SlideAffordance, - obj_poses: torch.Tensor, - approach_direction: torch.Tensor, - ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: - grasp_calls.append((obj_poses, approach_direction)) - return ( - torch.ones(NUM_ENVS, dtype=torch.bool), - torch.eye(4).repeat(NUM_ENVS, 1, 1), - torch.full((NUM_ENVS,), 0.03), - ) - - monkeypatch.setattr( - SlideAffordance, - "get_best_grasp_poses", - sample_grasp, - ) - semantics = ObjectSemantics( - affordance=affordance, - geometry={}, - label="drawer_handle", - ) - generator = _motion_generator() - action = _bind_action(generator, Slide()) - options = SlideOptions( - direction=direction, - hand_interp_steps=3, - approach_distance=0.1, - translation_distance=0.15, - ) - - plan = _plan_action( - action, - ActionInvocation( - skill_id="slide", - goal=SlideGoal(semantics, link_pose), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=24), - skill_options=options, - ), - _context(), - ) - - trajectory = _joint_trajectory(plan) - assert plan.plan_success.tolist() == [True, True] - assert trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) - assert [segment.name for segment in plan.segments] == expected_segments - assert torch.all( - trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 - ) - assert torch.all( - trajectory.positions[:, plan.segment("open").stop - 1, ARM_DOF:] == 0.0 - ) - assert len(grasp_calls) == 1 - assert torch.equal(grasp_calls[0][0], link_pose) - assert torch.allclose( - grasp_calls[0][1], - torch.tensor([0.0, -1.0, 0.0]).expand(NUM_ENVS, -1), - ) - planned_targets = [ - call.kwargs["pose"] for call in generator.robot.compute_ik.call_args_list - ] - expected_axis = torch.tensor([0.0, -1.0, 0.0]) - motion_lengths = Slide._motion_segment_lengths( - 24, - options.hand_interp_steps, - direction=direction, - ) - assert torch.allclose( - planned_targets[0][:, :3, 3], - -expected_axis.expand(NUM_ENVS, -1) * options.approach_distance, - ) - reach_stop = 1 + motion_lengths[1] - 1 - assert torch.allclose( - planned_targets[reach_stop - 1][:, :3, 3], - torch.zeros(NUM_ENVS, 3), - ) - translate_stop = reach_stop + motion_lengths[2] - 1 - translated_targets = torch.stack( - [pose[:, :3, 3] for pose in planned_targets[reach_stop:translate_stop]], - dim=1, - ) - assert torch.allclose( - translated_targets[:, -1], - expected_axis.expand(NUM_ENVS, -1) - * (translation_sign * options.translation_distance), - ) - orthogonal = ( - translated_targets - - (translated_targets * expected_axis).sum(dim=-1, keepdim=True) * expected_axis - ) - assert torch.allclose(orthogonal, torch.zeros_like(orthogonal), atol=1.0e-6) - if direction == "push": - assert torch.allclose( - planned_targets[-1][:, :3, 3], - -expected_axis.expand(NUM_ENVS, -1) * options.approach_distance, - ) - - -def test_slide_holds_failed_environment() -> None: - affordance = SlideAffordance( - mesh_vertices=torch.zeros(3, 3), - mesh_triangles=torch.tensor([[0, 1, 2]]), - translation_axis=torch.tensor([0.0, -1.0, 0.0]), - ) - affordance.get_best_grasp_poses = Mock( - return_value=( - torch.tensor([True, False]), - torch.eye(4).repeat(NUM_ENVS, 1, 1), - torch.full((NUM_ENVS,), 0.03), - ) - ) - semantics = ObjectSemantics( - affordance=affordance, - geometry={}, - label="drawer_handle", - ) - generator = _motion_generator() - - def successful_ik( - pose: torch.Tensor | None = None, - name: str | None = None, - joint_seed: torch.Tensor | None = None, - **_: object, - ) -> tuple[torch.Tensor, torch.Tensor]: - assert joint_seed is not None - return torch.ones(NUM_ENVS, dtype=torch.bool), torch.ones_like(joint_seed) - - generator.robot.compute_ik.side_effect = successful_ik - action = _bind_action(generator, Slide()) - context = _context() - - plan = _plan_action( - action, - ActionInvocation( - skill_id="slide", - goal=SlideGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=18), - skill_options=SlideOptions(hand_interp_steps=3), - ), - context, - ) - - assert plan.plan_success.tolist() == [True, False] - trajectory = _joint_trajectory(plan) - assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) - assert torch.allclose( - trajectory.positions[1], - context.robot.qpos[1].unsqueeze(0).expand(18, -1), - ) - - -def test_slide_fk_path_remains_on_translation_axis( - monkeypatch: pytest.MonkeyPatch, -) -> None: - generator = _motion_generator() - - def position_ik( - pose: torch.Tensor, - name: str, - joint_seed: torch.Tensor, - ) -> tuple[torch.Tensor, torch.Tensor]: - qpos = joint_seed.clone() - qpos[:, :3] = pose[:, :3, 3] - return torch.ones(NUM_ENVS, dtype=torch.bool), qpos - - def position_fk( - qpos: torch.Tensor, - name: str, - to_matrix: bool, - ) -> torch.Tensor: - pose = torch.eye(4).repeat(qpos.shape[0], 1, 1) - pose[:, :3, 3] = qpos[:, :3] - return pose - - generator.robot.compute_ik.side_effect = position_ik - generator.robot.compute_fk.side_effect = position_fk - affordance = SlideAffordance( - mesh_vertices=torch.zeros(3, 3), - mesh_triangles=torch.tensor([[0, 1, 2]]), - translation_axis=torch.tensor([0.0, -1.0, 0.0]), - ) - monkeypatch.setattr( - affordance, - "get_best_grasp_poses", - Mock( - return_value=( - torch.ones(NUM_ENVS, dtype=torch.bool), - torch.eye(4).repeat(NUM_ENVS, 1, 1), - torch.full((NUM_ENVS,), 0.03), - ) - ), - ) - semantics = ObjectSemantics(affordance=affordance, geometry={}, label="handle") - action = _bind_action(generator, Slide()) - plan = _plan_action( - action, - ActionInvocation( - skill_id="slide", - goal=SlideGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=24), - skill_options=SlideOptions(direction="pull", hand_interp_steps=3), - ), - _context(), - ) - - pull_segment = plan.segment("pull") - arm_path = _joint_trajectory(plan).positions[ - :, pull_segment.start : pull_segment.stop, :ARM_DOF - ] - fk_path = position_fk(arm_path.reshape(-1, ARM_DOF), "arm", True).reshape( - NUM_ENVS, -1, 4, 4 - ) - positions = fk_path[:, :, :3, 3] - axis = torch.tensor([0.0, -1.0, 0.0]) - orthogonal = positions - (positions * axis).sum(dim=-1, keepdim=True) * axis - assert torch.allclose(orthogonal, torch.zeros_like(orthogonal), atol=1.0e-6) - - -def test_press_plans_close_approach_press_and_retract() -> None: - affordance = PressAffordance( - press_axis=torch.tensor([1.0, 0.0, 0.0]), - press_position=(0.0, 0.0, 0.0), - ) - semantics = ObjectSemantics( - affordance=affordance, - geometry={}, - label="button", - ) - generator = _motion_generator() - action = _bind_action(generator, Press()) - options = PressOptions( - hand_interp_steps=3, - approach_distance=0.1, - press_distance=0.02, - ) - - plan = _plan_action( - action, - ActionInvocation( - skill_id="press", - goal=PressGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=24), - skill_options=options, - ), - _context(), - ) - - assert plan.plan_success.tolist() == [True, True] - trajectory = _joint_trajectory(plan) - assert trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) - assert [segment.name for segment in plan.segments] == [ - "close", - "approach", - "contact", - "press", - "retract", - ] - assert torch.all( - trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 - ) - contact_pose = affordance.get_press_pose(torch.eye(4).repeat(NUM_ENVS, 1, 1)) - expected_approach = ( - contact_pose[:, :3, 3] - contact_pose[:, :3, 2] * options.approach_distance - ) - expected_pressed = ( - contact_pose[:, :3, 3] + contact_pose[:, :3, 2] * options.press_distance - ) - planned_targets = [ - call.kwargs["pose"] for call in generator.robot.compute_ik.call_args_list - ] - motion_lengths = Press._motion_segment_lengths(24, options.hand_interp_steps) - contact_stop = 1 + motion_lengths[1] - 1 - press_stop = contact_stop + motion_lengths[2] - 1 - assert torch.allclose(planned_targets[0][:, :3, 3], expected_approach) - assert torch.allclose( - planned_targets[contact_stop - 1][:, :3, 3], contact_pose[:, :3, 3] - ) - assert torch.allclose(planned_targets[press_stop - 1][:, :3, 3], expected_pressed) - assert torch.allclose(planned_targets[-1][:, :3, 3], expected_approach) - - -def test_press_plans_from_rigid_object_pose_snapshot_with_option_position() -> None: - affordance = PressAffordance( - press_axis=torch.tensor([1.0, 0.0, 0.0]), - press_position=(0.5, 0.5, 0.5), - ) - semantics = ObjectSemantics( - affordance=affordance, - geometry={}, - label="rigid-button", - ) - generator = _motion_generator() - action = _bind_action(generator, Press()) - - plan = _plan_action( - action, - ActionInvocation( - skill_id="press", - goal=PressGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=24), - skill_options=PressOptions( - hand_interp_steps=3, - press_position=(0.1, 0.2, 0.3), - ), - ), - _context(), - ) - - assert plan.plan_success.tolist() == [True, True] - planned_approach = generator.robot.compute_ik.call_args_list[0].kwargs["pose"] - assert torch.allclose( - planned_approach[:, :3, 3], - torch.tensor([0.0, 0.2, 0.3]).expand(NUM_ENVS, -1), - ) - - -def test_press_fk_path_passes_contact_and_remains_on_press_axis() -> None: - generator = _motion_generator() - - def position_ik( - pose: torch.Tensor, - name: str, - joint_seed: torch.Tensor, - ) -> tuple[torch.Tensor, torch.Tensor]: - qpos = joint_seed.clone() - qpos[:, :3] = pose[:, :3, 3] - return torch.ones(NUM_ENVS, dtype=torch.bool), qpos - - def position_fk( - qpos: torch.Tensor, - name: str, - to_matrix: bool, - ) -> torch.Tensor: - pose = torch.eye(4).repeat(qpos.shape[0], 1, 1) - pose[:, :3, 3] = qpos[:, :3] - return pose - - generator.robot.compute_ik.side_effect = position_ik - generator.robot.compute_fk.side_effect = position_fk - semantics = ObjectSemantics( - affordance=PressAffordance( - press_axis=torch.tensor([1.0, 0.0, 0.0]), - press_position=(0.0, 0.0, 0.0), - ), - geometry={}, - label="button", - ) - action = _bind_action(generator, Press()) - plan = _plan_action( - action, - ActionInvocation( - skill_id="press", - goal=PressGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=24), - skill_options=PressOptions(hand_interp_steps=3, press_distance=0.04), - ), - _context(), - ) - - trajectory = _joint_trajectory(plan) - contact_arm = trajectory.positions[:, plan.segment("contact").stop - 1, :ARM_DOF] - contact_fk = position_fk(contact_arm, "arm", True) - assert torch.allclose(contact_fk[:, :3, 3], torch.zeros(NUM_ENVS, 3)) - press_segment = plan.segment("press") - press_arm = trajectory.positions[ - :, press_segment.start : press_segment.stop, :ARM_DOF - ] - press_fk = position_fk(press_arm.reshape(-1, ARM_DOF), "arm", True).reshape( - NUM_ENVS, -1, 4, 4 - ) - positions = press_fk[:, :, :3, 3] - axis = torch.tensor([1.0, 0.0, 0.0]) - orthogonal = positions - (positions * axis).sum(dim=-1, keepdim=True) * axis - assert torch.allclose(orthogonal, torch.zeros_like(orthogonal), atol=1.0e-6) - assert torch.allclose(positions[:, -1], torch.tensor([0.04, 0.0, 0.0])) - - -def test_press_preserves_failed_environment_at_observed_qpos() -> None: - semantics = ObjectSemantics( - affordance=PressAffordance( - press_axis=torch.tensor([1.0, 0.0, 0.0]), - press_position=(0.0, 0.0, 0.0), - ), - geometry={}, - label="button", - ) - generator = _motion_generator() - - def partial_ik( - pose: torch.Tensor | None = None, - name: str | None = None, - joint_seed: torch.Tensor | None = None, - **_: object, - ) -> tuple[torch.Tensor, torch.Tensor]: - assert joint_seed is not None - return torch.tensor([True, False]), torch.ones_like(joint_seed) - - generator.robot.compute_ik.side_effect = partial_ik - action = _bind_action(generator, Press()) - context = _context() - - plan = _plan_action( - action, - ActionInvocation( - skill_id="press", - goal=PressGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy(sample_count=18), - skill_options=PressOptions(hand_interp_steps=3), - ), - context, - ) - - assert plan.plan_success.tolist() == [True, False] - trajectory = _joint_trajectory(plan) - assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) - assert torch.allclose( - trajectory.positions[1], - context.robot.qpos[1].unsqueeze(0).expand(18, -1), - ) - - -def test_press_rejects_non_press_affordance() -> None: - semantics = ObjectSemantics( - affordance=AntipodalAffordance( - mesh_vertices=torch.zeros(8, 3), - mesh_triangles=torch.zeros(4, 3, dtype=torch.long), - ), - geometry={}, - label="mesh-button", - ) - action = _bind_action(_motion_generator(), Press()) - - with pytest.raises(ValueError, match="PressAffordance"): - _plan_action( - action, - _invocation(action, PressGoal(semantics, torch.eye(4))), - _context(), - ) - - -def test_press_requires_primary_arm_and_end_effector_bindings() -> None: - semantics = ObjectSemantics( - affordance=AntipodalAffordance(), - geometry={}, - label="button", - ) - action = _bind_action(_motion_generator(), Press()) - invocation = ActionInvocation( - skill_id="press", - goal=PressGoal(semantics, torch.eye(4)), - binding=ActionBinding( - owner_id=_ACTION_ENGINES[id(action)].binding_owner_id, - ), - ) - - with pytest.raises(ValueError, match="missing=.*grasp"): - action.resolve_request(invocation) - - -def test_press_axis_belongs_to_affordance_not_action_options() -> None: - assert "press_axis" not in PressOptions.__dataclass_fields__ - - -@pytest.mark.parametrize( - "press_position", - ((0.0, 1.0), (0.0, 1.0, float("nan"))), -) -def test_press_options_reject_invalid_press_position( - press_position: tuple[float, ...], -) -> None: - with pytest.raises(ValueError, match="press_position"): - PressOptions(press_position=press_position) # type: ignore[arg-type] - - -def test_twist_rejects_non_twist_affordance() -> None: - semantics = ObjectSemantics( - affordance=AntipodalAffordance( - mesh_vertices=torch.zeros(8, 3), - mesh_triangles=torch.zeros(4, 3, dtype=torch.long), - ), - geometry={}, - label="mesh-knob", - ) - action = _bind_action(_motion_generator(), Twist()) - - with pytest.raises(ValueError, match="TwistAffordance"): - _plan_action( - action, - _invocation(action, TwistGoal(semantics, torch.eye(4))), - _context(), - ) - - -def test_twist_axis_belongs_to_affordance_not_action_options() -> None: - assert "twist_axis" not in TwistOptions.__dataclass_fields__ - assert "approach_direction" not in TwistOptions.__dataclass_fields__ - - -def test_twist_options_reject_non_finite_pre_grasp_distance() -> None: - with pytest.raises(ValueError, match="pre_grasp_distance must be finite"): - TwistOptions(pre_grasp_distance=float("nan")) - - -def test_slide_rejects_non_slide_affordance() -> None: - semantics = ObjectSemantics( - affordance=AntipodalAffordance( - mesh_vertices=torch.zeros(8, 3), - mesh_triangles=torch.zeros(4, 3, dtype=torch.long), - ), - geometry={}, - label="mesh-handle", - ) - action = _bind_action(_motion_generator(), Slide()) - - with pytest.raises(ValueError, match="SlideAffordance"): - _plan_action( - action, - _invocation( - action, - SlideGoal(semantics, torch.eye(4)), - ), - _context(), - ) - - -def test_slide_requires_primary_end_effector() -> None: - semantics = ObjectSemantics( - affordance=AntipodalAffordance(), - geometry={}, - label="drawer_handle", - ) - action = _bind_action(_motion_generator(), Slide()) - invocation = ActionInvocation( - skill_id="slide", - goal=SlideGoal(semantics, torch.eye(4)), - binding=ActionBinding( - owner_id=_ACTION_ENGINES[id(action)].binding_owner_id, - ), - ) - - with pytest.raises(ValueError, match="missing=.*grasp"): - action.resolve_request(invocation) - - -def test_slide_axis_belongs_to_affordance_not_action_options() -> None: - assert "translation_axis" not in SlideOptions.__dataclass_fields__ - - -def test_slide_options_reject_invalid_direction() -> None: - with pytest.raises(ValueError, match="direction"): - SlideOptions(direction="open") # type: ignore[arg-type] - - -def test_handover_does_not_mutate_cached_final_pose( - monkeypatch: pytest.MonkeyPatch, -) -> None: - generator = _dual_motion_generator() - handover_options = HandOverOptions( - middle_object_pose=torch.eye(4), - final_object_pose=torch.eye(4), - hand_interp_steps=4, - hold_steps=2, - retreat_steps=5, - ) - action = _bind_action( - generator, - HandOver(default_options=handover_options), + action = _bind_action( + generator, + HandOver(default_options=handover_options), ) assert handover_options.final_object_pose is not None original_final_pose = handover_options.final_object_pose.clone() @@ -2101,14 +1851,13 @@ def plan_from_start( ) assert torch.equal(handover_options.final_object_pose, original_final_pose) semantics.entity.get_local_pose.assert_not_called() - assert [segment.name for segment in plan.segments] == [ - "transfer", - "approach", - "close", - "hold", - "release", - "deliver", - ] + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_handover_replan_resolves_named_targets_from_latest_snapshot() -> None: @@ -2196,7 +1945,7 @@ def fail_second_receiving_arm( task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": _held(semantics)}, + held_objects={"logical_source": _held(semantics)}, ) action = _bind_action( generator, @@ -2220,7 +1969,15 @@ def fail_second_receiving_arm( invocation = ActionInvocation( skill_id="hand_over", goal=GraspGoal(semantics=semantics), - binding=_dual_binding(action, "source", "destination"), + binding=_dual_binding( + action, + "source", + "destination", + task_state_keys={ + "source": "logical_source", + "destination": "logical_destination", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) @@ -2236,9 +1993,10 @@ def fail_second_receiving_arm( context.robot.qpos[1].unsqueeze(0).expand(30, -1), ) assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) - received = projected.get_held_object("right_arm") + received = projected.get_held_object("logical_destination") assert received is not None assert received.env_mask.tolist() == [True, False] + assert projected.get_held_object("right_arm") is None semantics.entity.get_local_pose.assert_not_called() @@ -2273,70 +2031,24 @@ def test_handover_rejects_goal_for_a_different_held_object() -> None: goal_semantics.entity.get_local_pose.assert_not_called() -def test_handover_transfers_only_exclusively_held_rows() -> None: - generator = _dual_motion_generator() - semantics = _semantics() - task = TaskState( - batch_size=NUM_ENVS, - device="cpu", - held_objects={ - "left_arm": _held(semantics), - "right_arm": _held( - semantics, - env_mask=torch.tensor([True, False]), - ), - }, - ) - action = _bind_action( - generator, - HandOver( - default_options=HandOverOptions( - middle_object_pose=torch.eye(4), - final_object_pose=torch.eye(4), - hand_interp_steps=4, - hold_steps=2, - retreat_steps=5, - ) - ), - ) - action._resolve_receive_grasp = Mock( - return_value=( - torch.eye(4).repeat(NUM_ENVS, 1, 1), - torch.ones(NUM_ENVS, dtype=torch.bool), - ) - ) - context = _dual_context(task) - invocation = ActionInvocation( - skill_id="hand_over", - goal=GraspGoal(semantics=semantics), - binding=_dual_binding(action, "source", "destination"), - motion_policy=MotionPolicy(sample_count=30), - ) - - plan = _plan_action(action, invocation, context) - projected = plan.expected_effects.apply(context.task, plan.plan_success) - - assert plan.plan_success.tolist() == [False, True] - trajectory = _joint_trajectory(plan) - assert torch.allclose( - trajectory.positions[0], - context.robot.qpos[0].unsqueeze(0).expand(30, -1), - ) - transferred = projected.get_held_object("left_arm") - received = projected.get_held_object("right_arm") - assert transferred is not None and transferred.env_mask.tolist() == [True, False] - assert received is not None and received.env_mask.tolist() == [True, True] - assert received.semantics is semantics - - -def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None: +@pytest.mark.parametrize( + ("hold_steps", "expected_segments"), + ( + (0, ("approach", "close", "lift", "move")), + (2, ("approach", "close", "lift", "move", "hold")), + ), +) +def test_coordinated_pick_returns_full_dof_plan_and_omits_empty_hold( + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() action = _bind_action( generator, CoordinatedPickment( default_options=CoordinatedPickmentOptions( hand_interp_steps=4, - hold_steps=2, + hold_steps=hold_steps, object_motion_keyframes=3, ), ), @@ -2359,7 +2071,15 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None object_target_pose=torch.eye(4), object_initial_pose=torch.eye(4), ), - binding=_dual_binding(action, "left", "right"), + binding=_dual_binding( + action, + "left", + "right", + task_state_keys={ + "left": "logical_left", + "right": "logical_right", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context() @@ -2374,12 +2094,6 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None 30, DUAL_ROBOT_DOF, ) - left_held = projected.get_held_object("left_arm") - right_held = projected.get_held_object("right_arm") - assert isinstance(left_held, HeldObjectState) - assert isinstance(right_held, HeldObjectState) - assert left_held.semantics is right_held.semantics - assert left_held.semantics is not semantics assert plan.commands.frame_count == 30 assert {target.target_id for target in plan.commands.targets} == { "left_arm", @@ -2389,13 +2103,20 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None } assert plan.scene_dependencies == () request.goal.semantics.entity.get_local_pose.assert_not_called() - assert [segment.name for segment in plan.segments] == [ - "approach", - "close", - "lift", - "move", - "hold", - ] + assert projected.get_held_object("logical_left") is None + assert projected.get_held_object("logical_right") is None + assert isinstance( + projected.get_coordinated_held_object("logical_left", "logical_right"), + CoordinatedHeldObjectState, + ) + assert projected.get_coordinated_held_object("left_arm", "right_arm") is None + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: @@ -2448,12 +2169,10 @@ def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: assert torch.equal(sampled_pose, object_pose) assert plan.scene_dependencies == ("target",) request.goal.semantics.entity.get_local_pose.assert_not_called() - left_held = projected.get_held_object("left_arm") - right_held = projected.get_held_object("right_arm") - assert left_held is not None and right_held is not None - assert left_held.semantics is right_held.semantics - assert torch.allclose(left_held.object_to_eef, pose_inv(object_pose)) - assert torch.allclose(right_held.object_to_eef, pose_inv(object_pose)) + coordinated = projected.get_coordinated_held_object("left_arm", "right_arm") + assert coordinated is not None + assert torch.allclose(coordinated.left_object_to_eef, pose_inv(object_pose)) + assert torch.allclose(coordinated.right_object_to_eef, pose_inv(object_pose)) def test_assemble_place_uses_explicit_base_snapshot() -> None: @@ -2470,7 +2189,7 @@ def test_assemble_place_uses_explicit_base_snapshot() -> None: task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"arm": _held(_semantics(entity_id="assemble_object"))}, + held_objects={"logical_arm": _held(_semantics(entity_id="assemble_object"))}, ) base_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) base_pose[:, 0, 3] = torch.tensor([0.2, 0.4]) @@ -2484,12 +2203,15 @@ def test_assemble_place_uses_explicit_base_snapshot() -> None: ) request = action.resolve_request( - _invocation( - action, - AssembleGoal( - affordance=affordance, - base_pose=SceneEntityPose("base"), + replace( + _invocation( + action, + AssembleGoal( + affordance=affordance, + base_pose=SceneEntityPose("base"), + ), ), + binding=_binding(action, task_state_key="logical_arm"), ) ) plan = action.plan(request, context) @@ -2582,12 +2304,9 @@ def fail_second_environment( context.robot.qpos[1].unsqueeze(0).repeat(30, 1), ) assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) - left_held = projected.get_held_object("left_arm") - right_held = projected.get_held_object("right_arm") - assert left_held is not None and right_held is not None - assert left_held.semantics is right_held.semantics - assert left_held.env_mask.tolist() == [True, False] - assert right_held.env_mask.tolist() == [True, False] + coordinated = projected.get_coordinated_held_object("left_arm", "right_arm") + assert coordinated is not None + assert coordinated.env_mask.tolist() == [True, False] def test_coordinated_pick_fails_when_affordance_has_no_grasp() -> None: @@ -2645,14 +2364,26 @@ def test_coordinated_pick_fails_when_affordance_has_no_grasp() -> None: ) -def test_coordinated_placement_projects_release_and_support_attachment() -> None: +@pytest.mark.parametrize( + ("release", "hold_steps", "expected_segments"), + ( + (False, 0, ("approach", "retreat")), + (True, 3, ("approach", "hold", "release", "retreat")), + ), +) +def test_coordinated_placement_projects_effects_and_omits_empty_segments( + release: bool, + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() action = _bind_action( generator, CoordinatedPlacement( default_options=CoordinatedPlacementOptions( + release=release, hand_interp_steps=4, - hold_steps=3, + hold_steps=hold_steps, retreat_steps=5, ), ), @@ -2666,7 +2397,10 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": placing, "right_arm": support}, + held_objects={ + "logical_placing": placing, + "logical_support": support, + }, ) invocation = ActionInvocation( skill_id="coordinated_placement", @@ -2674,7 +2408,15 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None placing_object_target_pose=torch.eye(4), support_object_target_pose=torch.eye(4), ), - binding=_dual_binding(action, "placing", "support"), + binding=_dual_binding( + action, + "placing", + "support", + task_state_keys={ + "placing": "logical_placing", + "support": "logical_support", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context(task) @@ -2690,15 +2432,23 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None "right_arm", "right_hand", } - assert projected.get_held_object("left_arm") is None - assert projected.get_held_object("right_arm") is not None - assert projected.get_held_object("right_arm").semantics is support.semantics - assert [segment.name for segment in plan.segments] == [ - "approach", - "hold", - "release", - "retreat", - ] + projected_placing = projected.get_held_object("logical_placing") + if release: + assert projected_placing is None + else: + assert projected_placing is not None + assert projected_placing.semantics is placing.semantics + assert torch.equal(projected_placing.object_to_eef, placing.object_to_eef) + assert projected.get_held_object("logical_support") is not None + assert projected.get_held_object("logical_support").semantics is support.semantics + assert projected.get_held_object("right_arm") is None + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_coordinated_placement_rejects_one_object_held_by_both_arms() -> None: diff --git a/tests/sim/atomic_actions/test_articulation_effects.py b/tests/sim/atomic_actions/test_articulation_effects.py new file mode 100644 index 000000000..26603cb89 --- /dev/null +++ b/tests/sim/atomic_actions/test_articulation_effects.py @@ -0,0 +1,120 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for verified articulation state and masked symbolic effects.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + StateDelta, + TaskState, +) + + +def test_task_state_normalizes_and_owns_articulation_joint_state() -> None: + position = torch.tensor([0.35], dtype=torch.float32) + state = TaskState( + batch_size=2, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState(position), + }, + ) + + position.fill_(99.0) + observed = state.get_articulation_joint_state("drawer", "slide") + assert observed is not None + assert torch.equal(observed.position, torch.tensor([[0.35], [0.35]])) + assert torch.equal(observed.env_mask, torch.tensor([True, True])) + + +def test_state_delta_merges_articulation_rows_without_overwriting_others() -> None: + state = TaskState( + batch_size=3, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState( + torch.tensor([[0.0], [0.1], [0.2]]), + ) + }, + ) + candidate = ArticulationJointState( + torch.tensor([[0.5], [0.6], [0.7]]), + env_mask=torch.tensor([True, True, False]), + ) + + updated = StateDelta( + articulation_joint_updates={("drawer", "slide"): candidate} + ).apply(state, torch.tensor([True, False, True])) + + joint = updated.get_articulation_joint_state("drawer", "slide") + assert joint is not None + assert torch.equal(joint.position, torch.tensor([[0.5], [0.1], [0.7]])) + assert torch.equal(joint.env_mask, torch.tensor([True, True, False])) + + +def test_state_delta_removes_only_selected_articulation_rows() -> None: + state = TaskState( + batch_size=2, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + }, + ) + updated = StateDelta(articulation_joint_updates={("drawer", "slide"): None}).apply( + state, torch.tensor([False, True]) + ) + + joint = updated.get_articulation_joint_state("drawer", "slide") + assert joint is not None + assert torch.equal(joint.env_mask, torch.tensor([True, False])) + + removed = StateDelta(articulation_joint_updates={("drawer", "slide"): None}).apply( + updated, torch.tensor([True, False]) + ) + assert removed.get_articulation_joint_state("drawer", "slide") is None + + +def test_articulation_state_and_delta_validate_strictly() -> None: + with pytest.raises(TypeError, match="floating"): + ArticulationJointState(torch.tensor([1], dtype=torch.long)) + with pytest.raises(ValueError, match="finite"): + ArticulationJointState(torch.tensor([float("nan")])) + with pytest.raises(ValueError, match="articulation/joint pairs"): + StateDelta(articulation_joint_updates={("drawer", ""): None}) + with pytest.raises(TypeError, match="ArticulationJointState"): + StateDelta( + articulation_joint_updates={("drawer", "slide"): torch.tensor([0.1])} + ) + + +def test_articulation_state_delta_snapshot_is_independently_owned() -> None: + source = ArticulationJointState(torch.tensor([[0.2], [0.3]])) + delta = StateDelta(articulation_joint_updates={("drawer", "slide"): source}) + snapshot = delta.snapshot() + copied = snapshot.articulation_joint_updates[("drawer", "slide")] + + assert copied is not None + assert copied is not source + assert copied.position.data_ptr() != source.position.data_ptr() + assert torch.equal(copied.position, source.position) + + +__all__: list[str] = [] diff --git a/tests/sim/atomic_actions/test_control.py b/tests/sim/atomic_actions/test_control.py index 0bae379fe..16fd6649e 100644 --- a/tests/sim/atomic_actions/test_control.py +++ b/tests/sim/atomic_actions/test_control.py @@ -174,6 +174,64 @@ def test_control_profile_is_resolved_from_robot_control_part() -> None: ) +def test_direct_binding_shares_motion_task_state_key_across_slot_endpoints() -> None: + resolved = _binding(_services()) + + assert resolved.endpoint("primary", "motion").task_state_key == "arm" + assert resolved.endpoint("primary", "grasp").task_state_key == "arm" + + +def test_direct_binding_accepts_explicit_stable_task_state_key() -> None: + resolved = _services().bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + task_state_keys={"primary": "logical_manipulator"}, + ) + + assert {endpoint.task_state_key for endpoint in resolved.endpoints} == { + "logical_manipulator" + } + + +def test_direct_binding_without_motion_requires_unambiguous_task_state_key() -> None: + contract = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement(endpoint_id="grasp"), + SkillEndpointRequirement(endpoint_id="support"), + ), + ), + ) + ) + endpoints = {"primary": {"grasp": "hand", "support": "arm"}} + services = _services() + + with pytest.raises(ValueError, match="no 'motion'.*task_state_keys"): + services.bind_control_parts(contract, endpoints) + + resolved = services.bind_control_parts( + contract, + endpoints, + task_state_keys={"primary": "logical_manipulator"}, + ) + assert {endpoint.task_state_key for endpoint in resolved.endpoints} == { + "logical_manipulator" + } + + +def test_direct_binding_requires_exact_task_state_key_slot_coverage() -> None: + services = _services() + + with pytest.raises(ValueError, match="cover the binding slots exactly"): + services.bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + task_state_keys={}, + ) + + def test_invocation_override_replaces_only_resolved_endpoint_snapshot() -> None: services = _services() override_source = torch.full((2,), 0.4) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 45351344e..72d7ed079 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -37,6 +37,7 @@ EndpointCommand, EndEffectorPoseGoal, EntityState, + EffectVerificationRequirement, ExecutionFeedbackMode, HeldObjectState, JointPositionPayload, @@ -191,6 +192,11 @@ def _action_plan( plan_success: torch.Tensor | None = None, joint_trajectory: TimedTrajectory | None = None, feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, + expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, + diagnostics: PlannerDiagnostics | None = None, + scene_dependencies: tuple[str, ...] = (), + scene_dependency_monitor_until: dict[str, int] | None = None, ) -> ActionPlan: if plan_success is None: plan_success = torch.ones( @@ -205,10 +211,71 @@ def _action_plan( recovery_policy=RecoveryPolicy(), planned_scene_version=0, planned_collision_world_revision=(0,) * commands.batch_size, - diagnostics=PlannerDiagnostics(backend="test"), + diagnostics=( + PlannerDiagnostics(backend="test") if diagnostics is None else diagnostics + ), feedback_mode=feedback_mode, joint_trajectory=joint_trajectory, + scene_dependencies=scene_dependencies, + scene_dependency_monitor_until=( + {} + if scene_dependency_monitor_until is None + else scene_dependency_monitor_until + ), + expected_effects=StateDelta() if expected_effects is None else expected_effects, + effect_verification=effect_verification, + ) + + +@pytest.mark.parametrize("kind", ("", " physical", "physical ", 1, True)) +def test_effect_verification_requirement_rejects_invalid_kind(kind: object) -> None: + with pytest.raises(ValueError, match="kind"): + EffectVerificationRequirement(kind=kind) # type: ignore[arg-type] + + +def test_action_plan_owns_explicit_effect_verification_requirement() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, ) + requirement = EffectVerificationRequirement(kind="articulation.joint_progress") + + plan = _action_plan(commands, effect_verification=requirement) + requirement_snapshot = plan.effect_verification + + assert plan.requires_effect_verification is True + assert requirement_snapshot is not None + assert requirement_snapshot is not requirement + assert requirement_snapshot.kind == requirement.kind + assert requirement_snapshot.snapshot() is not requirement_snapshot + + +def test_action_plan_implicitly_verifies_nonempty_state_delta() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + effects = StateDelta(held_object_updates={"arm": _held(batch_size=1)}) + + implicit = _action_plan(commands, expected_effects=effects) + no_effect = _action_plan(commands) + + assert implicit.effect_verification is None + assert implicit.requires_effect_verification is True + assert no_effect.requires_effect_verification is False + + +def test_action_plan_rejects_untyped_effect_verification_requirement() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + with pytest.raises(TypeError, match="EffectVerificationRequirement"): + _action_plan( + commands, + effect_verification=object(), # type: ignore[arg-type] + ) class _DependencyAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): @@ -734,6 +801,28 @@ def test_build_plan_uses_action_scene_dependency_hook() -> None: assert plan.scene_dependencies == ("extra", "tracked") +def test_build_segments_omits_zero_length_entry_and_preserves_offsets() -> None: + approach_length = 2 + release_length = 3 + segment_lengths = { + "approach": approach_length, + "hold": 0, + "release": release_length, + } + + segments = AtomicAction._build_segments( + segment_lengths, + frame_count=sum(segment_lengths.values()), + ) + + assert tuple( + (segment.name, segment.start, segment.stop) for segment in segments + ) == ( + ("approach", 0, approach_length), + ("release", approach_length, approach_length + release_length), + ) + + def test_build_command_plan_rejects_unbound_runtime_destination() -> None: context = _context() generator = Mock() @@ -865,25 +954,6 @@ def test_command_target_authorization_rejects_custom_claim_conflicts() -> None: ) -def test_action_plan_rejects_unknown_scene_dependency_end_segment() -> None: - plan = _action_plan( - _command_sequence( - env_ids=torch.tensor([0, 1], dtype=torch.long), - frame_count=2, - ) - ) - - with pytest.raises( - ValueError, - match="scene_dependency_end_segment must name an ActionPlan segment", - ): - replace( - plan, - scene_dependencies=("target",), - scene_dependency_end_segment="approach", - ) - - def test_action_plan_owns_commands_and_optional_joint_trajectory() -> None: env_ids = torch.tensor([4, 7], dtype=torch.long) commands = _command_sequence(env_ids=env_ids, frame_count=2) @@ -927,6 +997,90 @@ def test_action_plan_owns_commands_and_optional_joint_trajectory() -> None: assert torch.equal(plan.joint_trajectory.positions, trajectory_positions) +def test_planner_diagnostics_and_plan_snapshots_own_nested_metadata() -> None: + nested = {"solver": {"iterations": [3, 5]}} + diagnostics = PlannerDiagnostics(backend="test", metadata=nested) + plan = _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ), + diagnostics=diagnostics, + ) + + nested["solver"]["iterations"][0] = 99 + diagnostics.metadata["solver"]["iterations"][1] = 77 + snapshot = plan.snapshot() + plan.diagnostics.metadata["solver"]["iterations"][0] = 42 + + assert snapshot.diagnostics.metadata["solver"]["iterations"] == [3, 5] + + +def test_planner_diagnostics_rejects_non_string_messages() -> None: + with pytest.raises(TypeError, match="messages must contain strings"): + PlannerDiagnostics( + backend="test", + messages=("valid", 1), # type: ignore[arg-type] + ) + + +def test_action_plan_owns_scene_dependency_monitor_cutoffs() -> None: + source = {"disabled": 0, "full_sequence": 2} + plan = _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("disabled", "full_sequence"), + scene_dependency_monitor_until=source, + ) + + source["disabled"] = 1 + source["full_sequence"] = 1 + snapshot = plan.snapshot() + + assert plan.scene_dependency_monitor_until == { + "disabled": 0, + "full_sequence": 2, + } + assert snapshot.scene_dependency_monitor_until == { + "disabled": 0, + "full_sequence": 2, + } + assert snapshot.scene_dependency_monitor_until is not ( + plan.scene_dependency_monitor_until + ) + + +@pytest.mark.parametrize("waypoint_index", (-1, 3, True, 1.5)) +def test_action_plan_rejects_invalid_scene_dependency_monitor_cutoff( + waypoint_index: object, +) -> None: + with pytest.raises(ValueError, match="waypoint indices"): + _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("tracked",), + scene_dependency_monitor_until={ + "tracked": waypoint_index # type: ignore[dict-item] + }, + ) + + +def test_action_plan_rejects_monitor_cutoff_for_non_dependency() -> None: + with pytest.raises(ValueError, match="keys must be scene dependencies"): + _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("tracked",), + scene_dependency_monitor_until={"other": 1}, + ) + + def test_action_plan_allows_timed_commands_without_joint_trajectory() -> None: commands = _command_sequence( env_ids=torch.tensor([4], dtype=torch.long), diff --git a/tests/sim/atomic_actions/test_engine.py b/tests/sim/atomic_actions/test_engine.py index da8b75408..6c57c9f07 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -350,11 +350,14 @@ def test_engine_resolves_action_binding_from_robot_control_parts() -> None: resolved = engine.bind_control_parts( "stub", {"primary": {"motion": "all"}}, + task_state_keys={"primary": "logical_robot"}, ) - target = resolved.endpoint("primary", "motion").require_target(JointPositionTarget) + endpoint = resolved.endpoint("primary", "motion") + target = endpoint.require_target(JointPositionTarget) assert target.control_part == "all" assert target.joint_ids == (0, 1, 2) + assert endpoint.task_state_key == "logical_robot" def test_engine_make_invocation_binds_direct_control_parts() -> None: diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index d4f833d42..7d6c9d9ea 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -20,6 +20,7 @@ from collections.abc import Sequence from dataclasses import replace +import math from typing import ClassVar from unittest.mock import Mock @@ -43,6 +44,7 @@ ExecutionSession, ExecutionStatus, ExecutionTick, + EffectVerificationRequirement, EffectVerificationResult, GraspGoal, HeldObjectState, @@ -50,6 +52,7 @@ JointPositionTarget, MotionPolicy, ObjectSemantics, + PlannerDiagnostics, PlanningContext, RecoveryPolicy, ResolvedActionRequest, @@ -149,10 +152,10 @@ def _plan( ) -class StagedDynamicAction(DynamicAction): - """Monitor its scene target only during a pre-contact segment.""" +class VerificationOnlyAction(DynamicAction): + """Dynamic action requiring a physical check without symbolic effects.""" - skill_id: ClassVar[str] = "staged_dynamic" + skill_id: ClassVar[str] = "verification_only" binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( @@ -160,14 +163,21 @@ def _plan( request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], context: PlanningContext, ) -> ActionPlan: - plan = super()._plan(request, context) - return replace( - plan, - segments=( - TrajectorySegment("approach", 0, 1), - TrajectorySegment("manipulate", 1, 2), + goal = self.require_goal(request) + pose = resolve_pose_goal(goal.xpos, context, name="xpos") + target = pose[:, 0, 3].unsqueeze(1).expand_as(context.robot.qpos) + return self.build_plan( + request, + context, + success=True, + trajectory=TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, target], dim=1), + env_ids=context.env_ids, + step_dt=0.1, + ), + effect_verification=EffectVerificationRequirement( + kind="physical.test_completion" ), - scene_dependency_end_segment="approach", ) @@ -186,6 +196,65 @@ def _plan( return replace(plan, plan_success=torch.zeros_like(plan.plan_success)) +class DiagnosticAction(DynamicAction): + """Dynamic action exposing its installed plan for snapshot isolation tests.""" + + skill_id: ClassVar[str] = "diagnostic" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def __init__(self) -> None: + super().__init__() + self.metadata = {"solver": {"iterations": [3, 5]}} + self.returned_plans: list[ActionPlan] = [] + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + plan = replace( + plan, + diagnostics=PlannerDiagnostics( + backend="diagnostic", + metadata=self.metadata, + ), + ) + self.returned_plans.append(plan) + return plan + + +class WindowedDependencyAction(DynamicAction): + """Stop monitoring a goal pose once its first command was issued.""" + + skill_id: ClassVar[str] = "windowed_dependency" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + return replace( + plan, + scene_dependency_monitor_until={"target": 1}, + ) + + +class MultiDependencyAction(DynamicAction): + """Track the goal plus one auxiliary scene dependency.""" + + skill_id: ClassVar[str] = "multi_dependency" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _scene_dependencies( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + ) -> tuple[str, ...]: + return tuple(sorted((*super()._scene_dependencies(request), "obstacle"))) + + class MixedEffectAction(EffectAction): """Effect action whose final environment row always fails planning.""" @@ -409,6 +478,41 @@ def _context( ) +def _multi_dependency_context( + timestamp: float, + *, + target_x: float, + obstacle_x: float | None, + version: int, + target_yaw: float = 0.0, +) -> PlanningContext: + """Build one-row context with an optional auxiliary dependency.""" + context = _context(timestamp, 0.0, target_x, version) + entities = dict(context.scene.entities) + target_pose = entities["target"].pose + cosine = math.cos(target_yaw) + sine = math.sin(target_yaw) + target_pose[:, 0, 0] = cosine + target_pose[:, 0, 1] = -sine + target_pose[:, 1, 0] = sine + target_pose[:, 1, 1] = cosine + entities["target"] = EntityState(target_pose) + if obstacle_x is not None: + obstacle_pose = torch.eye(4).unsqueeze(0) + obstacle_pose[:, 0, 3] = obstacle_x + entities["obstacle"] = EntityState(obstacle_pose) + return PlanningContext( + robot=context.robot, + task=context.task, + scene=SceneSnapshot( + timestamp=timestamp, + version=version, + entities=entities, + ), + env_ids=context.env_ids, + ) + + def _collision_context( timestamp: float, qpos: torch.Tensor, @@ -489,6 +593,7 @@ def _destination_invocation( "second": "arm_b", } }, + task_state_keys={"primary": "destination_resource"}, ), recovery_policy=RecoveryPolicy( max_replans=2, @@ -505,7 +610,7 @@ def _effect_session( max_action_retries: int = 2, action_timeout: float = 30.0, eligible_mask: torch.Tensor | None = None, - action: EffectAction | None = None, + action: DynamicAction | None = None, ) -> tuple[ExecutionSession, ExecutionTick]: """Advance a test effect action to its verification boundary.""" engine, _ = _engine(batch_size=batch_size) @@ -561,6 +666,109 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +def test_all_rows_planning_failure_skips_inactive_command_frames() -> None: + engine, _ = _engine() + action = FailedEffectAction() + engine.register(action) + base = _invocation(engine, max_action_retries=0) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + + failed = session.tick(_context(0.0, 0.0, 0.2, 0)) + + assert failed.command is None + assert failed.status is ExecutionStatus.FAILED + assert failed.eligible_mask.tolist() == [False] + assert any( + event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + for event in failed.events + ) + + +def test_plan_attempt_records_snapshot_nested_metadata_at_installation() -> None: + engine, _ = _engine() + action = DiagnosticAction() + engine.register(action) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + + action.metadata["solver"]["iterations"][0] = 99 + action.returned_plans[0].diagnostics.metadata["solver"]["iterations"][1] = 77 + first_read = session.plan_attempts[0] + first_read.plan.diagnostics.metadata["solver"]["iterations"][0] = 42 + second_read = session.plan_attempts[0] + + assert second_read.plan.diagnostics.metadata["solver"]["iterations"] == [3, 5] + + +def test_scene_dependency_window_ignores_expected_self_motion() -> None: + engine, _ = _engine() + action = WindowedDependencyAction() + engine.register(action) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + + first = session.tick(_context(0.0, 0.0, 0.2, 0)) + moved = session.tick(_context(0.1, 0.0, 0.8, 1)) + + assert first.command is not None + assert moved.command is not None + assert action.plan_count == 1 + assert not any( + event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED for event in moved.events + ) + + +def test_scene_dependency_window_reports_motion_before_cutoff() -> None: + engine, _ = _engine() + action = WindowedDependencyAction() + engine.register(action) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + + moved = session.tick(_context(0.1, 0.0, 0.8, 1)) + + changed = next( + event + for event in moved.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=0: " + "entity_id='target', monitor_cutoff=1, max_translation=0.600000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266." + ) + assert action.plan_count == 2 + + def test_initial_eligibility_is_sticky_across_invocation_barriers() -> None: engine, _ = _engine(batch_size=2) invocation = _invocation(engine) @@ -666,6 +874,7 @@ def test_deactivate_rows_is_sticky_and_masks_the_next_command() -> None: assert deactivated[0].env_mask.tolist() == [False, True] assert deactivated[0].message == "environment terminated" + def test_session_commands_schedule_arrivals_and_final_settling() -> None: engine, _ = _engine() engine.register(NonuniformTimingAction()) @@ -734,32 +943,102 @@ def test_scene_motion_replans_late_bound_goal() -> None: tick = session.tick(_context(0.1, 0.0, 0.3, 1)) kinds = {event.kind for event in tick.events} + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in kinds assert ExecutionEventKind.REPLANNED in kinds + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='target', monitor_cutoff=none, max_translation=0.200000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266." + ) assert action.plan_count == 2 assert action.requests[0] is action.requests[1] assert tick.command is not None -def test_scene_motion_is_ignored_after_dependency_segment_is_dispatched() -> None: +def test_scene_motion_diagnostic_orders_multiple_changed_entities() -> None: engine, _ = _engine() - action = StagedDynamicAction() + action = MultiDependencyAction() engine.register(action) - initial = _context(0.0, 0.0, 0.1, 0) + initial = _multi_dependency_context( + 0.0, + target_x=0.1, + obstacle_x=0.4, + version=0, + ) session = engine.start( - (_invocation(engine, skill_id=StagedDynamicAction.skill_id),), + (_invocation(engine, skill_id=action.skill_id),), initial, ) + session.tick(initial) - approach = session.tick(initial) - after_contact = session.tick(_context(0.1, 0.0, 0.4, 1)) + tick = session.tick( + _multi_dependency_context( + 0.1, + target_x=0.4, + obstacle_x=0.8, + version=1, + target_yaw=0.2, + ) + ) - assert approach.command is not None - assert after_contact.command is not None - assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED not in { - event.kind for event in after_contact.events - } - assert action.plan_count == 1 + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='obstacle', monitor_cutoff=none, max_translation=0.400000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266 | " + "entity_id='target', monitor_cutoff=none, max_translation=0.300000, " + "translation_threshold=0.020000, max_rotation=0.200000, " + "rotation_threshold=0.087266." + ) + + +def test_scene_motion_diagnostic_identifies_missing_entity() -> None: + engine, _ = _engine() + action = MultiDependencyAction() + engine.register(action) + initial = _multi_dependency_context( + 0.0, + target_x=0.1, + obstacle_x=0.4, + version=0, + ) + session = engine.start( + (_invocation(engine, skill_id=action.skill_id),), + initial, + ) + session.tick(initial) + + tick = session.tick( + _multi_dependency_context( + 0.1, + target_x=0.1, + obstacle_x=None, + version=1, + ) + ) + + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='obstacle', monitor_cutoff=none, missing=current_scene, " + "max_translation=unavailable, translation_threshold=0.020000, " + "max_rotation=unavailable, rotation_threshold=0.087266." + ) def test_recovery_replan_rejects_runtime_destination_change() -> None: @@ -843,6 +1122,18 @@ def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: latest_obstacles = generator.bind_collision_world.call_args.kwargs["obstacle_poses"] assert latest_obstacles["obstacle"][0, 0, 3] == pytest.approx(0.6) assert tick.command is not None + attempts = session.plan_attempts + assert [attempt.attempt_generation for attempt in attempts] == [0, 1] + assert [attempt.event_kind for attempt in attempts] == [ + ExecutionEventKind.ACTION_PLANNED, + ExecutionEventKind.REPLANNED, + ] + assert [attempt.plan.planned_scene_version for attempt in attempts] == [0, 1] + assert [attempt.plan.planned_collision_world_revision for attempt in attempts] == [ + (0,), + (1,), + ] + assert [attempt.replan_counts for attempt in attempts] == [(0,), (1,)] def test_collision_world_exhaustion_only_disables_changed_environment() -> None: @@ -1156,10 +1447,15 @@ def test_session_revision_replans_from_latest_context() -> None: session.revise_current(revised) first = session.tick(_context(0.0, 0.0, 0.1, 0)) second = session.tick(_context(0.1, 0.0, 0.1, 0)) + attempts = session.plan_attempts assert action.plan_count == 2 assert action.requests[0] is not action.requests[1] assert [request.revision for request in action.requests] == [0, 1] + assert [attempt.request.revision for attempt in attempts] == [0, 1] + assert attempts[0].request is not attempts[1].request + assert attempts[1].request.motion_policy == revised.motion_policy + assert attempts[1].request.recovery_policy == revised.recovery_policy assert any( event.kind is ExecutionEventKind.INVOCATION_REVISED and event.invocation_revision == 1 @@ -1347,6 +1643,159 @@ def test_session_rejects_regressing_collision_world_revision() -> None: session.tick(regressed) +def test_explicit_verification_with_empty_delta_preserves_task_state() -> None: + session, waiting = _effect_session(action=VerificationOnlyAction()) + request = waiting.pending_effect + assert request is not None + assert request.expected_effects.is_empty + assert request.effect_verification is not None + assert request.effect_verification.kind == "physical.test_completion" + initial_task_state = waiting.task_state + + preserved = session.pending_effect + assert preserved is not None + assert preserved.effect_verification is not None + assert preserved.effect_verification is not request.effect_verification + with pytest.raises(ValueError, match="explicit physical-effect requirement"): + replace(request, effect_verification=None) + + completed = session.tick( + _context(0.21, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.pending_effect is None + assert completed.task_state is initial_task_state + assert not completed.task_state.held_objects + + +def test_explicit_verification_keeps_partial_and_retry_row_lifecycle() -> None: + session, waiting = _effect_session( + batch_size=2, + max_action_retries=1, + action=VerificationOnlyAction(), + ) + first_request = waiting.pending_effect + assert first_request is not None + initial_task_state = waiting.task_state + + retry = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, True]), + ), + ) + + assert retry.pending_effect is None + assert retry.task_state is initial_task_state + assert any( + event.kind is ExecutionEventKind.ACTION_RETRY + and event.env_mask.tolist() == [False, True] + for event in retry.events + ) + + first_command = session.tick(_context(0.22, (0.2, 0.2), (0.2, 0.2), 0)) + second_command = session.tick(_context(0.23, (0.2, 0.2), (0.2, 0.2), 0)) + second_wait = session.tick(_context(0.24, (0.2, 0.2), (0.2, 0.2), 0)) + assert first_command.command is not None + assert first_command.command.active_mask.tolist() == [False, True] + assert second_command.command is not None + assert second_command.command.active_mask.tolist() == [False, True] + second_request = second_wait.pending_effect + assert second_request is not None + assert second_request.env_mask.tolist() == [False, True] + assert second_request.verification_id != first_request.verification_id + assert second_request.deadline > first_request.deadline + + completed = session.tick( + _context(0.25, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + second_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.eligible_mask.tolist() == [True, True] + assert completed.task_state is initial_task_state + + +def test_explicit_verification_partial_success_shrinks_request_without_state_delta() -> ( + None +): + session, waiting = _effect_session( + batch_size=2, + action=VerificationOnlyAction(), + ) + first_request = waiting.pending_effect + assert first_request is not None + initial_task_state = waiting.task_state + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + second_request = partial.pending_effect + assert second_request is not None + assert second_request.env_mask.tolist() == [False, True] + assert second_request.verification_id != first_request.verification_id + assert second_request.requested_at == first_request.requested_at + assert second_request.deadline == first_request.deadline + assert second_request.effect_verification is not None + assert second_request.effect_verification.kind == "physical.test_completion" + assert partial.task_state is initial_task_state + + completed = session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + second_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.task_state is initial_task_state + + +def test_explicit_verification_empty_delta_obeys_action_timeout() -> None: + session, waiting = _effect_session( + max_action_retries=0, + action_timeout=0.25, + action=VerificationOnlyAction(), + ) + request = waiting.pending_effect + assert request is not None + initial_task_state = waiting.task_state + + timed_out = session.tick(_context(0.3, 0.2, 0.2, 0)) + + assert timed_out.status is ExecutionStatus.FAILED + assert timed_out.pending_effect is None + assert timed_out.task_state is initial_task_state + assert any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT + for event in timed_out.events + ) + assert any( + event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + for event in timed_out.events + ) + + def test_nonempty_effect_is_committed_only_after_external_verification() -> None: engine, _ = _engine() effect = EffectAction() @@ -2038,12 +2487,10 @@ def test_failed_effect_plan_retries_without_requesting_effect_verification() -> recovery_policy=base.recovery_policy, ) session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) - session.tick(_context(0.0, 0.0, 0.2, 0)) - session.tick(_context(0.1, 0.0, 0.2, 0)) - - failed = session.tick(_context(0.2, 0.0, 0.2, 0)) + failed = session.tick(_context(0.0, 0.0, 0.2, 0)) assert failed.status is ExecutionStatus.FAILED + assert failed.command is None assert failed.pending_effect is None assert not any( event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index 8a731a6f8..e294f3c0a 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -17,8 +17,10 @@ from __future__ import annotations import os -import torch +from types import SimpleNamespace + import pytest +import torch from embodichain.lab.sim import ( SimulationManager, @@ -41,6 +43,16 @@ NUM_ARENAS = 10 +def test_get_qf_returns_all_articulation_joint_efforts(): + expected_qf = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], dtype=torch.float32) + articulation = object.__new__(Articulation) + articulation._data = SimpleNamespace(qf=expected_qf) + + actual_qf = articulation.get_qf() + + assert torch.equal(actual_qf, expected_qf) + + def _link_static_friction(art: Articulation, link_name: str, env_idx: int = 0) -> float: return art._entities[env_idx].get_physical_attr(link_name).static_friction diff --git a/tests/sim/objects/test_robot.py b/tests/sim/objects/test_robot.py index 876781a60..e6e050f91 100644 --- a/tests/sim/objects/test_robot.py +++ b/tests/sim/objects/test_robot.py @@ -17,9 +17,11 @@ from __future__ import annotations import os -import torch -import pytest +from types import SimpleNamespace + import numpy as np +import pytest +import torch from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.objects import Robot @@ -49,6 +51,20 @@ } +def test_get_qf_selects_control_part_joint_efforts(): + full_qf = torch.tensor( + [[1.0, 2.0, 3.0, 4.0], [5.0, 6.0, 7.0, 8.0]], dtype=torch.float32 + ) + robot = object.__new__(Robot) + robot._data = SimpleNamespace(qf=full_qf) + robot.cfg = SimpleNamespace(control_parts={"arm": ["joint_3", "joint_1"]}) + robot._joint_ids = {"arm": [3, 1]} + + actual_qf = robot.get_qf(name="arm") + + assert torch.equal(actual_qf, full_qf[:, [3, 1]]) + + # Base test class for CPU and CUDA class BaseRobotTest: @classmethod diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index d3df21288..e348cddbb 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -470,7 +470,7 @@ def _held_context( object_to_eef: torch.Tensor, *, env_mask: torch.Tensor | None = None, - control_part: str = "arm", + control_part: str = "manipulator", robot_dof: int = 2, ) -> PlanningContext: held = HeldObjectState( @@ -751,7 +751,7 @@ def test_handover_uses_profile_selected_named_provider_and_stops_lookahead() -> registry, pick.invocation.goal.semantics, torch.eye(4).repeat(2, 1, 1), - control_part="left_arm", + control_part="left", robot_dof=4, ) handover = compiler.ground(workflow, 1, held_context) @@ -786,7 +786,7 @@ def capture_middle(matrix: torch.Tensor, name: str) -> torch.Tensor: registry, pick.invocation.goal.semantics, torch.eye(4).repeat(2, 1, 1), - control_part="left_arm", + control_part="left", robot_dof=4, ) with pytest.raises(RuntimeError, match="captured target"): diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py index 9c93189d1..e90405308 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -99,9 +99,8 @@ class _InstantPick(AtomicAction): def _plan(self, request, context): goal = self.require_goal(request) - target = request.binding.endpoint("primary", "motion").require_target( - JointPositionTarget - ) + endpoint = request.binding.endpoint("primary", "motion") + endpoint.require_target(JointPositionTarget) held = HeldObjectState( semantics=goal.semantics, object_to_eef=torch.eye(4).repeat(context.batch_size, 1, 1), @@ -113,7 +112,7 @@ def _plan(self, request, context): success=True, commands=TimedCommandSequence((), context.env_ids), expected_effects=StateDelta( - held_object_updates={target.control_part: held} + held_object_updates={endpoint.task_state_key: held} ), ) @@ -126,16 +125,15 @@ class _InstantPlace(AtomicAction): def _plan(self, request, context): self.require_goal(request) - target = request.binding.endpoint("primary", "motion").require_target( - JointPositionTarget - ) + endpoint = request.binding.endpoint("primary", "motion") + endpoint.require_target(JointPositionTarget) return self.build_command_plan( request, context, success=True, commands=TimedCommandSequence((), context.env_ids), expected_effects=StateDelta( - held_object_updates={target.control_part: None} + held_object_updates={endpoint.task_state_key: None} ), ) @@ -358,7 +356,7 @@ def test_task_preserves_verified_state_across_dynamic_segments() -> None: pick_result = task.run_segment((Pick(object=cube),), segment_id="acquire") assert pick_result.status is SemanticExecutionStatus.COMPLETED - assert task.task_state.held_object_mask("arm").tolist() == [True, True] + assert task.task_state.held_object_mask("manipulator").tolist() == [True, True] place_result = task.run_segment( ( @@ -392,7 +390,19 @@ def test_manual_execution_blocks_until_effect_mask_is_submitted() -> None: assert blocked.pending_effect is not None assert execution.task_result is None - completed = execution.step(effect_success=torch.tensor([True, True])) + completed = blocked + for _ in range(10): + if completed.status is SemanticExecutionStatus.COMPLETED: + break + effect_success = ( + torch.tensor([True, True]) if execution.pending_effect is not None else None + ) + completed = execution.step(effect_success=effect_success) + if ( + completed.runner_step is not None + and completed.runner_step.wait_duration > 0 + ): + runtime.clock.sleep(completed.runner_step.wait_duration) assert completed.status is SemanticExecutionStatus.COMPLETED assert execution.task_result is not None assert execution.task_result.status is SemanticTaskStatus.SUCCEEDED @@ -440,7 +450,7 @@ def test_effect_failures_produce_partial_task_success_after_bounded_retries() -> assert result.status is SemanticTaskStatus.PARTIAL_SUCCESS assert result.eligible_mask.tolist() == [True, False] - assert result.task_state.held_object_mask("arm").tolist() == [True, False] + assert result.task_state.held_object_mask("manipulator").tolist() == [True, False] def test_runtime_rejects_concurrent_tasks() -> None: From 370e5f83957afd4ee8f72d3a5c3e5a2b17c45489 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:18:03 +0800 Subject: [PATCH 06/29] feat(sim): add semantic runtime effects and parallelism --- agent_context/MAP.yaml | 8 +- .../topics/atomic-actions/atomic-actions.md | 72 +- .../embodichain.lab.sim.skills.rst | 346 +- docs/source/api_reference/public_api.rst | 145 +- .../overview/sim/atomic_actions/index.md | 23 +- .../atomic_actions/robot_skill_profiles.md | 111 +- docs/source/overview/sim/index.rst | 4 - docs/source/overview/sim/scene_registry.md | 4 +- docs/source/overview/sim/semantic_skills.md | 312 -- docs/source/tutorial/index.rst | 14 +- docs/source/tutorial/semantic_skills.rst | 437 --- embodichain/lab/sim/skills/__init__.py | 258 +- embodichain/lab/sim/skills/calls.py | 148 +- embodichain/lab/sim/skills/compiler.py | 847 ++++- embodichain/lab/sim/skills/effects.py | 2250 +++++++++++ embodichain/lab/sim/skills/evidence.py | 1467 ++++++++ embodichain/lab/sim/skills/integration.py | 189 +- embodichain/lab/sim/skills/parallel.py | 354 ++ .../lab/sim/skills/parallel_runtime.py | 1487 ++++++++ embodichain/lab/sim/skills/profiles.py | 164 +- embodichain/lab/sim/skills/runtime.py | 3283 ++++++++++------- embodichain/lab/sim/skills/scene.py | 265 +- scripts/tutorials/semantic_skill/hand_over.py | 573 --- scripts/tutorials/semantic_skill/place.py | 407 -- .../semantic_skill/tutorial_utils.py | 331 -- .../sim/skills/test_articulation_semantics.py | 594 +++ tests/sim/skills/test_calls.py | 20 + tests/sim/skills/test_compiler.py | 501 ++- ...o_semantic_runtime_dynamic_recovery_gpu.py | 375 ++ tests/sim/skills/test_effects.py | 863 +++++ tests/sim/skills/test_evidence.py | 666 ++++ tests/sim/skills/test_integration.py | 363 +- tests/sim/skills/test_parallel.py | 251 ++ tests/sim/skills/test_parallel_runtime.py | 1264 +++++++ tests/sim/skills/test_profiles.py | 111 + tests/sim/skills/test_runtime.py | 1168 +++--- tests/sim/skills/test_scene.py | 111 +- .../skills/test_semantic_skill_tutorials.py | 480 --- 38 files changed, 15638 insertions(+), 4628 deletions(-) delete mode 100644 docs/source/overview/sim/semantic_skills.md delete mode 100644 docs/source/tutorial/semantic_skills.rst create mode 100644 embodichain/lab/sim/skills/effects.py create mode 100644 embodichain/lab/sim/skills/evidence.py create mode 100644 embodichain/lab/sim/skills/parallel.py create mode 100644 embodichain/lab/sim/skills/parallel_runtime.py delete mode 100644 scripts/tutorials/semantic_skill/hand_over.py delete mode 100644 scripts/tutorials/semantic_skill/place.py delete mode 100644 scripts/tutorials/semantic_skill/tutorial_utils.py create mode 100644 tests/sim/skills/test_articulation_semantics.py create mode 100644 tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py create mode 100644 tests/sim/skills/test_effects.py create mode 100644 tests/sim/skills/test_evidence.py create mode 100644 tests/sim/skills/test_parallel.py create mode 100644 tests/sim/skills/test_parallel_runtime.py delete mode 100644 tests/sim/skills/test_semantic_skill_tutorials.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index aebc0a6d2..9ed83f5d1 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -646,10 +646,10 @@ topics: - SemanticIntegrationManifest - BoundSemanticIntegration - SemanticSkillCompiler - - SemanticSkillRuntime - - SemanticTask - - SemanticExecution - - SemanticTaskResult + - SkillRuntime + - SkillResult + - ParallelSkillRuntime + - AtomicSkills - SemanticWorkflow - SemanticLowering - GroundedSemanticCall diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index c6743b397..57e36f25e 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -328,39 +328,26 @@ The compiler identity prevents a workflow from crossing lowerer/grounder registries. Engine/profile staleness is checked through the bound integration; the workflow does not duplicate engine-owner or catalog-revision fields. -## Semantic runtime facade - -`embodichain.lab.sim.skills.SemanticSkillRuntime` is the application-facing -orchestration layer. `bind()` connects an explicit manifest, registry, engine, -observation provider, command sink, and clock; `from_simulation()` assembles the -standard joint-position simulation ports while still requiring an explicit -registry, robot profile, and motion generator. Its optional `control_dt` -selects a command cadence independently of the simulation physics period. A -runtime-level `runner_cfg` overrides all calls; when omitted, each grounded -call uses the `ExecutionRunnerCfg` owned by its selected `SkillPolicyPreset`. -The runtime exposes only calls supported by both the semantic catalog and the -currently bound robot profile, and allows exactly one active `SemanticTask` -because no resource scheduler or lease manager exists. - -`SemanticSkillRuntime.run()` is the blocking one-segment convenience path and -requires a `SemanticEffectVerifier`. Use `start()` when effect verification is -asynchronous. A `SemanticTask` retains externally verified `TaskState`, stable -environment IDs, and the sticky eligible cohort across several independently -analyzed segments. `run_segment()` supports dynamic application decisions at -safe semantic-call boundaries; submit all known calls in one segment when Pick -look-ahead should account for a downstream Place or HandOver target. - -`SemanticExecution` always JIT-grounds and starts one invocation at a time. It -uses a fresh observation before each call, delegates local recovery and safe -stop to `ExecutionRunner`, commits only verified effects, then carries the -session's task state and eligibility into the next grounding boundary. Manual -execution reports `WAITING_FOR_EFFECT` and resumes through `step(effect_success=...)`; -compatible in-place call changes use `revise_current()`, which reanalyzes the -workflow and still inherits the runner's same-skill, same-invocation, and -same-runtime-address restrictions. Runtime failures remain terminal; automatic -task-level skill replacement or symbolic-state reconciliation is not provided. -A failed or cancelled segment closes its task and releases runtime ownership; -successful dynamic segments retain ownership until `finish()` or cancellation. +## Semantic runtime + +`embodichain.lab.sim.skills.SkillRuntime` is the single application execution +boundary. `start()` analyzes the complete semantic call window, captures a fresh +observation before each executed call, JIT-grounds one `ActionInvocation`, and +delegates planning, transport, recovery, acknowledgement, and safe stop to the +canonical Atomic Action runtime. `execution_prefix_length` lets later calls +participate in static look-ahead without executing them. + +`SkillResult` is the only runtime result adopted by higher layers. It retains +verified `TaskState`, row-local eligibility/success/failure masks, typed events, +effect traces, and failures. `step()` is non-blocking; `run()` is the synchronous +convenience path. `cancel()` delegates safe stop to the active runner. The +runtime never exposes a second physical state or command scheduler. + +`ParallelSkillRuntime` is the only physical-concurrency boundary. It requires +disjoint canonical `ResourceClaim` values and an explicit +`ParallelCommandSafetyValidator`; resource non-overlap alone is insufficient. +`AtomicSkills` is a thin facade over the same `SkillRuntime`, not another +execution implementation. `registry.make_planning_scene_provider(motion_generator, batch_size=...)` returns a fresh `RegistrySceneProvider` with independent baselines and revision @@ -670,25 +657,6 @@ Runnable closed-loop examples live under `scripts/tutorials/atomic_action/`: `dynamic_obstacle_recovery.py`. Each injects one disturbance, reports the structured invalidation/replan events, and requires terminal completion. -Semantic integration tutorials live under `scripts/tutorials/semantic_skill/`. -Both examples separate `create_*_application()` (scene/profile/runtime and -default verifier wiring), `create_*_task()` (robot-independent semantic calls), -and the application-facing `app.run(task, ...)` entry. `app` remains a -`SemanticSkillRuntime`; there is no tutorial-specific facade. `place.py` -executes `Pick -> Place`, verifying the observed lift, planned object-to-EEF -relation, release pose, and open hand. `hand_over.py` demonstrates disjoint -dual-arm resources plus an explicit `RegisteredSemanticLowerer`, then verifies -source release and receiver ownership at the final target. Both report -structured recovery events and use `--diagnose_plan` only for a separate -offline compile that projects hypothetical effects without executing them. -Release and ownership-transfer presets disable whole-action effect retries -because those physical changes are not safely repeatable without state -reconciliation. - -Human-facing architecture and lifecycle documentation lives in -`docs/source/overview/sim/semantic_skills.md`; the runnable walkthrough is -indexed at `docs/source/tutorial/semantic_skills.rst`. - The latest validated session context is retained for safe hold if the first live observation fails. Environment IDs must remain stable and ordered for the entire session; robot and scene timestamps and scene versions must be monotonic. diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst index 4a33cb46b..6bdb46b30 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -3,54 +3,6 @@ embodichain.lab.sim.skills .. automodule:: embodichain.lab.sim.skills - .. rubric:: Semantic calls and catalog - - .. autosummary:: - - SemanticPose - Pick - Place - HandOver - RegisteredSemanticCall - SemanticCallDescriptor - SemanticCallCatalog - builtin_semantic_call_catalog - - .. rubric:: Semantic compilation and grounding - - .. autosummary:: - - SemanticWorkflow - SemanticLowering - GroundedSemanticCall - SemanticObjectTarget - SemanticRelationTarget - RegisteredSemanticLowerer - RelationTargetGrounder - HandOverPoseTargets - HandOverPoseProvider - SemanticSkillCompiler - - .. rubric:: Semantic integration and execution - - .. autosummary:: - - SceneEntityManifest - SceneManifest - SemanticIntegrationManifest - SemanticDiagnostic - SemanticValidationError - SemanticEffectVerifier - SemanticSkillRuntime - SemanticTask - SemanticExecution - SemanticExecutionStatus - SemanticTaskStatus - SemanticExecutionStep - SemanticCallRecord - SemanticSegmentResult - SemanticTaskResult - .. rubric:: Scene integration contracts .. autosummary:: @@ -58,7 +10,6 @@ embodichain.lab.sim.skills SceneRegistry RegistrySceneProvider SceneEntityRegistration - SceneEntityMetadata SceneEntityRef SceneObjectRef SceneArticulationRef @@ -69,11 +20,6 @@ embodichain.lab.sim.skills SceneDynamics SceneCollisionRole SceneCollisionWorldMode - GRASP_AFFORDANCE_CAPABILITY - PLACE_ON_AFFORDANCE_CAPABILITY - PLACE_IN_AFFORDANCE_CAPABILITY - UnsupportedSceneAffordanceError - AmbiguousSceneAffordanceError .. rubric:: Robot skill profiles @@ -97,170 +43,290 @@ embodichain.lab.sim.skills UnsupportedSkillError AmbiguousSkillBindingError -.. currentmodule:: embodichain.lab.sim.skills + .. rubric:: Semantic calls and runtime -Semantic calls and catalog --------------------------- + .. autosummary:: -.. autoclass:: SemanticPose - :members: + SemanticCallSpec + SemanticPose + Pick + Place + HandOver + OperateArticulation + RegisteredSemanticCall + SemanticCallCatalog + SemanticSkillCompiler + AtomicSkills + SkillRuntime + SkillResult + SkillCallTrace + SkillPlanAttemptTrace + SkillEffectTrace -.. autoclass:: Pick - :members: + .. rubric:: Effects, evidence, and parallel execution -.. autoclass:: Place - :members: + .. autosummary:: -.. autoclass:: HandOver - :members: + SemanticEffectSpec + EffectMonitorRef + EffectMonitor + EffectEvidenceCollector + ParallelSkillRuntime + ParallelSkillResult + ParallelCommandSafetyValidator -.. autoclass:: RegisteredSemanticCall - :members: + .. rubric:: Additional public contracts -.. autoclass:: SemanticCallDescriptor - :members: + .. autosummary:: -.. autoclass:: SemanticCallCatalog - :members: + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + AmbiguousSceneAffordanceError + AnalyzedSemanticCall + ArticulationJointEvidenceAddress + ArticulationJointObservationCallback + ArticulationJointStateExpectation + BinaryEffectClause + BinaryEffectEvidenceBatch + BinaryEffectEvidenceQuery + BinaryEffectObservation + BinaryEvidenceKind + BinaryObservationCallback + BoundSemanticCall + BoundSemanticIntegration + COMPOSITE_EFFECT_MONITOR_ID + COMPOSITE_EFFECT_MONITOR_REVISION + CONSTRAINT_EFFECT_CHANNEL + CONTACT_EFFECT_CHANNEL + CONTROL_PART_EVIDENCE_PROVIDER_ID + CONTROL_PART_EVIDENCE_PROVIDER_REVISION + CompositeEffectMonitor + CompositeEffectMonitorCfg + CompositeEffectMonitorFactory + ControlPartEvidenceAddress + ControlPartRobotEvidenceSource + ControlPartSimulationEvidenceProvider + CoordinatedHeldObjectCleanupExpectation + DeclarativeValue + EffectClause + EffectEvidenceAddress + EffectEvidenceBatch + EffectEvidenceCollectionContext + EffectEvidenceCollectorPort + EffectEvidenceProvider + EffectEvidenceProviderRegistry + EffectEvidenceQuery + EffectEvidenceQueryValue + EffectEvidenceSourceRef + EffectMonitorDecision + EffectMonitorFactory + EffectMonitorParam + EffectMonitorRegistry + EffectStateExpectation + FORCE_EFFECT_CHANNEL + GRASP_AFFORDANCE_CAPABILITY + GroundedSemanticCall + HandOverPoseProvider + HandOverPoseTargets + HeldObjectRelation + HeldObjectStateExpectation + JOINT_STATE_EFFECT_CHANNEL + JointStateEffectClause + JointStateEvidenceBatch + JointStateEvidenceQuery + JointStateObservation + LinkedSemanticCall + PLACE_IN_AFFORDANCE_CAPABILITY + PLACE_ON_AFFORDANCE_CAPABILITY + POSE_RELATION_EFFECT_CHANNEL + ParallelBarrierUpdate + ParallelBranchPlan + ParallelBranchRuntime + ParallelBranchStaticAnalysis + ParallelConflictError + ParallelLaneCommandSink + ParallelRuntimeBranch + ParallelSafetyError + ParallelStateConflictError + ParallelTimingError + ParallelTimingPolicy + PathPart + PlaceRelationTarget + PoseRelationClause + PoseRelationEvidenceBatch + PoseRelationEvidenceQuery + PoseRelationExpectation + RegisteredSemanticLowerer + RelationTargetGrounder + ResolvedCorePolicyTrace + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + ScalarEffectClause + ScalarEffectEvidenceBatch + ScalarEffectEvidenceQuery + ScalarEffectObservation + ScalarEvidenceKind + ScalarExpectation + ScalarObservationCallback + SceneArticulationEvidenceProvider + SceneArticulationJointStateProvider + SceneEntityManifest + SceneEntityMetadata + SceneManifest + SemanticCallDescriptor + SemanticDiagnostic + SemanticEffectDependency + SemanticEffectKind + SemanticHandOverTarget + SemanticIntegrationManifest + SemanticLowering + SemanticObjectTarget + SemanticRelationTarget + SemanticValidationError + SemanticWorkflow + SkillEndpointBindingTrace + SkillFailure + SkillRuntimeProvider + SkillScene + SkillStatus + SymbolicStateDomain + SymbolicStateKey + UnsupportedSceneAffordanceError + align_parallel_commands + analyze_parallel_branches + build_effect_evidence_queries + builtin_semantic_call_catalog + merge_parallel_effects + resolve_parallel_barrier + task_state_to_metadata + validate_parallel_claims -.. autofunction:: builtin_semantic_call_catalog +.. currentmodule:: embodichain.lab.sim.skills -Semantic compilation and grounding ------------------------------------ +Robot resources and profiles +---------------------------- -.. autoclass:: SemanticWorkflow +.. autoclass:: RobotSkillProfile :members: -.. autoclass:: SemanticLowering +.. autoclass:: BoundRobotSkillProfile :members: -.. autoclass:: GroundedSemanticCall +.. autoclass:: RobotResource :members: -.. autoclass:: SemanticObjectTarget +.. autoclass:: ResourceEndpoint :members: -.. autoclass:: SemanticRelationTarget +.. autoclass:: ResourceEndpointAdapter :members: -.. autoclass:: RegisteredSemanticLowerer +.. autoclass:: EndpointResolution :members: -.. autoclass:: RelationTargetGrounder +.. autoclass:: ControlPartEndpoint :members: -.. autoclass:: HandOverPoseTargets +.. autoclass:: ControlPartEndpointAdapter :members: -.. autoclass:: HandOverPoseProvider +.. autoclass:: ResourceBinding :members: -.. autoclass:: SemanticSkillCompiler +.. autoclass:: ResolvedResourceEndpoint :members: -Semantic integration --------------------- - -.. autoclass:: SceneEntityManifest +.. autoclass:: ResolvedRobotResource :members: -.. autoclass:: SceneManifest +.. autoclass:: ResolvedSkillBinding :members: -.. autoclass:: SemanticIntegrationManifest +.. autoclass:: ResourceClaim :members: -.. autoclass:: SemanticDiagnostic +.. autoclass:: SkillPolicyPreset :members: -.. autoclass:: SemanticValidationError - :members: +Profile errors +-------------- -Semantic runtime ----------------- +.. autoclass:: ProfileValidationError -.. autodata:: SemanticEffectVerifier +.. autoclass:: UnsupportedSkillError -.. autoclass:: SemanticSkillRuntime - :members: +.. autoclass:: AmbiguousSkillBindingError -.. autoclass:: SemanticTask - :members: +Semantic calls and runtime +-------------------------- -.. autoclass:: SemanticExecution +.. autoclass:: SemanticCallSpec :members: -.. autoclass:: SemanticExecutionStatus +.. autoclass:: SemanticPose :members: -.. autoclass:: SemanticTaskStatus +.. autoclass:: Pick :members: -.. autoclass:: SemanticExecutionStep +.. autoclass:: Place :members: -.. autoclass:: SemanticCallRecord +.. autoclass:: HandOver :members: -.. autoclass:: SemanticSegmentResult +.. autoclass:: OperateArticulation :members: -.. autoclass:: SemanticTaskResult +.. autoclass:: RegisteredSemanticCall :members: -Robot resources and profiles ----------------------------- - -.. autoclass:: RobotSkillProfile +.. autoclass:: SemanticCallCatalog :members: -.. autoclass:: BoundRobotSkillProfile +.. autoclass:: SemanticSkillCompiler :members: -.. autoclass:: RobotResource +.. autoclass:: AtomicSkills :members: -.. autoclass:: ResourceEndpoint +.. autoclass:: SkillRuntime :members: -.. autoclass:: ResourceEndpointAdapter +.. autoclass:: SkillResult :members: -.. autoclass:: EndpointResolution +.. autoclass:: SkillCallTrace :members: -.. autoclass:: ControlPartEndpoint +.. autoclass:: SkillPlanAttemptTrace :members: -.. autoclass:: ControlPartEndpointAdapter +.. autoclass:: SkillEffectTrace :members: -.. autoclass:: ResourceBinding - :members: +Effects, evidence, and parallel execution +----------------------------------------- -.. autoclass:: ResolvedResourceEndpoint +.. autoclass:: SemanticEffectSpec :members: -.. autoclass:: ResolvedRobotResource +.. autoclass:: EffectMonitorRef :members: -.. autoclass:: ResolvedSkillBinding +.. autoclass:: EffectMonitor :members: -.. autoclass:: ResourceClaim +.. autoclass:: EffectEvidenceCollector :members: -.. autoclass:: SkillPolicyPreset +.. autoclass:: ParallelSkillRuntime :members: -Profile errors --------------- - -.. autoclass:: ProfileValidationError - -.. autoclass:: UnsupportedSkillError +.. autoclass:: ParallelSkillResult + :members: -.. autoclass:: AmbiguousSkillBindingError +.. autoclass:: ParallelCommandSafetyValidator + :members: Registry and provider --------------------- @@ -277,9 +343,6 @@ Registration contracts .. autoclass:: SceneEntityRegistration :members: -.. autoclass:: SceneEntityMetadata - :members: - .. autoclass:: SceneEntityStateProvider :members: @@ -312,16 +375,3 @@ References and enums .. autoclass:: SceneCollisionWorldMode :members: - -Affordance capabilities and errors ----------------------------------- - -.. autodata:: GRASP_AFFORDANCE_CAPABILITY - -.. autodata:: PLACE_ON_AFFORDANCE_CAPABILITY - -.. autodata:: PLACE_IN_AFFORDANCE_CAPABILITY - -.. autoclass:: UnsupportedSceneAffordanceError - -.. autoclass:: AmbiguousSceneAffordanceError diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 779327edc..adb2a063e 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -900,6 +900,7 @@ embodichain.lab.sim.skills.calls DeclarativeValue HandOver + OperateArticulation Pick Place PlaceRelationTarget @@ -932,6 +933,86 @@ embodichain.lab.sim.skills.compiler SemanticSkillCompiler SemanticWorkflow +embodichain.lab.sim.skills.effects +---------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.effects + +.. autosummary:: + + ArticulationJointStateExpectation + BinaryEffectClause + BinaryEffectEvidenceBatch + BinaryEvidenceKind + COMPOSITE_EFFECT_MONITOR_ID + COMPOSITE_EFFECT_MONITOR_REVISION + CONSTRAINT_EFFECT_CHANNEL + CONTACT_EFFECT_CHANNEL + CONTROL_PART_EVIDENCE_PROVIDER_ID + CONTROL_PART_EVIDENCE_PROVIDER_REVISION + CompositeEffectMonitor + CompositeEffectMonitorCfg + CompositeEffectMonitorFactory + ControlPartEvidenceAddress + CoordinatedHeldObjectCleanupExpectation + EffectClause + EffectEvidenceAddress + EffectEvidenceBatch + EffectEvidenceSourceRef + EffectMonitor + EffectMonitorDecision + EffectMonitorFactory + EffectMonitorParam + EffectMonitorRef + EffectMonitorRegistry + EffectStateExpectation + FORCE_EFFECT_CHANNEL + HeldObjectRelation + HeldObjectStateExpectation + JOINT_STATE_EFFECT_CHANNEL + JointStateEffectClause + JointStateEvidenceBatch + POSE_RELATION_EFFECT_CHANNEL + PoseRelationClause + PoseRelationEvidenceBatch + PoseRelationExpectation + ScalarEffectClause + ScalarEffectEvidenceBatch + ScalarEvidenceKind + ScalarExpectation + SemanticEffectKind + SemanticEffectSpec + SymbolicStateDomain + SymbolicStateKey + +embodichain.lab.sim.skills.evidence +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.evidence + +.. autosummary:: + + ArticulationJointObservationCallback + BinaryEffectEvidenceQuery + BinaryEffectObservation + BinaryObservationCallback + ControlPartRobotEvidenceSource + ControlPartSimulationEvidenceProvider + EffectEvidenceCollectionContext + EffectEvidenceCollector + EffectEvidenceProvider + EffectEvidenceProviderRegistry + EffectEvidenceQuery + EffectEvidenceQueryValue + JointStateEvidenceQuery + JointStateObservation + PoseRelationEvidenceQuery + ScalarEffectEvidenceQuery + ScalarEffectObservation + ScalarObservationCallback + SceneArticulationEvidenceProvider + build_effect_evidence_queries + embodichain.lab.sim.skills.integration -------------------------------------- @@ -948,6 +1029,41 @@ embodichain.lab.sim.skills.integration SemanticIntegrationManifest SemanticValidationError +embodichain.lab.sim.skills.parallel +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.parallel + +.. autosummary:: + + ParallelBarrierUpdate + ParallelBranchPlan + ParallelConflictError + ParallelStateConflictError + ParallelTimingError + ParallelTimingPolicy + align_parallel_commands + merge_parallel_effects + resolve_parallel_barrier + validate_parallel_claims + +embodichain.lab.sim.skills.parallel_runtime +------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.parallel_runtime + +.. autosummary:: + + ParallelBranchRuntime + ParallelBranchStaticAnalysis + ParallelCommandSafetyValidator + ParallelLaneCommandSink + ParallelRuntimeBranch + ParallelSafetyError + ParallelSkillResult + ParallelSkillRuntime + analyze_parallel_branches + embodichain.lab.sim.skills.profiles ----------------------------------- @@ -980,16 +1096,20 @@ embodichain.lab.sim.skills.runtime .. autosummary:: - SemanticCallRecord - SemanticEffectVerifier - SemanticExecution - SemanticExecutionStatus - SemanticExecutionStep - SemanticSegmentResult - SemanticSkillRuntime - SemanticTask - SemanticTaskResult - SemanticTaskStatus + AtomicSkills + EffectEvidenceCollectorPort + ResolvedCorePolicyTrace + SkillCallTrace + SkillEndpointBindingTrace + SkillEffectTrace + SkillFailure + SkillPlanAttemptTrace + SkillResult + SkillRuntime + SkillRuntimeProvider + SkillScene + SkillStatus + task_state_to_metadata embodichain.lab.sim.skills.scene -------------------------------- @@ -998,6 +1118,11 @@ embodichain.lab.sim.skills.scene .. autosummary:: + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + ArticulationJointEvidenceAddress + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + SceneArticulationJointStateProvider RegistrySceneProvider SceneAffordanceRef SceneArticulationRef diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index c66f5656e..f1a920345 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -36,7 +36,7 @@ payloads, and transports without adding fixed resource categories to the core. +---------------+----------------+ +---------------+----------------+ | | v | - SemanticSkillCompiler / SemanticSkillRuntime: | + SemanticSkillCompiler / SkillRuntime: | schema validation, SceneRegistry grounding, binding | | | +------------------+------------------+ @@ -105,7 +105,7 @@ state. The engine supports two first-class caller paths. An Action Agent or configuration-driven application can emit a semantic call for {class}`~embodichain.lab.sim.skills.SemanticSkillCompiler` and -{class}`~embodichain.lab.sim.skills.SemanticSkillRuntime` to validate, ground, +{class}`~embodichain.lab.sim.skills.SkillRuntime` to validate, ground, and convert into an `ActionInvocation`. A user can instead author the typed invocation directly in Python or load it from an application-owned configuration layer: @@ -838,7 +838,7 @@ An MLLM should not construct `ActionInvocation` by copying arbitrary JSON into runtime objects. The `embodichain.lab.sim.skills` package provides the semantic boundary: stable call descriptors, immutable call values, scene/profile manifests, a compiler, and a runtime facade. The agent selects among -`SemanticSkillRuntime.available_calls` and supplies declarative object-centric +the `SemanticCallCatalog` descriptors and supplies declarative object-centric values; the compiler performs validation and grounding before the atomic engine sees the request: @@ -850,7 +850,7 @@ MLLM / application SemanticCallSpec object / affordance / resource / effect-flow validation -> SemanticSkillCompiler.ground(latest_context) participant binding + safe options + ActionInvocation - -> SemanticSkillRuntime / AtomicActionEngine + -> SkillRuntime / AtomicActionEngine -> verified task state + structured execution events ``` @@ -861,13 +861,12 @@ installed version-matched lowerer. Invocation IDs and monotonic revisions correlate compatible in-flight updates with planner diagnostics and execution events without mutating a request implicitly. -The semantic runtime is also useful without an agent. `run()` executes one -known workflow, while `open_task()` and `run_segment()` retain verified state -across safe application decision boundaries. Call-local recovery remains owned -by `ExecutionRunner`; automatic skill replacement or symbolic-state -reconciliation after a terminal failure is intentionally not provided. See -{doc}`../semantic_skills` for the complete compiler/runtime and dynamic-task -contract. +The semantic runtime is also useful without an agent. `start()` and `step()` +provide the non-blocking path, while `run()` is the synchronous convenience. +An analysis window may include future calls while `execution_prefix_length` +limits physical execution to a verified prefix. Call-local recovery remains +owned by `ExecutionRunner`; automatic task-level route replacement is not +provided by this layer. ## Extending the module @@ -896,7 +895,7 @@ See {doc}`builtin_actions` for the shipped skill catalog and visual demos, and ## Further reading - {doc}`../scene_registry` — canonical scene identity, snapshots, and collision integration -- {doc}`../semantic_skills` — semantic calls, compilation, runtime execution, and dynamic task boundaries +- {doc}`robot_skill_profiles` — semantic calls, resource binding, and runtime presets - {doc}`../planners/motion_generator` — the motion generator owned by the engine - {doc}`../sim_robot` — robot control parts and kinematic configuration - {doc}`/tutorial/atomic_actions` — static, closed-loop, and recovery examples diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md index 854d7bc6c..66488542d 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -77,7 +77,6 @@ from embodichain.lab.sim.atomic_actions import ( FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, ControlPartCommandProfile, - ExecutionRunnerCfg, MotionPolicy, ) from embodichain.lab.sim.skills import ( @@ -139,63 +138,12 @@ profile = RobotSkillProfile( "default": SkillPolicyPreset( preset_id="default", motion_policy=MotionPolicy(strategy="ik_interp"), - runner_cfg=ExecutionRunnerCfg(command_timeout=2.0), ), }, default_preset="default", ) ``` -Set `SkillPolicyPreset.required_planner` only when a preset depends on one -planner backend, typically because it carries backend-specific typed planning -options. Profile binding checks that requirement against the engine's configured -backend and fails early on a mismatch. Leave it as `None` for portable presets. - -A {class}`SkillPolicyPreset` owns three independently snapshotted policy layers: -`motion_policy`, `recovery_policy`, and `runner_cfg`. Semantic integration -selects a preset in this order: an integration-wide `runtime_preset`, the -profile's `skill_presets[atomic_skill_id]`, then `default_preset`. At execution -time, an explicit `SemanticSkillRuntime.runner_cfg` overrides the selected -preset's runner configuration for every call; otherwise each call keeps its -selected preset's transport timeouts, minimum cycle time, and completion-hold -behavior. - -## Select semantic grounding providers - -Some semantic calls require embodiment knowledge that does not belong in the -agent-facing call or the atomic action. The built-in semantic HandOver is the -canonical example: the robot profile selects a named provider that supplies a -safe middle and default final object target for that embodiment. An explicit -semantic `HandOver.final_target` overrides the provider's final target. - -```python -profile = RobotSkillProfile( - profile_id="dual_arm_robot", - resources=dual_arm_resources, - command_profiles=hand_command_profiles, - defaults=dual_arm_skill_defaults, - presets={"default": default_preset}, - default_preset="default", - grounding_providers={"hand_over": "center_workspace_handover"}, -) - -runtime = SemanticSkillRuntime.from_simulation( - simulation=sim, - robot=robot, - motion_generator=motion_generator, - scene_registry=scene_registry, - robot_profile=profile, - handover_pose_providers=(CenterWorkspaceHandOverProvider(),), -) -``` - -`grounding_providers` maps a **semantic call ID** to a provider ID. The selected -ID must match one explicitly installed {class}`HandOverPoseProvider`; missing or -unknown providers fail during workflow analysis, before observation, planning, -or controller work. The provider is executable integration code and therefore -is passed to the runtime/compiler rather than stored inside the declarative -profile. - Every `ControlPartEndpoint.control_part` must be a key in `robot.control_parts`. A composite endpoint may reuse a member's control part, but all joints controlled directly by the composite must already be covered by @@ -209,6 +157,55 @@ One-dimensional joint-position commands are broadcast across environments. Their last dimension must equal the resolved endpoint's degree of freedom. Use invocation-level command overrides for object- or environment-specific values. +## Safe preset and dynamic collision worlds + +When the authoritative scene registry declares dynamic collision entities and +`safe` is reachable through the integration-wide, per-skill, or +profile-default preset selection, semantic integration validates that path +conservatively during binding. The `safe` preset must use `motion_gen`, and the +active motion generator must explicitly support dynamic collision worlds; +otherwise binding fails before provider observation, planning, or command +emission. + +A linked call receives an effective immutable preset snapshot with +`DynamicCollisionMode.REQUIRED`; the source profile preset is not mutated. +Other presets, and scenes without dynamic collision entities, retain their +configured collision mode. + +## Select semantic effect monitors with the preset + +A {class}`SkillPolicyPreset` owns one coherent runtime choice: planning and +recovery policy, runner cadence, and the exact semantic-effect monitors used to +confirm physical postconditions. `effect_monitors` maps a semantic call ID to a +versioned {class}`EffectMonitorRef`. Its parameters are bounded declarative +values; executable objects, tensors, cyclic containers, and non-finite numbers +are rejected. + +When `effect_monitors` is omitted, the preset selects the built-in +pose-relation hysteresis monitor for `pick`, `place`, and `hand_over`. Passing an +explicit empty mapping disables that default; static analysis then reports +`missing_effect_monitor` if a curated effectful call selects that preset. A +manifest also rejects monitor entries whose semantic ID is absent from its call +catalog, and the compiler requires the exact monitor ID/revision and validates +its parameters before grounding. + +The semantic compiler creates a fresh monitor for every grounded call. Pick +expects one attached destination relation, place one detached source relation, +and handover both source-detached and destination-attached relations in the +same observation. The monitor compares fresh backend evidence with owned +object-to-endpoint baselines; it never treats the planned `StateDelta` or +current `TaskState` as proof that the physical effect occurred. Invalid or +missing per-environment evidence remains unresolved. Consecutive-sample state +survives request-mask shrinkage within one attempt and resets when recovery +installs a new attempt. + +```{note} +The monitor contract is backend-neutral. Simulation, hardware perception, or +controller feedback supplies typed pose-relation evidence. The semantic +runtime adapter that connects that evidence to `ExecutionRunner` is separate +from the profile and monitor configuration. +``` + ## Bind, discover, and resolve Pass the profile to @@ -259,13 +256,6 @@ and `bound.skills` is the profile-supported catalog. Registering or replacing an action invalidates the bound profile; bind it again before discovery or resolution. -{attr}`BoundRobotSkillProfile.source_profile` identifies the exact immutable -profile used for the binding. The bound view also snapshots the engine's -monotonic semantic skill-catalog revision. A later agent-visible action -registration or replacement makes discovery, preset selection, and resolution -fail until the profile and semantic integration are rebound; an equal public -descriptor does not make a different implementation owner safe to reuse. - ## Extend the graph beyond manipulation Resource and capability identifiers are open strings. A joint-driven mobile @@ -342,6 +332,5 @@ may retain a full-robot trajectory for feedback and offline compilation, but runtime dispatch is scoped to the endpoints in each command frame. ``` -See {doc}`index` for the direct atomic-action core, -{doc}`../semantic_skills` for compiler/runtime integration, and +See {doc}`index` for the direct atomic-action core and {doc}`../scene_registry` for canonical scene identity and snapshots. diff --git a/docs/source/overview/sim/index.rst b/docs/source/overview/sim/index.rst index 07aaa9eb9..dcd93d472 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -150,9 +150,6 @@ Choosing Where to Start - Use :doc:`atomic_actions/robot_skill_profiles` when semantic skills should resolve robot resources and policy presets from reusable embodiment configuration. -- Use :doc:`semantic_skills` when an application or agent should issue - robot-independent object-centric calls and retain verified state across - dynamic task segments. - Use :doc:`atomic actions ` when building scripted manipulation from reusable motion primitives. @@ -179,5 +176,4 @@ See Also solvers/index planners/index scene_registry.md - semantic_skills.md atomic_actions/index diff --git a/docs/source/overview/sim/scene_registry.md b/docs/source/overview/sim/scene_registry.md index 5e2804f56..a2a97bf46 100644 --- a/docs/source/overview/sim/scene_registry.md +++ b/docs/source/overview/sim/scene_registry.md @@ -334,6 +334,6 @@ core by hand. The list form derives obstacle names from each object's `uid` (or an `obstacle_` fallback). It is not the registry-backed path and does not provide alias normalization or registry/provider/planner construction checks. -See {doc}`semantic_skills` for manifest and semantic-call integration, -{doc}`atomic_actions/index` for snapshot grounding and recovery semantics, and +See {doc}`atomic_actions/robot_skill_profiles` for manifest and semantic-call +integration, {doc}`atomic_actions/index` for snapshot grounding and recovery semantics, and {doc}`planners/curobo_planner` for cuRobo world representation and frame details. diff --git a/docs/source/overview/sim/semantic_skills.md b/docs/source/overview/sim/semantic_skills.md deleted file mode 100644 index 65ef61d29..000000000 --- a/docs/source/overview/sim/semantic_skills.md +++ /dev/null @@ -1,312 +0,0 @@ -(semantic-skills)= - -# Semantic skills - -```{currentmodule} embodichain.lab.sim.skills -``` - -Semantic skills are the application-facing layer above -{doc}`atomic actions `. A semantic call names an object, -relation, and optional robot participant without exposing joint groups, planner -instances, raw controller commands, or an `ActionBinding`. The semantic layer -validates that declaration against one scene and robot embodiment, then lowers -it to the same typed atomic-action runtime used by direct Python callers. - -This boundary is useful for MLLM agents, task planners, configuration-driven -applications, and users who want robot-independent task code. It is not an -agent loop: the application still owns task selection, perception policy, -physical-effect verification, and any task-level fallback strategy. - -```text -SemanticCallSpec values - | - v -SemanticIntegrationManifest - +-- SceneManifest canonical IDs and affordance metadata - +-- RobotSkillProfile resources, commands, presets, providers - `-- SemanticCallCatalog discoverable call schemas - | - v bind live registry + action engine -BoundSemanticIntegration - | - v -SemanticSkillCompiler - analyze() -> SemanticWorkflow provider-free validation and look-ahead - ground() -> GroundedSemanticCall latest observation -> ActionInvocation - | - v -SemanticSkillRuntime / SemanticTask - | - v -ExecutionRunner -> controller transports -> verified effects -``` - -## Semantic skills or direct atomic actions? - -Both paths use `AtomicActionEngine` and therefore share planning, controller -authorization, recovery, and effect semantics. - -| Choose | When it is the better boundary | -|---|---| -| Semantic skills | Task code should be robot-independent; an agent or planner emits object-centric calls; scene and resource validation should happen before controller work; dynamic task segments must retain verified state. | -| Direct atomic actions | A scripted application already knows exact goals, bindings, policies, and options; low-level tuning or a custom controller contract is part of the application. | - -For a user writing a small fixed robot script, direct atomic actions usually -have fewer integration objects. For an MLLM agent or an application targeting -multiple robot profiles, semantic skills provide the safer and more stable -interface. - -## Public semantic calls - -The built-in catalog returned by {func}`builtin_semantic_call_catalog` exposes -three curated call values: - -| Call | Intent | Main lowering behavior | -|---|---|---| -| {class}`Pick` | Acquire a registered object, optionally through an explicit grasp affordance. | Selects a capability-compatible grasp affordance and lowers to atomic `pick_up`. | -| {class}`Place` | Release a held object at an absolute pose, on a support, or inside a container. | Requires exactly one of `at`, `on`, or `inside`; relation targets use an explicitly installed typed grounder. | -| {class}`HandOver` | Transfer a held object to another robot resource. | Uses a robot-profile-selected provider for the middle and default final pose; an explicit `final_target` overrides the latter. | - -{class}`SemanticPose` expresses an absolute object-space pose with a position -and normalized WXYZ quaternion. Scene objects and affordances use typed -{class}`SceneObjectRef` and {class}`SceneAffordanceRef` values, so aliases are -resolved at the registry boundary instead of being propagated into execution. - -Extensions use {class}`RegisteredSemanticCall`. Its argument tree accepts only -declarative values; tensors, callables, classes, modules, and live simulator -objects are rejected. A registered descriptor must identify one exact -agent-visible atomic target, and the compiler must install a matching -{class}`RegisteredSemanticLowerer` with the same call ID and schema version. -Curated calls cannot be remapped through this extension mechanism. - -## Static integration - -Create the static declaration before execution: - -```python -from embodichain.lab.sim.skills import ( - SceneManifest, - SemanticIntegrationManifest, - builtin_semantic_call_catalog, -) - -manifest = SemanticIntegrationManifest( - scene=SceneManifest.from_registry(scene_registry), - robot_profile=robot_profile, - call_catalog=builtin_semantic_call_catalog(), -) -``` - -`SceneManifest` is provider-free: creating it does not observe simulation or -perception. It snapshots canonical identity, aliases, topology, affordance -capabilities and revisions, and collision-world mode. `manifest.bind(...)` -requires the live {class}`SceneRegistry` to match that snapshot and binds the -profile to the exact action engine. Replacing an installed agent-visible action, -changing the bound profile, or changing scene metadata invalidates the old -integration rather than silently reusing stale contracts. - -Policy preset selection is deterministic: - -1. `SemanticIntegrationManifest.runtime_preset`, when configured; -2. `RobotSkillProfile.skill_presets[atomic_skill_id]`; -3. `RobotSkillProfile.default_preset`. - -A missing or unknown preset is a validation error. The selected -{class}`SkillPolicyPreset` owns the motion policy, recovery policy, and -`ExecutionRunnerCfg` used by that call. - -## Analyze first, ground from fresh state - -{meth}`SemanticSkillCompiler.analyze` performs provider-free work: - -- catalog and schema discovery; -- canonical scene and affordance resolution; -- robot-resource and preset selection; -- verified-held-object flow analysis; -- first-release look-ahead for Pick grasp selection; -- validation that required lowerers and grounders are installed. - -It returns an immutable {class}`SemanticWorkflow`. No scene provider is read and -no planner is run at this stage. - -{meth}`SemanticSkillCompiler.ground` lowers exactly one analyzed call from the -latest {class}`~embodichain.lab.sim.atomic_actions.PlanningContext`. It resolves -late-bound relation or handover targets and returns a -{class}`GroundedSemanticCall` containing an `ActionInvocation` and an owned -per-environment `eligible_mask`: - -```python -workflow = compiler.analyze(calls, workflow_id="sort_workpiece") -grounded = compiler.ground( - workflow, - call_index=0, - context=latest_context, - eligible_mask=active_rows, -) -session = engine.start( - (grounded.invocation,), - latest_context, - eligible_mask=grounded.eligible_mask, -) -``` - -The runtime performs this JIT grounding automatically before every call. Known -calls should be submitted together when possible: a `Pick -> Place` or -`Pick -> HandOver` segment lets analysis pass the first downstream object target -into grasp selection. Splitting those calls into separate dynamic segments is -valid, but removes that look-ahead information from the earlier Pick. - -## Construct a runtime - -Use {meth}`SemanticSkillRuntime.from_simulation` for the standard simulation -path. It creates a registry-backed planning scene provider, a -`SimulationExecutionAdapter`, an `AtomicActionEngine` with built-ins, and the -semantic manifest/compiler: - -```python -runtime = SemanticSkillRuntime.from_simulation( - simulation=sim, - robot=robot, - motion_generator=motion_generator, - scene_registry=scene_registry, - robot_profile=robot_profile, - effect_verifier=verify_effect, - control_dt=4 * sim.sim_config.physics_dt, -) -``` - -Use {meth}`SemanticSkillRuntime.bind` when the application owns custom -observation, command, clock, endpoint-adapter, or hardware ports. Only one -{class}`SemanticTask` may own a runtime at a time; this layer does not implement -a resource scheduler or lease manager. - -`runtime.runner_cfg`, when supplied, overrides the runner configuration from -every selected skill preset. When omitted, each grounded call uses its own -preset's runner configuration. `control_dt` is the command cadence and is -independent of the simulation physics period. - -## Execute a fixed workflow - -The minimal robot-independent program names only the registered object and its -desired final pose: - -```python -from embodichain.lab.sim.skills import Pick, Place, SceneObjectRef, SemanticPose - -workpiece = SceneObjectRef("workpiece") -calls = ( - Pick(object=workpiece), - Place( - object=workpiece, - at=SemanticPose( - position=(-0.40, 0.48, 0.025), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), - ), - ), -) - -result = runtime.run( - calls, - task_id="pick_and_place", - effect_verifier=verify_effect, -) -result.require_all_succeeded() -``` - -{meth}`SemanticSkillRuntime.run` is blocking and requires a -`SemanticEffectVerifier`. Use {meth}`SemanticSkillRuntime.start` plus -{meth}`SemanticExecution.step` or -{meth}`SemanticExecution.run_until_blocked` when physical verification arrives -asynchronously. A pending effect produces -`SemanticExecutionStatus.WAITING_FOR_EFFECT`; resume it with a boolean -per-environment `effect_success` mask. - -## Dynamic tasks - -Dynamic task construction is supported at completed semantic-segment -boundaries. A {class}`SemanticTask` carries verified `TaskState`, the latest -observation, and a sticky eligible cohort across those decisions: - -```python -with runtime.open_task("clear_table") as task: - first = task.run_segment( - (Pick(object=workpiece),), - segment_id="acquire", - effect_verifier=verify_effect, - ) - - next_calls = decide_next_calls(first.task_state, task.latest_context) - task.run_segment( - next_calls, - segment_id="agent_decision_1", - effect_verifier=verify_effect, - ) - result = task.finish() -``` - -The following boundaries are intentional: - -- only one segment executes at a time; -- successful segments leave the task open until `finish()` or `cancel()`; -- failed or cancelled segments are terminal and release runtime ownership; -- environment rows that become ineligible remain excluded in later calls and - segments; -- `revise_current()` can update a compatible in-flight call, but it cannot - replace the semantic skill, logical invocation, or runtime endpoint addresses. - -The runtime does not automatically choose a replacement skill, re-run an agent, -or reconcile symbolic state after an uncertain physical effect. Implement those -task-level policies in the application at a safe segment boundary. - -## Recovery and physical success - -Each grounded call runs through the existing closed-loop `ExecutionRunner`. -Depending on its `RecoveryPolicy`, it can detect and recover from tracking -errors, supported scene-target motion, collision-world revisions, timeouts, and -per-environment planning failure. Recovery is bounded and emits structured -atomic-action events retained in {class}`SemanticCallRecord` and aggregated by -{class}`SemanticSegmentResult` and {class}`SemanticTaskResult`. - -Dynamic target recovery follows the atomic primitive's dependency contract. -For example, Pick monitors its object/grasp dependency only through the -`approach` segment; contact-, close-, and lift-induced object movement is not -treated as an external target update. Atomic HandOver can monitor -`SceneEntityPose` values supplied for its middle and final option poses. - -Planning success is not physical success. Attachment, release, and ownership -transfer are committed only after the application verifier accepts the pending -effect for each environment. A failed verification follows the configured -atomic recovery budget; if the runner terminates unsuccessfully, the semantic -segment and task fail. There is no implicit success assumption. - -Final task status is: - -- `SemanticTaskStatus.SUCCEEDED` when all initially eligible rows remain; -- `SemanticTaskStatus.PARTIAL_SUCCESS` when a non-empty subset remains; -- `SemanticTaskStatus.FAILED` when execution fails or no row remains; -- `SemanticTaskStatus.CANCELLED` after explicit cancellation. - -Call {meth}`SemanticTaskResult.require_all_succeeded` when partial batch success -is not acceptable. - -## Diagnostics and extension points - -{meth}`SemanticSkillRuntime.validate` exposes static analysis without observing, -planning, or executing. Static integration and grounding errors use -{class}`SemanticValidationError`, whose {class}`SemanticDiagnostic` contains a -stable code, a complete path, a human-readable message, and sorted candidates. -Agents should consume the structured fields rather than parse the exception -string. - -Three explicit extension points keep executable objects outside semantic calls: - -- {class}`RegisteredSemanticLowerer` lowers a catalog-registered call; -- {class}`RelationTargetGrounder` converts a capability-, payload-type-, and - revision-matched relation into an object pose; -- {class}`HandOverPoseProvider` supplies embodiment-appropriate middle and - default final object targets and is selected through - `RobotSkillProfile.grounding_providers["hand_over"]`. - -See {doc}`/tutorial/semantic_skills` for complete runnable Place and dual-arm -HandOver examples, {doc}`scene_registry` for affordance registration, and -{doc}`atomic_actions/robot_skill_profiles` for embodiment resource binding. diff --git a/docs/source/tutorial/index.rst b/docs/source/tutorial/index.rst index c383e02f4..10b76881f 100644 --- a/docs/source/tutorial/index.rst +++ b/docs/source/tutorial/index.rst @@ -22,19 +22,18 @@ Follow the tutorials in this order for the best learning experience: 9. :doc:`motion_gen` — Generate smooth trajectories with motion planners. 10. :doc:`robot_articulation` — Plan contact-rich motion to open a passive drawer and push it halfway back. 11. :doc:`atomic_actions` — Use built-in action primitives (move, move joints, pick, move held object, place). -12. :doc:`semantic_skills` — Build robot-independent Pick, Place, and dual-arm workflows on top of atomic actions. -13. :doc:`gizmo` — Interactively control robots with on-screen gizmos. +12. :doc:`gizmo` — Interactively control robots with on-screen gizmos. **Phase 2: Environments** -14. :doc:`basic_env` — Create a simple Gymnasium environment with ``BaseEnv``. Prerequisite: Phase 1 basics. -15. :doc:`modular_env` — Build a config-driven environment with ``EmbodiedEnv``, managers, and randomization. Prerequisite: :doc:`basic_env`. -16. :doc:`data_generation` — Generate expert demonstration datasets for imitation learning. Prerequisite: :doc:`modular_env`. -17. :doc:`rl` — Train RL agents with PPO or GRPO. Prerequisite: :doc:`basic_env`. +13. :doc:`basic_env` — Create a simple Gymnasium environment with ``BaseEnv``. Prerequisite: Phase 1 basics. +14. :doc:`modular_env` — Build a config-driven environment with ``EmbodiedEnv``, managers, and randomization. Prerequisite: :doc:`basic_env`. +15. :doc:`data_generation` — Generate expert demonstration datasets for imitation learning. Prerequisite: :doc:`modular_env`. +16. :doc:`rl` — Train RL agents with PPO or GRPO. Prerequisite: :doc:`basic_env`. **Phase 3: Extending the Framework** -18. :doc:`/guides/add_robot` — Add a new robot model to EmbodiChain. +17. :doc:`/guides/add_robot` — Add a new robot model to EmbodiChain. .. toctree:: :maxdepth: 1 @@ -52,7 +51,6 @@ Follow the tutorials in this order for the best learning experience: motion_gen robot_articulation atomic_actions - semantic_skills gizmo basic_env modular_env diff --git a/docs/source/tutorial/semantic_skills.rst b/docs/source/tutorial/semantic_skills.rst deleted file mode 100644 index 4422ea7ae..000000000 --- a/docs/source/tutorial/semantic_skills.rst +++ /dev/null @@ -1,437 +0,0 @@ -Semantic skills -=============== - -Semantic skills let task code describe object-centric intent while the scene -registry and robot profile own simulator- and embodiment-specific details. This -tutorial covers two complete examples: - -* ``Pick -> Place`` with one manipulator; -* ``Pick -> RegisteredSemanticCall`` lowered to a dual-arm HandOver. - -The runnable sources are: - -* ``scripts/tutorials/semantic_skill/place.py``; -* ``scripts/tutorials/semantic_skill/hand_over.py``; -* ``scripts/tutorials/semantic_skill/tutorial_utils.py`` for shared setup and - verification helpers. - -Both runnable examples use the same three-part structure: - -* ``create_*_application(...)`` assembles a fully bound - ``SemanticSkillRuntime`` and installs its default physical-effect verifier; -* ``create_*_task()`` declares only robot-independent semantic calls; -* ``app.run(task, ...)`` is the application-facing execution entry point. - -``app`` is still a ``SemanticSkillRuntime`` rather than another wrapper class. -The factory only keeps simulator, scene-registry, robot-profile, and verifier -construction out of the task declaration. - -Read :doc:`atomic_actions` first if you need the underlying planning, execution, -and effect-verification model. The complete semantic architecture is documented -in :doc:`/overview/sim/semantic_skills`; canonical scene registration is covered -by :doc:`/overview/sim/scene_registry`. - -Run the examples ----------------- - -The examples are interactive by default: - -.. code-block:: bash - - python scripts/tutorials/semantic_skill/place.py - python scripts/tutorials/semantic_skill/hand_over.py - -For an unattended simulation run, use: - -.. code-block:: bash - - python scripts/tutorials/semantic_skill/place.py --headless --auto_play --device cpu - python scripts/tutorials/semantic_skill/hand_over.py --headless --auto_play --device cpu - -Both examples accept the common simulation tutorial flags. Use ``--help`` for -the complete list. ``--diagnose_plan`` takes a separate offline path that -analyzes, grounds, and statically compiles the workflow without executing -controller commands: - -.. code-block:: bash - - python scripts/tutorials/semantic_skill/place.py --headless --device cpu --diagnose_plan - python scripts/tutorials/semantic_skill/hand_over.py --headless --device cpu --diagnose_plan - -.. attention:: - - Diagnostic compilation projects expected attachment changes hypothetically - between calls. It proves that the current workflow can be lowered and - planned; it does not prove that a physical grasp, release, or transfer - occurred. Normal execution uses ``SemanticSkillRuntime`` and an explicit - effect verifier. - -The application entry ---------------------- - -After constructing the simulator entities, normal task-facing code is compact: - -.. code-block:: python - - app = create_place_application( - sim, - robot, - workpiece, - hand_open=hand_open, - hand_grasp=hand_grasp, - n_sample=args.n_sample, - force_reannotate=args.force_reannotate, - ) - - result = app.run( - create_place_task(), - task_id="tutorial.semantic_pick_place", - on_step=observe_runtime_step, - ) - result.require_all_succeeded() - -The factory installs the live effect verifier on the runtime, so it does not -appear in every ``run`` call. ``on_step`` remains explicit because it is -optional tutorial observability rather than semantic task intent. - -Keep the whole known task in one tuple when possible. Use ``open_task`` and -multiple segments only when a later call genuinely depends on a new -observation or an application/agent decision. - -Example 1: semantic Pick and Place ----------------------------------- - -The Place example demonstrates the normal built-in path. Its workflow contains -no robot control-part names: - -.. code-block:: python - - from embodichain.lab.sim.skills import ( - Pick, - Place, - SceneObjectRef, - SemanticPose, - ) - - def create_place_task() -> tuple[Pick, Place]: - workpiece = SceneObjectRef("workpiece") - return ( - Pick(object=workpiece), - Place( - object=workpiece, - at=SemanticPose( - position=(-0.40, 0.48, 0.025), - quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), - ), - ), - ) - -The scene registry owns object identity -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -The simulation object is registered under the canonical semantic ID -``workpiece``. The grasp affordance is a direct child with the open -``affordance.grasp`` capability and an explicit payload revision. The object -selects it as its capability-scoped default: - -.. code-block:: python - - object_ref = SceneObjectRef("workpiece") - grasp_ref = SceneAffordanceRef("workpiece.grasp.antipodal") - - object_registration = replace( - simulation_registry.lookup(object_ref), - semantic_type="cube", - default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp_ref}, - ) - registry = SceneRegistry( - ( - object_registration, - SceneEntityRegistration( - ref=grasp_ref, - parent=object_ref, - native_name="antipodal_grasp", - affordance=antipodal_affordance, - affordance_capabilities=frozenset( - {GRASP_AFFORDANCE_CAPABILITY} - ), - affordance_revision="antipodal-v1", - relative_pose=torch.eye(4), - ), - ) - ) - -``Pick(object=workpiece)`` can now omit an explicit affordance. Resolution is -deterministic because the parent owns one default for the required capability. - -The robot profile owns embodiment details -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -The tutorial profile maps one logical ``primary_manipulator`` resource onto the -robot's ``arm`` motion endpoint and ``hand`` grasp endpoint. It also owns the -semantic ``open`` and ``grasp`` joint commands, resource default, and per-skill -policy presets: - -.. code-block:: python - - profile = RobotSkillProfile( - profile_id="tutorial.single_arm", - resources={ - "primary_manipulator": create_manipulator_resource( - "primary_manipulator", - motion_control_part="arm", - grasp_control_part="hand", - ) - }, - command_profiles={ - "hand": ControlPartCommandProfile.joint_positions( - open=hand_open, - grasp=hand_grasp, - ) - }, - defaults={ - "pick_up": ResourceBinding( - resources={"primary": "primary_manipulator"} - ), - "place": ResourceBinding( - resources={"primary": "primary_manipulator"} - ), - }, - presets={...}, - default_preset="default", - ) - -The semantic calls remain unchanged if another robot profile can satisfy the -same atomic skill contracts. - -Assemble and run the application -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -``SemanticSkillRuntime.from_simulation`` assembles the standard planning scene, -simulation ports, action engine, manifest, and compiler: - -.. code-block:: python - - def create_place_application( - simulation, - robot, - workpiece, - *, - hand_open, - hand_grasp, - n_sample, - force_reannotate, - ) -> SemanticSkillRuntime: - registry = ... - profile = ... - verify_effect = ... - return SemanticSkillRuntime.from_simulation( - simulation=simulation, - robot=robot, - motion_generator=create_curobo_motion_generator(robot), - scene_registry=registry, - robot_profile=profile, - effect_verifier=verify_effect, - control_dt=4 * simulation.sim_config.physics_dt, - ) - - app = create_place_application(...) - result = app.run( - create_place_task(), - task_id="tutorial.semantic_pick_place", - on_step=observe_runtime_step, - ) - result.require_all_succeeded() - -The verifier checks the live lift, the planned object-to-EEF relation, final -object position, and open hand before accepting the symbolic effects. The -``on_step`` callback reports recovery events and does not decide whether an -effect succeeded. - -Submitting both calls in one segment is important for planning quality. Static -analysis can pass the Place target to Pick as a downstream reachability target. -The runtime still grounds and executes one call at a time from fresh -observations. - -Example 2: registered dual-arm HandOver ---------------------------------------- - -The dual-arm example demonstrates the extension path in addition to resource -disjointness. It registers ``tutorial.hand_over`` against the existing atomic -HandOver descriptor: - -.. code-block:: python - - call_catalog = builtin_semantic_call_catalog().with_descriptor( - SemanticCallDescriptor( - call_id="tutorial.hand_over", - spec_type=RegisteredSemanticCall, - target_descriptor=AtomicHandOver.descriptor(), - ) - ) - - def create_handover_task() -> tuple[Pick, RegisteredSemanticCall]: - return ( - Pick(object=workpiece), - RegisteredSemanticCall( - call_id="tutorial.hand_over", - arguments={"object": workpiece}, - ), - ) - -Why use a registered call here? -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -The public semantic :class:`~embodichain.lab.sim.skills.HandOver` path delegates -middle and final object targets to a named ``HandOverPoseProvider`` selected by -the robot profile. This tutorial instead demonstrates how an application can -publish a separate versioned call schema and explicitly lower it to tuned -``HandOverOptions``. - -The lowerer is executable integration code, so it is installed on the compiler -rather than placed inside ``RegisteredSemanticCall.arguments``: - -.. code-block:: python - - class TutorialHandOverLowerer(RegisteredSemanticLowerer): - call_id = "tutorial.hand_over" - schema_version = 1 - - def lower(self, call, *, context, bound): - semantics = registry.object_semantics( - call.arguments["object"], - affordance=registry.resolve_affordance( - call.arguments["object"], - capability=GRASP_AFFORDANCE_CAPABILITY, - ), - ) - return SemanticLowering( - goal=GraspGoal(semantics), - skill_options=HandOverOptions( - middle_object_pose=middle_pose.to(context.robot.qpos.device), - final_object_pose=final_pose.to(context.robot.qpos.device), - receive_pick_object_part="bottom", - ), - ) - -The descriptor and lowerer call IDs and schema versions must match exactly. A -registered call cannot replace the curated ``pick``, ``place``, or -``hand_over`` meanings. - -The dual-arm profile declares two physically disjoint manipulator resources. -Its defaults map Pick's ``primary`` slot to the source and HandOver's ``source`` -and ``destination`` slots to different resources. Binding rejects overlapping -claims before execution. - -The application factory passes the extension objects and default verifier to -the runtime explicitly: - -.. code-block:: python - - def create_handover_application( - simulation, - robot, - workpiece, - *, - left_open, - left_grasp, - right_open, - right_grasp, - n_sample, - force_reannotate, - ) -> SemanticSkillRuntime: - registry = ... - profile = ... - verify_effect = ... - return SemanticSkillRuntime.from_simulation( - simulation=simulation, - robot=robot, - motion_generator=create_toppra_motion_generator(robot), - scene_registry=registry, - robot_profile=profile, - call_catalog=call_catalog, - effect_verifier=verify_effect, - registered_lowerers=(TutorialHandOverLowerer(registry),), - control_dt=4 * simulation.sim_config.physics_dt, - ) - - app = create_handover_application(...) - result = app.run( - create_handover_task(), - task_id="tutorial.semantic_pick_handover", - on_step=observe_runtime_step, - ) - -The effect verifier first accepts the source Pick only after observing the held -relation. At the transfer boundary it verifies source release, destination -grasp, destination ownership, and the final object target before committing the -new ``TaskState``. - -Dynamic decisions between segments ----------------------------------- - -Use ``open_task`` when an application or agent cannot know the whole task in -advance: - -.. code-block:: python - - with app.open_task("agent_task") as task: - acquire = task.run_segment( - (Pick(object=workpiece),), - segment_id="acquire", - ) - - # Decide only after the successful segment has committed verified state. - destination = choose_destination(acquire.task_state, task.latest_context) - task.run_segment( - (Place(object=workpiece, at=destination),), - segment_id="deliver", - ) - result = task.finish() - -Successful segments retain verified symbolic state and the per-environment -eligible mask. A failed or cancelled segment is terminal. Only one task and one -segment may own a runtime at a time; scheduling multiple independent tasks is an -application responsibility. - -For non-blocking integration, replace ``run_segment`` with ``start_segment`` and -advance the returned ``SemanticExecution`` through ``step``. When its status is -``WAITING_FOR_EFFECT``, inspect ``pending_effect`` and submit an -``effect_success`` boolean mask on a later step. - -Recovery boundaries -------------------- - -The semantic runtime delegates call-local recovery to the atomic -``ExecutionRunner``. Tracking errors, supported scene-target movement, -collision-world revisions, timeouts, and planning failure produce structured -events and consume the selected preset's bounded recovery budget. - -Keep these distinctions in mind: - -* Pick monitors its object and grasp target only through the approach segment; - contact- or lift-induced motion does not look like an external target update. -* Physical effects are never committed from planning success alone. -* Rows that exhaust recovery become ineligible and remain excluded from later - calls and dynamic segments. -* ``revise_current`` can change a compatible active call, but cannot switch to a - different skill or controller address. -* A terminal runtime failure does not automatically choose another skill or - reconcile uncertain physical state. Perform that task-level recovery at an - application-controlled segment boundary. - -Inspect ``SemanticTaskResult.status``, ``eligible_mask``, ``segments``, and -aggregated ``events`` for structured feedback. Call ``require_all_succeeded`` -when partial vectorized success should be treated as an application error. - -Further reading ---------------- - -* :doc:`/overview/sim/semantic_skills` — architecture, ownership, dynamic tasks, - and extension contracts; -* :doc:`/overview/sim/scene_registry` — canonical IDs, affordances, snapshots, - and collision integration; -* :doc:`/overview/sim/atomic_actions/robot_skill_profiles` — resource graphs, - policy presets, and grounding-provider selection; -* :doc:`/overview/sim/atomic_actions/builtin_actions` — behavior and recovery - contracts of the lowered atomic primitives. diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index b9ae2951f..576d2f24a 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -19,34 +19,136 @@ from __future__ import annotations from .calls import ( + DeclarativeValue, HandOver, + OperateArticulation, Pick, Place, + PlaceRelationTarget, RegisteredSemanticCall, SemanticCallCatalog, SemanticCallDescriptor, + SemanticCallSpec, SemanticPose, builtin_semantic_call_catalog, ) from .compiler import ( + AnalyzedSemanticCall, GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, RegisteredSemanticLowerer, RelationTargetGrounder, + SemanticEffectDependency, + SemanticHandOverTarget, SemanticLowering, SemanticObjectTarget, SemanticRelationTarget, SemanticSkillCompiler, SemanticWorkflow, ) +from .effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + CompositeEffectMonitor, + CompositeEffectMonitorCfg, + CompositeEffectMonitorFactory, + ControlPartEvidenceAddress, + CoordinatedHeldObjectCleanupExpectation, + EffectClause, + EffectEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectMonitor, + EffectMonitorDecision, + EffectMonitorFactory, + EffectMonitorParam, + EffectMonitorRef, + EffectMonitorRegistry, + EffectStateExpectation, + FORCE_EFFECT_CHANNEL, + HeldObjectRelation, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + JointStateEvidenceBatch, + POSE_RELATION_EFFECT_CHANNEL, + PoseRelationClause, + PoseRelationEvidenceBatch, + PoseRelationExpectation, + ScalarEffectClause, + ScalarEffectEvidenceBatch, + ScalarEvidenceKind, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, + SymbolicStateDomain, + SymbolicStateKey, +) +from .evidence import ( + ArticulationJointObservationCallback, + BinaryEffectEvidenceQuery, + BinaryEffectObservation, + BinaryObservationCallback, + ControlPartRobotEvidenceSource, + ControlPartSimulationEvidenceProvider, + EffectEvidenceCollectionContext, + EffectEvidenceCollector, + EffectEvidenceProvider, + EffectEvidenceProviderRegistry, + EffectEvidenceQuery, + EffectEvidenceQueryValue, + JointStateEvidenceQuery, + JointStateObservation, + PoseRelationEvidenceQuery, + ScalarEffectEvidenceQuery, + ScalarEffectObservation, + ScalarObservationCallback, + SceneArticulationEvidenceProvider, + build_effect_evidence_queries, +) from .integration import ( + BoundSemanticCall, + BoundSemanticIntegration, + LinkedSemanticCall, + PathPart, SceneEntityManifest, SceneManifest, SemanticDiagnostic, SemanticIntegrationManifest, SemanticValidationError, ) +from .parallel import ( + ParallelBarrierUpdate, + ParallelBranchPlan, + ParallelConflictError, + ParallelStateConflictError, + ParallelTimingError, + ParallelTimingPolicy, + align_parallel_commands, + merge_parallel_effects, + resolve_parallel_barrier, + validate_parallel_claims, +) +from .parallel_runtime import ( + ParallelBranchStaticAnalysis, + ParallelBranchRuntime, + ParallelCommandSafetyValidator, + ParallelLaneCommandSink, + ParallelRuntimeBranch, + ParallelSafetyError, + ParallelSkillResult, + ParallelSkillRuntime, + analyze_parallel_branches, +) from .profiles import ( AmbiguousSkillBindingError, BoundRobotSkillProfile, @@ -66,25 +168,18 @@ SkillPolicyPreset, UnsupportedSkillError, ) -from .runtime import ( - SemanticCallRecord, - SemanticEffectVerifier, - SemanticExecution, - SemanticExecutionStatus, - SemanticExecutionStep, - SemanticSegmentResult, - SemanticSkillRuntime, - SemanticTask, - SemanticTaskResult, - SemanticTaskStatus, -) from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, AmbiguousSceneAffordanceError, + ArticulationJointEvidenceAddress, GRASP_AFFORDANCE_CAPABILITY, PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, RegistrySceneProvider, SceneAffordanceRef, + SceneArticulationJointStateProvider, SceneArticulationRef, SceneCollisionRole, SceneCollisionWorldMode, @@ -99,23 +194,117 @@ SceneRegistry, UnsupportedSceneAffordanceError, ) +from .runtime import ( + AtomicSkills, + EffectEvidenceCollectorPort, + ResolvedCorePolicyTrace, + SkillCallTrace, + SkillEndpointBindingTrace, + SkillEffectTrace, + SkillFailure, + SkillPlanAttemptTrace, + SkillResult, + SkillRuntime, + SkillRuntimeProvider, + SkillScene, + SkillStatus, + task_state_to_metadata, +) __all__ = [ + "ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY", "AmbiguousSceneAffordanceError", "AmbiguousSkillBindingError", + "AnalyzedSemanticCall", + "ArticulationJointEvidenceAddress", + "ArticulationJointObservationCallback", + "ArticulationJointStateExpectation", + "AtomicSkills", + "BinaryEffectClause", + "BinaryEffectEvidenceBatch", + "BinaryEffectEvidenceQuery", + "BinaryEffectObservation", + "BinaryEvidenceKind", + "BinaryObservationCallback", + "BoundSemanticCall", + "BoundSemanticIntegration", "BoundRobotSkillProfile", "ControlPartEndpoint", "ControlPartEndpointAdapter", + "ControlPartEvidenceAddress", + "ControlPartRobotEvidenceSource", + "ControlPartSimulationEvidenceProvider", + "CoordinatedHeldObjectCleanupExpectation", + "COMPOSITE_EFFECT_MONITOR_ID", + "COMPOSITE_EFFECT_MONITOR_REVISION", + "CONTACT_EFFECT_CHANNEL", + "CONSTRAINT_EFFECT_CHANNEL", + "CONTROL_PART_EVIDENCE_PROVIDER_ID", + "CONTROL_PART_EVIDENCE_PROVIDER_REVISION", + "CompositeEffectMonitor", + "CompositeEffectMonitorCfg", + "CompositeEffectMonitorFactory", + "DeclarativeValue", "EndpointResolution", + "EffectClause", + "EffectEvidenceAddress", + "EffectEvidenceBatch", + "EffectEvidenceCollectionContext", + "EffectEvidenceCollector", + "EffectEvidenceCollectorPort", + "EffectEvidenceProvider", + "EffectEvidenceProviderRegistry", + "EffectEvidenceQuery", + "EffectEvidenceQueryValue", + "EffectEvidenceSourceRef", + "EffectMonitor", + "EffectMonitorDecision", + "EffectMonitorFactory", + "EffectMonitorParam", + "EffectMonitorRef", + "EffectMonitorRegistry", + "EffectStateExpectation", + "FORCE_EFFECT_CHANNEL", "GRASP_AFFORDANCE_CAPABILITY", "GroundedSemanticCall", + "HeldObjectRelation", + "HeldObjectStateExpectation", "HandOver", "HandOverPoseProvider", "HandOverPoseTargets", + "LinkedSemanticCall", + "JOINT_STATE_EFFECT_CHANNEL", + "JointStateEffectClause", + "JointStateEvidenceBatch", + "JointStateEvidenceQuery", + "JointStateObservation", + "POSE_RELATION_EFFECT_CHANNEL", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", + "PathPart", + "OperateArticulation", + "ParallelBarrierUpdate", + "ParallelBranchStaticAnalysis", + "ParallelBranchRuntime", + "ParallelCommandSafetyValidator", + "ParallelBranchPlan", + "ParallelConflictError", + "ParallelStateConflictError", + "ParallelTimingError", + "ParallelTimingPolicy", + "ParallelLaneCommandSink", + "ParallelRuntimeBranch", + "ParallelSafetyError", + "ParallelSkillResult", + "ParallelSkillRuntime", + "analyze_parallel_branches", "Pick", "Place", + "PlaceRelationTarget", + "PoseRelationClause", + "PoseRelationEvidenceBatch", + "PoseRelationEvidenceQuery", + "PoseRelationExpectation", "ProfileValidationError", "RegistrySceneProvider", "ResolvedRobotResource", @@ -125,12 +314,15 @@ "ResourceClaim", "ResourceEndpoint", "ResourceEndpointAdapter", + "ResolvedCorePolicyTrace", "RegisteredSemanticCall", "RegisteredSemanticLowerer", "RelationTargetGrounder", "RobotResource", "RobotSkillProfile", "SceneAffordanceRef", + "SceneArticulationJointStateProvider", + "SceneArticulationEvidenceProvider", "SceneArticulationRef", "SceneCollisionRole", "SceneCollisionWorldMode", @@ -144,30 +336,52 @@ "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION", "SceneManifest", + "ScalarEffectClause", + "ScalarEffectEvidenceBatch", + "ScalarEffectEvidenceQuery", + "ScalarEffectObservation", + "ScalarEvidenceKind", + "ScalarExpectation", + "ScalarObservationCallback", "SemanticCallCatalog", "SemanticCallDescriptor", - "SemanticCallRecord", + "SemanticCallSpec", "SemanticDiagnostic", - "SemanticEffectVerifier", - "SemanticExecution", - "SemanticExecutionStatus", - "SemanticExecutionStep", + "SemanticEffectDependency", + "SemanticEffectKind", + "SemanticEffectSpec", + "SymbolicStateDomain", + "SymbolicStateKey", + "SemanticHandOverTarget", "SemanticIntegrationManifest", "SemanticLowering", "SemanticObjectTarget", "SemanticPose", "SemanticRelationTarget", - "SemanticSegmentResult", "SemanticSkillCompiler", - "SemanticSkillRuntime", - "SemanticTask", - "SemanticTaskResult", - "SemanticTaskStatus", "SemanticValidationError", "SemanticWorkflow", "SkillPolicyPreset", + "SkillCallTrace", + "SkillEndpointBindingTrace", + "SkillEffectTrace", + "SkillFailure", + "SkillPlanAttemptTrace", + "SkillResult", + "SkillRuntime", + "SkillRuntimeProvider", + "SkillScene", + "SkillStatus", + "task_state_to_metadata", "UnsupportedSkillError", "UnsupportedSceneAffordanceError", + "build_effect_evidence_queries", + "align_parallel_commands", "builtin_semantic_call_catalog", + "merge_parallel_effects", + "resolve_parallel_barrier", + "validate_parallel_claims", ] diff --git a/embodichain/lab/sim/skills/calls.py b/embodichain/lab/sim/skills/calls.py index 12207be04..21377b28d 100644 --- a/embodichain/lab/sim/skills/calls.py +++ b/embodichain/lab/sim/skills/calls.py @@ -19,7 +19,7 @@ from __future__ import annotations from collections.abc import Iterable, Mapping -from dataclasses import dataclass, field +from dataclasses import dataclass, field, fields import math import re from types import MappingProxyType @@ -295,6 +295,36 @@ def to_matrix(self) -> torch.Tensor: output[:, :3, 3] = position return output[0] if was_unbatched else output + def to_metadata(self) -> dict[str, object]: + """Return the pose as deterministic JSON-safe semantic data.""" + return { + "position": self._position.detach().cpu().tolist(), + "quaternion_wxyz": self._quaternion_wxyz.detach().cpu().tolist(), + } + + +def _call_value_to_metadata(value: DeclarativeValue | object) -> object: + """Serialize one already validated semantic-call payload value.""" + if value is None or type(value) in (bool, int, float, str): + return value + if isinstance(value, SceneEntityRef): + return { + "entity_type": type(value).__name__, + "entity_id": value.entity_id, + } + if type(value) is SemanticPose: + return value.to_metadata() + if isinstance(value, Mapping): + return { + key: _call_value_to_metadata(nested) + for key, nested in sorted(value.items()) + } + if isinstance(value, tuple): + return [_call_value_to_metadata(nested) for nested in value] + raise TypeError( + f"Unsupported validated semantic-call metadata value {type(value).__name__}." + ) + @dataclass(frozen=True, slots=True, kw_only=True, eq=False) class SemanticCallSpec: @@ -316,6 +346,21 @@ def semantic_id(self) -> str: """Return the stable catalog identifier for this call.""" return self.call_kind + def to_metadata(self) -> dict[str, object]: + """Return this semantic call as deterministic JSON-safe data.""" + arguments = { + data_field.name: _call_value_to_metadata(getattr(self, data_field.name)) + for data_field in fields(self) + if data_field.name != "resources" + } + return { + "semantic_id": self.semantic_id, + "call_kind": self.call_kind, + "call_type": type(self).__name__, + "resources": _call_value_to_metadata(self.resources), + "arguments": arguments, + } + @dataclass(frozen=True, slots=True, eq=False) class Pick(SemanticCallSpec): @@ -423,6 +468,76 @@ def __post_init__(self) -> None: ) +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulation(SemanticCallSpec): + """Operate one registered articulation through a typed handle affordance. + + Select either a named affordance target or an explicit absolute joint + position plus handle-relative displacement. Grounding captures the current + live joint position as the source of that declared stroke. Recovery + replans then combine the latest handle pose and joint position to execute + only the remaining signed displacement. + + Args: + articulation: Authoritative articulation reference. + handle: Optional explicit operation affordance. Omission requests the + capability-scoped default registered on the articulation. + target: Optional target name registered by the affordance. + target_position: Explicit absolute desired joint position. + target_displacement: Explicit full signed operation displacement from + the joint position and handle pose captured during grounding. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "operate_articulation" + + articulation: SceneArticulationRef + handle: SceneAffordanceRef | None = None + target: str | None = None + target_position: float | None = None + target_displacement: float | None = None + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + if type(self.articulation) is not SceneArticulationRef: + raise TypeError( + "OperateArticulation.articulation must be a SceneArticulationRef." + ) + if self.handle is not None and type(self.handle) is not SceneAffordanceRef: + raise TypeError( + "OperateArticulation.handle must be a SceneAffordanceRef or None." + ) + named = self.target is not None + explicit_position = self.target_position is not None + explicit_displacement = self.target_displacement is not None + if named: + _validate_identifier( + self.target, + field_name="OperateArticulation.target", + ) + if explicit_position or explicit_displacement: + raise ValueError( + "OperateArticulation.target is mutually exclusive with " + "target_position and target_displacement." + ) + return + if not (explicit_position and explicit_displacement): + raise ValueError( + "OperateArticulation requires either target or the explicit " + "target_position and target_displacement pair." + ) + for field_name in ("target_position", "target_displacement"): + value = getattr(self, field_name) + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError( + f"OperateArticulation.{field_name} must be a finite scalar." + ) + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"OperateArticulation.{field_name} must be finite.") + object.__setattr__(self, field_name, normalized) + + DeclarativeValue: TypeAlias = ( None | bool @@ -577,11 +692,18 @@ class SemanticCallDescriptor: def __post_init__(self) -> None: _validate_identifier(self.call_id, field_name="SemanticCallDescriptor.call_id") - if self.spec_type not in (Pick, Place, HandOver, RegisteredSemanticCall): + if self.spec_type not in ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ): raise TypeError( - "spec_type must be exactly Pick, Place, HandOver, or " - "RegisteredSemanticCall; extensions use the registered payload " - "contract rather than executable call subclasses." + "spec_type must be exactly Pick, Place, HandOver, " + "OperateArticulation, or RegisteredSemanticCall; extensions use " + "the registered payload contract rather than executable call " + "subclasses." ) if not isinstance(self.schema_version, int) or isinstance( self.schema_version, bool @@ -633,6 +755,7 @@ def __post_init__(self) -> None: Pick.call_kind, Place.call_kind, HandOver.call_kind, + OperateArticulation.call_kind, RegisteredSemanticCall.call_kind, }: raise ValueError( @@ -711,7 +834,13 @@ def discover( if type(call) is str: call_id = _validate_identifier(call, field_name="semantic call ID") call_value = None - elif type(call) in (Pick, Place, HandOver, RegisteredSemanticCall): + elif type(call) in ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ): call_id = call.semantic_id call_value = call else: @@ -747,6 +876,9 @@ def _builtin_call_target( from embodichain.lab.sim.atomic_actions.primitives.hand_over import ( HandOver as HandOverAction, ) + from embodichain.lab.sim.atomic_actions.primitives.operate_articulation import ( + OperateArticulation as OperateArticulationAction, + ) from embodichain.lab.sim.atomic_actions.primitives.pick_up import PickUp from embodichain.lab.sim.atomic_actions.primitives.place import Place as PlaceAction @@ -754,6 +886,7 @@ def _builtin_call_target( Pick: PickUp.descriptor(), Place: PlaceAction.descriptor(), HandOver: HandOverAction.descriptor(), + OperateArticulation: OperateArticulationAction.descriptor(), } try: return targets[spec_type] @@ -773,7 +906,7 @@ def builtin_semantic_call_catalog() -> SemanticCallCatalog: call_id=spec_type.call_kind, spec_type=spec_type, ) - for spec_type in (Pick, Place, HandOver) + for spec_type in (Pick, Place, HandOver, OperateArticulation) ) return SemanticCallCatalog(descriptors) @@ -781,6 +914,7 @@ def builtin_semantic_call_catalog() -> SemanticCallCatalog: __all__ = [ "DeclarativeValue", "HandOver", + "OperateArticulation", "Pick", "Place", "PlaceRelationTarget", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 3e1f0c21b..1c463ba6f 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -21,7 +21,6 @@ from abc import ABC, abstractmethod from collections.abc import Iterable, Mapping from dataclasses import dataclass, field, replace -from enum import Enum from types import MappingProxyType from typing import ClassVar from uuid import uuid4 @@ -33,25 +32,52 @@ ActionInvocation, ActionOptions, Affordance, + ArticulationOperationAffordance, GraspGoal, HandOverOptions, - JointPositionTarget, HeldObjectState, PickUpOptions, PlaceGoal, PlaceOptions, + OperateArticulationGoal, PlanningContext, PoseGoalValue, + SceneArticulationOperationGeometry, SceneEntityPose, ) from .calls import ( HandOver, + OperateArticulation, Pick, Place, RegisteredSemanticCall, SemanticCallSpec, SemanticPose, ) +from .effects import ( + ArticulationJointStateExpectation, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + POSE_RELATION_EFFECT_CHANNEL, + BinaryEffectClause, + BinaryEvidenceKind, + CompositeEffectMonitorFactory, + CoordinatedHeldObjectCleanupExpectation, + EffectClause, + EffectMonitor, + EffectMonitorRef, + EffectMonitorRegistry, + EffectEvidenceSourceRef, + EffectStateExpectation, + HeldObjectRelation, + HeldObjectStateExpectation, + JointStateEffectClause, + PoseRelationClause, + PoseRelationExpectation, + SemanticEffectKind, + SemanticEffectSpec, + SymbolicStateKey, +) from .integration import ( BoundSemanticCall, BoundSemanticIntegration, @@ -60,6 +86,10 @@ SemanticValidationError, ) from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, @@ -86,15 +116,6 @@ def _diagnostic( return SemanticValidationError(SemanticDiagnostic(code, path, message, candidates)) -class SemanticEffectKind(str, Enum): - """Symbolic effect boundary inferred for a semantic call.""" - - ATTACH = "attach" - RELEASE = "release" - TRANSFER = "transfer" - REGISTERED = "registered" - - @dataclass(frozen=True, slots=True) class SemanticRelationTarget: """Statically selected relation affordance awaiting typed grounding.""" @@ -238,7 +259,12 @@ class AnalyzedSemanticCall: index: int bound: BoundSemanticCall effect_kind: SemanticEffectKind - downstream_object_target: SemanticObjectTarget | None = None + symbolic_writes: frozenset[SymbolicStateKey] = frozenset() + opaque_symbolic_effect: bool = False + effect_monitor_ref: EffectMonitorRef | None = None + downstream_object_targets: tuple[SemanticObjectTarget, ...] = () + requires_verified_held_object: bool = False + requires_fresh_observation: bool = True def __post_init__(self) -> None: if type(self.index) is not int or self.index < 0: @@ -247,13 +273,40 @@ def __post_init__(self) -> None: raise TypeError("bound must be exactly BoundSemanticCall.") if not isinstance(self.effect_kind, SemanticEffectKind): raise TypeError("effect_kind must be a SemanticEffectKind.") - if self.downstream_object_target is not None and ( - type(self.downstream_object_target) is not SemanticObjectTarget + if type(self.symbolic_writes) is not frozenset or not all( + type(write) is SymbolicStateKey for write in self.symbolic_writes ): raise TypeError( - "downstream_object_target must be exactly SemanticObjectTarget " - "or None." + "symbolic_writes must be an exact frozenset of " + "SymbolicStateKey values." ) + if type(self.opaque_symbolic_effect) is not bool: + raise TypeError("opaque_symbolic_effect must be a bool.") + if self.opaque_symbolic_effect and self.symbolic_writes: + raise ValueError( + "Opaque symbolic effects cannot also claim inferred exact keys." + ) + if self.effect_monitor_ref is not None: + if not isinstance(self.effect_monitor_ref, EffectMonitorRef): + raise TypeError( + "effect_monitor_ref must be an EffectMonitorRef or None." + ) + object.__setattr__( + self, + "effect_monitor_ref", + self.effect_monitor_ref.snapshot(), + ) + targets = tuple(self.downstream_object_targets) + if not all(type(target) is SemanticObjectTarget for target in targets): + raise TypeError( + "downstream_object_targets must contain exact " + "SemanticObjectTarget values." + ) + object.__setattr__(self, "downstream_object_targets", targets) + if type(self.requires_verified_held_object) is not bool: + raise TypeError("requires_verified_held_object must be a bool.") + if type(self.requires_fresh_observation) is not bool: + raise TypeError("requires_fresh_observation must be a bool.") @property def call(self) -> SemanticCallSpec: @@ -373,8 +426,38 @@ class GroundedSemanticCall: analyzed: AnalyzedSemanticCall invocation: ActionInvocation + effect_spec: SemanticEffectSpec | None + effect_monitor: EffectMonitor | None = field(repr=False, compare=False) _eligible_mask: torch.Tensor = field(repr=False, compare=False) + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :class:`SemanticSkillCompiler`.""" + del args, kwargs + raise TypeError( + "GroundedSemanticCall values are created by " + "SemanticSkillCompiler.ground()." + ) + + @classmethod + def _create( + cls, + *, + analyzed: AnalyzedSemanticCall, + invocation: ActionInvocation, + effect_spec: SemanticEffectSpec | None, + effect_monitor: EffectMonitor | None, + eligible_mask: torch.Tensor, + ) -> GroundedSemanticCall: + """Create one compiler-owned grounded result.""" + instance = object.__new__(cls) + object.__setattr__(instance, "analyzed", analyzed) + object.__setattr__(instance, "invocation", invocation) + object.__setattr__(instance, "effect_spec", effect_spec) + object.__setattr__(instance, "effect_monitor", effect_monitor) + object.__setattr__(instance, "_eligible_mask", eligible_mask.clone()) + instance.__post_init__() + return instance + def __post_init__(self) -> None: if type(self.analyzed) is not AnalyzedSemanticCall: raise TypeError("analyzed must be exactly AnalyzedSemanticCall.") @@ -382,6 +465,20 @@ def __post_init__(self) -> None: raise TypeError("invocation must be exactly ActionInvocation.") if self.invocation.skill_id != self.analyzed.bound.linked.descriptor.skill_id: raise ValueError("invocation skill_id must match the analyzed call.") + if (self.effect_spec is None) != (self.effect_monitor is None): + raise ValueError( + "effect_spec and effect_monitor must either both be set or both be None." + ) + if self.effect_spec is not None: + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec or None.") + if not isinstance(self.effect_monitor, EffectMonitor): + raise TypeError("effect_monitor must be an EffectMonitor or None.") + if self.effect_spec.semantic_id != self.analyzed.call.semantic_id: + raise ValueError( + "effect_spec semantic_id must match the analyzed call." + ) + object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) 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: @@ -406,6 +503,7 @@ def __init__( registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), relation_grounders: Iterable[RelationTargetGrounder] = (), handover_pose_providers: Iterable[HandOverPoseProvider] = (), + effect_monitor_registry: EffectMonitorRegistry | None = None, ) -> None: """Install immutable semantic lowering and grounding registries. @@ -414,6 +512,7 @@ def __init__( registered_lowerers: Explicit implementations for registered calls. relation_grounders: Exact capability/payload/revision dispatch entries. handover_pose_providers: Named embodiment-owned handover providers. + effect_monitor_registry: Versioned semantic-effect monitor factories. """ if type(integration) is not BoundSemanticIntegration: raise TypeError("integration must be exactly BoundSemanticIntegration.") @@ -522,6 +621,16 @@ def __init__( self._registered_lowerers = MappingProxyType(lowerers) self._relation_grounders = MappingProxyType(normalized_grounders) self._handover_pose_providers = MappingProxyType(normalized_handover_providers) + selected_monitor_registry = ( + EffectMonitorRegistry((CompositeEffectMonitorFactory(),)) + if effect_monitor_registry is None + else effect_monitor_registry + ) + if not isinstance(selected_monitor_registry, EffectMonitorRegistry): + raise TypeError( + "effect_monitor_registry must be an EffectMonitorRegistry or None." + ) + self._effect_monitor_registry = selected_monitor_registry @property def integration(self) -> BoundSemanticIntegration: @@ -545,6 +654,11 @@ def handover_pose_providers(self) -> Mapping[str, HandOverPoseProvider]: """Return installed handover pose providers by stable provider ID.""" return self._handover_pose_providers + @property + def effect_monitor_registry(self) -> EffectMonitorRegistry: + """Return the immutable versioned effect-monitor factory registry.""" + return self._effect_monitor_registry + def analyze( self, calls: Iterable[SemanticCallSpec], @@ -578,7 +692,13 @@ def analyze( ) from exc if not supplied: raise ValueError("Semantic workflow requires at least one call.") - allowed_types = (Pick, Place, HandOver, RegisteredSemanticCall) + allowed_types = ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ) if not all(type(call) in allowed_types for call in supplied): raise TypeError("calls must contain exact supported semantic call values.") @@ -684,6 +804,8 @@ def analyze( index, bound.binding.resource_ids["destination"], ) + elif type(call) is OperateArticulation: + effect_kind = SemanticEffectKind.ARTICULATION else: effect_kind = SemanticEffectKind.REGISTERED # A registered extension has no declarative state-flow contract @@ -696,17 +818,31 @@ def analyze( zip(bound_calls, effect_kinds, strict=True) ): call = bound.linked.call - downstream_target = ( - self._downstream_target(index, bound_calls) + requires_held = type(call) in (Place, HandOver) + downstream_targets = ( + self._downstream_targets(index, bound_calls) if type(call) is Pick - else None + else () + ) + effect_monitor_ref = self._effect_monitor_ref( + bound, + effect_kind, + path=(*path, index, "effect_monitor"), + ) + symbolic_writes, opaque_symbolic_effect = self._static_symbolic_writes( + bound, + path=(*path, index, "call"), ) analyzed.append( AnalyzedSemanticCall( index=index, bound=bound, effect_kind=effect_kind, - downstream_object_target=downstream_target, + symbolic_writes=symbolic_writes, + opaque_symbolic_effect=opaque_symbolic_effect, + effect_monitor_ref=effect_monitor_ref, + downstream_object_targets=downstream_targets, + requires_verified_held_object=requires_held, ) ) return SemanticWorkflow( @@ -735,6 +871,124 @@ def _inherit_held_resource( resources[slot_id] = holder[1] return replace(call, resources=resources) + def _static_symbolic_writes( + self, + bound: BoundSemanticCall, + *, + path: tuple[PathPart, ...], + ) -> tuple[frozenset[SymbolicStateKey], bool]: + """Return exact provider-free ``TaskState`` keys for one linked call. + + Curated calls own these contracts. Registered calls remain an opaque + physical-effect boundary until their public descriptor grows an + explicit static-effect contract; lowering arguments are never guessed. + Conditional coordinated-held cleanup is likewise omitted because its + exact pair keys depend on the verified input ``TaskState``. + """ + call = bound.linked.call + if type(call) in (Pick, Place): + return ( + frozenset( + { + SymbolicStateKey.held_object( + self._participant_task_state_key( + bound, + slot_id="primary", + path=(*path, "resources", "primary"), + ) + ) + } + ), + False, + ) + if type(call) is HandOver: + return ( + frozenset( + SymbolicStateKey.held_object( + self._participant_task_state_key( + bound, + slot_id=slot_id, + path=(*path, "resources", slot_id), + ) + ) + for slot_id in ("source", "destination") + ), + False, + ) + if type(call) is OperateArticulation: + handle_ref = bound.linked.affordances.get("handle") + if handle_ref is None: + raise AssertionError( + "Linked articulation call lacks an operation affordance." + ) + registration = self._integration.scene_registry.lookup( + handle_ref, + expected_type=SceneAffordanceRef, + ) + affordance = registration.affordance + if ( + type(affordance) is not ArticulationOperationAffordance + or ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + not in registration.affordance_capabilities + ): + raise _diagnostic( + "invalid_articulation_affordance", + (*path, "handle"), + f"Handle {handle_ref.entity_id!r} must expose an exact " + "ArticulationOperationAffordance payload and the articulation " + "operation capability.", + ) + return ( + frozenset( + { + SymbolicStateKey.articulation_joint( + call.articulation.entity_id, + affordance.joint_id, + ) + } + ), + False, + ) + if type(call) is RegisteredSemanticCall: + return frozenset(), True + raise AssertionError(f"Unsupported linked call {type(call).__name__}.") + + @staticmethod + def _participant_task_state_key( + bound: BoundSemanticCall, + *, + slot_id: str, + path: tuple[PathPart, ...], + ) -> str: + """Resolve the exact held-object key shared by participant endpoints.""" + resource = bound.binding.resources.get(slot_id) + if resource is None: + raise _diagnostic( + "missing_effect_resource", + path, + f"Held-object effects require bound resource slot {slot_id!r}.", + tuple(bound.binding.resources), + ) + motion_endpoint = resource.endpoints.get("motion") + grasp_endpoint = resource.endpoints.get("grasp") + if motion_endpoint is None or grasp_endpoint is None: + raise _diagnostic( + "missing_effect_endpoint", + (*path, "endpoints"), + "Held-object effects require bound motion and grasp endpoints.", + tuple(resource.endpoints), + ) + task_state_key = motion_endpoint.task_state_key + assert isinstance(task_state_key, str) + if grasp_endpoint.task_state_key != task_state_key: + raise _diagnostic( + "effect_state_key_mismatch", + (*path, "task_state_key"), + "Motion and grasp endpoints for one participant must share one " + "logical task-state key.", + ) + return task_state_key + def ground( self, workflow: SemanticWorkflow, @@ -781,6 +1035,12 @@ def ground( lowering = self._lower_place(analyzed, context, eligible, path=path) elif type(call) is HandOver: lowering = self._lower_handover(analyzed, context, eligible, path=path) + elif type(call) is OperateArticulation: + lowering = self._lower_operate_articulation( + analyzed, + context, + path=path, + ) elif type(call) is RegisteredSemanticCall: lowering = self._lower_registered(analyzed, context, path=path) else: # pragma: no cover - exact workflow construction prevents this @@ -798,10 +1058,31 @@ def ground( invocation_id=f"{workflow.workflow_id}:{call_index}", revision=revision, ) - return GroundedSemanticCall( + effect_spec = self._ground_effect_spec( + analyzed, + invocation, + context, + path=(*path, call_index, "effect"), + ) + effect_monitor: EffectMonitor | None = None + if effect_spec is not None and analyzed.effect_monitor_ref is not None: + try: + effect_monitor = self._effect_monitor_registry.create( + effect_spec, + analyzed.effect_monitor_ref, + ) + except (KeyError, TypeError, ValueError) as exc: + raise _diagnostic( + "effect_monitor_creation_failed", + (*path, call_index, "effect_monitor"), + f"Could not create the grounded effect monitor: {exc}", + ) from exc + return GroundedSemanticCall._create( analyzed=analyzed, invocation=invocation, - _eligible_mask=eligible, + effect_spec=effect_spec, + effect_monitor=effect_monitor, + eligible_mask=eligible, ) def _assert_current(self, *, path: tuple[PathPart, ...]) -> None: @@ -862,15 +1143,89 @@ def _normalize_eligible_mask( raise ValueError("eligible_mask must use the context device.") return eligible_mask.clone() - def _downstream_target( + def _validate_context(self, context: PlanningContext) -> None: + """Require the grounding observation to match the bound engine batch.""" + engine = self._integration.engine + if context.robot.robot_dof != engine.robot.dof: + raise ValueError( + "PlanningContext robot_dof must match the compiler engine, " + f"got {context.robot.robot_dof} and {engine.robot.dof}." + ) + engine_qpos = engine.robot.get_qpos() + if context.batch_size != int(engine_qpos.shape[0]): + raise ValueError( + "PlanningContext batch size must match the compiler engine, " + f"got {context.batch_size} and {engine_qpos.shape[0]}." + ) + if context.robot.qpos.device != engine.device: + raise ValueError("PlanningContext and compiler engine must share a device.") + + def _effect_monitor_ref( + self, + bound: BoundSemanticCall, + effect_kind: SemanticEffectKind, + *, + path: tuple[PathPart, ...], + ) -> EffectMonitorRef | None: + """Resolve one preset-owned exact monitor reference without creating it.""" + semantic_id = bound.linked.call.semantic_id + monitor_ref = bound.preset.effect_monitors.get(semantic_id) + if monitor_ref is None: + if type(bound.linked.call) in ( + Pick, + Place, + HandOver, + OperateArticulation, + ): + raise _diagnostic( + "missing_effect_monitor", + path, + f"Semantic call {semantic_id!r} requires an effect monitor " + f"for its {effect_kind.value!r} postcondition.", + tuple(bound.preset.effect_monitors), + ) + return None + if type(bound.linked.call) is RegisteredSemanticCall: + raise _diagnostic( + "registered_effect_contract_not_installed", + path, + f"Registered semantic call {semantic_id!r} selects an effect " + "monitor but no declarative effect-contract grounder is " + "installed.", + ) + try: + self._effect_monitor_registry.validate_ref(monitor_ref) + except KeyError as exc: + available = tuple( + f"{monitor_id}@{revision}" + for monitor_id, revision in self._effect_monitor_registry.factories + ) + raise _diagnostic( + "effect_monitor_not_installed", + path, + f"Effect monitor {monitor_ref.monitor_id!r} revision " + f"{monitor_ref.revision!r} is not installed.", + available, + ) from exc + except (TypeError, ValueError) as exc: + raise _diagnostic( + "invalid_effect_monitor_config", + path, + f"Effect monitor {monitor_ref.monitor_id!r} revision " + f"{monitor_ref.revision!r} has invalid configuration: {exc}", + ) from exc + return monitor_ref.snapshot() + + def _downstream_targets( self, pick_index: int, bound_calls: list[BoundSemanticCall], - ) -> SemanticObjectTarget | None: - """Return the first target at which the picked object is released.""" + ) -> tuple[SemanticObjectTarget, ...]: + """Propagate object targets until the picked object is released.""" pick = bound_calls[pick_index].linked.call assert type(pick) is Pick object_id = pick.object.entity_id + targets: list[SemanticObjectTarget] = [] for call_index, bound in enumerate( bound_calls[pick_index + 1 :], start=pick_index + 1, @@ -890,17 +1245,22 @@ def _downstream_target( call, path=("workflow", call_index, "call"), ) - return SemanticObjectTarget( - SemanticHandOverTarget( - provider_id=provider_id, - bound=bound, + targets.append( + SemanticObjectTarget( + SemanticHandOverTarget( + provider_id=provider_id, + bound=bound, + ) ) ) + break if type(call) is Place: if call.at is not None: - return SemanticObjectTarget(call.at) - return self._relation_target(bound) - return None + targets.append(SemanticObjectTarget(call.at)) + else: + targets.append(self._relation_target(bound)) + break + return tuple(targets) def _lower_pick( self, @@ -920,16 +1280,10 @@ def _lower_pick( return SemanticLowering( goal=GraspGoal(semantics=semantics), skill_options=PickUpOptions( - downstream_object_target_poses=( - () - if analyzed.downstream_object_target is None - else ( - self._ground_object_target( - analyzed.downstream_object_target, - context, - ), - ) - ), + downstream_object_target_poses=tuple( + self._ground_object_target(target, context) + for target in analyzed.downstream_object_targets + ) ), ) @@ -944,14 +1298,14 @@ def _lower_place( """Convert an object-space place target using verified held state.""" call = analyzed.call assert type(call) is Place - control_part, held = self._require_held_object( + task_state_key, held = self._require_held_object( analyzed, context, eligible, slot_id="primary", path=(*path, analyzed.index, "call"), ) - del control_part + del task_state_key if call.at is not None: object_target = self._broadcast_pose( call.at.to_matrix(), @@ -1019,6 +1373,136 @@ def _lower_handover( ), ) + def _lower_operate_articulation( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> SemanticLowering: + """Ground one handle operation from the latest scene snapshot.""" + call = analyzed.call + assert type(call) is OperateArticulation + handle_ref = analyzed.bound.linked.affordances.get("handle") + if handle_ref is None: + raise AssertionError( + "Linked articulation call lacks an operation affordance." + ) + registration = self._integration.scene_registry.lookup( + handle_ref, + expected_type=SceneAffordanceRef, + ) + affordance = registration.affordance + if ( + type(affordance) is not ArticulationOperationAffordance + or ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + not in registration.affordance_capabilities + ): + raise _diagnostic( + "invalid_articulation_affordance", + (*path, analyzed.index, "call", "handle"), + f"Handle {handle_ref.entity_id!r} must expose an exact " + "ArticulationOperationAffordance payload and the articulation " + "operation capability.", + ) + + if call.target is not None: + try: + resolved_target = affordance.resolve_target(call.target) + except KeyError as exc: + raise _diagnostic( + "unknown_articulation_target", + (*path, analyzed.index, "call", "target"), + f"Handle {handle_ref.entity_id!r} has no semantic target " + f"{call.target!r}.", + tuple(affordance.semantic_targets), + ) from exc + target_position = resolved_target.target_position + displacement = resolved_target.displacement + else: + assert call.target_position is not None + assert call.target_displacement is not None + target_position = call.target_position + displacement = call.target_displacement + + try: + handle_state = context.scene.entities[handle_ref.entity_id] + except KeyError as exc: + raise _diagnostic( + "missing_handle_observation", + (*path, analyzed.index, "call", "handle"), + f"The current planning snapshot has no pose for handle " + f"{handle_ref.entity_id!r}.", + ) from exc + try: + self._broadcast_pose( + handle_state.pose, + context, + name=f"handle {handle_ref.entity_id!r}", + ) + except (TypeError, ValueError) as exc: + raise _diagnostic( + "articulation_grounding_failed", + (*path, analyzed.index, "call", "handle"), + f"Could not ground articulation handle geometry: {exc}", + ) from exc + joint_address = call.articulation.entity_id, affordance.joint_id + observed_joint = context.scene.get_articulation_joint_state(*joint_address) + if observed_joint is None: + raise _diagnostic( + "missing_articulation_joint_observation", + (*path, analyzed.index, "call", "articulation"), + "Recovery-safe articulation grounding requires a live " + "ObservedArticulationJointState for " + f"{joint_address!r} in the current scene snapshot.", + ) + try: + source_position = self._broadcast_joint_position( + observed_joint.position, + context, + name=f"articulation joint {joint_address!r}", + ) + except (TypeError, ValueError) as exc: + raise _diagnostic( + "invalid_articulation_joint_observation", + (*path, analyzed.index, "call", "articulation"), + f"Could not use live articulation joint state: {exc}", + ) from exc + if observed_joint.valid_mask is not None: + valid = observed_joint.valid_mask.to(device=context.robot.qpos.device) + if bool((~valid).any()): + rows = (~valid).nonzero(as_tuple=False).flatten().tolist() + raise _diagnostic( + "invalid_articulation_joint_observation", + (*path, analyzed.index, "call", "articulation"), + "Live articulation joint state is unavailable for planning " + f"rows {rows}.", + ) + target = torch.full( + (context.batch_size, 1), + target_position, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + return SemanticLowering( + goal=OperateArticulationGoal( + articulation_id=call.articulation.entity_id, + joint_id=affordance.joint_id, + geometry=SceneArticulationOperationGeometry( + handle_pose=SceneEntityPose(handle_ref.entity_id), + approach_offset=affordance.approach_offset, + contact_offset=affordance.contact_offset, + operation_offset=affordance.operation_offset, + retract_offset=affordance.retract_offset, + operation_axis=affordance.operation_axis, + position_scale=affordance.position_scale, + ), + source_position=source_position, + target_position=target, + target_displacement=displacement, + ) + ) + def _lower_registered( self, analyzed: AnalyzedSemanticCall, @@ -1068,6 +1552,244 @@ def _lower_registered( ) return lowering + def _ground_effect_spec( + self, + analyzed: AnalyzedSemanticCall, + invocation: ActionInvocation, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> SemanticEffectSpec | None: + """Ground typed symbolic state and raw-evidence clauses.""" + if analyzed.effect_monitor_ref is None: + return None + call = analyzed.call + state_expectations: list[EffectStateExpectation] = [] + clauses: list[EffectClause] = [] + if type(call) is Pick: + expectation, grounded_clauses = self._ground_held_effect( + analyzed, + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + slot_id="primary", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "destination"), + ) + state_expectations.append(expectation) + clauses.extend(grounded_clauses) + state_expectations.extend( + self._coordinated_cleanup_expectations( + context, + task_state_keys=(expectation.task_state_key,), + ) + ) + elif type(call) is Place: + expectation, grounded_clauses = self._ground_held_effect( + analyzed, + expectation_id="source", + relation=HeldObjectRelation.DETACHED, + slot_id="primary", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "source"), + ) + state_expectations.append(expectation) + clauses.extend(grounded_clauses) + state_expectations.extend( + self._coordinated_cleanup_expectations( + context, + task_state_keys=(expectation.task_state_key,), + ) + ) + elif type(call) is HandOver: + source, source_clauses = self._ground_held_effect( + analyzed, + expectation_id="source", + relation=HeldObjectRelation.DETACHED, + slot_id="source", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "source"), + ) + destination, destination_clauses = self._ground_held_effect( + analyzed, + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + slot_id="destination", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "destination"), + ) + state_expectations.extend((source, destination)) + clauses.extend((*source_clauses, *destination_clauses)) + elif type(call) is OperateArticulation: + goal = invocation.goal + if type(goal) is not OperateArticulationGoal: + raise AssertionError( + "OperateArticulation lowering produced an incompatible goal." + ) + expectation = ArticulationJointStateExpectation( + expectation_id="joint", + articulation_id=goal.articulation_id, + joint_id=goal.joint_id, + target_position=goal.target_position, + ) + source = EffectEvidenceSourceRef( + provider_id=SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + revision=SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + address=ArticulationJointEvidenceAddress( + articulation_id=goal.articulation_id, + joint_id=goal.joint_id, + ), + ) + state_expectations.append(expectation) + clauses.append( + JointStateEffectClause( + clause_id="joint.position", + expectation_id=expectation.expectation_id, + source=source, + target_position=goal.target_position, + ) + ) + else: # pragma: no cover - exact workflow construction prevents this + raise AssertionError(f"Unsupported analyzed call {type(call).__name__}.") + return SemanticEffectSpec( + semantic_id=call.semantic_id, + effect_kind=analyzed.effect_kind, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + env_ids=context.env_ids, + state_expectations=tuple(state_expectations), + clauses=tuple(clauses), + ) + + @staticmethod + def _coordinated_cleanup_expectations( + context: PlanningContext, + *, + task_state_keys: tuple[str, ...], + ) -> tuple[CoordinatedHeldObjectCleanupExpectation, ...]: + """Declare the exact coordinated relations a primitive must remove.""" + related = set(task_state_keys) + return tuple( + CoordinatedHeldObjectCleanupExpectation( + expectation_id=f"cleanup:{resources[0]}:{resources[1]}", + task_state_keys=resources, + ) + for resources in context.task.coordinated_held_objects + if not set(resources).isdisjoint(related) + ) + + @staticmethod + def _effect_source( + sources: Mapping[str, EffectEvidenceSourceRef], + channel: str, + *, + path: tuple[PathPart, ...], + ) -> EffectEvidenceSourceRef: + """Resolve one exact endpoint-owned observation source.""" + source = sources.get(channel) + if source is None: + raise _diagnostic( + "missing_effect_source", + (*path, "effect_sources", channel), + f"The endpoint does not expose required effect channel {channel!r}.", + tuple(sources), + ) + return source.snapshot() + + def _ground_held_effect( + self, + analyzed: AnalyzedSemanticCall, + *, + expectation_id: str, + relation: HeldObjectRelation, + slot_id: str, + object_id: str, + context: PlanningContext, + path: tuple[PathPart, ...], + ) -> tuple[HeldObjectStateExpectation, tuple[EffectClause, ...]]: + """Bind one held-object state relation to generic endpoint sources.""" + resource = analyzed.bound.binding.resources[slot_id] + motion_endpoint = resource.endpoints.get("motion") + grasp_endpoint = resource.endpoints.get("grasp") + if motion_endpoint is None or grasp_endpoint is None: + raise _diagnostic( + "missing_effect_endpoint", + (*path, "endpoints"), + "Held-object effects require bound motion and grasp endpoints.", + tuple(resource.endpoints), + ) + task_state_key = motion_endpoint.task_state_key + assert isinstance(task_state_key, str) + if grasp_endpoint.task_state_key != task_state_key: + raise _diagnostic( + "effect_state_key_mismatch", + (*path, "task_state_key"), + "Motion and grasp endpoints for one participant must share one " + "logical task-state key.", + ) + baseline: torch.Tensor | None = None + if relation is HeldObjectRelation.DETACHED: + held = context.task.get_held_object(task_state_key) + if held is None or held.semantics.entity_id != object_id: + raise _diagnostic( + "verified_held_object_required", + (*path, "baseline"), + f"Detached relation requires verified object {object_id!r} " + f"held under logical state key {task_state_key!r}.", + ) + baseline = held.object_to_eef + state_expectation = HeldObjectStateExpectation( + expectation_id=expectation_id, + relation=relation, + object_id=object_id, + slot_id=slot_id, + resource_id=resource.resource_id, + task_state_key=task_state_key, + ) + pose_source = self._effect_source( + motion_endpoint.effect_sources, + POSE_RELATION_EFFECT_CHANNEL, + path=(*path, "motion"), + ) + binary_channel = ( + CONSTRAINT_EFFECT_CHANNEL + if CONSTRAINT_EFFECT_CHANNEL in grasp_endpoint.effect_sources + else CONTACT_EFFECT_CHANNEL + ) + binary_source = self._effect_source( + grasp_endpoint.effect_sources, + binary_channel, + path=(*path, "grasp"), + ) + pose_clause = PoseRelationClause( + clause_id=f"{expectation_id}.pose", + expectation_id=expectation_id, + source=pose_source, + expectation=( + PoseRelationExpectation.MATCHED + if relation is HeldObjectRelation.ATTACHED + else PoseRelationExpectation.SEPARATED + ), + baseline_object_to_endpoint=baseline, + ) + binary_kind = ( + BinaryEvidenceKind.CONSTRAINT + if binary_channel == CONSTRAINT_EFFECT_CHANNEL + else BinaryEvidenceKind.CONTACT + ) + binary_clause = BinaryEffectClause( + clause_id=f"{expectation_id}.{binary_kind.value}", + expectation_id=expectation_id, + source=binary_source, + evidence_kind=binary_kind, + expected=relation is HeldObjectRelation.ATTACHED, + ) + return state_expectation, (pose_clause, binary_clause) + def _relation_target( self, bound: BoundSemanticCall, @@ -1283,7 +2005,15 @@ def _require_held_object( path: tuple[PathPart, ...], ) -> tuple[str, HeldObjectState]: """Resolve the logical participant key and verify held-object identity.""" - endpoint = analyzed.bound.binding.action_binding.endpoint(slot_id, "motion") + resource = analyzed.bound.binding.resources[slot_id] + endpoint = resource.endpoints.get("motion") + if endpoint is None: + raise _diagnostic( + "missing_effect_endpoint", + (*path, "resources", slot_id, "motion"), + "The semantic lowerer requires a bound motion endpoint.", + tuple(resource.endpoints), + ) task_state_key = endpoint.task_state_key assert isinstance(task_state_key, str) held = context.task.get_held_object(task_state_key) @@ -1329,6 +2059,31 @@ def _broadcast_pose( ) return pose.clone() + @staticmethod + def _broadcast_joint_position( + position: torch.Tensor, + context: PlanningContext, + *, + name: str, + ) -> torch.Tensor: + """Move and broadcast one scalar articulation joint observation.""" + if not isinstance(position, torch.Tensor): + raise TypeError(f"{name} position must be a torch.Tensor.") + if not position.is_floating_point() or not torch.isfinite(position).all(): + raise ValueError(f"{name} position must be a finite floating tensor.") + position = position.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if position.shape == (1,): + return position.unsqueeze(0).expand(context.batch_size, -1).clone() + if position.shape != (context.batch_size, 1): + raise ValueError( + f"{name} position must have shape (1,) or " + f"({context.batch_size}, 1)." + ) + return position.clone() + __all__ = [ "AnalyzedSemanticCall", diff --git a/embodichain/lab/sim/skills/effects.py b/embodichain/lab/sim/skills/effects.py new file mode 100644 index 000000000..852cb99a0 --- /dev/null +++ b/embodichain/lab/sim/skills/effects.py @@ -0,0 +1,2250 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Backend-neutral semantic-effect contracts, evidence, and monitors.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from collections.abc import Hashable, Iterable, Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass +from enum import Enum +import math +from types import MappingProxyType +from typing import ClassVar, TypeAlias + +import torch + +from embodichain.lab.sim.atomic_actions.execution import EffectVerificationRequest +from embodichain.lab.sim.atomic_actions.state import ( + ArticulationJointState, + HeldObjectState, +) + +EffectMonitorParam: TypeAlias = ( + None + | bool + | int + | float + | str + | tuple["EffectMonitorParam", ...] + | Mapping[str, "EffectMonitorParam"] +) +"""Recursively immutable, non-executable monitor configuration value.""" + +COMPOSITE_EFFECT_MONITOR_ID = "builtin.composite_effect" +"""Stable ID of the built-in typed-clause monitor.""" + +COMPOSITE_EFFECT_MONITOR_REVISION = "1" +"""Exact behavior/configuration revision of the built-in monitor.""" + +CONTROL_PART_EVIDENCE_PROVIDER_ID = "builtin.control_part" +"""Stable provider ID used by generic control-part evidence addresses.""" + +CONTROL_PART_EVIDENCE_PROVIDER_REVISION = "1" +"""Exact contract revision of control-part evidence addresses.""" + +POSE_RELATION_EFFECT_CHANNEL = "pose_relation" +CONTACT_EFFECT_CHANNEL = "contact" +CONSTRAINT_EFFECT_CHANNEL = "constraint" +FORCE_EFFECT_CHANNEL = "force" +JOINT_STATE_EFFECT_CHANNEL = "joint_state" + +_EFFECT_CHANNELS = frozenset( + { + POSE_RELATION_EFFECT_CHANNEL, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } +) +_SE3_BASE_ATOL = 1.0e-5 +_SE3_EPS_MULTIPLIER = 10.0 + + +def _metadata_value(value: object) -> object: + """Convert one typed effect value to deterministic JSON-safe data.""" + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + return value if math.isfinite(value) else None + if isinstance(value, Enum): + return value.value + if isinstance(value, torch.Tensor): + return _metadata_value(value.detach().cpu().tolist()) + if isinstance(value, Mapping): + return { + str(key): _metadata_value(nested) + for key, nested in sorted(value.items(), key=lambda item: str(item[0])) + } + if isinstance(value, (tuple, list)): + return [_metadata_value(nested) for nested in value] + if is_dataclass(value) and not isinstance(value, type): + return { + "type": f"{type(value).__module__}.{type(value).__qualname__}", + **{ + data_field.name: _metadata_value(getattr(value, data_field.name)) + for data_field in fields(value) + }, + } + return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _snapshot_declarative_value( + value: object, + *, + path: str, + active: set[int] | None = None, + budget: list[int] | None = None, + depth: int = 0, +) -> EffectMonitorParam: + """Own one bounded, acyclic, non-executable declarative value.""" + if active is None: + active = set() + if budget is None: + budget = [4096] + if depth > 32: + raise ValueError(f"{path} exceeds the maximum declarative depth of 32.") + budget[0] -= 1 + if budget[0] < 0: + raise ValueError(f"{path} exceeds the maximum declarative node count.") + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{path} must be finite.") + return value + if type(value) in (dict, MappingProxyType): + container_id = id(value) + if container_id in active: + raise ValueError(f"{path} contains a cyclic mapping.") + active.add(container_id) + try: + snapshot: dict[str, EffectMonitorParam] = {} + for key, nested in value.items(): + _validate_identifier(key, field_name=f"{path} keys") + snapshot[key] = _snapshot_declarative_value( + nested, + path=f"{path}.{key}", + active=active, + budget=budget, + depth=depth + 1, + ) + return MappingProxyType(snapshot) + finally: + active.remove(container_id) + if type(value) in (tuple, list): + container_id = id(value) + if container_id in active: + raise ValueError(f"{path} contains a cyclic sequence.") + active.add(container_id) + try: + return tuple( + _snapshot_declarative_value( + nested, + path=f"{path}[{index}]", + active=active, + budget=budget, + depth=depth + 1, + ) + for index, nested in enumerate(value) + ) + finally: + active.remove(container_id) + raise TypeError( + f"{path} contains non-declarative {type(value).__name__}; callables, " + "classes, tensors, and live objects are not allowed." + ) + + +def _snapshot_monitor_params( + values: Mapping[str, EffectMonitorParam], +) -> Mapping[str, EffectMonitorParam]: + """Validate and own a monitor-parameter mapping.""" + if type(values) not in (dict, MappingProxyType): + raise TypeError( + "EffectMonitorRef.params must be an exact dict or mapping proxy." + ) + snapshot = _snapshot_declarative_value(values, path="EffectMonitorRef.params") + assert isinstance(snapshot, Mapping) + return snapshot + + +def _validate_pose_batch( + value: torch.Tensor, + *, + field_name: str, + valid_mask: torch.Tensor | None = None, +) -> torch.Tensor: + """Validate and own unbatched or batched proper SE(3) transforms.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.shape != (4, 4) and ( + value.dim() != 3 or value.shape[0] == 0 or value.shape[-2:] != (4, 4) + ): + raise ValueError(f"{field_name} must have shape (4, 4) or (B, 4, 4).") + if not value.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + poses = value.unsqueeze(0) if value.dim() == 2 else value + if valid_mask is not None: + if not isinstance(valid_mask, torch.Tensor): + raise TypeError("valid_mask must be a torch.Tensor.") + if valid_mask.dtype != torch.bool or valid_mask.shape != (poses.shape[0],): + raise ValueError("valid_mask must be a bool tensor with shape (B,).") + if valid_mask.device != poses.device: + raise ValueError("valid_mask and poses must share a device.") + poses = poses[valid_mask] + if poses.numel() == 0: + return value.clone() + if not torch.isfinite(poses).all(): + raise ValueError(f"{field_name} must contain only finite values.") + tolerance = max( + _SE3_BASE_ATOL, + _SE3_EPS_MULTIPLIER * float(torch.finfo(value.dtype).eps), + ) + checked = poses.to(dtype=torch.float64) + expected_bottom = checked.new_tensor((0.0, 0.0, 0.0, 1.0)) + if not torch.isclose( + checked[:, 3, :], + expected_bottom.expand(checked.shape[0], -1), + atol=tolerance, + rtol=0.0, + ).all(): + raise ValueError( + f"{field_name} must contain SE(3) transforms with homogeneous " + "bottom row [0, 0, 0, 1]." + ) + rotations = checked[:, :3, :3] + gram = rotations.transpose(-1, -2) @ rotations + identity = torch.eye(3, dtype=checked.dtype, device=checked.device).expand_as(gram) + if not torch.isclose(gram, identity, atol=tolerance, rtol=0.0).all(): + raise ValueError( + f"{field_name} must contain SE(3) transforms with orthonormal rotations." + ) + determinants = torch.linalg.det(rotations) + if not torch.isclose( + determinants, + torch.ones_like(determinants), + atol=tolerance, + rtol=0.0, + ).all(): + raise ValueError( + f"{field_name} must contain SE(3) transforms with rotation " + "determinant +1." + ) + return value.clone() + + +class SemanticEffectKind(str, Enum): + """Trace-level semantic effect category; clause types define behavior.""" + + ATTACH = "attach" + RELEASE = "release" + TRANSFER = "transfer" + ARTICULATION = "articulation" + REGISTERED = "registered" + + +class SymbolicStateDomain(str, Enum): + """Typed mapping domains owned by :class:`~atomic_actions.TaskState`.""" + + HELD_OBJECT = "held_object" + COORDINATED_HELD_OBJECT = "coordinated_held_object" + ARTICULATION_JOINT = "articulation_joint" + + +@dataclass(frozen=True, slots=True) +class SymbolicStateKey: + """Provider-free key for one exact symbolic ``TaskState`` write. + + The domain makes otherwise similar string and pair addresses impossible to + conflate during static parallel analysis. This contract intentionally + describes only exact keys; dynamic or opaque effects must not manufacture + a guessed key. + """ + + domain: SymbolicStateDomain + address: tuple[str, ...] + + def __post_init__(self) -> None: + if not isinstance(self.domain, SymbolicStateDomain): + raise TypeError("domain must be a SymbolicStateDomain.") + address = tuple(self.address) + expected_size = 1 if self.domain is SymbolicStateDomain.HELD_OBJECT else 2 + if len(address) != expected_size: + raise ValueError( + f"{self.domain.value} symbolic keys require exactly " + f"{expected_size} address component(s)." + ) + for component in address: + _validate_identifier( + component, + field_name=f"{self.domain.value} symbolic key components", + ) + object.__setattr__(self, "address", address) + + @classmethod + def held_object(cls, task_state_key: str) -> SymbolicStateKey: + """Build one held-object mapping key.""" + return cls(SymbolicStateDomain.HELD_OBJECT, (task_state_key,)) + + @classmethod + def coordinated_held_object( + cls, + first_task_state_key: str, + second_task_state_key: str, + ) -> SymbolicStateKey: + """Build one ordered coordinated-held-object mapping key.""" + return cls( + SymbolicStateDomain.COORDINATED_HELD_OBJECT, + (first_task_state_key, second_task_state_key), + ) + + @classmethod + def articulation_joint( + cls, + articulation_id: str, + joint_id: str, + ) -> SymbolicStateKey: + """Build one articulation-joint mapping key.""" + return cls( + SymbolicStateDomain.ARTICULATION_JOINT, + (articulation_id, joint_id), + ) + + @property + def rendered(self) -> str: + """Return a deterministic domain-qualified diagnostic form.""" + return f"{self.domain.value}[{', '.join(repr(item) for item in self.address)}]" + + +@dataclass(frozen=True, slots=True) +class EffectMonitorRef: + """Versioned, declarative reference to an effect-monitor factory.""" + + monitor_id: str + revision: str + params: Mapping[str, EffectMonitorParam] = field(default_factory=dict) + + def __post_init__(self) -> None: + _validate_identifier(self.monitor_id, field_name="EffectMonitorRef.monitor_id") + _validate_identifier(self.revision, field_name="EffectMonitorRef.revision") + object.__setattr__(self, "params", _snapshot_monitor_params(self.params)) + + def snapshot(self) -> EffectMonitorRef: + """Return an independently owned declarative reference.""" + return EffectMonitorRef(self.monitor_id, self.revision, self.params) + + def to_metadata(self) -> dict[str, object]: + """Return a deterministic JSON-safe monitor selection.""" + return { + "monitor_id": self.monitor_id, + "revision": self.revision, + "params": _metadata_value(self.params), + } + + +class EffectEvidenceAddress(ABC): + """Immutable observation address, deliberately separate from command targets.""" + + @property + @abstractmethod + def address_fingerprint(self) -> Hashable: + """Return a stable, hashable physical observation address.""" + + def snapshot(self) -> EffectEvidenceAddress: + """Return an independently owned address of the exact same type.""" + return deepcopy(self) + + +@dataclass(frozen=True, slots=True) +class ControlPartEvidenceAddress(EffectEvidenceAddress): + """Provider-neutral robot control-part observation address.""" + + control_part: str + channel: str + + def __post_init__(self) -> None: + _validate_identifier( + self.control_part, + field_name="ControlPartEvidenceAddress.control_part", + ) + _validate_identifier( + self.channel, field_name="ControlPartEvidenceAddress.channel" + ) + if self.channel not in _EFFECT_CHANNELS: + raise ValueError( + f"Unknown control-part effect channel {self.channel!r}; expected " + f"one of {sorted(_EFFECT_CHANNELS)}." + ) + + @property + def address_fingerprint(self) -> Hashable: + """Return the channel-scoped control-part observation address.""" + return type(self), self.control_part, self.channel + + +@dataclass(frozen=True, slots=True) +class EffectEvidenceSourceRef: + """Versioned provider route plus one immutable observation address.""" + + provider_id: str + revision: str + address: EffectEvidenceAddress + + def __post_init__(self) -> None: + _validate_identifier( + self.provider_id, + field_name="EffectEvidenceSourceRef.provider_id", + ) + _validate_identifier( + self.revision, + field_name="EffectEvidenceSourceRef.revision", + ) + if not isinstance(self.address, EffectEvidenceAddress): + raise TypeError( + "EffectEvidenceSourceRef.address must be an EffectEvidenceAddress." + ) + snapshot = self.address.snapshot() + if type(snapshot) is not type(self.address) or snapshot is self.address: + raise TypeError( + "EffectEvidenceAddress.snapshot() must return an independently " + "owned address of the same exact type." + ) + try: + source_fingerprint = self.address.address_fingerprint + snapshot_fingerprint = snapshot.address_fingerprint + hash(source_fingerprint) + hash(snapshot_fingerprint) + except TypeError as exc: + raise TypeError( + "EffectEvidenceAddress.address_fingerprint must be hashable." + ) from exc + if snapshot_fingerprint != source_fingerprint: + raise ValueError( + "EffectEvidenceAddress.snapshot() must preserve its fingerprint." + ) + object.__setattr__(self, "address", snapshot) + + @property + def source_fingerprint(self) -> Hashable: + """Return the provider-scoped source address fingerprint.""" + return ( + self.provider_id, + self.revision, + type(self.address), + self.address.address_fingerprint, + ) + + def snapshot(self) -> EffectEvidenceSourceRef: + """Return an independently owned source reference.""" + return EffectEvidenceSourceRef( + self.provider_id, + self.revision, + self.address, + ) + + def to_metadata(self) -> dict[str, object]: + """Return the versioned physical observation address as JSON-safe data.""" + return { + "provider_id": self.provider_id, + "revision": self.revision, + "address": _metadata_value(self.address), + } + + +class HeldObjectRelation(str, Enum): + """Expected symbolic held-object state at an effect boundary.""" + + ATTACHED = "attached" + DETACHED = "detached" + + +@dataclass(frozen=True, slots=True) +class HeldObjectStateExpectation: + """Typed individual held-object postcondition.""" + + expectation_id: str + relation: HeldObjectRelation + object_id: str + slot_id: str + resource_id: str + task_state_key: str + + def __post_init__(self) -> None: + for field_name in ( + "expectation_id", + "object_id", + "slot_id", + "resource_id", + "task_state_key", + ): + _validate_identifier( + getattr(self, field_name), + field_name=f"HeldObjectStateExpectation.{field_name}", + ) + if not isinstance(self.relation, HeldObjectRelation): + raise TypeError("relation must be a HeldObjectRelation.") + + def snapshot(self) -> HeldObjectStateExpectation: + """Return an independently constructed state expectation.""" + return HeldObjectStateExpectation( + self.expectation_id, + self.relation, + self.object_id, + self.slot_id, + self.resource_id, + self.task_state_key, + ) + + +@dataclass(frozen=True, slots=True) +class CoordinatedHeldObjectCleanupExpectation: + """Typed removal of one coordinated held-object relation.""" + + expectation_id: str + task_state_keys: tuple[str, str] + + def __post_init__(self) -> None: + _validate_identifier( + self.expectation_id, + field_name="CoordinatedHeldObjectCleanupExpectation.expectation_id", + ) + keys = tuple(self.task_state_keys) + if len(keys) != 2: + raise ValueError("task_state_keys must contain exactly two keys.") + for key in keys: + _validate_identifier(key, field_name="coordinated task-state keys") + if keys[0] == keys[1]: + raise ValueError("Coordinated task-state keys must be distinct.") + object.__setattr__(self, "task_state_keys", keys) + + def snapshot(self) -> CoordinatedHeldObjectCleanupExpectation: + """Return an independently constructed cleanup expectation.""" + return CoordinatedHeldObjectCleanupExpectation( + self.expectation_id, + self.task_state_keys, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class ArticulationJointStateExpectation: + """Future-compatible symbolic articulation-joint postcondition.""" + + expectation_id: str + articulation_id: str + joint_id: str + target_position: torch.Tensor + + def __post_init__(self) -> None: + for field_name in ("expectation_id", "articulation_id", "joint_id"): + _validate_identifier( + getattr(self, field_name), + field_name=f"ArticulationJointStateExpectation.{field_name}", + ) + target = self.target_position + if not isinstance(target, torch.Tensor) or target.dim() not in (1, 2): + raise ValueError("target_position must have shape (J,) or (B, J).") + if target.numel() == 0 or not target.is_floating_point(): + raise TypeError("target_position must be a non-empty floating tensor.") + if not torch.isfinite(target).all(): + raise ValueError("target_position must be finite.") + object.__setattr__(self, "target_position", target.clone()) + + def snapshot(self) -> ArticulationJointStateExpectation: + """Return an independently owned articulation expectation.""" + return ArticulationJointStateExpectation( + self.expectation_id, + self.articulation_id, + self.joint_id, + self.target_position, + ) + + +EffectStateExpectation: TypeAlias = ( + HeldObjectStateExpectation + | CoordinatedHeldObjectCleanupExpectation + | ArticulationJointStateExpectation +) + + +class PoseRelationExpectation(str, Enum): + """Expected relationship to a grounded pose baseline.""" + + MATCHED = "matched" + SEPARATED = "separated" + + +class BinaryEvidenceKind(str, Enum): + """Raw boolean evidence channel.""" + + CONTACT = "contact" + CONSTRAINT = "constraint" + + +class ScalarEvidenceKind(str, Enum): + """Raw scalar physical evidence channel.""" + + FORCE = "force" + WRENCH = "wrench" + + +class ScalarExpectation(str, Enum): + """Expected high/low magnitude band for scalar evidence.""" + + PRESENT = "present" + ABSENT = "absent" + + +def _validate_clause_identity( + clause_id: str, + expectation_id: str, + source: EffectEvidenceSourceRef, +) -> EffectEvidenceSourceRef: + """Validate common clause identity and own its source.""" + _validate_identifier(clause_id, field_name="effect clause_id") + _validate_identifier(expectation_id, field_name="effect expectation_id") + if not isinstance(source, EffectEvidenceSourceRef): + raise TypeError("effect clause source must be an EffectEvidenceSourceRef.") + return source.snapshot() + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseRelationClause: + """Object-to-endpoint pose condition with monitor-owned tolerances.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + expectation: PoseRelationExpectation + baseline_object_to_endpoint: torch.Tensor | None = None + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + if not isinstance(self.expectation, PoseRelationExpectation): + raise TypeError("expectation must be a PoseRelationExpectation.") + baseline = self.baseline_object_to_endpoint + if self.expectation is PoseRelationExpectation.SEPARATED: + if baseline is None: + raise ValueError("A separated pose clause requires a baseline.") + object.__setattr__( + self, + "baseline_object_to_endpoint", + _validate_pose_batch( + baseline, + field_name="PoseRelationClause.baseline_object_to_endpoint", + ), + ) + elif baseline is not None: + raise ValueError( + "A matched pose clause obtains its baseline from the expected " + "held-object StateDelta and must not embed one." + ) + + def snapshot(self) -> PoseRelationClause: + """Return an independently owned pose clause.""" + return PoseRelationClause( + self.clause_id, + self.expectation_id, + self.source, + self.expectation, + self.baseline_object_to_endpoint, + ) + + +@dataclass(frozen=True, slots=True) +class BinaryEffectClause: + """Raw contact or constraint-state condition.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + evidence_kind: BinaryEvidenceKind + expected: bool + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + if not isinstance(self.evidence_kind, BinaryEvidenceKind): + raise TypeError("evidence_kind must be a BinaryEvidenceKind.") + if type(self.expected) is not bool: + raise TypeError("expected must be a bool.") + + def snapshot(self) -> BinaryEffectClause: + """Return an independently owned binary clause.""" + return BinaryEffectClause( + self.clause_id, + self.expectation_id, + self.source, + self.evidence_kind, + self.expected, + ) + + +@dataclass(frozen=True, slots=True) +class ScalarEffectClause: + """Raw force/wrench magnitude condition with monitor-owned thresholds.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + evidence_kind: ScalarEvidenceKind + expectation: ScalarExpectation + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + if not isinstance(self.evidence_kind, ScalarEvidenceKind): + raise TypeError("evidence_kind must be a ScalarEvidenceKind.") + if not isinstance(self.expectation, ScalarExpectation): + raise TypeError("expectation must be a ScalarExpectation.") + + def snapshot(self) -> ScalarEffectClause: + """Return an independently owned scalar clause.""" + return ScalarEffectClause( + self.clause_id, + self.expectation_id, + self.source, + self.evidence_kind, + self.expectation, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateEffectClause: + """Raw articulation/robot joint-position target condition.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + target_position: torch.Tensor + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + target = self.target_position + if not isinstance(target, torch.Tensor) or target.dim() not in (1, 2): + raise ValueError("target_position must have shape (J,) or (B, J).") + if target.numel() == 0 or not target.is_floating_point(): + raise TypeError("target_position must be a non-empty floating tensor.") + if not torch.isfinite(target).all(): + raise ValueError("target_position must be finite.") + object.__setattr__(self, "target_position", target.clone()) + + def snapshot(self) -> JointStateEffectClause: + """Return an independently owned joint-state clause.""" + return JointStateEffectClause( + self.clause_id, + self.expectation_id, + self.source, + self.target_position, + ) + + +EffectClause: TypeAlias = ( + PoseRelationClause + | BinaryEffectClause + | ScalarEffectClause + | JointStateEffectClause +) +_STATE_EXPECTATION_TYPES = ( + HeldObjectStateExpectation, + CoordinatedHeldObjectCleanupExpectation, + ArticulationJointStateExpectation, +) +_CLAUSE_TYPES = ( + PoseRelationClause, + BinaryEffectClause, + ScalarEffectClause, + JointStateEffectClause, +) + + +@dataclass(frozen=True, slots=True, eq=False) +class SemanticEffectSpec: + """Grounded typed physical clauses and symbolic postconditions for one call.""" + + semantic_id: str + effect_kind: SemanticEffectKind + skill_id: str + invocation_id: str | None + invocation_revision: int + env_ids: torch.Tensor + state_expectations: tuple[EffectStateExpectation, ...] + clauses: tuple[EffectClause, ...] + + def __post_init__(self) -> None: + _validate_identifier( + self.semantic_id, + field_name="SemanticEffectSpec.semantic_id", + ) + _validate_identifier(self.skill_id, field_name="SemanticEffectSpec.skill_id") + if not isinstance(self.effect_kind, SemanticEffectKind): + raise TypeError("effect_kind must be a SemanticEffectKind.") + if self.invocation_id is not None: + _validate_identifier( + self.invocation_id, + field_name="SemanticEffectSpec.invocation_id", + ) + if type(self.invocation_revision) is not int or self.invocation_revision < 0: + raise ValueError("invocation_revision must be a non-negative integer.") + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if self.env_ids.dtype != torch.long or self.env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if self.env_ids.numel() == 0: + raise ValueError("env_ids must contain at least one environment ID.") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + object.__setattr__(self, "env_ids", self.env_ids.clone()) + + expectations = tuple(self.state_expectations) + if not expectations or not all( + type(value) in _STATE_EXPECTATION_TYPES for value in expectations + ): + raise TypeError( + "state_expectations must contain exact typed state expectations." + ) + expectation_ids = [value.expectation_id for value in expectations] + if len(set(expectation_ids)) != len(expectation_ids): + raise ValueError("State expectation IDs must be unique.") + held_keys = [ + value.task_state_key + for value in expectations + if type(value) is HeldObjectStateExpectation + ] + if len(set(held_keys)) != len(held_keys): + raise ValueError("Held-object task-state keys must be unique.") + cleanup_keys = [ + value.task_state_keys + for value in expectations + if type(value) is CoordinatedHeldObjectCleanupExpectation + ] + if len(set(cleanup_keys)) != len(cleanup_keys): + raise ValueError("Coordinated cleanup keys must be unique.") + + clauses = tuple(self.clauses) + if not clauses or not all(type(value) in _CLAUSE_TYPES for value in clauses): + raise TypeError("clauses must contain exact typed effect clauses.") + clause_ids = [value.clause_id for value in clauses] + if len(set(clause_ids)) != len(clause_ids): + raise ValueError("Effect clause IDs must be unique.") + unknown_expectations = {value.expectation_id for value in clauses}.difference( + expectation_ids + ) + if unknown_expectations: + raise ValueError( + "Effect clauses reference unknown state expectations: " + f"{sorted(unknown_expectations)}." + ) + uncovered = set(expectation_ids).difference( + value.expectation_id for value in clauses + ) + uncovered.difference_update( + value.expectation_id + for value in expectations + if type(value) is CoordinatedHeldObjectCleanupExpectation + ) + if uncovered: + raise ValueError( + "Every physical state expectation needs at least one clause; " + f"missing {sorted(uncovered)}." + ) + for value in expectations: + if ( + type(value) is ArticulationJointStateExpectation + and value.target_position.dim() == 2 + and value.target_position.shape[0] != self.env_ids.numel() + ): + raise ValueError( + "Batched articulation targets must match env_ids length." + ) + for value in clauses: + if type(value) is PoseRelationClause: + baseline = value.baseline_object_to_endpoint + if baseline is not None and baseline.dim() == 3: + if baseline.shape[0] != self.env_ids.numel(): + raise ValueError( + "Batched pose baselines must match env_ids length." + ) + if baseline.device != self.env_ids.device: + raise ValueError( + "Batched pose baselines and env_ids must share a device." + ) + elif ( + type(value) is JointStateEffectClause + and value.target_position.dim() == 2 + and value.target_position.shape[0] != self.env_ids.numel() + ): + raise ValueError("Batched joint targets must match env_ids length.") + + held_relations = { + value.relation + for value in expectations + if type(value) is HeldObjectStateExpectation + } + if self.effect_kind is SemanticEffectKind.ATTACH and held_relations != { + HeldObjectRelation.ATTACHED + }: + raise ValueError("An attach effect requires only attached state.") + if self.effect_kind is SemanticEffectKind.RELEASE and held_relations != { + HeldObjectRelation.DETACHED + }: + raise ValueError("A release effect requires only detached state.") + if self.effect_kind is SemanticEffectKind.TRANSFER and held_relations != { + HeldObjectRelation.ATTACHED, + HeldObjectRelation.DETACHED, + }: + raise ValueError("A transfer effect requires attached and detached state.") + if self.effect_kind is SemanticEffectKind.ARTICULATION and not any( + type(value) is ArticulationJointStateExpectation for value in expectations + ): + raise ValueError( + "An articulation effect requires an articulation-joint expectation." + ) + + object.__setattr__( + self, + "state_expectations", + tuple(value.snapshot() for value in expectations), + ) + object.__setattr__( + self, + "clauses", + tuple(value.snapshot() for value in clauses), + ) + + def snapshot(self) -> SemanticEffectSpec: + """Return an independently owned grounded effect contract.""" + return SemanticEffectSpec( + semantic_id=self.semantic_id, + effect_kind=self.effect_kind, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + env_ids=self.env_ids, + state_expectations=self.state_expectations, + clauses=self.clauses, + ) + + def to_metadata(self) -> dict[str, object]: + """Return this grounded effect contract as deterministic JSON-safe data.""" + return { + "semantic_id": self.semantic_id, + "effect_kind": self.effect_kind.value, + "skill_id": self.skill_id, + "invocation_id": self.invocation_id, + "invocation_revision": self.invocation_revision, + "env_ids": _metadata_value(self.env_ids), + "state_expectations": [ + _metadata_value(value) for value in self.state_expectations + ], + "clauses": [_metadata_value(value) for value in self.clauses], + } + + def state_expectation(self, expectation_id: str) -> EffectStateExpectation: + """Return an owned state expectation by effect-local ID.""" + for value in self.state_expectations: + if value.expectation_id == expectation_id: + return value.snapshot() + raise KeyError(f"Unknown effect state expectation {expectation_id!r}.") + + def validate_request(self, request: EffectVerificationRequest) -> None: + """Validate execution identity and typed symbolic postconditions.""" + if not isinstance(request, EffectVerificationRequest): + raise TypeError("request must be an EffectVerificationRequest.") + if request.skill_id != self.skill_id: + raise ValueError("Effect request skill_id does not match the spec.") + if request.invocation_id != self.invocation_id: + raise ValueError("Effect request invocation_id does not match the spec.") + if request.invocation_revision != self.invocation_revision: + raise ValueError( + "Effect request invocation_revision does not match the spec." + ) + if request.env_mask.shape != self.env_ids.shape: + raise ValueError("Effect request row count does not match spec env_ids.") + if request.env_mask.device != self.env_ids.device: + raise ValueError( + "Effect request mask and spec env_ids must share a device." + ) + + held_expectations = { + value.task_state_key: value + for value in self.state_expectations + if type(value) is HeldObjectStateExpectation + } + expected_held = request.expected_effects.held_object_updates + if set(expected_held) != set(held_expectations): + raise ValueError( + "Effect request held-object updates must exactly match typed " + "state expectation keys." + ) + for task_state_key, expectation in held_expectations.items(): + candidate = expected_held[task_state_key] + if expectation.relation is HeldObjectRelation.DETACHED: + if candidate is not None: + raise ValueError( + f"Detached expectation {expectation.expectation_id!r} must " + "remove its held-object state." + ) + continue + if not isinstance(candidate, HeldObjectState): + raise ValueError( + f"Attached expectation {expectation.expectation_id!r} requires " + "a HeldObjectState postcondition." + ) + if candidate.semantics.entity_id != expectation.object_id: + raise ValueError( + f"Attached expectation {expectation.expectation_id!r} targets " + "the wrong canonical object." + ) + if candidate.object_to_eef.device != request.env_mask.device: + raise ValueError( + "Attached postcondition poses and request rows must share a device." + ) + if ( + candidate.object_to_eef.dim() == 3 + and candidate.object_to_eef.shape[0] != self.env_ids.numel() + ): + raise ValueError( + "Batched attached postcondition poses must match spec env_ids." + ) + _validate_pose_batch( + candidate.object_to_eef, + field_name=( + f"Attached expectation {expectation.expectation_id!r} pose" + ), + ) + if candidate.env_mask is not None: + if ( + candidate.env_mask.shape != request.env_mask.shape + or candidate.env_mask.device != request.env_mask.device + ): + raise ValueError( + "Attached postcondition masks must match request rows and device." + ) + if (request.env_mask & ~candidate.env_mask).any(): + raise ValueError( + "Attached postconditions must cover every requested row." + ) + + cleanup_expectations = { + value.task_state_keys + for value in self.state_expectations + if type(value) is CoordinatedHeldObjectCleanupExpectation + } + expected_cleanup = request.expected_effects.coordinated_held_object_updates + if set(expected_cleanup) != cleanup_expectations: + raise ValueError( + "Effect request coordinated updates must exactly match typed " + "cleanup expectations." + ) + if any(value is not None for value in expected_cleanup.values()): + raise ValueError( + "Coordinated held-object cleanup expectations may only remove state." + ) + + articulation_expectations = { + (value.articulation_id, value.joint_id): value + for value in self.state_expectations + if type(value) is ArticulationJointStateExpectation + } + articulation_updates = request.expected_effects.articulation_joint_updates + if set(articulation_updates) != set(articulation_expectations): + raise ValueError( + "Articulation-joint updates must exactly match typed state " + "expectations." + ) + for key, expectation in articulation_expectations.items(): + candidate = articulation_updates[key] + if not isinstance(candidate, ArticulationJointState): + raise ValueError( + f"Articulation expectation {expectation.expectation_id!r} " + "requires an ArticulationJointState postcondition." + ) + if candidate.position.device != request.env_mask.device: + raise ValueError( + "Articulation postconditions and request rows must share a device." + ) + if candidate.position.dim() == 2: + if candidate.position.shape[0] != self.env_ids.numel(): + raise ValueError( + "Batched articulation postconditions must match spec env_ids." + ) + positions = candidate.position + else: + positions = candidate.position.unsqueeze(0).expand( + self.env_ids.numel(), -1 + ) + target = expectation.target_position + if target.device != positions.device or target.dtype != positions.dtype: + raise ValueError( + "Articulation postconditions must match target device and dtype." + ) + if target.dim() == 1: + target = target.unsqueeze(0).expand(self.env_ids.numel(), -1) + if positions.shape != target.shape or not torch.equal( + positions[request.env_mask], + target[request.env_mask], + ): + raise ValueError( + f"Articulation expectation {expectation.expectation_id!r} " + "postcondition does not match its target position." + ) + if candidate.env_mask is not None: + if ( + candidate.env_mask.shape != request.env_mask.shape + or candidate.env_mask.device != request.env_mask.device + ): + raise ValueError( + "Articulation postcondition masks must match request rows " + "and device." + ) + if (request.env_mask & ~candidate.env_mask).any(): + raise ValueError( + "Articulation postconditions must cover every requested row." + ) + + +def _validate_evidence_common( + *, + evidence_id: str, + valid: torch.Tensor, + acquisition_errors: tuple[str | None, ...], + timestamp: float, + env_ids: torch.Tensor, + observation_revision: int, + batch_size: int, + device: torch.device, +) -> tuple[torch.Tensor, tuple[str | None, ...], float, torch.Tensor]: + """Validate and own fields shared by every raw evidence batch.""" + _validate_identifier(evidence_id, field_name="effect evidence_id") + if not isinstance(valid, torch.Tensor): + raise TypeError("valid must be a torch.Tensor.") + if valid.dtype != torch.bool or valid.shape != (batch_size,): + raise ValueError("valid must be a bool tensor with shape (B,).") + if valid.device != device: + raise ValueError("valid and evidence payload must share a device.") + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.shape != (batch_size,): + raise ValueError("env_ids must be a torch.long tensor with shape (B,).") + if env_ids.device != device: + raise ValueError("env_ids and evidence payload must share a device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("Evidence env_ids must be unique.") + errors = tuple(acquisition_errors) + if len(errors) != batch_size: + raise ValueError("acquisition_errors must contain one entry per row.") + for row, (row_valid, error) in enumerate(zip(valid.tolist(), errors)): + if row_valid and error is not None: + raise ValueError(f"Valid evidence row {row} must not carry an error.") + if not row_valid and ( + type(error) is not str or not error or error != error.strip() + ): + raise ValueError(f"Invalid evidence row {row} requires a non-empty error.") + if not isinstance(timestamp, (int, float)) or isinstance(timestamp, bool): + raise TypeError("timestamp must be a number.") + normalized_timestamp = float(timestamp) + if not math.isfinite(normalized_timestamp) or normalized_timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if type(observation_revision) is not int or observation_revision < 0: + raise ValueError("observation_revision must be a non-negative integer.") + return valid.clone(), errors, normalized_timestamp, env_ids.clone() + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseRelationEvidenceBatch: + """Raw object-to-endpoint transform observations.""" + + evidence_id: str + object_to_endpoint: torch.Tensor + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + poses = self.object_to_endpoint + if not isinstance(poses, torch.Tensor) or ( + poses.dim() != 3 or poses.shape[0] == 0 or poses.shape[-2:] != (4, 4) + ): + raise ValueError("object_to_endpoint must have shape (B, 4, 4).") + if not poses.is_floating_point(): + raise TypeError("object_to_endpoint must use a floating-point dtype.") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=poses.shape[0], + device=poses.device, + ) + object.__setattr__( + self, + "object_to_endpoint", + _validate_pose_batch( + poses, + field_name="Valid pose-relation evidence", + valid_mask=valid, + ), + ) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> PoseRelationEvidenceBatch: + """Return an independently owned evidence batch.""" + return PoseRelationEvidenceBatch( + self.evidence_id, + self.object_to_endpoint, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw pose evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={"object_to_endpoint": self.object_to_endpoint}, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class BinaryEffectEvidenceBatch: + """Raw per-row contact or constraint-state observations.""" + + evidence_id: str + evidence_kind: BinaryEvidenceKind + values: torch.Tensor + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + if not isinstance(self.evidence_kind, BinaryEvidenceKind): + raise TypeError("evidence_kind must be a BinaryEvidenceKind.") + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if values.dtype != torch.bool or values.dim() != 1 or values.numel() == 0: + raise ValueError("binary evidence values must have bool shape (B,).") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=values.shape[0], + device=values.device, + ) + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> BinaryEffectEvidenceBatch: + """Return an independently owned evidence batch.""" + return BinaryEffectEvidenceBatch( + self.evidence_id, + self.evidence_kind, + self.values, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw binary evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={ + "evidence_kind": self.evidence_kind.value, + "values": self.values, + }, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class ScalarEffectEvidenceBatch: + """Raw per-row force or wrench-magnitude observations.""" + + evidence_id: str + evidence_kind: ScalarEvidenceKind + values: torch.Tensor + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + if not isinstance(self.evidence_kind, ScalarEvidenceKind): + raise TypeError("evidence_kind must be a ScalarEvidenceKind.") + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if values.dim() != 1 or values.numel() == 0 or not values.is_floating_point(): + raise ValueError("scalar evidence values must have floating shape (B,).") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=values.shape[0], + device=values.device, + ) + if not torch.isfinite(values[valid]).all(): + raise ValueError("Valid scalar evidence values must be finite.") + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> ScalarEffectEvidenceBatch: + """Return an independently owned evidence batch.""" + return ScalarEffectEvidenceBatch( + self.evidence_id, + self.evidence_kind, + self.values, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw scalar evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={ + "evidence_kind": self.evidence_kind.value, + "values": self.values, + }, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateEvidenceBatch: + """Raw per-row joint position/velocity observations.""" + + evidence_id: str + positions: torch.Tensor + velocities: torch.Tensor | None + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + positions = self.positions + if not isinstance(positions, torch.Tensor): + raise TypeError("positions must be a torch.Tensor.") + if ( + positions.dim() != 2 + or positions.shape[0] == 0 + or positions.shape[1] == 0 + or not positions.is_floating_point() + ): + raise ValueError("positions must have non-empty floating shape (B, J).") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=positions.shape[0], + device=positions.device, + ) + if not torch.isfinite(positions[valid]).all(): + raise ValueError("Valid joint positions must be finite.") + velocities = self.velocities + if velocities is not None: + if not isinstance(velocities, torch.Tensor): + raise TypeError("velocities must be a torch.Tensor or None.") + if ( + velocities.shape != positions.shape + or velocities.device != positions.device + ): + raise ValueError("velocities must match positions shape and device.") + if not velocities.is_floating_point(): + raise TypeError("velocities must use a floating-point dtype.") + if not torch.isfinite(velocities[valid]).all(): + raise ValueError("Valid joint velocities must be finite.") + object.__setattr__(self, "positions", positions.clone()) + object.__setattr__( + self, + "velocities", + None if velocities is None else velocities.clone(), + ) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> JointStateEvidenceBatch: + """Return an independently owned evidence batch.""" + return JointStateEvidenceBatch( + self.evidence_id, + self.positions, + self.velocities, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw joint-state evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={ + "positions": self.positions, + "velocities": self.velocities, + }, + ) + + +def _evidence_metadata( + batch: EffectEvidenceBatch, + *, + payload: Mapping[str, object], +) -> dict[str, object]: + """Serialize fields shared by all raw physical-evidence batches.""" + return { + "evidence_id": batch.evidence_id, + **{key: _metadata_value(value) for key, value in payload.items()}, + "valid_mask": _metadata_value(batch.valid), + "acquisition_errors": list(batch.acquisition_errors), + "timestamp": batch.timestamp, + "env_ids": _metadata_value(batch.env_ids), + "observation_revision": batch.observation_revision, + } + + +EffectEvidenceBatch: TypeAlias = ( + PoseRelationEvidenceBatch + | BinaryEffectEvidenceBatch + | ScalarEffectEvidenceBatch + | JointStateEvidenceBatch +) +_EVIDENCE_TYPES = ( + PoseRelationEvidenceBatch, + BinaryEffectEvidenceBatch, + ScalarEffectEvidenceBatch, + JointStateEvidenceBatch, +) + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectMonitorDecision: + """Uncorrelated per-row decision; runtime adds the verification ID.""" + + success_mask: torch.Tensor + failure_mask: torch.Tensor + + def __post_init__(self) -> None: + for field_name in ("success_mask", "failure_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.") + if self.success_mask.shape != self.failure_mask.shape: + raise ValueError("Decision masks must have equal shapes.") + if self.success_mask.device != self.failure_mask.device: + raise ValueError("Decision masks must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("Decision masks must not overlap.") + object.__setattr__(self, "success_mask", self.success_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + + +class EffectMonitor(ABC): + """Stateful verifier owned by one grounded semantic call.""" + + @property + @abstractmethod + def spec(self) -> SemanticEffectSpec: + """Return an independently owned effect contract.""" + + @property + def resolved_params(self) -> Mapping[str, EffectMonitorParam]: + """Return resolved monitor thresholds for trace metadata. + + Custom monitors may override this property. The empty default keeps + third-party implementations source-compatible while built-ins expose + every effective threshold, including defaults omitted by configuration. + """ + return MappingProxyType({}) + + @abstractmethod + def observe( + self, + request: EffectVerificationRequest, + evidence: Mapping[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + """Consume one synchronized raw observation and decide requested rows.""" + + +class EffectMonitorFactory(ABC): + """Versioned constructor for independent semantic-effect monitors.""" + + monitor_id: ClassVar[str] + revision: ClassVar[str] + + @abstractmethod + def validate_ref(self, ref: EffectMonitorRef) -> None: + """Validate one reference without providers or state creation.""" + + @abstractmethod + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + """Create one independent monitor for ``spec`` and ``ref``.""" + + +def _same_tensor_value(left: torch.Tensor, right: torch.Tensor) -> bool: + """Return whether tensors have identical placement, type, shape, and value.""" + return ( + left.device == right.device + and left.dtype == right.dtype + and left.shape == right.shape + and torch.equal(left, right) + ) + + +def _same_source( + left: EffectEvidenceSourceRef, + right: EffectEvidenceSourceRef, +) -> bool: + return left.source_fingerprint == right.source_fingerprint + + +def _same_state_expectation( + left: EffectStateExpectation, + right: EffectStateExpectation, +) -> bool: + if type(left) is not type(right): + return False + if type(left) is HeldObjectStateExpectation: + assert type(right) is HeldObjectStateExpectation + return left == right + if type(left) is CoordinatedHeldObjectCleanupExpectation: + assert type(right) is CoordinatedHeldObjectCleanupExpectation + return left == right + assert type(left) is ArticulationJointStateExpectation + assert type(right) is ArticulationJointStateExpectation + return ( + left.expectation_id == right.expectation_id + and left.articulation_id == right.articulation_id + and left.joint_id == right.joint_id + and _same_tensor_value(left.target_position, right.target_position) + ) + + +def _same_clause(left: EffectClause, right: EffectClause) -> bool: + if type(left) is not type(right): + return False + if ( + left.clause_id != right.clause_id + or left.expectation_id != right.expectation_id + or not _same_source(left.source, right.source) + ): + return False + if type(left) is PoseRelationClause: + assert type(right) is PoseRelationClause + if left.expectation is not right.expectation: + return False + left_baseline = left.baseline_object_to_endpoint + right_baseline = right.baseline_object_to_endpoint + if left_baseline is None or right_baseline is None: + return left_baseline is None and right_baseline is None + return _same_tensor_value(left_baseline, right_baseline) + if type(left) is BinaryEffectClause: + assert type(right) is BinaryEffectClause + return ( + left.evidence_kind is right.evidence_kind + and left.expected is right.expected + ) + if type(left) is ScalarEffectClause: + assert type(right) is ScalarEffectClause + return ( + left.evidence_kind is right.evidence_kind + and left.expectation is right.expectation + ) + assert type(left) is JointStateEffectClause + assert type(right) is JointStateEffectClause + return _same_tensor_value(left.target_position, right.target_position) + + +def _same_effect_spec(left: SemanticEffectSpec, right: SemanticEffectSpec) -> bool: + """Return whether grounded typed effect specs are exactly equivalent.""" + return ( + left.semantic_id == right.semantic_id + and left.effect_kind is right.effect_kind + and left.skill_id == right.skill_id + and left.invocation_id == right.invocation_id + and left.invocation_revision == right.invocation_revision + and _same_tensor_value(left.env_ids, right.env_ids) + and len(left.state_expectations) == len(right.state_expectations) + and all( + _same_state_expectation(left_value, right_value) + for left_value, right_value in zip( + left.state_expectations, + right.state_expectations, + strict=True, + ) + ) + and len(left.clauses) == len(right.clauses) + and all( + _same_clause(left_value, right_value) + for left_value, right_value in zip( + left.clauses, + right.clauses, + strict=True, + ) + ) + ) + + +class EffectMonitorRegistry: + """Immutable exact-ID/revision registry of monitor factories.""" + + __slots__ = ("_factories",) + + def __init__(self, factories: Iterable[EffectMonitorFactory] = ()) -> None: + normalized: dict[tuple[str, str], EffectMonitorFactory] = {} + for factory in factories: + if not isinstance(factory, EffectMonitorFactory): + raise TypeError("factories must contain EffectMonitorFactory objects.") + monitor_id = _validate_identifier( + factory.monitor_id, + field_name="EffectMonitorFactory.monitor_id", + ) + revision = _validate_identifier( + factory.revision, + field_name="EffectMonitorFactory.revision", + ) + key = monitor_id, revision + if key in normalized: + raise ValueError(f"Duplicate effect-monitor factory {key!r}.") + normalized[key] = factory + self._factories = MappingProxyType(normalized) + + @property + def factories(self) -> Mapping[tuple[str, str], EffectMonitorFactory]: + """Return the immutable exact-key factory mapping.""" + return self._factories + + def resolve(self, ref: EffectMonitorRef) -> EffectMonitorFactory: + """Resolve the exact factory named by a declarative reference.""" + if not isinstance(ref, EffectMonitorRef): + raise TypeError("ref must be an EffectMonitorRef.") + key = ref.monitor_id, ref.revision + try: + return self._factories[key] + except KeyError as exc: + raise KeyError(f"Unknown effect-monitor factory {key!r}.") from exc + + def validate_ref(self, ref: EffectMonitorRef) -> None: + """Validate a reference provider-free through its exact factory.""" + self.resolve(ref).validate_ref(ref) + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + """Create one independent monitor through exact factory lookup.""" + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + factory = self.resolve(ref) + factory.validate_ref(ref) + monitor = factory.create(spec, ref) + if not isinstance(monitor, EffectMonitor): + raise TypeError( + "EffectMonitorFactory.create() must return an EffectMonitor." + ) + monitor_spec = monitor.spec + if not isinstance(monitor_spec, SemanticEffectSpec): + raise TypeError("EffectMonitor.spec must be a SemanticEffectSpec.") + if monitor_spec is spec: + raise TypeError( + "EffectMonitor.spec must return an independently owned contract." + ) + if not _same_effect_spec(monitor_spec, spec): + raise ValueError( + "EffectMonitorFactory created a monitor for a different effect spec." + ) + return monitor + + +@dataclass(frozen=True, slots=True) +class CompositeEffectMonitorCfg: + """Strict hysteresis policy for typed pose/binary/scalar/joint clauses.""" + + attached_translation_threshold: float = 0.02 + attached_rotation_threshold: float = 0.20 + detached_translation_threshold: float = 0.05 + detached_rotation_threshold: float = 0.50 + force_absent_threshold: float = 0.20 + force_present_threshold: float = 1.00 + joint_success_tolerance: float = 0.02 + joint_failure_tolerance: float = 0.10 + consecutive_samples: int = 2 + + def __post_init__(self) -> None: + for field_name in ( + "attached_translation_threshold", + "attached_rotation_threshold", + "detached_translation_threshold", + "detached_rotation_threshold", + "force_absent_threshold", + "force_present_threshold", + "joint_success_tolerance", + "joint_failure_tolerance", + ): + value = getattr(self, field_name) + if not isinstance(value, (int, float)) or isinstance(value, bool): + raise TypeError(f"{field_name} must be a number.") + if not math.isfinite(float(value)) or value < 0.0: + raise ValueError(f"{field_name} must be finite and non-negative.") + object.__setattr__(self, field_name, float(value)) + if self.attached_translation_threshold >= self.detached_translation_threshold: + raise ValueError( + "attached_translation_threshold must be less than " + "detached_translation_threshold." + ) + if self.attached_rotation_threshold >= self.detached_rotation_threshold: + raise ValueError( + "attached_rotation_threshold must be less than " + "detached_rotation_threshold." + ) + if self.detached_rotation_threshold > math.pi: + raise ValueError("detached_rotation_threshold must not exceed pi.") + if self.force_absent_threshold >= self.force_present_threshold: + raise ValueError( + "force_absent_threshold must be less than force_present_threshold." + ) + if self.joint_success_tolerance >= self.joint_failure_tolerance: + raise ValueError( + "joint_success_tolerance must be less than joint_failure_tolerance." + ) + if type(self.consecutive_samples) is not int or self.consecutive_samples <= 0: + raise ValueError("consecutive_samples must be a positive integer.") + + @classmethod + def from_params( + cls, + params: Mapping[str, EffectMonitorParam], + ) -> CompositeEffectMonitorCfg: + """Decode strict declarative factory parameters.""" + allowed = { + "attached_translation_threshold", + "attached_rotation_threshold", + "detached_translation_threshold", + "detached_rotation_threshold", + "force_absent_threshold", + "force_present_threshold", + "joint_success_tolerance", + "joint_failure_tolerance", + "consecutive_samples", + } + unknown = set(params).difference(allowed) + if unknown: + raise ValueError( + f"Unknown composite effect monitor parameters: {sorted(unknown)}." + ) + return cls(**dict(params)) # type: ignore[arg-type] + + def to_metadata(self) -> dict[str, object]: + """Return every resolved hysteresis threshold as JSON-safe data.""" + return { + "attached_translation_threshold": self.attached_translation_threshold, + "attached_rotation_threshold": self.attached_rotation_threshold, + "detached_translation_threshold": self.detached_translation_threshold, + "detached_rotation_threshold": self.detached_rotation_threshold, + "force_absent_threshold": self.force_absent_threshold, + "force_present_threshold": self.force_present_threshold, + "joint_success_tolerance": self.joint_success_tolerance, + "joint_failure_tolerance": self.joint_failure_tolerance, + "consecutive_samples": self.consecutive_samples, + } + + +def _pose_errors( + observed: torch.Tensor, + baseline: torch.Tensor, +) -> tuple[float, float]: + baseline = baseline.to(device=observed.device, dtype=observed.dtype) + translation = torch.linalg.vector_norm(observed[:3, 3] - baseline[:3, 3]) + relative_rotation = baseline[:3, :3].transpose(0, 1) @ observed[:3, :3] + cosine = torch.clamp((torch.trace(relative_rotation) - 1.0) * 0.5, -1.0, 1.0) + rotation = torch.acos(cosine) + return float(translation.item()), float(rotation.item()) + + +class CompositeEffectMonitor(EffectMonitor): + """Stateful conjunction monitor over typed physical evidence clauses.""" + + def __init__( + self, + spec: SemanticEffectSpec, + cfg: CompositeEffectMonitorCfg, + ) -> None: + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + if not isinstance(cfg, CompositeEffectMonitorCfg): + raise TypeError("cfg must be a CompositeEffectMonitorCfg.") + self._spec = spec.snapshot() + 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._last_observations: dict[int, tuple[float, int]] = {} + + @property + def spec(self) -> SemanticEffectSpec: + """Return an independently owned effect contract.""" + return self._spec.snapshot() + + @property + def resolved_params(self) -> Mapping[str, EffectMonitorParam]: + """Return all effective typed-clause thresholds, including defaults.""" + return MappingProxyType(self._cfg.to_metadata()) + + def _prepare_request(self, request: EffectVerificationRequest) -> None: + self._spec.validate_request(request) + active_env_ids = frozenset( + int(value) + for value in self._spec.env_ids[request.env_mask].detach().cpu().tolist() + ) + if self._attempt_generation != request.attempt_generation: + self._attempt_generation = request.attempt_generation + self._active_env_ids = active_env_ids + self._success_counts.clear() + self._failure_counts.clear() + self._last_observations.clear() + return + if not active_env_ids.issubset(self._active_env_ids): + raise ValueError( + "An effect-verification request may only shrink within one " + "attempt_generation." + ) + 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 + } + self._failure_counts = { + env_id: count + for env_id, count in self._failure_counts.items() + if env_id in active_env_ids + } + self._last_observations = { + env_id: observation + for env_id, observation in self._last_observations.items() + if env_id in active_env_ids + } + + @staticmethod + def _validate_evidence_type( + clause: EffectClause, + batch: EffectEvidenceBatch, + ) -> None: + if type(clause) is PoseRelationClause: + if type(batch) is not PoseRelationEvidenceBatch: + raise TypeError("PoseRelationClause requires pose evidence.") + return + if type(clause) is BinaryEffectClause: + if type(batch) is not BinaryEffectEvidenceBatch: + raise TypeError("BinaryEffectClause requires binary evidence.") + if batch.evidence_kind is not clause.evidence_kind: + raise ValueError("Binary evidence kind does not match its clause.") + return + if type(clause) is ScalarEffectClause: + if type(batch) is not ScalarEffectEvidenceBatch: + raise TypeError("ScalarEffectClause requires scalar evidence.") + if batch.evidence_kind is not clause.evidence_kind: + raise ValueError("Scalar evidence kind does not match its clause.") + return + if type(batch) is not JointStateEvidenceBatch: + raise TypeError("JointStateEffectClause requires joint-state evidence.") + + def _normalize_evidence( + self, + evidence: Mapping[str, EffectEvidenceBatch], + *, + requested_at: float, + deadline: float, + ) -> tuple[Mapping[str, EffectEvidenceBatch], tuple[int, ...]]: + if not isinstance(evidence, Mapping): + raise TypeError("evidence must be a mapping.") + clause_by_id = {value.clause_id: value for value in self._spec.clauses} + if set(evidence) != set(clause_by_id): + raise ValueError("Evidence keys must exactly match effect clause IDs.") + normalized: dict[str, EffectEvidenceBatch] = {} + first: EffectEvidenceBatch | None = None + for clause_id, batch in evidence.items(): + if type(batch) not in _EVIDENCE_TYPES: + raise TypeError( + "evidence values must be typed effect evidence batches." + ) + if batch.evidence_id != clause_id: + raise ValueError("Evidence keys must match batch evidence_id values.") + self._validate_evidence_type(clause_by_id[clause_id], batch) + if batch.timestamp < requested_at: + raise ValueError("Effect evidence must not predate the request.") + if batch.timestamp > deadline: + raise ValueError( + "Effect evidence must not exceed the request deadline." + ) + if first is None: + first = batch + elif ( + batch.timestamp != first.timestamp + or batch.observation_revision != first.observation_revision + or not torch.equal(batch.env_ids, first.env_ids) + ): + raise ValueError( + "All effect evidence must share timestamp, observation_revision, " + "and env_ids." + ) + normalized[clause_id] = batch.snapshot() + assert first is not None + known_env_ids = set(self._spec.env_ids.detach().cpu().tolist()) + observed_env_ids = tuple(int(value) for value in first.env_ids.cpu().tolist()) + if not set(observed_env_ids).issubset(known_env_ids): + raise ValueError("Evidence contains env_ids outside the effect spec.") + missing = self._active_env_ids.difference(observed_env_ids) + if missing: + for env_id in missing: + self._success_counts[env_id] = 0 + self._failure_counts[env_id] = 0 + raise ValueError( + "Evidence must cover every active request env_id exactly once; " + f"missing {sorted(missing)}. Acquisition failures must be explicit " + "valid=False rows." + ) + return MappingProxyType(normalized), observed_env_ids + + def _pose_baseline( + self, + clause: PoseRelationClause, + request: EffectVerificationRequest, + spec_row: int, + ) -> torch.Tensor: + baseline = clause.baseline_object_to_endpoint + if baseline is None: + expectation = self._spec.state_expectation(clause.expectation_id) + if type(expectation) is not HeldObjectStateExpectation: + raise ValueError( + "A request-derived pose baseline requires a held-object " + "state expectation." + ) + candidate = request.expected_effects.held_object_updates[ + expectation.task_state_key + ] + assert isinstance(candidate, HeldObjectState) + baseline = candidate.object_to_eef + return baseline if baseline.dim() == 2 else baseline[spec_row] + + def _classify_clause( + self, + clause: EffectClause, + batch: EffectEvidenceBatch, + *, + evidence_row: int, + spec_row: int, + request: EffectVerificationRequest, + ) -> int: + """Return 1 expected, -1 contradicted, or 0 unresolved.""" + if not bool(batch.valid[evidence_row].item()): + return 0 + if type(clause) is PoseRelationClause: + assert type(batch) is PoseRelationEvidenceBatch + observed = batch.object_to_endpoint[evidence_row] + baseline = self._pose_baseline(clause, request, spec_row) + translation_error, rotation_error = _pose_errors(observed, baseline) + matched = ( + translation_error <= self._cfg.attached_translation_threshold + and rotation_error <= self._cfg.attached_rotation_threshold + ) + separated = ( + translation_error >= self._cfg.detached_translation_threshold + or rotation_error >= self._cfg.detached_rotation_threshold + ) + if clause.expectation is PoseRelationExpectation.MATCHED: + return 1 if matched else (-1 if separated else 0) + return 1 if separated else (-1 if matched else 0) + if type(clause) is BinaryEffectClause: + assert type(batch) is BinaryEffectEvidenceBatch + return ( + 1 if bool(batch.values[evidence_row].item()) is clause.expected else -1 + ) + if type(clause) is ScalarEffectClause: + assert type(batch) is ScalarEffectEvidenceBatch + magnitude = abs(float(batch.values[evidence_row].item())) + present = magnitude >= self._cfg.force_present_threshold + absent = magnitude <= self._cfg.force_absent_threshold + if clause.expectation is ScalarExpectation.PRESENT: + return 1 if present else (-1 if absent else 0) + return 1 if absent else (-1 if present else 0) + assert type(clause) is JointStateEffectClause + assert type(batch) is JointStateEvidenceBatch + target = clause.target_position + if target.dim() == 2: + target = target[spec_row] + observed = batch.positions[evidence_row] + if target.shape != observed.shape: + raise ValueError("Joint evidence width does not match its clause target.") + error = float(torch.max(torch.abs(observed - target)).item()) + if error <= self._cfg.joint_success_tolerance: + return 1 + if error >= self._cfg.joint_failure_tolerance: + return -1 + return 0 + + def observe( + self, + request: EffectVerificationRequest, + evidence: Mapping[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + """Update typed-clause hysteresis and decide current request rows.""" + self._prepare_request(request) + batches, observed_env_ids = self._normalize_evidence( + evidence, + 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()) + } + request_rows = { + int(env_id): row + for row, env_id in enumerate(self._spec.env_ids.detach().cpu().tolist()) + if bool(request.env_mask[row].item()) + } + first_batch = next(iter(batches.values())) + observation_token = ( + first_batch.timestamp, + first_batch.observation_revision, + ) + for env_id in self._active_env_ids: + previous = self._last_observations.get(env_id) + if previous is None: + continue + if observation_token[0] < previous[0]: + raise ValueError( + "Evidence timestamps must be monotonic for every active env_id." + ) + if observation_token[1] < previous[1]: + raise ValueError( + "Evidence observation_revision values must be monotonic for " + "every active env_id." + ) + + 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) + + for evidence_row, env_id in enumerate(observed_env_ids): + request_row = request_rows.get(env_id) + if request_row is None: + continue + if self._last_observations.get(env_id) == observation_token: + 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( + clause, + batches[clause.clause_id], + evidence_row=evidence_row, + spec_row=spec_row, + request=request, + ) + for clause in clauses_by_expectation[expectation_id] + ] + 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) + + +class CompositeEffectMonitorFactory(EffectMonitorFactory): + """Factory for the built-in typed-clause monitor.""" + + monitor_id = COMPOSITE_EFFECT_MONITOR_ID + revision = COMPOSITE_EFFECT_MONITOR_REVISION + + def validate_ref(self, ref: EffectMonitorRef) -> None: + """Validate exact built-in selection and typed thresholds.""" + if not isinstance(ref, EffectMonitorRef): + raise TypeError("ref must be an EffectMonitorRef.") + if (ref.monitor_id, ref.revision) != (self.monitor_id, self.revision): + raise ValueError("EffectMonitorRef does not select this exact factory.") + CompositeEffectMonitorCfg.from_params(ref.params) + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> CompositeEffectMonitor: + """Create one independently stateful typed-clause monitor.""" + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + self.validate_ref(ref) + return CompositeEffectMonitor( + spec.snapshot(), + CompositeEffectMonitorCfg.from_params(ref.params), + ) + + +__all__ = [ + "ArticulationJointStateExpectation", + "BinaryEffectClause", + "BinaryEffectEvidenceBatch", + "BinaryEvidenceKind", + "COMPOSITE_EFFECT_MONITOR_ID", + "COMPOSITE_EFFECT_MONITOR_REVISION", + "CONTACT_EFFECT_CHANNEL", + "CONSTRAINT_EFFECT_CHANNEL", + "CONTROL_PART_EVIDENCE_PROVIDER_ID", + "CONTROL_PART_EVIDENCE_PROVIDER_REVISION", + "CompositeEffectMonitor", + "CompositeEffectMonitorCfg", + "CompositeEffectMonitorFactory", + "ControlPartEvidenceAddress", + "CoordinatedHeldObjectCleanupExpectation", + "EffectClause", + "EffectEvidenceAddress", + "EffectEvidenceBatch", + "EffectEvidenceSourceRef", + "EffectMonitor", + "EffectMonitorDecision", + "EffectMonitorFactory", + "EffectMonitorParam", + "EffectMonitorRef", + "EffectMonitorRegistry", + "EffectStateExpectation", + "FORCE_EFFECT_CHANNEL", + "HeldObjectRelation", + "HeldObjectStateExpectation", + "JOINT_STATE_EFFECT_CHANNEL", + "JointStateEffectClause", + "JointStateEvidenceBatch", + "POSE_RELATION_EFFECT_CHANNEL", + "PoseRelationClause", + "PoseRelationEvidenceBatch", + "PoseRelationExpectation", + "ScalarEffectClause", + "ScalarEffectEvidenceBatch", + "ScalarEvidenceKind", + "ScalarExpectation", + "SemanticEffectKind", + "SemanticEffectSpec", + "SymbolicStateDomain", + "SymbolicStateKey", +] diff --git a/embodichain/lab/sim/skills/evidence.py b/embodichain/lab/sim/skills/evidence.py new file mode 100644 index 000000000..56fe7e94a --- /dev/null +++ b/embodichain/lab/sim/skills/evidence.py @@ -0,0 +1,1467 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Backend-neutral acquisition ports for typed semantic-effect evidence.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from collections.abc import Callable, Iterable, Mapping, Sequence +from dataclasses import dataclass +import math +from types import MappingProxyType +from typing import ClassVar, Protocol, TypeAlias, runtime_checkable + +import torch + +from embodichain.utils.math import pose_inv + +from ..atomic_actions import SceneProvider, SceneSnapshot +from .effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress, + CoordinatedHeldObjectCleanupExpectation, + EffectClause, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectStateExpectation, + FORCE_EFFECT_CHANNEL, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + JointStateEvidenceBatch, + POSE_RELATION_EFFECT_CHANNEL, + PoseRelationClause, + PoseRelationEvidenceBatch, + ScalarEffectClause, + ScalarEffectEvidenceBatch, + ScalarEvidenceKind, + SemanticEffectSpec, +) +from .scene import ( + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, +) + +_EFFECT_EXPECTATION_TYPES = ( + HeldObjectStateExpectation, + CoordinatedHeldObjectCleanupExpectation, + ArticulationJointStateExpectation, +) +_EFFECT_BATCH_TYPES = ( + PoseRelationEvidenceBatch, + BinaryEffectEvidenceBatch, + ScalarEffectEvidenceBatch, + JointStateEvidenceBatch, +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectEvidenceCollectionContext: + """One synchronized acquisition tick shared by all effect clauses. + + Args: + timestamp: Non-negative backend observation time. + observation_revision: Monotonic revision chosen by the runtime port. + env_ids: Ordered environment correlation IDs to observe. + """ + + timestamp: float + observation_revision: int + env_ids: torch.Tensor + + def __post_init__(self) -> None: + if isinstance(self.timestamp, bool) or not isinstance( + self.timestamp, (int, float) + ): + raise TypeError("timestamp must be a number.") + timestamp = float(self.timestamp) + if not math.isfinite(timestamp) or timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if type(self.observation_revision) is not int or self.observation_revision < 0: + raise ValueError("observation_revision must be a non-negative integer.") + env_ids = self.env_ids + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.dim() != 1 or env_ids.numel() == 0: + raise ValueError( + "env_ids must be a non-empty one-dimensional int64 tensor." + ) + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids.clone()) + + def snapshot(self) -> EffectEvidenceCollectionContext: + """Return an independently owned acquisition context.""" + return EffectEvidenceCollectionContext( + self.timestamp, + self.observation_revision, + self.env_ids, + ) + + +def _snapshot_expectation( + expectation: EffectStateExpectation, +) -> EffectStateExpectation: + """Validate and own one exact typed effect expectation.""" + if type(expectation) not in _EFFECT_EXPECTATION_TYPES: + raise TypeError("expectation must be an exact typed effect expectation.") + return expectation.snapshot() + + +class EffectEvidenceQuery(ABC): + """Typed request for the raw evidence of exactly one effect clause.""" + + @property + @abstractmethod + def evidence_id(self) -> str: + """Return the clause-local evidence identifier.""" + + @property + @abstractmethod + def source(self) -> EffectEvidenceSourceRef: + """Return an owned exact provider route and physical address.""" + + @property + @abstractmethod + def expectation(self) -> EffectStateExpectation: + """Return an owned symbolic expectation related to this query.""" + + @abstractmethod + def snapshot(self) -> EffectEvidenceQuery: + """Return an independently owned query of the exact same type.""" + + +def _validate_query( + clause: EffectClause, + expectation: EffectStateExpectation, +) -> EffectStateExpectation: + """Validate common clause/expectation correlation.""" + owned = _snapshot_expectation(expectation) + if clause.expectation_id != owned.expectation_id: + raise ValueError("Query clause and expectation IDs must match.") + return owned + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseRelationEvidenceQuery(EffectEvidenceQuery): + """Query for an object's pose relative to a resource endpoint.""" + + clause: PoseRelationClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not PoseRelationClause: + raise TypeError("clause must be a PoseRelationClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> PoseRelationEvidenceQuery: + """Return an independently owned pose query.""" + return PoseRelationEvidenceQuery(self.clause, self._expectation) + + +@dataclass(frozen=True, slots=True, eq=False) +class BinaryEffectEvidenceQuery(EffectEvidenceQuery): + """Query for one raw contact or constraint boolean.""" + + clause: BinaryEffectClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not BinaryEffectClause: + raise TypeError("clause must be a BinaryEffectClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> BinaryEffectEvidenceQuery: + """Return an independently owned binary query.""" + return BinaryEffectEvidenceQuery(self.clause, self._expectation) + + +@dataclass(frozen=True, slots=True, eq=False) +class ScalarEffectEvidenceQuery(EffectEvidenceQuery): + """Query for one raw force or wrench magnitude.""" + + clause: ScalarEffectClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not ScalarEffectClause: + raise TypeError("clause must be a ScalarEffectClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> ScalarEffectEvidenceQuery: + """Return an independently owned scalar query.""" + return ScalarEffectEvidenceQuery(self.clause, self._expectation) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateEvidenceQuery(EffectEvidenceQuery): + """Query for current joint positions and optional velocities.""" + + clause: JointStateEffectClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not JointStateEffectClause: + raise TypeError("clause must be a JointStateEffectClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> JointStateEvidenceQuery: + """Return an independently owned joint-state query.""" + return JointStateEvidenceQuery(self.clause, self._expectation) + + +EffectEvidenceQueryValue: TypeAlias = ( + PoseRelationEvidenceQuery + | BinaryEffectEvidenceQuery + | ScalarEffectEvidenceQuery + | JointStateEvidenceQuery +) +"""Closed set of typed clause queries accepted by evidence providers.""" + + +def build_effect_evidence_queries( + spec: SemanticEffectSpec, +) -> tuple[EffectEvidenceQueryValue, ...]: + """Build one independently owned typed query per effect clause. + + Args: + spec: Grounded semantic effect contract. + + Returns: + Queries in the contract's deterministic clause order. + """ + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + queries: list[EffectEvidenceQueryValue] = [] + for clause in spec.clauses: + expectation = spec.state_expectation(clause.expectation_id) + if type(clause) is PoseRelationClause: + queries.append(PoseRelationEvidenceQuery(clause, expectation)) + elif type(clause) is BinaryEffectClause: + queries.append(BinaryEffectEvidenceQuery(clause, expectation)) + elif type(clause) is ScalarEffectClause: + queries.append(ScalarEffectEvidenceQuery(clause, expectation)) + elif type(clause) is JointStateEffectClause: + queries.append(JointStateEvidenceQuery(clause, expectation)) + else: + raise TypeError(f"Unsupported effect clause type {type(clause).__name__}.") + return tuple(queries) + + +class EffectEvidenceProvider(ABC): + """Versioned backend port that acquires a group of exact-source queries.""" + + provider_id: ClassVar[str] + revision: ClassVar[str] + + @abstractmethod + def collect( + self, + queries: tuple[EffectEvidenceQueryValue, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire one synchronized batch for every supplied query.""" + + +class EffectEvidenceProviderRegistry: + """Immutable exact-ID/revision registry of live evidence providers.""" + + __slots__ = ("_providers",) + + def __init__(self, providers: Iterable[EffectEvidenceProvider] = ()) -> None: + normalized: dict[tuple[str, str], EffectEvidenceProvider] = {} + for provider in providers: + if not isinstance(provider, EffectEvidenceProvider): + raise TypeError( + "providers must contain EffectEvidenceProvider instances." + ) + provider_id = _validate_identifier( + provider.provider_id, + field_name="EffectEvidenceProvider.provider_id", + ) + revision = _validate_identifier( + provider.revision, + field_name="EffectEvidenceProvider.revision", + ) + key = provider_id, revision + if key in normalized: + raise ValueError(f"Duplicate effect-evidence provider {key!r}.") + normalized[key] = provider + self._providers = MappingProxyType(normalized) + + @property + def providers(self) -> Mapping[tuple[str, str], EffectEvidenceProvider]: + """Return the immutable exact-key provider mapping.""" + return self._providers + + def resolve(self, source: EffectEvidenceSourceRef) -> EffectEvidenceProvider: + """Resolve the exact provider selected by ``source``. + + Args: + source: Versioned evidence route from one effect clause. + + Returns: + Registered provider with the exact ID and revision. + + Raises: + KeyError: If no exact provider version is installed. + """ + if not isinstance(source, EffectEvidenceSourceRef): + raise TypeError("source must be an EffectEvidenceSourceRef.") + key = source.provider_id, source.revision + try: + return self._providers[key] + except KeyError as exc: + raise KeyError( + f"Unknown effect-evidence provider {key!r}; exact versions are " + "required." + ) from exc + + +def _expected_batch_type(query: EffectEvidenceQueryValue) -> type[EffectEvidenceBatch]: + """Return the exact evidence batch type required by one query.""" + if type(query) is PoseRelationEvidenceQuery: + return PoseRelationEvidenceBatch + if type(query) is BinaryEffectEvidenceQuery: + return BinaryEffectEvidenceBatch + if type(query) is ScalarEffectEvidenceQuery: + return ScalarEffectEvidenceBatch + if type(query) is JointStateEvidenceQuery: + return JointStateEvidenceBatch + raise TypeError(f"Unsupported effect evidence query {type(query).__name__}.") + + +class EffectEvidenceCollector: + """Dispatch and normalize a synchronized observation for one effect spec.""" + + __slots__ = ("_registry",) + + def __init__(self, registry: EffectEvidenceProviderRegistry) -> None: + if not isinstance(registry, EffectEvidenceProviderRegistry): + raise TypeError("registry must be an EffectEvidenceProviderRegistry.") + self._registry = registry + + @property + def registry(self) -> EffectEvidenceProviderRegistry: + """Return the immutable provider registry.""" + return self._registry + + def collect( + self, + spec: SemanticEffectSpec, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire and strictly synchronize evidence for every effect clause. + + Args: + spec: Grounded semantic effect contract. + timestamp: Backend observation time for this acquisition tick. + observation_revision: Runtime-owned observation revision. + env_ids: Optional ordered subset of ``spec.env_ids``. Acquisition + failures must remain present as rows with ``valid=False``. + + Returns: + Immutable mapping keyed exactly by effect clause ID. + """ + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + selected_env_ids = spec.env_ids if env_ids is None else env_ids + context = EffectEvidenceCollectionContext( + timestamp, + observation_revision, + selected_env_ids, + ) + known_ids = set(spec.env_ids.detach().cpu().tolist()) + selected_ids = set(context.env_ids.detach().cpu().tolist()) + if not selected_ids.issubset(known_ids): + raise ValueError("env_ids must be a subset of the effect spec env_ids.") + + queries = build_effect_evidence_queries(spec) + groups: dict[ + tuple[str, str], + list[EffectEvidenceQueryValue], + ] = {} + for query in queries: + source = query.source + self._registry.resolve(source) + groups.setdefault((source.provider_id, source.revision), []).append(query) + + batches: dict[str, EffectEvidenceBatch] = {} + for key, grouped_queries in groups.items(): + provider = self._registry.providers[key] + owned_queries = tuple(query.snapshot() for query in grouped_queries) + supplied = provider.collect(owned_queries, context.snapshot()) + if not isinstance(supplied, Mapping): + raise TypeError( + f"Effect-evidence provider {key!r} must return a mapping." + ) + expected_ids = {query.evidence_id for query in grouped_queries} + if set(supplied) != expected_ids: + raise ValueError( + f"Effect-evidence provider {key!r} must return exactly query " + f"IDs {sorted(expected_ids)}; got {sorted(supplied)}." + ) + for query in grouped_queries: + batch = supplied[query.evidence_id] + expected_type = _expected_batch_type(query) + if type(batch) is not expected_type: + raise TypeError( + f"Evidence {query.evidence_id!r} must be " + f"{expected_type.__name__}." + ) + if batch.evidence_id != query.evidence_id: + raise ValueError( + "Evidence mapping keys must match batch evidence_id values." + ) + if batch.timestamp != context.timestamp: + raise ValueError( + "Every evidence batch must use the collection timestamp." + ) + if batch.observation_revision != context.observation_revision: + raise ValueError( + "Every evidence batch must use the collection revision." + ) + if batch.env_ids.device != context.env_ids.device or not torch.equal( + batch.env_ids, context.env_ids + ): + raise ValueError( + "Every evidence batch must use the ordered collection env_ids." + ) + if type(query) is BinaryEffectEvidenceQuery: + assert type(batch) is BinaryEffectEvidenceBatch + if batch.evidence_kind is not query.clause.evidence_kind: + raise ValueError("Binary evidence kind must match its query.") + if type(query) is ScalarEffectEvidenceQuery: + assert type(batch) is ScalarEffectEvidenceBatch + if batch.evidence_kind is not query.clause.evidence_kind: + raise ValueError("Scalar evidence kind must match its query.") + batches[query.evidence_id] = batch.snapshot() + + expected_all = {query.evidence_id for query in queries} + if set(batches) != expected_all: + raise AssertionError("Evidence dispatch lost one or more effect clauses.") + return MappingProxyType(batches) + + +@dataclass(frozen=True, slots=True, eq=False) +class BinaryEffectObservation: + """Callback-owned raw binary values with explicit row validity.""" + + values: torch.Tensor + valid: torch.Tensor | None = None + acquisition_errors: tuple[str | None, ...] = () + + def __post_init__(self) -> None: + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if values.dtype != torch.bool or values.dim() != 1 or values.numel() == 0: + raise ValueError("values must have non-empty bool shape (B,).") + valid = torch.ones_like(values) if self.valid is None else self.valid + if ( + not isinstance(valid, torch.Tensor) + or valid.dtype != torch.bool + or valid.shape != values.shape + or valid.device != values.device + ): + raise ValueError("valid must match values shape, bool dtype, and device.") + errors = self.acquisition_errors or (None,) * values.shape[0] + _validate_observation_errors(valid, errors) + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid.clone()) + object.__setattr__(self, "acquisition_errors", tuple(errors)) + + +@dataclass(frozen=True, slots=True, eq=False) +class ScalarEffectObservation: + """Callback-owned raw scalar values with explicit row validity.""" + + values: torch.Tensor + valid: torch.Tensor | None = None + acquisition_errors: tuple[str | None, ...] = () + + def __post_init__(self) -> None: + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if not values.is_floating_point() or values.dim() != 1 or values.numel() == 0: + raise ValueError("values must have non-empty floating shape (B,).") + valid = ( + torch.ones_like(values, dtype=torch.bool) + if self.valid is None + else self.valid + ) + if ( + not isinstance(valid, torch.Tensor) + or valid.dtype != torch.bool + or valid.shape != values.shape + or valid.device != values.device + ): + raise ValueError("valid must match values shape, bool dtype, and device.") + if not torch.isfinite(values[valid]).all(): + raise ValueError("Valid scalar observations must be finite.") + errors = self.acquisition_errors or (None,) * values.shape[0] + _validate_observation_errors(valid, errors) + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid.clone()) + object.__setattr__(self, "acquisition_errors", tuple(errors)) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateObservation: + """Callback-owned raw joint state with explicit row validity.""" + + positions: torch.Tensor + velocities: torch.Tensor | None = None + valid: torch.Tensor | None = None + acquisition_errors: tuple[str | None, ...] = () + + def __post_init__(self) -> None: + positions = self.positions + if not isinstance(positions, torch.Tensor): + raise TypeError("positions must be a torch.Tensor.") + if ( + not positions.is_floating_point() + or positions.dim() != 2 + or positions.shape[0] == 0 + or positions.shape[1] == 0 + ): + raise ValueError("positions must have non-empty floating shape (B, J).") + valid = ( + torch.ones(positions.shape[0], dtype=torch.bool, device=positions.device) + if self.valid is None + else self.valid + ) + if ( + not isinstance(valid, torch.Tensor) + or valid.dtype != torch.bool + or valid.shape != (positions.shape[0],) + or valid.device != positions.device + ): + raise ValueError("valid must have bool shape (B,) on the positions device.") + if not torch.isfinite(positions[valid]).all(): + raise ValueError("Valid joint positions must be finite.") + velocities = self.velocities + if velocities is not None: + if not isinstance(velocities, torch.Tensor): + raise TypeError("velocities must be a torch.Tensor or None.") + if ( + velocities.shape != positions.shape + or velocities.device != positions.device + ): + raise ValueError("velocities must match positions shape and device.") + if not velocities.is_floating_point(): + raise TypeError("velocities must use a floating-point dtype.") + if not torch.isfinite(velocities[valid]).all(): + raise ValueError("Valid joint velocities must be finite.") + errors = self.acquisition_errors or (None,) * positions.shape[0] + _validate_observation_errors(valid, errors) + object.__setattr__(self, "positions", positions.clone()) + object.__setattr__( + self, + "velocities", + None if velocities is None else velocities.clone(), + ) + object.__setattr__(self, "valid", valid.clone()) + object.__setattr__(self, "acquisition_errors", tuple(errors)) + + +def _validate_observation_errors( + valid: torch.Tensor, + errors: Sequence[str | None], +) -> None: + """Validate explicit per-row acquisition errors.""" + if len(errors) != valid.shape[0]: + raise ValueError("acquisition_errors must contain one entry per row.") + for row, (row_valid, error) in enumerate(zip(valid.tolist(), errors)): + if row_valid and error is not None: + raise ValueError(f"Valid observation row {row} must not carry an error.") + if not row_valid and ( + type(error) is not str or not error or error != error.strip() + ): + raise ValueError( + f"Invalid observation row {row} requires a non-empty error." + ) + + +BinaryObservationCallback: TypeAlias = Callable[ + [BinaryEffectEvidenceQuery, EffectEvidenceCollectionContext], + BinaryEffectObservation, +] +ScalarObservationCallback: TypeAlias = Callable[ + [ScalarEffectEvidenceQuery, EffectEvidenceCollectionContext], + ScalarEffectObservation, +] +ArticulationJointObservationCallback: TypeAlias = Callable[ + [JointStateEvidenceQuery, EffectEvidenceCollectionContext], + JointStateObservation, +] + + +class SceneArticulationEvidenceProvider(EffectEvidenceProvider): + """Typed adapter for scene-articulation joint-state observations. + + Integrations inject either a direct observer or a :class:`SceneProvider` + whose snapshot contains ``ObservedArticulationJointState`` values. + The adapter never discovers live simulator objects from an environment. + Repeated clauses share one synchronized snapshot and one sample per exact + physical address. + """ + + provider_id = SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + revision = SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + + def __init__( + self, + observer: ArticulationJointObservationCallback | None = None, + *, + scene_provider: SceneProvider | None = None, + ) -> None: + if (observer is None) == (scene_provider is None): + raise ValueError( + "Exactly one of observer or scene_provider must be supplied." + ) + if observer is not None and not callable(observer): + raise TypeError("observer must be callable or None.") + if scene_provider is not None and not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider or be None.") + self._observer = observer + self._scene_provider = scene_provider + + def collect( + self, + queries: tuple[EffectEvidenceQueryValue, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + """Collect synchronized joint state for exact scene addresses.""" + if not isinstance(context, EffectEvidenceCollectionContext): + raise TypeError("context must be an EffectEvidenceCollectionContext.") + if not isinstance(queries, tuple) or not queries: + raise ValueError("queries must be a non-empty tuple.") + owned_queries = tuple(self._validate_query(query) for query in queries) + if len({query.evidence_id for query in owned_queries}) != len(owned_queries): + raise ValueError("queries must have unique evidence IDs.") + + observations: dict[object, JointStateObservation] = {} + batches: dict[str, JointStateEvidenceBatch] = {} + scene_snapshot: SceneSnapshot | None = None + if self._scene_provider is not None: + scene_snapshot = self._scene_provider.snapshot( + timestamp=context.timestamp, + env_ids=context.env_ids.clone(), + ) + if not isinstance(scene_snapshot, SceneSnapshot): + raise TypeError("scene_provider.snapshot() must return SceneSnapshot.") + if scene_snapshot.timestamp != context.timestamp: + raise ValueError( + "Scene snapshot timestamp must match the evidence tick." + ) + for query in owned_queries: + address = query.source.address + assert type(address) is ArticulationJointEvidenceAddress + fingerprint = address.address_fingerprint + observation = observations.get(fingerprint) + if observation is None: + supplied = ( + self._observe_scene_snapshot(query, context, scene_snapshot) + if scene_snapshot is not None + else self._observer(query.snapshot(), context.snapshot()) + ) + if not isinstance(supplied, JointStateObservation): + raise TypeError( + "Articulation observers must return JointStateObservation." + ) + observation = supplied + observations[fingerprint] = observation + self._validate_observation(query, observation, context) + assert observation.valid is not None + batches[query.evidence_id] = JointStateEvidenceBatch( + query.evidence_id, + observation.positions, + observation.velocities, + observation.valid, + observation.acquisition_errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + return MappingProxyType(batches) + + @staticmethod + def _observe_scene_snapshot( + query: JointStateEvidenceQuery, + context: EffectEvidenceCollectionContext, + snapshot: SceneSnapshot, + ) -> JointStateObservation: + """Adapt one typed live scene joint into raw effect evidence.""" + address = query.source.address + assert type(address) is ArticulationJointEvidenceAddress + state = snapshot.get_articulation_joint_state( + address.articulation_id, + address.joint_id, + ) + batch_size = int(context.env_ids.numel()) + if state is None: + width = int(query.clause.target_position.shape[-1]) + return JointStateObservation( + positions=torch.zeros( + (batch_size, width), + dtype=query.clause.target_position.dtype, + device=context.env_ids.device, + ), + valid=torch.zeros( + batch_size, + dtype=torch.bool, + device=context.env_ids.device, + ), + acquisition_errors=( + f"Scene snapshot has no live articulation joint " + f"{(address.articulation_id, address.joint_id)!r}.", + ) + * batch_size, + ) + positions = state.position + if positions.dim() == 1: + positions = positions.unsqueeze(0).expand(batch_size, -1) + if positions.shape[0] != batch_size: + raise ValueError( + "Scene articulation observation rows must match context env_ids." + ) + positions = positions.to(device=context.env_ids.device) + valid = state.valid_mask + if valid is None: + valid = torch.ones( + batch_size, + dtype=torch.bool, + device=context.env_ids.device, + ) + else: + valid = valid.to(device=context.env_ids.device) + errors = tuple( + None if bool(row_valid) else "Scene articulation joint row is invalid." + for row_valid in valid.tolist() + ) + return JointStateObservation( + positions=positions, + valid=valid, + acquisition_errors=errors, + ) + + def _validate_query( + self, + query: EffectEvidenceQueryValue, + ) -> JointStateEvidenceQuery: + """Require one exact joint query and matching canonical address.""" + if type(query) is not JointStateEvidenceQuery: + raise TypeError( + "SceneArticulationEvidenceProvider accepts only " + "JointStateEvidenceQuery values." + ) + source = query.source + if (source.provider_id, source.revision) != ( + self.provider_id, + self.revision, + ): + raise ValueError("Query does not select this exact provider version.") + if type(source.address) is not ArticulationJointEvidenceAddress: + raise TypeError( + "Scene articulation evidence requires " + "ArticulationJointEvidenceAddress." + ) + expectation = query.expectation + if type(expectation) is not ArticulationJointStateExpectation: + raise TypeError( + "Scene articulation evidence requires an " + "ArticulationJointStateExpectation." + ) + if ( + expectation.articulation_id != source.address.articulation_id + or expectation.joint_id != source.address.joint_id + ): + raise ValueError( + "Articulation evidence address must exactly match its typed " + "state expectation." + ) + return query.snapshot() + + @staticmethod + def _validate_observation( + query: JointStateEvidenceQuery, + observation: JointStateObservation, + context: EffectEvidenceCollectionContext, + ) -> None: + """Require callback rows/device/width to match the synchronized query.""" + if observation.positions.shape[0] != context.env_ids.numel(): + raise ValueError( + "Articulation observation rows must match context env_ids." + ) + if observation.positions.device != context.env_ids.device: + raise ValueError( + "Articulation observations and context env_ids must share a device." + ) + target_width = int(query.clause.target_position.shape[-1]) + if observation.positions.shape[1] != target_width: + raise ValueError( + f"Joint observation width {observation.positions.shape[1]} does " + f"not match query target width {target_width}." + ) + + +@runtime_checkable +class ControlPartRobotEvidenceSource(Protocol): + """Minimal simulation robot API used by the built-in provider.""" + + def get_qpos(self, name: str | None = None, target: bool = False) -> torch.Tensor: + """Return current robot or control-part joint positions.""" + + def get_qvel(self, name: str | None = None, target: bool = False) -> torch.Tensor: + """Return current robot or control-part joint velocities.""" + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: Sequence[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + """Return the selected endpoint pose for current joint positions.""" + + +class ControlPartSimulationEvidenceProvider(EffectEvidenceProvider): + """Built-in simulation acquisition for control-part evidence addresses. + + Pose evidence is computed as ``inverse(object_pose) @ endpoint_pose`` from + one scene snapshot and :meth:`Robot.compute_fk`. Joint evidence reads the + control part's measured positions and velocities. Contact, constraint, + force, and wrench signals are backend-specific, so callers inject raw + observation callbacks. An omitted callback yields explicit invalid rows; + the effect monitor can then retry until its normal deadline. + """ + + provider_id = CONTROL_PART_EVIDENCE_PROVIDER_ID + revision = CONTROL_PART_EVIDENCE_PROVIDER_REVISION + + def __init__( + self, + robot: ControlPartRobotEvidenceSource, + *, + scene_provider: SceneProvider | None = None, + contact_observer: BinaryObservationCallback | None = None, + constraint_observer: BinaryObservationCallback | None = None, + force_observer: ScalarObservationCallback | None = None, + wrench_observer: ScalarObservationCallback | None = None, + ) -> None: + if not isinstance(robot, ControlPartRobotEvidenceSource): + raise TypeError("robot must implement ControlPartRobotEvidenceSource.") + if scene_provider is not None and not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider or be None.") + for name, callback in ( + ("contact_observer", contact_observer), + ("constraint_observer", constraint_observer), + ("force_observer", force_observer), + ("wrench_observer", wrench_observer), + ): + if callback is not None and not callable(callback): + raise TypeError(f"{name} must be callable or None.") + self._robot = robot + self._scene_provider = scene_provider + self._binary_observers = { + BinaryEvidenceKind.CONTACT: contact_observer, + BinaryEvidenceKind.CONSTRAINT: constraint_observer, + } + self._scalar_observers = { + ScalarEvidenceKind.FORCE: force_observer, + ScalarEvidenceKind.WRENCH: wrench_observer, + } + + def collect( + self, + queries: tuple[EffectEvidenceQueryValue, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire all supplied control-part queries at one observation tick.""" + if not isinstance(context, EffectEvidenceCollectionContext): + raise TypeError("context must be an EffectEvidenceCollectionContext.") + if not isinstance(queries, tuple) or not queries: + raise ValueError("queries must be a non-empty tuple.") + owned_queries = tuple( + self._validate_and_snapshot_query(query) for query in queries + ) + if len({query.evidence_id for query in owned_queries}) != len(owned_queries): + raise ValueError("queries must have unique evidence IDs.") + + pose_queries = tuple( + query for query in owned_queries if type(query) is PoseRelationEvidenceQuery + ) + scene_snapshot = self._capture_scene(pose_queries, context) + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]] = {} + endpoint_cache: dict[str, torch.Tensor] = {} + results: dict[str, EffectEvidenceBatch] = {} + for query in owned_queries: + address = query.source.address + assert type(address) is ControlPartEvidenceAddress + if type(query) is PoseRelationEvidenceQuery: + results[query.evidence_id] = self._collect_pose( + query, + address, + context, + scene_snapshot, + joint_cache, + endpoint_cache, + ) + elif type(query) is BinaryEffectEvidenceQuery: + results[query.evidence_id] = self._collect_binary( + query, + address, + context, + ) + elif type(query) is ScalarEffectEvidenceQuery: + results[query.evidence_id] = self._collect_scalar( + query, + address, + context, + ) + elif type(query) is JointStateEvidenceQuery: + results[query.evidence_id] = self._collect_joint_state( + query, + address, + context, + joint_cache, + ) + else: + raise TypeError(f"Unsupported query type {type(query).__name__}.") + return MappingProxyType(results) + + def _validate_and_snapshot_query( + self, + query: EffectEvidenceQueryValue, + ) -> EffectEvidenceQueryValue: + """Require the exact built-in route and a control-part address.""" + if type(query) not in { + PoseRelationEvidenceQuery, + BinaryEffectEvidenceQuery, + ScalarEffectEvidenceQuery, + JointStateEvidenceQuery, + }: + raise TypeError("queries must contain exact typed evidence queries.") + source = query.source + if (source.provider_id, source.revision) != ( + self.provider_id, + self.revision, + ): + raise ValueError("Query does not select this exact provider version.") + if type(source.address) is not ControlPartEvidenceAddress: + raise TypeError( + "ControlPartSimulationEvidenceProvider requires " + "ControlPartEvidenceAddress values." + ) + return query.snapshot() + + def _capture_scene( + self, + queries: tuple[PoseRelationEvidenceQuery, ...], + context: EffectEvidenceCollectionContext, + ) -> SceneSnapshot | None: + """Capture one shared scene snapshot if pose queries need it.""" + if not queries or self._scene_provider is None: + return None + snapshot = self._scene_provider.snapshot( + timestamp=context.timestamp, + env_ids=context.env_ids.clone(), + ) + if not isinstance(snapshot, SceneSnapshot): + raise TypeError("scene_provider.snapshot() must return SceneSnapshot.") + if snapshot.timestamp != context.timestamp: + raise ValueError("Scene snapshot timestamp must match the evidence tick.") + return snapshot + + @staticmethod + def _require_channel( + address: ControlPartEvidenceAddress, + expected: str, + *, + evidence_id: str, + ) -> None: + """Reject clause/address channel mismatches before acquisition.""" + if address.channel != expected: + raise ValueError( + f"Evidence query {evidence_id!r} requires channel {expected!r}, " + f"not {address.channel!r}." + ) + + @staticmethod + def _select_rows( + value: torch.Tensor, context: EffectEvidenceCollectionContext + ) -> torch.Tensor: + """Select simulator rows addressed by the context's integer env IDs.""" + if not isinstance(value, torch.Tensor): + raise TypeError("Robot state accessors must return torch.Tensor values.") + if value.dim() != 2 or value.shape[0] == 0 or value.shape[1] == 0: + raise ValueError("Robot joint state must have non-empty shape (N, J).") + indices = context.env_ids.to(device=value.device) + if bool((indices < 0).any()) or int(indices.max().item()) >= value.shape[0]: + raise ValueError( + "The built-in simulation provider requires env_ids to address " + "valid simulator batch rows." + ) + selected = value.index_select(0, indices) + if selected.device != context.env_ids.device: + raise ValueError( + "Robot evidence and collection env_ids must share a device." + ) + return selected.clone() + + def _joint_state( + self, + control_part: str, + context: EffectEvidenceCollectionContext, + cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + ) -> tuple[torch.Tensor, torch.Tensor]: + """Read and cache measured positions and velocities for one part.""" + cached = cache.get(control_part) + if cached is not None: + return cached[0].clone(), cached[1].clone() + qpos = self._select_rows( + self._robot.get_qpos(name=control_part, target=False), + context, + ) + qvel = self._select_rows( + self._robot.get_qvel(name=control_part, target=False), + context, + ) + if qvel.shape != qpos.shape or qvel.device != qpos.device: + raise ValueError("Robot qvel must match qpos shape and device.") + if not qpos.is_floating_point() or not qvel.is_floating_point(): + raise TypeError("Robot qpos and qvel must use floating-point dtypes.") + cache[control_part] = qpos.clone(), qvel.clone() + return qpos, qvel + + def _endpoint_pose( + self, + control_part: str, + context: EffectEvidenceCollectionContext, + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + endpoint_cache: dict[str, torch.Tensor], + ) -> torch.Tensor: + """Compute and cache one control-part endpoint pose.""" + cached = endpoint_cache.get(control_part) + if cached is not None: + return cached.clone() + qpos, _ = self._joint_state(control_part, context, joint_cache) + pose = self._robot.compute_fk( + qpos=qpos, + name=control_part, + env_ids=context.env_ids.detach().cpu().tolist(), + to_matrix=True, + ) + if not isinstance(pose, torch.Tensor): + raise TypeError("robot.compute_fk() must return a torch.Tensor.") + if pose.shape != (context.env_ids.numel(), 4, 4): + raise ValueError("robot.compute_fk() must return shape (B, 4, 4).") + if pose.device != context.env_ids.device: + raise ValueError( + "Endpoint poses and collection env_ids must share a device." + ) + endpoint_cache[control_part] = pose.clone() + return pose + + @staticmethod + def _pose_entity_id(query: PoseRelationEvidenceQuery) -> str: + """Resolve the canonical scene entity observed by a pose relation.""" + expectation = query.expectation + if type(expectation) is HeldObjectStateExpectation: + return expectation.object_id + if type(expectation) is ArticulationJointStateExpectation: + return expectation.articulation_id + raise ValueError( + "Pose relation evidence requires an expectation with one canonical " + "scene entity." + ) + + def _collect_pose( + self, + query: PoseRelationEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + scene_snapshot: SceneSnapshot | None, + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + endpoint_cache: dict[str, torch.Tensor], + ) -> PoseRelationEvidenceBatch: + """Collect object-to-endpoint transforms from scene and FK state.""" + self._require_channel( + address, + POSE_RELATION_EFFECT_CHANNEL, + evidence_id=query.evidence_id, + ) + if scene_snapshot is None: + return self._invalid_pose( + query.evidence_id, + context, + "No scene provider is configured for pose-relation evidence.", + ) + entity_id = self._pose_entity_id(query) + try: + state = scene_snapshot.entities[entity_id] + except KeyError as exc: + raise KeyError( + f"Pose evidence references missing scene entity {entity_id!r}." + ) from exc + object_pose = state.pose + batch_size = int(context.env_ids.numel()) + if object_pose.shape == (4, 4): + object_pose = object_pose.unsqueeze(0).expand(batch_size, -1, -1) + if object_pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Scene entity {entity_id!r} pose must have shape (B, 4, 4)." + ) + endpoint_pose = self._endpoint_pose( + address.control_part, + context, + joint_cache, + endpoint_cache, + ) + object_pose = object_pose.to( + device=endpoint_pose.device, + dtype=endpoint_pose.dtype, + ) + relative = torch.bmm(pose_inv(object_pose), endpoint_pose) + valid = torch.full( + (batch_size,), + state.confidence > 0.0, + dtype=torch.bool, + device=relative.device, + ) + errors: tuple[str | None, ...] + if bool(valid.all()): + errors = (None,) * batch_size + else: + errors = ( + f"Scene entity {entity_id!r} has zero observation confidence.", + ) * batch_size + return PoseRelationEvidenceBatch( + query.evidence_id, + relative, + valid, + errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + def _collect_binary( + self, + query: BinaryEffectEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectEvidenceBatch: + """Collect callback-provided contact or constraint state.""" + expected_channel = ( + CONTACT_EFFECT_CHANNEL + if query.clause.evidence_kind is BinaryEvidenceKind.CONTACT + else CONSTRAINT_EFFECT_CHANNEL + ) + self._require_channel(address, expected_channel, evidence_id=query.evidence_id) + callback = self._binary_observers[query.clause.evidence_kind] + if callback is None: + return self._invalid_binary( + query, + context, + f"No {query.clause.evidence_kind.value} observation callback is configured.", + ) + observation = callback(query.snapshot(), context.snapshot()) + if not isinstance(observation, BinaryEffectObservation): + raise TypeError( + "Binary observation callbacks must return BinaryEffectObservation." + ) + self._validate_callback_rows(observation.values, context) + assert observation.valid is not None + return BinaryEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + observation.values, + observation.valid, + observation.acquisition_errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + def _collect_scalar( + self, + query: ScalarEffectEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + ) -> ScalarEffectEvidenceBatch: + """Collect callback-provided force or wrench magnitude.""" + self._require_channel( + address, FORCE_EFFECT_CHANNEL, evidence_id=query.evidence_id + ) + callback = self._scalar_observers[query.clause.evidence_kind] + if callback is None: + return self._invalid_scalar( + query, + context, + f"No {query.clause.evidence_kind.value} observation callback is configured.", + ) + observation = callback(query.snapshot(), context.snapshot()) + if not isinstance(observation, ScalarEffectObservation): + raise TypeError( + "Scalar observation callbacks must return ScalarEffectObservation." + ) + self._validate_callback_rows(observation.values, context) + assert observation.valid is not None + return ScalarEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + observation.values, + observation.valid, + observation.acquisition_errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + def _collect_joint_state( + self, + query: JointStateEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + ) -> JointStateEvidenceBatch: + """Collect measured control-part joint positions and velocities.""" + self._require_channel( + address, + JOINT_STATE_EFFECT_CHANNEL, + evidence_id=query.evidence_id, + ) + qpos, qvel = self._joint_state(address.control_part, context, joint_cache) + target_width = int(query.clause.target_position.shape[-1]) + if qpos.shape[1] != target_width: + raise ValueError( + f"Joint evidence width {qpos.shape[1]} does not match query target " + f"width {target_width}." + ) + batch_size = int(context.env_ids.numel()) + return JointStateEvidenceBatch( + query.evidence_id, + qpos, + qvel, + torch.ones(batch_size, dtype=torch.bool, device=qpos.device), + (None,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + @staticmethod + def _validate_callback_rows( + values: torch.Tensor, + context: EffectEvidenceCollectionContext, + ) -> None: + """Require callback values to follow the synchronized context rows.""" + if values.shape != context.env_ids.shape: + raise ValueError("Observation callback rows must match context env_ids.") + if values.device != context.env_ids.device: + raise ValueError( + "Observation callback values and context env_ids must share a device." + ) + + @staticmethod + def _invalid_pose( + evidence_id: str, + context: EffectEvidenceCollectionContext, + message: str, + ) -> PoseRelationEvidenceBatch: + """Create explicit invalid rows for unavailable pose acquisition.""" + batch_size = int(context.env_ids.numel()) + poses = torch.eye( + 4, + dtype=torch.float32, + device=context.env_ids.device, + ).expand(batch_size, -1, -1) + return PoseRelationEvidenceBatch( + evidence_id, + poses, + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + (message,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + @staticmethod + def _invalid_binary( + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + message: str, + ) -> BinaryEffectEvidenceBatch: + """Create explicit invalid rows for an unavailable binary channel.""" + batch_size = int(context.env_ids.numel()) + return BinaryEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + (message,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + @staticmethod + def _invalid_scalar( + query: ScalarEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + message: str, + ) -> ScalarEffectEvidenceBatch: + """Create explicit invalid rows for an unavailable scalar channel.""" + batch_size = int(context.env_ids.numel()) + return ScalarEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + torch.zeros(batch_size, dtype=torch.float32, device=context.env_ids.device), + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + (message,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + +__all__ = [ + "ArticulationJointObservationCallback", + "BinaryEffectEvidenceQuery", + "BinaryEffectObservation", + "BinaryObservationCallback", + "ControlPartRobotEvidenceSource", + "ControlPartSimulationEvidenceProvider", + "EffectEvidenceCollectionContext", + "EffectEvidenceCollector", + "EffectEvidenceProvider", + "EffectEvidenceProviderRegistry", + "EffectEvidenceQuery", + "EffectEvidenceQueryValue", + "JointStateEvidenceQuery", + "JointStateObservation", + "PoseRelationEvidenceQuery", + "ScalarEffectEvidenceQuery", + "ScalarEffectObservation", + "ScalarObservationCallback", + "SceneArticulationEvidenceProvider", + "build_effect_evidence_queries", +] diff --git a/embodichain/lab/sim/skills/integration.py b/embodichain/lab/sim/skills/integration.py index 68e0134cf..08c1bdb6e 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -25,6 +25,7 @@ from embodichain.lab.sim.atomic_actions import ( AtomicActionEngine, + DynamicCollisionMode, DisjointResourceSlots, DisjointSlotEndpoints, SkillResourceSlot, @@ -32,6 +33,7 @@ from .calls import ( HandOver, + OperateArticulation, Pick, Place, RegisteredSemanticCall, @@ -50,6 +52,7 @@ SkillPolicyPreset, ) from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, GRASP_AFFORDANCE_CAPABILITY, PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, @@ -407,7 +410,13 @@ class LinkedSemanticCall: affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) def __post_init__(self) -> None: - if type(self.call) not in (Pick, Place, HandOver, RegisteredSemanticCall): + if type(self.call) not in ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ): raise TypeError("call must be an exact supported semantic call value.") if type(self.descriptor) is not SemanticCallDescriptor: raise TypeError("descriptor must be exactly SemanticCallDescriptor.") @@ -429,16 +438,42 @@ def __post_init__(self) -> None: object.__setattr__(self, "affordances", MappingProxyType(normalized)) -@dataclass(frozen=True, slots=True) +@dataclass(frozen=True, slots=True, init=False) class BoundSemanticCall: - """Call linked to one installed engine/profile combination.""" + """Factory-owned call linked to one installed engine/profile combination.""" linked: LinkedSemanticCall binding: ResolvedSkillBinding preset: SkillPolicyPreset _robot_profile: BoundRobotSkillProfile = field(repr=False, compare=False) - def __post_init__(self) -> None: + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :class:`BoundSemanticIntegration`.""" + del args, kwargs + raise TypeError( + "BoundSemanticCall values are created by " + "BoundSemanticIntegration.link_call()." + ) + + @classmethod + def _create( + cls, + *, + linked: LinkedSemanticCall, + binding: ResolvedSkillBinding, + preset: SkillPolicyPreset, + robot_profile: BoundRobotSkillProfile, + ) -> BoundSemanticCall: + """Create and validate one engine/profile-owned result.""" + instance = object.__new__(cls) + object.__setattr__(instance, "linked", linked) + object.__setattr__(instance, "binding", binding) + object.__setattr__(instance, "preset", preset) + object.__setattr__(instance, "_robot_profile", robot_profile) + instance._validate() + return instance + + def _validate(self) -> None: """Validate the static and live ownership links.""" if not isinstance(self.linked, LinkedSemanticCall): raise TypeError("linked must be a LinkedSemanticCall.") @@ -489,6 +524,29 @@ def __post_init__(self) -> None: raise TypeError("robot_profile must be exactly RobotSkillProfile.") if type(self.call_catalog) is not SemanticCallCatalog: raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + known_semantic_ids = set(self.call_catalog.descriptors) + for preset_id, preset in self.robot_profile.presets.items(): + unknown_monitor_ids = sorted( + set(preset.effect_monitors).difference(known_semantic_ids) + ) + if unknown_monitor_ids: + semantic_id = unknown_monitor_ids[0] + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_effect_monitor_call", + ( + "integration", + "robot_profile", + "presets", + preset_id, + "effect_monitors", + semantic_id, + ), + f"Effect monitor configuration references unknown semantic " + f"call {semantic_id!r}.", + tuple(self.call_catalog.descriptors), + ) + ) if self.runtime_preset is not None: _validate_identifier( self.runtime_preset, @@ -585,6 +643,24 @@ def link_call( ) normalized_call = replace(call, object=object_ref) affordances["receiver_grasp"] = grasp + elif isinstance(call, OperateArticulation): + articulation_ref = self.scene.resolve( + call.articulation, + expected_type=SceneArticulationRef, + path=(*path, "articulation"), + ) + handle = self.scene.resolve_affordance( + articulation_ref, + capability=ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + explicit=call.handle, + path=(*path, "handle"), + ) + normalized_call = replace( + call, + articulation=articulation_ref, + handle=handle, + ) + affordances["handle"] = handle elif isinstance(call, RegisteredSemanticCall): normalized_call = replace( call, @@ -617,6 +693,34 @@ def link_call( affordances=affordances, ) + def _selects_preset(self, preset_id: str) -> bool: + """Return whether one preset is reachable through this integration. + + Args: + preset_id: Stable policy preset identifier. + + Returns: + ``True`` when the integration-wide override or at least one + catalogued target skill can resolve to ``preset_id`` through its + per-skill or profile-default selection. This is intentionally a + conservative integration-level check, not a concrete-program + reachability analysis. + """ + _validate_identifier(preset_id, field_name="preset_id") + if self.runtime_preset is not None: + return self.runtime_preset == preset_id + skill_ids = { + descriptor.skill_id for descriptor in self.call_catalog.descriptors.values() + } + return any( + self.robot_profile.skill_presets.get( + skill_id, + self.robot_profile.default_preset, + ) + == preset_id + for skill_id in skill_ids + ) + def _resolve_declared_preset( self, descriptor: SemanticCallDescriptor, @@ -923,6 +1027,12 @@ def bind( ) -> BoundSemanticIntegration: """Validate live scene and robot bindings without observing or planning.""" self.scene.validate_registry(scene_registry) + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + self._validate_safe_dynamic_collision_policy( + scene_registry=scene_registry, + engine=engine, + ) try: bound_profile = engine.bind_skill_profile( self.robot_profile, @@ -943,6 +1053,53 @@ def bind( engine=engine, ) + def _validate_safe_dynamic_collision_policy( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> None: + """Fail before observation when selected safe planning cannot be strict.""" + if not scene_registry.dynamic_collision_entity_ids or not self._selects_preset( + "safe" + ): + return + preset = self.robot_profile.presets["safe"] + policy_path: tuple[PathPart, ...] = ( + "integration", + "robot_profile", + "presets", + "safe", + "motion_policy", + ) + if preset.motion_policy.strategy != "motion_gen": + raise SemanticValidationError( + SemanticDiagnostic( + "safe_dynamic_collision_unsupported", + (*policy_path, "strategy"), + "The 'safe' preset requires strategy='motion_gen' when the " + "scene registry declares dynamic collision entities.", + ("motion_gen",), + ) + ) + if ( + getattr( + engine.motion_generator, + "supports_dynamic_collision_world", + False, + ) + is not True + ): + raise SemanticValidationError( + SemanticDiagnostic( + "safe_dynamic_collision_unsupported", + (*policy_path, "dynamic_collision_mode"), + "The 'safe' preset requires an active planner with dynamic " + "collision-world support for the registered dynamic entities " + f"{scene_registry.dynamic_collision_entity_ids!r}.", + ) + ) + class BoundSemanticIntegration: """Live-installed, still side-effect-free semantic integration link.""" @@ -963,6 +1120,11 @@ def __init__( raise TypeError("robot_profile must be exactly BoundRobotSkillProfile.") if not isinstance(engine, AtomicActionEngine): raise TypeError("engine must be an AtomicActionEngine.") + manifest.scene.validate_registry(scene_registry) + manifest._validate_safe_dynamic_collision_policy( + scene_registry=scene_registry, + engine=engine, + ) if robot_profile.engine is not engine: raise ValueError("robot_profile belongs to a different engine.") if engine.skill_profile is not robot_profile: @@ -1043,11 +1205,26 @@ def link_call( str(exc), ) ) from exc - return BoundSemanticCall( + if ( + linked.preset_id == "safe" + and self._scene_registry.dynamic_collision_entity_ids + ): + preset = SkillPolicyPreset( + preset_id=preset.preset_id, + schema_version=preset.schema_version, + motion_policy=replace( + preset.motion_policy, + dynamic_collision_mode=DynamicCollisionMode.REQUIRED, + ), + recovery_policy=preset.recovery_policy, + runner_cfg=preset.runner_cfg, + effect_monitors=preset.effect_monitors, + ) + return BoundSemanticCall._create( linked=linked, binding=binding, preset=preset, - _robot_profile=self._robot_profile, + robot_profile=self._robot_profile, ) diff --git a/embodichain/lab/sim/skills/parallel.py b/embodichain/lab/sim/skills/parallel.py new file mode 100644 index 000000000..fe6d06d54 --- /dev/null +++ b/embodichain/lab/sim/skills/parallel.py @@ -0,0 +1,354 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Deterministic resource, timing, and state contracts for parallel skills.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from types import MappingProxyType +from typing import Mapping + +import torch + +from embodichain.lab.sim.atomic_actions import ( + RuntimeCommandFrame, + StateDelta, + TaskState, + TimedCommandSequence, +) + +from .profiles import ResourceClaim + + +def _validate_identifier(value: str, *, field_name: str) -> None: + """Validate one non-empty stable identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty stable identifier.") + + +@dataclass(frozen=True, slots=True) +class ParallelTimingPolicy: + """Strict environment-grid policy for one parallel barrier. + + Version 2 deliberately rejects fractional frame durations. Padding repeats + the last controller target, which is a deterministic position/tool hold; + no interpolation is hidden inside the scheduler. + """ + + step_dt: float + tolerance: float = 1.0e-6 + + def __post_init__(self) -> None: + for field_name in ("step_dt", "tolerance"): + value = getattr(self, field_name) + if not isinstance(value, (int, float)) or isinstance(value, bool): + raise TypeError(f"{field_name} must be a number.") + value = float(value) + if not math.isfinite(value) or value <= 0.0: + raise ValueError(f"{field_name} must be finite and positive.") + object.__setattr__(self, field_name, value) + + +@dataclass(frozen=True, slots=True, eq=False) +class ParallelBranchPlan: + """One independently planned lane entering a common barrier.""" + + branch_id: str + claim: ResourceClaim + commands: TimedCommandSequence + expected_effects: StateDelta = StateDelta() + + def __post_init__(self) -> None: + _validate_identifier(self.branch_id, field_name="branch_id") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + if not isinstance(self.commands, TimedCommandSequence): + raise TypeError("commands must be a TimedCommandSequence.") + if not isinstance(self.expected_effects, StateDelta): + raise TypeError("expected_effects must be a StateDelta.") + object.__setattr__(self, "commands", self.commands.snapshot()) + object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + + +class ParallelConflictError(ValueError): + """Raised before execution when parallel lanes claim overlapping resources.""" + + +class ParallelTimingError(ValueError): + """Raised when a command sequence cannot use the environment step grid.""" + + +class ParallelStateConflictError(ValueError): + """Raised when successful lanes update the same symbolic state row.""" + + +def validate_parallel_claims(branches: tuple[ParallelBranchPlan, ...]) -> None: + """Reject duplicate IDs and every pair of overlapping physical claims.""" + if not isinstance(branches, tuple) or len(branches) < 2: + raise ValueError("Parallel execution requires at least two branch plans.") + if not all(type(branch) is ParallelBranchPlan for branch in branches): + raise TypeError("branches must contain exact ParallelBranchPlan values.") + branch_ids = tuple(branch.branch_id for branch in branches) + if len(set(branch_ids)) != len(branch_ids): + raise ParallelConflictError("Parallel branch IDs must be unique.") + for index, left in enumerate(branches): + for right in branches[index + 1 :]: + if left.claim.conflicts_with(right.claim): + raise ParallelConflictError( + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} have overlapping physical claims." + ) + + +def _validate_grid_frame( + branch_id: str, + frame_index: int, + frame: RuntimeCommandFrame, + policy: ParallelTimingPolicy, +) -> None: + """Require one frame to occupy exactly one environment control step.""" + durations = frame.hold_duration + expected = torch.full_like(durations, policy.step_dt) + if not torch.allclose(durations, expected, atol=policy.tolerance, rtol=0.0): + values = sorted({float(value) for value in durations.detach().cpu().tolist()}) + raise ParallelTimingError( + f"Parallel branch {branch_id!r} frame {frame_index} has durations " + f"{values}; every emitted frame must equal step_dt={policy.step_dt}." + ) + + +def align_parallel_commands( + branches: tuple[ParallelBranchPlan, ...], + policy: ParallelTimingPolicy, +) -> TimedCommandSequence: + """Merge disjoint lanes on one grid and hold-pad shorter trajectories. + + Each merged frame is a single transport transaction. Runtime frame + validation independently rejects duplicate destinations or joint overlap, + defending against an incorrect custom ``ResourceClaim`` implementation. + """ + if not isinstance(policy, ParallelTimingPolicy): + raise TypeError("policy must be a ParallelTimingPolicy.") + validate_parallel_claims(branches) + first = branches[0].commands + if any( + branch.commands.device != first.device + or not torch.equal(branch.commands.env_ids, first.env_ids) + for branch in branches[1:] + ): + raise ParallelTimingError( + "Parallel command sequences must share ordered env_ids and device." + ) + if any(branch.commands.frame_count == 0 for branch in branches): + raise ParallelTimingError( + "Parallel branches must emit at least one command frame." + ) + for branch in branches: + for frame_index, frame in enumerate(branch.commands.frames): + _validate_grid_frame(branch.branch_id, frame_index, frame, policy) + + frame_count = max(branch.commands.frame_count for branch in branches) + merged: list[RuntimeCommandFrame] = [] + for frame_index in range(frame_count): + lane_frames = tuple( + branch.commands.frames[min(frame_index, branch.commands.frame_count - 1)] + for branch in branches + ) + reference_mask = lane_frames[0].active_mask + if any( + not torch.equal(frame.active_mask, reference_mask) + for frame in lane_frames[1:] + ): + raise ParallelTimingError( + "Parallel lanes cannot merge different per-environment active " + f"masks at frame {frame_index}; RuntimeCommandFrame owns one " + "mask for every command in the transaction." + ) + merged.append( + RuntimeCommandFrame( + commands=tuple( + command for frame in lane_frames for command in frame.commands + ), + active_mask=reference_mask, + env_ids=first.env_ids, + hold_duration=torch.full( + (first.batch_size,), + policy.step_dt, + dtype=lane_frames[0].hold_duration.dtype, + device=first.device, + ), + ) + ) + return TimedCommandSequence(frames=tuple(merged), env_ids=first.env_ids) + + +def _delta_keys(delta: StateDelta) -> frozenset[tuple[str, object]]: + """Return domain-qualified symbolic keys written by one delta.""" + return frozenset( + [("held", key) for key in delta.held_object_updates] + + [("coordinated", key) for key in delta.coordinated_held_object_updates] + + [("articulation", key) for key in delta.articulation_joint_updates] + ) + + +def merge_parallel_effects( + state: TaskState, + effects: Mapping[str, tuple[StateDelta, torch.Tensor]], +) -> TaskState: + """Apply disjoint branch effects with deterministic row-local conflict checks. + + Args: + state: Verified task state before the barrier. + effects: Branch ID to ``(delta, verified_success_mask)``. + + Returns: + New verified task state after all non-conflicting updates. + """ + if not isinstance(state, TaskState): + raise TypeError("state must be a TaskState.") + if not isinstance(effects, Mapping) or not effects: + raise ValueError("effects must be a non-empty branch mapping.") + normalized: dict[str, tuple[StateDelta, torch.Tensor]] = {} + for branch_id, value in effects.items(): + _validate_identifier(branch_id, field_name="effect branch IDs") + if not isinstance(value, tuple) or len(value) != 2: + raise TypeError("effect entries must be (StateDelta, success_mask) pairs.") + delta, mask = value + if not isinstance(delta, StateDelta): + raise TypeError("effect deltas must be StateDelta values.") + if ( + not isinstance(mask, torch.Tensor) + or mask.dtype != torch.bool + or mask.shape != (state.batch_size,) + or mask.device != state.device + ): + raise ValueError("effect masks must match TaskState batch and device.") + normalized[branch_id] = delta.snapshot(), mask.clone() + + entries = tuple(normalized.items()) + for index, (left_id, (left_delta, left_mask)) in enumerate(entries): + for right_id, (right_delta, right_mask) in entries[index + 1 :]: + overlapping_keys = _delta_keys(left_delta) & _delta_keys(right_delta) + overlapping_rows = left_mask & right_mask + if overlapping_keys and overlapping_rows.any(): + raise ParallelStateConflictError( + f"Parallel effects {left_id!r} and {right_id!r} write " + f"the same symbolic keys on rows " + f"{overlapping_rows.nonzero().flatten().tolist()}." + ) + result = state + for branch_id in sorted(normalized): + delta, mask = normalized[branch_id] + result = delta.apply(result, mask) + return result + + +@dataclass(frozen=True, slots=True, eq=False) +class ParallelBarrierUpdate: + """Per-row barrier status after one synchronized lane observation.""" + + completed_mask: torch.Tensor + failure_mask: torch.Tensor + cancellation_masks: Mapping[str, torch.Tensor] + + def __post_init__(self) -> None: + if ( + not isinstance(self.completed_mask, torch.Tensor) + or self.completed_mask.dtype != torch.bool + or self.completed_mask.dim() != 1 + ): + raise ValueError("completed_mask must be a one-dimensional bool tensor.") + if ( + not isinstance(self.failure_mask, torch.Tensor) + or self.failure_mask.dtype != torch.bool + or self.failure_mask.shape != self.completed_mask.shape + or self.failure_mask.device != self.completed_mask.device + ): + raise ValueError("failure_mask must match completed_mask.") + cancellations: dict[str, torch.Tensor] = {} + for branch_id, mask in self.cancellation_masks.items(): + _validate_identifier(branch_id, field_name="cancellation branch IDs") + if ( + not isinstance(mask, torch.Tensor) + or mask.dtype != torch.bool + or mask.shape != self.completed_mask.shape + or mask.device != self.completed_mask.device + ): + raise ValueError("cancellation masks must match completed_mask.") + cancellations[branch_id] = mask.clone() + object.__setattr__(self, "completed_mask", self.completed_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__( + self, + "cancellation_masks", + MappingProxyType(cancellations), + ) + + +def resolve_parallel_barrier( + *, + pending_masks: Mapping[str, torch.Tensor], + success_masks: Mapping[str, torch.Tensor], + failure_masks: Mapping[str, torch.Tensor], +) -> ParallelBarrierUpdate: + """Apply deterministic per-row fail-fast semantics at one barrier update.""" + branch_ids = tuple(pending_masks) + if ( + not branch_ids + or set(success_masks) != set(branch_ids) + or set(failure_masks) != set(branch_ids) + ): + raise ValueError( + "pending, success, and failure mappings must share branch IDs." + ) + reference = pending_masks[branch_ids[0]] + if not isinstance(reference, torch.Tensor): + raise TypeError("barrier masks must be torch.Tensor values.") + for mapping in (pending_masks, success_masks, failure_masks): + for mask in mapping.values(): + if ( + not isinstance(mask, torch.Tensor) + or mask.dtype != torch.bool + or mask.shape != reference.shape + or mask.device != reference.device + ): + raise ValueError("all barrier masks must share bool shape and device.") + failed = torch.stack(tuple(failure_masks.values()), dim=0).any(dim=0) + succeeded_all = torch.stack(tuple(success_masks.values()), dim=0).all(dim=0) + cancellations = { + branch_id: failed & pending_masks[branch_id] for branch_id in branch_ids + } + return ParallelBarrierUpdate( + completed_mask=succeeded_all | failed, + failure_mask=failed, + cancellation_masks=cancellations, + ) + + +__all__ = [ + "ParallelBarrierUpdate", + "ParallelBranchPlan", + "ParallelConflictError", + "ParallelStateConflictError", + "ParallelTimingError", + "ParallelTimingPolicy", + "align_parallel_commands", + "merge_parallel_effects", + "resolve_parallel_barrier", + "validate_parallel_claims", +] diff --git a/embodichain/lab/sim/skills/parallel_runtime.py b/embodichain/lab/sim/skills/parallel_runtime.py new file mode 100644 index 000000000..235bb7e18 --- /dev/null +++ b/embodichain/lab/sim/skills/parallel_runtime.py @@ -0,0 +1,1487 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Branch-local semantic execution joined by one deterministic barrier.""" + +from __future__ import annotations + +from collections.abc import Hashable, Mapping +from dataclasses import dataclass, field +import math +from types import MappingProxyType +from typing import Protocol, runtime_checkable + +import torch + +from embodichain.lab.sim.atomic_actions import ( + CommandAcknowledgement, + CommandSink, + ExecutionClock, + PlanningContext, + RuntimeCommandFrame, + RuntimeEndpointTarget, + StateDelta, + TaskState, + TimedCommandSequence, +) + +from .calls import SemanticCallSpec +from .compiler import SemanticSkillCompiler +from .effects import SymbolicStateKey +from .integration import ( + PathPart, + SemanticDiagnostic, + SemanticValidationError, +) +from .parallel import ( + ParallelBranchPlan, + ParallelTimingPolicy, + align_parallel_commands, + merge_parallel_effects, + resolve_parallel_barrier, +) +from .profiles import ResourceClaim +from .runtime import SkillResult, SkillRuntime, SkillStatus, task_state_to_metadata + + +def _validate_identifier(value: str, *, field_name: str) -> None: + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty stable identifier.") + + +def _snapshot_target(target: RuntimeEndpointTarget) -> RuntimeEndpointTarget: + snapshot = target.snapshot() + if type(snapshot) is not type(target) or snapshot is target: + raise TypeError("Runtime target snapshots must be independent exact values.") + return snapshot + + +def _target_fingerprint(target: RuntimeEndpointTarget) -> Hashable: + """Return one validated target address and safe-hold fingerprint.""" + fingerprint = target.address_fingerprint + try: + hash(fingerprint) + except TypeError as exc: + raise TypeError( + "RuntimeEndpointTarget.address_fingerprint must be hashable." + ) from exc + return fingerprint + + +@runtime_checkable +class ParallelBranchRuntime(Protocol): + """Minimal branch-local runtime surface required by the coordinator.""" + + @property + def result(self) -> SkillResult: + """Return the current immutable branch result.""" + + def start( + self, + *calls: SemanticCallSpec, + workflow_id: str, + eligible_mask: torch.Tensor | None = None, + ) -> SkillResult: + """Start one branch-local semantic workflow.""" + + def step(self) -> SkillResult: + """Advance the branch by one due runtime cycle.""" + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + """Remove peer-failed rows while other rows continue.""" + + def cancel(self, reason: str) -> SkillResult: + """Cancel the complete branch and apply its safe stop.""" + + +@runtime_checkable +class ParallelCommandSafetyValidator(Protocol): + """Fail-closed physical-safety boundary for one merged command tick. + + Resource claims prevent controller arbitration conflicts but cannot prove + that independently generated robot motions are collision-free when + executed together. Environment integrations must install a validator + backed by their authoritative robot/collision model before parallel + commands can leave the coordinator. + """ + + def validate( + self, + *, + branch_frames: Mapping[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + """Raise when the synchronized command is not physically safe.""" + + +class ParallelSafetyError(RuntimeError): + """Raised when physical parallel-command safety cannot be established.""" + + +class ParallelLaneCommandSink: + """Acknowledge one branch locally and expose its frame to a coordinator. + + The coordinator is the only object allowed to forward commands to the real + transport. A lane retains its last frame so shorter or temporarily waiting + branches use deterministic hold-last padding. + """ + + def __init__(self) -> None: + self._fresh_frame: RuntimeCommandFrame | None = None + self._last_frame: RuntimeCommandFrame | None = None + self._hold_requests: list[ + tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext] + ] = [] + self._cancel_targets: tuple[RuntimeEndpointTarget, ...] = () + + @property + def last_frame(self) -> RuntimeCommandFrame | None: + """Return an owned hold-last frame, if this lane has sent one.""" + return None if self._last_frame is None else self._last_frame.snapshot() + + @property + def hold_request( + self, + ) -> tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext | None]: + """Return all pending targets and their latest planning context.""" + targets: dict[Hashable, RuntimeEndpointTarget] = {} + context: PlanningContext | None = None + for requested, request_context in self._hold_requests: + for target in requested: + targets[_target_fingerprint(target)] = target + context = request_context + return ( + tuple(_snapshot_target(target) for target in targets.values()), + context, + ) + + @property + def cancel_targets(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return target snapshots from the most recent cancel request.""" + return tuple(_snapshot_target(target) for target in self._cancel_targets) + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Capture exactly one fresh frame for the current coordinator tick.""" + del timeout + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") + if self._fresh_frame is not None: + raise RuntimeError( + "A parallel lane emitted multiple command frames before drain." + ) + self._fresh_frame = command.snapshot() + self._last_frame = command.snapshot() + return CommandAcknowledgement.accepted_ack("buffered by parallel lane") + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Capture a target-scoped hold; hold-last remains the grid command.""" + del timeout + if not isinstance(context, PlanningContext): + raise TypeError("context must be a PlanningContext.") + self._hold_requests.append( + ( + tuple(_snapshot_target(target) for target in targets), + context, + ) + ) + return CommandAcknowledgement.accepted_ack("buffered parallel hold") + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Capture cancellation ownership for coordinator-level safe stop.""" + del timeout + self._fresh_frame = None + self._cancel_targets = tuple(_snapshot_target(target) for target in targets) + return CommandAcknowledgement.accepted_ack("buffered parallel cancel") + + def drain_frame(self) -> RuntimeCommandFrame | None: + """Consume the frame emitted since the previous coordinator step.""" + frame = self._fresh_frame + self._fresh_frame = None + return None if frame is None else frame.snapshot() + + def drain_hold_requests( + self, + ) -> tuple[tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext], ...]: + """Consume every completion/safe hold buffered since the last tick.""" + requests = tuple( + ( + tuple(_snapshot_target(target) for target in targets), + context, + ) + for targets, context in self._hold_requests + ) + self._hold_requests.clear() + return requests + + +@dataclass(frozen=True, slots=True) +class ParallelRuntimeBranch: + """One semantic-call lane and its exclusive resource claim.""" + + branch_id: str + calls: tuple[SemanticCallSpec, ...] + claim: ResourceClaim + runtime: ParallelBranchRuntime = field(repr=False, compare=False) + command_sink: ParallelLaneCommandSink = field(repr=False, compare=False) + + def __post_init__(self) -> None: + _validate_identifier(self.branch_id, field_name="branch_id") + calls = tuple(self.calls) + if not calls or not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError("calls must contain SemanticCallSpec values.") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + if not isinstance(self.runtime, ParallelBranchRuntime): + raise TypeError("runtime must implement ParallelBranchRuntime.") + if type(self.command_sink) is not ParallelLaneCommandSink: + raise TypeError("command_sink must be ParallelLaneCommandSink.") + object.__setattr__(self, "calls", calls) + + +@dataclass(frozen=True, slots=True) +class ParallelBranchStaticAnalysis: + """Provider-free physical and symbolic claims for one semantic lane.""" + + branch_id: str + calls: tuple[SemanticCallSpec, ...] + claim: ResourceClaim + symbolic_writes: frozenset[SymbolicStateKey] + opaque_symbolic_call_indices: tuple[int, ...] + source_path: tuple[PathPart, ...] + + def __post_init__(self) -> None: + _validate_identifier(self.branch_id, field_name="branch_id") + calls = tuple(self.calls) + if not calls or not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError("calls must contain SemanticCallSpec values.") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + if type(self.symbolic_writes) is not frozenset or not all( + type(write) is SymbolicStateKey for write in self.symbolic_writes + ): + raise TypeError( + "symbolic_writes must be an exact frozenset of " + "SymbolicStateKey values." + ) + opaque_indices = tuple(self.opaque_symbolic_call_indices) + if not all( + type(index) is int and 0 <= index < len(calls) for index in opaque_indices + ): + raise ValueError( + "opaque_symbolic_call_indices must select branch call indices." + ) + if len(set(opaque_indices)) != len(opaque_indices): + raise ValueError("opaque_symbolic_call_indices must be unique.") + source_path = tuple(self.source_path) + if not source_path or not all( + (type(part) is str and bool(part)) or type(part) is int + for part in source_path + ): + raise ValueError("source_path must contain valid diagnostic components.") + object.__setattr__(self, "calls", calls) + object.__setattr__(self, "opaque_symbolic_call_indices", opaque_indices) + object.__setattr__(self, "source_path", source_path) + + +def analyze_parallel_branches( + compiler: SemanticSkillCompiler, + branch_calls: Mapping[str, tuple[SemanticCallSpec, ...]], + *, + workflow_id: str = "parallel_static_analysis", + branch_paths: Mapping[str, tuple[PathPart, ...]] | None = None, +) -> tuple[ParallelBranchStaticAnalysis, ...]: + """Reject overlapping physical claims and exact symbolic write keys. + + This is the canonical provider-free parallel preflight shared by the core + runtime factory and higher-level declarative frontends. Dynamic command + collision safety remains the responsibility of + :class:`ParallelCommandSafetyValidator`. + + Args: + compiler: Canonical semantic compiler owning the current integration. + branch_calls: Ordered branch IDs and their complete semantic calls. + workflow_id: Stable diagnostic prefix for branch workflows. + branch_paths: Optional exact source path for every supplied branch. + + Returns: + Ordered owned branch analyses with combined resource claims. + + Raises: + ValueError: If fewer than two branches are supplied or claims overlap. + SemanticValidationError: If branches write one exact symbolic key. + """ + if not isinstance(compiler, SemanticSkillCompiler): + raise TypeError("compiler must be a SemanticSkillCompiler.") + if not isinstance(branch_calls, Mapping) or len(branch_calls) < 2: + raise ValueError("branch_calls must contain at least two branches.") + _validate_identifier(workflow_id, field_name="workflow_id") + if branch_paths is not None: + if not isinstance(branch_paths, Mapping): + raise TypeError("branch_paths must be a mapping or None.") + if set(branch_paths) != set(branch_calls): + raise ValueError("branch_paths keys must exactly match branch_calls.") + + analyses: list[ParallelBranchStaticAnalysis] = [] + for branch_index, (branch_id, supplied_calls) in enumerate(branch_calls.items()): + _validate_identifier(branch_id, field_name="parallel branch IDs") + calls = tuple(supplied_calls) + if not calls or not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError( + "parallel branch calls must contain SemanticCallSpec values." + ) + source_path = ( + ("parallel", "branches", branch_index) + if branch_paths is None + else tuple(branch_paths[branch_id]) + ) + workflow = compiler.analyze( + calls, + workflow_id=f"{workflow_id}:{branch_index}:{branch_id}", + path=source_path, + ) + analyses.append( + ParallelBranchStaticAnalysis( + branch_id=branch_id, + calls=calls, + claim=ResourceClaim.combine( + tuple(call.bound.binding.claim for call in workflow.calls) + ), + symbolic_writes=frozenset( + write + for analyzed_call in workflow.calls + for write in analyzed_call.symbolic_writes + ), + opaque_symbolic_call_indices=tuple( + analyzed_call.index + for analyzed_call in workflow.calls + if analyzed_call.opaque_symbolic_effect + ), + source_path=source_path, + ) + ) + for index, left in enumerate(analyses): + for right in analyses[index + 1 :]: + if left.claim.conflicts_with(right.claim): + raise SemanticValidationError( + SemanticDiagnostic( + "parallel_resource_conflict", + right.source_path, + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} have overlapping resource claims.", + (left.branch_id, right.branch_id), + ) + ) + shared_writes = left.symbolic_writes & right.symbolic_writes + if shared_writes: + conflict = min( + shared_writes, + key=lambda write: (write.domain.value, write.address), + ) + raise SemanticValidationError( + SemanticDiagnostic( + "parallel_symbolic_write_conflict", + right.source_path, + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} both write symbolic TaskState key " + f"{conflict.rendered}.", + (left.branch_id, right.branch_id), + ) + ) + return tuple(analyses) + + +@dataclass(frozen=True, slots=True, eq=False) +class ParallelSkillResult: + """Owned coordinator status at one explicit barrier.""" + + status: SkillStatus + env_ids: torch.Tensor + success_mask: torch.Tensor + failure_mask: torch.Tensor + cancelled_mask: torch.Tensor + pending_mask: torch.Tensor + task_state: TaskState + branch_results: Mapping[str, SkillResult] + elapsed_steps: int + command_count: int + wait_duration: float = 0.0 + message: str | None = None + + def __post_init__(self) -> None: + if not isinstance(self.status, SkillStatus): + raise TypeError("status must be a SkillStatus.") + if ( + not isinstance(self.env_ids, torch.Tensor) + or self.env_ids.dtype != torch.long + or self.env_ids.dim() != 1 + ): + raise ValueError("env_ids must be a one-dimensional int64 tensor.") + batch_size = int(self.env_ids.numel()) + for field_name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "pending_mask", + ): + value = getattr(self, field_name) + if ( + not isinstance(value, torch.Tensor) + or value.dtype != torch.bool + or value.shape != (batch_size,) + or value.device != self.env_ids.device + ): + raise ValueError(f"{field_name} must match env_ids.") + if ( + (self.success_mask & (self.failure_mask | self.cancelled_mask)).any() + or (self.failure_mask & self.cancelled_mask).any() + or ( + self.pending_mask + & (self.success_mask | self.failure_mask | self.cancelled_mask) + ).any() + ): + raise ValueError("parallel result masks must be disjoint.") + if not isinstance(self.task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if ( + self.task_state.batch_size != batch_size + or self.task_state.device != self.env_ids.device + ): + raise ValueError("task_state must match env_ids.") + if type(self.elapsed_steps) is not int or self.elapsed_steps < 0: + raise ValueError("elapsed_steps must be non-negative.") + if type(self.command_count) is not int or self.command_count < 0: + raise ValueError("command_count must be non-negative.") + if not math.isfinite(self.wait_duration) or self.wait_duration < 0.0: + 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.") + branches: dict[str, SkillResult] = {} + for branch_id, result in self.branch_results.items(): + _validate_identifier(branch_id, field_name="branch result IDs") + if not isinstance(result, SkillResult): + raise TypeError("branch_results values must be SkillResult values.") + branches[branch_id] = result + object.__setattr__(self, "env_ids", self.env_ids.clone()) + for field_name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "pending_mask", + ): + object.__setattr__(self, field_name, getattr(self, field_name).clone()) + object.__setattr__( + self, + "task_state", + TaskState( + batch_size=self.task_state.batch_size, + device=self.task_state.device, + held_objects=self.task_state.held_objects, + coordinated_held_objects=self.task_state.coordinated_held_objects, + articulation_joints=self.task_state.articulation_joints, + ), + ) + object.__setattr__(self, "branch_results", MappingProxyType(branches)) + + @property + def terminal(self) -> bool: + """Whether every row has left the barrier.""" + return self.status in { + SkillStatus.COMPLETED, + SkillStatus.FAILED, + SkillStatus.CANCELLED, + } + + def to_metadata(self) -> dict[str, object]: + """Return a fresh deterministic JSON-safe parallel barrier result.""" + return { + "schema_version": 1, + "kind": "parallel_skill_result", + "status": self.status.value, + "env_ids": self.env_ids.detach().cpu().tolist(), + "masks": { + "success": self.success_mask.detach().cpu().tolist(), + "failure": self.failure_mask.detach().cpu().tolist(), + "cancelled": self.cancelled_mask.detach().cpu().tolist(), + "pending": self.pending_mask.detach().cpu().tolist(), + }, + "task_state": task_state_to_metadata(self.task_state), + "branches": { + branch_id: result.to_metadata() + for branch_id, result in sorted(self.branch_results.items()) + }, + "elapsed_steps": self.elapsed_steps, + "command_count": self.command_count, + "wait_duration": self.wait_duration, + "message": self.message, + } + + +def _optional_tensor_equal( + left: torch.Tensor | None, right: torch.Tensor | None +) -> bool: + return (left is None and right is None) or ( + left is not None and right is not None and torch.equal(left, right) + ) + + +def _state_value_equal(left: object, right: object) -> bool: + if type(left) is not type(right): + return False + if left is None or right is None: + return left is right + if hasattr(left, "position"): + return torch.equal(left.position, right.position) and _optional_tensor_equal( + left.env_mask, + right.env_mask, + ) + if hasattr(left, "left_object_to_eef"): + return ( + left.semantics.entity_id == right.semantics.entity_id + and torch.equal(left.left_object_to_eef, right.left_object_to_eef) + and torch.equal(left.right_object_to_eef, right.right_object_to_eef) + and torch.equal(left.left_grasp_xpos, right.left_grasp_xpos) + and torch.equal(left.right_grasp_xpos, right.right_grasp_xpos) + and _optional_tensor_equal(left.env_mask, right.env_mask) + ) + return ( + left.semantics.entity_id == right.semantics.entity_id + and torch.equal(left.object_to_eef, right.object_to_eef) + and torch.equal(left.grasp_xpos, right.grasp_xpos) + and _optional_tensor_equal(left.env_mask, right.env_mask) + ) + + +def _mapping_delta( + before: Mapping[object, object], after: Mapping[object, object] +) -> dict: + updates: dict[object, object | None] = {} + for key in set(before) | set(after): + if key not in after: + updates[key] = None + elif key not in before or not _state_value_equal(before[key], after[key]): + updates[key] = after[key] + return updates + + +def _task_state_delta(before: TaskState, after: TaskState) -> StateDelta: + if before.batch_size != after.batch_size or before.device != after.device: + raise ValueError("Parallel branch TaskState changed batch or device.") + return StateDelta( + held_object_updates=_mapping_delta( + before.held_objects, + after.held_objects, + ), + coordinated_held_object_updates=_mapping_delta( + before.coordinated_held_objects, + after.coordinated_held_objects, + ), + articulation_joint_updates=_mapping_delta( + before.articulation_joints, + after.articulation_joints, + ), + ) + + +class ParallelSkillRuntime: + """Run independent JIT semantic lanes on one synchronized command grid. + + Schema v2 deliberately uses conservative barrier ownership: branches are + not assigned disjoint environment-row partitions, so two branches that + write the same symbolic key conflict for the complete started batch even + when their observed value masks happen to be disjoint. A future schema + may add explicit row partitioning before relaxing this invariant. + + A lane completion hold is forwarded as an explicit grid action. Other + lanes therefore receive deterministic hold-padding for that environment + step; a merged frame generated in the same coordinator cycle is retained + and dispatched only after the clock advances. Branch runners are not + stepped while that retained frame is being dispatched. This keeps the + physical order ``observed hold -> next command`` and limits every normal + coordinator step to one action-producing transport operation. + """ + + def __init__( + self, + branches: tuple[ParallelRuntimeBranch, ...], + command_sink: CommandSink, + clock: ExecutionClock, + timing_policy: ParallelTimingPolicy, + safety_validator: ParallelCommandSafetyValidator, + *, + timeout_steps: int, + failure_policy: str = "fail_fast", + ) -> None: + if not isinstance(branches, tuple) or len(branches) < 2: + raise ValueError("ParallelSkillRuntime requires at least two branches.") + if not all(type(branch) is ParallelRuntimeBranch for branch in branches): + raise TypeError("branches must contain ParallelRuntimeBranch values.") + branch_ids = tuple(branch.branch_id for branch in branches) + if len(set(branch_ids)) != len(branch_ids): + raise ValueError("Parallel branch IDs must be unique.") + for index, left in enumerate(branches): + for right in branches[index + 1 :]: + if left.claim.conflicts_with(right.claim): + raise ValueError( + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} have overlapping resource claims." + ) + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + if not isinstance(clock, ExecutionClock): + raise TypeError("clock must implement ExecutionClock.") + if not isinstance(timing_policy, ParallelTimingPolicy): + raise TypeError("timing_policy must be ParallelTimingPolicy.") + if not isinstance(safety_validator, ParallelCommandSafetyValidator): + raise TypeError( + "safety_validator must implement ParallelCommandSafetyValidator; " + "resource claims alone do not establish collision safety." + ) + if type(timeout_steps) is not int or timeout_steps <= 0: + raise ValueError("timeout_steps must be positive.") + if failure_policy != "fail_fast": + raise ValueError("failure_policy must be exactly 'fail_fast'.") + initial = branches[0].runtime.result + for branch in branches[1:]: + result = branch.runtime.result + if ( + result.env_ids.device != initial.env_ids.device + or not torch.equal(result.env_ids, initial.env_ids) + or result.task_state.batch_size != initial.task_state.batch_size + or result.task_state.device != initial.task_state.device + ): + raise ValueError( + "Parallel branch runtimes must share env_ids, batch, and device." + ) + if not _task_state_delta(initial.task_state, result.task_state).is_empty: + raise ValueError( + "Parallel branch runtimes must start from the same verified " + "TaskState barrier snapshot." + ) + self._branches = branches + self._command_sink = command_sink + self._clock = clock + self._timing_policy = timing_policy + self._safety_validator = safety_validator + self._timeout_steps = timeout_steps + self._initial_state = initial.task_state + self._task_state = initial.task_state + self._env_ids = initial.env_ids + self._status = SkillStatus.IDLE + self._success = torch.zeros_like(initial.success_mask) + self._failure = torch.zeros_like(initial.failure_mask) + self._cancelled = torch.zeros_like(initial.cancelled_mask) + self._pending = torch.ones_like(initial.success_mask) + self._started_eligible = torch.zeros_like(initial.success_mask) + self._elapsed_steps = 0 + self._start_timestamp: float | None = None + self._command_count = 0 + self._wait_duration = 0.0 + self._message: str | None = None + self._force_mask_dispatch = False + self._terminal_stop_forwarded = False + self._held_target_fingerprints: set[Hashable] = set() + self._last_hold_context: PlanningContext | None = None + self._deferred_frame: RuntimeCommandFrame | None = None + self._deferred_lane_frames: dict[str, RuntimeCommandFrame] = {} + self._terminal_hold_pending = False + self._next_transport_at: float | None = None + + @classmethod + def from_template( + cls, + template_runtime: SkillRuntime, + branch_calls: Mapping[str, tuple[SemanticCallSpec, ...]], + command_sink: CommandSink, + timing_policy: ParallelTimingPolicy, + safety_validator: ParallelCommandSafetyValidator, + *, + timeout_steps: int, + failure_policy: str = "fail_fast", + workflow_id: str = "parallel_static_analysis", + branch_paths: Mapping[str, tuple[PathPart, ...]] | None = None, + ) -> ParallelSkillRuntime: + """Analyze claims and derive independent lanes from one runtime. + + This factory deliberately accepts semantic calls instead of compiled + Gym-program types. It keeps the simulation runtime independent of the + higher-level configuration package while giving every frontend one + canonical resource-conflict and lane-construction path. + + Args: + template_runtime: Idle runtime providing shared compiler and ports. + branch_calls: Ordered branch ID to semantic-call sequence mapping. + command_sink: The sole outbound merged command sink. + timing_policy: Exact shared environment grid. + safety_validator: Required physical/collision safety gate for each + synchronized outbound command. + timeout_steps: Maximum environment steps at the barrier. + failure_policy: Row-local barrier failure policy. + workflow_id: Stable prefix for provider-free claim analysis. + branch_paths: Optional exact source path for every branch. + + Returns: + A one-shot parallel runtime whose branches share no mutable runner + state. + """ + if not isinstance(template_runtime, SkillRuntime): + raise TypeError("template_runtime must be a SkillRuntime.") + if template_runtime.status is SkillStatus.RUNNING: + raise RuntimeError("template_runtime must not be running.") + branches: list[ParallelRuntimeBranch] = [] + for analysis in analyze_parallel_branches( + template_runtime.compiler, + branch_calls, + workflow_id=workflow_id, + branch_paths=branch_paths, + ): + lane_sink = ParallelLaneCommandSink() + lane_runtime = template_runtime.fork( + lane_sink, + task_state=template_runtime.task_state, + ) + branches.append( + ParallelRuntimeBranch( + branch_id=analysis.branch_id, + calls=analysis.calls, + claim=analysis.claim, + runtime=lane_runtime, + command_sink=lane_sink, + ) + ) + return cls( + tuple(branches), + command_sink, + template_runtime.clock, + timing_policy, + safety_validator, + timeout_steps=timeout_steps, + failure_policy=failure_policy, + ) + + @property + def result(self) -> ParallelSkillResult: + """Return an owned barrier snapshot.""" + return ParallelSkillResult( + status=self._status, + env_ids=self._env_ids, + success_mask=self._success, + failure_mask=self._failure, + cancelled_mask=self._cancelled, + pending_mask=self._pending, + task_state=self._task_state, + branch_results={ + branch.branch_id: branch.runtime.result for branch in self._branches + }, + elapsed_steps=self._elapsed_steps, + command_count=self._command_count, + wait_duration=self._wait_duration, + message=self._message, + ) + + @property + def clock(self) -> ExecutionClock: + """Return the exact clock shared by the coordinator and every lane.""" + return self._clock + + @property + def branch_claims(self) -> Mapping[str, ResourceClaim]: + """Return immutable statically analyzed claims in branch order.""" + return MappingProxyType( + {branch.branch_id: branch.claim for branch in self._branches} + ) + + def start( + self, + *, + workflow_id: str = "parallel_workflow", + eligible_mask: torch.Tensor | None = None, + ) -> ParallelSkillResult: + """Start all lanes from the same verified barrier state.""" + if self._status is not SkillStatus.IDLE: + raise RuntimeError("ParallelSkillRuntime instances are one-shot.") + _validate_identifier(workflow_id, field_name="workflow_id") + if eligible_mask is None: + eligible = torch.ones_like(self._pending) + else: + if ( + not isinstance(eligible_mask, torch.Tensor) + or eligible_mask.dtype != torch.bool + or eligible_mask.shape != self._pending.shape + or eligible_mask.device != self._pending.device + ): + raise ValueError("eligible_mask must match the parallel batch.") + eligible = eligible_mask.clone() + if not eligible.any(): + raise ValueError("eligible_mask must contain an active row.") + self._success.zero_() + self._failure.zero_() + self._cancelled.zero_() + self._pending = eligible.clone() + self._started_eligible = eligible.clone() + self._elapsed_steps = 0 + self._start_timestamp = self._read_clock() + self._command_count = 0 + self._wait_duration = 0.0 + self._message = None + self._force_mask_dispatch = False + self._terminal_stop_forwarded = False + self._held_target_fingerprints.clear() + self._last_hold_context = None + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + self._status = SkillStatus.RUNNING + started: list[ParallelRuntimeBranch] = [] + try: + for branch in self._branches: + branch.runtime.start( + *branch.calls, + workflow_id=f"{workflow_id}:{branch.branch_id}", + eligible_mask=eligible, + ) + started.append(branch) + except Exception as exc: + reason = "Parallel branch startup failed: " f"{type(exc).__name__}: {exc}" + for branch in started: + branch.runtime.cancel(reason) + self._failure = eligible.clone() + self._pending.zero_() + self._status = SkillStatus.FAILED + self._message = reason + return self.result + try: + self._sync_branch_identity() + self._update_barrier() + self._finish_if_complete() + except Exception as exc: + self._abort_coordinator("Parallel startup coordination failed", exc) + return self.result + + def step(self) -> ParallelSkillResult: + """Advance one deterministic coordinator state-machine transition.""" + if self._status is not SkillStatus.RUNNING: + return self.result + try: + self._update_elapsed_steps() + if self._elapsed_steps >= self._timeout_steps and ( + self._pending.any() or self._transport_flush_pending + ): + self._timeout_pending_rows() + self._finish_if_complete() + return self.result + transport_wait = self._remaining_transport_wait() + if transport_wait > 0.0: + self._wait_duration = transport_wait + return self.result + if self._deferred_frame is not None: + accepted = self._dispatch_deferred_frame() + if ( + accepted + and not self._pending.any() + and self._status is SkillStatus.RUNNING + ): + self._terminal_hold_pending = True + self._finish_if_complete() + return self.result + if self._terminal_hold_pending: + self._terminal_hold_pending = False + self._dispatch_requested_hold( + required=True, + include_last_targets=True, + ) + self._finish_if_complete() + return self.result + for branch in self._branches: + if not branch.runtime.result.terminal: + branch.runtime.step() + self._update_barrier() + self._dispatch_grid_frame() + self._finish_if_complete() + except Exception as exc: + self._abort_coordinator("Parallel coordinator step failed", exc) + return self.result + + def _timeout_pending_rows(self) -> None: + """Fail and safe-stop deadline-expired rows before another command.""" + timed_out = self._pending.clone() + if not timed_out.any() and self._transport_flush_pending: + timed_out = self._started_eligible.clone() + if not timed_out.any(): + return + self._failure |= timed_out + self._success &= ~timed_out + self._pending &= ~timed_out + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + self._message = f"Parallel barrier timed out after {self._timeout_steps} steps." + errors: list[str] = [] + for branch in self._branches: + if branch.runtime.result.terminal: + continue + try: + branch.runtime.cancel(self._message) + except Exception as exc: + errors.append(f"{branch.branch_id}: {type(exc).__name__}: {exc}") + stopped, stop_message = self._forward_safe_stop() + self._terminal_stop_forwarded = True + if not stopped and stop_message is not None: + errors.append(stop_message) + if errors: + self._message += " Safe stop errors: " + "; ".join(errors) + + def _read_clock(self) -> float: + """Read one finite non-negative timestamp from the shared clock.""" + now = float(self._clock.now()) + if not math.isfinite(now) or now < 0.0: + raise ValueError("ExecutionClock.now() must be finite and non-negative.") + return now + + def _update_elapsed_steps(self) -> None: + """Measure completed environment-grid intervals since start.""" + assert self._start_timestamp is not None + now = self._read_clock() + elapsed = now - self._start_timestamp + if elapsed < -self._timing_policy.tolerance: + raise RuntimeError("Parallel execution clock moved backwards.") + ratio = max(0.0, elapsed) / self._timing_policy.step_dt + tolerance = self._timing_policy.tolerance / self._timing_policy.step_dt + self._elapsed_steps = max( + self._elapsed_steps, + int(math.floor(ratio + tolerance)), + ) + + def _sync_branch_identity(self) -> None: + """Adopt and verify env IDs after every lane's first observation.""" + reference = self._branches[0].runtime.result + for branch in self._branches[1:]: + result = branch.runtime.result + if ( + result.env_ids.device != reference.env_ids.device + or not torch.equal(result.env_ids, reference.env_ids) + or result.task_state.batch_size != reference.task_state.batch_size + or result.task_state.device != reference.task_state.device + ): + raise ValueError( + "Parallel branch observations must share env_ids, batch, " + "and device." + ) + self._env_ids = reference.env_ids.clone() + + def cancel( + self, + reason: str = "Parallel workflow cancelled by caller.", + ) -> ParallelSkillResult: + """Cancel every lane and forward one target-scoped transport cancel.""" + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + if self._status is not SkillStatus.RUNNING: + return self.result + had_transport_flush = self._transport_flush_pending + cancelled = self._pending.clone() + if not cancelled.any() and had_transport_flush: + cancelled = self._started_eligible.clone() + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + errors: list[str] = [] + for branch in self._branches: + try: + branch.runtime.cancel(reason) + except Exception as exc: + errors.append(f"{branch.branch_id}: {type(exc).__name__}: {exc}") + stopped, stop_message = self._forward_safe_stop() + self._terminal_stop_forwarded = True + if stop_message is not None: + errors.append(stop_message) + self._pending &= ~cancelled + self._success &= ~cancelled + merge_succeeded = self._merge_verified_state() + if errors or not stopped or not merge_succeeded: + self._failure |= cancelled + self._cancelled &= ~cancelled + self._status = SkillStatus.FAILED + if errors or not stopped: + stop_detail = "; ".join(errors) or "unknown safe-stop failure" + self._message = reason + " Safe stop failed: " + stop_detail + elif self._message is None: + self._message = reason + " Verified-state merge failed." + else: + self._cancelled |= cancelled + self._status = SkillStatus.CANCELLED + self._message = reason + self._wait_duration = 0.0 + return self.result + + @property + def _transport_flush_pending(self) -> bool: + """Whether a retained command or mandatory final hold is outstanding.""" + return self._deferred_frame is not None or self._terminal_hold_pending + + def _remaining_transport_wait(self) -> float: + """Return time until another normal grid action may be forwarded.""" + ready_at = self._next_transport_at + if ready_at is None: + return 0.0 + remaining = ready_at - self._read_clock() + if remaining <= self._timing_policy.tolerance: + self._next_transport_at = None + return 0.0 + return remaining + + def _record_transport_action(self) -> None: + """Arm the next physical grid boundary after one accepted action.""" + self._next_transport_at = self._read_clock() + self._timing_policy.step_dt + self._wait_duration = self._timing_policy.step_dt + + def _update_barrier(self) -> None: + results = {branch.branch_id: branch.runtime.result for branch in self._branches} + pending = { + branch_id: ( + result.eligible_mask + & ~result.success_mask + & ~result.failure_mask + & ~result.cancelled_mask + ) + for branch_id, result in results.items() + } + update = resolve_parallel_barrier( + pending_masks=pending, + success_masks={ + branch_id: result.success_mask for branch_id, result in results.items() + }, + failure_masks={ + branch_id: result.failure_mask | result.cancelled_mask + for branch_id, result in results.items() + }, + ) + new_failure = update.failure_mask & ~self._failure + self._failure |= update.failure_mask + self._success |= update.completed_mask & ~update.failure_mask + self._pending &= ~update.completed_mask + if new_failure.any(): + self._force_mask_dispatch = True + reason = "A peer parallel branch failed for these environment rows." + for branch in self._branches: + mask = update.cancellation_masks[branch.branch_id] + if mask.any(): + branch.runtime.deactivate_rows(mask, reason=reason) + running = tuple(result for result in results.values() if not result.terminal) + if not running or any(result.wait_duration <= 0.0 for result in running): + self._wait_duration = 0.0 + else: + self._wait_duration = min(result.wait_duration for result in running) + + def _dispatch_grid_frame(self) -> None: + fresh: dict[str, RuntimeCommandFrame] = {} + for branch in self._branches: + frame = branch.command_sink.drain_frame() + if frame is not None: + if branch.runtime.result.terminal: + raise ParallelSafetyError( + f"Parallel branch {branch.branch_id!r} became terminal " + "while emitting a fresh command frame. A post-command " + "observation is required before a safe terminal hold." + ) + fresh[branch.branch_id] = frame + force_mask_dispatch = self._force_mask_dispatch + self._force_mask_dispatch = False + if not fresh and not force_mask_dispatch: + self._dispatch_requested_hold() + return + plans: list[ParallelBranchPlan] = [] + lane_frames: dict[str, RuntimeCommandFrame] = {} + requested_holds = { + _target_fingerprint(target) + for branch in self._branches + for target in branch.command_sink.hold_request[0] + } + for branch in self._branches: + frame = fresh.get(branch.branch_id) + is_fresh = frame is not None + if frame is None: + frame = branch.command_sink.last_frame + if frame is None: + continue + if not is_fresh: + commands = tuple( + command + for command in frame.commands + if _target_fingerprint(command.target) + not in self._held_target_fingerprints | requested_holds + ) + if not commands: + continue + frame = RuntimeCommandFrame( + commands=commands, + active_mask=frame.active_mask, + env_ids=frame.env_ids, + hold_duration=frame.hold_duration, + ) + frame = frame.with_active_mask(frame.active_mask & ~self._failure) + lane_frames[branch.branch_id] = frame.snapshot() + plans.append( + ParallelBranchPlan( + branch_id=branch.branch_id, + claim=branch.claim, + commands=TimedCommandSequence( + frames=(frame,), + env_ids=frame.env_ids, + ), + ) + ) + if not plans: + self._dispatch_requested_hold() + return + if len(plans) == 1: + frame = plans[0].commands.frames[0] + durations = frame.hold_duration + expected = torch.full_like( + durations, + self._timing_policy.step_dt, + ) + if not torch.allclose( + durations, + expected, + atol=self._timing_policy.tolerance, + rtol=0.0, + ): + raise ValueError( + "Parallel command frames must equal the environment step grid." + ) + merged = plans[0].commands + else: + merged = align_parallel_commands(tuple(plans), self._timing_policy) + frame = merged.frames[0] + if not frame.active_mask.any(): + self._dispatch_requested_hold(extra_targets=frame.targets) + return + + if self._has_unforwarded_hold_targets(): + self._deferred_frame = frame.snapshot() + self._deferred_lane_frames = { + branch_id: branch_frame.snapshot() + for branch_id, branch_frame in lane_frames.items() + } + if not self._dispatch_requested_hold(): + self._deferred_frame = None + self._deferred_lane_frames.clear() + return + + # Drain duplicate requests to refresh the latest synchronized context + # without producing another action, then send exactly one grid frame. + self._dispatch_requested_hold() + accepted = self._send_merged_frame(frame, lane_frames) + if accepted and not self._pending.any() and self._status is SkillStatus.RUNNING: + self._terminal_hold_pending = True + + def _dispatch_deferred_frame(self) -> bool: + """Send a frame retained behind one explicit hold-padding step.""" + frame = self._deferred_frame + if frame is None: + raise RuntimeError("No deferred parallel frame is available.") + lane_frames = { + branch_id: branch_frame.snapshot() + for branch_id, branch_frame in self._deferred_lane_frames.items() + } + self._deferred_frame = None + self._deferred_lane_frames.clear() + return self._send_merged_frame(frame, lane_frames) + + def _send_merged_frame( + self, + frame: RuntimeCommandFrame, + lane_frames: Mapping[str, RuntimeCommandFrame], + ) -> bool: + """Validate and forward one active synchronized command frame.""" + try: + safety_result = self._safety_validator.validate( + branch_frames=MappingProxyType(dict(lane_frames)), + merged_frame=frame.snapshot(), + ) + except ParallelSafetyError: + raise + except Exception as exc: + raise ParallelSafetyError( + "Parallel command safety validation failed: " + f"{type(exc).__name__}: {exc}" + ) from exc + if safety_result is not None: + raise ParallelSafetyError( + "ParallelCommandSafetyValidator.validate() must return None." + ) + acknowledgement = self._command_sink.send(frame, timeout=1.0) + if not isinstance(acknowledgement, CommandAcknowledgement): + raise TypeError("CommandSink.send() returned an invalid value.") + if not acknowledgement.accepted: + self._fail_transport(acknowledgement.message) + return False + self._command_count += 1 + self._record_transport_action() + self._held_target_fingerprints.difference_update( + _target_fingerprint(target) for target in frame.targets + ) + return True + + def _has_unforwarded_hold_targets(self) -> bool: + """Whether lane requests contain a target not already physically held.""" + for branch in self._branches: + targets, _ = branch.command_sink.hold_request + if any( + _target_fingerprint(target) not in self._held_target_fingerprints + for target in targets + ): + return True + return False + + def _dispatch_requested_hold( + self, + *, + extra_targets: tuple[RuntimeEndpointTarget, ...] = (), + include_last_targets: bool = False, + required: bool = False, + ) -> bool: + """Forward every lane hold without dropping earlier call targets.""" + targets: dict[Hashable, RuntimeEndpointTarget] = { + _target_fingerprint(target): target for target in extra_targets + } + context: PlanningContext | None = None + for branch in self._branches: + for ( + branch_targets, + branch_context, + ) in branch.command_sink.drain_hold_requests(): + for target in branch_targets: + targets[_target_fingerprint(target)] = target + context = branch_context + self._last_hold_context = branch_context + if include_last_targets: + last_frame = branch.command_sink.last_frame + if last_frame is not None: + for target in last_frame.targets: + targets[_target_fingerprint(target)] = target + targets = { + key: target + for key, target in targets.items() + if key not in self._held_target_fingerprints + } + if not targets: + return True + if context is None: + context = self._last_hold_context + if context is None: + message = "Parallel hold targets have no synchronized planning context." + if required or targets: + self._fail_transport(message) + return False + acknowledgement = self._command_sink.hold( + tuple(targets.values()), + context, + timeout=1.0, + ) + if not isinstance(acknowledgement, CommandAcknowledgement): + raise TypeError("CommandSink.hold() returned an invalid value.") + if not acknowledgement.accepted: + self._fail_transport(acknowledgement.message) + return False + self._held_target_fingerprints.update(targets) + self._last_hold_context = context + self._record_transport_action() + return True + + def _fail_transport(self, message: str) -> None: + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + failed = self._pending.clone() + if not failed.any(): + failed = self._started_eligible.clone() + self._failure |= failed + self._success &= ~failed + self._pending &= ~failed + self._message = "Parallel command transport rejected the merged operation." + if message: + self._message += f" {message}" + for branch in self._branches: + branch.runtime.cancel(self._message) + self._forward_safe_stop() + self._terminal_stop_forwarded = True + + def _forward_safe_stop(self) -> tuple[bool, str | None]: + """Forward lane-owned cancellation and hold once to the real sink.""" + targets: dict[Hashable, RuntimeEndpointTarget] = {} + context: PlanningContext | None = self._last_hold_context + for branch in self._branches: + for target in branch.command_sink.cancel_targets: + targets[_target_fingerprint(target)] = target + branch_targets, branch_context = branch.command_sink.hold_request + for target in branch_targets: + targets[_target_fingerprint(target)] = target + if branch_context is not None: + context = branch_context + last_frame = branch.command_sink.last_frame + if last_frame is not None: + for target in last_frame.targets: + targets[_target_fingerprint(target)] = target + if not targets: + return True, None + snapshots = tuple(targets.values()) + errors: list[str] = [] + try: + cancel_ack = self._command_sink.cancel(snapshots, timeout=1.0) + if not isinstance(cancel_ack, CommandAcknowledgement): + raise TypeError("CommandSink.cancel() returned an invalid value.") + if not cancel_ack.accepted: + errors.append(cancel_ack.message or "transport cancel was rejected") + except Exception as exc: + errors.append(f"cancel {type(exc).__name__}: {exc}") + if context is None: + errors.append("no planning context was available for final safe hold") + else: + try: + hold_ack = self._command_sink.hold( + snapshots, + context, + timeout=1.0, + ) + if not isinstance(hold_ack, CommandAcknowledgement): + raise TypeError("CommandSink.hold() returned an invalid value.") + if not hold_ack.accepted: + errors.append(hold_ack.message or "transport hold was rejected") + except Exception as exc: + errors.append(f"hold {type(exc).__name__}: {exc}") + if not errors: + self._held_target_fingerprints.update( + _target_fingerprint(target) for target in snapshots + ) + self._last_hold_context = context + return (not errors), (None if not errors else "; ".join(errors)) + + def _abort_coordinator(self, prefix: str, exc: Exception) -> None: + """Convert an internal tick exception into a safe terminal failure.""" + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + reason = f"{prefix}: {type(exc).__name__}: {exc}" + failed = self._pending.clone() + if not failed.any(): + failed = self._started_eligible.clone() + self._failure |= failed + self._success &= ~failed + self._pending &= ~failed + errors: list[str] = [] + for branch in self._branches: + if branch.runtime.result.terminal: + continue + try: + branch.runtime.cancel(reason) + except Exception as cancel_exc: + errors.append( + f"{branch.branch_id}: {type(cancel_exc).__name__}: {cancel_exc}" + ) + stopped, stop_message = self._forward_safe_stop() + self._terminal_stop_forwarded = True + if not stopped and stop_message is not None: + errors.append(stop_message) + self._message = reason + if errors: + self._message += " Safe stop errors: " + "; ".join(errors) + self._merge_verified_state() + self._status = SkillStatus.FAILED + self._wait_duration = 0.0 + + def _merge_verified_state(self) -> bool: + """Merge every branch-local verified patch at a terminal barrier.""" + effects = { + branch.branch_id: ( + _task_state_delta( + self._initial_state, + branch.runtime.result.task_state, + ), + self._started_eligible, + ) + for branch in self._branches + } + try: + self._task_state = merge_parallel_effects(self._initial_state, effects) + except Exception as exc: + self._failure |= self._started_eligible + self._success.zero_() + merge_message = ( + "Parallel verified-state merge failed: " f"{type(exc).__name__}: {exc}" + ) + self._message = ( + merge_message + if self._message is None + else f"{self._message} {merge_message}" + ) + return False + return True + + def _finish_if_complete(self) -> None: + if self._pending.any(): + return + if self._deferred_frame is not None or self._terminal_hold_pending: + return + self._merge_verified_state() + if self._status is SkillStatus.RUNNING and not self._terminal_stop_forwarded: + self._dispatch_requested_hold(required=True, include_last_targets=True) + self._wait_duration = 0.0 + if self._failure.any(): + self._status = SkillStatus.FAILED + elif self._cancelled.any(): + self._status = SkillStatus.CANCELLED + else: + self._status = SkillStatus.COMPLETED + + +__all__ = [ + "analyze_parallel_branches", + "ParallelBranchRuntime", + "ParallelBranchStaticAnalysis", + "ParallelCommandSafetyValidator", + "ParallelLaneCommandSink", + "ParallelRuntimeBranch", + "ParallelSkillResult", + "ParallelSkillRuntime", + "ParallelSafetyError", +] diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index f3715f20b..290799dc1 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -43,11 +43,26 @@ DisjointResourceSlots, DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, INVERSE_KINEMATICS_CAPABILITY, SkillBindingContract, SkillResourceSlot, ) from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg +from .effects import ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + POSE_RELATION_EFFECT_CHANNEL, + ControlPartEvidenceAddress, + EffectEvidenceSourceRef, + EffectMonitorRef, +) if TYPE_CHECKING: from embodichain.lab.sim.atomic_actions.engine import AtomicActionEngine @@ -123,6 +138,39 @@ def _snapshot_endpoint_commands( return MappingProxyType(snapshots) +def _snapshot_effect_sources( + values: Mapping[str, EffectEvidenceSourceRef], + *, + field_name: str, +) -> Mapping[str, EffectEvidenceSourceRef]: + """Validate, own, and freeze endpoint observation sources by channel.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + snapshots: dict[str, EffectEvidenceSourceRef] = {} + for channel, source in values.items(): + _validate_identifier(channel, field_name=f"{field_name} channel names") + if not isinstance(source, EffectEvidenceSourceRef): + raise TypeError( + f"{field_name} values must be EffectEvidenceSourceRef instances." + ) + snapshot = source.snapshot() + if snapshot is source: + raise TypeError( + f"{field_name}[{channel!r}].snapshot() must return an independent " + "source reference." + ) + if ( + isinstance(snapshot.address, ControlPartEvidenceAddress) + and snapshot.address.channel != channel + ): + raise ValueError( + f"{field_name}[{channel!r}] disagrees with its control-part " + f"address channel {snapshot.address.channel!r}." + ) + snapshots[channel] = snapshot + return MappingProxyType(snapshots) + + @dataclass(frozen=True, slots=True, kw_only=True) class ResourceEndpoint(ABC): """Extensible execution endpoint in a robot resource graph. @@ -188,6 +236,12 @@ class EndpointResolution: runtime_target: RuntimeEndpointTarget """Typed immutable destination consumed by an endpoint command transport.""" + task_state_key: str | None = None + """Optional symbolic state key; profile binding defaults to its resource ID.""" + + effect_sources: Mapping[str, EffectEvidenceSourceRef] = field(default_factory=dict) + """Provider-routed raw observation sources keyed by open channel ID.""" + command_profile_key: str | None = None """Profile key that owns semantic commands for this endpoint, when any.""" @@ -226,6 +280,19 @@ def __post_init__(self) -> None: field_name="RuntimeEndpointTarget.target_id", ) object.__setattr__(self, "runtime_target", target) + if self.task_state_key is not None: + _validate_identifier( + self.task_state_key, + field_name="EndpointResolution.task_state_key", + ) + object.__setattr__( + self, + "effect_sources", + _snapshot_effect_sources( + self.effect_sources, + field_name="EndpointResolution.effect_sources", + ), + ) if self.command_profile_key is not None: _validate_identifier( self.command_profile_key, @@ -356,6 +423,18 @@ def resolve( f"Control part {endpoint.control_part!r} declares solver-backed " f"capabilities {sorted(declared)}, but has no configured solver." ) + effect_channels = { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } + if GRASP_CAPABILITY in endpoint.capabilities: + effect_channels.update( + { + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + } + ) return EndpointResolution( runtime_target=JointPositionTarget( control_part=endpoint.control_part, @@ -367,6 +446,14 @@ def resolve( else endpoint.command_profile ), requires_command_profile=endpoint.command_profile is not None, + effect_sources={ + channel: EffectEvidenceSourceRef( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress(endpoint.control_part, channel), + ) + for channel in sorted(effect_channels) + }, claim_tokens=frozenset({f"robot.control_part:{endpoint.control_part}"}), joint_ids=joint_ids, ) @@ -379,6 +466,8 @@ class ResolvedResourceEndpoint: endpoint: ResourceEndpoint adapter_id: str runtime_target: RuntimeEndpointTarget + task_state_key: str | None = None + effect_sources: Mapping[str, EffectEvidenceSourceRef] = field(default_factory=dict) command_profile_key: str | None = None requires_command_profile: bool = False commands: Mapping[str, ControlCommand] = field(default_factory=dict) @@ -405,6 +494,8 @@ def __post_init__(self) -> None: ) resolution = EndpointResolution( runtime_target=self.runtime_target, + task_state_key=self.task_state_key, + effect_sources=self.effect_sources, command_profile_key=self.command_profile_key, requires_command_profile=self.requires_command_profile, claim_tokens=self.claim_tokens, @@ -412,6 +503,13 @@ def __post_init__(self) -> None: exclusive=self.exclusive, ) object.__setattr__(self, "runtime_target", resolution.runtime_target) + resolved_state_key = ( + resolution.runtime_target.target_id + if resolution.task_state_key is None + else resolution.task_state_key + ) + object.__setattr__(self, "task_state_key", resolved_state_key) + object.__setattr__(self, "effect_sources", resolution.effect_sources) object.__setattr__( self, "command_profile_key", @@ -548,16 +646,7 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True, init=False) class SkillPolicyPreset: - """Versioned planning, recovery, and runner policy bundle. - - Args: - preset_id: Stable preset identifier. - schema_version: Preset schema version. Version 1 is currently supported. - motion_policy: Reusable atomic motion policy. - recovery_policy: Bounded action recovery policy. - runner_cfg: Execution transport and scheduling policy. - required_planner: Optional planner backend required by this preset. - """ + """Versioned planning, recovery, runner, and effect-monitor bundle.""" preset_id: str schema_version: int @@ -566,6 +655,7 @@ class SkillPolicyPreset: _motion_policy: MotionPolicy _recovery_policy: RecoveryPolicy _runner_cfg: ExecutionRunnerCfg + _effect_monitors: Mapping[str, EffectMonitorRef] def __init__( self, @@ -574,6 +664,7 @@ def __init__( motion_policy: MotionPolicy | None = None, recovery_policy: RecoveryPolicy | None = None, runner_cfg: ExecutionRunnerCfg | None = None, + effect_monitors: Mapping[str, EffectMonitorRef] | None = None, required_planner: str | None = None, ) -> None: """Own one policy bundle without exposing mutable nested configuration.""" @@ -601,12 +692,46 @@ def __init__( raise TypeError("recovery_policy must be a RecoveryPolicy.") if not isinstance(selected_runner, ExecutionRunnerCfg): raise TypeError("runner_cfg must be an ExecutionRunnerCfg.") + selected_effect_monitors = ( + { + semantic_id: EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + for semantic_id in ( + "pick", + "place", + "hand_over", + "operate_articulation", + ) + } + if effect_monitors is None + else effect_monitors + ) + if not isinstance(selected_effect_monitors, Mapping): + raise TypeError("effect_monitors must be a mapping or None.") + normalized_effect_monitors: dict[str, EffectMonitorRef] = {} + for semantic_id, monitor_ref in selected_effect_monitors.items(): + _validate_identifier( + semantic_id, + field_name="SkillPolicyPreset effect semantic IDs", + ) + if not isinstance(monitor_ref, EffectMonitorRef): + raise TypeError( + "effect_monitors values must be EffectMonitorRef instances." + ) + normalized_effect_monitors[semantic_id] = monitor_ref.snapshot() object.__setattr__(self, "preset_id", preset_id) object.__setattr__(self, "schema_version", schema_version) object.__setattr__(self, "required_planner", required_planner) object.__setattr__(self, "_motion_policy", deepcopy(selected_motion)) object.__setattr__(self, "_recovery_policy", deepcopy(selected_recovery)) object.__setattr__(self, "_runner_cfg", deepcopy(selected_runner)) + object.__setattr__( + self, + "_effect_monitors", + MappingProxyType(normalized_effect_monitors), + ) @property def motion_policy(self) -> MotionPolicy: @@ -623,6 +748,16 @@ def runner_cfg(self) -> ExecutionRunnerCfg: """Return an independently owned runner configuration.""" return deepcopy(self._runner_cfg) + @property + def effect_monitors(self) -> Mapping[str, EffectMonitorRef]: + """Return effect-monitor selections keyed by exact semantic call ID.""" + return MappingProxyType( + { + semantic_id: monitor_ref.snapshot() + for semantic_id, monitor_ref in self._effect_monitors.items() + } + ) + def snapshot(self) -> SkillPolicyPreset: """Return an independently owned preset value.""" return SkillPolicyPreset( @@ -631,6 +766,7 @@ def snapshot(self) -> SkillPolicyPreset: motion_policy=self.motion_policy, recovery_policy=self.recovery_policy, runner_cfg=self.runner_cfg, + effect_monitors=self.effect_monitors, required_planner=self.required_planner, ) @@ -1473,6 +1609,12 @@ def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: endpoint=endpoint, adapter_id=adapter.adapter_id, runtime_target=resolution.runtime_target, + task_state_key=( + resource_id + if resolution.task_state_key is None + else resolution.task_state_key + ), + effect_sources=resolution.effect_sources, command_profile_key=resolution.command_profile_key, requires_command_profile=resolution.requires_command_profile, commands=( @@ -1841,7 +1983,7 @@ def _lower_binding( resource_id=resource.resource_id, adapter_id=endpoint.adapter_id, target=endpoint.runtime_target, - task_state_key=resource.resource_id, + task_state_key=endpoint.task_state_key, capabilities=endpoint.capabilities, commands=endpoint.commands, claim_tokens=endpoint.claim_tokens, diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index 69d4f8306..a877644b2 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -14,26 +14,29 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""High-level orchestration for semantic skill workflows.""" +"""Canonical execution service and convenience facade for semantic skills.""" from __future__ import annotations -from collections.abc import Callable, Iterable, Mapping -from copy import deepcopy -from dataclasses import dataclass +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, replace from enum import Enum -from types import MappingProxyType, TracebackType -from typing import TYPE_CHECKING +import math +from types import MappingProxyType +from typing import Protocol, runtime_checkable import torch +from ..atomic_actions.bindings import EndpointBinding from ..atomic_actions.engine import AtomicActionEngine from ..atomic_actions.execution import ( EffectVerificationRequest, EffectVerificationResult, ExecutionEvent, - ExecutionTick, + ExecutionPlanAttempt, ) +from ..atomic_actions.plans import ExecutionFeedbackMode, TrajectorySegment +from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy from ..atomic_actions.runner import ( CommandSink, ExecutionClock, @@ -43,261 +46,1199 @@ ObservationProvider, RunnerStatus, RunnerStep, - RunnerStepCallback, ) -from ..atomic_actions.sim_adapter import SimulationExecutionAdapter from ..atomic_actions.state import PlanningContext, TaskState -from .calls import ( - SemanticCallCatalog, - SemanticCallDescriptor, - SemanticCallSpec, - builtin_semantic_call_catalog, +from .calls import SemanticCallSpec +from .compiler import SemanticSkillCompiler +from .effects import ( + BinaryEffectEvidenceBatch, + EffectEvidenceBatch, + EffectMonitor, + EffectMonitorRef, + JointStateEvidenceBatch, + PoseRelationEvidenceBatch, + ScalarEffectEvidenceBatch, + SemanticEffectSpec, ) -from .compiler import ( - GroundedSemanticCall, - HandOverPoseProvider, - RegisteredSemanticLowerer, - RelationTargetGrounder, - SemanticSkillCompiler, - SemanticWorkflow, +from .scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, ) -from .integration import SceneManifest, SemanticIntegrationManifest -from .profiles import ( - ResourceEndpoint, - ResourceEndpointAdapter, - RobotSkillProfile, -) -from .scene import SceneRegistry - -if TYPE_CHECKING: - from embodichain.lab.sim.objects import Robot - from embodichain.lab.sim.planners import MotionGenerator - from embodichain.lab.sim.sim_manager import SimulationManager -SemanticEffectVerifier = Callable[ - [SemanticCallSpec, EffectVerificationRequest, PlanningContext], - torch.Tensor, -] -"""Verify one semantic effect and return a per-environment success mask.""" +def _snapshot_task_state(state: TaskState) -> TaskState: + """Return a tensor-owning snapshot of verified symbolic state.""" + return TaskState( + batch_size=state.batch_size, + device=state.device, + held_objects=state.held_objects, + coordinated_held_objects=state.coordinated_held_objects, + articulation_joints=state.articulation_joints, + ) + + +def _snapshot_event(event: ExecutionEvent) -> ExecutionEvent: + """Return an independently owned execution event.""" + return ExecutionEvent( + kind=event.kind, + timestamp=event.timestamp, + skill_id=event.skill_id, + invocation_id=event.invocation_id, + invocation_revision=event.invocation_revision, + invocation_index=event.invocation_index, + env_mask=event.env_mask, + message=event.message, + ) + + +def _metadata_value(value: object, *, depth: int = 0) -> object: + """Convert supported runtime diagnostics to deterministic JSON-safe data.""" + if depth > 16: + return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + return value if math.isfinite(value) else None + if isinstance(value, Enum): + return value.value + if isinstance(value, torch.Tensor): + return _metadata_value(value.detach().cpu().tolist(), depth=depth + 1) + if isinstance(value, torch.device): + return str(value) + if isinstance(value, Mapping): + items = sorted(value.items(), key=lambda item: str(item[0])) + if all(type(key) is str and key and key == key.strip() for key, _ in items): + return { + key: _metadata_value(nested, depth=depth + 1) for key, nested in items + } + return { + "__entries__": [ + { + "key": _metadata_value(key, depth=depth + 1), + "value": _metadata_value(nested, depth=depth + 1), + } + for key, nested in items + ] + } + if isinstance(value, (tuple, list)): + return [_metadata_value(nested, depth=depth + 1) for nested in value] + if isinstance(value, (set, frozenset)): + return [ + _metadata_value(nested, depth=depth + 1) + for nested in sorted(value, key=str) + ] + return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} + + +def _snapshot_metadata_mapping(value: Mapping[str, object]) -> Mapping[str, object]: + """Own one JSON-safe string-keyed metadata mapping.""" + if not isinstance(value, Mapping): + raise TypeError("metadata must be a mapping.") + normalized = _metadata_value(value) + if not isinstance(normalized, dict): + raise TypeError("metadata normalization must produce a dict.") + return MappingProxyType(normalized) + + +def _event_to_metadata(event: ExecutionEvent) -> dict[str, object]: + """Serialize one execution/recovery event without exposing tensors.""" + return { + "kind": event.kind.value, + "timestamp": _metadata_value(event.timestamp), + "skill_id": event.skill_id, + "invocation_id": event.invocation_id, + "invocation_revision": event.invocation_revision, + "invocation_index": event.invocation_index, + "env_mask": _metadata_value(event.env_mask), + "message": event.message, + } + + +def task_state_to_metadata(state: TaskState) -> dict[str, object]: + """Return verified symbolic task state as deterministic JSON-safe data.""" + if not isinstance(state, TaskState): + raise TypeError("state must be a TaskState.") + held = [] + for resource_id, value in sorted(state.held_objects.items()): + held.append( + { + "resource_id": resource_id, + "object_id": value.semantics.entity_id, + "object_label": value.semantics.label, + "object_to_eef": _metadata_value(value.object_to_eef), + "grasp_xpos": _metadata_value(value.grasp_xpos), + "active_mask": _metadata_value(value.env_mask), + } + ) + coordinated = [] + for resource_ids, value in sorted(state.coordinated_held_objects.items()): + coordinated.append( + { + "resource_ids": list(resource_ids), + "object_id": value.semantics.entity_id, + "object_label": value.semantics.label, + "left_object_to_eef": _metadata_value(value.left_object_to_eef), + "right_object_to_eef": _metadata_value(value.right_object_to_eef), + "left_grasp_xpos": _metadata_value(value.left_grasp_xpos), + "right_grasp_xpos": _metadata_value(value.right_grasp_xpos), + "active_mask": _metadata_value(value.env_mask), + } + ) + articulations = [] + for (articulation_id, joint_id), value in sorted(state.articulation_joints.items()): + articulations.append( + { + "articulation_id": articulation_id, + "joint_id": joint_id, + "position": _metadata_value(value.position), + "active_mask": _metadata_value(value.env_mask), + } + ) + return { + "batch_size": state.batch_size, + "device": str(state.device), + "held_objects": held, + "coordinated_held_objects": coordinated, + "articulation_joints": articulations, + } -class SemanticExecutionStatus(str, Enum): - """Lifecycle status of one semantic workflow segment.""" +class SkillStatus(str, Enum): + """Lifecycle state of one semantic workflow run.""" + IDLE = "idle" RUNNING = "running" - WAITING_FOR_EFFECT = "waiting_for_effect" COMPLETED = "completed" FAILED = "failed" CANCELLED = "cancelled" -class SemanticTaskStatus(str, Enum): - """Terminal status of one semantic task.""" +@dataclass(frozen=True, slots=True) +class SkillEndpointBindingTrace: + """JSON-safe typed projection of one resolved execution endpoint.""" - SUCCEEDED = "succeeded" - PARTIAL_SUCCESS = "partial_success" - FAILED = "failed" - CANCELLED = "cancelled" + slot_id: str + endpoint_id: str + resource_id: str + adapter_id: str + transport_id: str + target_id: str + target_type: str + task_state_key: str + capabilities: tuple[str, ...] + command_ids: tuple[str, ...] + claim_tokens: tuple[str, ...] + joint_ids: tuple[int, ...] + + def __post_init__(self) -> None: + for name in ( + "slot_id", + "endpoint_id", + "resource_id", + "adapter_id", + "transport_id", + "target_id", + "target_type", + "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.") + for name in ("capabilities", "command_ids", "claim_tokens"): + values = tuple(getattr(self, name)) + if tuple(sorted(set(values))) != values or not all( + type(value) is str and value for value in values + ): + raise ValueError(f"{name} must contain sorted unique identifiers.") + object.__setattr__(self, name, values) + joint_ids = tuple(self.joint_ids) + if len(set(joint_ids)) != len(joint_ids) or not all( + type(value) is int and value >= 0 for value in joint_ids + ): + raise ValueError("joint_ids must contain unique non-negative integers.") + object.__setattr__(self, "joint_ids", joint_ids) + + @classmethod + def from_binding(cls, binding: EndpointBinding) -> SkillEndpointBindingTrace: + """Project one owned endpoint binding without retaining its target.""" + if not isinstance(binding, EndpointBinding): + raise TypeError("binding must be an EndpointBinding.") + target = binding.target + return cls( + slot_id=binding.slot_id, + endpoint_id=binding.endpoint_id, + resource_id=binding.resource_id, + adapter_id=binding.adapter_id, + transport_id=target.transport_id, + target_id=target.target_id, + target_type=f"{type(target).__module__}.{type(target).__qualname__}", + task_state_key=binding.task_state_key, + capabilities=tuple(sorted(binding.capabilities)), + command_ids=tuple(sorted(binding.commands)), + claim_tokens=tuple(sorted(binding.claim_tokens)), + joint_ids=binding.joint_ids, + ) + + def to_metadata(self) -> dict[str, object]: + """Return stable endpoint, resource, adapter, and transport metadata.""" + return { + "slot_id": self.slot_id, + "endpoint_id": self.endpoint_id, + "resource_id": self.resource_id, + "adapter_id": self.adapter_id, + "transport_id": self.transport_id, + "target_id": self.target_id, + "target_type": self.target_type, + "task_state_key": self.task_state_key, + "capabilities": list(self.capabilities), + "command_ids": list(self.command_ids), + "claim_tokens": list(self.claim_tokens), + "joint_ids": list(self.joint_ids), + } + + +def _motion_policy_to_metadata(policy: MotionPolicy) -> dict[str, object]: + """Serialize one owned core motion policy without retaining planner objects.""" + plan_options = policy.plan_opts + options_metadata: object = None + if plan_options is not None: + values = ( + plan_options.to_dict() + if callable(getattr(plan_options, "to_dict", None)) + else None + ) + options_metadata = { + "type": f"{type(plan_options).__module__}.{type(plan_options).__qualname__}", + "values": _metadata_value(values), + } + return { + "strategy": policy.strategy, + "sample_count": policy.sample_count, + "dynamic_collision_mode": policy.dynamic_collision_mode.value, + "plan_options": options_metadata, + } + + +def _recovery_policy_to_metadata(policy: RecoveryPolicy) -> dict[str, object]: + """Serialize all bounded-recovery settings.""" + return { + "max_replans": policy.max_replans, + "max_action_retries": policy.max_action_retries, + "tracking_error_threshold": _metadata_value(policy.tracking_error_threshold), + "goal_translation_threshold": _metadata_value( + policy.goal_translation_threshold + ), + "goal_rotation_threshold": _metadata_value(policy.goal_rotation_threshold), + "action_timeout": _metadata_value(policy.action_timeout), + } + + +@dataclass(frozen=True, slots=True) +class ResolvedCorePolicyTrace: + """Resolved preset, core policies, and execution binding for one plan.""" + + profile_id: str + preset_id: str + preset_schema_version: int + motion_policy: MotionPolicy + recovery_policy: RecoveryPolicy + endpoints: tuple[SkillEndpointBindingTrace, ...] + + def __post_init__(self) -> None: + for name in ("profile_id", "preset_id"): + 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.preset_schema_version) is not int + or self.preset_schema_version < 1 + ): + raise ValueError("preset_schema_version must be a positive integer.") + if not isinstance(self.motion_policy, MotionPolicy): + raise TypeError("motion_policy must be a MotionPolicy.") + if not isinstance(self.recovery_policy, RecoveryPolicy): + raise TypeError("recovery_policy must be a RecoveryPolicy.") + endpoints = tuple(self.endpoints) + if not all(type(value) is SkillEndpointBindingTrace for value in endpoints): + raise TypeError( + "endpoints must contain exact SkillEndpointBindingTrace values." + ) + keys = tuple((value.slot_id, value.endpoint_id) for value in endpoints) + if len(set(keys)) != len(keys): + raise ValueError("endpoints must use unique slot/endpoint keys.") + object.__setattr__(self, "motion_policy", replace(self.motion_policy)) + object.__setattr__(self, "recovery_policy", replace(self.recovery_policy)) + object.__setattr__(self, "endpoints", endpoints) + + @classmethod + def from_resolved_binding( + cls, + *, + profile_id: str, + preset_id: str, + preset_schema_version: int, + motion_policy: MotionPolicy, + recovery_policy: RecoveryPolicy, + endpoints: Iterable[EndpointBinding], + ) -> ResolvedCorePolicyTrace: + """Project one resolved preset and action binding to a trace.""" + return cls( + profile_id=profile_id, + preset_id=preset_id, + preset_schema_version=preset_schema_version, + motion_policy=motion_policy, + recovery_policy=recovery_policy, + endpoints=tuple( + SkillEndpointBindingTrace.from_binding(endpoint) + for endpoint in endpoints + ), + ) + + def snapshot(self) -> ResolvedCorePolicyTrace: + """Return an independently owned core-policy and binding trace.""" + return ResolvedCorePolicyTrace( + profile_id=self.profile_id, + preset_id=self.preset_id, + preset_schema_version=self.preset_schema_version, + motion_policy=self.motion_policy, + recovery_policy=self.recovery_policy, + endpoints=self.endpoints, + ) + + def to_metadata(self) -> dict[str, object]: + """Return deterministic policy and endpoint-binding metadata.""" + return { + "profile_id": self.profile_id, + "preset": { + "preset_id": self.preset_id, + "schema_version": self.preset_schema_version, + }, + "motion_policy": _motion_policy_to_metadata(self.motion_policy), + "recovery_policy": _recovery_policy_to_metadata(self.recovery_policy), + "endpoints": [endpoint.to_metadata() for endpoint in self.endpoints], + } @dataclass(frozen=True, slots=True, eq=False) -class SemanticCallRecord: - """Terminal execution record for one grounded semantic call.""" +class SkillPlanAttemptTrace: + """Compact, typed trace of one installed action-plan generation. - call_index: int - semantic_id: str + ``scene_dependency_monitor_until`` preserves the plan's per-entity exclusive + waypoint cutoff: an entity is monitored only while the current waypoint index + is smaller than its configured value. + """ + + attempt_generation: int + trigger: str + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] skill_id: str invocation_id: str | None invocation_revision: int - status: RunnerStatus - eligible_mask: torch.Tensor - events: tuple[ExecutionEvent, ...] - command_count: int - message: str | None = None + plan_success_mask: torch.Tensor + command_frame_count: int + trajectory_segments: tuple[TrajectorySegment, ...] + planned_scene_version: int + planned_collision_world_revision: tuple[int, ...] + scene_dependencies: tuple[str, ...] + scene_dependency_monitor_until: Mapping[str, int] + collision_world_sensitive: bool + replannable: bool + feedback_mode: ExecutionFeedbackMode + effect_verification_kind: str | None + resolved_core_policy: ResolvedCorePolicyTrace + planner_backend: str + planner_messages: tuple[str, ...] + planner_metadata: Mapping[str, object] def __post_init__(self) -> None: - if self.call_index < 0: - raise ValueError("call_index must be non-negative.") - for name in ("semantic_id", "skill_id"): + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be non-negative.") + if type(self.trigger) is not str or not self.trigger: + raise ValueError("trigger must be a non-empty string.") + if not math.isfinite(self.planned_at) or self.planned_at < 0.0: + raise ValueError("planned_at must be finite and non-negative.") + if type(self.invocation_index) is not int or self.invocation_index < 0: + raise ValueError("invocation_index must be non-negative.") + for name in ("planned_mask", "plan_success_mask"): value = getattr(self, name) - if not isinstance(value, str) or not value: - raise ValueError(f"{name} must be a non-empty string.") - if self.invocation_revision < 0: - raise ValueError("invocation_revision must be non-negative.") + if ( + not isinstance(value, torch.Tensor) + or value.dtype != torch.bool + or value.dim() != 1 + ): + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if self.planned_mask.shape != self.plan_success_mask.shape: + raise ValueError("Plan-attempt masks must have equal shapes.") + if self.planned_mask.device != self.plan_success_mask.device: + raise ValueError("Plan-attempt masks must share a device.") + batch_size = int(self.planned_mask.numel()) + retries = tuple(self.action_retry_counts) + replans = tuple(self.replan_counts) + if len(retries) != batch_size or len(replans) != batch_size: + raise ValueError("Recovery counters must contain one value per row.") + if any(type(value) is not int or value < 0 for value in (*retries, *replans)): + raise ValueError("Recovery counters must be non-negative integers.") + if 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 ( - not isinstance(self.invocation_id, str) or not self.invocation_id + 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 isinstance(self.status, RunnerStatus): - raise TypeError("status must be a RunnerStatus.") + if type(self.invocation_revision) is not int or self.invocation_revision < 0: + raise ValueError("invocation_revision must be non-negative.") + if type(self.command_frame_count) is not int or self.command_frame_count < 0: + raise ValueError("command_frame_count must be non-negative.") + segments = tuple(self.trajectory_segments) + if not all(type(value) is TrajectorySegment for value in segments): + raise TypeError( + "trajectory_segments must contain TrajectorySegment values." + ) if ( - not isinstance(self.eligible_mask, torch.Tensor) - or self.eligible_mask.dtype != torch.bool - or self.eligible_mask.dim() != 1 + type(self.planned_scene_version) is not int + or self.planned_scene_version < 0 ): - raise ValueError("eligible_mask must be a one-dimensional bool tensor.") - if not all(isinstance(event, ExecutionEvent) for event in self.events): - raise TypeError("events must contain ExecutionEvent values.") - if self.command_count < 0: - raise ValueError("command_count must be non-negative.") - if self.message is not None and not isinstance(self.message, str): - raise TypeError("message must be a string or None.") - object.__setattr__(self, "eligible_mask", self.eligible_mask.clone()) - object.__setattr__(self, "events", tuple(self.events)) + raise ValueError("planned_scene_version must be non-negative.") + collision_revisions = tuple(self.planned_collision_world_revision) + if len(collision_revisions) != batch_size or any( + type(value) is not int or value < 0 for value in collision_revisions + ): + raise ValueError( + "planned_collision_world_revision must contain one non-negative " + "integer per row." + ) + dependencies = tuple(self.scene_dependencies) + if len(set(dependencies)) != len(dependencies) or not all( + type(value) is str and value for value in dependencies + ): + raise ValueError("scene_dependencies must contain unique identifiers.") + if not isinstance(self.scene_dependency_monitor_until, Mapping): + raise TypeError("scene_dependency_monitor_until must be a mapping.") + monitor_until = dict(self.scene_dependency_monitor_until) + if not set(monitor_until).issubset(dependencies): + raise ValueError( + "scene_dependency_monitor_until keys must be scene dependencies." + ) + for entity_id, waypoint_index in monitor_until.items(): + if ( + type(entity_id) is not str + or not entity_id + or type(waypoint_index) is not int + or not 0 <= waypoint_index <= self.command_frame_count + ): + raise ValueError( + "scene_dependency_monitor_until must map non-empty entity IDs " + "to waypoint indices within the command sequence." + ) + if type(self.collision_world_sensitive) is not bool: + raise TypeError("collision_world_sensitive must be a bool.") + if type(self.replannable) is not bool: + raise TypeError("replannable must be a bool.") + if not isinstance(self.feedback_mode, ExecutionFeedbackMode): + raise TypeError("feedback_mode must be an ExecutionFeedbackMode.") + if self.effect_verification_kind is not None and ( + type(self.effect_verification_kind) is not str + or not self.effect_verification_kind + ): + raise ValueError("effect_verification_kind must be non-empty or None.") + if type(self.resolved_core_policy) is not ResolvedCorePolicyTrace: + raise TypeError( + "resolved_core_policy must be exactly ResolvedCorePolicyTrace." + ) + if type(self.planner_backend) is not str or not self.planner_backend: + raise ValueError("planner_backend must be a non-empty string.") + messages = tuple(self.planner_messages) + if not all(type(value) is str for value in messages): + raise TypeError("planner_messages must contain strings.") + object.__setattr__(self, "planned_mask", self.planned_mask.clone()) + object.__setattr__(self, "plan_success_mask", self.plan_success_mask.clone()) + object.__setattr__(self, "action_retry_counts", retries) + object.__setattr__(self, "replan_counts", replans) + object.__setattr__(self, "trajectory_segments", segments) + object.__setattr__( + self, + "planned_collision_world_revision", + collision_revisions, + ) + object.__setattr__(self, "scene_dependencies", dependencies) + object.__setattr__( + self, + "scene_dependency_monitor_until", + MappingProxyType(monitor_until), + ) + object.__setattr__( + self, + "resolved_core_policy", + self.resolved_core_policy.snapshot(), + ) + object.__setattr__(self, "planner_messages", messages) + object.__setattr__( + self, + "planner_metadata", + _snapshot_metadata_mapping(self.planner_metadata), + ) + + @classmethod + def from_execution_attempt( + cls, + attempt: ExecutionPlanAttempt, + *, + profile_id: str, + preset_id: str, + preset_schema_version: int, + ) -> SkillPlanAttemptTrace: + """Project one session-owned plan attempt to compact trace metadata.""" + if not isinstance(attempt, ExecutionPlanAttempt): + raise TypeError("attempt must be an ExecutionPlanAttempt.") + plan = attempt.plan + request = attempt.request + return cls( + attempt_generation=attempt.attempt_generation, + trigger=attempt.event_kind.value, + planned_at=attempt.planned_at, + invocation_index=attempt.invocation_index, + planned_mask=attempt.planned_mask, + action_retry_counts=attempt.action_retry_counts, + replan_counts=attempt.replan_counts, + skill_id=plan.skill_id, + invocation_id=plan.invocation_id, + invocation_revision=plan.invocation_revision, + plan_success_mask=plan.plan_success, + command_frame_count=plan.commands.frame_count, + trajectory_segments=plan.segments, + planned_scene_version=plan.planned_scene_version, + planned_collision_world_revision=plan.planned_collision_world_revision, + scene_dependencies=plan.scene_dependencies, + scene_dependency_monitor_until=plan.scene_dependency_monitor_until, + collision_world_sensitive=plan.collision_world_sensitive, + replannable=plan.replannable, + feedback_mode=plan.feedback_mode, + effect_verification_kind=( + None + if plan.effect_verification is None + else plan.effect_verification.kind + ), + resolved_core_policy=ResolvedCorePolicyTrace.from_resolved_binding( + profile_id=profile_id, + preset_id=preset_id, + preset_schema_version=preset_schema_version, + motion_policy=request.motion_policy, + recovery_policy=request.recovery_policy, + endpoints=request.binding.endpoints, + ), + planner_backend=plan.diagnostics.backend, + planner_messages=plan.diagnostics.messages, + planner_metadata=plan.diagnostics.metadata, + ) + + def snapshot(self) -> SkillPlanAttemptTrace: + """Return an independently owned compact plan-attempt trace.""" + return SkillPlanAttemptTrace( + attempt_generation=self.attempt_generation, + trigger=self.trigger, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + plan_success_mask=self.plan_success_mask, + command_frame_count=self.command_frame_count, + trajectory_segments=self.trajectory_segments, + planned_scene_version=self.planned_scene_version, + planned_collision_world_revision=self.planned_collision_world_revision, + scene_dependencies=self.scene_dependencies, + scene_dependency_monitor_until=self.scene_dependency_monitor_until, + collision_world_sensitive=self.collision_world_sensitive, + replannable=self.replannable, + feedback_mode=self.feedback_mode, + effect_verification_kind=self.effect_verification_kind, + resolved_core_policy=self.resolved_core_policy, + planner_backend=self.planner_backend, + planner_messages=self.planner_messages, + planner_metadata=self.planner_metadata, + ) + + def to_metadata(self) -> dict[str, object]: + """Return one plan generation as deterministic JSON-safe data.""" + return { + "attempt_generation": self.attempt_generation, + "trigger": self.trigger, + "planned_at": self.planned_at, + "invocation_index": self.invocation_index, + "planned_mask": _metadata_value(self.planned_mask), + "recovery_counters": { + "action_retries": list(self.action_retry_counts), + "replans": list(self.replan_counts), + }, + "skill_id": self.skill_id, + "invocation_id": self.invocation_id, + "invocation_revision": self.invocation_revision, + "plan_success_mask": _metadata_value(self.plan_success_mask), + "command_frame_count": self.command_frame_count, + "trajectory_segments": [ + { + "name": segment.name, + "start": segment.start, + "stop": segment.stop, + "waypoint_count": segment.waypoint_count, + } + for segment in self.trajectory_segments + ], + "planned_scene_version": self.planned_scene_version, + "planned_collision_world_revision": list( + self.planned_collision_world_revision + ), + "scene_dependencies": list(self.scene_dependencies), + "scene_dependency_monitor_until": { + entity_id: self.scene_dependency_monitor_until[entity_id] + for entity_id in sorted(self.scene_dependency_monitor_until) + }, + "collision_world_sensitive": self.collision_world_sensitive, + "replannable": self.replannable, + "feedback_mode": self.feedback_mode.value, + "effect_verification_kind": self.effect_verification_kind, + "resolved_core_policy": self.resolved_core_policy.to_metadata(), + "planner_diagnostics": { + "backend": self.planner_backend, + "messages": list(self.planner_messages), + "metadata": _metadata_value(self.planner_metadata), + }, + } @dataclass(frozen=True, slots=True, eq=False) -class SemanticSegmentResult: - """Terminal result of one statically analyzed workflow segment.""" +class SkillEffectTrace: + """One monitor decision correlated with an atomic verification boundary.""" - segment_id: str - workflow_id: str - status: SemanticExecutionStatus - eligible_mask: torch.Tensor - task_state: TaskState - calls: tuple[SemanticCallRecord, ...] - message: str | None = None + call_index: int + verification_id: int + observation_revision: int + timestamp: float + success_mask: torch.Tensor + failure_mask: torch.Tensor + 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] + + def __post_init__(self) -> None: + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + 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.observation_revision) is not int or self.observation_revision < 0: + raise ValueError("observation_revision must be non-negative.") + 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"): + 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("Effect trace masks must have equal shapes.") + if self.success_mask.device != self.failure_mask.device: + raise ValueError("Effect trace masks must share a device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("Effect trace masks must not overlap.") + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec.") + 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 ( + type(self.monitor_revision) is not str or not self.monitor_revision + ): + raise ValueError("monitor_revision must be non-empty or None.") + evidence_types = ( + PoseRelationEvidenceBatch, + BinaryEffectEvidenceBatch, + ScalarEffectEvidenceBatch, + JointStateEvidenceBatch, + ) + evidence: dict[str, EffectEvidenceBatch] = {} + for evidence_id, batch in self.evidence.items(): + if type(evidence_id) is not str or not evidence_id: + raise ValueError("evidence keys must be non-empty strings.") + if type(batch) not in evidence_types: + raise TypeError("evidence values must be exact evidence batches.") + if batch.evidence_id != evidence_id: + raise ValueError("evidence keys must match batch evidence_id values.") + 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, "effect_spec", self.effect_spec.snapshot()) + object.__setattr__( + self, + "configured_monitor_params", + _snapshot_metadata_mapping(self.configured_monitor_params), + ) + object.__setattr__( + self, + "resolved_monitor_params", + _snapshot_metadata_mapping(self.resolved_monitor_params), + ) + object.__setattr__(self, "evidence", MappingProxyType(evidence)) + + def snapshot(self) -> SkillEffectTrace: + """Return an independently owned trace.""" + return SkillEffectTrace( + call_index=self.call_index, + verification_id=self.verification_id, + observation_revision=self.observation_revision, + timestamp=self.timestamp, + success_mask=self.success_mask, + failure_mask=self.failure_mask, + 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, + ) + + def to_metadata(self) -> dict[str, object]: + """Return monitor contract, evidence, thresholds, and decision metadata.""" + return { + "call_index": self.call_index, + "verification_id": self.verification_id, + "observation_revision": self.observation_revision, + "timestamp": self.timestamp, + "effect_spec": self.effect_spec.to_metadata(), + "monitor": { + "monitor_id": self.monitor_id, + "revision": self.monitor_revision, + "configured_params": _metadata_value(self.configured_monitor_params), + "resolved_params": _metadata_value(self.resolved_monitor_params), + }, + "evidence": { + evidence_id: batch.to_metadata() + for evidence_id, batch in sorted(self.evidence.items()) + }, + "decision": { + "success_mask": _metadata_value(self.success_mask), + "failure_mask": _metadata_value(self.failure_mask), + }, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillFailure: + """Per-environment semantic workflow failure.""" + + call_index: int + semantic_id: str + env_mask: torch.Tensor + message: str + + def __post_init__(self) -> None: + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + if type(self.semantic_id) is not str or not self.semantic_id: + raise ValueError("semantic_id 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 type(self.message) is not str or not self.message: + raise ValueError("message must be a non-empty string.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + + def snapshot(self) -> SkillFailure: + """Return an independently owned failure.""" + return SkillFailure( + call_index=self.call_index, + semantic_id=self.semantic_id, + env_mask=self.env_mask, + message=self.message, + ) + + def to_metadata(self) -> dict[str, object]: + """Return one row-local failure as JSON-safe data.""" + return { + "call_index": self.call_index, + "semantic_id": self.semantic_id, + "env_mask": _metadata_value(self.env_mask), + "message": self.message, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillCallTrace: + """Terminal trace for exactly one semantic call and execution session.""" + + call_index: int + semantic_id: str + call_metadata: Mapping[str, object] + skill_id: str + invocation_id: str | None + invocation_revision: int + status: RunnerStatus + entered_mask: torch.Tensor + completed_mask: torch.Tensor + failed_mask: torch.Tensor + command_count: int + resolved_core_policy: ResolvedCorePolicyTrace + plan_attempts: tuple[SkillPlanAttemptTrace, ...] + events: tuple[ExecutionEvent, ...] = () + effects: tuple[SkillEffectTrace, ...] = () def __post_init__(self) -> None: - for name in ("segment_id", "workflow_id"): + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + for name in ("semantic_id", "skill_id"): value = getattr(self, name) - if not isinstance(value, str) or not value: + if type(value) is not str or not value: raise ValueError(f"{name} must be a non-empty string.") - if not isinstance(self.status, SemanticExecutionStatus) or self.status not in { - SemanticExecutionStatus.COMPLETED, - SemanticExecutionStatus.FAILED, - SemanticExecutionStatus.CANCELLED, - }: - raise ValueError("SemanticSegmentResult status must be terminal.") - if ( - not isinstance(self.eligible_mask, torch.Tensor) - or self.eligible_mask.dtype != torch.bool - or self.eligible_mask.dim() != 1 + normalized_call = _snapshot_metadata_mapping(self.call_metadata) + if normalized_call.get("semantic_id") != self.semantic_id: + raise ValueError("call_metadata semantic_id must match semantic_id.") + if self.invocation_id is not None and ( + type(self.invocation_id) is not str or not self.invocation_id ): - raise ValueError("eligible_mask must be a one-dimensional bool tensor.") - if not isinstance(self.task_state, TaskState): - raise TypeError("task_state must be a TaskState.") - if not all(isinstance(call, SemanticCallRecord) for call in self.calls): - raise TypeError("calls must contain SemanticCallRecord values.") - if self.message is not None and not isinstance(self.message, str): - raise TypeError("message must be a string or None.") - object.__setattr__(self, "eligible_mask", self.eligible_mask.clone()) - object.__setattr__(self, "calls", tuple(self.calls)) + raise ValueError("invocation_id must be a non-empty string or None.") + if self.invocation_revision < 0: + raise ValueError("invocation_revision must be non-negative.") + if not isinstance(self.status, RunnerStatus): + raise TypeError("status must be a RunnerStatus.") + if self.status is RunnerStatus.RUNNING: + raise ValueError("A terminal call trace cannot have running status.") + 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 + ): + raise ValueError("Call trace masks must have equal shapes.") + if not ( + self.entered_mask.device + == self.completed_mask.device + == self.failed_mask.device + ): + raise ValueError("Call trace masks must share a device.") + if (self.completed_mask & ~self.entered_mask).any(): + raise ValueError("completed_mask must be a subset of entered_mask.") + if (self.failed_mask & ~self.entered_mask).any(): + raise ValueError("failed_mask must be a subset of entered_mask.") + if (self.completed_mask & self.failed_mask).any(): + raise ValueError("completed_mask and failed_mask must not overlap.") + if type(self.command_count) is not int or self.command_count < 0: + raise ValueError("command_count must be non-negative.") + if type(self.resolved_core_policy) is not ResolvedCorePolicyTrace: + raise TypeError( + "resolved_core_policy must be exactly ResolvedCorePolicyTrace." + ) + attempts = tuple(self.plan_attempts) + if not all(type(attempt) is SkillPlanAttemptTrace for attempt in attempts): + raise TypeError("plan_attempts must contain SkillPlanAttemptTrace values.") + if attempts: + generations = tuple(attempt.attempt_generation for attempt in attempts) + if generations != tuple( + range(generations[0], generations[0] + len(attempts)) + ): + raise ValueError( + "plan_attempts must use contiguous ordered generations." + ) + if attempts[-1].skill_id != self.skill_id: + raise ValueError("The active plan-attempt skill must match skill_id.") + elif self.status is not RunnerStatus.FAILED or self.command_count != 0: + raise ValueError( + "Only a preparation failure with no commands may omit plan_attempts." + ) + object.__setattr__(self, "entered_mask", self.entered_mask.clone()) + object.__setattr__(self, "completed_mask", self.completed_mask.clone()) + object.__setattr__(self, "failed_mask", self.failed_mask.clone()) + object.__setattr__(self, "call_metadata", normalized_call) + object.__setattr__( + self, + "resolved_core_policy", + self.resolved_core_policy.snapshot(), + ) + object.__setattr__( + self, + "plan_attempts", + tuple(attempt.snapshot() for attempt in attempts), + ) + object.__setattr__( + self, + "events", + tuple(_snapshot_event(event) for event in self.events), + ) + object.__setattr__( + self, + "effects", + tuple(effect.snapshot() for effect in self.effects), + ) + + def snapshot(self) -> SkillCallTrace: + """Return an independently owned call trace.""" + return SkillCallTrace( + call_index=self.call_index, + semantic_id=self.semantic_id, + call_metadata=self.call_metadata, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + status=self.status, + entered_mask=self.entered_mask, + completed_mask=self.completed_mask, + failed_mask=self.failed_mask, + command_count=self.command_count, + resolved_core_policy=self.resolved_core_policy, + plan_attempts=self.plan_attempts, + events=self.events, + effects=self.effects, + ) @property - def events(self) -> tuple[ExecutionEvent, ...]: - """Return all structured execution events in call order.""" - return tuple(event for call in self.calls for event in call.events) + def active_plan(self) -> SkillPlanAttemptTrace: + """Return the final installed plan generation as an owned trace.""" + if not self.plan_attempts: + raise RuntimeError("This call failed before an action plan was installed.") + return self.plan_attempts[-1].snapshot() + + def to_metadata(self) -> dict[str, object]: + """Return one semantic call, recovery history, and effects as JSON-safe data.""" + attempts = [attempt.to_metadata() for attempt in self.plan_attempts] + return { + "call_index": self.call_index, + "semantic_id": self.semantic_id, + "call": _metadata_value(self.call_metadata), + "skill_id": self.skill_id, + "invocation_id": self.invocation_id, + "invocation_revision": self.invocation_revision, + "status": self.status.value, + "masks": { + "entered": _metadata_value(self.entered_mask), + "completed": _metadata_value(self.completed_mask), + "failed": _metadata_value(self.failed_mask), + }, + "command_count": self.command_count, + "active_plan_attempt_generation": ( + None + if not self.plan_attempts + else self.plan_attempts[-1].attempt_generation + ), + "resolved_core_policy": self.resolved_core_policy.to_metadata(), + "plan_attempts": attempts, + "events": [_event_to_metadata(event) for event in self.events], + "effects": [effect.to_metadata() for effect in self.effects], + } @dataclass(frozen=True, slots=True, eq=False) -class SemanticTaskResult: - """Terminal result of a complete static or dynamically segmented task.""" - - task_id: str - status: SemanticTaskStatus - initial_eligible_mask: torch.Tensor +class SkillResult: + """Immutable workflow snapshot returned by sync and step-wise execution.""" + + status: SkillStatus + workflow_id: str | None + current_call_index: int | None + env_ids: torch.Tensor + success_mask: torch.Tensor + failure_mask: torch.Tensor + cancelled_mask: torch.Tensor eligible_mask: torch.Tensor task_state: TaskState - latest_context: PlanningContext - segments: tuple[SemanticSegmentResult, ...] + events: tuple[ExecutionEvent, ...] = () + calls: tuple[SkillCallTrace, ...] = () + effects: tuple[SkillEffectTrace, ...] = () + failures: tuple[SkillFailure, ...] = () + wait_duration: float = 0.0 message: str | None = None def __post_init__(self) -> None: - if not isinstance(self.task_id, str) or not self.task_id: - raise ValueError("task_id must be a non-empty string.") - if not isinstance(self.status, SemanticTaskStatus): - raise TypeError("status must be a SemanticTaskStatus.") - for name in ("initial_eligible_mask", "eligible_mask"): + if not isinstance(self.status, SkillStatus): + raise TypeError("status must be a SkillStatus.") + if self.workflow_id is not None and ( + type(self.workflow_id) is not str or not self.workflow_id + ): + raise ValueError("workflow_id must be a non-empty string or None.") + if self.current_call_index is not None and ( + type(self.current_call_index) is not int or self.current_call_index < 0 + ): + raise ValueError("current_call_index must be non-negative or None.") + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if self.env_ids.dtype != torch.long or self.env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if self.env_ids.numel() == 0: + raise ValueError("env_ids must contain at least one environment.") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + batch_size = int(self.env_ids.numel()) + for name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "eligible_mask", + ): value = getattr(self, name) - if ( - not isinstance(value, torch.Tensor) - or value.dtype != torch.bool - or value.dim() != 1 - ): - raise ValueError(f"{name} must be a one-dimensional bool tensor.") - object.__setattr__(self, name, value.clone()) - if self.initial_eligible_mask.shape != self.eligible_mask.shape: - raise ValueError("Task eligibility masks must share a shape.") + 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},).") + if value.device != self.env_ids.device: + raise ValueError(f"{name} and env_ids must share a device.") + if (self.success_mask & (self.failure_mask | self.cancelled_mask)).any(): + raise ValueError("Successful rows cannot also fail or be cancelled.") + if (self.failure_mask & self.cancelled_mask).any(): + raise ValueError("Failed and cancelled masks must not overlap.") + if (self.eligible_mask & (self.failure_mask | self.cancelled_mask)).any(): + raise ValueError("Eligible rows cannot also fail or be cancelled.") + if (self.success_mask & ~self.eligible_mask).any(): + raise ValueError("success_mask must be a subset of eligible_mask.") if not isinstance(self.task_state, TaskState): raise TypeError("task_state must be a TaskState.") - if not isinstance(self.latest_context, PlanningContext): - raise TypeError("latest_context must be a PlanningContext.") - if not all(isinstance(item, SemanticSegmentResult) for item in self.segments): - raise TypeError("segments must contain SemanticSegmentResult values.") - if self.message is not None and not isinstance(self.message, str): + if self.task_state.batch_size != batch_size: + raise ValueError("task_state batch size must match env_ids.") + if self.task_state.device != self.env_ids.device: + raise ValueError("task_state and env_ids must share a device.") + if not math.isfinite(self.wait_duration) or self.wait_duration < 0.0: + 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.") - object.__setattr__(self, "segments", tuple(self.segments)) + object.__setattr__(self, "env_ids", self.env_ids.clone()) + for name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "eligible_mask", + ): + object.__setattr__(self, name, getattr(self, name).clone()) + object.__setattr__(self, "task_state", _snapshot_task_state(self.task_state)) + object.__setattr__( + self, + "events", + tuple(_snapshot_event(event) for event in self.events), + ) + object.__setattr__( + self, + "calls", + tuple(call.snapshot() for call in self.calls), + ) + object.__setattr__( + self, + "effects", + tuple(effect.snapshot() for effect in self.effects), + ) + object.__setattr__( + self, + "failures", + tuple(failure.snapshot() for failure in self.failures), + ) @property - def events(self) -> tuple[ExecutionEvent, ...]: - """Return all structured execution events in segment order.""" - return tuple(event for segment in self.segments for event in segment.events) - - def require_all_succeeded(self) -> None: - """Require every initially eligible environment to have succeeded. - - Raises: - RuntimeError: If the task failed, was cancelled, or retained only a - subset of its initial environment cohort. + def terminal(self) -> bool: + """Whether the workflow no longer accepts execution steps.""" + return self.status in { + SkillStatus.COMPLETED, + SkillStatus.FAILED, + SkillStatus.CANCELLED, + } + + 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)``. """ - if self.status is not SemanticTaskStatus.SUCCEEDED: - detail = "" if self.message is None else f" {self.message}" - raise RuntimeError( - f"Semantic task {self.task_id!r} finished with " - f"{self.status.value!r}.{detail}" - ) + return { + "schema_version": 1, + "kind": "skill_result", + "status": self.status.value, + "workflow_id": self.workflow_id, + "current_call_index": self.current_call_index, + "env_ids": _metadata_value(self.env_ids), + "masks": { + "success": _metadata_value(self.success_mask), + "failure": _metadata_value(self.failure_mask), + "cancelled": _metadata_value(self.cancelled_mask), + "eligible": _metadata_value(self.eligible_mask), + }, + "task_state": task_state_to_metadata(self.task_state), + "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], + "failures": [failure.to_metadata() for failure in self.failures], + "wait_duration": self.wait_duration, + "message": self.message, + } + + +@runtime_checkable +class EffectEvidenceCollectorPort(Protocol): + """Minimal collector surface consumed by :class:`SkillRuntime`.""" + + def collect( + self, + spec: SemanticEffectSpec, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire synchronized raw evidence for one grounded effect.""" -@dataclass(frozen=True, slots=True, eq=False) -class SemanticExecutionStep: - """Latest high-level state after advancing a semantic segment.""" +@runtime_checkable +class SkillRuntimeProvider(Protocol): + """Explicit environment adapter installed for :meth:`AtomicSkills.from_env`.""" - status: SemanticExecutionStatus - task_id: str - segment_id: str - call_index: int - eligible_mask: torch.Tensor - runner_step: RunnerStep | None = None - pending_effect: EffectVerificationRequest | None = None - message: str | None = None + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + """Build a fully connected semantic runtime for this environment.""" - def __post_init__(self) -> None: - if not isinstance(self.status, SemanticExecutionStatus): - raise TypeError("status must be a SemanticExecutionStatus.") - if not isinstance(self.task_id, str) or not self.task_id: - raise ValueError("task_id must be a non-empty string.") - if not isinstance(self.segment_id, str) or not self.segment_id: - raise ValueError("segment_id must be a non-empty string.") - if self.call_index < 0: - raise ValueError("call_index must be non-negative.") - if ( - not isinstance(self.eligible_mask, torch.Tensor) - or self.eligible_mask.dtype != torch.bool - or self.eligible_mask.dim() != 1 - ): - raise ValueError("eligible_mask must be a one-dimensional bool tensor.") - if self.message is not None and not isinstance(self.message, str): - raise TypeError("message must be a string or None.") - object.__setattr__(self, "eligible_mask", self.eligible_mask.clone()) +class _PrimedObservationProvider: + """Return a JIT-grounding observation once before delegating fresh reads.""" -class SemanticSkillRuntime: - """Bind semantic declarations to closed-loop planning and execution ports. + def __init__( + self, + context: PlanningContext, + delegate: ObservationProvider, + ) -> None: + self._context: PlanningContext | None = context + self._delegate = delegate + + def observe(self, task_state: TaskState) -> PlanningContext: + """Reuse the grounding snapshot for the session's first due cycle.""" + context = self._context + if context is None: + return self._delegate.observe(task_state) + self._context = None + return PlanningContext( + robot=context.robot, + task=task_state, + scene=context.scene, + env_ids=context.env_ids, + ) - The runtime is intentionally thin. It owns one compiler and the controller - ports used to construct existing :class:`ExecutionRunner` instances. A - :class:`SemanticTask` owns verified symbolic state across workflow segments. - Args: - compiler: Bound compiler used for static analysis and JIT grounding. - observation_provider: Source of fresh robot and scene observations. - command_sink: Destination for controller commands and safe-stop requests. - clock: Optional execution clock. Wall-clock time is used when omitted. - effect_verifier: Optional default callback for physical effect checks. - runner_cfg: Optional transport and scheduling policy overriding each - call's skill preset. +class SkillRuntime: + """JIT-ground and execute semantic calls through one runner per call. + + Static workflow analysis occurs once in :meth:`start`. Each call then gets + a fresh observation, one grounded invocation, one execution session, and + one :class:`ExecutionRunner`. Verified task state and row eligibility cross + call barriers; execution sessions never do. """ def __init__( @@ -305,9 +1246,10 @@ def __init__( compiler: SemanticSkillCompiler, observation_provider: ObservationProvider, command_sink: CommandSink, + evidence_collector: EffectEvidenceCollectorPort, *, + task_state: TaskState | None = None, clock: ExecutionClock | None = None, - effect_verifier: SemanticEffectVerifier | None = None, runner_cfg: ExecutionRunnerCfg | None = None, ) -> None: if not isinstance(compiler, SemanticSkillCompiler): @@ -316,1166 +1258,1033 @@ def __init__( raise TypeError("observation_provider must implement ObservationProvider.") if not isinstance(command_sink, CommandSink): raise TypeError("command_sink must implement CommandSink.") + if not isinstance(evidence_collector, EffectEvidenceCollectorPort): + raise TypeError( + "evidence_collector must implement EffectEvidenceCollectorPort." + ) if clock is not None and not isinstance(clock, ExecutionClock): raise TypeError("clock must implement ExecutionClock.") - if effect_verifier is not None and not callable(effect_verifier): - raise TypeError("effect_verifier must be callable or None.") if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") - self.compiler = compiler - self.observation_provider = observation_provider - self.command_sink = command_sink - self.clock = MonotonicExecutionClock() if clock is None else clock - self.effect_verifier = effect_verifier - self.runner_cfg: ExecutionRunnerCfg | None = ( - None if runner_cfg is None else deepcopy(runner_cfg) + integration = compiler.integration + engine = integration.engine + if not isinstance(engine, AtomicActionEngine): + raise TypeError( + "compiler.integration.engine must be an AtomicActionEngine." + ) + initial_task = ( + engine.initial_context().task if task_state is None else task_state + ) + if not isinstance(initial_task, TaskState): + raise TypeError("task_state must be a TaskState or None.") + if initial_task.device != engine.device: + raise ValueError("task_state and compiler engine must share a device.") + + self._compiler = compiler + self._engine = engine + self._observation_provider = observation_provider + self._command_sink = command_sink + self._evidence_collector = evidence_collector + self._clock = clock or MonotonicExecutionClock() + self._runner_cfg = runner_cfg or ExecutionRunnerCfg() + self._task_state = _snapshot_task_state(initial_task) + self._env_ids = torch.arange( + self._task_state.batch_size, + dtype=torch.long, + device=self._task_state.device, + ) + self._has_observed_env_ids = False + self._status = SkillStatus.IDLE + self._workflow: object | None = None + self._workflow_id: str | None = None + self._calls: tuple[SemanticCallSpec, ...] = () + self._execution_prefix_length = 0 + self._current_call_index: int | None = None + self._runner: ExecutionRunner | None = None + self._grounded: object | None = None + self._call_entered_mask = torch.zeros( + self._task_state.batch_size, + dtype=torch.bool, + device=self._task_state.device, ) - self._active_task: SemanticTask | None = None + self._eligible = torch.ones_like(self._call_entered_mask) + self._success = torch.zeros_like(self._eligible) + self._failed = torch.zeros_like(self._eligible) + self._cancelled = torch.zeros_like(self._eligible) + self._events: list[ExecutionEvent] = [] + self._call_traces: list[SkillCallTrace] = [] + self._effect_traces: list[SkillEffectTrace] = [] + self._failures: list[SkillFailure] = [] + self._call_event_offset = 0 + self._call_effect_offset = 0 + self._observation_revision = 0 + self._wait_duration = 0.0 + self._message: str | None = None @classmethod - def bind( + def from_components( cls, - *, - manifest: SemanticIntegrationManifest, - scene_registry: SceneRegistry, - engine: AtomicActionEngine, + compiler: SemanticSkillCompiler, observation_provider: ObservationProvider, command_sink: CommandSink, + evidence_collector: EffectEvidenceCollectorPort, + *, + task_state: TaskState | None = None, clock: ExecutionClock | None = None, - effect_verifier: SemanticEffectVerifier | None = None, - registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), - relation_grounders: Iterable[RelationTargetGrounder] = (), - handover_pose_providers: Iterable[HandOverPoseProvider] = (), - endpoint_adapters: ( - Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None - ) = None, runner_cfg: ExecutionRunnerCfg | None = None, - ) -> SemanticSkillRuntime: - """Bind an explicit integration to generic execution ports. - - Args: - manifest: Provider-free scene, robot, and semantic-call declaration. - scene_registry: Live registry matching ``manifest.scene``. - engine: Atomic-action engine owning the target robot and planners. - observation_provider: Source of fresh execution observations. - command_sink: Destination for runtime commands and safe stops. - clock: Optional scheduler clock shared by every per-call runner. - effect_verifier: Optional default semantic effect verifier. - registered_lowerers: Lowerers for registered extension calls. - relation_grounders: Providers for late-bound relation targets. - handover_pose_providers: Named handover-pose providers. - endpoint_adapters: Optional adapters for custom resource endpoints. - runner_cfg: Optional runner policy overriding per-skill presets. - - Returns: - A validated runtime ready to analyze or execute semantic calls. - - Raises: - TypeError: If an integration object or execution port is invalid. - SemanticValidationError: If the live registry, engine, and manifest - do not describe the same scene and robot capabilities. - """ - if type(manifest) is not SemanticIntegrationManifest: - raise TypeError("manifest must be exactly SemanticIntegrationManifest.") - if not isinstance(scene_registry, SceneRegistry): - raise TypeError("scene_registry must be a SceneRegistry.") - if not isinstance(engine, AtomicActionEngine): - raise TypeError("engine must be an AtomicActionEngine.") - integration = manifest.bind( - scene_registry, - engine, - endpoint_adapters=endpoint_adapters, - ) - compiler = SemanticSkillCompiler( - integration, - registered_lowerers=registered_lowerers, - relation_grounders=relation_grounders, - handover_pose_providers=handover_pose_providers, - ) + ) -> SkillRuntime: + """Construct the canonical runtime from explicit reusable ports.""" return cls( compiler, observation_provider, command_sink, + evidence_collector, + task_state=task_state, clock=clock, - effect_verifier=effect_verifier, runner_cfg=runner_cfg, ) - @classmethod - def from_simulation( - cls, - *, - simulation: SimulationManager, - robot: Robot, - motion_generator: MotionGenerator, - scene_registry: SceneRegistry, - robot_profile: RobotSkillProfile, - call_catalog: SemanticCallCatalog | None = None, - effect_verifier: SemanticEffectVerifier | None = None, - registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), - relation_grounders: Iterable[RelationTargetGrounder] = (), - handover_pose_providers: Iterable[HandOverPoseProvider] = (), - endpoint_adapters: ( - Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None - ) = None, - runner_cfg: ExecutionRunnerCfg | None = None, - control_dt: float | None = None, - scene_translation_threshold: float = 1.0e-4, - scene_rotation_threshold: float = 1.0e-3, - ) -> SemanticSkillRuntime: - """Build the standard joint-position semantic runtime for simulation. - - This factory validates the registry against the motion generator, - constructs a :class:`SimulationExecutionAdapter`, installs built-in - atomic actions, and delegates the remaining binding to :meth:`bind`. - - Args: - simulation: Simulation advanced by the runtime execution clock. - robot: Robot observed and controlled through joint-position ports. - motion_generator: Planner used by built-in atomic actions. - scene_registry: Canonical live scene and affordance registry. - robot_profile: Embodiment-specific skill and resource declaration. - call_catalog: Optional semantic-call catalog. Built-ins are used by - default. - effect_verifier: Optional default semantic effect verifier. - registered_lowerers: Lowerers for registered extension calls. - relation_grounders: Providers for late-bound relation targets. - handover_pose_providers: Named handover-pose providers. - endpoint_adapters: Optional adapters for custom resource endpoints. - runner_cfg: Optional runner policy overriding per-skill presets. - control_dt: Optional semantic command period. The simulation physics - period is used when omitted. - scene_translation_threshold: Translation needed to advance the - registry-backed scene version. - scene_rotation_threshold: Rotation needed to advance the - registry-backed scene version. + @property + def compiler(self) -> SemanticSkillCompiler: + """Return the installed semantic compiler.""" + return self._compiler - Returns: - A runtime using one simulation adapter for observation, commands, - and deterministic simulated time. + @property + def clock(self) -> ExecutionClock: + """Return the shared execution clock used by this runtime. - Raises: - TypeError: If a registry, profile, catalog, or port is invalid. - ValueError: If robot state or collision integration is inconsistent. + Parallel coordinators use the same clock for every derived lane so a + branch cannot advance independently of the environment step grid. """ - if not isinstance(scene_registry, SceneRegistry): - raise TypeError("scene_registry must be a SceneRegistry.") - if type(robot_profile) is not RobotSkillProfile: - raise TypeError("robot_profile must be exactly RobotSkillProfile.") - if call_catalog is not None and type(call_catalog) is not SemanticCallCatalog: - raise TypeError("call_catalog must be exactly SemanticCallCatalog or None.") - qpos = robot.get_qpos() - if not isinstance(qpos, torch.Tensor) or qpos.dim() != 2: - raise ValueError("robot.get_qpos() must return shape (B, robot_dof).") - batch_size = int(qpos.shape[0]) - scene_provider = scene_registry.make_planning_scene_provider( - motion_generator, - batch_size=batch_size, - translation_threshold=scene_translation_threshold, - rotation_threshold=scene_rotation_threshold, - ) - adapter = SimulationExecutionAdapter( - simulation, - robot, - control_dt=control_dt, - scene_provider=scene_provider, - ) - engine = AtomicActionEngine( - motion_generator, - control_profiles=robot_profile.action_control_profiles(), - ) - manifest = SemanticIntegrationManifest( - scene=SceneManifest.from_registry(scene_registry), - robot_profile=robot_profile, - call_catalog=( - builtin_semantic_call_catalog() - if call_catalog is None - else call_catalog - ), - ) - return cls.bind( - manifest=manifest, - scene_registry=scene_registry, - engine=engine, - observation_provider=adapter, - command_sink=adapter, - clock=adapter, - effect_verifier=effect_verifier, - registered_lowerers=registered_lowerers, - relation_grounders=relation_grounders, - handover_pose_providers=handover_pose_providers, - endpoint_adapters=endpoint_adapters, - runner_cfg=runner_cfg, - ) - - @property - def engine(self) -> AtomicActionEngine: - """Return the bound atomic-action engine.""" - return self.compiler.integration.engine + return self._clock @property def scene_registry(self) -> SceneRegistry: - """Return the bound canonical scene registry.""" - return self.compiler.integration.scene_registry + """Return the authoritative semantic scene registry.""" + return self._compiler.integration.scene_registry @property - def available_calls(self) -> Mapping[str, SemanticCallDescriptor]: - """Return semantic calls executable by the currently bound profile.""" - supported_skills = set(self.compiler.integration.robot_profile.skills) - return MappingProxyType( - { - call_id: descriptor - for call_id, descriptor in self.compiler.integration.manifest.call_catalog.descriptors.items() - if descriptor.skill_id in supported_skills - } - ) - - @property - def active_task(self) -> SemanticTask | None: - """Return the task currently owning this runtime.""" - return self._active_task + def task_state(self) -> TaskState: + """Return an owned snapshot of persistent verified task state.""" + return _snapshot_task_state(self._task_state) - def validate( + def fork( self, - calls: Iterable[SemanticCallSpec], + command_sink: CommandSink, *, - workflow_id: str = "semantic_workflow", - ) -> SemanticWorkflow: - """Analyze one workflow without observing, planning, or executing it. + task_state: TaskState | None = None, + ) -> SkillRuntime: + """Create an independent execution lane from the same runtime ports. + + The derived runtime shares the immutable compiler integration, + observation/evidence providers, clock, and runner policy, but owns its + workflow, runner, masks, and verified task state. Its command sink is + supplied explicitly so a parallel coordinator can buffer commands + until all lanes have reached the same environment tick. Args: - calls: Ordered robot-independent semantic calls. - workflow_id: Stable identifier used for diagnostics and revisions. + command_sink: Lane-local command sink. + task_state: Optional verified barrier state. The current owned + task state is used when omitted. Returns: - Provider-free linked workflow accepted by the current integration. + A new idle semantic runtime for one independent lane. """ - return self.compiler.analyze(calls, workflow_id=workflow_id) - - def open_task( - self, - task_id: str, - *, - initial_task_state: TaskState | None = None, - eligible_mask: torch.Tensor | None = None, - ) -> SemanticTask: - """Open one exclusive task that may execute several workflow segments. - - Args: - task_id: Stable task identifier without outer whitespace. - initial_task_state: Optional previously verified symbolic state. - eligible_mask: Optional initial per-environment execution cohort. + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + initial_state = self.task_state if task_state is None else task_state + if not isinstance(initial_state, TaskState): + raise TypeError("task_state must be a TaskState or None.") + return SkillRuntime( + self._compiler, + self._observation_provider, + command_sink, + self._evidence_collector, + task_state=initial_state, + clock=self._clock, + runner_cfg=self._runner_cfg, + ) - Returns: - A task retaining verified state across dynamic segment boundaries. + @property + def status(self) -> SkillStatus: + """Return the current workflow status.""" + return self._status - Raises: - RuntimeError: If another task already owns this runtime. - ValueError: If the identifier or eligibility mask is invalid. - """ - _validate_identifier(task_id, name="task_id") - if self._active_task is not None: - raise RuntimeError( - f"Semantic task {self._active_task.task_id!r} already owns this runtime." - ) - task = SemanticTask( - self, - task_id, - initial_task_state=initial_task_state, - eligible_mask=eligible_mask, + @property + def result(self) -> SkillResult: + """Return an immutable snapshot of the current workflow.""" + return SkillResult( + status=self._status, + workflow_id=self._workflow_id, + current_call_index=self._current_call_index, + env_ids=self._env_ids, + success_mask=self._success, + failure_mask=self._failed, + cancelled_mask=self._cancelled, + eligible_mask=self._eligible, + task_state=self._task_state, + events=tuple(self._events), + calls=tuple(self._call_traces), + effects=tuple(self._effect_traces), + failures=tuple(self._failures), + wait_duration=self._wait_duration, + message=self._message, ) - self._active_task = task - return task def start( self, - calls: Iterable[SemanticCallSpec], - *, - task_id: str = "semantic_task", - segment_id: str = "main", - initial_task_state: TaskState | None = None, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", eligible_mask: torch.Tensor | None = None, - ) -> SemanticExecution: - """Start a one-segment task and return a non-blocking execution handle. + execution_prefix_length: int | None = None, + ) -> SkillResult: + """Analyze once and prepare the first call without blocking on motion. Args: - calls: Ordered semantic calls analyzed before controller work starts. - task_id: Stable identifier for the one-shot task. - segment_id: Stable identifier for its only workflow segment. - initial_task_state: Optional previously verified symbolic state. - eligible_mask: Optional initial per-environment execution cohort. + *calls: Complete ordered semantic analysis window. Calls after the + execution prefix participate in static look-ahead but are not + grounded or executed by this run. + workflow_id: Stable workflow identifier used in diagnostics. + eligible_mask: Optional row-local execution eligibility. + execution_prefix_length: Number of leading calls to execute. When + omitted, the complete analysis window is executed. Returns: - A handle advanced through :meth:`SemanticExecution.step` or - :meth:`SemanticExecution.run_until_blocked`. + Immutable initial runtime result. """ - task = self.open_task( - task_id, - initial_task_state=initial_task_state, + if self._status is SkillStatus.RUNNING: + raise RuntimeError("A semantic workflow is already running.") + normalized = self._normalize_calls(calls) + if type(workflow_id) is not str or not workflow_id: + raise ValueError("workflow_id must be a non-empty string.") + prefix_length = self._normalize_execution_prefix_length( + execution_prefix_length, + call_count=len(normalized), + ) + workflow = self._compiler.analyze(normalized, workflow_id=workflow_id) + self._reset_workflow( + normalized, + workflow, + workflow_id=workflow_id, eligible_mask=eligible_mask, + execution_prefix_length=prefix_length, ) try: - return task._start_segment( - calls, - segment_id=segment_id, - finish_task_on_completion=True, + self._prepare_call(0) + except Exception as exc: # noqa: BLE001 - return one uniform result + self._fail_preparation(0, exc) + return self.result + + def step(self) -> SkillResult: + """Advance the current call by at most one due runner cycle.""" + if self._status is not SkillStatus.RUNNING: + return self.result + runner = self._require_runner() + 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) + self._consume_runner_step(runner_step) + if ( + runner_step.status is RunnerStatus.RUNNING + and runner_step.tick is not None + and runner_step.tick.pending_effect is not None + and monitor is None + ): + self._abort( + "The atomic plan requested effect verification, but the grounded " + "semantic call did not install an effect monitor." ) - except Exception: - task.cancel("Semantic task could not be started.") - raise + 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 + else: + self._status = SkillStatus.FAILED + self._current_call_index = None + self._wait_duration = 0.0 + return self.result def run( self, - calls: Iterable[SemanticCallSpec], - *, - task_id: str = "semantic_task", - segment_id: str = "main", - initial_task_state: TaskState | None = None, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", eligible_mask: torch.Tensor | None = None, - effect_verifier: SemanticEffectVerifier | None = None, - on_step: RunnerStepCallback | None = None, - max_steps_per_call: int = 100_000, - ) -> SemanticTaskResult: - """Run one semantic workflow to a terminal task result. + execution_prefix_length: int | None = None, + max_steps: int = 100_000, + ) -> SkillResult: + """Synchronously execute an analyzed semantic-call prefix.""" + if type(max_steps) is not int or max_steps <= 0: + raise ValueError("max_steps must be a positive integer.") + result = self.start( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + ) + for _ in range(max_steps): + if result.terminal: + return result + if result.wait_duration > 0.0: + self._clock.sleep(result.wait_duration) + result = self.step() + self._abort(f"Semantic runtime exceeded max_steps={max_steps}.") + return self.result + + def cancel( + self, reason: str = "Semantic workflow cancelled by caller." + ) -> SkillResult: + """Cancel the active runner and inherit its cancel-then-hold behavior.""" + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + if self._status is not SkillStatus.RUNNING: + return self.result + 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 + 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._current_call_index = None + self._wait_duration = 0.0 + return self.result + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + """Cancel selected rows while the remaining shared call keeps running. + + This is the row-local cancellation boundary used by a parallel + fail-fast coordinator. The active runner remains the sole owner of + controller neutralization and effect-request correlation. Args: - calls: Ordered semantic calls analyzed before execution. - task_id: Stable identifier for the one-shot task. - segment_id: Stable identifier for its only workflow segment. - initial_task_state: Optional previously verified symbolic state. - eligible_mask: Optional initial per-environment execution cohort. - effect_verifier: Per-call effect verifier overriding the runtime - default. - on_step: Optional observer for each low-level runner step. - max_steps_per_call: Hard loop bound applied separately to each call. + env_mask: Rows to remove permanently from this workflow. + reason: Human-readable cancellation reason. Returns: - Terminal result containing verified state, eligibility, and events. - - Raises: - ValueError: If no effect verifier is available or a bound is invalid. + Updated immutable workflow result. """ - verifier = self.effect_verifier if effect_verifier is None else effect_verifier - if verifier is None: + if self._status is not SkillStatus.RUNNING: + return self.result + if not isinstance(env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") + if ( + env_mask.dtype != torch.bool + or env_mask.shape != self._eligible.shape + or env_mask.device != self._eligible.device + ): raise ValueError( - "run() requires an effect_verifier; use start() for manual " - "effect submission." + "env_mask must be bool and match the runtime batch/device." ) - execution = self.start( - calls, - task_id=task_id, - segment_id=segment_id, - initial_task_state=initial_task_state, - eligible_mask=eligible_mask, + 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, + reason=reason, ) - execution.run_until_blocked( - effect_verifier=verifier, - on_step=on_step, - max_steps_per_call=max_steps_per_call, - ) - result = execution.task_result - if result is None: - execution.cancel("Blocking semantic execution did not terminate.") - result = execution.task_result - assert result is not None - return result - - def _release_task(self, task: SemanticTask) -> None: - """Release task ownership after an exact active-task match.""" - if self._active_task is task: - self._active_task = None - - -class SemanticTask: - """Own verified state across one or more semantic workflow segments. - - Tasks are created by :meth:`SemanticSkillRuntime.open_task`. Successful - segments leave the task open for a later application or agent decision. - Failed or cancelled segments are terminal and release runtime ownership. - - Args: - runtime: Runtime exclusively owned until this task terminates. - task_id: Stable task identifier. - initial_task_state: Optional externally verified symbolic state. - eligible_mask: Optional initial per-environment execution cohort. - """ + self._cancelled |= changed + self._eligible &= ~changed + if not self._eligible.any(): + runner_step = self._require_runner().cancel(reason) + self._consume_runner_step(runner_step) + self._finish_current_call(runner_step) + self._status = ( + SkillStatus.CANCELLED + if runner_step.status is RunnerStatus.CANCELLED + else SkillStatus.FAILED + ) + if self._status is SkillStatus.FAILED: + failed = self._call_entered_mask & ~self._cancelled + self._failed |= failed + self._current_call_index = None + self._wait_duration = 0.0 + return self.result + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + """Install a verified state snapshot between independent workflows. + + Parallel coordinators use this explicit barrier operation after + deterministically merging branch-local effects. Running workflows + cannot replace their runner-owned state. + """ + if self._status is SkillStatus.RUNNING: + raise RuntimeError("Cannot replace task state while a workflow is running.") + if not isinstance(task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if ( + task_state.batch_size != self._task_state.batch_size + or task_state.device != self._task_state.device + ): + raise ValueError("task_state must match the runtime batch and device.") + self._task_state = _snapshot_task_state(task_state) + return self.result - def __init__( + @property + def _has_next_call(self) -> bool: + assert self._current_call_index is not None + return self._current_call_index + 1 < self._execution_prefix_length + + @staticmethod + def _normalize_execution_prefix_length( + value: int | None, + *, + call_count: int, + ) -> int: + """Normalize a non-empty execution prefix inside one analysis window.""" + if value is None: + return call_count + if type(value) is not int: + raise TypeError("execution_prefix_length must be an integer or None.") + if not 1 <= value <= call_count: + raise ValueError( + "execution_prefix_length must be in " f"[1, {call_count}], got {value}." + ) + return value + + def _normalize_calls( + self, + supplied: tuple[SemanticCallSpec | Iterable[SemanticCallSpec], ...], + ) -> tuple[SemanticCallSpec, ...]: + """Normalize varargs and one explicit iterable to the same compiler path.""" + if len(supplied) == 1 and not isinstance(supplied[0], SemanticCallSpec): + candidate = supplied[0] + if isinstance(candidate, (str, bytes)): + raise TypeError("calls must contain SemanticCallSpec values.") + try: + calls = tuple(candidate) + except TypeError as exc: + raise TypeError( + "A single run argument must be a SemanticCallSpec or iterable." + ) from exc + else: + calls = tuple(supplied) + if not calls: + raise ValueError("A semantic workflow requires at least one call.") + if not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError("calls must contain SemanticCallSpec values.") + return calls + + def _reset_workflow( self, - runtime: SemanticSkillRuntime, - task_id: str, + calls: tuple[SemanticCallSpec, ...], + workflow: object, *, - initial_task_state: TaskState | None, + workflow_id: str, eligible_mask: torch.Tensor | None, + execution_prefix_length: int, ) -> None: - self.runtime = runtime - self.task_id = _validate_identifier(task_id, name="task_id") - if initial_task_state is not None and not isinstance( - initial_task_state, TaskState - ): - raise TypeError("initial_task_state must be a TaskState or None.") - self._task_state = ( - runtime.engine.initial_context().task - if initial_task_state is None - else initial_task_state - ) - self._latest_context: PlanningContext | None = None - self._latest_context = self._observe() - self._initial_eligible_mask = _normalize_eligible_mask( - eligible_mask, - self._latest_context, + """Reset per-run state while retaining verified symbolic state.""" + if eligible_mask is None: + eligible = torch.ones( + self._task_state.batch_size, + dtype=torch.bool, + device=self._task_state.device, + ) + else: + if not isinstance(eligible_mask, torch.Tensor): + raise TypeError("eligible_mask must be a torch.Tensor or None.") + if eligible_mask.dtype != torch.bool or eligible_mask.shape != ( + self._task_state.batch_size, + ): + raise ValueError( + "eligible_mask must be bool with shape " + f"({self._task_state.batch_size},)." + ) + eligible = eligible_mask.to(self._task_state.device).clone() + if not eligible.any(): + raise ValueError("eligible_mask must contain at least one active row.") + self._workflow = workflow + self._workflow_id = workflow_id + self._calls = calls + self._execution_prefix_length = execution_prefix_length + self._current_call_index = 0 + self._runner = None + self._grounded = None + self._eligible = eligible + self._success = torch.zeros_like(eligible) + self._failed = torch.zeros_like(eligible) + self._cancelled = torch.zeros_like(eligible) + self._events = [] + self._call_traces = [] + self._effect_traces = [] + self._failures = [] + self._call_event_offset = 0 + self._call_effect_offset = 0 + self._observation_revision = 0 + self._wait_duration = 0.0 + self._message = None + self._status = SkillStatus.RUNNING + + def _observe_for_grounding(self) -> PlanningContext: + """Capture and normalize one fresh context for JIT lowering.""" + context = self._observation_provider.observe(self._task_state) + if not isinstance(context, PlanningContext): + raise TypeError( + "ObservationProvider.observe() must return PlanningContext." + ) + normalized = PlanningContext( + robot=context.robot, + task=self._task_state, + scene=context.scene, + env_ids=context.env_ids, ) - self._eligible_mask = self._initial_eligible_mask.clone() - self._segments: list[SemanticSegmentResult] = [] - self._segment_ids: set[str] = set() - self._active_execution: SemanticExecution | None = None - self._failed = False - self._cancelled = False - self._message: str | None = None - self._result: SemanticTaskResult | None = None - - def __enter__(self) -> SemanticTask: - """Return this task for scoped dynamic execution.""" - return self - - def __exit__( - self, - exc_type: type[BaseException] | None, - exc: BaseException | None, - traceback: TracebackType | None, - ) -> None: - """Cancel unfinished work on scope exit and release runtime ownership.""" - del exc_type, traceback - if self._result is not None: - return - if exc is not None or self._active_execution is not None or not self._segments: - self.cancel( - "Semantic task scope exited before normal completion." - if exc is None - else f"Semantic task scope exited with {type(exc).__name__}." + if normalized.batch_size != self._task_state.batch_size: + raise ValueError( + "Observation batch size changed during semantic execution." ) + if normalized.robot.qpos.device != self._task_state.device: + raise ValueError("Observation and verified TaskState must share a device.") + if self._has_observed_env_ids: + if normalized.env_ids.device != self._env_ids.device or not torch.equal( + normalized.env_ids, + self._env_ids, + ): + raise ValueError( + "Observation env_ids must remain stable across call barriers." + ) else: - self.finish() - - @property - def task_state(self) -> TaskState: - """Return the latest externally verified symbolic state.""" - return self._task_state - - @property - def latest_context(self) -> PlanningContext: - """Return the latest observation carrying verified task state.""" - assert self._latest_context is not None - return self._latest_context - - @property - def eligible_mask(self) -> torch.Tensor: - """Return environments still eligible to finish this task.""" - return self._eligible_mask.clone() - - @property - def active_execution(self) -> SemanticExecution | None: - """Return the currently active workflow segment, if any.""" - return self._active_execution - - @property - def segments(self) -> tuple[SemanticSegmentResult, ...]: - """Return terminal segment results in execution order.""" - return tuple(self._segments) - - @property - def result(self) -> SemanticTaskResult | None: - """Return the terminal task result, when finished.""" - return self._result - - def start_segment( - self, - calls: Iterable[SemanticCallSpec], - *, - segment_id: str | None = None, - ) -> SemanticExecution: - """Analyze and start one non-blocking workflow segment. - - Args: - calls: Semantic calls known at the current decision boundary. - segment_id: Optional stable segment identifier. A deterministic - task-local identifier is generated when omitted. - - Returns: - A non-blocking execution handle for this segment. - - Raises: - RuntimeError: If the task is terminal or another segment is active. - ValueError: If the segment identifier is invalid or already used. - """ - return self._start_segment( - calls, - segment_id=segment_id, - finish_task_on_completion=False, + self._env_ids = normalized.env_ids.clone() + self._has_observed_env_ids = True + return normalized + + def _prepare_call(self, call_index: int) -> None: + """Freshly ground and create exactly one session and runner.""" + assert self._workflow is not None + context = self._observe_for_grounding() + grounded = self._compiler.ground( + self._workflow, + call_index, + context, + eligible_mask=self._eligible, ) + 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) + 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, + ): + raise ValueError("Grounded call must preserve runtime eligibility.") + if (effect_spec is None) != (effect_monitor is None): + raise ValueError( + "Grounded effect_spec and effect_monitor must be set together." + ) + if effect_spec is not None: + if not isinstance(effect_spec, SemanticEffectSpec): + raise TypeError("Grounded effect_spec must be a SemanticEffectSpec.") + if not isinstance(effect_monitor, EffectMonitor): + raise TypeError("Grounded effect_monitor must be an EffectMonitor.") + if effect_spec.env_ids.device != context.env_ids.device or not torch.equal( + effect_spec.env_ids, + context.env_ids, + ): + raise ValueError("Grounded effect env_ids must match the call context.") - def _start_segment( - self, - calls: Iterable[SemanticCallSpec], - *, - segment_id: str | None, - finish_task_on_completion: bool, - ) -> SemanticExecution: - if self._result is not None: - raise RuntimeError("A finished semantic task cannot start another segment.") - if self._failed or self._cancelled: - raise RuntimeError("A failed or cancelled task cannot start a segment.") - if self._active_execution is not None: - raise RuntimeError("Only one semantic segment may execute at a time.") - selected_segment_id = ( - f"segment_{len(self._segments)}" if segment_id is None else segment_id - ) - _validate_identifier(selected_segment_id, name="segment_id") - if selected_segment_id in self._segment_ids: - raise ValueError(f"Duplicate segment_id {selected_segment_id!r}.") - workflow = self.runtime.compiler.analyze( - calls, - workflow_id=f"{self.task_id}.{selected_segment_id}", + self._grounded = grounded + session = self._engine.start( + (invocation,), + context, + eligible_mask=self._eligible, ) - execution = SemanticExecution( - self, - workflow, - selected_segment_id, - finish_task_on_completion=finish_task_on_completion, + primed = _PrimedObservationProvider(context, self._observation_provider) + runner = ExecutionRunner( + session, + primed, + self._command_sink, + clock=self._clock, + cfg=self._runner_cfg, ) - self._segment_ids.add(selected_segment_id) - self._active_execution = execution - return execution - - def run_segment( + self._current_call_index = call_index + self._runner = runner + self._call_entered_mask = self._eligible.clone() + self._call_event_offset = len(self._events) + self._call_effect_offset = len(self._effect_traces) + self._wait_duration = 0.0 + + def _effect_verifier( self, - calls: Iterable[SemanticCallSpec], - *, - segment_id: str | None = None, - effect_verifier: SemanticEffectVerifier | None = None, - on_step: RunnerStepCallback | None = None, - max_steps_per_call: int = 100_000, - ) -> SemanticSegmentResult: - """Run one segment while retaining successful task state for more work. - - Args: - calls: Semantic calls known at the current decision boundary. - segment_id: Optional stable segment identifier. - effect_verifier: Verifier overriding the runtime default. - on_step: Optional observer for low-level runner steps. - max_steps_per_call: Hard loop bound applied separately to each call. - - Returns: - Terminal segment result. A successful task remains open; a failed - or cancelled task closes automatically. - - Raises: - ValueError: If no effect verifier is available or a bound is invalid. - RuntimeError: If this task cannot start another segment. - """ - verifier = ( - self.runtime.effect_verifier if effect_verifier is None else effect_verifier - ) - if verifier is None: + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + """Collect raw evidence and feed the grounded call's monitor.""" + grounded = self._require_grounded() + spec = getattr(grounded, "effect_spec", None) + monitor = getattr(grounded, "effect_monitor", None) + if not isinstance(spec, SemanticEffectSpec) or not isinstance( + monitor, + EffectMonitor, + ): + raise RuntimeError( + "The active atomic plan requested effect verification, but its " + "semantic call has no grounded effect monitor." + ) + if request.skill_id != spec.skill_id: + raise ValueError("Effect request skill_id does not match the effect spec.") + if request.invocation_id != spec.invocation_id: raise ValueError( - "run_segment() requires an effect_verifier; use start_segment() " - "for manual effect submission." + "Effect request invocation_id does not match the effect spec." ) - execution = self.start_segment(calls, segment_id=segment_id) - execution.run_until_blocked( - effect_verifier=verifier, - on_step=on_step, - max_steps_per_call=max_steps_per_call, + if request.invocation_revision != spec.invocation_revision: + raise ValueError("Effect request revision does not match the effect spec.") + observation_revision = self._observation_revision + self._observation_revision += 1 + selected_env_ids = spec.env_ids[request.env_mask.to(spec.env_ids.device)] + evidence = self._evidence_collector.collect( + spec, + timestamp=context.robot.timestamp, + observation_revision=observation_revision, + env_ids=selected_env_ids, ) - result = execution.segment_result - if result is None: - execution.cancel("Blocking semantic segment did not terminate.") - result = execution.segment_result - assert result is not None - return result - - def cancel( - self, reason: str = "Semantic task cancelled by caller." - ) -> SemanticTaskResult: - """Cancel active controller work and release runtime ownership. - - Args: - reason: Non-empty cancellation diagnostic. - - Returns: - Idempotent terminal task result after best-effort safe stop. - """ - _validate_identifier(reason, name="reason") - if self._result is not None: - return self._result - if self._active_execution is not None: - self._active_execution.cancel(reason) + decision = monitor.observe(request, evidence) + 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): + raise TypeError("Grounded effect monitor reference must be typed.") + if monitor_ref is None: + monitor_id = f"{type(monitor).__module__}.{type(monitor).__qualname__}" + monitor_revision = None + configured_monitor_params: Mapping[str, object] = {} else: - self._cancelled = True - self._message = reason - return self.finish() - - def finish(self) -> SemanticTaskResult: - """Finalize this task and release its exclusive runtime ownership. - - Returns: - Idempotent terminal result derived from sticky eligibility and - segment outcomes. + monitor_id = monitor_ref.monitor_id + monitor_revision = monitor_ref.revision + configured_monitor_params = monitor_ref.params + resolved_monitor_params = monitor.resolved_params + if not isinstance(resolved_monitor_params, Mapping): + raise TypeError("EffectMonitor.resolved_params must return a mapping.") + trace = SkillEffectTrace( + call_index=self._require_call_index(), + verification_id=request.verification_id, + observation_revision=observation_revision, + timestamp=context.robot.timestamp, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + 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, + ) + self._effect_traces.append(trace) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + ) - Raises: - RuntimeError: If a segment is active or no segment has run. - """ - if self._result is not None: - return self._result - if self._active_execution is not None: - raise RuntimeError("Cannot finish while a semantic segment is running.") - if not self._segments and not self._cancelled and not self._failed: - raise RuntimeError("Cannot finish a semantic task with no segments.") - if self._cancelled: - status = SemanticTaskStatus.CANCELLED - elif self._failed or not self._eligible_mask.any(): - status = SemanticTaskStatus.FAILED + def _consume_runner_step(self, runner_step: RunnerStep) -> None: + """Merge one runner update into workflow-level traces.""" + self._wait_duration = runner_step.wait_duration + if runner_step.tick is not None: + self._task_state = _snapshot_task_state(runner_step.tick.task_state) + self._events.extend( + _snapshot_event(event) for event in runner_step.tick.events + ) + 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.""" + runner = self._require_runner() + grounded = self._require_grounded() + call_index = self._require_call_index() + self._task_state = _snapshot_task_state(runner.session.task_state) + after = runner.session.eligible_mask + invocation = getattr(grounded, "invocation") + if runner_step.status is RunnerStatus.COMPLETED: + completed = self._call_entered_mask & after + failed = self._call_entered_mask & ~after & ~self._cancelled + elif runner_step.status is RunnerStatus.CANCELLED: + completed = torch.zeros_like(self._call_entered_mask) + failed = torch.zeros_like(self._call_entered_mask) else: - initial = self._initial_eligible_mask - retained = self._eligible_mask & initial - status = ( - SemanticTaskStatus.SUCCEEDED - if torch.equal(retained, initial) - else SemanticTaskStatus.PARTIAL_SUCCESS + 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, + ) ) - result = SemanticTaskResult( - task_id=self.task_id, - status=status, - initial_eligible_mask=self._initial_eligible_mask, - eligible_mask=self._eligible_mask, - task_state=self._task_state, - latest_context=self.latest_context, - segments=tuple(self._segments), - message=self._message, + plan_attempts = tuple( + SkillPlanAttemptTrace.from_execution_attempt( + attempt, + profile_id=grounded.analyzed.bound.robot_profile.profile_id, + preset_id=grounded.analyzed.bound.preset.preset_id, + preset_schema_version=grounded.analyzed.bound.preset.schema_version, + ) + for attempt in runner.session.plan_attempts ) - self._result = result - self.runtime._release_task(self) - return result - - def _observe(self) -> PlanningContext: - """Capture and validate a fresh context carrying verified task state.""" - observed = self.runtime.observation_provider.observe(self._task_state) - if type(observed) is not PlanningContext: - raise TypeError( - "ObservationProvider.observe() must return PlanningContext." + 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 :]), ) - context = PlanningContext( - robot=observed.robot, - task=self._task_state, - scene=observed.scene, - env_ids=observed.env_ids, - control_dt=observed.control_dt, ) - self.runtime.engine._validate_context(context) - previous = self._latest_context - if previous is not None: - _validate_context_progress(previous, context) - self._latest_context = context - return context - - def _adopt_runner(self, runner: ExecutionRunner) -> None: - """Carry verified state and sticky eligibility across call boundaries.""" - session = runner.session - self._task_state = session.task_state - self._eligible_mask = session.eligible_mask - self._latest_context = session.latest_context - - def _accept_segment( + self._runner = None + self._grounded = None + + def _fail_preparation(self, call_index: int, exc: Exception) -> None: + """Convert a post-barrier grounding failure to a terminal result.""" + failed = self._eligible.clone() + self._failed |= failed + self._eligible &= ~failed + semantic_id = self._calls[call_index].semantic_id + message = ( + f"Could not prepare semantic call {call_index} ({semantic_id!r}): " + f"{type(exc).__name__}: {exc}" + ) + self._failures.append(SkillFailure(call_index, semantic_id, failed, message)) + self._append_preparation_failure_trace(call_index, failed) + self._message = message + self._status = SkillStatus.FAILED + self._current_call_index = None + self._runner = None + self._grounded = None + self._wait_duration = 0.0 + + def _append_preparation_failure_trace( self, - execution: SemanticExecution, - result: SemanticSegmentResult, + call_index: int, + failed_mask: torch.Tensor, ) -> None: - """Install one exact active segment result.""" - if self._active_execution is not execution: - raise RuntimeError("Semantic segment no longer owns this task.") - self._segments.append(result) - self._active_execution = None - if result.status is SemanticExecutionStatus.FAILED: - self._failed = True - self._message = result.message - elif result.status is SemanticExecutionStatus.CANCELLED: - self._cancelled = True - self._message = result.message - - -class SemanticExecution: - """Drive one analyzed workflow through one JIT-grounded call at a time. - - Instances are created by :meth:`SemanticSkillRuntime.start` or - :meth:`SemanticTask.start_segment`; direct construction is not required. - - Args: - task: Task retaining verified state and sticky eligibility. - workflow: Statically analyzed semantic workflow. - segment_id: Stable identifier of the owning segment. - finish_task_on_completion: Whether a successful segment also finalizes - its task. Failures and cancellations always finalize the task. - """ + """Record statically resolved policy choices when planning never starts.""" + grounded = self._grounded + analyzed = getattr(grounded, "analyzed", None) + invocation = getattr(grounded, "invocation", None) + if analyzed is None: + workflow_calls = getattr(self._workflow, "calls", ()) + if call_index < len(workflow_calls): + analyzed = workflow_calls[call_index] + bound = getattr(analyzed, "bound", None) + if bound is None: + return + try: + profile = bound.robot_profile + preset = bound.preset + action_binding = ( + bound.binding.action_binding + if invocation is None + else invocation.binding + ) + resolved = ResolvedCorePolicyTrace.from_resolved_binding( + profile_id=profile.profile_id, + preset_id=preset.preset_id, + preset_schema_version=preset.schema_version, + motion_policy=( + preset.motion_policy + if invocation is None + else invocation.motion_policy + ), + recovery_policy=( + preset.recovery_policy + if invocation is None + else invocation.recovery_policy + ), + endpoints=action_binding.endpoints, + ) + skill_id = bound.linked.descriptor.skill_id + except (AttributeError, TypeError, ValueError): + return + 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=skill_id, + invocation_id=( + None if invocation is None else invocation.invocation_id + ), + invocation_revision=(0 if invocation is None else invocation.revision), + status=RunnerStatus.FAILED, + entered_mask=failed_mask, + completed_mask=torch.zeros_like(failed_mask), + failed_mask=failed_mask, + command_count=0, + resolved_core_policy=resolved, + plan_attempts=(), + ) + ) - def __init__( - self, - task: SemanticTask, - workflow: SemanticWorkflow, - segment_id: str, - *, - finish_task_on_completion: bool, - ) -> None: - self.task = task - self.workflow = workflow - self.segment_id = segment_id - self._finish_task_on_completion = finish_task_on_completion - self._call_index = 0 - self._runner: ExecutionRunner | None = None - self._grounded: GroundedSemanticCall | None = None - self._current_events: list[ExecutionEvent] = [] - self._current_event_ids: set[int] = set() - self._call_records: list[SemanticCallRecord] = [] - self._status = SemanticExecutionStatus.RUNNING - self._message: str | None = None - self._segment_result: SemanticSegmentResult | None = None - self._task_result: SemanticTaskResult | None = None - self._last_step: SemanticExecutionStep | None = None - self._start_current_call() + def _abort(self, reason: str) -> None: + """Safe-stop the active runner and mark remaining rows failed.""" + if self._runner is not None: + safe_stop_step = self._runner.cancel(reason) + runner_step = replace( + safe_stop_step, + status=RunnerStatus.FAILED, + message=reason, + ) + self._consume_runner_step(runner_step) + self._finish_current_call(runner_step) + failed = self._eligible.clone() + self._failed |= failed + self._eligible &= ~failed + if failed.any() and self._calls: + call_index = min( + self._current_call_index or 0, + len(self._calls) - 1, + ) + self._failures.append( + SkillFailure( + call_index, + self._calls[call_index].semantic_id, + failed, + reason, + ) + ) + self._message = reason + self._status = SkillStatus.FAILED + self._current_call_index = None + self._wait_duration = 0.0 - @property - def status(self) -> SemanticExecutionStatus: - """Return the current segment lifecycle status.""" - return self._status + def _require_runner(self) -> ExecutionRunner: + if self._runner is None: + raise RuntimeError("No semantic call runner is active.") + return self._runner - @property - def call_index(self) -> int: - """Return the currently active or last call index.""" - return self._call_index + def _require_grounded(self) -> object: + if self._grounded is None: + raise RuntimeError("No grounded semantic call is active.") + return self._grounded - @property - def segment_result(self) -> SemanticSegmentResult | None: - """Return the terminal segment result, when available.""" - return self._segment_result + def _require_call_index(self) -> int: + if self._current_call_index is None: + raise RuntimeError("No semantic call is active.") + return self._current_call_index - @property - def task_result(self) -> SemanticTaskResult | None: - """Return the terminal task result for one-shot runtime execution.""" - return self._task_result - @property - def pending_effect(self) -> EffectVerificationRequest | None: - """Return the effect currently awaiting external verification.""" - runner = self._runner - if runner is None or not runner.effect_verification_pending: - return None - step = self._last_step - return None if step is None else step.pending_effect - - def step( - self, - *, - effect_success: torch.Tensor | None = None, - ) -> SemanticExecutionStep: - """Advance the active call without sleeping. +class SkillScene: + """Typed convenience lookup surface backed by one immutable registry.""" - Args: - effect_success: Optional per-environment result for a currently - pending effect request. Premature submissions are rejected. + def __init__(self, registry: SceneRegistry) -> None: + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + self._registry = registry - Returns: - Latest semantic status and its underlying runner step. + @property + def registry(self) -> SceneRegistry: + """Return the authoritative scene registry.""" + return self._registry - Raises: - RuntimeError: If an effect result is submitted before the explicit - verification boundary. - """ - if self._segment_result is not None: - assert self._last_step is not None - return self._last_step - assert self._runner is not None - if effect_success is not None and not self._runner.effect_verification_pending: - raise RuntimeError( - "effect_success may only be submitted for pending effect " - "verification." - ) - effect_result = None - if effect_success is not None: - pending = self.pending_effect - if pending is None: - raise RuntimeError("No effect request is currently pending.") - success = ( - effect_success.to( - device=pending.env_mask.device, - dtype=torch.bool, - ) - & pending.env_mask - ) - effect_result = EffectVerificationResult( - verification_id=pending.verification_id, - success_mask=success, - failure_mask=pending.env_mask & ~success, - ) - runner_step = self._runner.step(effect_result=effect_result) - self._record_runner_step(runner_step) - return self._consume_runner_step(runner_step) + def entity(self, identifier: str | SceneEntityRef) -> SceneEntityRef: + """Resolve any registered semantic entity.""" + return self._registry.resolve(identifier) - def run_until_blocked( - self, - *, - effect_verifier: SemanticEffectVerifier | None = None, - on_step: RunnerStepCallback | None = None, - max_steps_per_call: int = 100_000, - ) -> SemanticExecutionStep: - """Run until the segment terminates or external verification is needed. + def object(self, identifier: str | SceneObjectRef) -> SceneObjectRef: + """Resolve a registered semantic object.""" + return self._registry.resolve(identifier, expected_type=SceneObjectRef) - Args: - effect_verifier: Optional callback used at every physical effect - boundary. Without one, execution returns ``WAITING_FOR_EFFECT``. - on_step: Optional observer for each low-level runner step. - max_steps_per_call: Hard loop bound reset for every semantic call. + def articulation( + self, + identifier: str | SceneArticulationRef, + ) -> SceneArticulationRef: + """Resolve a registered articulation.""" + return self._registry.resolve(identifier, expected_type=SceneArticulationRef) - Returns: - Terminal segment step or an external-verification boundary. + def link(self, identifier: str | SceneLinkRef) -> SceneLinkRef: + """Resolve a registered articulation link.""" + return self._registry.resolve(identifier, expected_type=SceneLinkRef) - Raises: - ValueError: If ``max_steps_per_call`` is not positive. - """ - if max_steps_per_call <= 0: - raise ValueError("max_steps_per_call must be greater than zero.") - if self._segment_result is not None: - assert self._last_step is not None - return self._last_step - while True: - assert self._runner is not None - runner = self._runner - callback_step: RunnerStep | None = None - - def record_step(step: RunnerStep) -> None: - nonlocal callback_step - callback_step = step - self._record_runner_step(step) - if on_step is not None: - on_step(step) - - runner_step = runner.run_until_blocked( - effect_verifier=( - None - if effect_verifier is None - else self._adapt_effect_verifier(effect_verifier) - ), - on_step=record_step, - max_steps=max_steps_per_call, - ) - if callback_step is not runner_step: - self._record_runner_step(runner_step) - result = self._consume_runner_step(runner_step) - if result.status is not SemanticExecutionStatus.RUNNING: - return result + def affordance( + self, + identifier: str | SceneAffordanceRef, + ) -> SceneAffordanceRef: + """Resolve a registered semantic affordance.""" + return self._registry.resolve(identifier, expected_type=SceneAffordanceRef) - def revise_current(self, replacement: SemanticCallSpec) -> None: - """Reanalyze and stage a compatible revision of the active call. - The low-level runner still enforces the same semantic skill, logical - invocation ID, and runtime endpoint addresses. This method is for a - newer target or policy revision, not task-level skill replacement. +class AtomicSkills: + """Small application-facing facade over :class:`SkillRuntime`.""" - Args: - replacement: Replacement semantic call for the active workflow slot. + def __init__(self, runtime: SkillRuntime) -> None: + if not isinstance(runtime, SkillRuntime): + raise TypeError("runtime must be a SkillRuntime.") + self._runtime = runtime + self._scene = SkillScene(runtime.scene_registry) - Raises: - TypeError: If ``replacement`` is not a semantic call. - RuntimeError: If the segment is not running or awaits verification. - ValueError: If the replacement violates compiler or runner revision - invariants. - """ - if not isinstance(replacement, SemanticCallSpec): - raise TypeError("replacement must be a SemanticCallSpec.") - if self._status is not SemanticExecutionStatus.RUNNING: - raise RuntimeError("Only a running semantic call can be revised.") - assert self._runner is not None and self._grounded is not None - calls = [item.call for item in self.workflow.calls] - calls[self._call_index] = replacement - revised_workflow = self.task.runtime.compiler.analyze( - calls, - workflow_id=self.workflow.workflow_id, - ) - context = self.task._observe() - grounded = self.task.runtime.compiler.ground( - revised_workflow, - self._call_index, - context, - eligible_mask=self.task.eligible_mask, - revision=self._grounded.invocation.revision + 1, + @classmethod + def from_components( + cls, + compiler: SemanticSkillCompiler, + observation_provider: ObservationProvider, + command_sink: CommandSink, + evidence_collector: EffectEvidenceCollectorPort, + *, + task_state: TaskState | None = None, + clock: ExecutionClock | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> AtomicSkills: + """Build a facade from explicit compiler and runtime ports.""" + return cls( + SkillRuntime.from_components( + compiler, + observation_provider, + command_sink, + evidence_collector, + task_state=task_state, + clock=clock, + runner_cfg=runner_cfg, + ) ) - self._runner.revise_current(grounded.invocation) - self.workflow = revised_workflow - self._grounded = grounded - def cancel( - self, - reason: str = "Semantic execution cancelled by caller.", - ) -> SemanticExecutionStep: - """Cancel the active low-level runner and finalize this segment. - - Args: - reason: Non-empty cancellation diagnostic. + @classmethod + def from_env(cls, env: object, *, preset: str = "safe") -> AtomicSkills: + """Build through an explicitly installed environment integration adapter. - Returns: - Terminal semantic step after best-effort cancel and safe hold. + The method deliberately does not inspect generic environment attributes + for robots, scenes, controllers, or managers. An environment integration + must implement :class:`SkillRuntimeProvider` and own those decisions. """ - _validate_identifier(reason, name="reason") - if self._segment_result is not None: - assert self._last_step is not None - return self._last_step - assert self._runner is not None - runner_step = self._runner.cancel(reason) - self._record_runner_step(runner_step) - return self._consume_runner_step(runner_step) - - def _start_current_call(self) -> None: - """Observe, ground, plan, and install one semantic call runner.""" - context = self.task._observe() - grounded = self.task.runtime.compiler.ground( - self.workflow, - self._call_index, - context, - eligible_mask=self.task.eligible_mask, - ) - session = self.task.runtime.engine.start( - (grounded.invocation,), - context, - eligible_mask=grounded.eligible_mask, - ) - self._grounded = grounded - self._runner = ExecutionRunner( - session, - self.task.runtime.observation_provider, - self.task.runtime.command_sink, - clock=self.task.runtime.clock, - cfg=deepcopy( - self._grounded.analyzed.bound.preset.runner_cfg - if self.task.runtime.runner_cfg is None - else self.task.runtime.runner_cfg - ), - ) - self._current_events = [] - self._current_event_ids = set() - - def _adapt_effect_verifier( - self, - verifier: SemanticEffectVerifier, - ) -> Callable[ - [PlanningContext, EffectVerificationRequest], - EffectVerificationResult, - ]: - """Adapt the semantic verifier to the low-level runner callback.""" - - def verify( - context: PlanningContext, - pending: EffectVerificationRequest, - ) -> EffectVerificationResult: - call = self.workflow.calls[self._call_index].call - result = verifier(call, pending, context) - if not isinstance(result, torch.Tensor): - raise TypeError("SemanticEffectVerifier must return a torch.Tensor.") - success = result.to(device=pending.env_mask.device, dtype=torch.bool) - if success.shape != pending.env_mask.shape: - raise ValueError( - "SemanticEffectVerifier must return one boolean per environment." - ) - success &= pending.env_mask - return EffectVerificationResult( - verification_id=pending.verification_id, - success_mask=success, - failure_mask=pending.env_mask & ~success, + if type(preset) is not str or not preset: + raise ValueError("preset must be a non-empty string.") + if not isinstance(env, SkillRuntimeProvider): + raise TypeError( + "Environment has no semantic-skill integration adapter. Install " + "SkillRuntimeProvider.create_skill_runtime(*, preset=...) or use " + "AtomicSkills.from_components(...) with explicit ports." ) - - return verify - - def _record_runner_step(self, step: RunnerStep) -> None: - """Retain each structured event exactly once.""" - if step.tick is None: - return - for event in step.tick.events: - event_id = id(event) - if event_id in self._current_event_ids: - continue - self._current_event_ids.add(event_id) - self._current_events.append(event) - - def _consume_runner_step(self, runner_step: RunnerStep) -> SemanticExecutionStep: - """Advance the semantic call barrier from one low-level result.""" - assert self._runner is not None - previous_pending = self.pending_effect - pending = None if runner_step.tick is None else runner_step.tick.pending_effect - if ( - pending is None - and self._runner.effect_verification_pending - and previous_pending is not None - ): - pending = previous_pending - if runner_step.status is RunnerStatus.RUNNING: - self._status = ( - SemanticExecutionStatus.WAITING_FOR_EFFECT - if pending is not None - else SemanticExecutionStatus.RUNNING + runtime = env.create_skill_runtime(preset=preset) + if not isinstance(runtime, SkillRuntime): + raise TypeError( + "SkillRuntimeProvider.create_skill_runtime() must return " + "SkillRuntime." ) - return self._make_step(runner_step, pending_effect=pending) + return cls(runtime) - self.task._adopt_runner(self._runner) - self._record_call(runner_step) - if runner_step.status is RunnerStatus.COMPLETED: - if self._call_index + 1 < len(self.workflow.calls): - self._call_index += 1 - try: - self._start_current_call() - except Exception as exc: # noqa: BLE001 - normalize call boundary - self._message = ( - f"Could not start semantic call {self._call_index}: " - f"{type(exc).__name__}: {exc}" - ) - self._finish_segment(SemanticExecutionStatus.FAILED) - return self._make_step(None, message=self._message) - self._status = SemanticExecutionStatus.RUNNING - return self._make_step(runner_step) - self._finish_segment(SemanticExecutionStatus.COMPLETED) - return self._make_step(runner_step) - - terminal_status = ( - SemanticExecutionStatus.CANCELLED - if runner_step.status is RunnerStatus.CANCELLED - else SemanticExecutionStatus.FAILED - ) - self._message = runner_step.message - self._finish_segment(terminal_status) - return self._make_step(runner_step, message=self._message) - - def _record_call(self, runner_step: RunnerStep) -> None: - """Snapshot the terminal state of the current grounded call.""" - assert self._runner is not None and self._grounded is not None - invocation = self._grounded.invocation - call = self.workflow.calls[self._call_index].call - self._call_records.append( - SemanticCallRecord( - call_index=self._call_index, - semantic_id=call.semantic_id, - skill_id=invocation.skill_id, - invocation_id=invocation.invocation_id, - invocation_revision=invocation.revision, - status=runner_step.status, - eligible_mask=self.task.eligible_mask, - events=tuple(self._current_events), - command_count=self._runner.command_count, - message=runner_step.message, - ) - ) + @property + def runtime(self) -> SkillRuntime: + """Return the canonical runtime for advanced step-wise use.""" + return self._runtime - def _finish_segment(self, status: SemanticExecutionStatus) -> None: - """Install one terminal segment and optionally finalize its task.""" - self._status = status - result = SemanticSegmentResult( - segment_id=self.segment_id, - workflow_id=self.workflow.workflow_id, - status=status, - eligible_mask=self.task.eligible_mask, - task_state=self.task.task_state, - calls=tuple(self._call_records), - message=self._message, - ) - self._segment_result = result - self.task._accept_segment(self, result) - if ( - self._finish_task_on_completion - or status is not SemanticExecutionStatus.COMPLETED - ): - self._task_result = self.task.finish() + @property + def scene(self) -> SkillScene: + """Return typed semantic scene lookup helpers.""" + return self._scene + + @property + def result(self) -> SkillResult: + """Return the current immutable runtime result.""" + return self._runtime.result - def _make_step( + def start( self, - runner_step: RunnerStep | None, - *, - pending_effect: EffectVerificationRequest | None = None, - message: str | None = None, - ) -> SemanticExecutionStep: - """Build and retain the latest high-level execution step.""" - step = SemanticExecutionStep( - status=self._status, - task_id=self.task.task_id, - segment_id=self.segment_id, - call_index=self._call_index, - eligible_mask=self.task.eligible_mask, - runner_step=runner_step, - pending_effect=pending_effect, - message=message, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + """Start non-blocking semantic execution without exposing sessions.""" + return self._runtime.start( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, ) - self._last_step = step - return step - - -def _validate_identifier(value: str, *, name: str) -> str: - """Return one exact non-empty identifier.""" - 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.") - return value + def step(self) -> SkillResult: + """Advance non-blocking execution by one due runner cycle.""" + return self._runtime.step() -def _normalize_eligible_mask( - eligible_mask: torch.Tensor | None, - context: PlanningContext, -) -> torch.Tensor: - """Return one owned eligibility mask matching an observed context.""" - if eligible_mask is None: - return torch.ones( - context.batch_size, - dtype=torch.bool, - device=context.robot.qpos.device, - ) - if not isinstance(eligible_mask, torch.Tensor): - raise TypeError("eligible_mask must be a torch.Tensor or None.") - if eligible_mask.dtype != torch.bool or eligible_mask.shape != ( - context.batch_size, - ): - raise ValueError("eligible_mask must be bool with one value per environment.") - if eligible_mask.device != context.robot.qpos.device: - raise ValueError("eligible_mask and the planning context must share a device.") - return eligible_mask.clone() - - -def _validate_context_progress( - previous: PlanningContext, - current: PlanningContext, -) -> None: - """Validate monotonic observations across semantic call sessions.""" - if not torch.equal(previous.env_ids, current.env_ids): - raise ValueError("Semantic task env_ids must remain stable and ordered.") - if current.robot.timestamp < previous.robot.timestamp: - raise ValueError("Semantic task robot timestamps must be monotonic.") - if current.scene.timestamp < previous.scene.timestamp: - raise ValueError("Semantic task scene timestamps must be monotonic.") - if current.scene.version < previous.scene.version: - raise ValueError("Semantic task scene versions must be monotonic.") - previous_revisions = previous.scene.collision_world_revisions(previous.batch_size) - current_revisions = current.scene.collision_world_revisions(current.batch_size) - if any( - current_revision < previous_revision - for previous_revision, current_revision in zip( - previous_revisions, - current_revisions, - strict=True, + def run( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + max_steps: int = 100_000, + ) -> SkillResult: + """Synchronously execute calls without exposing core runtime objects.""" + return self._runtime.run( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + max_steps=max_steps, ) - ): - raise ValueError("Semantic task collision-world revisions must be monotonic.") + + def cancel( + self, reason: str = "Semantic workflow cancelled by caller." + ) -> SkillResult: + """Cancel and safe-stop the active semantic workflow.""" + return self._runtime.cancel(reason) __all__ = [ - "SemanticCallRecord", - "SemanticEffectVerifier", - "SemanticExecution", - "SemanticExecutionStatus", - "SemanticExecutionStep", - "SemanticSegmentResult", - "SemanticSkillRuntime", - "SemanticTask", - "SemanticTaskResult", - "SemanticTaskStatus", + "AtomicSkills", + "EffectEvidenceCollectorPort", + "ResolvedCorePolicyTrace", + "SkillCallTrace", + "SkillEndpointBindingTrace", + "SkillEffectTrace", + "SkillFailure", + "SkillPlanAttemptTrace", + "SkillResult", + "SkillRuntime", + "SkillRuntimeProvider", + "SkillScene", + "SkillStatus", + "task_state_to_metadata", ] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py index 325e126c7..3cc15309d 100644 --- a/embodichain/lab/sim/skills/scene.py +++ b/embodichain/lab/sim/skills/scene.py @@ -18,7 +18,7 @@ from __future__ import annotations -from collections.abc import Iterable, Iterator, Mapping +from collections.abc import Hashable, Iterable, Iterator, Mapping from copy import deepcopy from dataclasses import dataclass, field, fields, is_dataclass, replace from enum import Enum @@ -32,11 +32,14 @@ from embodichain.lab.sim.atomic_actions import ( Affordance, AntipodalAffordance, + ArticulationOperationAffordance, EntityState, ObjectSemantics, + ObservedArticulationJointState, SceneProvider, SceneSnapshot, ) +from .effects import EffectEvidenceAddress if TYPE_CHECKING: from embodichain.lab.sim.planners import MotionGenerator @@ -48,12 +51,38 @@ GRASP_AFFORDANCE_CAPABILITY = "affordance.grasp" """Capability for an affordance usable by object pickup or handover.""" +ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY = "affordance.articulation.operation" +"""Capability for a typed handle-driven articulation operation.""" + PLACE_ON_AFFORDANCE_CAPABILITY = "affordance.place.on" """Capability for an affordance that defines an ``on`` placement relation.""" PLACE_IN_AFFORDANCE_CAPABILITY = "affordance.place.in" """Capability for an affordance that defines an ``inside`` placement relation.""" +SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID = "builtin.scene_articulation" +"""Stable route for explicitly injected articulation-joint observations.""" + +SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION = "1" +"""Exact contract revision for articulation-joint evidence addresses.""" + + +@dataclass(frozen=True, slots=True) +class ArticulationJointEvidenceAddress(EffectEvidenceAddress): + """Canonical scene articulation and joint observation address.""" + + articulation_id: str + joint_id: str + + def __post_init__(self) -> None: + _validate_identifier(self.articulation_id, "articulation_id") + _validate_identifier(self.joint_id, "joint_id") + + @property + def address_fingerprint(self) -> Hashable: + """Return the exact provider-independent joint address.""" + return type(self), self.articulation_id, self.joint_id + class UnsupportedSceneAffordanceError(ValueError): """Raised when a parent has no affordance for a required capability.""" @@ -322,6 +351,18 @@ def _validate_topology(self) -> None: f"{GRASP_AFFORDANCE_CAPABILITY!r} requires an " "AntipodalAffordance payload." ) + if ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + in self.affordance_capabilities + and not issubclass( + self.affordance_payload_type, + ArticulationOperationAffordance, + ) + ): + raise TypeError( + f"{ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY!r} requires " + "an ArticulationOperationAffordance payload." + ) return if self.parent is not None or self.native_name is not None: raise ValueError("Generic scene metadata cannot declare a parent.") @@ -637,6 +678,19 @@ def observe( """ +@runtime_checkable +class SceneArticulationJointStateProvider(Protocol): + """Observe canonical joints for one registered scene articulation.""" + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> Mapping[str, ObservedArticulationJointState]: + """Return live joint observations whose rows follow ``env_ids``.""" + + @runtime_checkable class SceneGeometryProvider(Protocol): """Provide one entity's planner-facing collision geometry descriptor.""" @@ -660,6 +714,7 @@ class SceneEntityRegistration: Args: ref: Canonical typed reference. state_provider: Optional dynamic pose/confidence source. + joint_state_provider: Optional live articulation-joint source. aliases: External names normalized at the registry boundary. parent: Canonical parent for a link or affordance. native_name: Backend-local member name under ``parent``. @@ -719,6 +774,9 @@ class SceneEntityRegistration: relative_pose: torch.Tensor | None = None """Optional parent-relative pose when no explicit state provider exists.""" + joint_state_provider: SceneArticulationJointStateProvider | None = None + """Explicit live joint source for an articulation registration.""" + def __post_init__(self) -> None: if type(self.ref) not in { SceneEntityRef, @@ -733,6 +791,14 @@ def __post_init__(self) -> None: SceneEntityStateProvider, ): raise TypeError("state_provider must implement SceneEntityStateProvider.") + if self.joint_state_provider is not None and not isinstance( + self.joint_state_provider, + SceneArticulationJointStateProvider, + ): + raise TypeError( + "joint_state_provider must implement " + "SceneArticulationJointStateProvider." + ) if isinstance(self.aliases, (str, bytes)): raise TypeError("aliases must be an iterable of identifiers, not a string.") @@ -831,9 +897,22 @@ def _validate_reference_contract(self) -> None: "affordance_capabilities require a SceneAffordanceRef " "registration." ) + if ( + isinstance(self.ref, SceneObjectRef) + and self.joint_state_provider is not None + ): + raise ValueError( + "joint_state_provider requires a SceneArticulationRef " + "registration." + ) return if isinstance(self.ref, SceneLinkRef): + if self.joint_state_provider is not None: + raise ValueError( + "joint_state_provider requires a SceneArticulationRef " + "registration." + ) if ( not isinstance(self.parent, SceneArticulationRef) or self.native_name is None @@ -855,6 +934,11 @@ def _validate_reference_contract(self) -> None: return if isinstance(self.ref, SceneAffordanceRef): + if self.joint_state_provider is not None: + raise ValueError( + "joint_state_provider requires a SceneArticulationRef " + "registration." + ) if ( not isinstance( self.parent, @@ -879,6 +963,10 @@ def _validate_reference_contract(self) -> None: if self.parent is not None or self.native_name is not None: raise ValueError("Generic entity registrations cannot declare a parent.") + if self.joint_state_provider is not None: + raise ValueError( + "joint_state_provider requires a SceneArticulationRef registration." + ) if self.state_provider is None: raise ValueError("Generic entity registrations require state_provider.") if self.affordance_capabilities: @@ -1550,7 +1638,7 @@ def from_simulation( SceneEntityRegistration( ref=SceneObjectRef(registry_id), state_provider=_SimulationEntityStateProvider(entity), - aliases=(uid,), + aliases=(() if uid == registry_id else (uid,)), geometry_provider=geometry.get( registry_id, _SimulationEntityGeometryProvider(entity), @@ -1572,7 +1660,10 @@ def from_simulation( SceneEntityRegistration( ref=SceneArticulationRef(registry_id), state_provider=_SimulationEntityStateProvider(entity), - aliases=(uid,), + joint_state_provider=( + _SimulationArticulationJointStateProvider(entity) + ), + aliases=(() if uid == registry_id else (uid,)), geometry_provider=geometry.get(registry_id), collision_role=roles.get( registry_id, @@ -1640,6 +1731,51 @@ def observe( return EntityState(pose) +@dataclass(frozen=True, slots=True) +class _SimulationArticulationJointStateProvider: + """Read named measured qpos from one selected simulation articulation.""" + + entity: Any + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> Mapping[str, ObservedArticulationJointState]: + del timestamp + qpos = self.entity.get_qpos(target=False) + if not isinstance(qpos, torch.Tensor): + raise TypeError("Simulation articulation get_qpos() must return a tensor.") + if qpos.dim() != 2 or qpos.shape[0] == 0 or qpos.shape[1] == 0: + raise ValueError( + "Simulation articulation qpos must have non-empty shape (N, J)." + ) + joint_names = tuple(self.entity.joint_names) + if len(joint_names) != qpos.shape[1]: + raise ValueError( + "Simulation articulation joint_names must match qpos width." + ) + for joint_name in joint_names: + _validate_identifier(joint_name, "simulation articulation joint name") + if len(set(joint_names)) != len(joint_names): + raise ValueError("Simulation articulation joint_names must be unique.") + indices = env_ids.to(device=qpos.device) + if bool((indices < 0).any()) or int(indices.max().item()) >= qpos.shape[0]: + raise ValueError( + "Simulation scene env_ids must address valid articulation rows." + ) + selected = qpos.index_select(0, indices) + return MappingProxyType( + { + joint_name: ObservedArticulationJointState( + selected[:, index : index + 1] + ) + for index, joint_name in enumerate(joint_names) + } + ) + + @dataclass(frozen=True, slots=True) class _SimulationEntityGeometryProvider: """Expose a selected live rigid object as planner geometry input.""" @@ -1696,6 +1832,8 @@ def __init__( self._env_ids: torch.Tensor | None = None self._published_poses: dict[str, torch.Tensor] = {} self._published_confidences: dict[str, float] = {} + self._published_joint_positions: dict[tuple[str, str], torch.Tensor] = {} + self._published_joint_validity: dict[tuple[str, str], torch.Tensor] = {} self._scene_version = 0 self._collision_revisions: list[int] = [] self._effective_collision_world_mode = ( @@ -1771,6 +1909,10 @@ def snapshot( timestamp=float(timestamp), env_ids=env_ids, ) + articulation_joints = self._observe_articulation_joints( + timestamp=float(timestamp), + env_ids=env_ids, + ) poses = {entity_id: state.pose for entity_id, state in states.items()} confidences = { entity_id: state.confidence for entity_id, state in states.items() @@ -1787,8 +1929,11 @@ def snapshot( confidences[entity_id] != self._published_confidences[entity_id] for entity_id in confidences ) - if confidence_changed or any( - changed.any().item() for changed in changed_by_entity.values() + joint_changed = self._joint_observations_changed(articulation_joints) + if ( + confidence_changed + or joint_changed + or any(changed.any().item() for changed in changed_by_entity.values()) ): self._scene_version += 1 collision_changed = torch.zeros(batch_size, dtype=torch.bool) @@ -1814,6 +1959,7 @@ def snapshot( entity_id: pose.clone() for entity_id, pose in poses.items() } self._published_confidences = confidences.copy() + self._store_joint_baseline(articulation_joints) self._last_timestamp = float(timestamp) return SceneSnapshot( @@ -1822,8 +1968,112 @@ def snapshot( entities=states, collision_world_revision=tuple(self._collision_revisions), collision_entity_ids=self.collision_entity_ids, + articulation_joints=articulation_joints, ) + def _observe_articulation_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[tuple[str, str], ObservedArticulationJointState]: + """Observe every explicitly registered articulation-joint provider.""" + batch_size = int(env_ids.numel()) + observed: dict[tuple[str, str], ObservedArticulationJointState] = {} + for registration in self.registry._registrations: + provider = registration.joint_state_provider + if provider is None: + continue + assert isinstance(registration.ref, SceneArticulationRef) + supplied = provider.observe_joints( + timestamp=timestamp, + env_ids=env_ids.clone(), + ) + if not isinstance(supplied, Mapping): + raise TypeError( + f"Joint provider for {registration.ref.entity_id!r} must " + "return a mapping." + ) + for joint_id, state in supplied.items(): + _validate_identifier(joint_id, "joint provider joint_id") + if not isinstance(state, ObservedArticulationJointState): + raise TypeError( + f"Joint provider for {registration.ref.entity_id!r} must " + "return ObservedArticulationJointState values." + ) + key = registration.ref.entity_id, joint_id + observed[key] = self._normalize_joint_observation( + state, + batch_size=batch_size, + address=key, + ) + return observed + + @staticmethod + def _normalize_joint_observation( + state: ObservedArticulationJointState, + *, + batch_size: int, + address: tuple[str, str], + ) -> ObservedArticulationJointState: + """Broadcast one live joint observation to the scene batch.""" + position = state.position + if position.dim() == 1: + position = position.unsqueeze(0).expand(batch_size, -1).clone() + elif position.shape[0] != batch_size: + raise ValueError( + f"Articulation joint {address!r} observation must have {batch_size} " + "rows." + ) + valid = state.valid_mask + if valid is None: + valid = torch.ones( + batch_size, + dtype=torch.bool, + device=position.device, + ) + return ObservedArticulationJointState(position, valid) + + def _joint_observations_changed( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> bool: + """Update live joint baselines and report any material value change.""" + changed = set(states) != set(self._published_joint_positions) + if not changed: + for key, state in states.items(): + previous_position = self._published_joint_positions[key] + previous_validity = self._published_joint_validity[key] + current_position = state.position.to( + device=previous_position.device, + dtype=previous_position.dtype, + ) + assert state.valid_mask is not None + current_validity = state.valid_mask.to(previous_validity.device) + if not torch.equal( + current_position, previous_position + ) or not torch.equal( + current_validity, + previous_validity, + ): + changed = True + break + self._store_joint_baseline(states) + return changed + + def _store_joint_baseline( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> None: + """Own the current live joint values used for scene revisioning.""" + self._published_joint_positions = { + key: state.position.clone() for key, state in states.items() + } + self._published_joint_validity = {} + for key, state in states.items(): + assert state.valid_mask is not None + self._published_joint_validity[key] = state.valid_mask.clone() + def _observe_states( self, *, @@ -1913,11 +2163,16 @@ def _pose_change_mask( __all__ = [ + "ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY", + "ArticulationJointEvidenceAddress", "AmbiguousSceneAffordanceError", "GRASP_AFFORDANCE_CAPABILITY", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", "RegistrySceneProvider", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION", + "SceneArticulationJointStateProvider", "SceneAffordanceRef", "SceneArticulationRef", "SceneCollisionRole", diff --git a/scripts/tutorials/semantic_skill/hand_over.py b/scripts/tutorials/semantic_skill/hand_over.py deleted file mode 100644 index e646b592c..000000000 --- a/scripts/tutorials/semantic_skill/hand_over.py +++ /dev/null @@ -1,573 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""Use semantic skills to pick with one arm and hand over to the other. - -The workflow contains an object-centric ``Pick`` followed by a registered -dual-arm transfer call. The robot profile chooses the left and right resources; -an explicit lowerer supplies the atomic HandOver goal and embodiment-specific -receive behavior at grounding time. :class:`SemanticSkillRuntime` executes each -call from fresh observations and commits transfer state only after physical -verification. -""" - -from __future__ import annotations - -import argparse -from collections.abc import Mapping -import sys -from pathlib import Path -from typing import ClassVar, TYPE_CHECKING - -_REPO_ROOT = Path(__file__).resolve().parents[3] -if str(_REPO_ROOT) not in sys.path: - sys.path.insert(0, str(_REPO_ROOT)) - -import torch - -from embodichain.lab.sim import SimulationManager -from embodichain.lab.sim.atomic_actions import ( - ControlPartCommandProfile, - EffectVerificationRequest, - GraspGoal, - HandOver as AtomicHandOver, - HandOverOptions, - MotionPolicy, - PlanningContext, - RecoveryPolicy, -) -from embodichain.lab.sim.objects import RigidObject, Robot -from embodichain.lab.sim.skills import ( - GRASP_AFFORDANCE_CAPABILITY, - Pick, - RegisteredSemanticCall, - RegisteredSemanticLowerer, - ResourceBinding, - RobotSkillProfile, - SceneObjectRef, - SceneRegistry, - SemanticCallDescriptor, - SemanticEffectVerifier, - SemanticLowering, - SemanticPose, - SemanticSkillRuntime, - SkillPolicyPreset, - builtin_semantic_call_catalog, -) -from embodichain.utils import logger -from scripts.tutorials.atomic_action.hand_over import ( - HANDOVER_RECORD_LOOK_AT, - HAND_CLOSE_QPOS, - TRAJECTORY_SIM_STEPS, - create_dual_robot, - create_handover_object, - create_support_surface, -) -from scripts.tutorials.atomic_action.scenario_utils import settle_object -from scripts.tutorials.atomic_action.tutorial_utils import ( - clone_local_pose_from_first_env, - create_antipodal_semantics, - create_toppra_motion_generator, - create_tutorial_argument_parser, - create_tutorial_simulation, - get_hand_open_close_qpos, - prepare_tutorial_scene, - publish_tutorial_scene, - run_tutorial, - serve_tutorial_scene, - start_auto_play_recording, - stop_auto_play_recording, -) -from scripts.tutorials.semantic_skill.tutorial_utils import ( - compile_semantic_workflow_for_diagnostics, - create_graspable_object_registry, - create_manipulator_resource, - create_runtime_step_observer, - joint_target_error, - object_to_eef_translation_error, -) - -if TYPE_CHECKING: - from embodichain.lab.sim.skills.integration import BoundSemanticCall - -OBJECT_ID = "workpiece" -OBJECT_SIMULATION_UID = "handover_object" -PICK_SAMPLE_COUNT = 80 -HANDOVER_SAMPLE_COUNT = 140 -MIDDLE_OBJECT_POSITION = (0.0, 0.0, 0.70) -FINAL_OBJECT_POSITION = (0.0, -0.20, 0.70) -OBJECT_QUATERNION_WXYZ = (0.70710678, 0.70710678, 0.0, 0.0) -HANDOVER_CALL_ID = "tutorial.hand_over" -HANDOVER_PRE_GRASP_DISTANCE = 0.08 -HANDOVER_LIFT_HEIGHT = 0.08 -HANDOVER_HAND_INTERP_STEPS = 10 -HANDOVER_HOLD_STEPS = 4 -HANDOVER_RETREAT_STEPS = 28 -HANDOVER_RECEIVE_APPROACH_DIRECTION = (0.0, 0.70710678, -0.70710678) -TRACKING_ERROR_THRESHOLD = 1.0 -MINIMUM_PICK_LIFT = 0.05 -MAXIMUM_HELD_RELATION_ERROR = 0.06 -MAXIMUM_FINAL_POSITION_ERROR = 0.10 -MAXIMUM_HAND_ERROR = 0.03 -POST_EXECUTION_UPDATES = 120 - - -class TutorialHandOverLowerer(RegisteredSemanticLowerer): - """Lower the tutorial's registered transfer call to atomic HandOver.""" - - call_id: ClassVar[str] = HANDOVER_CALL_ID - schema_version: ClassVar[int] = 1 - - def __init__(self, registry: SceneRegistry) -> None: - if not isinstance(registry, SceneRegistry): - raise TypeError("registry must be a SceneRegistry.") - self._registry = registry - - def lower( - self, - call: RegisteredSemanticCall, - *, - context: PlanningContext, - bound: BoundSemanticCall, - ) -> SemanticLowering: - """Build tuned HandOver options from the latest planning device.""" - del bound - if not isinstance(call.arguments, Mapping): - raise TypeError("tutorial.hand_over arguments must be a mapping.") - object_ref = call.arguments.get("object") - if type(object_ref) is not SceneObjectRef: - raise TypeError("tutorial.hand_over requires a SceneObjectRef object.") - grasp_ref = self._registry.resolve_affordance( - object_ref, - capability=GRASP_AFFORDANCE_CAPABILITY, - ) - semantics = self._registry.object_semantics( - object_ref, - affordance=grasp_ref, - ) - device = context.robot.qpos.device - return SemanticLowering( - goal=GraspGoal(semantics), - skill_options=HandOverOptions( - receive_pick_object_part="bottom", - middle_object_pose=SemanticPose( - MIDDLE_OBJECT_POSITION, - OBJECT_QUATERNION_WXYZ, - ) - .to_matrix() - .to(device), - final_object_pose=SemanticPose( - FINAL_OBJECT_POSITION, - OBJECT_QUATERNION_WXYZ, - ) - .to_matrix() - .to(device), - pre_grasp_distance=HANDOVER_PRE_GRASP_DISTANCE, - lift_height=HANDOVER_LIFT_HEIGHT, - hand_interp_steps=HANDOVER_HAND_INTERP_STEPS, - hold_steps=HANDOVER_HOLD_STEPS, - retreat_steps=HANDOVER_RETREAT_STEPS, - receive_approach_direction=torch.tensor( - HANDOVER_RECEIVE_APPROACH_DIRECTION, - dtype=torch.float32, - device=device, - ), - ), - ) - - -def parse_arguments() -> argparse.Namespace: - """Parse command-line arguments for the semantic HandOver tutorial.""" - parser = create_tutorial_argument_parser( - "Execute and verify semantic Pick -> HandOver through the skill runtime.", - features=("diagnose_plan", "grasp_sampling", "headless_play"), - default_device="cpu", - default_renderer="hybrid", - ) - return parser.parse_args() - - -def create_robot_profile( - left_open: torch.Tensor, - left_grasp: torch.Tensor, - right_open: torch.Tensor, - right_grasp: torch.Tensor, -) -> RobotSkillProfile: - """Declare two disjoint manipulators and their semantic skill defaults. - - Args: - left_open: Left-hand joint positions for ``open``. - left_grasp: Left-hand joint positions for ``grasp``. - right_open: Right-hand joint positions for ``open``. - right_grasp: Right-hand joint positions for ``grasp``. - - Returns: - A dual-arm profile with deterministic Pick and HandOver assignments. - """ - return RobotSkillProfile( - profile_id="tutorial.dual_arm", - resources={ - "left": create_manipulator_resource( - "left", - motion_control_part="left_arm", - grasp_control_part="left_hand", - ), - "right": create_manipulator_resource( - "right", - motion_control_part="right_arm", - grasp_control_part="right_hand", - ), - }, - command_profiles={ - "left_hand": ControlPartCommandProfile.joint_positions( - open=left_open, - grasp=left_grasp, - ), - "right_hand": ControlPartCommandProfile.joint_positions( - open=right_open, - grasp=right_grasp, - ), - }, - defaults={ - "pick_up": ResourceBinding({"primary": "left"}), - "hand_over": ResourceBinding({"source": "left", "destination": "right"}), - }, - presets={ - "pick": SkillPolicyPreset( - "pick", - motion_policy=MotionPolicy( - strategy="motion_gen", - sample_count=PICK_SAMPLE_COUNT, - ), - recovery_policy=RecoveryPolicy( - tracking_error_threshold=TRACKING_ERROR_THRESHOLD, - ), - ), - "hand_over": SkillPolicyPreset( - "hand_over", - motion_policy=MotionPolicy( - strategy="motion_gen", - sample_count=HANDOVER_SAMPLE_COUNT, - ), - # Retrying after either gripper has changed ownership is not - # safe without reconciling the physical attachment first. - recovery_policy=RecoveryPolicy( - max_action_retries=0, - tracking_error_threshold=TRACKING_ERROR_THRESHOLD, - ), - ), - }, - default_preset="pick", - skill_presets={"pick_up": "pick", "hand_over": "hand_over"}, - ) - - -def create_handover_task() -> tuple[Pick, RegisteredSemanticCall]: - """Declare the robot-independent calls submitted at the application entry.""" - object_ref = SceneObjectRef(OBJECT_ID) - return ( - Pick(object=object_ref), - RegisteredSemanticCall( - call_id=HANDOVER_CALL_ID, - arguments={"object": object_ref}, - ), - ) - - -def create_handover_effect_verifier( - obj: RigidObject, - robot: Robot, - *, - left_open: torch.Tensor, - right_grasp: torch.Tensor, -) -> SemanticEffectVerifier: - """Create physical Pick and HandOver verification for the tutorial scene. - - Args: - obj: Object transferred between manipulators. - robot: Dual-arm robot executing the workflow. - left_open: Source-hand release target. - right_grasp: Destination-hand grasp target. - - Returns: - Runtime callback producing a boolean result per environment. - """ - initial_pose = obj.get_local_pose(to_matrix=True) - if not isinstance(initial_pose, torch.Tensor) or initial_pose.dim() != 3: - raise ValueError("The tutorial object pose must have shape (B, 4, 4).") - initial_height = initial_pose[:, 2, 3].clone() - final_position = torch.tensor(FINAL_OBJECT_POSITION, dtype=torch.float32) - - def verify( - call: object, - request: EffectVerificationRequest, - context: PlanningContext, - ) -> torch.Tensor: - object_position = obj.get_local_pose(to_matrix=True)[:, :3, 3] - if type(call) is Pick and request.skill_id == "pick_up": - lift = object_position[:, 2] - initial_height.to(object_position.device) - held = request.expected_effects.held_object_updates.get("left_arm") - if held is None: - raise RuntimeError("Pick verification requires a left-arm attachment.") - held_error = object_to_eef_translation_error( - obj, - robot, - motion_control_part="left_arm", - expected_object_to_eef=held.object_to_eef, - ) - success = (lift >= MINIMUM_PICK_LIFT) & ( - held_error <= MAXIMUM_HELD_RELATION_ERROR - ) - logger.log_info( - "Semantic Pick verification: " - f"lift={lift.detach().cpu().tolist()} m, " - "object-to-left-EEF translation error=" - f"{held_error.detach().cpu().tolist()} m, " - f"success={success.detach().cpu().tolist()}." - ) - elif ( - type(call) is RegisteredSemanticCall - and call.call_id == HANDOVER_CALL_ID - and request.skill_id == "hand_over" - ): - final_error = torch.linalg.vector_norm( - object_position - - final_position.to( - device=object_position.device, - dtype=object_position.dtype, - ), - dim=1, - ) - held = request.expected_effects.held_object_updates.get("right_arm") - if held is None: - raise RuntimeError( - "HandOver verification requires a right-arm attachment." - ) - receiver_error = object_to_eef_translation_error( - obj, - robot, - motion_control_part="right_arm", - expected_object_to_eef=held.object_to_eef, - ) - source_error = joint_target_error( - robot, - control_part="left_hand", - target=left_open, - ) - receiver_hand_error = joint_target_error( - robot, - control_part="right_hand", - target=right_grasp, - ) - success = ( - (final_error <= MAXIMUM_FINAL_POSITION_ERROR) - & (receiver_error <= MAXIMUM_HELD_RELATION_ERROR) - & (source_error <= MAXIMUM_HAND_ERROR) - & (receiver_hand_error <= MAXIMUM_HAND_ERROR) - ) - logger.log_info( - "Semantic HandOver verification: " - f"final_error={final_error.detach().cpu().tolist()} m, " - "object-to-right-EEF translation error=" - f"{receiver_error.detach().cpu().tolist()} m, " - f"source_open_error={source_error.detach().cpu().tolist()} rad, " - "receiver_grasp_error=" - f"{receiver_hand_error.detach().cpu().tolist()} rad, " - f"success={success.detach().cpu().tolist()}." - ) - else: - raise TypeError( - f"Unexpected effect request {request.skill_id!r} for " - f"{type(call).__name__}." - ) - return success.to(context.robot.qpos.device) - - return verify - - -def create_handover_application( - simulation: SimulationManager, - robot: Robot, - obj: RigidObject, - *, - left_open: torch.Tensor, - left_grasp: torch.Tensor, - right_open: torch.Tensor, - right_grasp: torch.Tensor, - n_sample: int, - force_reannotate: bool, -) -> SemanticSkillRuntime: - """Assemble the application-facing runtime for the HandOver tutorial. - - The returned runtime owns the registered call extension, robot binding, - scene catalog, and default physical-effect verifier. Task code only needs - to submit semantic calls through :meth:`SemanticSkillRuntime.run`. - - Args: - simulation: Simulation containing the robot and workpiece. - robot: Dual-arm robot executing the semantic calls. - obj: Workpiece registered under :data:`OBJECT_ID`. - left_open: Left-hand target for the semantic ``open`` command. - left_grasp: Left-hand target for the semantic ``grasp`` command. - right_open: Right-hand target for the semantic ``open`` command. - right_grasp: Right-hand target for the semantic ``grasp`` command. - n_sample: Number of grasp candidates generated during annotation. - force_reannotate: Whether to regenerate cached grasp annotations. - - Returns: - A fully bound semantic runtime with a default effect verifier. - """ - object_semantics = create_antipodal_semantics( - obj, - label="handover object", - n_sample=n_sample, - force_reannotate=force_reannotate, - ) - registry, _ = create_graspable_object_registry( - simulation, - object_id=OBJECT_ID, - simulation_uid=OBJECT_SIMULATION_UID, - semantic_type="handover object", - affordance=object_semantics.affordance, - ) - profile = create_robot_profile( - left_open, - left_grasp, - right_open, - right_grasp, - ) - call_catalog = builtin_semantic_call_catalog().with_descriptor( - SemanticCallDescriptor( - call_id=HANDOVER_CALL_ID, - spec_type=RegisteredSemanticCall, - target_descriptor=AtomicHandOver.descriptor(), - ) - ) - return SemanticSkillRuntime.from_simulation( - simulation=simulation, - robot=robot, - motion_generator=create_toppra_motion_generator(robot), - scene_registry=registry, - robot_profile=profile, - call_catalog=call_catalog, - effect_verifier=create_handover_effect_verifier( - obj, - robot, - left_open=left_open, - right_grasp=right_grasp, - ), - registered_lowerers=(TutorialHandOverLowerer(registry),), - control_dt=TRAJECTORY_SIM_STEPS * simulation.sim_config.physics_dt, - ) - - -def main() -> None: - """Execute and physically verify the semantic dual-arm workflow.""" - args = parse_arguments() - sim = create_tutorial_simulation( - args, - arena_space=3.0, - light_pos=(0.0, -0.4, 3.0), - ) - robot = create_dual_robot(sim, args.robot) - create_support_surface(sim) - obj = create_handover_object(sim) - settle_object(sim, obj, step=0) - clone_local_pose_from_first_env(obj) - obj.clear_dynamics() - publish_tutorial_scene(sim, args) - left_open, left_grasp = get_hand_open_close_qpos( - robot, - hand_control_part="left_hand", - close_qpos=HAND_CLOSE_QPOS, - ) - right_open, right_grasp = get_hand_open_close_qpos( - robot, - hand_control_part="right_hand", - close_qpos=HAND_CLOSE_QPOS, - ) - app = create_handover_application( - sim, - robot=robot, - obj=obj, - left_open=left_open, - left_grasp=left_grasp, - right_open=right_open, - right_grasp=right_grasp, - n_sample=args.n_sample, - force_reannotate=args.force_reannotate, - ) - calls = create_handover_task() - - wait_for_user = prepare_tutorial_scene( - sim, - args, - "Inspect the scene, then press Enter to execute Pick -> HandOver...", - ) - for _ in range(20): - sim.update(step=10) - - if args.diagnose_plan: - try: - trajectory, skill_ids = compile_semantic_workflow_for_diagnostics( - app, - calls, - workflow_id="tutorial.semantic_pick_handover", - ) - except RuntimeError as exc: - logger.log_warning(str(exc)) - return - logger.log_info( - f"Diagnostic compile lowered {' -> '.join(skill_ids)} with " - f"{trajectory.waypoint_count} waypoints." - ) - return - - recording_started = start_auto_play_recording( - sim, - args, - video_prefix="semantic_handover_auto_play", - look_at=HANDOVER_RECORD_LOOK_AT, - ) - try: - result = app.run( - calls, - task_id="tutorial.semantic_pick_handover", - on_step=create_runtime_step_observer( - obj, - robot, - grasp_control_part="left_hand", - grasp_target=left_grasp, - ), - ) - result.require_all_succeeded() - for _ in range(POST_EXECUTION_UPDATES): - app.clock.sleep(sim.sim_config.physics_dt) - finally: - stop_auto_play_recording(sim, recording_started) - logger.log_info( - "Closed-loop semantic Pick -> HandOver completed with " - f"{sum(call.command_count for call in result.segments[0].calls)} " - "accepted commands.", - color="green", - ) - if wait_for_user: - input("Press Enter to exit the simulation...") - serve_tutorial_scene(sim, args) - - -if __name__ == "__main__": - run_tutorial(main) diff --git a/scripts/tutorials/semantic_skill/place.py b/scripts/tutorials/semantic_skill/place.py deleted file mode 100644 index 417dbcea6..000000000 --- a/scripts/tutorials/semantic_skill/place.py +++ /dev/null @@ -1,407 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""Use semantic skills to pick a registered cube and place it at an object pose. - -Unlike the direct atomic-action tutorial, this example never names ``arm`` or -``hand`` in the workflow. The scene registry owns object identity, the robot -profile owns embodiment-specific resources, and :class:`SemanticSkillRuntime` -lowers each call from fresh observations, executes it, and commits only -verified effects. -""" - -from __future__ import annotations - -import argparse -import sys -from pathlib import Path - -_REPO_ROOT = Path(__file__).resolve().parents[3] -if str(_REPO_ROOT) not in sys.path: - sys.path.insert(0, str(_REPO_ROOT)) - -import torch - -from embodichain.lab.sim import SimulationManager -from embodichain.lab.sim.atomic_actions import ( - ControlPartCommandProfile, - EffectVerificationRequest, - MotionPolicy, - PlanningContext, - RecoveryPolicy, -) -from embodichain.lab.sim.objects import RigidObject, Robot -from embodichain.lab.sim.skills import ( - Pick, - Place, - ResourceBinding, - RobotSkillProfile, - SceneObjectRef, - SemanticEffectVerifier, - SemanticPose, - SemanticSkillRuntime, - SkillPolicyPreset, -) -from embodichain.utils import logger -from scripts.tutorials.atomic_action.place import create_pick_object -from scripts.tutorials.atomic_action.tutorial_utils import ( - add_tutorial_robot, - broadcast_pose_batch, - create_antipodal_semantics, - create_curobo_motion_generator, - create_tutorial_argument_parser, - create_tutorial_simulation, - draw_axis_marker, - get_hand_open_close_qpos, - initialize_pre_pick_robot_pose, - prepare_tutorial_scene, - run_tutorial, - serve_tutorial_scene, - start_auto_play_recording, - stop_auto_play_recording, -) -from scripts.tutorials.semantic_skill.tutorial_utils import ( - compile_semantic_workflow_for_diagnostics, - create_graspable_object_registry, - create_manipulator_resource, - create_runtime_step_observer, - joint_target_error, - object_to_eef_translation_error, -) - -OBJECT_ID = "workpiece" -OBJECT_SIMULATION_UID = "cube" -TARGET_OBJECT_POSITION = (-0.40, 0.48, 0.025) -TARGET_OBJECT_QUATERNION_WXYZ = (1.0, 0.0, 0.0, 0.0) -PICK_SAMPLE_COUNT = 120 -PLACE_SAMPLE_COUNT = 120 -TRAJECTORY_SIM_STEPS = 4 -TRACKING_ERROR_THRESHOLD = 0.25 -MINIMUM_PICK_LIFT = 0.08 -MAXIMUM_HELD_RELATION_ERROR = 0.05 -MAXIMUM_PLACE_POSITION_ERROR = 0.05 -MAXIMUM_OPEN_HAND_ERROR = 0.02 -POST_EXECUTION_UPDATES = 240 - - -def parse_arguments() -> argparse.Namespace: - """Parse command-line arguments for the semantic Place tutorial.""" - parser = create_tutorial_argument_parser( - "Execute and verify semantic Pick -> Place through the skill runtime.", - features=("diagnose_plan", "grasp_sampling", "visualize_axes"), - ) - return parser.parse_args() - - -def create_robot_profile( - hand_open: torch.Tensor, - hand_grasp: torch.Tensor, -) -> RobotSkillProfile: - """Declare how semantic manipulation maps onto the tutorial robot. - - Args: - hand_open: Joint positions for the semantic ``open`` command. - hand_grasp: Joint positions for the semantic ``grasp`` command. - - Returns: - A profile with one manipulation resource and per-skill policies. - """ - manipulator_id = "primary_manipulator" - return RobotSkillProfile( - profile_id="tutorial.single_arm", - resources={ - manipulator_id: create_manipulator_resource( - manipulator_id, - motion_control_part="arm", - grasp_control_part="hand", - ) - }, - command_profiles={ - "hand": ControlPartCommandProfile.joint_positions( - open=hand_open, - grasp=hand_grasp, - ) - }, - defaults={ - "pick_up": ResourceBinding({"primary": manipulator_id}), - "place": ResourceBinding({"primary": manipulator_id}), - }, - presets={ - "pick": SkillPolicyPreset( - "pick", - motion_policy=MotionPolicy( - strategy="motion_gen", - sample_count=PICK_SAMPLE_COUNT, - ), - # The PGI gripper closes in five interpolated commands. Its - # position controller can legitimately trail one command by - # more than the generic 0.05-rad threshold. - recovery_policy=RecoveryPolicy( - tracking_error_threshold=TRACKING_ERROR_THRESHOLD, - ), - ), - "place": SkillPolicyPreset( - "place", - motion_policy=MotionPolicy( - strategy="motion_gen", - sample_count=PLACE_SAMPLE_COUNT, - ), - # A failed release is not safely repeatable without first - # reconciling the physical object state. - recovery_policy=RecoveryPolicy( - max_action_retries=0, - tracking_error_threshold=TRACKING_ERROR_THRESHOLD, - ), - ), - }, - default_preset="pick", - skill_presets={"pick_up": "pick", "place": "place"}, - ) - - -def create_place_task() -> tuple[Pick, Place]: - """Declare the robot-independent calls submitted at the application entry.""" - object_ref = SceneObjectRef(OBJECT_ID) - return ( - Pick(object=object_ref), - Place( - object=object_ref, - at=SemanticPose( - TARGET_OBJECT_POSITION, - TARGET_OBJECT_QUATERNION_WXYZ, - ), - ), - ) - - -def create_place_effect_verifier( - obj: RigidObject, - robot: Robot, - hand_open: torch.Tensor, -) -> SemanticEffectVerifier: - """Create physical Pick and Place verification for the live tutorial scene. - - Args: - obj: Cube manipulated by the workflow. - robot: Robot executing the semantic calls. - hand_open: Joint target representing a released object. - - Returns: - Runtime callback producing a boolean result per environment. - """ - initial_pose = obj.get_local_pose(to_matrix=True) - if not isinstance(initial_pose, torch.Tensor) or initial_pose.dim() != 3: - raise ValueError("The tutorial object pose must have shape (B, 4, 4).") - initial_height = initial_pose[:, 2, 3].clone() - target_position = torch.tensor(TARGET_OBJECT_POSITION, dtype=torch.float32) - - def verify( - call: object, - request: EffectVerificationRequest, - context: PlanningContext, - ) -> torch.Tensor: - object_position = obj.get_local_pose(to_matrix=True)[:, :3, 3] - if type(call) is Pick and request.skill_id == "pick_up": - lift = object_position[:, 2] - initial_height.to(object_position.device) - held = request.expected_effects.held_object_updates.get("arm") - if held is None: - raise RuntimeError("Pick verification requires an arm attachment.") - held_error = object_to_eef_translation_error( - obj, - robot, - motion_control_part="arm", - expected_object_to_eef=held.object_to_eef, - ) - success = (lift >= MINIMUM_PICK_LIFT) & ( - held_error <= MAXIMUM_HELD_RELATION_ERROR - ) - logger.log_info( - "Semantic Pick verification: " - f"lift={lift.detach().cpu().tolist()} m, " - "object-to-EEF translation error=" - f"{held_error.detach().cpu().tolist()} m, " - f"success={success.detach().cpu().tolist()}." - ) - elif type(call) is Place and request.skill_id == "place": - position_error = torch.linalg.vector_norm( - object_position - - target_position.to( - device=object_position.device, - dtype=object_position.dtype, - ), - dim=1, - ) - hand_error = joint_target_error( - robot, - control_part="hand", - target=hand_open, - ) - success = (position_error <= MAXIMUM_PLACE_POSITION_ERROR) & ( - hand_error <= MAXIMUM_OPEN_HAND_ERROR - ) - logger.log_info( - "Semantic Place verification: " - f"position_error={position_error.detach().cpu().tolist()} m, " - f"open_hand_error={hand_error.detach().cpu().tolist()} rad, " - f"success={success.detach().cpu().tolist()}." - ) - else: - raise TypeError( - f"Unexpected effect request {request.skill_id!r} for " - f"{type(call).__name__}." - ) - return success.to(context.robot.qpos.device) - - return verify - - -def create_place_application( - simulation: SimulationManager, - robot: Robot, - obj: RigidObject, - *, - hand_open: torch.Tensor, - hand_grasp: torch.Tensor, - n_sample: int, - force_reannotate: bool, -) -> SemanticSkillRuntime: - """Assemble the application-facing runtime for the Place tutorial. - - The returned runtime owns the scene/profile/compiler binding and the - default physical-effect verifier. Task code only needs to submit semantic - calls through :meth:`SemanticSkillRuntime.run`. - - Args: - simulation: Simulation containing the robot and workpiece. - robot: Robot executing the semantic calls. - obj: Workpiece registered under :data:`OBJECT_ID`. - hand_open: Joint target for the semantic ``open`` command. - hand_grasp: Joint target for the semantic ``grasp`` command. - n_sample: Number of grasp candidates generated during annotation. - force_reannotate: Whether to regenerate cached grasp annotations. - - Returns: - A fully bound semantic runtime with a default effect verifier. - """ - object_semantics = create_antipodal_semantics( - obj, - label="cube", - n_sample=n_sample, - force_reannotate=force_reannotate, - ) - registry, _ = create_graspable_object_registry( - simulation, - object_id=OBJECT_ID, - simulation_uid=OBJECT_SIMULATION_UID, - semantic_type="cube", - affordance=object_semantics.affordance, - ) - return SemanticSkillRuntime.from_simulation( - simulation=simulation, - robot=robot, - motion_generator=create_curobo_motion_generator(robot), - scene_registry=registry, - robot_profile=create_robot_profile(hand_open, hand_grasp), - effect_verifier=create_place_effect_verifier(obj, robot, hand_open), - control_dt=TRAJECTORY_SIM_STEPS * simulation.sim_config.physics_dt, - ) - - -def main() -> None: - """Execute and physically verify the semantic Pick-to-Place workflow.""" - args = parse_arguments() - sim = create_tutorial_simulation(args) - robot = add_tutorial_robot(sim, args.robot) - obj = create_pick_object(sim) - hand_open, hand_grasp = get_hand_open_close_qpos(robot) - initialize_pre_pick_robot_pose(robot, obj, hand_open) - app = create_place_application( - sim, - robot, - obj, - hand_open=hand_open, - hand_grasp=hand_grasp, - n_sample=args.n_sample, - force_reannotate=args.force_reannotate, - ) - calls = create_place_task() - - target_pose = calls[1].at - assert target_pose is not None - if not args.no_vis_eef_axis: - draw_axis_marker( - sim, - "semantic_place_object_target", - broadcast_pose_batch( - target_pose.to_matrix().to(sim.device), - robot.get_qpos().shape[0], - ), - ) - wait_for_user = prepare_tutorial_scene( - sim, - args, - "Inspect the scene, then press Enter to execute Pick -> Place...", - ) - if args.diagnose_plan: - try: - trajectory, skill_ids = compile_semantic_workflow_for_diagnostics( - app, - calls, - workflow_id="tutorial.semantic_pick_place", - ) - except RuntimeError as exc: - logger.log_warning(str(exc)) - return - logger.log_info( - f"Diagnostic compile lowered {' -> '.join(skill_ids)} with " - f"{trajectory.waypoint_count} waypoints." - ) - return - - recording_started = start_auto_play_recording( - sim, - args, - video_prefix="semantic_place_auto_play", - ) - try: - result = app.run( - calls, - task_id="tutorial.semantic_pick_place", - on_step=create_runtime_step_observer( - obj, - robot, - grasp_control_part="hand", - grasp_target=hand_grasp, - ), - ) - result.require_all_succeeded() - for _ in range(POST_EXECUTION_UPDATES): - app.clock.sleep(sim.sim_config.physics_dt) - finally: - stop_auto_play_recording(sim, recording_started) - logger.log_info( - "Closed-loop semantic Pick -> Place completed with " - f"{sum(call.command_count for call in result.segments[0].calls)} " - "accepted commands.", - color="green", - ) - if wait_for_user: - input("Press Enter to exit the simulation...") - serve_tutorial_scene(sim, args) - - -if __name__ == "__main__": - run_tutorial(main) diff --git a/scripts/tutorials/semantic_skill/tutorial_utils.py b/scripts/tutorials/semantic_skill/tutorial_utils.py deleted file mode 100644 index afdf8dc95..000000000 --- a/scripts/tutorials/semantic_skill/tutorial_utils.py +++ /dev/null @@ -1,331 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""Shared construction helpers for semantic-skill tutorials.""" - -from __future__ import annotations - -from collections.abc import Iterable -from dataclasses import replace - -import torch - -from embodichain.lab.sim import SimulationManager -from embodichain.lab.sim.atomic_actions import ( - AntipodalAffordance, - BATCH_INVERSE_KINEMATICS_CAPABILITY, - CARTESIAN_POSE_CAPABILITY, - ExecutionEventKind, - FORWARD_KINEMATICS_CAPABILITY, - GRASP_CAPABILITY, - RunnerStep, - RunnerStepCallback, - TimedTrajectory, -) -from embodichain.lab.sim.objects import RigidObject, Robot -from embodichain.lab.sim.skills import ( - GRASP_AFFORDANCE_CAPABILITY, - ControlPartEndpoint, - RobotResource, - SceneAffordanceRef, - SceneEntityRegistration, - SceneObjectRef, - SceneRegistry, - SemanticSkillRuntime, -) -from embodichain.lab.sim.skills.calls import SemanticCallSpec -from embodichain.utils import logger - -_MOTION_CAPABILITIES = frozenset( - { - BATCH_INVERSE_KINEMATICS_CAPABILITY, - CARTESIAN_POSE_CAPABILITY, - FORWARD_KINEMATICS_CAPABILITY, - } -) - - -def create_graspable_object_registry( - simulation: SimulationManager, - *, - object_id: str, - simulation_uid: str, - semantic_type: str, - affordance: AntipodalAffordance, -) -> tuple[SceneRegistry, SceneObjectRef]: - """Register one live object and its default antipodal grasp affordance. - - Args: - simulation: Simulation containing the selected rigid object. - object_id: Canonical semantic object identifier. - simulation_uid: Backend-local rigid-object identifier. - semantic_type: Human-readable object category. - affordance: Target-local grasp metadata copied into the registry. - - Returns: - The immutable registry and its canonical object reference. - """ - object_ref = SceneObjectRef(object_id) - grasp_ref = SceneAffordanceRef(f"{object_id}.grasp.antipodal") - simulation_registry = SceneRegistry.from_simulation( - simulation, - rigid_objects={object_id: simulation_uid}, - ) - object_registration = replace( - simulation_registry.lookup(object_ref, expected_type=SceneObjectRef), - semantic_type=semantic_type, - default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp_ref}, - ) - registry = SceneRegistry( - ( - object_registration, - SceneEntityRegistration( - ref=grasp_ref, - parent=object_ref, - native_name="antipodal_grasp", - affordance=affordance, - affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), - affordance_revision="antipodal-v1", - relative_pose=torch.eye(4, dtype=torch.float32), - ), - ) - ) - return registry, object_ref - - -def create_manipulator_resource( - resource_id: str, - *, - motion_control_part: str, - grasp_control_part: str, -) -> RobotResource: - """Declare one arm-and-gripper resource for semantic skill binding. - - Args: - resource_id: Stable embodiment-level resource identifier. - motion_control_part: Robot control part used for Cartesian motion. - grasp_control_part: Robot control part used for open/grasp commands. - - Returns: - A resource satisfying the built-in manipulation-skill contracts. - """ - return RobotResource( - resource_id=resource_id, - endpoints={ - "motion": ControlPartEndpoint( - control_part=motion_control_part, - capabilities=_MOTION_CAPABILITIES, - ), - "grasp": ControlPartEndpoint( - control_part=grasp_control_part, - capabilities=frozenset({GRASP_CAPABILITY}), - ), - }, - ) - - -def compile_semantic_workflow_for_diagnostics( - runtime: SemanticSkillRuntime, - calls: Iterable[SemanticCallSpec], - *, - workflow_id: str, -) -> tuple[TimedTrajectory, tuple[str, ...]]: - """Compile a semantic workflow without physically executing it. - - This helper exists only for the tutorials' ``--diagnose-plan`` path. - Expected effects are projected hypothetically between calls; normal runs - must use :class:`SemanticSkillRuntime` and verify physical effects. - - Args: - runtime: Fully bound semantic runtime. - calls: Ordered semantic calls to analyze and compile. - workflow_id: Stable workflow identifier used in diagnostics. - - Returns: - Concatenated diagnostic trajectory and lowered atomic skill IDs. - - Raises: - RuntimeError: If any call fails to plan for any environment. - """ - workflow = runtime.validate(tuple(calls), workflow_id=workflow_id) - empty_task_state = runtime.engine.initial_context().task - context = runtime.observation_provider.observe(empty_task_state) - trajectories: list[TimedTrajectory] = [] - skill_ids: list[str] = [] - for call_index in range(len(workflow.calls)): - grounded = runtime.compiler.ground(workflow, call_index, context) - compiled = runtime.engine.compile((grounded.invocation,), context) - if not compiled.plan_success.all(): - failed_rows = ( - (~compiled.plan_success) - .nonzero(as_tuple=False) - .flatten() - .detach() - .cpu() - .tolist() - ) - raise RuntimeError( - f"Semantic call {call_index} ({grounded.invocation.skill_id!r}) " - f"failed to plan for environment rows {failed_rows}." - ) - trajectories.append(compiled.trajectory) - skill_ids.append(grounded.invocation.skill_id) - context = compiled.projected_context - return TimedTrajectory.concatenate(trajectories), tuple(skill_ids) - - -def object_to_eef_translation_error( - obj: RigidObject, - robot: Robot, - *, - motion_control_part: str, - expected_object_to_eef: torch.Tensor, -) -> torch.Tensor: - """Compare the observed and expected object-to-EEF translations. - - Args: - obj: Live object whose relative transform is measured. - robot: Robot providing current joint state and forward kinematics. - motion_control_part: Control part identifying the target end effector. - expected_object_to_eef: Relation declared by the pending symbolic effect. - - Returns: - Translation error in metres for every environment. - """ - object_pose = obj.get_local_pose(to_matrix=True) - eef_pose = robot.compute_fk( - qpos=robot.get_qpos(name=motion_control_part), - name=motion_control_part, - to_matrix=True, - ) - if ( - not isinstance(object_pose, torch.Tensor) - or not isinstance(eef_pose, torch.Tensor) - or object_pose.dim() != 3 - or eef_pose.shape != object_pose.shape - or object_pose.shape[-2:] != (4, 4) - ): - raise ValueError("Object and end-effector poses must share shape (B, 4, 4).") - expected = torch.as_tensor( - expected_object_to_eef, - dtype=object_pose.dtype, - device=object_pose.device, - ) - if expected.shape == (4, 4): - expected = expected.unsqueeze(0).expand(object_pose.shape[0], -1, -1) - if expected.shape != object_pose.shape: - raise ValueError( - "Expected object-to-EEF pose must have shape (4, 4) or (B, 4, 4)." - ) - observed = torch.bmm(torch.linalg.inv(object_pose), eef_pose) - return torch.linalg.vector_norm( - observed[:, :3, 3] - expected[:, :3, 3], - dim=1, - ) - - -def joint_target_error( - robot: Robot, - *, - control_part: str, - target: torch.Tensor, -) -> torch.Tensor: - """Measure maximum absolute joint error per environment. - - Args: - robot: Robot providing current control-part positions. - control_part: Control part whose joints are compared. - target: One-dimensional target or a full ``(B, D)`` batch. - - Returns: - Maximum absolute joint error for every environment. - """ - current = robot.get_qpos(name=control_part) - if not isinstance(current, torch.Tensor) or current.dim() != 2: - raise ValueError("Control-part qpos must have shape (B, D).") - expected = torch.as_tensor(target, dtype=current.dtype, device=current.device) - if expected.dim() == 1: - expected = expected.unsqueeze(0).expand(current.shape[0], -1) - if expected.shape != current.shape: - raise ValueError("Joint target must have shape (D,) or match qpos (B, D).") - return torch.amax(torch.abs(current - expected), dim=1) - - -def create_runtime_step_observer( - obj: RigidObject, - robot: Robot, - *, - grasp_control_part: str, - grasp_target: torch.Tensor, - grasp_tolerance: float = 1.0e-2, -) -> RunnerStepCallback: - """Create a runner observer that logs recovery and stabilizes one grasp. - - Args: - obj: Physical object stabilized once the grasp target is reached. - robot: Robot whose gripper state is observed. - grasp_control_part: Control part executing the initial grasp. - grasp_target: Joint target representing a closed grasp. - grasp_tolerance: Maximum joint error before dynamics are cleared once. - - Returns: - Callback accepted by ``SemanticSkillRuntime.run(on_step=...)``. - """ - if grasp_tolerance <= 0.0: - raise ValueError("grasp_tolerance must be greater than zero.") - target = grasp_target.clone() - dynamics_cleared = False - reported_events = { - ExecutionEventKind.REPLANNED, - ExecutionEventKind.TRACKING_ERROR, - ExecutionEventKind.DYNAMIC_GOAL_CHANGED, - ExecutionEventKind.COLLISION_WORLD_CHANGED, - ExecutionEventKind.ACTION_RETRY, - ExecutionEventKind.RECOVERY_EXHAUSTED, - } - - def observe(step: RunnerStep) -> None: - nonlocal dynamics_cleared - if step.tick is not None: - for event in step.tick.events: - if event.kind in reported_events: - env_rows = event.env_mask.nonzero(as_tuple=False).flatten().tolist() - logger.log_info( - f"Runtime event {event.kind.value}: rows={env_rows}; " - f"{event.message}" - ) - if dynamics_cleared: - return - error = joint_target_error( - robot, - control_part=grasp_control_part, - target=target, - ) - if torch.all(error <= grasp_tolerance): - obj.clear_dynamics() - dynamics_cleared = True - - return observe - - -__all__ = [ - "compile_semantic_workflow_for_diagnostics", - "create_graspable_object_registry", - "create_manipulator_resource", - "create_runtime_step_observer", - "joint_target_error", - "object_to_eef_translation_error", -] diff --git a/tests/sim/skills/test_articulation_semantics.py b/tests/sim/skills/test_articulation_semantics.py new file mode 100644 index 000000000..4cdcd1227 --- /dev/null +++ b/tests/sim/skills/test_articulation_semantics.py @@ -0,0 +1,594 @@ +# ---------------------------------------------------------------------------- +# 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 first-class semantic articulation operations.""" + +from __future__ import annotations + +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, + AtomicActionEngine, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + EntityState, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + ObservedArticulationJointState, + OperateArticulationGoal, + PlanningContext, + RobotObservation, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.skills.calls import ( + OperateArticulation, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + JointStateEffectClause, + SemanticEffectKind, + SymbolicStateKey, +) +from embodichain.lab.sim.skills.integration import ( + SceneManifest, + SemanticIntegrationManifest, + SemanticValidationError, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRegistration, + SceneRegistry, +) + +_BATCH_SIZE = 2 +_TARGET_POSITION = 0.42 +_TARGET_DISPLACEMENT = 0.4 +_POSITION_SCALE = 0.5 + + +class _MutablePoseProvider: + """Expose a mutable pose and count semantic observation calls.""" + + def __init__( + self, + pose: torch.Tensor, + *, + joint_position: torch.Tensor | None = None, + ) -> None: + self.pose = pose.clone() + self.joint_position = None if joint_position is None else joint_position.clone() + self.calls = 0 + self.joint_calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + return EntityState(self.pose) + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, ObservedArticulationJointState]: + del timestamp, env_ids + self.joint_calls += 1 + if self.joint_position is None: + raise RuntimeError("This provider has no articulation joint fixture.") + return {"drawer_slide": ObservedArticulationJointState(self.joint_position)} + + +def _translated_offset(x: float, y: float, z: float) -> torch.Tensor: + """Build one test-only proper local offset.""" + pose = torch.eye(4, dtype=torch.float32) + pose[:3, 3] = torch.tensor((x, y, z), dtype=torch.float32) + return pose + + +def _operation_affordance() -> ArticulationOperationAffordance: + """Build the canonical drawer-handle fixture.""" + return ArticulationOperationAffordance( + joint_id="drawer_slide", + approach_offset=_translated_offset(0.0, 0.0, -0.1), + contact_offset=torch.eye(4), + operation_offset=_translated_offset(0.0, 0.02, 0.0), + retract_offset=_translated_offset(0.0, 0.0, -0.1), + operation_axis=torch.tensor((1.0, 0.0, 0.0)), + position_scale=_POSITION_SCALE, + semantic_targets={ + "open": ArticulationOperationTarget( + target_position=_TARGET_POSITION, + displacement=_TARGET_DISPLACEMENT, + ) + }, + ) + + +def _registry() -> tuple[SceneRegistry, _MutablePoseProvider, _MutablePoseProvider]: + """Build an articulation plus one directly registered handle affordance.""" + articulation = SceneArticulationRef("drawer") + handle = SceneAffordanceRef("drawer_handle") + articulation_provider = _MutablePoseProvider( + torch.eye(4).repeat(_BATCH_SIZE, 1, 1), + joint_position=torch.zeros(_BATCH_SIZE, 1), + ) + handle_pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + handle_pose[:, 0, 3] = 0.3 + handle_provider = _MutablePoseProvider(handle_pose) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=articulation, + state_provider=articulation_provider, + joint_state_provider=articulation_provider, + semantic_type="drawer", + default_affordances={ + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: handle + }, + ), + SceneEntityRegistration( + ref=handle, + state_provider=handle_provider, + parent=articulation, + native_name="handle", + affordance=_operation_affordance(), + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision="drawer-handle-v1", + ), + ) + ) + return registry, articulation_provider, handle_provider + + +def _profile() -> RobotSkillProfile: + """Build one resource satisfying motion and interaction endpoints.""" + return RobotSkillProfile( + profile_id="articulation_test_robot", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + "interaction": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor((0.0,)), + grasp=torch.tensor((1.0,)), + ) + }, + presets={"safe": SkillPolicyPreset("safe")}, + default_preset="safe", + ) + + +def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: + """Construct a CPU-only engine with the minimum typed robot surface.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 2 + robot.control_parts = {"arm": object(), "hand": object()} + robot.get_qpos.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + robot.get_qvel.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + robot.get_joint_ids.side_effect = lambda name: {"arm": [0], "hand": [1]}[name] + robot.get_solver.return_value = object() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + return AtomicActionEngine(generator, skill_profile=profile) + + +def _compiler(registry: SceneRegistry) -> SemanticSkillCompiler: + """Bind the curated semantic catalog to the test scene and profile.""" + profile = _profile() + engine = _engine(profile) + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=builtin_semantic_call_catalog(), + ) + return SemanticSkillCompiler(manifest.bind(registry, engine)) + + +def _context(scene: SceneSnapshot, *, timestamp: float) -> PlanningContext: + """Build one immutable planning observation around a supplied scene.""" + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=torch.zeros(_BATCH_SIZE, 2), + qvel=torch.zeros(_BATCH_SIZE, 2), + ), + task=TaskState.empty(_BATCH_SIZE, "cpu"), + scene=scene, + env_ids=env_ids, + ) + + +def test_articulation_affordance_owns_configuration_and_grounds_geometry() -> None: + axis = torch.tensor((2.0, 0.0, 0.0)) + operation_offset = _translated_offset(0.0, 0.02, 0.0) + targets = { + "open": ArticulationOperationTarget( + _TARGET_POSITION, + _TARGET_DISPLACEMENT, + ) + } + affordance = ArticulationOperationAffordance( + joint_id="drawer_slide", + operation_axis=axis, + operation_offset=operation_offset, + position_scale=_POSITION_SCALE, + semantic_targets=targets, + ) + axis.zero_() + operation_offset.zero_() + targets.clear() + + handle = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + handle[:, 0, 3] = 0.3 + _, _, operation, _ = affordance.ground_poses( + handle, + displacement=_TARGET_DISPLACEMENT, + ) + + assert tuple(affordance.semantic_targets) == ("open",) + assert torch.allclose(affordance.operation_axis, torch.tensor((1.0, 0.0, 0.0))) + assert torch.allclose( + operation[:, :3, 3], + torch.tensor((0.5, 0.02, 0.0)).repeat(_BATCH_SIZE, 1), + ) + + +def test_registry_returns_owned_articulation_affordance_snapshots() -> None: + registry, _, _ = _registry() + + first = registry.lookup( + SceneAffordanceRef("drawer_handle"), + expected_type=SceneAffordanceRef, + ).affordance + second = registry.lookup( + SceneAffordanceRef("drawer_handle"), + expected_type=SceneAffordanceRef, + ).affordance + + assert type(first) is ArticulationOperationAffordance + assert type(second) is ArticulationOperationAffordance + assert first is not second + first.operation_offset[0, 3] = 99.0 + assert second.operation_offset[0, 3].item() == 0.0 + + +@pytest.mark.parametrize( + "kwargs", + ( + {}, + {"target_position": _TARGET_POSITION}, + {"target_displacement": _TARGET_DISPLACEMENT}, + { + "target": "open", + "target_position": _TARGET_POSITION, + "target_displacement": _TARGET_DISPLACEMENT, + }, + ), +) +def test_articulation_call_requires_exactly_one_complete_target(kwargs: dict) -> None: + with pytest.raises(ValueError): + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + **kwargs, + ) + + +def test_static_link_selects_default_without_observing_scene() -> None: + registry, articulation_provider, handle_provider = _registry() + compiler = _compiler(registry) + + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + + analyzed = workflow.calls[0] + assert analyzed.effect_kind is SemanticEffectKind.ARTICULATION + assert analyzed.bound.linked.affordances["handle"] == SceneAffordanceRef( + "drawer_handle" + ) + assert analyzed.bound.linked.descriptor.skill_id == "operate_articulation" + assert analyzed.symbolic_writes == frozenset( + {SymbolicStateKey.articulation_joint("drawer", "drawer_slide")} + ) + assert not analyzed.opaque_symbolic_effect + assert articulation_provider.calls == 0 + assert handle_provider.calls == 0 + + +def test_static_link_rejects_unknown_explicit_handle_with_path() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + handle=SceneAffordanceRef("missing_handle"), + target="open", + ), + ) + ) + + assert error.value.diagnostic.path == ("workflow", 0, "call", "handle") + + +def test_grounding_uses_fresh_handle_pose_and_lowers_typed_effect() -> None: + registry, articulation_provider, handle_provider = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + scene_provider = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ) + first_context = _context( + scene_provider.snapshot(timestamp=0.0, env_ids=env_ids), + timestamp=0.0, + ) + first = compiler.ground(workflow, 0, first_context) + handle_provider.pose[:, 0, 3] = 0.7 + assert articulation_provider.joint_position is not None + articulation_provider.joint_position[:, 0] = 0.1 + second_context = _context( + scene_provider.snapshot(timestamp=1.0, env_ids=env_ids), + timestamp=1.0, + ) + second = compiler.ground(workflow, 0, second_context, revision=1) + + first_goal = first.invocation.goal + second_goal = second.invocation.goal + assert type(first_goal) is OperateArticulationGoal + assert type(second_goal) is OperateArticulationGoal + first_poses = first_goal.geometry.resolve( + first_context, + displacement=torch.full((_BATCH_SIZE,), _TARGET_DISPLACEMENT), + ) + second_poses = second_goal.geometry.resolve( + second_context, + displacement=torch.full((_BATCH_SIZE,), _TARGET_DISPLACEMENT), + ) + assert torch.allclose(first_poses[0][:, 0, 3], torch.full((2,), 0.3)) + assert torch.allclose(second_poses[0][:, 0, 3], torch.full((2,), 0.7)) + assert torch.allclose(second_poses[2][:, 0, 3], torch.full((2,), 0.9)) + assert torch.equal( + first_goal.source_position, + torch.zeros(_BATCH_SIZE, 1), + ) + assert torch.equal( + second_goal.source_position, + torch.full((_BATCH_SIZE, 1), 0.1), + ) + assert second_goal.target_displacement == _TARGET_DISPLACEMENT + assert torch.allclose( + second_goal.target_position, + torch.full((_BATCH_SIZE, 1), _TARGET_POSITION), + ) + + effect = second.effect_spec + assert effect is not None + assert effect.effect_kind is SemanticEffectKind.ARTICULATION + expectation = effect.state_expectations[0] + clause = effect.clauses[0] + assert type(expectation) is ArticulationJointStateExpectation + assert expectation.articulation_id == "drawer" + assert expectation.joint_id == "drawer_slide" + assert type(clause) is JointStateEffectClause + assert torch.equal(clause.target_position, second_goal.target_position) + assert clause.source.provider_id == SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + assert clause.source.revision == SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + assert type(clause.source.address) is ArticulationJointEvidenceAddress + assert clause.source.address.articulation_id == "drawer" + assert clause.source.address.joint_id == "drawer_slide" + + +def test_explicit_target_pair_records_live_source_joint_state() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target_position=0.25, + target_displacement=-0.1, + ), + ) + ) + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + scene = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ).snapshot(timestamp=0.0, env_ids=env_ids) + + grounded = compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + goal = grounded.invocation.goal + assert type(goal) is OperateArticulationGoal + assert torch.allclose(goal.target_position, torch.full((_BATCH_SIZE, 1), 0.25)) + assert torch.equal(goal.source_position, torch.zeros(_BATCH_SIZE, 1)) + assert goal.target_displacement == -0.1 + operation = goal.geometry.resolve( + _context(scene, timestamp=0.0), + displacement=torch.full((_BATCH_SIZE,), -0.1), + )[2] + assert torch.allclose(operation[:, 0, 3], torch.full((2,), 0.25)) + + +def test_unknown_named_target_has_strict_grounding_diagnostic() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="closed", + ), + ) + ) + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + scene = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ).snapshot(timestamp=0.0, env_ids=env_ids) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_articulation_target" + assert diagnostic.path == ("workflow", 0, "call", "target") + assert diagnostic.candidates == ("open",) + + +def test_missing_handle_pose_has_strict_grounding_diagnostic() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer": EntityState(torch.eye(4).repeat(_BATCH_SIZE, 1, 1))}, + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "missing_handle_observation" + assert diagnostic.path == ("workflow", 0, "call", "handle") + + +def test_missing_live_joint_state_has_strict_grounding_diagnostic() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + handle = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer_handle": EntityState(handle)}, + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "missing_articulation_joint_observation" + assert diagnostic.path == ("workflow", 0, "call", "articulation") + + +def test_articulation_capability_rejects_untyped_affordance_payload() -> None: + articulation = SceneArticulationRef("drawer") + handle = SceneAffordanceRef("drawer_handle") + provider = _MutablePoseProvider(torch.eye(4).repeat(_BATCH_SIZE, 1, 1)) + + with pytest.raises(TypeError, match="ArticulationOperationAffordance"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=articulation, + state_provider=provider, + default_affordances={ + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: handle + }, + ), + SceneEntityRegistration( + ref=handle, + state_provider=provider, + parent=articulation, + native_name="handle", + affordance=Affordance(), + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision="bad-v1", + ), + ) + ) diff --git a/tests/sim/skills/test_calls.py b/tests/sim/skills/test_calls.py index ec9d91272..cde90d8ad 100644 --- a/tests/sim/skills/test_calls.py +++ b/tests/sim/skills/test_calls.py @@ -19,6 +19,7 @@ from __future__ import annotations from collections.abc import Callable, Mapping +import json import math import pytest @@ -111,6 +112,25 @@ def test_semantic_pose_converts_to_homogeneous_matrix() -> None: torch.testing.assert_close(pose.to_matrix(), expected, atol=1.0e-6, rtol=1.0e-6) +def test_semantic_call_metadata_is_deterministic_and_json_safe() -> None: + call = Place( + object=SceneObjectRef("cube"), + at=SemanticPose((1.0, 2.0, 3.0), (1.0, 0.0, 0.0, 0.0)), + resources={"primary": "left_arm"}, + ) + + metadata = call.to_metadata() + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["semantic_id"] == "place" + assert metadata["resources"] == {"primary": "left_arm"} + assert metadata["arguments"]["object"] == { + "entity_type": "SceneObjectRef", + "entity_id": "cube", + } + assert metadata["arguments"]["at"]["position"] == [1.0, 2.0, 3.0] + + @pytest.mark.parametrize( "factory", ( diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index e348cddbb..8d4ff1408 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -32,6 +32,7 @@ BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, ControlPartCommandProfile, + DynamicCollisionMode, EntityState, ExecutionStatus, FORWARD_KINEMATICS_CAPABILITY, @@ -39,6 +40,7 @@ GraspGoal, HandOverOptions, HeldObjectState, + MotionPolicy, ObjectSemantics, PickUp, PickUpOptions, @@ -67,6 +69,24 @@ SemanticRelationTarget, SemanticSkillCompiler, ) +from embodichain.lab.sim.skills.effects import ( + BinaryEffectClause, + BinaryEvidenceKind, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CompositeEffectMonitorFactory, + ControlPartEvidenceAddress, + EffectMonitor, + EffectMonitorRef, + EffectMonitorRegistry, + HeldObjectRelation, + HeldObjectStateExpectation, + PoseRelationClause, + PoseRelationExpectation, + SemanticEffectKind, + SemanticEffectSpec, + SymbolicStateKey, +) from embodichain.lab.sim.skills.integration import ( BoundSemanticCall, SceneManifest, @@ -84,6 +104,8 @@ GRASP_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, + SceneCollisionRole, + SceneCollisionWorldMode, SceneEntityRegistration, SceneObjectRef, SceneRegistry, @@ -117,6 +139,13 @@ def observe( return EntityState(self.pose) +class _GeometryProvider: + """Return one opaque planner-facing geometry descriptor.""" + + def get_geometry(self) -> object: + return object() + + class _FrameRelationGrounder(RelationTargetGrounder): """Explicit test contract: relation frame equals target object frame.""" @@ -230,7 +259,37 @@ def resolve( ) -def _scene_registry() -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider]]: +class _CountingRelationMonitorFactory(CompositeEffectMonitorFactory): + """Count monitor construction without changing built-in behavior.""" + + def __init__(self) -> None: + self.calls = 0 + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + self.calls += 1 + return super().create(spec, ref) + + +class _BadCreatingRelationMonitorFactory(CompositeEffectMonitorFactory): + """Return an invalid monitor value after successful static validation.""" + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + del spec, ref + return object() # type: ignore[return-value] + + +def _scene_registry( + *, + dynamic_collision: bool = False, +) -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider]]: cube_provider = _PoseProvider(torch.eye(4).repeat(2, 1, 1)) table_pose = torch.eye(4).repeat(2, 1, 1) table_pose[:, 0, 3] = 0.6 @@ -246,6 +305,12 @@ def _scene_registry() -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider state_provider=cube_provider, semantic_type="cube", default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp}, + geometry_provider=(_GeometryProvider() if dynamic_collision else None), + collision_role=( + SceneCollisionRole.DYNAMIC + if dynamic_collision + else SceneCollisionRole.NONE + ), ), SceneEntityRegistration( ref=grasp, @@ -271,12 +336,15 @@ def _scene_registry() -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider affordance_revision="relation-v1", relative_pose=torch.eye(4), ), - ) + ), + collision_world_mode=( + SceneCollisionWorldMode.PER_ENV if dynamic_collision else None + ), ) return registry, (cube_provider, table_provider) -def _profile() -> RobotSkillProfile: +def _profile(*, preset: SkillPolicyPreset | None = None) -> RobotSkillProfile: return RobotSkillProfile( profile_id="test_robot", resources={ @@ -300,7 +368,7 @@ def _profile() -> RobotSkillProfile: grasp=torch.tensor([1.0]), ) }, - presets={"safe": SkillPolicyPreset("safe")}, + presets={"safe": SkillPolicyPreset("safe") if preset is None else preset}, default_preset="safe", ) @@ -342,7 +410,11 @@ def _dual_profile(*, provider_id: str | None = "dual_center") -> RobotSkillProfi ) -def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: +def _engine( + profile: RobotSkillProfile, + *, + supports_dynamic_collision_world: bool = False, +) -> AtomicActionEngine: robot = Mock() robot.device = torch.device("cpu") control_parts = tuple( @@ -366,6 +438,7 @@ def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: generator.robot = robot generator.device = torch.device("cpu") generator.planner.cfg.planner_type = "stub_planner" + generator.supports_dynamic_collision_world = supports_dynamic_collision_world return AtomicActionEngine(generator, skill_profile=profile) @@ -373,8 +446,10 @@ def _integration( registry: SceneRegistry, *, registered: bool = False, + profile: RobotSkillProfile | None = None, + supports_dynamic_collision_world: bool = False, ) -> tuple[SemanticIntegrationManifest, AtomicActionEngine]: - profile = _profile() + selected_profile = _profile() if profile is None else profile catalog = builtin_semantic_call_catalog() if registered: assert _PICK_TARGET.binding_contract is not None @@ -387,10 +462,13 @@ def _integration( ) manifest = SemanticIntegrationManifest( scene=SceneManifest.from_registry(registry), - robot_profile=profile, + robot_profile=selected_profile, call_catalog=catalog, ) - return manifest, _engine(profile) + return manifest, _engine( + selected_profile, + supports_dynamic_collision_world=supports_dynamic_collision_world, + ) def _compiler( @@ -401,14 +479,25 @@ def _compiler( _FrameRelationGrounder(), ), registered_lowerers: tuple[RegisteredSemanticLowerer, ...] = (), + handover_pose_providers: tuple[HandOverPoseProvider, ...] = (), + profile: RobotSkillProfile | None = None, + effect_monitor_registry: EffectMonitorRegistry | None = None, + supports_dynamic_collision_world: bool = False, ) -> tuple[SemanticSkillCompiler, AtomicActionEngine]: - manifest, engine = _integration(registry, registered=registered) + manifest, engine = _integration( + registry, + registered=registered, + profile=profile, + supports_dynamic_collision_world=supports_dynamic_collision_world, + ) bound = manifest.bind(registry, engine) return ( SemanticSkillCompiler( bound, relation_grounders=relation_grounders, registered_lowerers=registered_lowerers, + handover_pose_providers=handover_pose_providers, + effect_monitor_registry=effect_monitor_registry, ), engine, ) @@ -470,7 +559,7 @@ def _held_context( object_to_eef: torch.Tensor, *, env_mask: torch.Tensor | None = None, - control_part: str = "manipulator", + task_state_key: str = "manipulator", robot_dof: int = 2, ) -> PlanningContext: held = HeldObjectState( @@ -484,12 +573,350 @@ def _held_context( task=TaskState( batch_size=2, device="cpu", - held_objects={control_part: held}, + held_objects={task_state_key: held}, ), robot_dof=robot_dof, ) +def test_curated_analysis_selects_exact_preset_monitor_without_creating_it() -> None: + registry, providers = _scene_registry() + factory = _CountingRelationMonitorFactory() + compiler, _ = _compiler( + registry, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + monitor_ref = workflow.calls[0].effect_monitor_ref + assert monitor_ref is not None + assert monitor_ref.monitor_id == COMPOSITE_EFFECT_MONITOR_ID + assert monitor_ref.revision == COMPOSITE_EFFECT_MONITOR_REVISION + assert workflow.calls[0].symbolic_writes == frozenset( + {SymbolicStateKey.held_object("manipulator")} + ) + assert not workflow.calls[0].opaque_symbolic_effect + assert factory.calls == 0 + assert [provider.calls for provider in providers] == [0, 0] + + +def test_curated_analysis_rejects_explicitly_missing_monitor() -> None: + registry, _ = _scene_registry() + profile = _profile( + preset=SkillPolicyPreset("safe", effect_monitors={}), + ) + compiler, _ = _compiler(registry, profile=profile) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + assert error.value.diagnostic.code == "missing_effect_monitor" + + +def test_uninstalled_effect_monitor_fails_analysis_without_factory_creation() -> None: + registry, providers = _scene_registry() + factory = _CountingRelationMonitorFactory() + profile = _profile( + preset=SkillPolicyPreset( + "safe", + effect_monitors={ + "pick": EffectMonitorRef("test.not_installed", "1"), + }, + ), + ) + compiler, _ = _compiler( + registry, + profile=profile, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + assert error.value.diagnostic.code == "effect_monitor_not_installed" + assert factory.calls == 0 + assert [provider.calls for provider in providers] == [0, 0] + + +def test_invalid_effect_monitor_config_fails_analysis_without_side_effects() -> None: + registry, providers = _scene_registry() + factory = _CountingRelationMonitorFactory() + profile = _profile( + preset=SkillPolicyPreset( + "safe", + effect_monitors={ + "pick": EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + { + "attached_translation_threshold": 0.10, + "detached_translation_threshold": 0.05, + }, + ), + }, + ), + ) + compiler, _ = _compiler( + registry, + profile=profile, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "invalid_effect_monitor_config" + assert diagnostic.path == ("workflow", 0, "effect_monitor") + assert factory.calls == 0 + assert [provider.calls for provider in providers] == [0, 0] + + +def test_pick_effect_spec_binds_destination_and_fresh_monitor_per_grounding() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + context = _context(registry) + + first = compiler.ground(workflow, 0, context) + repeated = compiler.ground(workflow, 0, context) + revised = compiler.ground(workflow, 0, context, revision=1) + + spec = first.effect_spec + assert spec is not None + assert spec.semantic_id == "pick" + assert spec.effect_kind is SemanticEffectKind.ATTACH + assert spec.skill_id == first.invocation.skill_id + assert spec.invocation_id == first.invocation.invocation_id + assert spec.invocation_revision == 0 + torch.testing.assert_close(spec.env_ids, context.env_ids) + assert len(spec.state_expectations) == 1 + relation = spec.state_expectations[0] + assert isinstance(relation, HeldObjectStateExpectation) + assert relation.expectation_id == "destination" + assert relation.relation is HeldObjectRelation.ATTACHED + assert relation.object_id == "cube" + assert relation.slot_id == "primary" + assert relation.resource_id == "manipulator" + assert relation.task_state_key == "manipulator" + pose, constraint = spec.clauses + assert isinstance(pose, PoseRelationClause) + assert pose.expectation is PoseRelationExpectation.MATCHED + assert pose.baseline_object_to_endpoint is None + assert pose.source.address == ControlPartEvidenceAddress("arm", "pose_relation") + assert isinstance(constraint, BinaryEffectClause) + assert constraint.evidence_kind is BinaryEvidenceKind.CONSTRAINT + assert constraint.expected is True + assert constraint.source.address == ControlPartEvidenceAddress("hand", "constraint") + assert ( + first.analyzed.bound.binding.action_binding.endpoint( + "primary", "motion" + ).task_state_key + == "manipulator" + ) + assert first.effect_monitor is not None + assert repeated.effect_monitor is not None + assert revised.effect_monitor is not None + assert repeated.effect_monitor is not first.effect_monitor + assert revised.effect_monitor is not first.effect_monitor + assert repeated.effect_spec is not None + assert repeated.effect_spec.invocation_revision == 0 + assert revised.effect_spec is not None + assert revised.effect_spec.invocation_revision == 1 + assert revised.effect_monitor.spec.invocation_revision == 1 + + +def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + pick = compiler.ground(pick_workflow, 0, _context(registry)) + semantics = pick.invocation.goal.semantics + object_to_eef = torch.eye(4).repeat(2, 1, 1) + object_to_eef[:, 2, 3] = 0.12 + context = _held_context(registry, semantics, object_to_eef) + workflow = compiler.analyze( + ( + Place( + object=SceneObjectRef("cube"), + at=SemanticPose( + (0.5, -0.2, 0.4), + (1.0, 0.0, 0.0, 0.0), + ), + ), + ) + ) + + assert workflow.calls[0].symbolic_writes == frozenset( + {SymbolicStateKey.held_object("manipulator")} + ) + grounded = compiler.ground(workflow, 0, context) + + spec = grounded.effect_spec + assert spec is not None + assert spec.semantic_id == "place" + assert spec.effect_kind is SemanticEffectKind.RELEASE + assert len(spec.state_expectations) == 1 + relation = spec.state_expectations[0] + assert isinstance(relation, HeldObjectStateExpectation) + assert relation.expectation_id == "source" + assert relation.relation is HeldObjectRelation.DETACHED + assert relation.object_id == "cube" + assert relation.slot_id == "primary" + assert relation.resource_id == "manipulator" + assert relation.task_state_key == "manipulator" + pose, constraint = spec.clauses + assert isinstance(pose, PoseRelationClause) + assert pose.expectation is PoseRelationExpectation.SEPARATED + assert pose.baseline_object_to_endpoint is not None + torch.testing.assert_close( + pose.baseline_object_to_endpoint, + object_to_eef, + ) + assert isinstance(constraint, BinaryEffectClause) + assert constraint.expected is False + + +def test_handover_effect_spec_binds_source_and_destination_relations() -> None: + registry, _ = _scene_registry() + provider = _DualCenterHandOverProvider() + compiler, _ = _compiler( + registry, + profile=_dual_profile(), + handover_pose_providers=(provider,), + ) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + pick = compiler.ground( + pick_workflow, + 0, + _context(registry, robot_dof=4), + ) + semantics = pick.invocation.goal.semantics + object_to_source = torch.eye(4).repeat(2, 1, 1) + object_to_source[:, 0, 3] = 0.08 + context = _held_context( + registry, + semantics, + object_to_source, + task_state_key="left", + robot_dof=4, + ) + workflow = compiler.analyze((HandOver(object=SceneObjectRef("cube")),)) + + assert workflow.calls[0].symbolic_writes == frozenset( + { + SymbolicStateKey.held_object("left"), + SymbolicStateKey.held_object("right"), + } + ) + grounded = compiler.ground(workflow, 0, context) + + spec = grounded.effect_spec + assert spec is not None + assert spec.semantic_id == "hand_over" + assert spec.effect_kind is SemanticEffectKind.TRANSFER + assert tuple(relation.expectation_id for relation in spec.state_expectations) == ( + "source", + "destination", + ) + source, destination = spec.state_expectations + assert isinstance(source, HeldObjectStateExpectation) + assert source.relation is HeldObjectRelation.DETACHED + assert source.object_id == "cube" + assert source.slot_id == "source" + assert source.resource_id == "left" + assert source.task_state_key == "left" + source_pose, source_constraint, destination_pose, destination_constraint = ( + spec.clauses + ) + assert isinstance(source_pose, PoseRelationClause) + assert source_pose.expectation is PoseRelationExpectation.SEPARATED + assert source_pose.baseline_object_to_endpoint is not None + torch.testing.assert_close( + source_pose.baseline_object_to_endpoint, + object_to_source, + ) + assert isinstance(source_constraint, BinaryEffectClause) + assert source_constraint.expected is False + assert isinstance(destination, HeldObjectStateExpectation) + assert destination.relation is HeldObjectRelation.ATTACHED + assert destination.object_id == "cube" + assert destination.slot_id == "destination" + assert destination.resource_id == "right" + assert destination.task_state_key == "right" + assert isinstance(destination_pose, PoseRelationClause) + assert destination_pose.expectation is PoseRelationExpectation.MATCHED + assert destination_pose.baseline_object_to_endpoint is None + assert isinstance(destination_constraint, BinaryEffectClause) + assert destination_constraint.expected is True + + +def test_registered_call_without_monitor_has_no_effect_contract() -> None: + registry, _ = _scene_registry() + factory = _CountingRelationMonitorFactory() + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_InspectLowerer(),), + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + + grounded = compiler.ground(workflow, 0, _context(registry)) + + assert workflow.calls[0].symbolic_writes == frozenset() + assert workflow.calls[0].opaque_symbolic_effect + assert workflow.calls[0].effect_monitor_ref is None + assert grounded.effect_spec is None + assert grounded.effect_monitor is None + assert factory.calls == 0 + + +def test_registered_monitor_without_effect_grounder_fails_during_analysis() -> None: + registry, _ = _scene_registry() + profile = _profile( + preset=SkillPolicyPreset( + "safe", + effect_monitors={ + "vendor.inspect": EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + }, + ) + ) + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_InspectLowerer(),), + profile=profile, + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + + assert error.value.diagnostic.code == "registered_effect_contract_not_installed" + assert error.value.diagnostic.path == ("workflow", 0, "effect_monitor") + + +def test_ground_wraps_effect_monitor_factory_contract_failure_with_path() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler( + registry, + effect_monitor_registry=EffectMonitorRegistry( + (_BadCreatingRelationMonitorFactory(),) + ), + ) + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(registry)) + + assert error.value.diagnostic.code == "effect_monitor_creation_failed" + assert error.value.diagnostic.path == ("workflow", 0, "effect_monitor") + + def test_analysis_is_provider_free_and_propagates_object_target() -> None: registry, providers = _scene_registry() compiler, engine = _compiler(registry) @@ -504,8 +931,8 @@ def test_analysis_is_provider_free_and_propagates_object_target() -> None: ) assert [provider.calls for provider in providers] == [0, 0] - assert workflow.calls[0].downstream_object_target is not None - assert workflow.calls[0].downstream_object_target.pose is not drop + assert len(workflow.calls[0].downstream_object_targets) == 1 + assert workflow.calls[0].downstream_object_targets[0].pose is not drop assert workflow.effect_dependencies[0].producer_index == 0 context = _context(registry) grounded = compiler.ground(workflow, 0, context) @@ -520,30 +947,32 @@ def test_analysis_is_provider_free_and_propagates_object_target() -> None: engine.resolve(grounded.invocation) -def test_grounded_eligibility_hands_off_to_execution_session() -> None: - registry, _ = _scene_registry() - compiler, engine = _compiler(registry) - context = _context(registry) +def test_grounded_safe_invocation_requires_registered_dynamic_collision() -> None: + registry, _ = _scene_registry(dynamic_collision=True) + profile = _profile( + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ) + ) + compiler, engine = _compiler( + registry, + profile=profile, + supports_dynamic_collision_world=True, + ) workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) - eligible_mask = torch.tensor([False, False]) - grounded = compiler.ground( - workflow, - 0, - context, - eligible_mask=eligible_mask, + grounded = compiler.ground(workflow, 0, _context(registry)) + + assert ( + grounded.invocation.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED ) - eligible_mask.fill_(True) - session = engine.start( - (grounded.invocation,), - context, - eligible_mask=grounded.eligible_mask, + assert ( + engine.resolve(grounded.invocation).motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED ) - assert grounded.eligible_mask.tolist() == [False, False] - assert session.status is ExecutionStatus.FAILED - assert session.eligible_mask.tolist() == [False, False] - def test_place_inherits_known_pick_resource_when_primary_is_omitted() -> None: registry, _ = _scene_registry() @@ -739,7 +1168,7 @@ def test_handover_uses_profile_selected_named_provider_and_stops_lookahead() -> assert provider.calls == 0 assert [scene_provider.calls for scene_provider in providers] == [0, 0] - assert workflow.calls[0].downstream_object_target is not None + assert workflow.calls[0].downstream_object_targets pick = compiler.ground(workflow, 0, _context(registry, robot_dof=4)) assert provider.calls == 1 pick_options = pick.invocation.skill_options @@ -751,7 +1180,7 @@ def test_handover_uses_profile_selected_named_provider_and_stops_lookahead() -> registry, pick.invocation.goal.semantics, torch.eye(4).repeat(2, 1, 1), - control_part="left", + task_state_key="left", robot_dof=4, ) handover = compiler.ground(workflow, 1, held_context) @@ -786,7 +1215,7 @@ def capture_middle(matrix: torch.Tensor, name: str) -> torch.Tensor: registry, pick.invocation.goal.semantics, torch.eye(4).repeat(2, 1, 1), - control_part="left", + task_state_key="left", robot_dof=4, ) with pytest.raises(RuntimeError, match="captured target"): @@ -978,7 +1407,7 @@ def test_registered_lowerer_is_explicit_and_opaque_to_lookahead() -> None: ) ) - assert workflow.calls[0].downstream_object_target is None + assert workflow.calls[0].downstream_object_targets == () assert workflow.effect_dependencies[0].producer_index is None grounded = compiler.ground(workflow, 1, _context(registry)) assert grounded.invocation.skill_id == "pick_up" diff --git a/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py new file mode 100644 index 000000000..cfdb3ca3a --- /dev/null +++ b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py @@ -0,0 +1,375 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Real semantic-runtime recovery gate for a dynamic cuRobo collision world.""" + +from __future__ import annotations + +from typing import ClassVar + +import pytest +import torch + +# Module-level guards must precede cuRobo-only imports. +pytest.importorskip("curobo") +if not torch.cuda.is_available(): + pytest.skip("cuRobo V2 requires CUDA", allow_module_level=True) + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg # noqa: E402 +from embodichain.lab.sim.atomic_actions import ( # noqa: E402 + CARTESIAN_POSE_CAPABILITY, + AtomicActionEngine, + CommandAcknowledgement, + DynamicCollisionMode, + EndEffectorPoseGoal, + ExecutionEventKind, + ExecutionRunnerCfg, + MotionPolicy, + MoveEndEffector, + PlanningContext, + RecoveryPolicy, + RuntimeCommandFrame, + RuntimeEndpointTarget, + SimulationExecutionAdapter, + SkillDescriptor, +) +from embodichain.lab.sim.cfg import RigidBodyAttributesCfg # noqa: E402 +from embodichain.lab.sim.objects import RigidObjectCfg # noqa: E402 +from embodichain.lab.sim.planners import MotionGenCfg, MotionGenerator # noqa: E402 +from embodichain.lab.sim.planners.curobo.curobo_planner import ( # noqa: E402 + CuroboAutoGenCfg, + CuroboPlannerCfg, + CuroboWorldCfg, +) +from embodichain.lab.sim.robots import FrankaPandaCfg # noqa: E402 +from embodichain.lab.sim.shapes import CubeCfg # noqa: E402 +from embodichain.lab.sim.skills import ( # noqa: E402 + BoundSemanticCall, + ControlPartEndpoint, + EffectEvidenceCollector, + EffectEvidenceProviderRegistry, + RegisteredSemanticCall, + RegisteredSemanticLowerer, + RobotResource, + RobotSkillProfile, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneManifest, + SceneRegistry, + SemanticCallDescriptor, + SemanticIntegrationManifest, + SemanticLowering, + SemanticSkillCompiler, + SkillPolicyPreset, + SkillRuntime, + SkillStatus, + builtin_semantic_call_catalog, +) + +pytestmark = [ + pytest.mark.requires_sim, + pytest.mark.gpu, + pytest.mark.slow, +] + +ROBOT_UID = "semantic_dynamic_scene_franka" +OBSTACLE_UID = "semantic_dynamic_obstacle" +CONTROL_PART = "arm" +CALL_ID = "test.move_end_effector" +SAMPLE_COUNT = 80 +COMMAND_CYCLE_TIME = 0.1 +MOVE_AFTER_COMMAND = 12 +OBSTACLE_SIZE = [0.10, 0.10, 0.12] +OBSTACLE_START_POSITION = [0.59, -0.20, 0.455] +MAXIMUM_FINAL_EEF_ERROR = 0.04 + +_MOVE_TARGET = MoveEndEffector.descriptor() +assert _MOVE_TARGET.binding_contract is not None + + +class _MoveEndEffectorLowerer(RegisteredSemanticLowerer): + """Lower a declarative matrix into the built-in Cartesian motion goal.""" + + call_id: ClassVar[str] = CALL_ID + schema_version: ClassVar[int] = 1 + target_descriptor: ClassVar[SkillDescriptor] = _MOVE_TARGET + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> SemanticLowering: + del bound + values = call.arguments.get("xpos") + if type(values) is not tuple or len(values) != 16: + raise ValueError("xpos must contain one flattened 4x4 pose matrix.") + pose = torch.tensor( + values, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ).reshape(4, 4) + return SemanticLowering(goal=EndEffectorPoseGoal(xpos=pose)) + + +class _CountingCommandSink: + """Count accepted real-simulation command frames while delegating transport.""" + + def __init__(self, delegate: SimulationExecutionAdapter) -> None: + self.delegate = delegate + self.command_count = 0 + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + acknowledgement = self.delegate.send(command, timeout=timeout) + if acknowledgement.accepted: + self.command_count += 1 + return acknowledgement + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + return self.delegate.hold(targets, context, timeout=timeout) + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + return self.delegate.cancel(targets, timeout=timeout) + + +def _profile() -> RobotSkillProfile: + """Declare the exact robot resource and bounded safe recovery policy.""" + return RobotSkillProfile( + profile_id="semantic_dynamic_scene_franka", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part=CONTROL_PART, + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ) + }, + ) + }, + presets={ + "safe": SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=SAMPLE_COUNT, + control_dt=COMMAND_CYCLE_TIME, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + tracking_error_threshold=0.1, + action_timeout=30.0, + ), + runner_cfg=ExecutionRunnerCfg(minimum_cycle_time=COMMAND_CYCLE_TIME), + ) + }, + default_preset="safe", + ) + + +def _compiler( + registry: SceneRegistry, + engine: AtomicActionEngine, +) -> SemanticSkillCompiler: + """Bind the test semantic extension to the real engine and scene registry.""" + catalog = builtin_semantic_call_catalog().with_descriptor( + SemanticCallDescriptor( + call_id=CALL_ID, + spec_type=RegisteredSemanticCall, + skill_id=_MOVE_TARGET.skill_id, + binding_contract=_MOVE_TARGET.binding_contract, + target_descriptor=_MOVE_TARGET, + ) + ) + integration = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=_profile(), + call_catalog=catalog, + ).bind(registry, engine) + return SemanticSkillCompiler( + integration, + registered_lowerers=(_MoveEndEffectorLowerer(),), + ) + + +def test_semantic_runtime_replans_after_dynamic_curobo_world_change() -> None: + """Run semantic lowering, real cuRobo planning, world update, and recovery.""" + sim = SimulationManager( + SimulationManagerCfg(headless=True, sim_device="cuda", num_envs=1) + ) + planner = None + try: + robot = sim.add_robot( + cfg=FrankaPandaCfg.from_dict({"uid": ROBOT_UID, "robot_type": "panda"}) + ) + obstacle = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid=OBSTACLE_UID, + shape=CubeCfg(size=OBSTACLE_SIZE), + attrs=RigidBodyAttributesCfg(), + body_type="kinematic", + init_pos=OBSTACLE_START_POSITION, + init_rot=[0.0, 0.0, 0.0], + ) + ) + sim.update(step=10) + + motion_generator = MotionGenerator( + MotionGenCfg( + planner_cfg=CuroboPlannerCfg( + robot_uid=ROBOT_UID, + auto_gen=CuroboAutoGenCfg( + fit_type="morphit", + sphere_density=0.3, + collision_sphere_buffer=0.005, + ), + world=CuroboWorldCfg( + rigid_objects=[obstacle], + obstacle_representation="cuboid", + dynamic_obstacle_names=[OBSTACLE_UID], + multi_env=False, + ), + warmup_iterations=0, + ) + ) + ) + planner = motion_generator.planner + registry = SceneRegistry.from_simulation( + sim, + rigid_objects={OBSTACLE_UID: OBSTACLE_UID}, + collision_roles={OBSTACLE_UID: SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.SHARED, + ) + scene_provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=1, + ) + adapter = SimulationExecutionAdapter( + sim, + robot, + scene_provider=scene_provider, + ) + sink = _CountingCommandSink(adapter) + engine = AtomicActionEngine(motion_generator) + runtime = SkillRuntime.from_components( + _compiler(registry, engine), + adapter, + sink, + EffectEvidenceCollector(EffectEvidenceProviderRegistry()), + clock=adapter, + ) + + start_pose = robot.compute_fk( + qpos=robot.get_qpos(name=CONTROL_PART), + name=CONTROL_PART, + to_matrix=True, + ) + target_pose = start_pose.clone() + target_pose[:, :3, 3] += torch.tensor( + [0.22, 0.24, 0.12], + dtype=target_pose.dtype, + device=target_pose.device, + ) + call = RegisteredSemanticCall( + call_id=CALL_ID, + arguments={ + "xpos": tuple( + float(value) + for value in target_pose[0].detach().cpu().reshape(-1).tolist() + ) + }, + resources={"primary": "manipulator"}, + ) + + result = runtime.start(call, workflow_id="dynamic_curobo_recovery") + assert result.status is SkillStatus.RUNNING + obstacle_moved = False + for _ in range(2_000): + if result.terminal: + break + if result.wait_duration > 0.0: + adapter.sleep(result.wait_duration) + result = runtime.step() + if not obstacle_moved and sink.command_count >= MOVE_AFTER_COMMAND: + blocking_pose = obstacle.get_local_pose(to_matrix=True).clone() + blocking_pose[:, :3, 3] = 0.5 * ( + start_pose[:, :3, 3] + target_pose[:, :3, 3] + ) + obstacle.set_local_pose(blocking_pose) + adapter.sleep(adapter.physics_dt) + obstacle_moved = True + + assert obstacle_moved + assert result.status is SkillStatus.COMPLETED, result.message + assert result.success_mask.tolist() == [True] + assert len(result.calls) == 1 + trace = result.calls[0] + assert trace.semantic_id == CALL_ID + assert trace.skill_id == MoveEndEffector.skill_id + assert len(trace.plan_attempts) >= 2 + + event_kinds = tuple(event.kind for event in result.events) + assert ExecutionEventKind.COLLISION_WORLD_CHANGED in event_kinds + assert ExecutionEventKind.REPLANNED in event_kinds + + initial_attempt = trace.plan_attempts[0] + changed_attempts = tuple( + attempt + for attempt in trace.plan_attempts[1:] + if attempt.planned_collision_world_revision[0] + > initial_attempt.planned_collision_world_revision[0] + ) + assert changed_attempts + assert changed_attempts[0].trigger == ExecutionEventKind.REPLANNED.value + assert changed_attempts[0].planner_backend == "curobo" + assert initial_attempt.collision_world_sensitive + assert ( + initial_attempt.resolved_core_policy.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + + final_pose = robot.compute_fk( + qpos=robot.get_qpos(name=CONTROL_PART), + name=CONTROL_PART, + to_matrix=True, + ) + final_error = torch.linalg.vector_norm( + final_pose[:, :3, 3] - target_pose[:, :3, 3], + dim=1, + ) + assert bool((final_error < MAXIMUM_FINAL_EEF_ERROR).all().item()) + finally: + if planner is not None: + planner.close() + sim.destroy() + SimulationManager.flush_cleanup_queue() diff --git a/tests/sim/skills/test_effects.py b/tests/sim/skills/test_effects.py new file mode 100644 index 000000000..3794dad03 --- /dev/null +++ b/tests/sim/skills/test_effects.py @@ -0,0 +1,863 @@ +# ---------------------------------------------------------------------------- +# 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 typed semantic-effect contracts and raw evidence monitors.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, replace +import json +import math +from types import MappingProxyType + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + ArticulationJointState, + EffectVerificationRequest, + HeldObjectState, + ObjectSemantics, + StateDelta, +) +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CompositeEffectMonitor, + CompositeEffectMonitorCfg, + CompositeEffectMonitorFactory, + CoordinatedHeldObjectCleanupExpectation, + EffectEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectMonitor, + EffectMonitorDecision, + EffectMonitorFactory, + EffectMonitorRef, + EffectMonitorRegistry, + HeldObjectRelation, + HeldObjectStateExpectation, + JointStateEffectClause, + JointStateEvidenceBatch, + PoseRelationClause, + PoseRelationEvidenceBatch, + PoseRelationExpectation, + ScalarEffectClause, + ScalarEffectEvidenceBatch, + ScalarEvidenceKind, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, +) + +_ENV_IDS = torch.tensor([101, 205, 309], dtype=torch.long) +_OBJECT_ID = "scene/cube" +_STATE_KEY = "left_actor" +_SKILL_ID = "pick_up" +_INVOCATION_ID = "call-7" + + +@dataclass(frozen=True, slots=True) +class _EvidenceAddress(EffectEvidenceAddress): + """Minimal custom observation address used by contract tests.""" + + endpoint: str + channel: str + + @property + def address_fingerprint(self) -> tuple[type, str, str]: + return type(self), self.endpoint, self.channel + + +class _AliasingAddress(_EvidenceAddress): + """Address intentionally violating snapshot ownership.""" + + def snapshot(self) -> EffectEvidenceAddress: + return self + + +def _source(channel: str) -> EffectEvidenceSourceRef: + return EffectEvidenceSourceRef( + "test.raw_evidence", + "1", + _EvidenceAddress("left_actor", channel), + ) + + +def _poses(*x_offsets: float) -> torch.Tensor: + poses = torch.eye(4).repeat(len(x_offsets), 1, 1) + poses[:, 0, 3] = torch.tensor(x_offsets) + return poses + + +def _semantics(object_id: str = _OBJECT_ID) -> ObjectSemantics: + return ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="object", + entity_id=object_id, + ) + + +def _held( + *, + object_id: str = _OBJECT_ID, + baseline: torch.Tensor | None = None, + env_mask: torch.Tensor | None = None, +) -> HeldObjectState: + poses = _poses(0.0, 0.0, 0.0) if baseline is None else baseline + if env_mask is None: + env_mask = torch.ones(3, dtype=torch.bool) + return HeldObjectState( + semantics=_semantics(object_id), + object_to_eef=poses, + grasp_xpos=_poses(0.0, 0.0, 0.0), + env_mask=env_mask, + ) + + +def _expectation( + relation: HeldObjectRelation = HeldObjectRelation.ATTACHED, + *, + expectation_id: str = "destination", + state_key: str = _STATE_KEY, +) -> HeldObjectStateExpectation: + return HeldObjectStateExpectation( + expectation_id=expectation_id, + relation=relation, + object_id=_OBJECT_ID, + slot_id="primary", + resource_id="left_actor", + task_state_key=state_key, + ) + + +def _attach_spec() -> SemanticEffectSpec: + return SemanticEffectSpec( + semantic_id="pick", + effect_kind=SemanticEffectKind.ATTACH, + skill_id=_SKILL_ID, + invocation_id=_INVOCATION_ID, + invocation_revision=2, + env_ids=_ENV_IDS, + state_expectations=(_expectation(),), + clauses=( + PoseRelationClause( + "destination.pose", + "destination", + _source("pose_relation"), + PoseRelationExpectation.MATCHED, + ), + BinaryEffectClause( + "destination.constraint", + "destination", + _source("constraint"), + BinaryEvidenceKind.CONSTRAINT, + True, + ), + ), + ) + + +def _request( + *, + env_mask: torch.Tensor | None = None, + attempt_generation: int = 0, + verification_id: int = 1, + effects: StateDelta | None = None, +) -> EffectVerificationRequest: + if env_mask is None: + env_mask = torch.ones(3, dtype=torch.bool) + if effects is None: + effects = StateDelta(held_object_updates={_STATE_KEY: _held()}) + return EffectVerificationRequest( + verification_id=verification_id, + skill_id=_SKILL_ID, + invocation_id=_INVOCATION_ID, + invocation_revision=2, + invocation_index=0, + attempt_generation=attempt_generation, + terminal_segment="close", + requested_at=1.0, + deadline=10.0, + env_mask=env_mask, + expected_effects=effects, + ) + + +def _pose_evidence( + offsets: tuple[float, ...], + *, + timestamp: float, + env_ids: torch.Tensor = _ENV_IDS, + valid: torch.Tensor | None = None, + revision: int = 4, +) -> PoseRelationEvidenceBatch: + if valid is None: + valid = torch.ones(len(offsets), dtype=torch.bool) + return PoseRelationEvidenceBatch( + evidence_id="destination.pose", + object_to_endpoint=_poses(*offsets), + valid=valid, + acquisition_errors=tuple( + None if row_valid else "pose unavailable" for row_valid in valid + ), + timestamp=timestamp, + env_ids=env_ids, + observation_revision=revision, + ) + + +def _binary_evidence( + values: tuple[bool, ...], + *, + timestamp: float, + env_ids: torch.Tensor = _ENV_IDS, + valid: torch.Tensor | None = None, + revision: int = 4, +) -> BinaryEffectEvidenceBatch: + if valid is None: + valid = torch.ones(len(values), dtype=torch.bool) + return BinaryEffectEvidenceBatch( + evidence_id="destination.constraint", + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + values=torch.tensor(values, dtype=torch.bool), + valid=valid, + acquisition_errors=tuple( + None if row_valid else "constraint unavailable" for row_valid in valid + ), + timestamp=timestamp, + env_ids=env_ids, + observation_revision=revision, + ) + + +def _evidence( + offsets: tuple[float, ...], + constraints: tuple[bool, ...], + *, + timestamp: float, + env_ids: torch.Tensor = _ENV_IDS, + valid: torch.Tensor | None = None, + revision: int = 4, +) -> Mapping[str, object]: + return { + "destination.pose": _pose_evidence( + offsets, + timestamp=timestamp, + env_ids=env_ids, + valid=valid, + revision=revision, + ), + "destination.constraint": _binary_evidence( + constraints, + timestamp=timestamp, + env_ids=env_ids, + valid=valid, + revision=revision, + ), + } + + +def test_monitor_ref_owns_bounded_non_executable_params() -> None: + params = {"limits": [1, {"enabled": True}]} + ref = EffectMonitorRef("monitor", "v1", params) + params["limits"][1]["enabled"] = False # type: ignore[index] + + assert isinstance(ref.params, MappingProxyType) + assert ref.params["limits"] == (1, MappingProxyType({"enabled": True})) + assert ref.snapshot().params is not ref.params + + +@pytest.mark.parametrize("value", [torch.tensor(1.0), lambda: None, math.inf]) +def test_monitor_ref_rejects_live_or_nonfinite_params(value: object) -> None: + with pytest.raises((TypeError, ValueError)): + EffectMonitorRef("monitor", "v1", {"bad": value}) + + +def test_monitor_ref_rejects_cyclic_params() -> None: + params: dict[str, object] = {} + params["cycle"] = params + + with pytest.raises(ValueError, match="cyclic"): + EffectMonitorRef("monitor", "v1", params) + + +def test_evidence_source_is_independent_from_runtime_command_addresses() -> None: + address = _EvidenceAddress("left_actor", "pose_relation") + source = EffectEvidenceSourceRef("provider", "2", address) + + assert source.address is not address + assert source.source_fingerprint == ( + "provider", + "2", + _EvidenceAddress, + (_EvidenceAddress, "left_actor", "pose_relation"), + ) + assert not hasattr(source, "transport_id") + + +def test_evidence_source_enforces_snapshot_ownership() -> None: + with pytest.raises(TypeError, match="independently owned"): + EffectEvidenceSourceRef( + "provider", + "1", + _AliasingAddress("left_actor", "pose_relation"), + ) + + +def test_semantic_spec_owns_typed_state_and_heterogeneous_clauses() -> None: + env_ids = _ENV_IDS.clone() + spec = _attach_spec() + env_ids[0] = -1 + + assert torch.equal(spec.env_ids, _ENV_IDS) + assert type(spec.state_expectations[0]) is HeldObjectStateExpectation + assert tuple(type(clause) for clause in spec.clauses) == ( + PoseRelationClause, + BinaryEffectClause, + ) + assert spec.snapshot().clauses[0] is not spec.clauses[0] + + +def test_spec_rejects_clause_without_typed_state_expectation() -> None: + with pytest.raises(ValueError, match="unknown state expectations"): + replace( + _attach_spec(), + clauses=(replace(_attach_spec().clauses[0], expectation_id="missing"),), + ) + + +def test_articulation_and_joint_clause_are_first_class_typed_contracts() -> None: + target = torch.tensor([0.42]) + spec = SemanticEffectSpec( + semantic_id="operate_articulation", + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id="operate_articulation", + invocation_id="drawer-1", + invocation_revision=0, + env_ids=_ENV_IDS, + state_expectations=( + ArticulationJointStateExpectation( + "drawer_joint", + "drawer", + "slide", + target, + ), + ), + clauses=( + JointStateEffectClause( + "drawer_joint.position", + "drawer_joint", + _source("joint_state"), + target, + ), + ), + ) + target.fill_(9.0) + + expectation = spec.state_expectations[0] + clause = spec.clauses[0] + assert isinstance(expectation, ArticulationJointStateExpectation) + assert isinstance(clause, JointStateEffectClause) + torch.testing.assert_close(expectation.target_position, torch.tensor([0.42])) + torch.testing.assert_close(clause.target_position, torch.tensor([0.42])) + + request = EffectVerificationRequest( + verification_id=1, + skill_id="operate_articulation", + invocation_id="drawer-1", + invocation_revision=0, + invocation_index=0, + attempt_generation=0, + terminal_segment="operate", + requested_at=1.0, + deadline=10.0, + env_mask=torch.ones(3, dtype=torch.bool), + expected_effects=StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.42])) + } + ), + ) + spec.validate_request(request) + + wrong = replace( + request, + expected_effects=StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.7])) + } + ), + ) + with pytest.raises(ValueError, match="target position"): + spec.validate_request(wrong) + + +def test_request_validation_uses_logical_state_key() -> None: + _attach_spec().validate_request(_request()) + + wrong_key = _request( + effects=StateDelta(held_object_updates={"arm_control_part": _held()}) + ) + with pytest.raises(ValueError, match="exactly match"): + _attach_spec().validate_request(wrong_key) + + +def test_request_validation_declares_coordinated_cleanup_explicitly() -> None: + cleanup = CoordinatedHeldObjectCleanupExpectation( + "cleanup:left_actor:support", + (_STATE_KEY, "support"), + ) + spec = replace( + _attach_spec(), + state_expectations=(*_attach_spec().state_expectations, cleanup), + ) + request = _request( + effects=StateDelta( + held_object_updates={_STATE_KEY: _held()}, + coordinated_held_object_updates={(_STATE_KEY, "support"): None}, + ) + ) + + spec.validate_request(request) + + +def test_pose_evidence_owns_rows_and_allows_invalid_nonfinite_payload() -> None: + poses = _poses(0.0, 0.1) + poses[1].fill_(math.nan) + valid = torch.tensor([True, False]) + batch = PoseRelationEvidenceBatch( + "pose", + poses, + valid, + (None, "occluded"), + 2.0, + torch.tensor([101, 205]), + 3, + ) + poses.zero_() + valid.fill_(True) + + assert torch.isnan(batch.object_to_endpoint[1]).all() + assert batch.valid.tolist() == [True, False] + + +def test_effect_contract_evidence_and_resolved_thresholds_are_json_safe() -> None: + poses = _poses(0.0, 0.1) + poses[1].fill_(math.nan) + batch = PoseRelationEvidenceBatch( + "pose", + poses, + torch.tensor([True, False]), + (None, "occluded"), + 2.0, + torch.tensor([101, 205]), + 3, + ) + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=3), + ) + + metadata = { + "spec": _attach_spec().to_metadata(), + "evidence": batch.to_metadata(), + "thresholds": dict(monitor.resolved_params), + } + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["evidence"]["object_to_endpoint"][1][0][0] is None + assert metadata["thresholds"]["attached_translation_threshold"] == 0.02 + assert metadata["thresholds"]["consecutive_samples"] == 3 + + +def test_binary_scalar_and_joint_evidence_are_distinct_raw_batches() -> None: + valid = torch.tensor([True, True]) + env_ids = torch.tensor([101, 205]) + binary = BinaryEffectEvidenceBatch( + "contact", + BinaryEvidenceKind.CONTACT, + torch.tensor([True, False]), + valid, + (None, None), + 2.0, + env_ids, + 3, + ) + scalar = ScalarEffectEvidenceBatch( + "force", + ScalarEvidenceKind.FORCE, + torch.tensor([2.0, 0.0]), + valid, + (None, None), + 2.0, + env_ids, + 3, + ) + joint = JointStateEvidenceBatch( + "joint", + torch.tensor([[0.4], [0.5]]), + torch.zeros(2, 1), + valid, + (None, None), + 2.0, + env_ids, + 3, + ) + + assert binary.values.dtype == torch.bool + assert scalar.values.tolist() == [2.0, 0.0] + assert joint.positions.shape == (2, 1) + + +def test_valid_raw_evidence_rejects_nonfinite_payload() -> None: + with pytest.raises(ValueError, match="finite"): + ScalarEffectEvidenceBatch( + "force", + ScalarEvidenceKind.FORCE, + torch.tensor([math.nan]), + torch.tensor([True]), + (None,), + 2.0, + torch.tensor([101]), + 3, + ) + + +def test_monitor_requires_pose_and_binary_physical_evidence() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + request = _request() + pose_only = monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (False, False, False), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + + assert not pose_only.success_mask.any() + assert pose_only.failure_mask.all() + + +def test_monitor_reports_success_only_for_complete_consecutive_evidence() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + request = _request() + first = monitor.observe( + request, + _evidence( + (0.0, 0.01, 0.019), + (True, True, True), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + second = monitor.observe( + request, + _evidence( + (0.0, 0.01, 0.019), + (True, True, True), + timestamp=3.0, + revision=5, + ), # type: ignore[arg-type] + ) + + assert not first.success_mask.any() + assert second.success_mask.all() + assert not second.failure_mask.any() + + +def test_invalid_evidence_is_unresolved_and_resets_hysteresis() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + request = _request() + monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + invalid = monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=3.0, + valid=torch.tensor([False, True, True]), + revision=5, + ), # type: ignore[arg-type] + ) + after_reset = monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=4.0, + revision=6, + ), # type: ignore[arg-type] + ) + + assert invalid.success_mask.tolist() == [False, True, True] + assert after_reset.success_mask.tolist() == [False, True, True] + + +def test_request_shrink_preserves_counts_and_generation_change_resets() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + monitor.observe( + _request(), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + shrunk = _request( + env_mask=torch.tensor([False, True, True]), + verification_id=2, + ) + preserved = monitor.observe( + shrunk, + _evidence( + (0.0, 0.0), + (True, True), + timestamp=3.0, + env_ids=torch.tensor([205, 309]), + revision=5, + ), # type: ignore[arg-type] + ) + reset = monitor.observe( + _request(attempt_generation=1, verification_id=3), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=3.0, + revision=5, + ), # type: ignore[arg-type] + ) + + assert preserved.success_mask.tolist() == [False, True, True] + assert not reset.success_mask.any() + + +def test_monitor_rejects_expansion_duplicate_counting_and_late_evidence() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + shrunk = _request(env_mask=torch.tensor([False, True, True])) + sample = _evidence( + (0.0, 0.0), + (True, True), + timestamp=2.0, + env_ids=torch.tensor([205, 309]), + ) + monitor.observe(shrunk, sample) # type: ignore[arg-type] + repeated = monitor.observe(shrunk, sample) # type: ignore[arg-type] + + assert not repeated.success_mask.any() + with pytest.raises(ValueError, match="only shrink"): + monitor.observe( + _request(verification_id=2), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=3.0, + revision=5, + ), # type: ignore[arg-type] + ) + with pytest.raises(ValueError, match="deadline"): + CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ).observe( + _request(), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=10.01, + ), # type: ignore[arg-type] + ) + + +def test_scalar_and_joint_clauses_use_monitor_owned_policy() -> None: + spec = replace( + _attach_spec(), + clauses=( + ScalarEffectClause( + "destination.force", + "destination", + _source("force"), + ScalarEvidenceKind.FORCE, + ScalarExpectation.PRESENT, + ), + JointStateEffectClause( + "destination.joint", + "destination", + _source("joint_state"), + torch.tensor([0.5]), + ), + ), + ) + monitor = CompositeEffectMonitor( + spec, + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + valid = torch.ones(3, dtype=torch.bool) + errors = (None, None, None) + evidence = { + "destination.force": ScalarEffectEvidenceBatch( + "destination.force", + ScalarEvidenceKind.FORCE, + torch.tensor([2.0, 0.0, 0.5]), + valid, + errors, + 2.0, + _ENV_IDS, + 4, + ), + "destination.joint": JointStateEvidenceBatch( + "destination.joint", + torch.tensor([[0.5], [0.5], [0.7]]), + None, + valid, + errors, + 2.0, + _ENV_IDS, + 4, + ), + } + + decision = monitor.observe(_request(), evidence) + + assert decision.success_mask.tolist() == [True, False, False] + assert decision.failure_mask.tolist() == [False, True, True] + + +class _BoundMonitor(EffectMonitor): + def __init__(self, spec: SemanticEffectSpec, *, alias: bool = False) -> None: + self._spec = spec if alias else spec.snapshot() + self._alias = alias + + @property + def spec(self) -> SemanticEffectSpec: + return self._spec if self._alias else self._spec.snapshot() + + def observe( + self, + request: EffectVerificationRequest, + evidence: Mapping[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + del evidence + return EffectMonitorDecision( + torch.zeros_like(request.env_mask), + torch.zeros_like(request.env_mask), + ) + + +class _BoundFactory(EffectMonitorFactory): + monitor_id = "test.bound" + revision = "1" + + def __init__(self, spec: SemanticEffectSpec, *, alias: bool = False) -> None: + self._spec = spec if alias else spec.snapshot() + self._alias = alias + + def validate_ref(self, ref: EffectMonitorRef) -> None: + if (ref.monitor_id, ref.revision) != (self.monitor_id, self.revision): + raise ValueError("wrong key") + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + del spec, ref + return _BoundMonitor(self._spec, alias=self._alias) + + +def test_registry_is_exact_versioned_and_enforces_bound_spec() -> None: + factory = CompositeEffectMonitorFactory() + registry = EffectMonitorRegistry((factory,)) + ref = EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + {"consecutive_samples": 1}, + ) + + first = registry.create(_attach_spec(), ref) + second = registry.create(_attach_spec(), ref) + + assert isinstance(first, CompositeEffectMonitor) + assert first is not second + with pytest.raises(KeyError): + registry.resolve(EffectMonitorRef(factory.monitor_id, "unknown")) + with pytest.raises(ValueError, match="Duplicate"): + EffectMonitorRegistry((factory, CompositeEffectMonitorFactory())) + + +def test_registry_rejects_factory_spec_drift_or_aliasing() -> None: + requested = _attach_spec() + changed = replace(requested, semantic_id="other") + drift = _BoundFactory(changed) + with pytest.raises(ValueError, match="different effect spec"): + EffectMonitorRegistry((drift,)).create( + requested, + EffectMonitorRef(drift.monitor_id, drift.revision), + ) + + alias = _BoundFactory(requested, alias=True) + with pytest.raises(TypeError, match="independently owned"): + EffectMonitorRegistry((alias,)).create( + requested, + EffectMonitorRef(alias.monitor_id, alias.revision), + ) + + +def test_composite_config_requires_real_hysteresis_gaps() -> None: + with pytest.raises(ValueError, match="less than"): + CompositeEffectMonitorCfg( + attached_translation_threshold=0.05, + detached_translation_threshold=0.05, + ) + with pytest.raises(ValueError, match="positive integer"): + CompositeEffectMonitorCfg(consecutive_samples=True) + with pytest.raises(ValueError, match="Unknown"): + CompositeEffectMonitorFactory().validate_ref( + EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + {"typo": 1}, + ) + ) diff --git a/tests/sim/skills/test_evidence.py b/tests/sim/skills/test_evidence.py new file mode 100644 index 000000000..3fb097383 --- /dev/null +++ b/tests/sim/skills/test_evidence.py @@ -0,0 +1,666 @@ +# ---------------------------------------------------------------------------- +# 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 synchronized semantic-effect evidence acquisition.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + EntityState, + ObservedArticulationJointState, + SceneSnapshot, +) +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + FORCE_EFFECT_CHANNEL, + HeldObjectRelation, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + POSE_RELATION_EFFECT_CHANNEL, + PoseRelationClause, + PoseRelationExpectation, + ScalarEffectClause, + ScalarEvidenceKind, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, +) +from embodichain.lab.sim.skills.evidence import ( + BinaryEffectEvidenceQuery, + BinaryEffectObservation, + ControlPartSimulationEvidenceProvider, + EffectEvidenceCollectionContext, + EffectEvidenceCollector, + EffectEvidenceProvider, + EffectEvidenceProviderRegistry, + JointStateEvidenceQuery, + JointStateObservation, + PoseRelationEvidenceQuery, + ScalarEffectEvidenceQuery, + ScalarEffectObservation, + SceneArticulationEvidenceProvider, + build_effect_evidence_queries, +) +from embodichain.lab.sim.skills.scene import ( + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, +) + + +def _source(channel: str, *, provider_id: str | None = None) -> EffectEvidenceSourceRef: + return EffectEvidenceSourceRef( + provider_id or CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress("arm", channel), + ) + + +def _held_expectation() -> HeldObjectStateExpectation: + return HeldObjectStateExpectation( + "held", + HeldObjectRelation.ATTACHED, + "cube", + "actor", + "arm_resource", + "arm_resource", + ) + + +def _attach_spec( + *clauses: object, + env_ids: torch.Tensor | None = None, +) -> SemanticEffectSpec: + return SemanticEffectSpec( + semantic_id="pick:cube", + effect_kind=SemanticEffectKind.ATTACH, + skill_id="PickUp", + invocation_id="pick-1", + invocation_revision=0, + env_ids=torch.tensor([0, 1], dtype=torch.long) if env_ids is None else env_ids, + state_expectations=(_held_expectation(),), + clauses=clauses, + ) + + +def _pose_clause(clause_id: str = "pose") -> PoseRelationClause: + return PoseRelationClause( + clause_id, + "held", + _source(POSE_RELATION_EFFECT_CHANNEL), + PoseRelationExpectation.MATCHED, + ) + + +def _binary_clause( + clause_id: str = "contact", + *, + provider_id: str | None = None, +) -> BinaryEffectClause: + return BinaryEffectClause( + clause_id, + "held", + _source(CONTACT_EFFECT_CHANNEL, provider_id=provider_id), + BinaryEvidenceKind.CONTACT, + True, + ) + + +def _scalar_clause(clause_id: str = "force") -> ScalarEffectClause: + return ScalarEffectClause( + clause_id, + "held", + _source(FORCE_EFFECT_CHANNEL), + ScalarEvidenceKind.FORCE, + ScalarExpectation.PRESENT, + ) + + +class _FakeSceneProvider: + def __init__(self, poses: torch.Tensor, *, confidence: float = 1.0) -> None: + self.poses = poses + self.confidence = confidence + self.calls = 0 + self.received_env_ids: torch.Tensor | None = None + + def snapshot(self, *, timestamp: float, env_ids: torch.Tensor) -> SceneSnapshot: + self.calls += 1 + self.received_env_ids = env_ids.clone() + poses = self.poses.index_select(0, env_ids.to(device=self.poses.device)) + return SceneSnapshot( + timestamp=timestamp, + version=self.calls, + entities={"cube": EntityState(poses, confidence=self.confidence)}, + ) + + +class _FakeRobot: + def __init__(self, qpos: torch.Tensor, qvel: torch.Tensor | None = None) -> None: + self.qpos = qpos + self.qvel = torch.zeros_like(qpos) if qvel is None else qvel + self.fk_calls = 0 + self.qpos_calls = 0 + self.qvel_calls = 0 + + def get_qpos(self, name: str | None = None, target: bool = False) -> torch.Tensor: + assert name == "arm" + assert target is False + self.qpos_calls += 1 + return self.qpos + + def get_qvel(self, name: str | None = None, target: bool = False) -> torch.Tensor: + assert name == "arm" + assert target is False + self.qvel_calls += 1 + return self.qvel + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: Sequence[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + assert name == "arm" + assert env_ids is not None + assert to_matrix is True + self.fk_calls += 1 + poses = torch.eye(4, dtype=qpos.dtype, device=qpos.device).repeat( + qpos.shape[0], 1, 1 + ) + poses[:, 0, 3] = qpos[:, 0] + return poses + + +class _WrongTimestampProvider(EffectEvidenceProvider): + provider_id = "test.provider" + revision = "1" + + def collect( + self, + queries: tuple[object, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + query = queries[0] + assert isinstance(query, BinaryEffectEvidenceQuery) + batch_size = int(context.env_ids.numel()) + return { + query.evidence_id: BinaryEffectEvidenceBatch( + query.evidence_id, + BinaryEvidenceKind.CONTACT, + torch.ones(batch_size, dtype=torch.bool), + torch.ones(batch_size, dtype=torch.bool), + (None,) * batch_size, + context.timestamp + 1.0, + context.env_ids, + context.observation_revision, + ) + } + + +class _SecondRevisionProvider(_WrongTimestampProvider): + revision = "2" + + +def test_collection_context_validates_and_owns_env_ids() -> None: + env_ids = torch.tensor([3, 1], dtype=torch.long) + context = EffectEvidenceCollectionContext(1.25, 7, env_ids) + env_ids[0] = 99 + + assert context.timestamp == 1.25 + assert context.observation_revision == 7 + assert context.env_ids.tolist() == [3, 1] + assert context.snapshot().env_ids.data_ptr() != context.env_ids.data_ptr() + + with pytest.raises(ValueError, match="unique"): + EffectEvidenceCollectionContext(0.0, 0, torch.tensor([1, 1])) + with pytest.raises(ValueError, match="non-negative"): + EffectEvidenceCollectionContext(-0.1, 0, torch.tensor([0])) + + +def test_build_queries_preserves_clause_order_and_exact_types() -> None: + spec = _attach_spec(_pose_clause(), _binary_clause(), _scalar_clause()) + + queries = build_effect_evidence_queries(spec) + + assert tuple(type(query) for query in queries) == ( + PoseRelationEvidenceQuery, + BinaryEffectEvidenceQuery, + ScalarEffectEvidenceQuery, + ) + assert tuple(query.evidence_id for query in queries) == ( + "pose", + "contact", + "force", + ) + assert all(query.expectation.expectation_id == "held" for query in queries) + + +def test_provider_registry_requires_exact_unique_versions() -> None: + first = _WrongTimestampProvider() + second = _SecondRevisionProvider() + registry = EffectEvidenceProviderRegistry((first, second)) + + source_v1 = _source(CONTACT_EFFECT_CHANNEL, provider_id="test.provider") + assert registry.resolve(source_v1) is first + assert registry.providers[("test.provider", "2")] is second + + with pytest.raises(ValueError, match="Duplicate"): + EffectEvidenceProviderRegistry((first, _WrongTimestampProvider())) + with pytest.raises(KeyError, match="exact versions"): + registry.resolve( + EffectEvidenceSourceRef( + "test.provider", + "missing", + ControlPartEvidenceAddress("arm", CONTACT_EFFECT_CHANNEL), + ) + ) + + +def test_collector_rejects_provider_metadata_drift() -> None: + spec = _attach_spec(_binary_clause(provider_id="test.provider")) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((_WrongTimestampProvider(),)) + ) + + with pytest.raises(ValueError, match="collection timestamp"): + collector.collect(spec, timestamp=2.0, observation_revision=4) + + +def test_control_part_provider_collects_pose_and_joint_state_once() -> None: + object_poses = torch.eye(4).repeat(2, 1, 1) + object_poses[:, 0, 3] = torch.tensor([0.25, 0.5]) + robot = _FakeRobot(torch.tensor([[0.75, 1.0], [1.5, 2.0]])) + scene = _FakeSceneProvider(object_poses) + joint_clause = JointStateEffectClause( + "joints", + "held", + _source(JOINT_STATE_EFFECT_CHANNEL), + torch.tensor([0.0, 0.0]), + ) + spec = _attach_spec(_pose_clause(), joint_clause) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (ControlPartSimulationEvidenceProvider(robot, scene_provider=scene),) + ) + ) + + evidence = collector.collect(spec, timestamp=3.5, observation_revision=11) + + assert list(evidence) == ["pose", "joints"] + assert evidence["pose"].timestamp == 3.5 + assert evidence["joints"].observation_revision == 11 + assert torch.allclose( + evidence["pose"].object_to_endpoint[:, 0, 3], + torch.tensor([0.5, 1.0]), + ) + assert torch.equal(evidence["joints"].positions, robot.qpos) + assert torch.equal(evidence["joints"].velocities, robot.qvel) + assert scene.calls == 1 + assert robot.qpos_calls == 1 + assert robot.qvel_calls == 1 + assert robot.fk_calls == 1 + + +def test_control_part_provider_selects_requested_simulator_rows() -> None: + env_ids = torch.tensor([2, 0], dtype=torch.long) + object_poses = torch.eye(4).repeat(3, 1, 1) + robot = _FakeRobot(torch.tensor([[1.0], [2.0], [3.0]])) + scene = _FakeSceneProvider(object_poses) + spec = _attach_spec(_pose_clause(), env_ids=env_ids) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (ControlPartSimulationEvidenceProvider(robot, scene_provider=scene),) + ) + ) + + evidence = collector.collect(spec, timestamp=0.0, observation_revision=0) + + assert evidence["pose"].env_ids.tolist() == [2, 0] + assert evidence["pose"].object_to_endpoint[:, 0, 3].tolist() == [3.0, 1.0] + assert scene.received_env_ids is not None + assert scene.received_env_ids.tolist() == [2, 0] + + +def test_pose_queries_share_one_scene_and_fk_snapshot() -> None: + robot = _FakeRobot(torch.tensor([[0.0], [0.0]])) + scene = _FakeSceneProvider(torch.eye(4).repeat(2, 1, 1)) + spec = _attach_spec(_pose_clause("first"), _pose_clause("second")) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (ControlPartSimulationEvidenceProvider(robot, scene_provider=scene),) + ) + ) + + evidence = collector.collect(spec, timestamp=1.0, observation_revision=1) + + assert set(evidence) == {"first", "second"} + assert scene.calls == 1 + assert robot.fk_calls == 1 + assert robot.qpos_calls == 1 + + +def test_missing_backend_specific_callbacks_return_explicit_invalid_rows() -> None: + robot = _FakeRobot(torch.zeros((2, 1))) + spec = _attach_spec(_binary_clause(), _scalar_clause()) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((ControlPartSimulationEvidenceProvider(robot),)) + ) + + evidence = collector.collect(spec, timestamp=1.0, observation_revision=2) + + assert not evidence["contact"].valid.any() + assert not evidence["force"].valid.any() + assert all("callback" in error for error in evidence["contact"].acquisition_errors) + assert all("callback" in error for error in evidence["force"].acquisition_errors) + + +def test_callbacks_receive_owned_queries_and_propagate_row_validity() -> None: + robot = _FakeRobot(torch.zeros((2, 1))) + binary_values = torch.tensor([True, False]) + scalar_values = torch.tensor([3.0, 0.0]) + received_query: BinaryEffectEvidenceQuery | None = None + + def observe_contact( + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectObservation: + nonlocal received_query + received_query = query + assert context.env_ids.tolist() == [0, 1] + return BinaryEffectObservation( + binary_values, + torch.tensor([True, False]), + (None, "contact sensor unavailable"), + ) + + def observe_force( + query: ScalarEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> ScalarEffectObservation: + del query, context + return ScalarEffectObservation(scalar_values) + + provider = ControlPartSimulationEvidenceProvider( + robot, + contact_observer=observe_contact, + force_observer=observe_force, + ) + collector = EffectEvidenceCollector(EffectEvidenceProviderRegistry((provider,))) + + evidence = collector.collect( + _attach_spec(_binary_clause(), _scalar_clause()), + timestamp=1.0, + observation_revision=2, + ) + binary_values[:] = False + scalar_values[:] = 99.0 + + assert received_query is not None + assert received_query.evidence_id == "contact" + assert evidence["contact"].values.tolist() == [True, False] + assert evidence["contact"].valid.tolist() == [True, False] + assert evidence["force"].values.tolist() == [3.0, 0.0] + + +def test_pose_without_scene_provider_is_invalid_not_fabricated() -> None: + robot = _FakeRobot(torch.zeros((2, 1))) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((ControlPartSimulationEvidenceProvider(robot),)) + ) + + evidence = collector.collect( + _attach_spec(_pose_clause()), + timestamp=0.0, + observation_revision=0, + ) + + assert not evidence["pose"].valid.any() + assert all( + "scene provider" in error for error in evidence["pose"].acquisition_errors + ) + assert robot.fk_calls == 0 + + +def test_channel_mismatch_fails_before_callback() -> None: + wrong_clause = BinaryEffectClause( + "contact", + "held", + _source(CONSTRAINT_EFFECT_CHANNEL), + BinaryEvidenceKind.CONTACT, + True, + ) + robot = _FakeRobot(torch.zeros((2, 1))) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((ControlPartSimulationEvidenceProvider(robot),)) + ) + + with pytest.raises(ValueError, match="requires channel"): + collector.collect( + _attach_spec(wrong_clause), + timestamp=0.0, + observation_revision=0, + ) + + +def test_joint_query_type_is_built_for_articulation_expectation() -> None: + expectation = ArticulationJointStateExpectation( + "drawer_joint", + "drawer", + "slide", + torch.tensor([0.4]), + ) + clause = JointStateEffectClause( + "joint", + "drawer_joint", + _source(JOINT_STATE_EFFECT_CHANNEL), + torch.tensor([0.4]), + ) + spec = SemanticEffectSpec( + semantic_id="open:drawer", + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id="OperateArticulation", + invocation_id="open-1", + invocation_revision=0, + env_ids=torch.tensor([0]), + state_expectations=(expectation,), + clauses=(clause,), + ) + + query = build_effect_evidence_queries(spec)[0] + + assert isinstance(query, JointStateEvidenceQuery) + assert query.expectation.articulation_id == "drawer" + + +def _articulation_spec( + *clauses: JointStateEffectClause, + expectation_joint: str = "slide", +) -> SemanticEffectSpec: + expectation = ArticulationJointStateExpectation( + "drawer_joint", + "drawer", + expectation_joint, + torch.tensor([[0.4], [0.4]]), + ) + return SemanticEffectSpec( + semantic_id="open:drawer", + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id="OperateArticulation", + invocation_id="open-1", + invocation_revision=0, + env_ids=torch.tensor([0, 1]), + state_expectations=(expectation,), + clauses=clauses, + ) + + +def _articulation_clause(clause_id: str = "joint") -> JointStateEffectClause: + return JointStateEffectClause( + clause_id, + "drawer_joint", + EffectEvidenceSourceRef( + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress("drawer", "slide"), + ), + torch.tensor([[0.4], [0.4]]), + ) + + +def test_scene_articulation_provider_uses_explicit_typed_observer() -> None: + calls = 0 + + def observe_joint( + query: JointStateEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> JointStateObservation: + nonlocal calls + calls += 1 + address = query.source.address + assert isinstance(address, ArticulationJointEvidenceAddress) + assert (address.articulation_id, address.joint_id) == ("drawer", "slide") + assert context.observation_revision == 8 + return JointStateObservation( + positions=torch.tensor([[0.4], [0.3]]), + velocities=torch.tensor([[0.0], [0.1]]), + valid=torch.tensor([True, False]), + acquisition_errors=(None, "joint sensor unavailable"), + ) + + provider = SceneArticulationEvidenceProvider(observe_joint) + collector = EffectEvidenceCollector(EffectEvidenceProviderRegistry((provider,))) + + evidence = collector.collect( + _articulation_spec(_articulation_clause()), + timestamp=4.0, + observation_revision=8, + ) + + assert calls == 1 + assert torch.allclose( + evidence["joint"].positions, + torch.tensor([[0.4], [0.3]]), + ) + assert evidence["joint"].valid.tolist() == [True, False] + assert evidence["joint"].acquisition_errors == ( + None, + "joint sensor unavailable", + ) + + +def test_scene_articulation_provider_reads_typed_scene_snapshot_once() -> None: + class _JointSceneProvider: + calls = 0 + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + self.calls += 1 + return SceneSnapshot( + timestamp=timestamp, + version=self.calls, + articulation_joints={ + ("drawer", "slide"): ObservedArticulationJointState( + torch.tensor([[0.4], [0.3]]), + torch.tensor([True, False]), + ) + }, + ) + + scene_provider = _JointSceneProvider() + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (SceneArticulationEvidenceProvider(scene_provider=scene_provider),) + ) + ) + + evidence = collector.collect( + _articulation_spec( + _articulation_clause("position"), + _articulation_clause("settled_position"), + ), + timestamp=2.0, + observation_revision=5, + ) + + assert scene_provider.calls == 1 + assert torch.equal(evidence["position"].positions, torch.tensor([[0.4], [0.3]])) + assert evidence["settled_position"].valid.tolist() == [True, False] + + +def test_scene_articulation_provider_samples_same_address_once() -> None: + calls = 0 + + def observe_joint( + query: JointStateEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> JointStateObservation: + nonlocal calls + del query, context + calls += 1 + return JointStateObservation(torch.tensor([[0.4], [0.4]])) + + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (SceneArticulationEvidenceProvider(observe_joint),) + ) + ) + spec = _articulation_spec( + _articulation_clause("position"), + _articulation_clause("settled_position"), + ) + + evidence = collector.collect(spec, timestamp=1.0, observation_revision=1) + + assert set(evidence) == {"position", "settled_position"} + assert calls == 1 + + +def test_scene_articulation_provider_rejects_address_expectation_drift() -> None: + provider = SceneArticulationEvidenceProvider( + lambda query, context: JointStateObservation(torch.tensor([[0.4], [0.4]])) + ) + collector = EffectEvidenceCollector(EffectEvidenceProviderRegistry((provider,))) + + with pytest.raises(ValueError, match="exactly match"): + collector.collect( + _articulation_spec( + _articulation_clause(), + expectation_joint="other_joint", + ), + timestamp=1.0, + observation_revision=1, + ) diff --git a/tests/sim/skills/test_integration.py b/tests/sim/skills/test_integration.py index 028cdd3f3..c28c99fb6 100644 --- a/tests/sim/skills/test_integration.py +++ b/tests/sim/skills/test_integration.py @@ -30,9 +30,11 @@ BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, ControlPartCommandProfile, + DynamicCollisionMode, EntityState, FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, + MotionPolicy, ) from embodichain.lab.sim.skills.calls import ( Pick, @@ -41,7 +43,10 @@ SemanticCallDescriptor, builtin_semantic_call_catalog, ) +from embodichain.lab.sim.skills.effects import EffectMonitorRef from embodichain.lab.sim.skills.integration import ( + BoundSemanticCall, + BoundSemanticIntegration, SceneEntityManifest, SceneManifest, SemanticIntegrationManifest, @@ -58,6 +63,7 @@ GRASP_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, + SceneCollisionRole, SceneCollisionWorldMode, SceneEntityRegistration, SceneObjectRef, @@ -102,9 +108,17 @@ def __deepcopy__(self, memo: dict[int, object]) -> _CopyTrackedAffordance: return _CopyTrackedAffordance() +class _GeometryProvider: + """Return one opaque planner-facing geometry descriptor.""" + + def get_geometry(self) -> object: + return object() + + def _scene_registry( *, with_default: bool, + dynamic_collision: bool = False, ) -> tuple[SceneRegistry, _NeverObservedStateProvider]: provider = _NeverObservedStateProvider() object_ref = SceneObjectRef("cube") @@ -118,6 +132,12 @@ def _scene_registry( state_provider=provider, aliases=("sim_cube",), default_affordances=defaults, + geometry_provider=(_GeometryProvider() if dynamic_collision else None), + collision_role=( + SceneCollisionRole.DYNAMIC + if dynamic_collision + else SceneCollisionRole.NONE + ), ), SceneEntityRegistration( ref=side_grasp, @@ -142,14 +162,31 @@ def _scene_registry( affordance_revision="grasp-v1", relative_pose=torch.eye(4), ), - ) + ), + collision_world_mode=( + SceneCollisionWorldMode.PER_ENV if dynamic_collision else None + ), ) return registry, provider def _semantic_integration( registry: SceneRegistry, + *, + preset: SkillPolicyPreset | None = None, + additional_presets: tuple[SkillPolicyPreset, ...] = (), + default_preset: str | None = None, + skill_presets: dict[str, str] | None = None, + runtime_preset: str | None = None, ) -> SemanticIntegrationManifest: + selected_preset = SkillPolicyPreset("safe") if preset is None else preset + presets = {selected_preset.preset_id: selected_preset} + presets.update( + { + additional_preset.preset_id: additional_preset + for additional_preset in additional_presets + } + ) robot_profile = RobotSkillProfile( profile_id="test_robot", resources={ @@ -173,18 +210,24 @@ def _semantic_integration( grasp=torch.tensor([1.0]), ) }, - presets={"safe": SkillPolicyPreset("safe")}, - default_preset="safe", + presets=presets, + default_preset=( + selected_preset.preset_id if default_preset is None else default_preset + ), + skill_presets={} if skill_presets is None else skill_presets, ) return SemanticIntegrationManifest( scene=SceneManifest.from_registry(registry), robot_profile=robot_profile, call_catalog=builtin_semantic_call_catalog(), + runtime_preset=runtime_preset, ) def _engine_for_integration( integration: SemanticIntegrationManifest, + *, + supports_dynamic_collision_world: bool = False, ) -> AtomicActionEngine: """Build a minimal live engine whose resource graph matches the manifest.""" robot = Mock() @@ -204,6 +247,7 @@ def _engine_for_integration( generator.robot = robot generator.device = torch.device("cpu") generator.planner.cfg.planner_type = "stub_planner" + generator.supports_dynamic_collision_world = supports_dynamic_collision_world return AtomicActionEngine( generator, skill_profile=integration.robot_profile, @@ -392,6 +436,36 @@ class LiveCatalog(SemanticCallCatalog): ) +def test_semantic_integration_rejects_monitor_for_unknown_call_with_path() -> None: + registry, _ = _scene_registry(with_default=True) + unknown_semantic_id = "not_catalogued" + + with pytest.raises(SemanticValidationError) as error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + effect_monitors={ + unknown_semantic_id: EffectMonitorRef("test.monitor", "1") + }, + ), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_effect_monitor_call" + assert diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "effect_monitors", + unknown_semantic_id, + ) + assert diagnostic.rendered_path == ( + "integration.robot_profile.presets.safe.effect_monitors.not_catalogued" + ) + + def test_scene_manifest_reports_structured_pathful_diagnostic() -> None: manifest = SceneManifest((SceneEntityManifest(ref=SceneObjectRef("cube")),)) @@ -516,6 +590,289 @@ def test_bound_semantic_call_retains_installed_profile_ownership() -> None: assert result.binding.action_binding.owner_id == engine.binding_owner_id +@pytest.mark.parametrize( + "source_mode", + [ + DynamicCollisionMode.AUTO, + DynamicCollisionMode.OFF, + DynamicCollisionMode.REQUIRED, + ], +) +def test_safe_preset_requires_dynamic_collision_for_dynamic_scene( + source_mode: DynamicCollisionMode, +) -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy( + strategy="motion_gen", + dynamic_collision_mode=source_mode, + ), + ), + ) + engine = _engine_for_integration( + integration, + supports_dynamic_collision_world=True, + ) + + bound = integration.bind(registry, engine).link_call( + Pick(object=SceneObjectRef("cube")) + ) + + assert ( + bound.preset.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + assert ( + integration.robot_profile.presets["safe"].motion_policy.dynamic_collision_mode + is source_mode + ) + assert provider.calls == 0 + + +def test_safe_preset_rejects_unsupported_dynamic_planner_before_observation() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ) + engine = _engine_for_integration(integration) + bind_skill_profile = Mock(wraps=engine.bind_skill_profile) + engine.bind_skill_profile = bind_skill_profile # type: ignore[method-assign] + + with pytest.raises(SemanticValidationError) as error: + integration.bind(registry, engine) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "safe_dynamic_collision_unsupported" + assert diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "motion_policy", + "dynamic_collision_mode", + ) + assert diagnostic.candidates == () + assert "('cube',)" in diagnostic.message + bind_skill_profile.assert_not_called() + assert provider.calls == 0 + + +def test_per_skill_safe_preset_is_conservatively_preflighted() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + pick_skill_id = builtin_semantic_call_catalog().descriptors["pick"].skill_id + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset("fast"), + additional_presets=( + SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ), + skill_presets={pick_skill_id: "safe"}, + ) + engine = _engine_for_integration(integration) + + with pytest.raises(SemanticValidationError) as error: + integration.bind(registry, engine) + + assert error.value.diagnostic.code == "safe_dynamic_collision_unsupported" + assert provider.calls == 0 + + +def test_fully_overridden_safe_default_is_not_reachable() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + catalog = builtin_semantic_call_catalog() + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + additional_presets=(SkillPolicyPreset("fast"),), + skill_presets={ + descriptor.skill_id: "fast" for descriptor in catalog.descriptors.values() + }, + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine) + + assert ( + bound.link_call(Pick(object=SceneObjectRef("cube"))).preset.preset_id == "fast" + ) + assert provider.calls == 0 + + +def test_runtime_non_safe_override_makes_safe_default_unreachable() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + additional_presets=(SkillPolicyPreset("fast"),), + runtime_preset="fast", + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine) + + assert ( + bound.link_call(Pick(object=SceneObjectRef("cube"))).preset.preset_id == "fast" + ) + assert provider.calls == 0 + + +def test_bound_integration_cannot_bypass_safe_dynamic_planner_preflight() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ) + engine = _engine_for_integration(integration) + bound_profile = engine.bind_skill_profile(integration.robot_profile) + + with pytest.raises(SemanticValidationError) as error: + BoundSemanticIntegration( + manifest=integration, + scene_registry=registry, + robot_profile=bound_profile, + engine=engine, + ) + + assert error.value.diagnostic.code == "safe_dynamic_collision_unsupported" + assert error.value.diagnostic.path[-1] == "dynamic_collision_mode" + assert provider.calls == 0 + + +def test_bind_rejects_invalid_engine_before_safe_capability_lookup() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ) + + with pytest.raises(TypeError, match="engine must be an AtomicActionEngine"): + integration.bind(registry, object()) # type: ignore[arg-type] + + assert provider.calls == 0 + + +def test_safe_preset_rejects_non_motion_generator_strategy_for_dynamic_scene() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="ik_interp"), + ), + ) + engine = _engine_for_integration( + integration, + supports_dynamic_collision_world=True, + ) + + with pytest.raises(SemanticValidationError) as error: + integration.bind(registry, engine) + + assert error.value.diagnostic.code == "safe_dynamic_collision_unsupported" + assert error.value.diagnostic.path[-1] == "strategy" + assert provider.calls == 0 + + +@pytest.mark.parametrize( + "source_mode", + [DynamicCollisionMode.AUTO, DynamicCollisionMode.OFF], +) +def test_non_safe_preset_preserves_dynamic_collision_policy( + source_mode: DynamicCollisionMode, +) -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "fast", + motion_policy=MotionPolicy(dynamic_collision_mode=source_mode), + ), + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine).link_call( + Pick(object=SceneObjectRef("cube")) + ) + + assert bound.preset.preset_id == "fast" + assert bound.preset.motion_policy.dynamic_collision_mode is source_mode + assert provider.calls == 0 + + +@pytest.mark.parametrize( + "source_mode", + [DynamicCollisionMode.AUTO, DynamicCollisionMode.OFF], +) +def test_safe_preset_preserves_policy_without_dynamic_collision( + source_mode: DynamicCollisionMode, +) -> None: + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(dynamic_collision_mode=source_mode), + ), + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine).link_call( + Pick(object=SceneObjectRef("cube")) + ) + + assert bound.preset.motion_policy.dynamic_collision_mode is source_mode + assert provider.calls == 0 + + def test_bound_semantic_integration_rejects_engine_profile_rebind() -> None: registry, _ = _scene_registry(with_default=True) integration = _semantic_integration(registry) diff --git a/tests/sim/skills/test_parallel.py b/tests/sim/skills/test_parallel.py new file mode 100644 index 000000000..9c6044288 --- /dev/null +++ b/tests/sim/skills/test_parallel.py @@ -0,0 +1,251 @@ +# ---------------------------------------------------------------------------- +# 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 deterministic parallel-skill contracts.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + EndpointCommand, + JointPositionPayload, + JointPositionTarget, + RuntimeCommandFrame, + StateDelta, + TaskState, + TimedCommandSequence, +) +from embodichain.lab.sim.skills.parallel import ( + ParallelBranchPlan, + ParallelConflictError, + ParallelStateConflictError, + ParallelTimingError, + ParallelTimingPolicy, + align_parallel_commands, + merge_parallel_effects, + resolve_parallel_barrier, +) +from embodichain.lab.sim.skills.profiles import ResourceClaim + +ENV_IDS = torch.tensor([3, 7], dtype=torch.long) + + +def _sequence( + control_part: str, + joint_id: int, + frame_count: int, + *, + duration: float = 0.1, +) -> TimedCommandSequence: + target = JointPositionTarget(control_part, (joint_id,)) + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target, + JointPositionPayload(torch.full((2, 1), float(index + joint_id))), + ), + ), + active_mask=torch.tensor([True, True]), + env_ids=ENV_IDS, + hold_duration=torch.full((2,), duration), + ) + for index in range(frame_count) + ) + return TimedCommandSequence(frames, ENV_IDS) + + +def _branch( + branch_id: str, + control_part: str, + joint_id: int, + frame_count: int, + *, + duration: float = 0.1, +) -> ParallelBranchPlan: + return ParallelBranchPlan( + branch_id=branch_id, + claim=ResourceClaim(frozenset({control_part}), (joint_id,)), + commands=_sequence( + control_part, + joint_id, + frame_count, + duration=duration, + ), + ) + + +def test_parallel_alignment_hold_pads_shorter_disjoint_branch() -> None: + merged = align_parallel_commands( + ( + _branch("left", "left_arm", 0, 2), + _branch("right", "right_arm", 1, 3), + ), + ParallelTimingPolicy(step_dt=0.1), + ) + + assert merged.frame_count == 3 + assert all(len(frame.commands) == 2 for frame in merged.frames) + left_final = merged.frames[-1].commands[0].payload + assert isinstance(left_final, JointPositionPayload) + assert torch.equal(left_final.positions, torch.full((2, 1), 1.0)) + assert torch.equal(merged.frames[-1].active_mask, torch.tensor([True, True])) + + +def test_parallel_alignment_rejects_claim_and_grid_conflicts() -> None: + with pytest.raises(ParallelConflictError, match="overlapping"): + align_parallel_commands( + ( + _branch("one", "arm", 0, 2), + _branch("two", "arm", 1, 2), + ), + ParallelTimingPolicy(0.1), + ) + + +def test_parallel_alignment_rejects_different_lane_active_masks() -> None: + left = _branch("left", "left", 0, 1) + right = _branch("right", "right", 1, 1) + right_frame = right.commands.frames[0].with_active_mask(torch.tensor([False, True])) + right = ParallelBranchPlan( + branch_id=right.branch_id, + claim=right.claim, + commands=TimedCommandSequence((right_frame,), ENV_IDS), + ) + + with pytest.raises(ParallelTimingError, match="active masks"): + align_parallel_commands( + (left, right), + ParallelTimingPolicy(0.1), + ) + + +def test_parallel_alignment_validates_inactive_row_durations_on_same_grid() -> None: + left = _branch("left", "left", 0, 1) + left_frame = RuntimeCommandFrame( + commands=left.commands.frames[0].commands, + active_mask=torch.tensor([False, True]), + env_ids=ENV_IDS, + hold_duration=torch.tensor([0.2, 0.1]), + ) + left = ParallelBranchPlan( + branch_id=left.branch_id, + claim=left.claim, + commands=TimedCommandSequence((left_frame,), ENV_IDS), + ) + right = _branch("right", "right", 1, 1) + right_frame = RuntimeCommandFrame( + commands=right.commands.frames[0].commands, + active_mask=torch.tensor([False, True]), + env_ids=ENV_IDS, + hold_duration=torch.tensor([0.1, 0.1]), + ) + right = ParallelBranchPlan( + branch_id=right.branch_id, + claim=right.claim, + commands=TimedCommandSequence((right_frame,), ENV_IDS), + ) + + with pytest.raises(ParallelTimingError, match="step_dt"): + align_parallel_commands((left, right), ParallelTimingPolicy(0.1)) + + +def test_parallel_alignment_rejects_off_grid_duration() -> None: + with pytest.raises(ParallelTimingError, match="step_dt"): + align_parallel_commands( + ( + _branch("left", "left", 0, 2, duration=0.05), + _branch("right", "right", 1, 2), + ), + ParallelTimingPolicy(0.1), + ) + + +def test_parallel_effects_merge_disjoint_keys_by_verified_row() -> None: + state = TaskState.empty(batch_size=2, device="cpu") + merged = merge_parallel_effects( + state, + { + "drawer": ( + StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + } + ), + torch.tensor([True, False]), + ), + "door": ( + StateDelta( + articulation_joint_updates={ + ("door", "hinge"): ArticulationJointState(torch.tensor([1.0])) + } + ), + torch.tensor([False, True]), + ), + }, + ) + + drawer = merged.get_articulation_joint_state("drawer", "slide") + door = merged.get_articulation_joint_state("door", "hinge") + assert drawer is not None and door is not None + assert torch.equal(drawer.env_mask, torch.tensor([True, False])) + assert torch.equal(door.env_mask, torch.tensor([False, True])) + + +def test_parallel_effects_reject_same_key_on_same_row() -> None: + delta = StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + } + ) + with pytest.raises(ParallelStateConflictError, match="same symbolic keys"): + merge_parallel_effects( + TaskState.empty(2, "cpu"), + { + "one": (delta, torch.tensor([True, False])), + "two": (delta, torch.tensor([True, True])), + }, + ) + + +def test_parallel_barrier_cancels_pending_siblings_per_failed_row() -> None: + update = resolve_parallel_barrier( + pending_masks={ + "left": torch.tensor([False, True, True]), + "right": torch.tensor([True, False, True]), + }, + success_masks={ + "left": torch.tensor([True, False, False]), + "right": torch.tensor([False, True, False]), + }, + failure_masks={ + "left": torch.tensor([False, False, True]), + "right": torch.tensor([False, False, False]), + }, + ) + + assert torch.equal(update.failure_mask, torch.tensor([False, False, True])) + assert torch.equal(update.completed_mask, torch.tensor([False, False, True])) + assert torch.equal( + update.cancellation_masks["right"], + torch.tensor([False, False, True]), + ) + + +__all__: list[str] = [] diff --git a/tests/sim/skills/test_parallel_runtime.py b/tests/sim/skills/test_parallel_runtime.py new file mode 100644 index 000000000..6d53cb1bf --- /dev/null +++ b/tests/sim/skills/test_parallel_runtime.py @@ -0,0 +1,1264 @@ +# ---------------------------------------------------------------------------- +# 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 branch-local semantic execution at a parallel barrier.""" + +from __future__ import annotations + +from dataclasses import dataclass +import json + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + CommandAcknowledgement, + EndpointCommand, + JointPositionPayload, + JointPositionTarget, + PlanningContext, + RobotObservation, + RuntimeCommandFrame, + SceneSnapshot, + StateDelta, + TaskState, +) +from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelLaneCommandSink, + ParallelRuntimeBranch, + ParallelSkillRuntime, +) +from embodichain.lab.sim.skills.profiles import ResourceClaim +from embodichain.lab.sim.skills.runtime import SkillResult, SkillStatus + +ENV_IDS = torch.tensor([4, 9], dtype=torch.long) + + +class _Clock: + """Deterministic environment-grid clock.""" + + def __init__(self) -> None: + self.time = 0.0 + + def now(self) -> float: + return self.time + + def sleep(self, duration: float) -> None: + self.time += duration + + +class _OutboundSink: + """Record the coordinator's one merged transport transaction.""" + + def __init__( + self, + *, + reject: bool = False, + reject_cancel: bool = False, + reject_hold: bool = False, + raise_send: bool = False, + ) -> None: + self.reject = reject + self.reject_cancel = reject_cancel + self.reject_hold = reject_hold + self.raise_send = raise_send + self.frames: list[RuntimeCommandFrame] = [] + self.hold_targets: list[tuple[str, ...]] = [] + self.hold_fingerprints: list[tuple[object, ...]] = [] + self.operations: list[str] = [] + self.holds = 0 + self.cancels = 0 + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + del timeout + if self.raise_send: + raise RuntimeError("send exploded") + self.operations.append("send") + self.frames.append(command.snapshot()) + if self.reject: + return CommandAcknowledgement.rejected_ack("test rejection") + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[object, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + del context, timeout + self.operations.append("hold") + self.holds += 1 + self.hold_targets.append( + tuple(getattr(target, "target_id") for target in targets) + ) + self.hold_fingerprints.append( + tuple(getattr(target, "address_fingerprint") for target in targets) + ) + if self.reject_hold: + return CommandAcknowledgement.rejected_ack("hold rejected") + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[object, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, timeout + self.operations.append("cancel") + self.cancels += 1 + if self.reject_cancel: + return CommandAcknowledgement.rejected_ack("cancel rejected") + return CommandAcknowledgement.accepted_ack() + + +class _AcceptSafety: + """Accept fake joint commands while recording validation calls.""" + + def __init__(self) -> None: + self.calls = 0 + + def validate( + self, + *, + branch_frames: dict[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + assert branch_frames + assert merged_frame.commands + self.calls += 1 + + +class _RejectSafety: + """Reject every synchronized motion as physically unsafe.""" + + def validate( + self, + *, + branch_frames: dict[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + del branch_frames, merged_frame + raise RuntimeError("predicted self collision") + + +@dataclass(frozen=True, slots=True) +class _ScriptStep: + """One fake lane cycle.""" + + status: SkillStatus + eligible: torch.Tensor + success: torch.Tensor + failure: torch.Tensor + cancelled: torch.Tensor + frame: RuntimeCommandFrame | None = None + task_state: TaskState | None = None + wait_duration: float = 0.0 + emit_hold: bool = False + hold_targets: tuple[JointPositionTarget, ...] = () + + +class _BranchRuntime: + """Small deterministic implementation of the parallel runtime protocol.""" + + def __init__( + self, + script: tuple[_ScriptStep, ...], + sink: ParallelLaneCommandSink, + *, + initial_state: TaskState | None = None, + emit_terminal_hold: bool = True, + ) -> None: + self._script = script + self._sink = sink + self._index = 0 + self._state = initial_state or TaskState.empty(2, "cpu") + self._emit_terminal_hold = emit_terminal_hold + self._result = self._make_result( + SkillStatus.IDLE, + eligible=torch.ones(2, dtype=torch.bool), + ) + + @property + def result(self) -> SkillResult: + return self._result + + @property + def step_count(self) -> int: + return self._index + + def start( + self, + *calls: RegisteredSemanticCall, + workflow_id: str, + eligible_mask: torch.Tensor | None = None, + ) -> SkillResult: + del calls + eligible = ( + torch.ones(2, dtype=torch.bool) + if eligible_mask is None + else eligible_mask.clone() + ) + self._result = self._make_result( + SkillStatus.RUNNING, + workflow_id=workflow_id, + eligible=eligible, + ) + return self._result + + def step(self) -> SkillResult: + scripted = self._script[min(self._index, len(self._script) - 1)] + self._index += 1 + if scripted.frame is not None: + self._sink.send(scripted.frame, timeout=1.0) + self._state = scripted.task_state or self._state + if scripted.hold_targets: + self._sink.hold( + scripted.hold_targets, + _context(self._state), + timeout=1.0, + ) + elif scripted.emit_hold or ( + scripted.status is not SkillStatus.RUNNING and self._emit_terminal_hold + ): + last_frame = scripted.frame or self._sink.last_frame + targets = () if last_frame is None else last_frame.targets + self._sink.hold(targets, _context(self._state), timeout=1.0) + self._result = self._make_result( + scripted.status, + workflow_id=self._result.workflow_id, + eligible=scripted.eligible & ~self._result.cancelled_mask, + success=scripted.success & ~self._result.cancelled_mask, + failure=scripted.failure, + cancelled=self._result.cancelled_mask | scripted.cancelled, + wait_duration=scripted.wait_duration, + ) + return self._result + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + del reason + changed = env_mask & self._result.eligible_mask + self._result = self._make_result( + self._result.status, + workflow_id=self._result.workflow_id, + eligible=self._result.eligible_mask & ~changed, + success=self._result.success_mask & ~changed, + failure=self._result.failure_mask, + cancelled=self._result.cancelled_mask | changed, + wait_duration=self._result.wait_duration, + ) + return self._result + + def cancel(self, reason: str) -> SkillResult: + del reason + active = self._result.eligible_mask & ~self._result.failure_mask + last_frame = self._sink.last_frame + targets = () if last_frame is None else last_frame.targets + self._sink.cancel(targets, timeout=1.0) + self._sink.hold(targets, _context(self._state), timeout=1.0) + self._result = self._make_result( + SkillStatus.CANCELLED, + workflow_id=self._result.workflow_id, + eligible=self._result.eligible_mask & ~active, + failure=self._result.failure_mask, + cancelled=self._result.cancelled_mask | active, + ) + return self._result + + def _make_result( + self, + status: SkillStatus, + *, + workflow_id: str | None = None, + eligible: torch.Tensor | None = None, + success: torch.Tensor | None = None, + failure: torch.Tensor | None = None, + cancelled: torch.Tensor | None = None, + wait_duration: float = 0.0, + ) -> SkillResult: + zeros = torch.zeros(2, dtype=torch.bool) + return SkillResult( + status=status, + workflow_id=workflow_id, + current_call_index=0 if status is SkillStatus.RUNNING else None, + env_ids=ENV_IDS, + success_mask=zeros if success is None else success, + failure_mask=zeros if failure is None else failure, + cancelled_mask=zeros if cancelled is None else cancelled, + eligible_mask=( + torch.ones(2, dtype=torch.bool) if eligible is None else eligible + ), + task_state=self._state, + wait_duration=wait_duration, + ) + + +def _mask(first: bool, second: bool) -> torch.Tensor: + return torch.tensor([first, second], dtype=torch.bool) + + +def _context(task_state: TaskState) -> PlanningContext: + return PlanningContext( + robot=RobotObservation( + timestamp=1.0, + qpos=torch.zeros(2, 3), + qvel=torch.zeros(2, 3), + ), + task=task_state, + scene=SceneSnapshot.empty(), + env_ids=ENV_IDS, + ) + + +def _frame(joint_id: int, values: tuple[float, float]) -> RuntimeCommandFrame: + target = JointPositionTarget(f"resource_{joint_id}", (joint_id,)) + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target, + JointPositionPayload(torch.tensor(values).reshape(2, 1)), + ), + ), + active_mask=_mask(True, True), + env_ids=ENV_IDS, + hold_duration=torch.full((2,), 0.1), + ) + + +def _branch( + branch_id: str, + joint_id: int, + script: tuple[_ScriptStep, ...], + *, + initial_state: TaskState | None = None, + emit_terminal_hold: bool = True, +) -> ParallelRuntimeBranch: + sink = ParallelLaneCommandSink() + return ParallelRuntimeBranch( + branch_id=branch_id, + calls=(RegisteredSemanticCall(f"test.{branch_id}"),), + claim=ResourceClaim(frozenset({f"resource_{joint_id}"}), (joint_id,)), + runtime=_BranchRuntime( + script, + sink, + initial_state=initial_state, + emit_terminal_hold=emit_terminal_hold, + ), + command_sink=sink, + ) + + +def _running_step( + *, + frame: RuntimeCommandFrame | None = None, + eligible: torch.Tensor | None = None, + failure: torch.Tensor | None = None, + task_state: TaskState | None = None, + wait_duration: float = 0.0, + emit_hold: bool = False, + hold_targets: tuple[JointPositionTarget, ...] = (), +) -> _ScriptStep: + return _ScriptStep( + SkillStatus.RUNNING, + _mask(True, True) if eligible is None else eligible, + _mask(False, False), + _mask(False, False) if failure is None else failure, + _mask(False, False), + frame, + task_state, + wait_duration=wait_duration, + emit_hold=emit_hold, + hold_targets=hold_targets, + ) + + +def _completed_step( + *, + frame: RuntimeCommandFrame | None = None, + success: torch.Tensor | None = None, + failure: torch.Tensor | None = None, + task_state: TaskState | None = None, +) -> _ScriptStep: + succeeded = _mask(True, True) if success is None else success + failed = _mask(False, False) if failure is None else failure + return _ScriptStep( + SkillStatus.COMPLETED, + succeeded, + succeeded, + failed, + _mask(False, False), + frame, + task_state, + ) + + +def test_parallel_runtime_merges_one_frame_and_hold_pads_short_lane() -> None: + left_state = TaskState.empty(2, "cpu") + left_state = StateDelta( + articulation_joint_updates={ + ("left_fixture", "joint"): ArticulationJointState(torch.full((2, 1), 0.5)) + } + ).apply(left_state, _mask(True, True)) + right_state = TaskState.empty(2, "cpu") + right_state = StateDelta( + articulation_joint_updates={ + ("right_fixture", "joint"): ArticulationJointState(torch.full((2, 1), 1.0)) + } + ).apply(right_state, _mask(True, True)) + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(task_state=left_state), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _running_step(frame=_frame(1, (3.0, 3.0))), + _completed_step(task_state=right_state), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=8, + ) + + result = runtime.start() + assert result.status is SkillStatus.RUNNING + result = runtime.step() + assert len(outbound.frames) == 1 + assert len(outbound.frames[0].commands) == 2 + assert outbound.operations == ["send"] + assert isinstance(left.runtime, _BranchRuntime) + assert isinstance(right.runtime, _BranchRuntime) + first_lane_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["send"] + assert (left.runtime.step_count, right.runtime.step_count) == first_lane_steps + + clock.time = 0.1 + result = runtime.step() + assert result.status is SkillStatus.RUNNING + assert outbound.operations == ["send", "hold"] + assert len(outbound.frames) == 1 + branch_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["send", "hold"] + assert (left.runtime.step_count, right.runtime.step_count) == branch_steps + + clock.time = 0.2 + result = runtime.step() + assert result.status is SkillStatus.RUNNING + assert outbound.operations == ["send", "hold", "send"] + assert len(outbound.frames[1].commands) == 1 + assert outbound.frames[1].commands[0].target.target_id == "resource_1" + assert (left.runtime.step_count, right.runtime.step_count) == branch_steps + + clock.time = 0.3 + result = runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert result.command_count == 2 + assert outbound.holds == 2 + assert outbound.operations == ["send", "hold", "send", "hold"] + assert ( + result.task_state.get_articulation_joint_state("left_fixture", "joint") + is not None + ) + assert ( + result.task_state.get_articulation_joint_state("right_fixture", "joint") + is not None + ) + metadata = result.to_metadata() + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["kind"] == "parallel_skill_result" + assert list(metadata["branches"]) == ["left", "right"] + assert metadata["elapsed_steps"] == 3 + + +def test_deferred_command_waits_for_clock_after_padding_hold() -> None: + left = _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ) + right = _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + padded = runtime.step() + lane_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + + assert padded.status is SkillStatus.RUNNING + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["hold"] + assert not outbound.frames + assert (left.runtime.step_count, right.runtime.step_count) == lane_steps + + clock.time = 0.1 + runtime.step() + + assert outbound.operations == ["hold", "send"] + assert len(outbound.frames) == 1 + assert (left.runtime.step_count, right.runtime.step_count) == lane_steps + + +def test_completion_hold_waits_for_clock_after_accepted_command() -> None: + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _completed_step(), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + lane_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + + assert same_tick.status is SkillStatus.RUNNING + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["send"] + assert (left.runtime.step_count, right.runtime.step_count) == lane_steps + + clock.time = 0.1 + completed = runtime.step() + + assert completed.status is SkillStatus.COMPLETED + assert outbound.operations == ["send", "hold"] + + +def test_parallel_runtime_fail_fast_is_row_local() -> None: + left = _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + eligible=_mask(False, True), + failure=_mask(True, False), + ), + _completed_step( + success=_mask(False, True), + failure=_mask(True, False), + ), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (3.0, 3.0))), + _completed_step( + success=_mask(False, True), + ), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + first = runtime.step() + + assert torch.equal(first.failure_mask, _mask(True, False)) + assert torch.equal( + first.branch_results["right"].cancelled_mask, + _mask(True, False), + ) + assert torch.equal(outbound.frames[0].active_mask, _mask(False, True)) + + clock.time = 0.1 + result = runtime.step() + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, False)) + assert torch.equal(result.success_mask, _mask(False, True)) + + +def test_parallel_failure_without_fresh_peer_frame_forces_masked_dispatch() -> None: + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _running_step( + eligible=_mask(False, True), + failure=_mask(True, False), + ), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _running_step(wait_duration=0.1), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + assert torch.equal(outbound.frames[-1].active_mask, _mask(True, True)) + + # The failure update has no fresh frame from either lane. The coordinator + # still replays the last transaction with row 0 inactive. + clock.time = 0.1 + runtime.step() + assert len(outbound.frames) == 2 + assert torch.equal(outbound.frames[-1].active_mask, _mask(False, True)) + + +def test_parallel_timeout_counts_completed_environment_steps() -> None: + left = _branch("left", 0, (_running_step(wait_duration=0.1),)) + right = _branch("right", 1, (_running_step(wait_duration=0.1),)) + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + _OutboundSink(), + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=1, + ) + + runtime.start() + before_step = runtime.step() + assert before_step.status is SkillStatus.RUNNING + assert before_step.elapsed_steps == 0 + + clock.time = 0.1 + timed_out = runtime.step() + assert timed_out.status is SkillStatus.FAILED + assert timed_out.elapsed_steps == 1 + assert torch.equal(timed_out.failure_mask, _mask(True, True)) + + +def test_parallel_timeout_does_not_execute_deadline_tick() -> None: + left_runtime_steps = ( + _running_step(frame=_frame(0, (1.0, 1.0)), wait_duration=0.1), + _running_step(frame=_frame(0, (2.0, 2.0))), + ) + right_runtime_steps = ( + _running_step(frame=_frame(1, (3.0, 3.0)), wait_duration=0.1), + _running_step(frame=_frame(1, (4.0, 4.0))), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, left_runtime_steps), + _branch("right", 1, right_runtime_steps), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=1, + ) + + runtime.start() + runtime.step() + assert len(outbound.frames) == 1 + clock.time = 0.1 + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert len(outbound.frames) == 1 + assert outbound.cancels == 1 + assert outbound.holds == 1 + + +def test_parallel_timeout_discards_frame_deferred_behind_completion_hold() -> None: + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ), + _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=1, + ) + + runtime.start() + padded = runtime.step() + assert padded.status is SkillStatus.RUNNING + assert outbound.operations == ["hold"] + assert not outbound.frames + + clock.time = 0.1 + timed_out = runtime.step() + + assert timed_out.status is SkillStatus.FAILED + assert torch.equal(timed_out.failure_mask, _mask(True, True)) + assert not timed_out.success_mask.any() + assert not outbound.frames + assert outbound.operations == ["hold", "cancel", "hold"] + + +def test_parallel_cancel_discards_deferred_frame_and_covers_started_rows() -> None: + outbound = _OutboundSink() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ), + _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + cancelled = runtime.cancel("operator stop during padding") + + assert cancelled.status is SkillStatus.CANCELLED + assert torch.equal(cancelled.cancelled_mask, _mask(True, True)) + assert not cancelled.success_mask.any() + assert not cancelled.failure_mask.any() + assert not outbound.frames + assert outbound.operations == ["hold", "cancel", "hold"] + + +def test_deferred_frame_validation_failure_does_not_advance_lanes() -> None: + outbound = _OutboundSink() + clock = _Clock() + left = _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ) + right = _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ) + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _RejectSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + assert isinstance(left.runtime, _BranchRuntime) + assert isinstance(right.runtime, _BranchRuntime) + steps_before_dispatch = (left.runtime.step_count, right.runtime.step_count) + + clock.time = 0.1 + failed = runtime.step() + + assert failed.status is SkillStatus.FAILED + assert (left.runtime.step_count, right.runtime.step_count) == steps_before_dispatch + assert not outbound.frames + assert outbound.operations == ["hold", "cancel", "hold"] + + +def test_terminal_fresh_frames_fail_closed_without_post_command_observation() -> None: + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + (_completed_step(frame=_frame(0, (1.0, 1.0))),), + ), + _branch( + "right", + 1, + (_completed_step(frame=_frame(1, (2.0, 2.0))),), + ), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.success_mask.any() + assert not outbound.frames + assert outbound.operations == ["cancel", "hold"] + assert "post-command observation" in (result.message or "") + + +def test_hold_aggregation_preserves_same_destination_distinct_fingerprints() -> None: + target_a = JointPositionTarget("shared_arm", (0,)) + target_b = JointPositionTarget("shared_arm", (1,)) + lane_sink = ParallelLaneCommandSink() + lane_sink.hold( + (target_a, target_b), + _context(TaskState.empty(2, "cpu")), + timeout=1.0, + ) + pending_targets, _ = lane_sink.hold_request + assert tuple(target.address_fingerprint for target in pending_targets) == ( + target_a.address_fingerprint, + target_b.address_fingerprint, + ) + + outbound = _OutboundSink() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + (_running_step(hold_targets=(target_a, target_b)),), + ), + _branch("right", 2, (_running_step(wait_duration=0.1),)), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + + assert outbound.hold_targets == [("shared_arm", "shared_arm")] + assert outbound.hold_fingerprints == [ + (target_a.address_fingerprint, target_b.address_fingerprint) + ] + + +def test_parallel_lane_does_not_drop_prior_call_completion_hold() -> None: + left_sink = ParallelLaneCommandSink() + left = ParallelRuntimeBranch( + branch_id="left", + calls=( + RegisteredSemanticCall("test.left_first"), + RegisteredSemanticCall("test.left_second"), + ), + claim=ResourceClaim(frozenset({"left"}), (0, 2)), + runtime=_BranchRuntime( + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + _running_step(frame=_frame(2, (2.0, 2.0))), + _completed_step(), + ), + left_sink, + ), + command_sink=left_sink, + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (3.0, 3.0))), + _running_step(frame=_frame(1, (4.0, 4.0))), + _completed_step(), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + result = runtime.result + for step_index in range(1, 8): + if result.terminal: + break + clock.time = step_index * 0.1 + result = runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert any("resource_0" in targets for targets in outbound.hold_targets) + assert any("resource_2" in targets for targets in outbound.hold_targets) + + +def test_parallel_runtime_rejects_overlapping_claims_before_start() -> None: + script = (_running_step(),) + left = _branch("left", 0, script) + right_sink = ParallelLaneCommandSink() + right = ParallelRuntimeBranch( + branch_id="right", + calls=(RegisteredSemanticCall("test.right"),), + claim=ResourceClaim(frozenset({"different_name"}), (0,)), + runtime=_BranchRuntime(script, right_sink), + command_sink=right_sink, + ) + + with pytest.raises(ValueError, match="overlapping resource claims"): + ParallelSkillRuntime( + (left, right), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + +def test_parallel_runtime_requires_equal_branch_barrier_state() -> None: + changed = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(TaskState.empty(2, "cpu"), _mask(True, True)) + + with pytest.raises(ValueError, match="same verified TaskState"): + ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(),)), + _branch( + "right", + 1, + (_running_step(),), + initial_state=changed, + ), + ), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + +def test_terminal_targets_without_hold_context_fail_closed() -> None: + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ), + _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ), + ), + _OutboundSink(), + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + result = runtime.step() + assert result.status is SkillStatus.RUNNING + clock.time = 0.1 + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert "no synchronized planning context" in (result.message or "") + + +@pytest.mark.parametrize( + ("sink_kwargs", "expected_status"), + [ + ({}, SkillStatus.CANCELLED), + ({"reject_cancel": True}, SkillStatus.FAILED), + ({"reject_hold": True}, SkillStatus.FAILED), + ], +) +def test_parallel_caller_cancel_checks_cancel_and_hold_acknowledgements( + sink_kwargs: dict[str, bool], + expected_status: SkillStatus, +) -> None: + outbound = _OutboundSink(**sink_kwargs) + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + runtime.start() + runtime.step() + + result = runtime.cancel("operator stop") + + assert result.status is expected_status + assert outbound.cancels == 1 + assert outbound.holds >= 1 + if expected_status is SkillStatus.CANCELLED: + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert not result.failure_mask.any() + else: + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.cancelled_mask.any() + + +@pytest.mark.parametrize( + ("safety", "sink"), + [ + (_RejectSafety(), _OutboundSink()), + (_AcceptSafety(), _OutboundSink(raise_send=True)), + ], +) +def test_parallel_tick_exception_safe_stops_with_disjoint_failure_masks( + safety: object, + sink: _OutboundSink, +) -> None: + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + sink, + _Clock(), + ParallelTimingPolicy(0.1), + safety, + timeout_steps=5, + ) + runtime.start() + + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.success_mask.any() + assert not result.cancelled_mask.any() + assert sink.cancels == 1 + assert sink.holds == 1 + + +def test_cancel_preserves_verified_state_from_an_earlier_branch_call() -> None: + changed = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(TaskState.empty(2, "cpu"), _mask(True, True)) + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + task_state=changed, + ), + ), + ), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + runtime.start() + runtime.step() + + result = runtime.cancel() + + assert result.status is SkillStatus.CANCELLED + assert ( + result.task_state.get_articulation_joint_state("fixture", "joint") is not None + ) + + +def test_disjoint_intrinsic_rows_still_conflict_on_same_unpartitioned_key() -> None: + initial = TaskState.empty(2, "cpu") + left_state = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(initial, _mask(True, False)) + right_state = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.full((2, 1), 2.0)) + } + ).apply(initial, _mask(False, True)) + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + (_running_step(frame=_frame(0, (1.0, 1.0)), task_state=left_state),), + ), + _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0)), task_state=right_state),), + ), + ), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + runtime.start() + runtime.step() + + result = runtime.cancel() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.cancelled_mask.any() + + +__all__: list[str] = [] diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index 68087a82d..f7d86c1e8 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -57,9 +57,19 @@ from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.skills import ( AmbiguousSkillBindingError, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CONSTRAINT_EFFECT_CHANNEL, + CONTACT_EFFECT_CHANNEL, ControlPartEndpoint, ControlPartEndpointAdapter, + ControlPartEvidenceAddress, + EffectEvidenceSourceRef, + EffectMonitorRef, EndpointResolution, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + POSE_RELATION_EFFECT_CHANNEL, ProfileValidationError, ResourceBinding, ResourceEndpoint, @@ -477,6 +487,32 @@ def test_endpoint_resolution_owns_runtime_target_snapshot() -> None: assert resolution.runtime_target.aliases == ["base"] +def test_endpoint_resolution_owns_and_freezes_effect_sources() -> None: + source = EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("left_arm", POSE_RELATION_EFFECT_CHANNEL), + ) + sources = {POSE_RELATION_EFFECT_CHANNEL: source} + + resolution = EndpointResolution( + runtime_target=_BaseVelocityTarget("base_controller"), + task_state_key="mobile_actor", + effect_sources=sources, + exclusive=False, + ) + sources.clear() + + assert resolution.task_state_key == "mobile_actor" + assert tuple(resolution.effect_sources) == (POSE_RELATION_EFFECT_CHANNEL,) + assert resolution.effect_sources[POSE_RELATION_EFFECT_CHANNEL] is not source + assert resolution.effect_sources[POSE_RELATION_EFFECT_CHANNEL].address == ( + ControlPartEvidenceAddress("left_arm", POSE_RELATION_EFFECT_CHANNEL) + ) + with pytest.raises(TypeError): + resolution.effect_sources["new"] = source # type: ignore[index] + + @pytest.mark.parametrize("returns_self", [False, True]) def test_endpoint_resolution_rejects_invalid_target_snapshot( returns_self: bool, @@ -1112,6 +1148,28 @@ def test_unique_capability_binding_lowers_to_exact_action_binding() -> None: ) assert motion.require_target(JointPositionTarget).control_part == "left_arm" assert grasp.require_target(JointPositionTarget).control_part == "left_hand" + assert motion.task_state_key == "left_actor" + assert grasp.task_state_key == "left_actor" + resource = resolved.resources["primary"] + motion_sources = resource.endpoints["motion"].effect_sources + grasp_sources = resource.endpoints["grasp"].effect_sources + assert set(motion_sources) == { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } + assert set(grasp_sources) == { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + } + assert motion_sources[POSE_RELATION_EFFECT_CHANNEL].address == ( + ControlPartEvidenceAddress("left_arm", POSE_RELATION_EFFECT_CHANNEL) + ) + assert grasp_sources[CONSTRAINT_EFFECT_CHANNEL].address == ( + ControlPartEvidenceAddress("left_hand", CONSTRAINT_EFFECT_CHANNEL) + ) assert resolved.claim.leaf_resource_ids == frozenset({"left_arm", "left_hand"}) assert resolved.claim.joint_ids == (0, 1, 2) @@ -1350,6 +1408,59 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: incompatible.bind(_engine(control_profiles=_command_profiles())) +def test_policy_preset_defaults_exact_builtin_effect_monitor_refs() -> None: + preset = SkillPolicyPreset("safe") + + assert set(preset.effect_monitors) == { + "pick", + "place", + "hand_over", + "operate_articulation", + } + for monitor_ref in preset.effect_monitors.values(): + assert monitor_ref.monitor_id == COMPOSITE_EFFECT_MONITOR_ID + assert monitor_ref.revision == COMPOSITE_EFFECT_MONITOR_REVISION + assert dict(monitor_ref.params) == {} + + +def test_policy_preset_distinguishes_explicit_empty_effect_monitor_mapping() -> None: + preset = SkillPolicyPreset("unmonitored", effect_monitors={}) + + assert dict(preset.effect_monitors) == {} + assert dict(preset.snapshot().effect_monitors) == {} + + +def test_policy_preset_owns_and_snapshots_effect_monitor_refs() -> None: + source_params = { + "consecutive_samples": 3, + "metadata": ["strict", {"source": "profile"}], + } + source_ref = EffectMonitorRef("test.monitor", "2", source_params) + source_mapping = {"pick": source_ref} + preset = SkillPolicyPreset("custom", effect_monitors=source_mapping) + + source_params["consecutive_samples"] = 99 + source_params["metadata"][1]["source"] = "mutated" # type: ignore[index] + source_mapping["pick"] = EffectMonitorRef("replacement", "1") + first = preset.effect_monitors + snapshot = preset.snapshot() + second = snapshot.effect_monitors + + assert first["pick"] is not source_ref + assert first["pick"].monitor_id == "test.monitor" + assert first["pick"].params["consecutive_samples"] == 3 + assert first["pick"].params["metadata"] == ( + "strict", + {"source": "profile"}, + ) + assert second["pick"] is not first["pick"] + assert second["pick"].params == first["pick"].params + with pytest.raises(TypeError): + first["place"] = source_ref # type: ignore[index] + with pytest.raises(TypeError): + first["pick"].params["consecutive_samples"] = 4 # type: ignore[index] + + def test_profile_owns_named_grounding_provider_selections() -> None: selections = {"hand_over": "dual_center"} profile = RobotSkillProfile( diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py index e90405308..49356f49c 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -14,549 +14,893 @@ # limitations under the License. # ---------------------------------------------------------------------------- +"""Tests for canonical semantic-skill execution and its public facade.""" + from __future__ import annotations +from dataclasses import dataclass, replace +import json +from types import MethodType, SimpleNamespace +from typing import ClassVar from unittest.mock import Mock import pytest import torch +import embodichain.lab.sim.skills.runtime as runtime_module from embodichain.lab.sim.atomic_actions import ( - AntipodalAffordance, - AssembleGoal, + ActionInvocation, + ActionOptions, + ActionPlan, + ArticulationJointState, AtomicAction, AtomicActionEngine, - BATCH_INVERSE_KINEMATICS_CAPABILITY, - CARTESIAN_POSE_CAPABILITY, CommandAcknowledgement, - ControlPartCommandProfile, - EntityState, - ExecutionRunnerCfg, - FORWARD_KINEMATICS_CAPABILITY, - GRASP_CAPABILITY, - GraspGoal, - HeldObjectState, + EffectVerificationRequirement, + EffectVerificationRequest, + EndpointBinding, JointPositionTarget, - PickUp, - PickUpOptions, - Place as AtomicPlace, - PlaceGoal, - PlaceOptions, + MotionPolicy, PlanningContext, + RecoveryPolicy, + ResolvedActionRequest, RobotObservation, - RuntimeCommandFrame, - RuntimeEndpointTarget, + SceneSnapshot, + SkillBindingContract, StateDelta, TaskState, TimedCommandSequence, ) -from embodichain.lab.sim.skills import ( - ControlPartEndpoint, - GRASP_AFFORDANCE_CAPABILITY, - Pick, - Place, - ResourceBinding, - RobotResource, - RobotSkillProfile, - SceneAffordanceRef, - SceneEntityRegistration, - SceneManifest, - SceneObjectRef, - SceneRegistry, - SemanticExecutionStatus, - SemanticEffectVerifier, - SemanticIntegrationManifest, - SemanticPose, - SemanticSkillRuntime, - SemanticTaskStatus, - SkillPolicyPreset, - builtin_semantic_call_catalog, +from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + ControlPartEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectMonitor, + EffectMonitorDecision, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + SemanticEffectKind, + SemanticEffectSpec, ) - -_MOTION_CAPABILITIES = frozenset( - { - BATCH_INVERSE_KINEMATICS_CAPABILITY, - CARTESIAN_POSE_CAPABILITY, - FORWARD_KINEMATICS_CAPABILITY, - } +from embodichain.lab.sim.skills.runtime import ( + AtomicSkills, + SkillEndpointBindingTrace, + SkillRuntime, + SkillStatus, ) +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 - -class _PoseProvider: - def __init__(self, pose: torch.Tensor) -> None: - self.pose = pose - - def observe(self, *, timestamp: float, env_ids: torch.Tensor) -> EntityState: - del timestamp, env_ids - return EntityState(self.pose) +BATCH_SIZE = 2 -class _InstantPick(AtomicAction): - skill_id = PickUp.skill_id - GoalType = GraspGoal - OptionsType = PickUpOptions - binding_contract = PickUp.binding_contract +class _Clock: + """Deterministic execution clock.""" - def _plan(self, request, context): - goal = self.require_goal(request) - endpoint = request.binding.endpoint("primary", "motion") - endpoint.require_target(JointPositionTarget) - held = HeldObjectState( - semantics=goal.semantics, - object_to_eef=torch.eye(4).repeat(context.batch_size, 1, 1), - grasp_xpos=torch.eye(4).repeat(context.batch_size, 1, 1), - ) - return self.build_command_plan( - request, - context, - success=True, - commands=TimedCommandSequence((), context.env_ids), - expected_effects=StateDelta( - held_object_updates={endpoint.task_state_key: held} - ), - ) + def __init__(self) -> None: + self.time = 0.0 + self.sleeps: list[float] = [] + def now(self) -> float: + return self.time -class _InstantPlace(AtomicAction): - skill_id = AtomicPlace.skill_id - GoalType = (PlaceGoal, AssembleGoal) - OptionsType = PlaceOptions - binding_contract = AtomicPlace.binding_contract + def sleep(self, duration: float) -> None: + self.sleeps.append(duration) + self.time += duration - def _plan(self, request, context): - self.require_goal(request) - endpoint = request.binding.endpoint("primary", "motion") - endpoint.require_target(JointPositionTarget) - return self.build_command_plan( - request, - context, - success=True, - commands=TimedCommandSequence((), context.env_ids), - expected_effects=StateDelta( - held_object_updates={endpoint.task_state_key: None} - ), - ) +class _ObservationProvider: + """Return a new timestamped context on every external observation.""" -class _ExecutionPorts: - def __init__(self, registry: SceneRegistry, robot: Mock) -> None: - self.registry = registry - self.robot = robot - self.env_ids = torch.tensor([0, 1], dtype=torch.long) - self.scene_provider = registry.make_scene_provider(batch_size=2) - self.time = 0.0 - self.hold_calls = 0 - self.cancel_calls = 0 + def __init__(self) -> None: + self.calls = 0 + self.task_states: list[TaskState] = [] def observe(self, task_state: TaskState) -> PlanningContext: - qpos = self.robot.get_qpos() + self.calls += 1 + self.task_states.append(task_state) + timestamp = float(self.calls) return PlanningContext( robot=RobotObservation( - timestamp=self.time, - qpos=qpos, - qvel=torch.zeros_like(qpos), + timestamp=timestamp, + qpos=torch.zeros(BATCH_SIZE, 1), + qvel=torch.zeros(BATCH_SIZE, 1), ), task=task_state, - scene=self.scene_provider.snapshot( - timestamp=self.time, - env_ids=self.env_ids, - ), - env_ids=self.env_ids, - control_dt=0.01, + scene=SceneSnapshot(timestamp=timestamp, version=self.calls), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), ) - def send( - self, - command: RuntimeCommandFrame, - *, - timeout: float, - ) -> CommandAcknowledgement: + +class _CommandSink: + """Accept every command while recording safe-stop operations.""" + + def __init__(self) -> None: + self.sent = 0 + self.held = 0 + self.cancelled = 0 + + def send(self, command: object, *, timeout: float) -> CommandAcknowledgement: del command, timeout + self.sent += 1 return CommandAcknowledgement.accepted_ack() def hold( self, - targets: tuple[RuntimeEndpointTarget, ...], + targets: tuple[object, ...], context: PlanningContext, *, timeout: float, ) -> CommandAcknowledgement: del targets, context, timeout - self.hold_calls += 1 + self.held += 1 return CommandAcknowledgement.accepted_ack() def cancel( self, - targets: tuple[RuntimeEndpointTarget, ...], + targets: tuple[object, ...], *, timeout: float, ) -> CommandAcknowledgement: del targets, timeout - self.cancel_calls += 1 + self.cancelled += 1 return CommandAcknowledgement.accepted_ack() - def now(self) -> float: - return self.time - def sleep(self, duration: float) -> None: - self.time += duration +class _Collector: + """Fake acquisition boundary; the test monitor owns decisions.""" + def __init__(self) -> None: + self.calls: list[tuple[int, float, torch.Tensor]] = [] -def _scene_registry() -> SceneRegistry: - cube = SceneObjectRef("cube") - grasp = SceneAffordanceRef("cube_grasp") - return SceneRegistry( - ( - SceneEntityRegistration( - ref=cube, - state_provider=_PoseProvider(torch.eye(4).repeat(2, 1, 1)), - semantic_type="cube", - default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp}, - ), - SceneEntityRegistration( - ref=grasp, - parent=cube, - native_name="grasp", - affordance=AntipodalAffordance(), - affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), - affordance_revision="grasp-v1", - relative_pose=torch.eye(4), + def collect( + self, + spec: SemanticEffectSpec, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> dict[str, EffectEvidenceBatch]: + assert env_ids is not None + self.calls.append((observation_revision, timestamp, env_ids.clone())) + del spec + return {} + + +class _DecisionMonitor(EffectMonitor): + """Return one deterministic row-local physical-effect decision.""" + + def __init__(self, spec: SemanticEffectSpec, decision: EffectMonitorDecision): + self._spec = spec + self._decision = decision + self.calls = 0 + self.requests: list[EffectVerificationRequest] = [] + + @property + def spec(self) -> SemanticEffectSpec: + return self._spec.snapshot() + + def observe( + self, + request: EffectVerificationRequest, + evidence: dict[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + del evidence + self.calls += 1 + self.requests.append(request.snapshot()) + return EffectMonitorDecision( + self._decision.success_mask, + self._decision.failure_mask, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _EffectGoal: + """Test-only goal carrying plan success and symbolic target value.""" + + goal_kind: ClassVar[str] = "runtime_test_effect" + + plan_success: torch.Tensor + target_position: float + + +class _EffectAction(AtomicAction[_EffectGoal, ActionOptions]): + """Zero-frame action with an explicit verified articulation effect.""" + + skill_id: ClassVar[str] = "runtime_test_effect" + GoalType: ClassVar[type] = _EffectGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract() + + def __init__(self) -> None: + super().__init__() + self.plan_count = 0 + + def _scene_dependencies( + self, + request: ResolvedActionRequest[_EffectGoal, ActionOptions], + ) -> tuple[str, ...]: + del request + return ("fixture",) + + def _plan( + self, + request: ResolvedActionRequest[_EffectGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + self.plan_count += 1 + position = torch.full( + (context.batch_size, 1), + goal.target_position, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + return self.build_command_plan( + request, + context, + success=goal.plan_success, + commands=TimedCommandSequence((), context.env_ids), + expected_effects=StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(position) + } ), + effect_verification=EffectVerificationRequirement("semantic_effect"), + replannable=False, + scene_dependency_monitor_until={"fixture": 0}, ) - ) -def _profile( - *, - runner_cfg: ExecutionRunnerCfg | None = None, -) -> RobotSkillProfile: - return RobotSkillProfile( - profile_id="runtime_test_robot", - resources={ - "manipulator": RobotResource( - resource_id="manipulator", - endpoints={ - "motion": ControlPartEndpoint( - control_part="arm", - capabilities=_MOTION_CAPABILITIES, - ), - "grasp": ControlPartEndpoint( - control_part="hand", - capabilities=frozenset({GRASP_CAPABILITY}), - ), - }, - ) - }, - command_profiles={ - "hand": ControlPartCommandProfile.joint_positions( - open=torch.tensor([0.0]), - grasp=torch.tensor([1.0]), +@dataclass(frozen=True, slots=True) +class _Workflow: + workflow_id: str + calls: tuple[RegisteredSemanticCall, ...] + + +@dataclass(frozen=True, slots=True) +class _Grounded: + analyzed: object + invocation: ActionInvocation + effect_spec: SemanticEffectSpec + effect_monitor: EffectMonitor + eligible_mask: torch.Tensor + + +@dataclass(frozen=True, slots=True) +class _Integration: + engine: AtomicActionEngine + scene_registry: SceneRegistry + + +class _Compiler(SemanticSkillCompiler): + """Semantic compiler test double retaining the production call boundaries.""" + + def __init__( + self, + engine: AtomicActionEngine, + decisions: tuple[EffectMonitorDecision, ...], + plan_success: tuple[torch.Tensor, ...], + ) -> None: + self._test_integration = _Integration(engine, SceneRegistry()) + self._decisions = decisions + self._plan_success = plan_success + self.analyze_count = 0 + self.ground_count = 0 + self.ground_timestamps: list[float] = [] + self.ground_task_masks: list[torch.Tensor | None] = [] + self.invocations: list[ActionInvocation] = [] + self.monitors: list[_DecisionMonitor] = [] + + @property + def integration(self) -> _Integration: + return self._test_integration + + def analyze( + self, + calls: tuple[RegisteredSemanticCall, ...], + *, + workflow_id: str = "semantic_workflow", + path: tuple[object, ...] = ("workflow",), + ) -> _Workflow: + del path + self.analyze_count += 1 + 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 + assert eligible_mask is not None + self.ground_count += 1 + self.ground_timestamps.append(context.robot.timestamp) + state = context.task.get_articulation_joint_state("fixture", "joint") + self.ground_task_masks.append(None if state is None else state.env_mask.clone()) + call = workflow.calls[call_index] + invocation = ActionInvocation( + skill_id=_EffectAction.skill_id, + goal=_EffectGoal( + self._plan_success[call_index].clone(), + float(call_index + 1), + ), + binding=self.integration.engine.bind_control_parts( + _EffectAction.skill_id, + {}, + ), + motion_policy=MotionPolicy( + strategy="ik_interp", + sample_count=7, + ), + recovery_policy=RecoveryPolicy( + max_replans=0, + max_action_retries=0, + action_timeout=100.0, + ), + invocation_id=f"{workflow.workflow_id}:{call_index}", + revision=revision, + ) + target = torch.full((BATCH_SIZE, 1), float(call_index + 1)) + expectation = ArticulationJointStateExpectation( + "joint_target", + "fixture", + "joint", + target, + ) + source = EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("virtual", JOINT_STATE_EFFECT_CHANNEL), + ) + spec = SemanticEffectSpec( + semantic_id=call.semantic_id, + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + env_ids=context.env_ids, + state_expectations=(expectation,), + clauses=( + JointStateEffectClause( + "joint_position", + expectation.expectation_id, + source, + target, + ), + ), + ) + monitor = _DecisionMonitor(spec, self._decisions[call_index]) + self.invocations.append(invocation) + self.monitors.append(monitor) + analyzed = SimpleNamespace( + bound=SimpleNamespace( + robot_profile=SimpleNamespace(profile_id="runtime_test_profile"), + binding=SimpleNamespace(action_binding=invocation.binding), + linked=SimpleNamespace( + descriptor=SimpleNamespace(skill_id=invocation.skill_id) + ), + preset=SimpleNamespace( + preset_id="runtime_test_preset", + schema_version=1, + motion_policy=invocation.motion_policy, + recovery_policy=invocation.recovery_policy, + ), ) - }, - defaults={ - "pick_up": ResourceBinding({"primary": "manipulator"}), - "place": ResourceBinding({"primary": "manipulator"}), - }, - presets={"safe": SkillPolicyPreset("safe", runner_cfg=runner_cfg)}, - default_preset="safe", - ) + ) + return _Grounded( + analyzed, + invocation, + spec, + monitor, + eligible_mask.clone(), + ) -def _robot() -> Mock: - robot = Mock() - robot.device = torch.device("cpu") - robot.dof = 2 - robot.control_parts = {"arm": object(), "hand": object()} - robot.get_qpos.return_value = torch.zeros(2, 2) - robot.get_qvel.return_value = torch.zeros(2, 2) - robot.get_joint_ids.side_effect = lambda name: {"arm": [0], "hand": [1]}[name] - robot.get_solver.return_value = object() - return robot +@dataclass(slots=True) +class _System: + runtime: SkillRuntime + compiler: _Compiler + engine: AtomicActionEngine + action: _EffectAction + observation: _ObservationProvider + sink: _CommandSink + collector: _Collector + clock: _Clock + + +def _mask(*values: bool) -> torch.Tensor: + return torch.tensor(values, dtype=torch.bool) + +def _call(name: str) -> RegisteredSemanticCall: + return RegisteredSemanticCall(call_id=f"test.{name}") -def _runtime( + +def _system( + decisions: tuple[EffectMonitorDecision, ...], *, - verifier: SemanticEffectVerifier | None = None, - profile_runner_cfg: ExecutionRunnerCfg | None = None, - runtime_runner_cfg: ExecutionRunnerCfg | None = None, -) -> tuple[SemanticSkillRuntime, _ExecutionPorts]: - registry = _scene_registry() - profile = _profile(runner_cfg=profile_runner_cfg) - robot = _robot() + plan_success: tuple[torch.Tensor, ...] | None = None, +) -> _System: + 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 = "stub_planner" - engine = AtomicActionEngine(generator, skill_profile=profile) - engine.register(_InstantPick(), replace=True) - engine.register(_InstantPlace(), replace=True) - manifest = SemanticIntegrationManifest( - scene=SceneManifest.from_registry(registry), - robot_profile=profile, - call_catalog=builtin_semantic_call_catalog(), + generator.planner.cfg.planner_type = "runtime_test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _EffectAction() + engine.register(action) + selected_plan_success = plan_success or tuple(_mask(True, True) for _ in decisions) + compiler = _Compiler(engine, decisions, selected_plan_success) + observation = _ObservationProvider() + sink = _CommandSink() + collector = _Collector() + clock = _Clock() + runtime = SkillRuntime.from_components( + compiler, + observation, + sink, + collector, + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + clock=clock, ) - ports = _ExecutionPorts(registry, robot) - runtime = SemanticSkillRuntime.bind( - manifest=manifest, - scene_registry=registry, - engine=engine, - observation_provider=ports, - command_sink=ports, - clock=ports, - effect_verifier=verifier, - runner_cfg=runtime_runner_cfg, + return _System( + runtime, + compiler, + engine, + action, + observation, + sink, + collector, + clock, ) - return runtime, ports -def _successful_verifier(call, request, context) -> torch.Tensor: - del call, request - return torch.ones(context.batch_size, dtype=torch.bool) +def test_runtime_analyzes_once_and_uses_one_fresh_session_per_call( + monkeypatch: pytest.MonkeyPatch, +) -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + ) + session_calls = 0 + runner_calls = 0 + original_start = system.engine.start + original_runner = runtime_module.ExecutionRunner + + def counted_start(self: AtomicActionEngine, *args: object, **kwargs: object): + nonlocal session_calls + del self + session_calls += 1 + return original_start(*args, **kwargs) + + system.engine.start = MethodType(counted_start, system.engine) + + def counted_runner(*args: object, **kwargs: object): + nonlocal runner_calls + runner_calls += 1 + return original_runner(*args, **kwargs) + + monkeypatch.setattr(runtime_module, "ExecutionRunner", counted_runner) + result = system.runtime.run((_call("first"), _call("second"))) + + assert result.status is SkillStatus.COMPLETED + assert system.compiler.analyze_count == 1 + assert system.compiler.ground_count == 2 + assert session_calls == 2 + assert runner_calls == 2 + assert system.action.plan_count == 2 + assert len(result.calls) == 2 + assert len(system.collector.calls) == 2 + assert system.compiler.ground_timestamps[1] > system.compiler.ground_timestamps[0] + assert system.observation.calls == 4 + + +def test_runtime_analyzes_downstream_calls_but_executes_only_requested_prefix() -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + ) + calls = (_call("current_segment"), _call("downstream_segment")) + result = system.runtime.run(calls, execution_prefix_length=1) -def _pick_place_calls() -> tuple[Pick, Place]: - cube = SceneObjectRef("cube") - return ( - Pick(object=cube), - Place( - object=cube, - at=SemanticPose((0.4, 0.0, 0.2), (1.0, 0.0, 0.0, 0.0)), - ), - ) + assert result.status is SkillStatus.COMPLETED + assert system.compiler.analyze_count == 1 + assert system.compiler.ground_count == 1 + assert len(system.compiler.invocations) == 1 + assert len(result.calls) == 1 + assert result.calls[0].semantic_id == "test.current_segment" -def test_runtime_runs_jit_grounded_workflow_to_verified_completion() -> None: - observed_calls: list[str] = [] +@pytest.mark.parametrize("prefix_length", (0, 3, True, 1.5)) +def test_runtime_rejects_invalid_execution_prefix_before_analysis( + prefix_length: object, +) -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + ) - def verifier(call, request, context) -> torch.Tensor: - del request - observed_calls.append(call.semantic_id) - return torch.ones(context.batch_size, dtype=torch.bool) + with pytest.raises((TypeError, ValueError), match="execution_prefix_length"): + system.runtime.start( + (_call("first"), _call("second")), + execution_prefix_length=prefix_length, # type: ignore[arg-type] + ) - runtime, ports = _runtime(verifier=verifier) + assert system.compiler.analyze_count == 0 + assert system.observation.calls == 0 - result = runtime.run(_pick_place_calls(), task_id="pick_place") - assert result.status is SemanticTaskStatus.SUCCEEDED - assert result.eligible_mask.tolist() == [True, True] - assert result.task_state.held_objects == {} - assert observed_calls == ["pick", "place"] - assert [record.skill_id for record in result.segments[0].calls] == [ - "pick_up", - "place", - ] - assert ports.hold_calls == 2 - assert runtime.active_task is None +def test_runtime_keeps_partial_rows_at_the_shared_call_barrier() -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, False), _mask(False, True)), + EffectMonitorDecision(_mask(True, False), _mask(False, False)), + ) + ) + result = system.runtime.run(_call("first"), _call("second")) + + 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 torch.equal(result.calls[0].completed_mask, _mask(True, False)) + assert torch.equal(result.calls[0].failed_mask, _mask(False, True)) + assert torch.equal(result.calls[1].entered_mask, _mask(True, False)) + assert torch.equal(system.compiler.ground_task_masks[1], _mask(True, False)) + joint = result.task_state.get_articulation_joint_state("fixture", "joint") + assert joint is not None + assert torch.equal(joint.env_mask, _mask(True, False)) + assert torch.allclose(joint.position[0], torch.tensor([2.0])) + assert len(result.failures) == 1 + + +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")) + + assert result.status is SkillStatus.RUNNING + 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 len(result.effects) == 1 + assert len(result.calls[0].effects) == 1 + assert system.collector.calls[0][0] == 0 + assert torch.equal(system.collector.calls[0][2], torch.tensor([0, 1])) + assert system.compiler.monitors[0].requests[0].verification_id == 0 + + +def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + + result = system.runtime.run(_call("metadata")) + metadata = result.to_metadata() + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["schema_version"] == 1 + assert metadata["kind"] == "skill_result" + call = metadata["calls"][0] + assert call["semantic_id"] == "test.metadata" + assert call["call"]["arguments"]["call_id"] == "test.metadata" + assert call["active_plan_attempt_generation"] == 0 + attempt = call["plan_attempts"][0] + assert attempt["trigger"] == "action_planned" + assert attempt["planned_scene_version"] == 1 + assert attempt["planned_collision_world_revision"] == [0, 0] + assert attempt["scene_dependencies"] == ["fixture"] + assert attempt["scene_dependency_monitor_until"] == {"fixture": 0} + typed_attempt = result.calls[0].plan_attempts[0] + assert typed_attempt.scene_dependency_monitor_until == {"fixture": 0} + assert typed_attempt.snapshot().scene_dependency_monitor_until == {"fixture": 0} + resolved = call["resolved_core_policy"] + assert resolved["profile_id"] == "runtime_test_profile" + assert resolved["preset"] == { + "preset_id": "runtime_test_preset", + "schema_version": 1, + } + assert resolved["motion_policy"]["strategy"] == "ik_interp" + assert resolved["motion_policy"]["sample_count"] == 7 + assert resolved["recovery_policy"]["max_replans"] == 0 + assert resolved["endpoints"] == [] + assert attempt["resolved_core_policy"] == resolved + assert result.calls[0].resolved_core_policy.preset_id == "runtime_test_preset" + effect = call["effects"][0] + assert effect["effect_spec"]["semantic_id"] == "test.metadata" + assert effect["monitor"]["monitor_id"].endswith("._DecisionMonitor") + assert effect["evidence"] == {} + + metadata["masks"]["success"][0] = False + assert system.runtime.result.to_metadata()["masks"]["success"] == [True, True] + + +@pytest.mark.parametrize("waypoint_index", (-1, 1, True, 1.5)) +def test_plan_attempt_trace_rejects_invalid_scene_dependency_monitor_cutoff( + waypoint_index: object, +) -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.run(_call("trace_cutoff")) + attempt = result.calls[0].plan_attempts[0] + + with pytest.raises(ValueError, match="waypoint indices"): + replace( + attempt, + scene_dependency_monitor_until={ + "fixture": waypoint_index # type: ignore[dict-item] + }, + ) -def test_task_preserves_verified_state_across_dynamic_segments() -> None: - runtime, _ = _runtime(verifier=_successful_verifier) - cube = SceneObjectRef("cube") - task = runtime.open_task("dynamic_delivery") +def test_plan_attempt_trace_rejects_monitor_cutoff_for_non_dependency() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.run(_call("trace_dependency")) + attempt = result.calls[0].plan_attempts[0] - pick_result = task.run_segment((Pick(object=cube),), segment_id="acquire") - assert pick_result.status is SemanticExecutionStatus.COMPLETED - assert task.task_state.held_object_mask("manipulator").tolist() == [True, True] + with pytest.raises(ValueError, match="keys must be scene dependencies"): + replace( + attempt, + scene_dependency_monitor_until={"other": 0}, + ) - place_result = task.run_segment( - ( - Place( - object=cube, - at=SemanticPose((0.5, 0.0, 0.2), (1.0, 0.0, 0.0, 0.0)), - ), - ), - segment_id="deliver", - ) - assert place_result.status is SemanticExecutionStatus.COMPLETED - assert task.task_state.held_objects == {} - - result = task.finish() - assert result.status is SemanticTaskStatus.SUCCEEDED - assert [segment.segment_id for segment in result.segments] == [ - "acquire", - "deliver", - ] +def test_endpoint_binding_trace_records_only_stable_binding_choices() -> None: + binding = EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="left_arm", + adapter_id="control_part", + target=JointPositionTarget("left_arm_control", (3, 1)), + task_state_key="left_arm_state", + capabilities=frozenset({"cartesian_pose", "joint_position"}), + claim_tokens=frozenset({"arm_workspace", "left_side"}), + joint_ids=(3, 1), + ) -def test_manual_execution_blocks_until_effect_mask_is_submitted() -> None: - runtime, _ = _runtime() - execution = runtime.start( - (Pick(object=SceneObjectRef("cube")),), - task_id="manual_pick", + trace = SkillEndpointBindingTrace.from_binding(binding) + metadata = trace.to_metadata() + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["resource_id"] == "left_arm" + assert metadata["adapter_id"] == "control_part" + assert metadata["transport_id"] == "robot.joint_position" + assert metadata["target_id"] == "left_arm_control" + assert metadata["capabilities"] == ["cartesian_pose", "joint_position"] + assert metadata["claim_tokens"] == ["arm_workspace", "left_side"] + assert metadata["joint_ids"] == [3, 1] + assert "target" not in metadata + + +def test_preparation_failure_keeps_resolved_policy_without_plan_attempt( + monkeypatch: pytest.MonkeyPatch, +) -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + monkeypatch.setattr( + system.engine, + "start", + Mock(side_effect=RuntimeError("planner unavailable")), ) - blocked = execution.run_until_blocked() - assert blocked.status is SemanticExecutionStatus.WAITING_FOR_EFFECT - assert blocked.pending_effect is not None - assert execution.task_result is None - - completed = blocked - for _ in range(10): - if completed.status is SemanticExecutionStatus.COMPLETED: - break - effect_success = ( - torch.tensor([True, True]) if execution.pending_effect is not None else None - ) - completed = execution.step(effect_success=effect_success) - if ( - completed.runner_step is not None - and completed.runner_step.wait_duration > 0 - ): - runtime.clock.sleep(completed.runner_step.wait_duration) - assert completed.status is SemanticExecutionStatus.COMPLETED - assert execution.task_result is not None - assert execution.task_result.status is SemanticTaskStatus.SUCCEEDED - assert runtime.active_task is None - - -def test_manual_execution_rejects_effect_before_verification_boundary() -> None: - runtime, _ = _runtime() - execution = runtime.start( - (Pick(object=SceneObjectRef("cube")),), - task_id="premature_effect", + result = system.runtime.start(_call("planning_failure")) + metadata = result.to_metadata() + + assert result.status is SkillStatus.FAILED + assert len(result.calls) == 1 + assert result.calls[0].plan_attempts == () + assert result.calls[0].resolved_core_policy.preset_id == "runtime_test_preset" + assert metadata["calls"][0]["active_plan_attempt_generation"] is None + assert ( + metadata["calls"][0]["resolved_core_policy"]["motion_policy"]["strategy"] + == "ik_interp" ) - with pytest.raises(RuntimeError, match="pending effect verification"): - execution.step(effect_success=torch.tensor([True, True])) - execution.cancel() - assert runtime.active_task is None +def test_cancel_inherits_runner_cancel_then_hold_safe_stop() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + system.runtime.start(_call("cancel")) + result = system.runtime.cancel("operator stop") -def test_execution_stages_same_call_revision_through_runner_boundary() -> None: - runtime, _ = _runtime(verifier=_successful_verifier) - cube = SceneObjectRef("cube") - execution = runtime.start((Pick(object=cube),), task_id="revised_pick") + assert result.status is SkillStatus.CANCELLED + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert not result.eligible_mask.any() + assert system.sink.cancelled == 1 + assert system.sink.held == 1 + assert result.calls[0].status.value == "cancelled" - execution.revise_current(Pick(object=cube)) - completed = execution.run_until_blocked( - effect_verifier=_successful_verifier, + +def test_facade_varargs_and_programmatic_iterable_share_runtime_path() -> None: + decisions = ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), ) + iterable_system = _system(decisions) + facade_system = _system(decisions) + calls = (_call("first"), _call("second")) - assert completed.status is SemanticExecutionStatus.COMPLETED - assert execution.task_result is not None - assert execution.task_result.segments[0].calls[0].invocation_revision == 1 + iterable_result = iterable_system.runtime.run(calls) + facade_result = AtomicSkills(facade_system.runtime).run(*calls) + assert iterable_result.status is facade_result.status + assert torch.equal(iterable_result.success_mask, facade_result.success_mask) + assert [trace.skill_id for trace in iterable_result.calls] == [ + trace.skill_id for trace in facade_result.calls + ] + assert iterable_system.compiler.analyze_count == 1 + assert facade_system.compiler.analyze_count == 1 + assert [item.skill_id for item in iterable_system.compiler.invocations] == [ + item.skill_id for item in facade_system.compiler.invocations + ] -def test_effect_failures_produce_partial_task_success_after_bounded_retries() -> None: - runtime, _ = _runtime( - verifier=lambda call, request, context: torch.tensor([True, False]) - ) - result = runtime.run( - (Pick(object=SceneObjectRef("cube")),), - task_id="partial_pick", - ) +def test_from_env_requires_an_explicit_runtime_provider() -> None: + class AttributeBag: + compiler = object() + robot = object() + scene = object() - assert result.status is SemanticTaskStatus.PARTIAL_SUCCESS - assert result.eligible_mask.tolist() == [True, False] - assert result.task_state.held_object_mask("manipulator").tolist() == [True, False] + with pytest.raises(TypeError, match="no semantic-skill integration adapter"): + AtomicSkills.from_env(AttributeBag()) -def test_runtime_rejects_concurrent_tasks() -> None: - runtime, _ = _runtime() - task = runtime.open_task("first") +def test_from_env_delegates_preset_to_installed_provider() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) - with pytest.raises(RuntimeError, match="already owns this runtime"): - runtime.open_task("second") + class Provider: + def __init__(self) -> None: + self.presets: list[str] = [] - result = task.cancel() - assert result.status is SemanticTaskStatus.CANCELLED - assert runtime.active_task is None + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + self.presets.append(preset) + return system.runtime + provider = Provider() + skills = AtomicSkills.from_env(provider, preset="precise") -def test_blocking_run_requires_effect_verifier_before_owning_runtime() -> None: - runtime, _ = _runtime() + assert skills.runtime is system.runtime + assert provider.presets == ["precise"] - with pytest.raises(ValueError, match="requires an effect_verifier"): - runtime.run((Pick(object=SceneObjectRef("cube")),)) - assert runtime.active_task is None +def test_result_snapshots_do_not_expose_runtime_masks() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.run(_call("owned")) + result.success_mask.zero_() + result.calls[0].completed_mask.zero_() + fresh = system.runtime.result -def test_verifier_exception_fails_safely_and_releases_runtime() -> None: - def failing_verifier(call, request, context) -> torch.Tensor: - del call, request, context - raise RuntimeError("camera unavailable") + assert torch.equal(fresh.success_mask, _mask(True, True)) + assert torch.equal(fresh.calls[0].completed_mask, _mask(True, True)) - runtime, ports = _runtime(verifier=failing_verifier) - result = runtime.run( - (Pick(object=SceneObjectRef("cube")),), - task_id="failed_verification", - ) +def test_fork_creates_an_independent_lane_on_the_shared_clock() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + lane_sink = _CommandSink() - assert result.status is SemanticTaskStatus.FAILED - assert result.segments[0].status is SemanticExecutionStatus.FAILED - assert "camera unavailable" in (result.message or "") - assert ports.cancel_calls == 1 - assert runtime.active_task is None + lane = system.runtime.fork(lane_sink) + assert lane is not system.runtime + assert lane.compiler is system.runtime.compiler + assert lane.clock is system.runtime.clock + assert lane.status is SkillStatus.IDLE + assert lane_sink.sent == 0 -def test_failed_dynamic_segment_closes_terminal_task_ownership() -> None: - runtime, _ = _runtime( - verifier=lambda call, request, context: torch.zeros( - context.batch_size, - dtype=torch.bool, - ) - ) - task = runtime.open_task("terminal_dynamic_failure") - segment = task.run_segment((Pick(object=SceneObjectRef("cube")),)) +def test_runner_failure_does_not_relabel_peer_cancelled_rows() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + system.runtime.start(_call("row_failure")) + system.runtime.deactivate_rows(_mask(True, False), reason="peer branch failed") - assert segment.status is SemanticExecutionStatus.FAILED - assert task.result is not None - assert task.result.status is SemanticTaskStatus.FAILED - assert runtime.active_task is None + def fail_observation(task_state: TaskState) -> PlanningContext: + del task_state + raise RuntimeError("observation unavailable") + system.observation.observe = fail_observation + result = system.runtime.step() + if result.wait_duration > 0.0: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() -def test_from_simulation_builds_ports_and_filters_agent_visible_calls() -> None: - registry = _scene_registry() - profile = _profile() - robot = _robot() - simulation = Mock() - simulation.sim_config.physics_dt = 0.01 - generator = Mock() - generator.robot = robot - generator.device = torch.device("cpu") - generator.planner.cfg.planner_type = "stub_planner" - generator.collision_world_info = None - - runtime = SemanticSkillRuntime.from_simulation( - simulation=simulation, - robot=robot, - motion_generator=generator, - scene_registry=registry, - robot_profile=profile, - control_dt=0.04, - ) + assert result.status is SkillStatus.FAILED + assert torch.equal(result.cancelled_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) - assert set(runtime.available_calls) == {"pick", "place"} - assert runtime.observation_provider is runtime.command_sink - assert runtime.clock is runtime.observation_provider - assert runtime.observation_provider.control_dt == pytest.approx(0.04) +def test_deactivate_all_rows_safe_stops_immediately_before_due_time() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + system.runtime.start(_call("deactivate_all")) -def test_runtime_uses_skill_preset_runner_cfg_without_global_override() -> None: - runtime, ports = _runtime( - verifier=_successful_verifier, - profile_runner_cfg=ExecutionRunnerCfg(hold_on_completion=False), + result = system.runtime.deactivate_rows( + _mask(True, True), + reason="parallel peer failed", ) - result = runtime.run((Pick(object=SceneObjectRef("cube")),)) + assert result.status is SkillStatus.CANCELLED + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert system.sink.cancelled == 1 + assert system.sink.held == 1 - assert result.status is SemanticTaskStatus.SUCCEEDED - assert ports.hold_calls == 0 +def test_parallel_factory_analyzes_claims_and_forks_owned_shared_clock_lanes() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) -def test_runtime_runner_cfg_overrides_skill_preset() -> None: - runtime, ports = _runtime( - verifier=_successful_verifier, - profile_runner_cfg=ExecutionRunnerCfg(hold_on_completion=False), - runtime_runner_cfg=ExecutionRunnerCfg(hold_on_completion=True), + def analyze_claims( + self: _Compiler, + calls: tuple[RegisteredSemanticCall, ...], + *, + workflow_id: str, + path: tuple[object, ...] = ("workflow",), + ) -> object: + del self, workflow_id, path + analyzed = [] + for call_index, call in enumerate(calls): + joint_id = 0 if call.call_id.endswith("left") else 1 + analyzed.append( + SimpleNamespace( + index=call_index, + symbolic_writes=frozenset(), + opaque_symbolic_effect=False, + bound=SimpleNamespace( + binding=SimpleNamespace( + claim=ResourceClaim( + frozenset({f"resource_{joint_id}"}), + (joint_id,), + ) + ) + ), + ) + ) + return SimpleNamespace(calls=tuple(analyzed)) + + class AcceptSafety: + def validate(self, *, branch_frames: object, merged_frame: object) -> None: + del branch_frames, merged_frame + + system.compiler.analyze = MethodType(analyze_claims, system.compiler) + parallel = ParallelSkillRuntime.from_template( + system.runtime, + { + "left": (_call("left"),), + "right": (_call("right"),), + }, + system.sink, + ParallelTimingPolicy(0.1), + AcceptSafety(), + timeout_steps=5, ) - result = runtime.run((Pick(object=SceneObjectRef("cube")),)) - - assert result.status is SemanticTaskStatus.SUCCEEDED - assert ports.hold_calls == 1 + assert parallel.clock is system.runtime.clock + assert parallel.branch_claims["left"].joint_ids == (0,) + assert parallel.branch_claims["right"].joint_ids == (1,) + + changed = StateDelta( + articulation_joint_updates={ + ("template", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(system.runtime.task_state, _mask(True, True)) + system.runtime.adopt_verified_task_state(changed) + assert all( + result.task_state.get_articulation_joint_state("template", "joint") is None + for result in parallel.result.branch_results.values() + ) diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py index 903cb3ef1..fc14262a6 100644 --- a/tests/sim/skills/test_scene.py +++ b/tests/sim/skills/test_scene.py @@ -28,6 +28,7 @@ Affordance, AntipodalAffordance, EntityState, + ObservedArticulationJointState, SceneSnapshot, ) from embodichain.lab.sim.planners.base_planner import CollisionWorldInfo @@ -91,6 +92,24 @@ def observe( return EntityState(self.pose) +class _MutableJointProvider: + """Expose one mutable canonical articulation joint observation.""" + + def __init__(self, position: torch.Tensor) -> None: + self.position = position + self.calls = 0 + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, ObservedArticulationJointState]: + del timestamp, env_ids + self.calls += 1 + return {"slide": ObservedArticulationJointState(self.position)} + + class _MotionGenerator: """Minimal dynamic-collision integration surface.""" @@ -129,13 +148,27 @@ def snapshot( class _SimulationEntity: """Simulation entity pose source used by the opt-in adapter tests.""" - def __init__(self, pose: torch.Tensor) -> None: + def __init__( + self, + pose: torch.Tensor, + *, + qpos: torch.Tensor | None = None, + joint_names: tuple[str, ...] = (), + ) -> None: self.pose = pose + self.qpos = qpos + self.joint_names = joint_names def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: assert to_matrix is True return self.pose + def get_qpos(self, *, target: bool) -> torch.Tensor: + assert target is False + if self.qpos is None: + raise RuntimeError("This simulation fixture has no articulation qpos.") + return self.qpos + class _Simulation: """Minimal simulation lookup surface with selected and unselected assets.""" @@ -146,7 +179,11 @@ def __init__(self) -> None: "ignored": _SimulationEntity(torch.eye(4) * 2.0), } self.articulations = { - "sim_drawer": _SimulationEntity(torch.eye(4)), + "sim_drawer": _SimulationEntity( + torch.eye(4), + qpos=torch.tensor([[0.25]]), + joint_names=("slide",), + ), } def get_rigid_object(self, uid: str) -> _SimulationEntity | None: @@ -222,6 +259,47 @@ def test_root_registration_requires_explicit_state_provider() -> None: SceneEntityRegistration(ref=SceneObjectRef("cube")) +def test_joint_state_provider_is_owned_by_articulation_registration() -> None: + joint_provider = _MutableJointProvider(torch.tensor([[0.1], [0.2]])) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=_StateProvider(), + joint_state_provider=joint_provider, + ), + ) + ) + provider = registry.make_scene_provider() + env_ids = torch.tensor([0, 1], dtype=torch.long) + + first = provider.snapshot(timestamp=0.0, env_ids=env_ids) + returned = first.articulation_joints[("drawer", "slide")] + returned.position.zero_() + assert torch.equal( + first.articulation_joints[("drawer", "slide")].position, + torch.tensor([[0.1], [0.2]]), + ) + + joint_provider.position[:, 0] = torch.tensor([0.3, 0.4]) + second = provider.snapshot(timestamp=1.0, env_ids=env_ids) + assert second.version == first.version + 1 + assert torch.equal( + second.articulation_joints[("drawer", "slide")].position, + torch.tensor([[0.3], [0.4]]), + ) + assert joint_provider.calls == 2 + + +def test_joint_state_provider_rejects_non_articulation_registration() -> None: + with pytest.raises(ValueError, match="SceneArticulationRef"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + joint_state_provider=_MutableJointProvider(torch.tensor([0.0])), + ) + + def test_link_registration_requires_parent_and_native_name() -> None: with pytest.raises(ValueError, match="parent and native_name"): SceneEntityRegistration( @@ -1027,6 +1105,35 @@ def test_from_simulation_is_explicit_and_uses_uid_only_as_alias() -> None: assert "ignored" not in snapshot.entities +def test_from_simulation_does_not_register_canonical_uid_as_alias() -> None: + simulation = _Simulation() + simulation.rigid_objects["cube"] = simulation.rigid_objects["sim_cube"] + + registry = SceneRegistry.from_simulation( + simulation, # type: ignore[arg-type] + rigid_objects={"cube": "cube"}, + ) + + assert registry.resolve("cube") == SceneObjectRef("cube") + assert registry.aliases == {} + + +def test_from_simulation_publishes_named_articulation_qpos() -> None: + registry = SceneRegistry.from_simulation( + _Simulation(), # type: ignore[arg-type] + articulations={"drawer": "sim_drawer"}, + ) + + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + state = snapshot.articulation_joints[("drawer", "slide")] + assert torch.equal(state.position, torch.tensor([[0.25]])) + assert state.valid_mask is not None and state.valid_mask.tolist() == [True] + + def test_from_simulation_derives_live_geometry_only_for_explicit_collision_role() -> ( None ): diff --git a/tests/sim/skills/test_semantic_skill_tutorials.py b/tests/sim/skills/test_semantic_skill_tutorials.py deleted file mode 100644 index 5a6bbf5cc..000000000 --- a/tests/sim/skills/test_semantic_skill_tutorials.py +++ /dev/null @@ -1,480 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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 semantic-skill tutorial declarations.""" - -from __future__ import annotations - -from typing import cast, TYPE_CHECKING -from unittest.mock import Mock - -import pytest -import torch - -from embodichain.lab.sim import SimulationManager -from embodichain.lab.sim.atomic_actions import ( - AntipodalAffordance, - EffectVerificationRequest, - GraspGoal, - HandOverOptions, - PlanningContext, -) -from embodichain.lab.sim.skills import ( - GRASP_AFFORDANCE_CAPABILITY, - Pick, - Place, - RegisteredSemanticCall, - SceneRegistry, -) -import scripts.tutorials.semantic_skill.hand_over as handover_tutorial -import scripts.tutorials.semantic_skill.place as place_tutorial -from scripts.tutorials.semantic_skill.hand_over import ( - FINAL_OBJECT_POSITION, - HANDOVER_CALL_ID, - MIDDLE_OBJECT_POSITION, - TRACKING_ERROR_THRESHOLD as HANDOVER_TRACKING_ERROR_THRESHOLD, - TutorialHandOverLowerer, - create_handover_effect_verifier, - create_handover_task, - create_robot_profile as create_dual_arm_profile, -) -from scripts.tutorials.semantic_skill.place import ( - MINIMUM_PICK_LIFT, - TARGET_OBJECT_POSITION, - TRACKING_ERROR_THRESHOLD as PLACE_TRACKING_ERROR_THRESHOLD, - create_place_effect_verifier, - create_place_task, - create_robot_profile as create_single_arm_profile, -) -from scripts.tutorials.semantic_skill.tutorial_utils import ( - create_graspable_object_registry, - create_runtime_step_observer, -) - -if TYPE_CHECKING: - from embodichain.lab.sim.objects import RigidObject, Robot - from embodichain.lab.sim.skills.integration import BoundSemanticCall - -_TEST_PHYSICS_DT = 0.01 -_TEST_GRASP_SAMPLE_COUNT = 8 - - -class _PhysicalObject: - """Small mutable pose source used by verifier tests.""" - - def __init__(self, pose: torch.Tensor) -> None: - self.pose = pose - self.clear_count = 0 - - def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: - assert to_matrix is True - return self.pose.clone() - - def clear_dynamics(self) -> None: - self.clear_count += 1 - - -class _PhysicalRobot: - """Expose only the joint and FK observations used by tutorial verifiers.""" - - def __init__(self) -> None: - self.qpos: dict[str, torch.Tensor] = {} - self.eef_pose: dict[str, torch.Tensor] = {} - - def get_qpos(self, name: str) -> torch.Tensor: - return self.qpos[name].clone() - - def compute_fk( - self, - *, - qpos: torch.Tensor, - name: str, - to_matrix: bool, - ) -> torch.Tensor: - del qpos - assert to_matrix is True - return self.eef_pose[name].clone() - - -def _pose_at(position: tuple[float, float, float]) -> torch.Tensor: - pose = torch.eye(4).unsqueeze(0) - pose[:, :3, 3] = torch.tensor(position) - return pose - - -def _request( - skill_id: str, - *, - held_control_part: str | None = None, -) -> EffectVerificationRequest: - request = Mock() - request.skill_id = skill_id - held = Mock() - held.object_to_eef = torch.eye(4).unsqueeze(0) - request.expected_effects.held_object_updates = ( - {} if held_control_part is None else {held_control_part: held} - ) - return cast(EffectVerificationRequest, request) - - -def _verification_context() -> PlanningContext: - context = Mock() - context.robot.qpos = torch.zeros(1, 1) - return cast(PlanningContext, context) - - -def _graspable_registry() -> SceneRegistry: - entity = Mock() - entity.get_local_pose.return_value = torch.eye(4) - simulation = Mock() - simulation.get_rigid_object.return_value = entity - - registry, _ = create_graspable_object_registry( - cast(SimulationManager, simulation), - object_id="workpiece", - simulation_uid="sim_cube", - semantic_type="cube", - affordance=AntipodalAffordance(), - ) - return registry - - -def test_graspable_registry_maps_simulation_identity_to_semantic_identity() -> None: - registry = _graspable_registry() - object_ref = registry.resolve("workpiece") - - assert registry.resolve("sim_cube") == object_ref - grasp_ref = registry.resolve_affordance( - object_ref, - capability=GRASP_AFFORDANCE_CAPABILITY, - ) - semantics = registry.object_semantics(object_ref, affordance=grasp_ref) - assert semantics.entity_id == "workpiece" - assert type(semantics.affordance) is AntipodalAffordance - - -def test_place_tutorial_task_contains_no_robot_resource_names() -> None: - calls = create_place_task() - - assert tuple(type(call) for call in calls) == (Pick, Place) - assert calls[0].object == calls[1].object - assert dict(calls[0].resources) == {} - assert dict(calls[1].resources) == {} - assert calls[1].at is not None - torch.testing.assert_close( - calls[1].at.position, - torch.tensor(TARGET_OBJECT_POSITION), - ) - - -def test_place_tutorial_profile_owns_single_arm_binding_and_policies() -> None: - profile = create_single_arm_profile( - torch.tensor([0.0, 0.0]), - torch.tensor([0.5, 0.5]), - ) - - resource = profile.resources["primary_manipulator"] - assert resource.endpoints["motion"].control_part == "arm" - assert resource.endpoints["grasp"].control_part == "hand" - assert dict(profile.defaults["pick_up"].resources) == { - "primary": "primary_manipulator" - } - assert dict(profile.defaults["place"].resources) == { - "primary": "primary_manipulator" - } - assert dict(profile.skill_presets) == {"pick_up": "pick", "place": "place"} - assert ( - profile.presets["pick"].recovery_policy.tracking_error_threshold - == PLACE_TRACKING_ERROR_THRESHOLD - ) - assert profile.presets["place"].recovery_policy.max_action_retries == 0 - assert ( - profile.presets["place"].recovery_policy.tracking_error_threshold - == PLACE_TRACKING_ERROR_THRESHOLD - ) - - -def test_place_application_installs_default_effect_verifier( - monkeypatch: pytest.MonkeyPatch, -) -> None: - simulation = Mock() - simulation.sim_config.physics_dt = _TEST_PHYSICS_DT - simulation.get_rigid_object.return_value = Mock() - robot = Mock() - obj = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) - semantics = Mock(affordance=AntipodalAffordance()) - motion_generator = Mock() - runtime = Mock() - runtime_factory = Mock(return_value=runtime) - - monkeypatch.setattr( - place_tutorial, - "create_antipodal_semantics", - Mock(return_value=semantics), - ) - monkeypatch.setattr( - place_tutorial, - "create_curobo_motion_generator", - Mock(return_value=motion_generator), - ) - monkeypatch.setattr( - place_tutorial.SemanticSkillRuntime, - "from_simulation", - runtime_factory, - ) - - result = place_tutorial.create_place_application( - cast(SimulationManager, simulation), - cast("Robot", robot), - cast("RigidObject", obj), - hand_open=torch.zeros(1), - hand_grasp=torch.ones(1), - n_sample=_TEST_GRASP_SAMPLE_COUNT, - force_reannotate=False, - ) - - assert result is runtime - assert type(runtime_factory.call_args.kwargs["scene_registry"]) is SceneRegistry - assert ( - runtime_factory.call_args.kwargs["robot_profile"].profile_id - == "tutorial.single_arm" - ) - assert runtime_factory.call_args.kwargs["motion_generator"] is motion_generator - assert callable(runtime_factory.call_args.kwargs["effect_verifier"]) - - -def test_place_tutorial_verifies_observed_pick_lift_and_eef_proximity() -> None: - physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) - physical_robot = _PhysicalRobot() - physical_robot.qpos["arm"] = torch.zeros(1, 1) - physical_robot.qpos["hand"] = torch.zeros(1, 1) - verifier = create_place_effect_verifier( - cast("RigidObject", physical_object), - cast("Robot", physical_robot), - torch.zeros(1), - ) - lifted_pose = _pose_at((0.0, 0.0, MINIMUM_PICK_LIFT + 0.01)) - physical_object.pose = lifted_pose - physical_robot.eef_pose["arm"] = lifted_pose - - success = verifier( - create_place_task()[0], - _request("pick_up", held_control_part="arm"), - _verification_context(), - ) - - assert success.tolist() == [True] - - -def test_place_tutorial_rejects_lift_with_wrong_grasp_relation() -> None: - physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) - physical_robot = _PhysicalRobot() - physical_robot.qpos["arm"] = torch.zeros(1, 1) - physical_robot.qpos["hand"] = torch.zeros(1, 1) - verifier = create_place_effect_verifier( - cast("RigidObject", physical_object), - cast("Robot", physical_robot), - torch.zeros(1), - ) - physical_object.pose = _pose_at((0.0, 0.0, MINIMUM_PICK_LIFT + 0.01)) - physical_robot.eef_pose["arm"] = _pose_at((0.2, 0.0, MINIMUM_PICK_LIFT + 0.01)) - - success = verifier( - create_place_task()[0], - _request("pick_up", held_control_part="arm"), - _verification_context(), - ) - - assert success.tolist() == [False] - - -def test_place_tutorial_verifies_release_at_requested_position() -> None: - physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) - physical_robot = _PhysicalRobot() - hand_open = torch.tensor([0.0, 0.0]) - physical_robot.qpos["hand"] = hand_open.unsqueeze(0) - verifier = create_place_effect_verifier( - cast("RigidObject", physical_object), - cast("Robot", physical_robot), - hand_open, - ) - physical_object.pose = _pose_at(TARGET_OBJECT_POSITION) - - success = verifier( - create_place_task()[1], - _request("place"), - _verification_context(), - ) - - assert success.tolist() == [True] - - -def test_handover_tutorial_registers_tuned_atomic_lowering() -> None: - calls = create_handover_task() - context = Mock() - context.robot.qpos = torch.zeros(1, 1) - lowerer = TutorialHandOverLowerer(_graspable_registry()) - - assert tuple(type(call) for call in calls) == (Pick, RegisteredSemanticCall) - assert calls[1].call_id == HANDOVER_CALL_ID - assert dict(calls[0].resources) == {} - assert calls[1].arguments["object"] == calls[0].object - lowering = lowerer.lower( - calls[1], - context=cast(PlanningContext, context), - bound=cast("BoundSemanticCall", object()), - ) - assert type(lowering.goal) is GraspGoal - assert type(lowering.skill_options) is HandOverOptions - assert lowering.goal.semantics.entity_id == "workpiece" - torch.testing.assert_close( - lowering.skill_options.middle_object_pose[:3, 3], - torch.tensor(MIDDLE_OBJECT_POSITION), - ) - torch.testing.assert_close( - lowering.skill_options.final_object_pose[:3, 3], - torch.tensor(FINAL_OBJECT_POSITION), - ) - - -def test_handover_tutorial_profile_binds_disjoint_arms() -> None: - profile = create_dual_arm_profile( - torch.tensor([0.0]), - torch.tensor([0.5]), - torch.tensor([0.0]), - torch.tensor([0.5]), - ) - - assert profile.resources["left"].endpoints["motion"].control_part == "left_arm" - assert profile.resources["right"].endpoints["motion"].control_part == "right_arm" - assert dict(profile.defaults["pick_up"].resources) == {"primary": "left"} - assert dict(profile.defaults["hand_over"].resources) == { - "source": "left", - "destination": "right", - } - assert ( - profile.presets["pick"].recovery_policy.tracking_error_threshold - == HANDOVER_TRACKING_ERROR_THRESHOLD - ) - assert profile.presets["hand_over"].recovery_policy.max_action_retries == 0 - assert ( - profile.presets["hand_over"].recovery_policy.tracking_error_threshold - == HANDOVER_TRACKING_ERROR_THRESHOLD - ) - - -def test_handover_application_installs_extension_and_default_verifier( - monkeypatch: pytest.MonkeyPatch, -) -> None: - simulation = Mock() - simulation.sim_config.physics_dt = _TEST_PHYSICS_DT - simulation.get_rigid_object.return_value = Mock() - robot = Mock() - obj = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) - semantics = Mock(affordance=AntipodalAffordance()) - motion_generator = Mock() - runtime = Mock() - runtime_factory = Mock(return_value=runtime) - - monkeypatch.setattr( - handover_tutorial, - "create_antipodal_semantics", - Mock(return_value=semantics), - ) - monkeypatch.setattr( - handover_tutorial, - "create_toppra_motion_generator", - Mock(return_value=motion_generator), - ) - monkeypatch.setattr( - handover_tutorial.SemanticSkillRuntime, - "from_simulation", - runtime_factory, - ) - - result = handover_tutorial.create_handover_application( - cast(SimulationManager, simulation), - cast("Robot", robot), - cast("RigidObject", obj), - left_open=torch.zeros(1), - left_grasp=torch.ones(1), - right_open=torch.zeros(1), - right_grasp=torch.ones(1), - n_sample=_TEST_GRASP_SAMPLE_COUNT, - force_reannotate=False, - ) - - assert result is runtime - assert type(runtime_factory.call_args.kwargs["scene_registry"]) is SceneRegistry - assert ( - runtime_factory.call_args.kwargs["robot_profile"].profile_id - == "tutorial.dual_arm" - ) - assert runtime_factory.call_args.kwargs["motion_generator"] is motion_generator - assert ( - HANDOVER_CALL_ID in runtime_factory.call_args.kwargs["call_catalog"].descriptors - ) - assert callable(runtime_factory.call_args.kwargs["effect_verifier"]) - assert type(runtime_factory.call_args.kwargs["registered_lowerers"][0]) is ( - TutorialHandOverLowerer - ) - - -def test_handover_tutorial_verifies_receiver_ownership_at_final_target() -> None: - physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) - physical_robot = _PhysicalRobot() - left_open = torch.tensor([0.0]) - right_grasp = torch.tensor([0.5]) - physical_robot.qpos["left_hand"] = left_open.unsqueeze(0) - physical_robot.qpos["right_hand"] = right_grasp.unsqueeze(0) - verifier = create_handover_effect_verifier( - cast("RigidObject", physical_object), - cast("Robot", physical_robot), - left_open=left_open, - right_grasp=right_grasp, - ) - final_pose = _pose_at(FINAL_OBJECT_POSITION) - physical_object.pose = final_pose - physical_robot.qpos["right_arm"] = torch.zeros(1, 1) - physical_robot.eef_pose["right_arm"] = final_pose - - success = verifier( - create_handover_task()[1], - _request("hand_over", held_control_part="right_arm"), - _verification_context(), - ) - - assert success.tolist() == [True] - - -def test_runtime_step_observer_stabilizes_initial_grasp_once() -> None: - physical_object = _PhysicalObject(_pose_at((0.0, 0.0, 0.0))) - physical_robot = _PhysicalRobot() - physical_robot.qpos["hand"] = torch.zeros(1, 1) - observer = create_runtime_step_observer( - cast("RigidObject", physical_object), - cast("Robot", physical_robot), - grasp_control_part="hand", - grasp_target=torch.ones(1), - ) - runner_step = Mock(tick=None) - - observer(runner_step) - physical_robot.qpos["hand"] = torch.ones(1, 1) - observer(runner_step) - observer(runner_step) - - assert physical_object.clear_count == 1 From 50c1395a709251808d283f796fecaa16c4c744ef Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:19:15 +0800 Subject: [PATCH 07/29] feat(gym): add declarative expert program runtime --- ...mbodichain.lab.gym.envs.expert_program.rst | 191 ++ .../embodichain/embodichain.lab.gym.envs.rst | 9 + .../embodichain/embodichain.utils.rst | 7 + docs/source/api_reference/public_api.rst | 211 ++ .../sim/atomic_actions/expert_programs.md | 255 ++ embodichain/lab/gym/envs/__init__.py | 1 + embodichain/lab/gym/envs/demo.py | 304 ++- embodichain/lab/gym/envs/embodied_env.py | 129 +- .../lab/gym/envs/expert_program/__init__.py | 257 +++ .../lab/gym/envs/expert_program/bridge.py | 1574 +++++++++++++ .../lab/gym/envs/expert_program/cfg.py | 857 +++++++ .../lab/gym/envs/expert_program/compiler.py | 1912 +++++++++++++++ .../lab/gym/envs/expert_program/decoder.py | 1361 +++++++++++ .../gym/envs/expert_program/environment.py | 843 +++++++ .../lab/gym/envs/expert_program/loader.py | 337 +++ .../lab/gym/envs/expert_program/simulation.py | 1240 ++++++++++ .../expert_program/simulation_environment.py | 1202 ++++++++++ .../expert_program/simulation_policies.py | 715 ++++++ .../_event_functors/dynamic_settling.py | 109 +- embodichain/lab/gym/envs/settling.py | 374 +++ embodichain/lab/gym/utils/gym_utils.py | 44 +- embodichain/lab/scripts/run_env.py | 29 + embodichain/lab/sim/skills/runtime.py | 2 + embodichain/utils/__init__.py | 9 + embodichain/utils/config_paths.py | 56 + embodichain/utils/utility.py | 17 +- .../test_articulation_program.py | 185 ++ tests/gym/envs/expert_program/test_bridge.py | 2049 +++++++++++++++++ tests/gym/envs/expert_program/test_cfg.py | 206 ++ .../gym/envs/expert_program/test_compiler.py | 549 +++++ .../test_completion_metadata.py | 502 ++++ tests/gym/envs/expert_program/test_decoder.py | 596 +++++ .../envs/expert_program/test_environment.py | 949 ++++++++ tests/gym/envs/expert_program/test_loader.py | 252 ++ .../expert_program/test_parallel_compiler.py | 221 ++ .../expert_program/test_parallel_schema.py | 137 ++ .../envs/expert_program/test_simulation.py | 436 ++++ .../test_simulation_environment.py | 1899 +++++++++++++++ .../test_simulation_policies.py | 451 ++++ tests/gym/envs/test_demo.py | 275 ++- .../envs/test_embodied_env_expert_program.py | 92 + tests/gym/envs/test_settling.py | 145 ++ tests/gym/utils/test_gym_utils.py | 132 ++ tests/lab/scripts/test_run_env.py | 171 ++ tests/utils/test_config_paths.py | 68 + 45 files changed, 21229 insertions(+), 131 deletions(-) create mode 100644 docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst create mode 100644 docs/source/overview/sim/atomic_actions/expert_programs.md create mode 100644 embodichain/lab/gym/envs/expert_program/__init__.py create mode 100644 embodichain/lab/gym/envs/expert_program/bridge.py create mode 100644 embodichain/lab/gym/envs/expert_program/cfg.py create mode 100644 embodichain/lab/gym/envs/expert_program/compiler.py create mode 100644 embodichain/lab/gym/envs/expert_program/decoder.py create mode 100644 embodichain/lab/gym/envs/expert_program/environment.py create mode 100644 embodichain/lab/gym/envs/expert_program/loader.py create mode 100644 embodichain/lab/gym/envs/expert_program/simulation.py create mode 100644 embodichain/lab/gym/envs/expert_program/simulation_environment.py create mode 100644 embodichain/lab/gym/envs/expert_program/simulation_policies.py create mode 100644 embodichain/lab/gym/envs/settling.py create mode 100644 embodichain/utils/config_paths.py create mode 100644 tests/gym/envs/expert_program/test_articulation_program.py create mode 100644 tests/gym/envs/expert_program/test_bridge.py create mode 100644 tests/gym/envs/expert_program/test_cfg.py create mode 100644 tests/gym/envs/expert_program/test_compiler.py create mode 100644 tests/gym/envs/expert_program/test_completion_metadata.py create mode 100644 tests/gym/envs/expert_program/test_decoder.py create mode 100644 tests/gym/envs/expert_program/test_environment.py create mode 100644 tests/gym/envs/expert_program/test_loader.py create mode 100644 tests/gym/envs/expert_program/test_parallel_compiler.py create mode 100644 tests/gym/envs/expert_program/test_parallel_schema.py create mode 100644 tests/gym/envs/expert_program/test_simulation.py create mode 100644 tests/gym/envs/expert_program/test_simulation_environment.py create mode 100644 tests/gym/envs/expert_program/test_simulation_policies.py create mode 100644 tests/gym/envs/test_embodied_env_expert_program.py create mode 100644 tests/gym/envs/test_settling.py create mode 100644 tests/utils/test_config_paths.py diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst new file mode 100644 index 000000000..eb8e473f4 --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst @@ -0,0 +1,191 @@ +embodichain.lab.gym.envs.expert_program +======================================= + +.. automodule:: embodichain.lab.gym.envs.expert_program + + .. autosummary:: + + ExpertProgramCfg + ExpertProgramIntegrationCfg + ExpertProgramCompiler + CompiledProgram + load_expert_program + loads_expert_program_json + parse_expert_program_json + decode_expert_program + ExpertProgramEnvironmentMixin + ExpertProgramEnvironmentAdapter + SimulationSceneBinding + SimulationResourceEndpointBinding + SimulationRobotResourceBinding + RobotResourceBinding + ControlPartEndpointBinding + ControlPartResourceBinding + SimulationRobotSkillProfileBinding + SimulationExpertProgramFactory + SimulationSegmentPolicyPort + ControlCommandStateEvidenceTracker + AcceptedRuntimeCommandObserver + AcceptedRuntimeCommandObserverFactory + AntipodalGraspAffordanceBinding + ArticulationOperationAffordanceBinding + ArticulationOperationTargetBinding + AtomicDemoBridge + BarrierCfg + BufferedGymCommandSink + CompiledBarrier + CompiledParallelBlock + CompiledParallelBranch + CompiledPostPolicy + CompiledProgramAnalysis + CompiledProgramCall + CompiledProgramSegment + CompiledProgramValidator + CompiledRepeatFrame + CompiledTargetSelection + ConfigPath + ConfigPathPart + ControlPartCommandPreset + CyclicPoseTargetCfg + DeclarativeCfgValue + DemoBridgeError + EXPERT_PROGRAM_SCHEMA_VERSION + EXPERT_PROGRAM_SCHEMA_VERSION_V2 + EnvironmentStepClock + EnvironmentStepTimingError + ExpertProgramCompileError + ExpertProgramConfigError + ExpertProgramDecodeError + ExpertProgramEnvironmentFactory + ExpertProgramRuntimeAssembly + ExpertProgramSceneResolver + ExpertProgramValidationContext + ExpertProgramValidationError + GymPlanningObservationProvider + HandOverCfg + InvokeCfg + MAX_DECLARATIVE_DEPTH + MAX_DECLARATIVE_NODES + MAX_EXPANDED_CALLS + MAX_EXPERT_PROGRAM_BYTES + MAX_PROGRAM_DEPTH + MAX_PROGRAM_NODES + MAX_REPEAT_COUNT + MaterializedCompiledProgram + MotionGeneratorFactory + ObjectNearTargetValidatorCfg + OperateArticulationCfg + ParallelCfg + PickCfg + PlaceCfg + PlanningObservationPort + PoseCfg + PostPolicyCfg + ProgramNodeCfg + RegisteredSemanticCallCfg + RepeatCfg + RuntimeCommandFrameEncoder + RuntimeTransportActionEncoder + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS + SceneReferenceRole + SceneRegistryProgramResolver + SegmentCfg + SegmentPostPolicyMetadataPort + SegmentPostPolicyPort + SegmentPostPolicyResultPort + SegmentValidatorMetadataPort + SegmentValidatorPort + SemanticCallCfg + SequenceCfg + SharedTickSceneProvider + SimulationArticulationBinding + SimulationArticulationLinkBinding + SimulationExpertProgramEnvironment + SimulationPlanningObservationProvider + SimulationRigidObjectBinding + SkillRuntimeAssemblyPort + TargetCfg + TargetRefCfg + UnsupportedRuntimeTransportError + ValidatorCfg + WaitStablePostCfg + create_simulation_expert_program_adapter + decode_semantic_call + encode_semantic_call + render_config_path + validate_expert_program + +.. currentmodule:: embodichain.lab.gym.envs.expert_program + +Schema and loading +------------------ + +The public decoders and file loaders support Expert Program schema versions 1 +and 2. Version 2 adds deterministic parallel blocks with explicit barriers. + +.. autoclass:: ExpertProgramCfg + :members: + +.. autoclass:: ExpertProgramIntegrationCfg + :members: + +.. autofunction:: load_expert_program + +.. autofunction:: loads_expert_program_json + +.. autofunction:: parse_expert_program_json + +.. autofunction:: decode_expert_program + +.. autofunction:: decode_semantic_call + +.. autofunction:: encode_semantic_call + +Compilation and environment integration +--------------------------------------- + +.. autoclass:: ExpertProgramCompiler + :members: + +.. autoclass:: CompiledProgram + :members: + +.. autoclass:: ExpertProgramEnvironmentMixin + :members: + +.. autoclass:: ExpertProgramEnvironmentAdapter + :members: + +.. autoclass:: SkillRuntimeAssemblyPort + :members: + +Simulation integration +---------------------- + +.. autoclass:: SimulationSceneBinding + :members: + +.. autoclass:: SimulationResourceEndpointBinding + +.. autoclass:: SimulationRobotResourceBinding + +.. autoclass:: RobotResourceBinding + :members: + +.. autoclass:: ControlPartEndpointBinding + :members: + +.. autoclass:: ControlPartResourceBinding + :members: + +.. autoclass:: SimulationRobotSkillProfileBinding + :members: + +.. autoclass:: SimulationExpertProgramFactory + :members: + +.. autoclass:: SimulationSegmentPolicyPort + :members: + +.. autoclass:: ControlCommandStateEvidenceTracker + :members: diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst index f5617a955..6c6c5dd91 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst @@ -21,9 +21,15 @@ through :func:`~embodichain.lab.gym.utils.registration.make`. .. autosummary:: demo + expert_program managers wrapper +.. toctree:: + :hidden: + + embodichain.lab.gym.envs.expert_program + .. currentmodule:: embodichain.lab.gym.envs Environment Classes @@ -60,6 +66,9 @@ segment spans. .. autoclass:: DemoSegment :members: +.. autoclass:: ProcessedEnvAction + :members: + .. autoclass:: DemoSegmentResult :members: diff --git a/docs/source/api_reference/embodichain/embodichain.utils.rst b/docs/source/api_reference/embodichain/embodichain.utils.rst index 36aa780f0..18b6021c8 100644 --- a/docs/source/api_reference/embodichain/embodichain.utils.rst +++ b/docs/source/api_reference/embodichain/embodichain.utils.rst @@ -19,6 +19,7 @@ and image processing. warp cfg configclass + config_paths device_utils file img_utils @@ -46,6 +47,12 @@ Configuration Classes :undoc-members: :show-inheritance: +Configuration Paths +------------------- + +.. automodule:: embodichain.utils.config_paths + :members: + Configuration Nodes ------------------- diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index adb2a063e..cb0d0fecf 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -75,6 +75,7 @@ embodichain.lab.gym.envs.demo DemoEpisodeResult DemoSegment DemoSegmentResult + ProcessedEnvAction execute_demo_episode resolve_demo_segments @@ -88,6 +89,204 @@ embodichain.lab.gym.envs.embodied_env EmbodiedEnvCfg EmbodiedEnv +embodichain.lab.gym.envs.expert_program.bridge +------------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.bridge + +.. autosummary:: + + AcceptedRuntimeCommandObserver + AtomicDemoBridge + BufferedGymCommandSink + CompiledProgramPort + CurrentQposProvider + DemoBridgeError + EnvironmentStepClock + EnvironmentStepTimingError + GymPlanningObservationProvider + JointPositionGymTransportEncoder + ParallelCommandSafetyValidator + RuntimeCommandFrameEncoder + RuntimeTransportActionEncoder + SegmentPostPolicyMetadataPort + SegmentPostPolicyPort + SegmentPostPolicyResultPort + SegmentValidatorMetadataPort + SegmentValidatorPort + SequentialSkillRuntimePort + UnsupportedRuntimeTransportError + +embodichain.lab.gym.envs.expert_program.cfg +--------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.cfg + +.. autosummary:: + + BarrierCfg + CyclicPoseTargetCfg + DeclarativeCfgValue + EXPERT_PROGRAM_SCHEMA_VERSION + EXPERT_PROGRAM_SCHEMA_VERSION_V2 + ExpertProgramCfg + ExpertProgramIntegrationCfg + HandOverCfg + InvokeCfg + MAX_DECLARATIVE_DEPTH + MAX_DECLARATIVE_NODES + MAX_EXPANDED_CALLS + MAX_PROGRAM_DEPTH + MAX_PROGRAM_NODES + MAX_REPEAT_COUNT + ObjectNearTargetValidatorCfg + OperateArticulationCfg + ParallelCfg + PickCfg + PlaceCfg + PoseCfg + PostPolicyCfg + ProgramNodeCfg + RegisteredSemanticCallCfg + RepeatCfg + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS + SegmentCfg + SemanticCallCfg + SequenceCfg + TargetCfg + TargetRefCfg + ValidatorCfg + WaitStablePostCfg + +embodichain.lab.gym.envs.expert_program.compiler +-------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.compiler + +.. autosummary:: + + CompiledBarrier + CompiledParallelBlock + CompiledParallelBranch + CompiledPostPolicy + CompiledProgram + CompiledProgramAnalysis + CompiledProgramCall + CompiledProgramSegment + CompiledProgramValidator + CompiledRepeatFrame + CompiledTargetSelection + ExpertProgramCompileError + ExpertProgramCompiler + ExpertProgramSceneResolver + MaterializedCompiledProgram + SceneRegistryProgramResolver + +embodichain.lab.gym.envs.expert_program.decoder +------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.decoder + +.. autosummary:: + + ConfigPath + ConfigPathPart + ExpertProgramConfigError + ExpertProgramDecodeError + ExpertProgramValidationContext + ExpertProgramValidationError + SceneReferenceRole + decode_expert_program + decode_semantic_call + encode_semantic_call + render_config_path + validate_expert_program + +embodichain.lab.gym.envs.expert_program.environment +----------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.environment + +.. autosummary:: + + AcceptedRuntimeCommandObserverFactory + ExpertProgramEnvironmentAdapter + ExpertProgramEnvironmentFactory + ExpertProgramEnvironmentMixin + ExpertProgramRuntimeAssembly + PlanningObservationPort + SkillRuntimeAssemblyPort + +embodichain.lab.gym.envs.expert_program.loader +------------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.loader + +.. autosummary:: + + MAX_EXPERT_PROGRAM_BYTES + load_expert_program + loads_expert_program_json + parse_expert_program_json + +embodichain.lab.gym.envs.expert_program.simulation +---------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation + +.. autosummary:: + + AntipodalGraspAffordanceBinding + ArticulationOperationAffordanceBinding + ArticulationOperationTargetBinding + ControlPartCommandPreset + ControlPartEndpointBinding + ControlPartResourceBinding + RobotResourceBinding + SimulationArticulationBinding + SimulationArticulationLinkBinding + SimulationResourceEndpointBinding + SimulationRigidObjectBinding + SimulationRobotResourceBinding + SimulationRobotSkillProfileBinding + SimulationSceneBinding + +embodichain.lab.gym.envs.expert_program.simulation_environment +---------------------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation_environment + +.. autosummary:: + + ControlCommandStateEvidenceTracker + MotionGeneratorFactory + SharedTickSceneProvider + SimulationExpertProgramEnvironment + SimulationExpertProgramFactory + SimulationPlanningObservationProvider + create_simulation_expert_program_adapter + +embodichain.lab.gym.envs.expert_program.simulation_policies +------------------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation_policies + +.. autosummary:: + + SimulationSegmentPolicyPort + +embodichain.lab.gym.envs.settling +--------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.settling + +.. autosummary:: + + DynamicSettleMonitor + DynamicSettleMonitorCfg + DynamicSettleSample + DynamicSettleState + embodichain.lab.gym.envs.managers.action_manager ------------------------------------------------ @@ -1772,6 +1971,18 @@ embodichain.workspace_cache_cli main +embodichain.utils +----------------- + +.. currentmodule:: embodichain.utils + +.. autosummary:: + + GLOBAL_SEED + is_configclass + resolve_config_path + set_seed + embodichain_tasks.configs ------------------------- diff --git a/docs/source/overview/sim/atomic_actions/expert_programs.md b/docs/source/overview/sim/atomic_actions/expert_programs.md new file mode 100644 index 000000000..532138d90 --- /dev/null +++ b/docs/source/overview/sim/atomic_actions/expert_programs.md @@ -0,0 +1,255 @@ +(expert-programs)= + +# Declarative Expert Programs + +Expert Programs let a task describe semantic intent without implementing a +task-local motion generator. A program names registered scene entities, robot +profiles, runtime presets, semantic calls, post-policies, and validators. The +shared compiler lowers every call just in time through the same +`SemanticSkillCompiler`, `AtomicActionEngine`, and `SkillRuntime` used by the +Python semantic API. + +Use an Expert Program when later motion depends on the physical result of an +earlier call. Each call receives a fresh scene observation, owns one +`ExecutionSession`, verifies its physical effect, and commits verified symbolic +state before the next call is grounded. + +## Author a program + +Schema version 1 supports bounded `sequence`, `repeat`, `segment`, and `invoke` +nodes. Schema version 2 additionally supports deterministic `parallel` blocks +and explicit `barrier` nodes. Unknown fields, unsupported discriminators, +unbounded structures, executable values, and dotted environment traversal are +rejected before physical execution or command emission. + +`RegisteredSemanticCall` is an opaque extension boundary in these schema +versions. An extension with a physical effect must also register its typed +compiler/effect contract; a serialized call ID alone cannot manufacture effect +verification semantics. + +The repeated-cube task is configured entirely as semantic calls: + +```yaml +schema_version: 1 +program_id: repeated_cube_pick_place +integration: + robot_profile: ur5_parallel_gripper_v1 + scene_registry: multi_segments_cube_v1 + runtime_preset: safe +targets: + drop_pose: + kind: cyclic_pose + values: + - position: [-0.40, 0.48, 0.10] + quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] +program: + kind: repeat + count: 3 + body: + kind: segment + name: move_cube + steps: + kind: sequence + items: + - kind: invoke + call: {kind: pick, object: cube} + - kind: invoke + call: + kind: place + object: cube + at: {kind: target_ref, target: drop_pose} + post: + - {kind: wait_stable, entity: cube, preset: rigid_object} + validators: + - kind: object_near_target + object: cube + target: drop_pose + position_tolerance: 0.12 +``` + +The top-level Gym configuration selects the file with a path relative to that +configuration file: + +```json +{ + "expert_program_path": "../../expert_program/my_task.yaml" +} +``` + +`run_env` can override it explicitly: + +```bash +python -m embodichain.lab.scripts.run_env \ + --gym_config path/to/gym.json \ + --expert-program path/to/program.yaml +``` + +## Accept untrusted model output + +Model-generated programs use the same decoder and compiler, but enter through +the narrower MLLM frontend. The trusted host owns the scene, robot profile, and +runtime preset; the model response must omit `integration` entirely: + +```python +from embodichain.agents.mllm import compile_mllm_expert_program +from embodichain.lab.gym.envs.expert_program import ExpertProgramIntegrationCfg + +compiled = compile_mllm_expert_program( + model_response, + adapter=adapter, + integration=ExpertProgramIntegrationCfg( + robot_profile="my_robot_v1", + scene_registry="my_scene_v1", + runtime_preset="safe", + ), +) +``` + +This entry point accepts exactly one bounded JSON document. It rejects duplicate +keys, non-finite or overflowing numeric values, invalid Unicode, Markdown +fences, trailing text, and every normal schema violation. Its initial policy is +deliberately smaller than the file format: only schema version 1 and curated +`pick`, `place`, `hand_over`, and `operate_articulation` calls are admitted. +The model cannot select `resources`, a hand-over `receiver`, a runtime preset, +or an explicit articulation position/displacement; articulation operations must +use a host-declared named target. Registered calls and parallel nodes remain +host-authored extensions. + +`compile_mllm_expert_program` delegates to the existing +`ExpertProgramEnvironmentAdapter.compile` method. It neither creates a second +compiler nor assembles a runtime while validating model output. + +## Integrate a simulation task + +Task code supplies typed scene and robot integration declarations once, while +the external Expert Program configuration owns task sequence and targets. The +task then delegates runtime assembly to the shared factory; it does not +construct approach, grasp, pull, or placement trajectories: + +```python +class MyTaskEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + def __init__(self, cfg, **kwargs): + super().__init__(cfg, **kwargs) + self._expert_program_adapter = create_simulation_expert_program_adapter( + self, + scene_binding=create_my_scene_binding(), + robot_profile_binding=create_my_robot_profile_binding(), + ) + + @property + def expert_program_adapter(self): + return self._expert_program_adapter +``` + +The scene binding is authoritative for semantic identity, live pose sources, +geometry, affordances, and collision roles. The robot profile owns reusable +resources, endpoint capabilities, semantic commands, policy presets, and effect +monitor selection. `SimulationRobotSkillProfileBinding` accepts generic +`RobotResourceBinding` declarations containing arbitrary typed +`ResourceEndpoint` values; `ControlPartResourceBinding` is its stricter +joint-backed convenience. Endpoint adapters and runtime transports are the +extension boundary for mobile-base, whole-body, or non-joint controllers and +are accepted by the standard simulation helper. Task programs keep the same +semantic calls and do not gain controller-shaped fields. + +Relation and rendezvous semantics are also explicit integration capabilities. +`Place(on=...)` and `Place(inside=...)` require an exact typed/versioned +`RelationTargetGrounder` for the selected affordance payload. `HandOver` +requires the profile-selected `HandOverPoseProvider`. A direct `Place(at=...)` +does not require a relation grounder. Missing, ambiguous, or stale providers +fail during provider-aware program preflight, before the first physical action. + +The same preflight rejects a reachable `safe` preset for a dynamic scene before +the first observation when the active motion generator cannot provide the +required dynamic collision world. + +## Execution and physical effects + +`AtomicDemoBridge` yields lazy `DemoSegment` actions. Every command and settling +hold is consumed by normal `env.step()`, so action managers, recorders, rewards, +timing, and dataset boundaries remain authoritative. `BaseEnv.step_dt` is the +only control cadence; a command duration that is not representable on that grid +fails instead of being silently resampled. + +Creating a bridge materializes the bounded segment stream and performs +provider-aware semantic analysis before any command can be emitted. Sequential +stretches retain downstream object-target look-ahead across segment boundaries; +an explicit parallel block is a conservative look-ahead barrier. Runtime still +re-observes and grounds each call just in time after prior verified effects. + +The standard simulation integration verifies grasp and release with two pieces +of evidence: + +- the last exact open/grasp command accepted by the buffered Gym command sink, + tracked independently for every stable environment ID; and +- the live object-to-endpoint pose relation from the shared scene snapshot. + +The command-state update is transactional: encoder, buffer, cancellation, or +safe-stop failures invalidate it. An integration with contact, constraint, +force, or wrench sensing can install typed evidence callbacks without changing +the semantic call or program. + +Program/demo-segment metadata records runtime call results, named trajectory +segments, effect decisions, recovery events, scene and collision revisions, +settling outcomes, and validator results in deterministic JSON-safe values. +Trajectory segments are trace ranges inside one atomic plan; they do not create +independent recovery or timeout boundaries. + +Schema-version-2 parallel blocks additionally require an authoritative +`ParallelCommandSafetyValidator`. Resource-claim disjointness is necessary but +is not treated as proof of physical safety. If no validator is installed, the +parallel block refuses to start; the standard simulation adapter intentionally +does not invent one from resource names. Every parallel frame must occupy +exactly one `BaseEnv.step_dt`; shorter lanes repeat their last safe target as +hold padding, while fractional frames are rejected rather than resampled. +Version 2 also uses strict symbolic key-level conflict detection at the barrier: +two branches may not commit the same task-state key, even when their physical +changes occurred in disjoint environment rows. + +## Python semantic calls + +Standalone applications can use the same compiler and runtime through +`AtomicSkills`: + +```python +skills = AtomicSkills.from_env(runtime_provider, preset="safe") +cube = skills.scene.object("cube") +tray = skills.scene.object("tray") +result = skills.run(Pick(object=cube), Place(object=cube, on=tray)) +``` + +In this example, `runtime_provider` owns the typed relation grounder for the +tray's placement affordance. Applications without such a provider can use a +direct `SemanticPose` through `Place(at=...)`. + +`from_env` requires an explicit `SkillRuntimeProvider`; it never scans arbitrary +environment attributes. Gym demonstration environments intentionally use the +lazy bridge instead, because a synchronous runtime would bypass the required +`env.step()` handshake. Advanced applications may use +`AtomicSkills.from_components(...)` with explicit observation, command, +evidence, and clock ports. + +For the lower-level planning and execution contracts, see {doc}`index`. For +robot resource and endpoint declarations, see {doc}`robot_skill_profiles`. + +## Capability status + +| Surface | Shared contract | Standard simulation integration | +| --- | --- | --- | +| `Pick` | Compiler, runtime, effect verification | Antipodal grasp binding plus motion/grasp resources | +| `Place(at=...)` | Object-centric lowering with verified held state | Direct semantic pose target | +| `Place(on=...)` / `Place(inside=...)` | Exact typed relation dispatch | Integration must install the matching `RelationTargetGrounder` | +| `HandOver` | Coordinated call, state flow, and effect contract | Embodiment must install its named `HandOverPoseProvider` and evidence sources | +| `OperateArticulation` | Named/absolute/displacement target and joint effect | Link, joint, operation-affordance, and interaction endpoint bindings | +| Registered calls | Typed call catalog and explicit lowerer | Physical extensions must add an explicit effect contract | +| Mobile/whole-body extensions | Generic resources, claims, endpoint targets, command frames, and routing | Requires a reusable semantic skill/lowerer plus matching adapter, payload, transport, and effect integration; no curated navigation or whole-body skill is installed today | +| Parallel blocks | Shared-clock coordinator and strict barrier merge | Requires an authoritative `ParallelCommandSafetyValidator`; none is inferred by default | + +The table separates implemented reusable contracts from embodiment-specific +providers. It is not a claim that every row has completed task-level physical +simulation acceptance. + +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; its full three-cycle +run remains in threshold calibration. diff --git a/embodichain/lab/gym/envs/__init__.py b/embodichain/lab/gym/envs/__init__.py index 5601100fe..af28a8dc8 100644 --- a/embodichain/lab/gym/envs/__init__.py +++ b/embodichain/lab/gym/envs/__init__.py @@ -21,6 +21,7 @@ from .base_env import * from .demo import * from .embodied_env import * +from .settling import * from .wrapper import * # Official task environments live in the bundled ``embodichain_tasks`` package diff --git a/embodichain/lab/gym/envs/demo.py b/embodichain/lab/gym/envs/demo.py index 695500a8d..dc3be1ef6 100644 --- a/embodichain/lab/gym/envs/demo.py +++ b/embodichain/lab/gym/envs/demo.py @@ -20,9 +20,14 @@ from collections.abc import Callable, Iterable, Mapping from dataclasses import dataclass, field, replace -from typing import Any +import math +from types import MappingProxyType +from typing import Any, Literal import torch +from tensordict import TensorDict + +from embodichain.lab.sim.types import EnvAction __all__ = [ "DEMO_ANNOTATION_KEYS", @@ -30,6 +35,7 @@ "DemoEpisodeResult", "DemoSegment", "DemoSegmentResult", + "ProcessedEnvAction", "execute_demo_episode", "resolve_demo_segments", ] @@ -50,6 +56,69 @@ """Per-frame annotation keys stored in expert rollout buffers.""" +def _json_safe_copy(value: Any, *, field_name: str) -> Any: + """Return an owned JSON value without implicit type coercion.""" + if value is None or type(value) in {bool, int, str}: + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{field_name} contains a non-finite float.") + return value + if isinstance(value, Mapping): + result: dict[str, Any] = {} + for key, item in value.items(): + if type(key) is not str or not key or key != key.strip(): + raise ValueError( + f"{field_name} mapping keys must be non-empty strings " + "without outer whitespace." + ) + result[key] = _json_safe_copy( + item, + field_name=f"{field_name}.{key}", + ) + return result + if isinstance(value, (list, tuple)): + return [ + _json_safe_copy(item, field_name=f"{field_name}[{index}]") + for index, item in enumerate(value) + ] + raise TypeError(f"{field_name} contains non-JSON value {type(value).__name__}.") + + +@dataclass(frozen=True, slots=True, eq=False) +class ProcessedEnvAction: + """Owned controller-ready action that must still pass through ``env.step``. + + Semantic runtimes and demonstration bridges may already have produced the + action-manager output (for example, a full joint-position command assembled + from typed runtime endpoints). Wrapping it prevents the environment from + applying the pre-action transform a second time while retaining the normal + simulation, manager, recorder, reward, and dataset step lifecycle. + + Args: + value: Controller-ready tensor or ``TensorDict``. + metadata: JSON-compatible provenance attached by the producer. The + environment does not interpret this mapping. + """ + + value: EnvAction + metadata: Mapping[str, Any] = field(default_factory=dict) + + def __post_init__(self) -> None: + if not isinstance(self.value, (torch.Tensor, TensorDict)): + raise TypeError("value must be a torch.Tensor or TensorDict.") + if not isinstance(self.metadata, Mapping): + raise TypeError("metadata must be a mapping.") + owned_value = self.value.clone() + owned_metadata = _json_safe_copy(self.metadata, field_name="metadata") + object.__setattr__(self, "value", owned_value) + object.__setattr__(self, "metadata", MappingProxyType(owned_metadata)) + + def snapshot(self) -> ProcessedEnvAction: + """Return an independently owned processed-action envelope.""" + return ProcessedEnvAction(value=self.value, metadata=self.metadata) + + @dataclass(frozen=True) class DemoSegment: """One semantic subtask inside a demonstration episode. @@ -69,6 +138,16 @@ class DemoSegment: parallel environment (or one scalar broadcast to every environment). Gym ``terminated`` and ``truncated`` remain episode-level signals; use this callback for subtask-level validation. + abort_actions: Optional callback invoked when the executor stops after + retrieving an action but before exhausting the iterable. It receives + a reason and ``last_action_consumed`` flag, and must return any + emergency controller actions that still need ordinary ``env.step`` + consumption. This is the explicit cancellation handshake for lazy + runtimes whose command acknowledgements only mean locally buffered. + failure_policy: ``"batch_abort"`` preserves legacy batch-atomic + behavior. ``"row_independent"`` permanently freezes only failed + environment rows while peers continue through the shared segment + and later lazy segments. """ actions: Iterable[Any] @@ -77,6 +156,20 @@ class DemoSegment: instruction: str | None = None metadata: Mapping[str, Any] = field(default_factory=dict) validator: Callable[[], Any] | None = field(default=None, repr=False, compare=False) + abort_actions: Callable[..., Iterable[Any]] | None = field( + default=None, + repr=False, + compare=False, + ) + failure_policy: Literal["batch_abort", "row_independent"] = "batch_abort" + + def __post_init__(self) -> None: + if self.abort_actions is not None and not callable(self.abort_actions): + raise TypeError("abort_actions must be callable or None.") + if self.failure_policy not in {"batch_abort", "row_independent"}: + raise ValueError( + "failure_policy must be 'batch_abort' or 'row_independent'." + ) @dataclass(frozen=True) @@ -118,6 +211,15 @@ class DemoSegmentResult: successes: tuple[bool, ...] = () failure_reasons: tuple[str | None, ...] = () + def __post_init__(self) -> None: + if not isinstance(self.metadata, Mapping): + raise TypeError("metadata must be a mapping.") + owned_metadata = _json_safe_copy( + self.metadata, + field_name="segment result metadata", + ) + object.__setattr__(self, "metadata", MappingProxyType(owned_metadata)) + def to_metadata(self, env_id: int | None = None) -> dict[str, Any]: """Return a JSON-compatible aggregate or per-environment representation. @@ -133,7 +235,10 @@ def to_metadata(self, env_id: int | None = None) -> dict[str, Any]: "name": self.name, "target_uid": self.target_uid, "instruction": self.instruction, - "metadata": dict(self.metadata), + "metadata": _json_safe_copy( + self.metadata, + field_name="segment result metadata", + ), } if env_id is not None and self.start_steps: metadata.update( @@ -269,6 +374,28 @@ def _as_bool_tuple(value: Any, num_envs: int) -> tuple[bool, ...]: return tuple(bool(item) for item in tensor.tolist()) +def _has_terminal_runtime_failure_trace(segment: DemoSegment) -> bool: + """Return whether a lazy segment recorded a canonical failed runtime. + + Expert-program action iterables may terminate before yielding a controller + command when planning fails. Their bridge finalizes the runtime trace while + exhausting the iterable and exposes a validator that commits row-local + failure. This marker distinguishes that outcome from an ordinary empty + ``DemoSegment``, whose existing ``empty_segment`` guard remains unchanged. + """ + runtime = segment.metadata.get("runtime") + if not isinstance(runtime, Mapping): + return False + return ( + runtime.get("kind") + in { + "skill_result", + "parallel_skill_result", + } + and runtime.get("status") == "failed" + ) + + def _dataset_instruction(env: Any) -> str: """Return the dataset-level instruction used for legacy demo segments.""" metadata = getattr(_env_target(env), "metadata", {}) @@ -448,7 +575,20 @@ def publish_active_mask() -> None: f"{segment.name}", ) - for action in actions: + action_iterator = iter(actions) + last_action_consumed: bool | None = None + action_error: Exception | None = None + while True: + try: + action = next(action_iterator) + except StopIteration: + break + except Exception as exc: + action_error = exc + actions_exhausted = False + segment_reason = "action_generation_failed" + break + last_action_consumed = False if should_stop is not None and should_stop(): actions_exhausted = False fatal_reason = "interrupted" @@ -461,18 +601,32 @@ def publish_active_mask() -> None: publish_active_mask() break - if normalize_action is not None: - action = normalize_action(action) - if not all(active): - if mask_action is None: - raise RuntimeError( - "A vector demo environment completed asynchronously but " - "does not implement _mask_demo_action(action, active_mask)." - ) - action = mask_action(action, tuple(active)) + try: + if normalize_action is not None: + action = normalize_action(action) + if not all(active): + if mask_action is None: + raise RuntimeError( + "A vector demo environment completed asynchronously " + "but does not implement " + "_mask_demo_action(action, active_mask)." + ) + action = mask_action(action, tuple(active)) + except Exception as exc: + action_error = exc + actions_exhausted = False + segment_reason = "action_processing_failed" + break active_before_step = tuple(active) - _, _, terminated_value, truncated_value, info = env.step(action) + try: + _, _, terminated_value, truncated_value, info = env.step(action) + except Exception as exc: + action_error = exc + actions_exhausted = False + segment_reason = "action_execution_failed" + break + last_action_consumed = True action_count += 1 last_info = info for env_id, was_active in enumerate(active_before_step): @@ -529,16 +683,21 @@ def publish_active_mask() -> None: step_failed = True if step_failed: - actions_exhausted = False - fatal_reason = "truncated" if active_step_truncated else "failure" - segment_reason = fatal_reason - for env_id, is_active in enumerate(active): - if is_active: - terminal_reasons[env_id] = "batch_aborted" - segment_failure_reasons[env_id] = "batch_aborted" - active[env_id] = False + if segment.failure_policy == "batch_abort": + actions_exhausted = False + fatal_reason = ( + "truncated" if active_step_truncated else "failure" + ) + segment_reason = fatal_reason + for env_id, is_active in enumerate(active): + if is_active: + terminal_reasons[env_id] = "batch_aborted" + segment_failure_reasons[env_id] = "batch_aborted" + active[env_id] = False publish_active_mask() - break + if segment.failure_policy == "batch_abort" or not any(active): + actions_exhausted = False + break publish_active_mask() if not any(active): @@ -558,7 +717,78 @@ def publish_active_mask() -> None: publish_active_mask() break - if action_count == 0 and segment_reason is None: + if not actions_exhausted: + if segment.abort_actions is not None: + reason = ( + segment_reason + or fatal_reason + or "demo segment execution stopped before exhaustion" + ) + try: + emergency_actions = segment.abort_actions( + reason, + last_action_consumed=bool(last_action_consumed), + ) + if isinstance(emergency_actions, (str, bytes)): + raise TypeError( + "abort_actions must return an iterable of actions." + ) + emergency_iterator = iter(emergency_actions) + try: + for emergency_action in emergency_iterator: + if normalize_action is not None: + emergency_action = normalize_action( + emergency_action + ) + try: + _, _, _, _, emergency_info = env.step( + emergency_action + ) + except Exception as exc: + raise RuntimeError( + "Emergency demo safe-stop action failed " + "during env.step()." + ) from exc + action_count += 1 + last_info = emergency_info + for env_id, is_participant in enumerate(participants): + if is_participant: + lengths[env_id] += 1 + finally: + close_emergency = getattr( + emergency_iterator, + "close", + None, + ) + if callable(close_emergency): + close_emergency() + finally: + close_actions = getattr(action_iterator, "close", None) + if callable(close_actions): + close_actions() + else: + close_actions = getattr(action_iterator, "close", None) + if callable(close_actions): + close_actions() + + if action_error is not None: + raise RuntimeError( + "Demo action generation, processing, or execution failed " + "after an emergency safe-stop attempt." + ) from action_error + + traced_terminal_runtime_failure = ( + action_count == 0 + and actions_exhausted + and segment_reason is None + and segment.validator is not None + and _has_terminal_runtime_failure_trace(segment) + ) + if ( + action_count == 0 + and segment_reason is None + and not traced_terminal_runtime_failure + ): fatal_reason = "empty_segment" segment_reason = fatal_reason for env_id, is_participant in enumerate(participants): @@ -588,15 +818,25 @@ def publish_active_mask() -> None: terminal_reasons[env_id] = "segment_validation_failed" if validation_failed: - fatal_reason = "segment_validation_failed" - segment_reason = fatal_reason - for env_id, is_active in enumerate(active): - if is_active: - if segment_failure_reasons[env_id] is None: - segment_failure_reasons[env_id] = "batch_aborted" - terminal_reasons[env_id] = "batch_aborted" - segment_successes[env_id] = False - active[env_id] = False + if segment.failure_policy == "batch_abort": + fatal_reason = "segment_validation_failed" + segment_reason = fatal_reason + for env_id, is_active in enumerate(active): + if is_active: + if segment_failure_reasons[env_id] is None: + segment_failure_reasons[env_id] = "batch_aborted" + terminal_reasons[env_id] = "batch_aborted" + segment_successes[env_id] = False + active[env_id] = False + else: + for env_id, is_active in enumerate(active): + if ( + is_active + and segment_failure_reasons[env_id] + == "segment_validation_failed" + ): + segment_successes[env_id] = False + active[env_id] = False publish_active_mask() participant_ids = [ diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 1f42df648..db40d02b7 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -27,7 +27,17 @@ import gymnasium as gym from dataclasses import MISSING -from typing import Dict, Union, Sequence, Tuple, Any, Iterable, List, Optional +from typing import ( + TYPE_CHECKING, + Dict, + Union, + Sequence, + Tuple, + Any, + Iterable, + List, + Optional, +) from tensordict import TensorDict from embodichain.lab.sim.cfg import ( @@ -52,6 +62,7 @@ DemoEpisodeResult, DemoSegment, DemoSegmentResult, + ProcessedEnvAction, ) from embodichain.lab.gym.envs.managers import ( EventManager, @@ -70,6 +81,13 @@ from embodichain.data import get_data_path from embodichain.data.constants import EMBODICHAIN_DEFAULT_DATA_ROOT +if TYPE_CHECKING: + from embodichain.lab.gym.envs.expert_program import ( + CompiledProgram, + ExpertProgramCfg, + ) + from embodichain.lab.gym.envs.expert_program.bridge import AtomicDemoBridge + __all__ = ["EmbodiedEnvCfg", "EmbodiedEnv"] @@ -240,6 +258,14 @@ class EnvLightCfg: """If True (and record_trajectory is True), auto-save each env's trajectory to ``trajectory_save_dir`` at episode end and on close().""" + expert_program: ExpertProgramCfg | None = None + """Optional declarative Expert Program used to generate demo segments. + + The program remains inert until :meth:`EmbodiedEnv.create_demo_segments` + requests an explicit environment compiler and bridge through the dedicated + hooks. No live provider, planner, or callable is stored in this config. + """ + @register_env("EmbodiedEnv-v1") class EmbodiedEnv(BaseEnv): @@ -1338,14 +1364,30 @@ def _write_rl_rollout_step( : self.num_envs, self.current_rollout_step ].copy_(truncateds.to(buffer_device), non_blocking=True) - def _normalize_demo_action(self, action: EnvAction) -> EnvAction: - """Normalize one legacy or segment action to the environment action space.""" + def _normalize_demo_action( + self, action: EnvAction | ProcessedEnvAction + ) -> EnvAction | ProcessedEnvAction: + """Normalize one raw action or preserve a controller-ready envelope.""" + if isinstance(action, ProcessedEnvAction): + value = action.value + if value.ndim == 0: + raise ValueError( + "Processed demo actions must have a leading environment " + "dimension." + ) + if value.shape[0] != self.num_envs: + raise ValueError( + "Processed demo action batch size must match num_envs." + ) + return action.snapshot() expected_dim = int(np.prod(self.single_action_space.shape)) return self._normalize_demo_action_tensor(action, expected_dim) def _mask_demo_action( - self, action: EnvAction, active_mask: Sequence[bool] - ) -> EnvAction: + self, + action: EnvAction | ProcessedEnvAction, + active_mask: Sequence[bool], + ) -> EnvAction | ProcessedEnvAction: """Accept an asynchronously completed vector-demo action. Raw actions may still require :class:`ActionManager` preprocessing, so @@ -1638,15 +1680,18 @@ def evaluate(self, **kwargs) -> Dict[str, Any]: eval_dict[key] = value return eval_dict - def _preprocess_action(self, action: EnvAction) -> EnvAction: - """Delegate to ActionManager when configured; stash raw action for trajectory.""" + def _preprocess_action(self, action: EnvAction | ProcessedEnvAction) -> EnvAction: + """Apply raw preprocessing once and stash the executed controller action.""" + is_processed = isinstance(action, ProcessedEnvAction) + if is_processed: + action = action.value if self._traj_buffer is not None: self._traj_raw_action = ( action.clone() if hasattr(action, "clone") else action ) - if self.action_manager is not None: + if self.action_manager is not None and not is_processed: action = self.action_manager.process_action(action, mode="pre") - else: + elif not is_processed: action = super()._preprocess_action(action) if getattr(self, "_demo_no_auto_reset", False): action = self._mask_processed_demo_action(action) @@ -1853,13 +1898,65 @@ def create_demo_action_list(self, *args, **kwargs) -> Sequence[EnvAction] | None "The method 'create_demo_action_list' must be implemented in subclasses." ) + def compile_expert_program( + self, + program: ExpertProgramCfg, + ) -> CompiledProgram: + """Compile a configured Expert Program through explicit scene providers. + + Declarative environments override this hook to supply their authoritative + scene registry/resolver to :class:`ExpertProgramCompiler`. Keeping the + provider boundary explicit prevents the base environment from inferring + identities or scanning mutable simulator internals. + + Args: + program: Strict Expert Program configuration attached to ``cfg``. + + Returns: + Provider-free compiled program ready for runtime assembly. + + Raises: + NotImplementedError: If an environment enables ``expert_program`` + without supplying the compiler/provider integration. + """ + raise NotImplementedError( + "An environment with cfg.expert_program must implement " + "compile_expert_program() using an explicit scene resolver." + ) + + def create_expert_program_bridge( + self, + program: CompiledProgram, + ) -> AtomicDemoBridge: + """Create the Gym demo bridge through explicit runtime-port factories. + + Declarative environments override this hook to assemble ``SkillRuntime`` + and Gym-aware command/clock/post-policy/validator ports. The returned + bridge must emit commands through normal ``env.step()`` processing. + + Args: + program: Compiled provider-free Expert Program. + + Returns: + Atomic demo bridge whose segments are consumed lazily. + + Raises: + NotImplementedError: If no explicit runtime factory is available. + """ + raise NotImplementedError( + "An environment with cfg.expert_program must implement " + "create_expert_program_bridge() using explicit runtime ports." + ) + def create_demo_segments(self, *args, **kwargs) -> Iterable[DemoSegment] | None: """Create the semantic segments that make up one task episode. - The default adapter preserves existing tasks by wrapping their single - ``create_demo_action_list`` result in one segment. Multi-object tasks - should override this method and may return a lazy generator so each - segment can be planned from the scene state left by the previous one. + When ``cfg.expert_program`` is configured, the environment compiles it + through an explicit scene-provider hook and creates an atomic demo bridge + through an explicit runtime-port factory hook. Otherwise, the default + adapter wraps ``create_demo_action_list`` in one segment. Multi-object + tasks may return a lazy generator so each segment can be planned from + the scene state left by the previous one. Args: *args: Positional arguments forwarded to the legacy planner. @@ -1868,6 +1965,12 @@ def create_demo_segments(self, *args, **kwargs) -> Iterable[DemoSegment] | None: Returns: Segment sequence, or ``None`` when planning fails. """ + expert_program = getattr(getattr(self, "cfg", None), "expert_program", None) + if expert_program is not None: + compiled_program = self.compile_expert_program(expert_program) + bridge = self.create_expert_program_bridge(compiled_program) + return bridge.iter_segments() + actions = self.create_demo_action_list(*args, **kwargs) if actions is None: return None diff --git a/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py new file mode 100644 index 000000000..12b371bec --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -0,0 +1,257 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Versioned declarative Expert Program schema, compiler, and runtime types.""" + +from __future__ import annotations + +from .cfg import ( + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, + MAX_DECLARATIVE_DEPTH, + MAX_DECLARATIVE_NODES, + MAX_EXPANDED_CALLS, + MAX_PROGRAM_DEPTH, + MAX_PROGRAM_NODES, + MAX_REPEAT_COUNT, + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS, + BarrierCfg, + CyclicPoseTargetCfg, + DeclarativeCfgValue, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + OperateArticulationCfg, + ParallelCfg, + PickCfg, + PlaceCfg, + PoseCfg, + PostPolicyCfg, + ProgramNodeCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SemanticCallCfg, + SequenceCfg, + TargetCfg, + TargetRefCfg, + ValidatorCfg, + WaitStablePostCfg, +) +from .decoder import ( + ConfigPath, + ConfigPathPart, + ExpertProgramConfigError, + ExpertProgramDecodeError, + ExpertProgramValidationContext, + ExpertProgramValidationError, + SceneReferenceRole, + decode_expert_program, + decode_semantic_call, + encode_semantic_call, + render_config_path, + validate_expert_program, +) +from .loader import ( + MAX_EXPERT_PROGRAM_BYTES, + load_expert_program, + loads_expert_program_json, + parse_expert_program_json, +) +from .bridge import ( + AcceptedRuntimeCommandObserver, + AtomicDemoBridge, + BufferedGymCommandSink, + DemoBridgeError, + EnvironmentStepClock, + EnvironmentStepTimingError, + GymPlanningObservationProvider, + RuntimeCommandFrameEncoder, + RuntimeTransportActionEncoder, + SegmentPostPolicyMetadataPort, + SegmentPostPolicyPort, + SegmentPostPolicyResultPort, + SegmentValidatorMetadataPort, + SegmentValidatorPort, + UnsupportedRuntimeTransportError, +) +from .compiler import ( + CompiledBarrier, + CompiledParallelBlock, + CompiledParallelBranch, + CompiledPostPolicy, + CompiledProgram, + CompiledProgramAnalysis, + CompiledProgramCall, + CompiledProgramSegment, + CompiledProgramValidator, + CompiledRepeatFrame, + CompiledTargetSelection, + ExpertProgramCompileError, + ExpertProgramCompiler, + ExpertProgramSceneResolver, + MaterializedCompiledProgram, + SceneRegistryProgramResolver, +) +from .environment import ( + AcceptedRuntimeCommandObserverFactory, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentFactory, + ExpertProgramEnvironmentMixin, + ExpertProgramRuntimeAssembly, + PlanningObservationPort, + SkillRuntimeAssemblyPort, +) +from .simulation import ( + AntipodalGraspAffordanceBinding, + ArticulationOperationAffordanceBinding, + ArticulationOperationTargetBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + RobotResourceBinding, + SimulationArticulationBinding, + SimulationArticulationLinkBinding, + SimulationRigidObjectBinding, + SimulationResourceEndpointBinding, + SimulationRobotResourceBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, +) +from .simulation_environment import ( + ControlCommandStateEvidenceTracker, + MotionGeneratorFactory, + SharedTickSceneProvider, + SimulationExpertProgramEnvironment, + SimulationExpertProgramFactory, + SimulationPlanningObservationProvider, + create_simulation_expert_program_adapter, +) +from .simulation_policies import SimulationSegmentPolicyPort + +__all__ = [ + "AcceptedRuntimeCommandObserver", + "AcceptedRuntimeCommandObserverFactory", + "AntipodalGraspAffordanceBinding", + "ArticulationOperationAffordanceBinding", + "ArticulationOperationTargetBinding", + "AtomicDemoBridge", + "BarrierCfg", + "BufferedGymCommandSink", + "ConfigPath", + "ConfigPathPart", + "CompiledBarrier", + "CompiledParallelBlock", + "CompiledParallelBranch", + "CompiledPostPolicy", + "CompiledProgram", + "CompiledProgramAnalysis", + "CompiledProgramCall", + "CompiledProgramSegment", + "CompiledProgramValidator", + "CompiledRepeatFrame", + "CompiledTargetSelection", + "ControlCommandStateEvidenceTracker", + "ControlPartCommandPreset", + "ControlPartEndpointBinding", + "ControlPartResourceBinding", + "CyclicPoseTargetCfg", + "DeclarativeCfgValue", + "DemoBridgeError", + "EXPERT_PROGRAM_SCHEMA_VERSION", + "EXPERT_PROGRAM_SCHEMA_VERSION_V2", + "EnvironmentStepClock", + "EnvironmentStepTimingError", + "ExpertProgramCfg", + "ExpertProgramCompileError", + "ExpertProgramCompiler", + "ExpertProgramConfigError", + "ExpertProgramDecodeError", + "ExpertProgramEnvironmentAdapter", + "ExpertProgramEnvironmentFactory", + "ExpertProgramEnvironmentMixin", + "ExpertProgramIntegrationCfg", + "ExpertProgramRuntimeAssembly", + "ExpertProgramSceneResolver", + "ExpertProgramValidationContext", + "ExpertProgramValidationError", + "HandOverCfg", + "GymPlanningObservationProvider", + "InvokeCfg", + "MAX_DECLARATIVE_DEPTH", + "MAX_DECLARATIVE_NODES", + "MAX_EXPANDED_CALLS", + "MAX_EXPERT_PROGRAM_BYTES", + "MAX_PROGRAM_DEPTH", + "MAX_PROGRAM_NODES", + "MAX_REPEAT_COUNT", + "MaterializedCompiledProgram", + "MotionGeneratorFactory", + "ObjectNearTargetValidatorCfg", + "OperateArticulationCfg", + "ParallelCfg", + "PickCfg", + "PlaceCfg", + "PlanningObservationPort", + "PoseCfg", + "PostPolicyCfg", + "ProgramNodeCfg", + "RegisteredSemanticCallCfg", + "RepeatCfg", + "RobotResourceBinding", + "RuntimeCommandFrameEncoder", + "RuntimeTransportActionEncoder", + "SceneReferenceRole", + "SceneRegistryProgramResolver", + "SegmentPostPolicyMetadataPort", + "SegmentPostPolicyPort", + "SegmentPostPolicyResultPort", + "SegmentCfg", + "SegmentValidatorMetadataPort", + "SegmentValidatorPort", + "SemanticCallCfg", + "SequenceCfg", + "SharedTickSceneProvider", + "SkillRuntimeAssemblyPort", + "SimulationArticulationBinding", + "SimulationArticulationLinkBinding", + "SimulationExpertProgramEnvironment", + "SimulationExpertProgramFactory", + "SimulationPlanningObservationProvider", + "SimulationRigidObjectBinding", + "SimulationResourceEndpointBinding", + "SimulationRobotResourceBinding", + "SimulationRobotSkillProfileBinding", + "SimulationSceneBinding", + "SimulationSegmentPolicyPort", + "SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS", + "TargetCfg", + "TargetRefCfg", + "UnsupportedRuntimeTransportError", + "ValidatorCfg", + "WaitStablePostCfg", + "create_simulation_expert_program_adapter", + "decode_expert_program", + "decode_semantic_call", + "encode_semantic_call", + "load_expert_program", + "loads_expert_program_json", + "parse_expert_program_json", + "render_config_path", + "validate_expert_program", +] diff --git a/embodichain/lab/gym/envs/expert_program/bridge.py b/embodichain/lab/gym/envs/expert_program/bridge.py new file mode 100644 index 000000000..f72e616d3 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/bridge.py @@ -0,0 +1,1574 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Gym ports and a lazy demo adapter for compiled Expert Programs. + +This module deliberately stops at the Gym action boundary. It never calls +``env.step`` and never updates a simulator directly. The demo executor owns +the environment step; when it asks the action generator for the next value, +the bridge treats the previously yielded value as consumed and advances the +environment-backed execution clock by exactly one step. +""" + +from __future__ import annotations + +from collections import deque +from collections.abc import Callable, Iterable, Iterator, Mapping +from dataclasses import dataclass, field +import math +from typing import Any, Protocol, runtime_checkable + +import torch + +from embodichain.lab.gym.envs.demo import DemoSegment, ProcessedEnvAction +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.runner import ( + CommandAcknowledgement, + ExecutionClock, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, +) +from embodichain.lab.sim.atomic_actions.state import PlanningContext, TaskState +from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, + ParallelSkillResult, + ParallelSkillRuntime, +) +from embodichain.lab.sim.skills.runtime import SkillResult, SkillRuntime, SkillStatus +from embodichain.lab.sim.types import EnvAction + +_SAFE_HOLD_ACTION_KINDS = frozenset( + {"runtime_safe_hold", "runtime_wait_hold", "runtime_abort_safe_hold"} +) + + +class DemoBridgeError(RuntimeError): + """Base error raised by the Expert Program Gym bridge.""" + + +class EnvironmentStepTimingError(DemoBridgeError, ValueError): + """Raised when runtime timing cannot be represented on the Gym step grid.""" + + +class UnsupportedRuntimeTransportError(DemoBridgeError, LookupError): + """Raised when a command frame names an unregistered transport.""" + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate and return one strict identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _validate_timeout(timeout: float) -> None: + """Validate a runner-supplied acknowledgement timeout.""" + if not isinstance(timeout, (int, float)) or isinstance(timeout, bool): + raise TypeError("timeout must be a real number.") + if not math.isfinite(float(timeout)) or float(timeout) <= 0.0: + raise ValueError("timeout must be finite and positive.") + + +@runtime_checkable +class CurrentQposProvider(Protocol): + """Source of full robot positions aligned to explicit environment IDs.""" + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + """Return ``(batch_size, robot_dof)`` positions for ``env_ids``.""" + + +@runtime_checkable +class RuntimeTransportActionEncoder(Protocol): + """Extensible lowering boundary for one runtime transport kind. + + An encoder receives the action produced by earlier registered transports + and returns the next owned action value. This permits a future transport + to promote the built-in tensor action to a ``TensorDict`` when the Gym + action manager exposes a structured controller boundary. + """ + + @property + def transport_id(self) -> str: + """Return the exact runtime transport ID handled by this encoder.""" + + def encode( + self, + command: EndpointCommand, + *, + base_action: EnvAction, + active_mask: torch.Tensor, + ) -> EnvAction: + """Merge one addressed command into ``base_action``.""" + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: EnvAction, + context: PlanningContext, + ) -> EnvAction: + """Merge this transport's safe state into ``base_action``.""" + + +@runtime_checkable +class AcceptedRuntimeCommandObserver(Protocol): + """Transactional observer of commands accepted by the buffered Gym sink. + + Implementations may maintain runtime-local evidence state, but must not + control the robot or advance the environment. ``accepted`` is called only + after a complete frame was encoded and appended to the local buffer. + Cancellation and discard notifications are fail-closed reset boundaries. + """ + + def accepted(self, command: RuntimeCommandFrame) -> None: + """Record one independently owned accepted command frame.""" + + def cancelled(self, targets: tuple[RuntimeEndpointTarget, ...]) -> None: + """Clear state owned by the cancelled endpoint targets.""" + + def discarded(self) -> None: + """Clear every runtime-local state value after a buffer discard.""" + + +@runtime_checkable +class CompiledProgramPort(Protocol): + """Minimal provider-free compiled-program surface consumed by the bridge.""" + + schema_version: int + program_id: str + + def iter_segments(self) -> Iterator[Any]: + """Lazily yield compiled logical segments.""" + + def sequential_execution_analysis(self, segment_index: int) -> Any: + """Return current prefix plus downstream calls up to the next barrier.""" + + +@runtime_checkable +class SequentialSkillRuntimePort(Protocol): + """Nonblocking semantic runtime surface used by sequential segments.""" + + @property + def result(self) -> SkillResult: + """Return the current immutable runtime result.""" + + @property + def status(self) -> SkillStatus: + """Return the current runtime status.""" + + def start( + self, + *calls: Any, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + """Start one semantic workflow without blocking on motion.""" + + def step(self) -> SkillResult: + """Advance the workflow by at most one due runner cycle.""" + + def cancel(self, reason: str) -> SkillResult: + """Cancel one running workflow through the runner's safe-stop path.""" + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + """Install state merged at an independent parallel barrier.""" + + +@runtime_checkable +class SegmentPostPolicyPort(Protocol): + """Environment-aware program post-policy boundary. + + Implementations may observe the environment after each resumed yield, but + must return every controller action to this iterable. The bridge then + routes those values through the ordinary demo executor and ``env.step``. + """ + + def validate_policy( + self, + policy: Any, + *, + segment: Any, + ) -> None: + """Validate one compiled policy without live observation or action.""" + + def actions( + self, + policy: Any, + *, + segment: Any, + active_mask: torch.Tensor, + ) -> Iterable[Any]: + """Yield holds until ``policy`` completes for the active rows only.""" + + +@runtime_checkable +class SegmentPostPolicyMetadataPort(Protocol): + """Optional result trace supplied by a segment post-policy port.""" + + def post_policy_metadata( + self, + policy: Any, + *, + segment: Any, + ) -> Mapping[str, Any]: + """Return JSON-safe metadata after one policy has run.""" + + +@runtime_checkable +class SegmentPostPolicyResultPort(Protocol): + """Optional row-local success result supplied by a post-policy port.""" + + def post_policy_result(self, policy: Any, *, segment: Any) -> Any: + """Return one boolean or one boolean per environment row.""" + + +@runtime_checkable +class SegmentValidatorPort(Protocol): + """Environment-aware boundary for compiled program validators.""" + + def validate_validator( + self, + validator: Any, + *, + segment: Any, + ) -> None: + """Validate one compiled validator without observing the environment.""" + + def validate(self, validator: Any, *, segment: Any) -> Any: + """Return one boolean or one boolean per environment row.""" + + +@runtime_checkable +class SegmentValidatorMetadataPort(Protocol): + """Optional result trace supplied by a segment validator port.""" + + def validator_metadata( + self, + validator: Any, + *, + segment: Any, + ) -> Mapping[str, Any]: + """Return JSON-safe metadata after one validator has run.""" + + +class GymPlanningObservationProvider: + """Callback-backed observation port that also exposes the latest qpos. + + Args: + capture: Callback accepting verified :class:`TaskState` and returning + one fresh :class:`PlanningContext` from the Gym environment. + + The callback is intentionally explicit: environment-specific scene, + simulator, and registry access remains in environment integration code. + """ + + def __init__(self, capture: Callable[[TaskState], PlanningContext]) -> None: + if not callable(capture): + raise TypeError("capture must be callable.") + self._capture = capture + self._latest: PlanningContext | None = None + + @property + def latest(self) -> PlanningContext | None: + """Return the latest immutable planning context, if one was captured.""" + return self._latest + + def observe(self, task_state: TaskState) -> PlanningContext: + """Capture and retain one fresh planning context.""" + if not isinstance(task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + context = self._capture(task_state) + if not isinstance(context, PlanningContext): + raise TypeError("capture must return a PlanningContext.") + self._latest = context + return context + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + """Return latest full qpos rows in the requested stable-ID order.""" + context = self._latest + if context is None: + raise RuntimeError("No planning context has been observed yet.") + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.dim() != 1 or env_ids.numel() == 0: + raise ValueError("env_ids must be a non-empty one-dimensional long tensor.") + if env_ids.device != context.env_ids.device: + raise ValueError("env_ids must share the latest context device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + + row_by_id = { + int(env_id): row + for row, env_id in enumerate(context.env_ids.detach().cpu().tolist()) + } + try: + rows = [ + row_by_id[int(env_id)] for env_id in env_ids.detach().cpu().tolist() + ] + except KeyError as exc: + raise ValueError( + f"Environment ID {int(exc.args[0])} is absent from the latest context." + ) from exc + return context.robot.qpos[rows].clone() + + +class EnvironmentStepClock(ExecutionClock): + """Monotonic execution clock advanced only by explicit Gym steps. + + ``sleep`` intentionally raises. Calling synchronous ``SkillRuntime.run`` + with this clock would otherwise advance execution without an environment + transition. Demo integrations must use the nonblocking ``start``/``step`` + path and call :meth:`advance_after_env_step` only after a yielded action was + passed to ``env.step``. + """ + + def __init__(self, step_dt: float, *, initial_step: int = 0) -> None: + if not isinstance(step_dt, (int, float)) or isinstance(step_dt, bool): + raise TypeError("step_dt must be a real number.") + if not math.isfinite(float(step_dt)) or float(step_dt) <= 0.0: + raise ValueError("step_dt must be finite and positive.") + if type(initial_step) is not int or initial_step < 0: + raise ValueError("initial_step must be a non-negative integer.") + self._step_dt = float(step_dt) + self._step_index = initial_step + + @property + def step_dt(self) -> float: + """Return the authoritative Gym control cadence.""" + return self._step_dt + + @property + def step_index(self) -> int: + """Return the number of explicitly acknowledged environment steps.""" + return self._step_index + + def now(self) -> float: + """Return deterministic environment time in seconds.""" + return self._step_index * self._step_dt + + def sleep(self, duration: float) -> None: + """Reject implicit waiting that is not backed by ``env.step``.""" + self.steps_for_duration(duration, field_name="sleep duration") + raise RuntimeError( + "EnvironmentStepClock cannot sleep or advance implicitly; use the " + "nonblocking runtime and advance_after_env_step() after env.step()." + ) + + def steps_for_duration( + self, + duration: float, + *, + field_name: str = "duration", + ) -> int: + """Return an exact integer-grid representation of ``duration``. + + Float32 command tensors receive a small ratio-space tolerance, but an + incompatible cadence is never rounded or resampled. + """ + if not isinstance(duration, (int, float)) or isinstance(duration, bool): + raise TypeError(f"{field_name} must be a real number.") + duration = float(duration) + if not math.isfinite(duration) or duration < 0.0: + raise ValueError(f"{field_name} must be finite and non-negative.") + ratio = duration / self._step_dt + nearest = round(ratio) + tolerance = max(1.0e-6, abs(ratio) * 1.0e-6) + if not math.isclose(ratio, nearest, rel_tol=0.0, abs_tol=tolerance): + raise EnvironmentStepTimingError( + f"{field_name}={duration:.9g}s is not an integer multiple of " + f"step_dt={self._step_dt:.9g}s; explicit resampling is not supported." + ) + return int(nearest) + + def validate_frame(self, frame: RuntimeCommandFrame) -> None: + """Validate every row's command hold duration against the step grid.""" + if not isinstance(frame, RuntimeCommandFrame): + raise TypeError("frame must be a RuntimeCommandFrame.") + for row, duration in enumerate(frame.hold_duration.detach().cpu().tolist()): + self.steps_for_duration( + float(duration), + field_name=f"RuntimeCommandFrame.hold_duration[{row}]", + ) + + def advance_after_env_step(self, steps: int = 1) -> None: + """Advance time after ``steps`` completed Gym environment transitions.""" + if type(steps) is not int or steps <= 0: + raise ValueError("steps must be a positive integer.") + self._step_index += steps + + +class JointPositionGymTransportEncoder: + """Built-in ``robot.joint_position`` to full-qpos action encoder.""" + + @property + def transport_id(self) -> str: + """Return the built-in joint-position transport ID.""" + return JointPositionTarget.TRANSPORT_ID + + def encode( + self, + command: EndpointCommand, + *, + base_action: EnvAction, + active_mask: torch.Tensor, + ) -> EnvAction: + """Write addressed joints while holding every other qpos column.""" + if not isinstance(command.target, JointPositionTarget): + raise TypeError("Joint-position transport requires JointPositionTarget.") + if not isinstance(command.payload, JointPositionPayload): + raise TypeError("Joint-position transport requires JointPositionPayload.") + if not isinstance(base_action, torch.Tensor): + raise TypeError( + "The built-in joint-position encoder requires a tensor base action; " + "register structured transports after it or provide a compatible " + "custom composition encoder." + ) + if base_action.dim() != 2 or base_action.shape[0] != command.batch_size: + raise ValueError( + "The full-qpos base action must have shape (batch_size, robot_dof)." + ) + if active_mask.dtype != torch.bool or active_mask.shape != ( + command.batch_size, + ): + raise ValueError("active_mask must be bool with one value per command row.") + if active_mask.device != base_action.device: + raise ValueError("active_mask and base_action must share a device.") + joint_ids = command.target.joint_ids + if max(joint_ids) >= base_action.shape[1]: + raise ValueError( + f"Joint ID {max(joint_ids)} exceeds full qpos width " + f"{base_action.shape[1]}." + ) + positions = command.payload.positions + if positions.device != base_action.device: + raise ValueError("Joint payload and base action must share a device.") + if not base_action.is_floating_point(): + raise TypeError("The full-qpos base action must be floating point.") + + action = base_action.clone() + columns = torch.tensor(joint_ids, dtype=torch.long, device=action.device) + selected = action.index_select(1, columns) + selected[active_mask] = positions[active_mask].to(dtype=action.dtype) + action[:, columns] = selected + return action + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: EnvAction, + context: PlanningContext, + ) -> EnvAction: + """Keep observed full qpos unchanged for addressed joint targets.""" + del context + if not all(isinstance(target, JointPositionTarget) for target in targets): + raise TypeError("Joint-position hold received an incompatible target.") + return base_action.clone() + + +class RuntimeCommandFrameEncoder: + """Encode transport-neutral command frames to controller-ready Gym actions. + + Args: + qpos_provider: Full-qpos source aligned to a frame's explicit ``env_ids``. + transports: Optional additional transport encoders. The built-in + joint-position encoder is always installed first. + """ + + def __init__( + self, + qpos_provider: CurrentQposProvider, + *, + transports: Iterable[RuntimeTransportActionEncoder] = (), + ) -> None: + if not isinstance(qpos_provider, CurrentQposProvider): + raise TypeError("qpos_provider must implement CurrentQposProvider.") + self._qpos_provider = qpos_provider + self._transports: dict[str, RuntimeTransportActionEncoder] = {} + self.register_transport(JointPositionGymTransportEncoder()) + for transport in transports: + self.register_transport(transport) + + @property + def transport_ids(self) -> tuple[str, ...]: + """Return registered transport IDs in deterministic encoding order.""" + return tuple(self._transports) + + def register_transport( + self, + transport: RuntimeTransportActionEncoder, + *, + replace: bool = False, + ) -> None: + """Register one shared transport-to-Gym action encoder.""" + if not isinstance(transport, RuntimeTransportActionEncoder): + raise TypeError("transport must implement RuntimeTransportActionEncoder.") + transport_id = _validate_identifier( + transport.transport_id, + field_name="RuntimeTransportActionEncoder.transport_id", + ) + if type(replace) is not bool: + raise TypeError("replace must be a bool.") + if transport_id in self._transports and not replace: + raise ValueError(f"Transport {transport_id!r} is already registered.") + self._transports[transport_id] = transport + + def _base_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + """Capture and validate one owned full-qpos hold action.""" + qpos = self._qpos_provider.current_qpos(env_ids) + if not isinstance(qpos, torch.Tensor): + raise TypeError("CurrentQposProvider.current_qpos() must return a tensor.") + if qpos.dim() != 2 or qpos.shape[0] != env_ids.shape[0] or qpos.shape[1] == 0: + raise ValueError( + "Current qpos must have shape (batch_size, robot_dof) with non-zero DOF." + ) + if qpos.device != env_ids.device: + raise ValueError("Current qpos and env_ids must share a device.") + if not qpos.is_floating_point() or not torch.isfinite(qpos).all().item(): + raise ValueError("Current qpos must contain finite floating-point values.") + return qpos.clone() + + def encode(self, frame: RuntimeCommandFrame) -> EnvAction: + """Encode one frame on top of a fresh full-qpos hold action.""" + if not isinstance(frame, RuntimeCommandFrame): + raise TypeError("frame must be a RuntimeCommandFrame.") + action: EnvAction = self._base_qpos(frame.env_ids) + for command in frame.commands: + transport = self._transports.get(command.transport_id) + if transport is None: + raise UnsupportedRuntimeTransportError( + f"No Gym action encoder is registered for runtime transport " + f"{command.transport_id!r}." + ) + action = transport.encode( + command, + base_action=action, + active_mask=frame.active_mask, + ) + return action + + def encode_hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + ) -> EnvAction: + """Encode an observed-position safe hold for addressed transports.""" + if not isinstance(context, PlanningContext): + raise TypeError("context must be a PlanningContext.") + action: EnvAction = context.robot.qpos.clone() + by_transport: dict[str, list[RuntimeEndpointTarget]] = {} + for target in targets: + if not isinstance(target, RuntimeEndpointTarget): + raise TypeError("targets must contain RuntimeEndpointTarget values.") + by_transport.setdefault(target.transport_id, []).append(target) + for transport_id, grouped in by_transport.items(): + transport = self._transports.get(transport_id) + if transport is None: + raise UnsupportedRuntimeTransportError( + f"No Gym action encoder is registered for runtime transport " + f"{transport_id!r}." + ) + action = transport.hold( + tuple(grouped), + base_action=action, + context=context, + ) + return action + + def encode_idle_hold(self, env_ids: torch.Tensor) -> EnvAction: + """Return a fresh full-qpos hold when no transport was armed yet.""" + return self._base_qpos(env_ids) + + +@dataclass(frozen=True, slots=True) +class _BufferedAction: + """One owned action plus command-boundary provenance.""" + + action: ProcessedEnvAction + + def snapshot(self) -> _BufferedAction: + """Return one independently owned buffered action.""" + return _BufferedAction(self.action.snapshot()) + + +class BufferedGymCommandSink: + """Runner command sink that buffers actions for the Gym demo generator. + + Acceptance means the command was validated and copied into the local + buffer; it does not claim that an environment transition already occurred. + """ + + def __init__( + self, + encoder: RuntimeCommandFrameEncoder, + clock: EnvironmentStepClock, + *, + accepted_command_observer: AcceptedRuntimeCommandObserver | None = None, + ) -> None: + if not isinstance(encoder, RuntimeCommandFrameEncoder): + raise TypeError("encoder must be a RuntimeCommandFrameEncoder.") + if not isinstance(clock, EnvironmentStepClock): + raise TypeError("clock must be an EnvironmentStepClock.") + if accepted_command_observer is not None and not isinstance( + accepted_command_observer, + AcceptedRuntimeCommandObserver, + ): + raise TypeError( + "accepted_command_observer must implement " + "AcceptedRuntimeCommandObserver or be None." + ) + self._encoder = encoder + self._clock = clock + self._accepted_command_observer = accepted_command_observer + self._pending: deque[_BufferedAction] = deque() + self._last_emitted: ProcessedEnvAction | None = None + self._accepted_action_count = 0 + + @property + def clock(self) -> EnvironmentStepClock: + """Return the exact environment-step clock used for timing checks.""" + return self._clock + + @property + def pending_count(self) -> int: + """Return the number of accepted actions not yet yielded to Gym.""" + return len(self._pending) + + @property + def accepted_action_count(self) -> int: + """Return the monotonic count of actions accepted by this sink.""" + return self._accepted_action_count + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Validate, encode, and buffer one runtime command frame.""" + _validate_timeout(timeout) + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") + self._clock.validate_frame(command) + action = self._encoder.encode(command) + metadata = { + "bridge_action_kind": "runtime_command", + "runtime_destinations": [ + [item.transport_id, item.target.target_id] for item in command.commands + ], + "active_mask": command.active_mask.detach().cpu().tolist(), + "hold_duration": command.hold_duration.detach().cpu().tolist(), + } + self._pending.append( + _BufferedAction(ProcessedEnvAction(value=action, metadata=metadata)) + ) + observer = self._accepted_command_observer + if observer is not None: + try: + observer.accepted(command.snapshot()) + except Exception: + self._pending.clear() + self._discard_observer_state() + raise + self._accepted_action_count += 1 + return CommandAcknowledgement.accepted_ack("Buffered for the Gym step loop.") + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Buffer one observed-position safe hold action.""" + _validate_timeout(timeout) + action = self._encoder.encode_hold(tuple(targets), context) + metadata = { + "bridge_action_kind": "runtime_safe_hold", + "runtime_destinations": [ + [target.transport_id, target.target_id] for target in targets + ], + } + self._pending.append( + _BufferedAction(ProcessedEnvAction(value=action, metadata=metadata)) + ) + self._accepted_action_count += 1 + return CommandAcknowledgement.accepted_ack("Safe hold buffered for Gym.") + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Discard accepted-but-not-yielded frames before a safe-stop hold.""" + _validate_timeout(timeout) + if not all(isinstance(target, RuntimeEndpointTarget) for target in targets): + raise TypeError("targets must contain RuntimeEndpointTarget values.") + self._pending.clear() + observer = self._accepted_command_observer + if observer is not None: + try: + observer.cancelled(tuple(target.snapshot() for target in targets)) + except Exception: + self._discard_observer_state() + raise + return CommandAcknowledgement.accepted_ack("Buffered commands cancelled.") + + def discard_pending(self) -> None: + """Discard actions that were accepted locally but never yielded.""" + self._pending.clear() + self._discard_observer_state() + + def drain_safe_stop_action( + self, + *, + fallback: ProcessedEnvAction | None = None, + ) -> ProcessedEnvAction | None: + """Select one buffered safe hold and discard every other local action. + + This method is used only by the demo abort handshake. A runtime + acknowledgement proves local buffering, not ``env.step`` consumption; + therefore an interrupted generator must explicitly surface the final + safe hold to the executor while dropping stale motion commands. + """ + candidates: list[ProcessedEnvAction] = [] + for candidate in (self._last_emitted, fallback): + if ( + candidate is not None + and candidate.metadata.get("bridge_action_kind") + in _SAFE_HOLD_ACTION_KINDS + ): + candidates.append(candidate.snapshot()) + while self._pending: + candidate = self._pending.popleft().action + if candidate.metadata.get("bridge_action_kind") in _SAFE_HOLD_ACTION_KINDS: + candidates.append(candidate.snapshot()) + self._discard_observer_state() + return None if not candidates else candidates[-1].snapshot() + + def _discard_observer_state(self) -> None: + """Reset observer state after any fail-closed local discard.""" + observer = self._accepted_command_observer + if observer is not None: + observer.discarded() + + def pop(self) -> ProcessedEnvAction: + """Pop the next accepted action and remember it as the active hold.""" + if not self._pending: + raise RuntimeError("No buffered Gym command is available.") + action = self._pending.popleft().action.snapshot() + self._last_emitted = action.snapshot() + return action + + def wait_hold(self, env_ids: torch.Tensor) -> ProcessedEnvAction: + """Return an owned hold action for one runtime waiting step.""" + if self._last_emitted is None: + value = self._encoder.encode_idle_hold(env_ids) + else: + value = self._last_emitted.value + return ProcessedEnvAction( + value=value, + metadata={"bridge_action_kind": "runtime_wait_hold"}, + ) + + +@dataclass(slots=True) +class _SegmentLifecycle: + """Mutable state shared by one lazy action generator and validator.""" + + complete: bool = False + result: SkillResult | ParallelSkillResult | None = None + validation: torch.Tensor | None = None + runtime: SequentialSkillRuntimePort | ParallelSkillRuntime | None = None + pending_action: ProcessedEnvAction | None = None + actions_started: bool = False + sink_acceptance_baseline: int | None = None + yielded_action_count: int = 0 + abort_started: bool = False + abort_complete: bool = False + metadata: dict[str, Any] = field(default_factory=dict) + post_policy_success: torch.Tensor | None = None + + +def _validate_runtime_result( + result: SkillResult | ParallelSkillResult, +) -> SkillResult | ParallelSkillResult: + """Validate one exact sequential or parallel runtime boundary.""" + if not isinstance(result, (SkillResult, ParallelSkillResult)): + raise TypeError( + "Runtime methods must return SkillResult or ParallelSkillResult values." + ) + return result + + +def _normalize_validation( + value: Any, + *, + batch_size: int, + device: torch.device, +) -> torch.Tensor: + """Normalize one validator output to an owned row-local boolean tensor.""" + tensor = torch.as_tensor(value, dtype=torch.bool, device=device).reshape(-1) + if tensor.numel() == 1 and batch_size > 1: + tensor = tensor.repeat(batch_size) + if tensor.numel() != batch_size: + raise ValueError( + f"Segment validator returned {tensor.numel()} flags, expected " + f"{batch_size}." + ) + return tensor.clone() + + +def _json_safe_copy(value: Any, *, field_name: str) -> Any: + """Return an owned JSON value while rejecting lossy coercions.""" + if value is None or type(value) in {bool, int, str}: + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{field_name} contains a non-finite float.") + return value + if isinstance(value, Mapping): + result: dict[str, Any] = {} + for key, item in value.items(): + if type(key) is not str or not key or key != key.strip(): + raise ValueError( + f"{field_name} mapping keys must be non-empty strings " + "without outer whitespace." + ) + result[key] = _json_safe_copy( + item, + field_name=f"{field_name}.{key}", + ) + return result + if isinstance(value, (list, tuple)): + return [ + _json_safe_copy(item, field_name=f"{field_name}[{index}]") + for index, item in enumerate(value) + ] + raise TypeError(f"{field_name} contains non-JSON value {type(value).__name__}.") + + +def _runtime_result_metadata( + result: SkillResult | ParallelSkillResult, +) -> dict[str, Any]: + """Snapshot one core runtime result through its canonical serializer.""" + serializer = getattr(result, "to_metadata", None) + if not callable(serializer): + raise TypeError( + f"{type(result).__name__} must provide to_metadata() for demo tracing." + ) + metadata = _json_safe_copy(serializer(), field_name="runtime result metadata") + if not isinstance(metadata, dict): + raise TypeError("Runtime result to_metadata() must return a mapping.") + return metadata + + +class AtomicDemoBridge: + """Adapt sequential compiled program segments to lazy Gym demonstrations. + + Args: + program: Provider-free compiled Expert Program. + runtime: Nonblocking semantic :class:`SkillRuntime` surface. + command_sink: The same buffered sink installed in ``runtime``. + clock: The same environment-step clock installed in ``runtime``. + post_policy_port: Optional environment-aware post-policy executor. + validator_port: Optional environment-aware validator executor. + parallel_safety_validator: Optional authoritative physical-safety gate + required before any parallel branch can start. + + Schema-v2 parallel blocks retain their branch lanes and explicit barrier. + They are lowered through :class:`ParallelSkillRuntime`; they are never + flattened into a sequential semantic-call list. + """ + + def __init__( + self, + program: CompiledProgramPort, + runtime: SequentialSkillRuntimePort, + command_sink: BufferedGymCommandSink, + clock: EnvironmentStepClock, + *, + post_policy_port: SegmentPostPolicyPort | None = None, + validator_port: SegmentValidatorPort | None = None, + parallel_safety_validator: ParallelCommandSafetyValidator | None = None, + ) -> None: + if not isinstance(program, CompiledProgramPort): + raise TypeError("program must implement CompiledProgramPort.") + _validate_identifier(program.program_id, field_name="program.program_id") + if type(program.schema_version) is not int or program.schema_version < 1: + raise ValueError("program.schema_version must be a positive integer.") + if not isinstance(runtime, SequentialSkillRuntimePort): + raise TypeError("runtime must implement SequentialSkillRuntimePort.") + if not isinstance(command_sink, BufferedGymCommandSink): + raise TypeError("command_sink must be a BufferedGymCommandSink.") + if not isinstance(clock, EnvironmentStepClock): + raise TypeError("clock must be an EnvironmentStepClock.") + if command_sink.clock is not clock: + raise ValueError("command_sink and bridge must share the exact clock.") + if post_policy_port is not None and not isinstance( + post_policy_port, SegmentPostPolicyPort + ): + raise TypeError("post_policy_port must implement SegmentPostPolicyPort.") + if validator_port is not None and not isinstance( + validator_port, SegmentValidatorPort + ): + raise TypeError("validator_port must implement SegmentValidatorPort.") + if parallel_safety_validator is not None and not isinstance( + parallel_safety_validator, ParallelCommandSafetyValidator + ): + raise TypeError( + "parallel_safety_validator must implement " + "ParallelCommandSafetyValidator." + ) + self._program = program + self._runtime = runtime + self._sink = command_sink + self._clock = clock + self._post_policy_port = post_policy_port + self._validator_port = validator_port + self._parallel_safety_validator = parallel_safety_validator + self._active_segment_id: str | None = None + self._eligible_mask: torch.Tensor | None = None + + @property + def clock(self) -> EnvironmentStepClock: + """Return the environment-step clock used by this bridge.""" + return self._clock + + def iter_segments(self) -> Iterator[DemoSegment]: + """Lazily adapt compiled program segments to ``DemoSegment`` values. + + Consumers must exhaust each segment's actions and invoke its validator + before requesting the next segment. Skipping either lifecycle boundary + raises :class:`DemoBridgeError` instead of silently carrying stale row + eligibility into downstream execution. + """ + for segment in self._program.iter_segments(): + metadata = self._segment_metadata(segment) + lifecycle = _SegmentLifecycle(metadata=metadata) + validator = self._segment_validator(segment, lifecycle) + yield DemoSegment( + actions=self._segment_actions(segment, lifecycle), + name=segment.name, + metadata=metadata, + validator=validator, + abort_actions=self._segment_abort_actions(segment, lifecycle), + failure_policy="row_independent", + ) + self._require_consumed_segment_lifecycle(segment, lifecycle) + + def __iter__(self) -> Iterator[DemoSegment]: + """Delegate iteration to :meth:`iter_segments`.""" + return self.iter_segments() + + @staticmethod + def _require_consumed_segment_lifecycle( + segment: Any, + lifecycle: _SegmentLifecycle, + ) -> None: + """Reject advancing past a segment with an unconsumed lifecycle. + + The public demo executor exhausts ``actions`` and then invokes the + segment validator before requesting the next lazy segment. Direct + bridge consumers must preserve the same ordering because validation is + also the commit point for runtime and post-policy row eligibility. + """ + if not lifecycle.complete: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} actions must be exhausted before " + "requesting the next compiled segment." + ) + if lifecycle.validation is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} validator must be called after " + "its actions are exhausted and before requesting the next " + "compiled segment." + ) + + def _segment_metadata(self, segment: Any) -> dict[str, Any]: + """Build mutable JSON-safe metadata completed at lifecycle boundaries.""" + return { + "expert_program_schema_version": self._program.schema_version, + "expert_program_id": self._program.program_id, + "program_segment_id": segment.segment_id, + "program_segment_index": segment.segment_index, + "program_segment_source_path": list(segment.source_path), + "program_segment_implicit": bool(segment.implicit), + "semantic_call_indices": [call.call_index for call in segment.calls], + "post_policy_count": len(segment.post_policies), + "validator_count": len(segment.validators), + "parallel": getattr(segment, "parallel_block", None) is not None, + "runtime": None, + "post_policies": [], + "validation": None, + } + + @staticmethod + def _record_runtime_result( + lifecycle: _SegmentLifecycle, + result: SkillResult | ParallelSkillResult, + ) -> None: + """Snapshot one runtime boundary into its owning segment metadata.""" + lifecycle.result = result + lifecycle.metadata["runtime"] = _runtime_result_metadata(result) + + def _decorate_action( + self, + action: Any, + *, + segment: Any, + result: SkillResult | ParallelSkillResult, + action_kind: str | None = None, + ) -> ProcessedEnvAction: + """Own one action and attach stable program/runtime provenance.""" + if isinstance(action, ProcessedEnvAction): + value = action.value + metadata = dict(action.metadata) + else: + value = action + metadata = {} + if action_kind is not None: + metadata["bridge_action_kind"] = action_kind + metadata.update( + { + "expert_program_id": self._program.program_id, + "program_segment_id": segment.segment_id, + "program_segment_index": segment.segment_index, + "environment_step": self._clock.step_index, + "runtime_status": result.status.value, + "runtime_call_index": getattr(result, "current_call_index", None), + } + ) + return ProcessedEnvAction(value=value, metadata=metadata) + + def _yield_and_advance( + self, + action: ProcessedEnvAction, + lifecycle: _SegmentLifecycle, + ) -> Iterator[ProcessedEnvAction]: + """Yield once and advance only after explicit consumption acknowledgement.""" + if lifecycle.pending_action is not None: + raise RuntimeError("A prior demo action is still awaiting acknowledgement.") + lifecycle.pending_action = action.snapshot() + lifecycle.yielded_action_count += 1 + yield action + if lifecycle.pending_action is not None: + self._clock.advance_after_env_step() + lifecycle.pending_action = None + + def _segment_actions( + self, + segment: Any, + lifecycle: _SegmentLifecycle, + ) -> Iterator[ProcessedEnvAction]: + """Drive one semantic segment without bypassing the Gym step loop.""" + segment_id = segment.segment_id + lifecycle.actions_started = True + lifecycle.sink_acceptance_baseline = self._sink.accepted_action_count + if self._active_segment_id is not None: + raise RuntimeError( + f"Segment {self._active_segment_id!r} is still active; exhaust or " + "close it before starting another lazy segment." + ) + self._active_segment_id = segment_id + result: SkillResult | ParallelSkillResult | None = None + segment_runtime: SequentialSkillRuntimePort | ParallelSkillRuntime = ( + self._runtime + ) + is_parallel = getattr(segment, "parallel_block", None) is not None + try: + if is_parallel: + segment_runtime = self._parallel_runtime(segment) + lifecycle.runtime = segment_runtime + result = _validate_runtime_result( + segment_runtime.start( + workflow_id=f"{self._program.program_id}/{segment_id}", + eligible_mask=self._eligible_mask, + ) + ) + else: + lifecycle.runtime = segment_runtime + analysis = self._program.sequential_execution_analysis( + segment.segment_index + ) + calls = tuple(analysis.calls) + if not calls: + raise DemoBridgeError( + f"Compiled segment {segment_id!r} contains no semantic calls." + ) + execution_prefix_length = analysis.execution_prefix_length + if execution_prefix_length != len(segment.calls): + raise DemoBridgeError( + f"Compiled segment {segment_id!r} analysis prefix length " + "does not match its owned semantic calls." + ) + result = _validate_runtime_result( + segment_runtime.start( + calls, + workflow_id=f"{self._program.program_id}/{segment_id}", + eligible_mask=self._eligible_mask, + execution_prefix_length=execution_prefix_length, + ) + ) + + while True: + emitted = False + while self._sink.pending_count: + action = self._decorate_action( + self._sink.pop(), + segment=segment, + result=result, + ) + yield from self._yield_and_advance(action, lifecycle) + emitted = True + + if emitted and not result.terminal: + # The result's wait duration was measured before the action + # just consumed by Gym. Refresh it against the advanced + # environment clock before deciding whether another hold is due. + result = _validate_runtime_result(segment_runtime.step()) + continue + + if result.terminal: + break + + if result.wait_duration > 0.0: + self._clock.steps_for_duration( + result.wait_duration, + field_name="SkillResult.wait_duration", + ) + hold = self._decorate_action( + self._sink.wait_hold(result.env_ids), + segment=segment, + result=result, + action_kind="runtime_wait_hold", + ) + yield from self._yield_and_advance(hold, lifecycle) + + result = _validate_runtime_result(segment_runtime.step()) + + self._record_runtime_result(lifecycle, result) + self._retain_eligible_rows(result.success_mask) + if is_parallel: + self._runtime.adopt_verified_task_state(result.task_state) + if result.status is SkillStatus.COMPLETED: + yield from self._post_policy_actions(segment, result, lifecycle) + lifecycle.complete = True + finally: + if not lifecycle.abort_started and lifecycle.pending_action is not None: + if result is not None and not result.terminal: + segment_runtime.cancel( + f"Demo segment {segment_id!r} action iteration stopped early." + ) + raise DemoBridgeError( + f"Demo segment {segment_id!r} was closed with an unacknowledged " + "action. Consume DemoSegment.abort_actions through env.step() " + "before closing the action iterator." + ) + self._active_segment_id = None + + def _segment_abort_actions( + self, + segment: Any, + lifecycle: _SegmentLifecycle, + ) -> Callable[..., Iterator[ProcessedEnvAction]]: + """Create the explicit executor-to-runtime cancellation handshake.""" + + def abort( + reason: str, + *, + last_action_consumed: bool, + ) -> Iterator[ProcessedEnvAction]: + return self._abort_segment( + segment, + lifecycle, + reason=reason, + last_action_consumed=last_action_consumed, + ) + + return abort + + def _abort_segment( + self, + segment: Any, + lifecycle: _SegmentLifecycle, + *, + reason: str, + last_action_consumed: bool, + ) -> Iterator[ProcessedEnvAction]: + """Abort one segment, surfacing a safe hold only after controller activity.""" + if type(reason) is not str or not reason: + raise ValueError("abort reason must be a non-empty string.") + if type(last_action_consumed) is not bool: + raise TypeError("last_action_consumed must be a bool.") + if lifecycle.abort_started: + raise RuntimeError( + f"Segment {segment.segment_id!r} abort handshake already started." + ) + if not lifecycle.actions_started: + raise RuntimeError( + f"Segment {segment.segment_id!r} has no started action iteration " + "to abort." + ) + baseline = lifecycle.sink_acceptance_baseline + if baseline is None: + raise RuntimeError( + f"Segment {segment.segment_id!r} has no sink lifecycle baseline." + ) + controller_activity_started = ( + lifecycle.yielded_action_count > 0 + or lifecycle.pending_action is not None + or self._sink.accepted_action_count > baseline + ) + if not controller_activity_started: + # Runtime construction and preflight are deliberately observation- and + # command-free. If either fails before the first accepted or yielded + # action, there is no physical controller state to safe-stop. Mark the + # handshake complete without touching the partially constructed runtime + # so the original action-generation exception remains authoritative. + lifecycle.abort_started = True + lifecycle.abort_complete = True + return + runtime = lifecycle.runtime + pending = lifecycle.pending_action + if runtime is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} accepted or yielded a controller " + "action without retaining a runtime capable of strict safe-stop." + ) + lifecycle.abort_started = True + if pending is not None: + pending = pending.snapshot() + if pending is not None and last_action_consumed: + self._clock.advance_after_env_step() + lifecycle.pending_action = None + + result = _validate_runtime_result(runtime.result) + if not result.terminal: + result = _validate_runtime_result(runtime.cancel(reason)) + self._record_runtime_result(lifecycle, result) + + pending_kind = ( + None if pending is None else pending.metadata.get("bridge_action_kind") + ) + if ( + pending is not None + and last_action_consumed + and pending_kind in _SAFE_HOLD_ACTION_KINDS + ): + self._sink.discard_pending() + lifecycle.abort_complete = True + return + + safe_action = self._sink.drain_safe_stop_action( + fallback=None if last_action_consumed else pending, + ) + if safe_action is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} stopped before exhaustion, but " + "no controller safe-hold action was available for env.step()." + ) + processed = self._decorate_action( + safe_action, + segment=segment, + result=result, + action_kind="runtime_abort_safe_hold", + ) + yield processed + self._clock.advance_after_env_step() + lifecycle.abort_complete = True + + def _parallel_runtime(self, segment: Any) -> ParallelSkillRuntime: + """Build one one-shot coordinator from a compiled explicit barrier.""" + if self._parallel_safety_validator is None: + raise DemoBridgeError( + f"Parallel segment {segment.segment_id!r} requires an explicit " + "ParallelCommandSafetyValidator; resource claims alone do not " + "establish physical collision safety." + ) + if not isinstance(self._runtime, SkillRuntime): + # Production integration always supplies SkillRuntime. Keeping the + # sequential protocol permits lightweight tests and alternate + # frontends, but the canonical parallel factory requires forkable + # runtime internals by design. + raise TypeError( + "Parallel compiled segments require a concrete SkillRuntime " + "template." + ) + block = segment.parallel_block + branches = tuple(block.branches) + if len(branches) < 2: + raise DemoBridgeError( + f"Parallel segment {segment.segment_id!r} requires at least two " + "compiled branches." + ) + branch_calls = { + f"branch_{branch.branch_index}": tuple( + compiled.call for compiled in branch.calls + ) + for branch in branches + } + branch_paths = { + f"branch_{branch.branch_index}": tuple( + getattr(branch, "source_path", segment.source_path) + ) + for branch in branches + } + if any(not calls for calls in branch_calls.values()): + raise DemoBridgeError( + f"Parallel segment {segment.segment_id!r} contains an empty branch." + ) + barrier = block.barrier + return ParallelSkillRuntime.from_template( + self._runtime, + branch_calls, + self._sink, + ParallelTimingPolicy(self._clock.step_dt), + self._parallel_safety_validator, + timeout_steps=barrier.timeout_steps, + failure_policy=barrier.failure_policy, + workflow_id=( + f"{self._program.program_id}/{segment.segment_id}:parallel_analysis" + ), + branch_paths=branch_paths, + ) + + def _retain_eligible_rows(self, accepted: torch.Tensor) -> None: + """Permanently remove failed rows before a later lazy segment starts.""" + if not isinstance(accepted, torch.Tensor): + raise TypeError("accepted must be a torch.Tensor.") + if accepted.dtype != torch.bool or accepted.dim() != 1: + raise ValueError("accepted must be a one-dimensional bool tensor.") + if self._eligible_mask is None: + self._eligible_mask = torch.ones_like(accepted) + elif ( + self._eligible_mask.shape != accepted.shape + or self._eligible_mask.device != accepted.device + ): + raise ValueError("Environment rows changed across program segments.") + self._eligible_mask &= accepted + + def _post_policy_actions( + self, + segment: Any, + result: SkillResult | ParallelSkillResult, + lifecycle: _SegmentLifecycle, + ) -> Iterator[ProcessedEnvAction]: + """Route environment-aware post-policy actions through the same generator.""" + policies = tuple(segment.post_policies) + if policies and self._post_policy_port is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} declares post-policies, but no " + "SegmentPostPolicyPort was installed." + ) + traces = lifecycle.metadata["post_policies"] + if not isinstance(traces, list): + raise TypeError("Segment post-policy metadata storage must be a list.") + for policy_index, policy in enumerate(policies): + assert self._post_policy_port is not None + active_mask = ( + result.success_mask.clone() + if self._eligible_mask is None + else self._eligible_mask.clone() + ) + if lifecycle.post_policy_success is not None: + active_mask &= lifecycle.post_policy_success + actions = self._post_policy_port.actions( + policy, + segment=segment, + active_mask=active_mask, + ) + if isinstance(actions, (str, bytes)): + raise TypeError("Post-policy actions must be an iterable of actions.") + action_iterator = iter(actions) + iteration_error: BaseException | None = None + try: + for action in action_iterator: + processed = self._decorate_action( + action, + segment=segment, + result=result, + action_kind="program_post_policy", + ) + yield from self._yield_and_advance(processed, lifecycle) + except BaseException as exc: + iteration_error = exc + raise + finally: + close = getattr(action_iterator, "close", None) + if callable(close): + close() + cfg = getattr(policy, "cfg", None) + trace: dict[str, Any] = { + "policy_index": policy_index, + "kind": getattr(cfg, "kind", type(policy).__name__), + "source_path": list(getattr(policy, "source_path", ())), + "result_mask": result.success_mask.detach().cpu().tolist(), + "result": None, + } + port = self._post_policy_port + policy_success = active_mask.clone() + if isinstance(port, SegmentPostPolicyResultPort): + try: + policy_success &= _normalize_validation( + port.post_policy_result(policy, segment=segment), + batch_size=result.env_ids.numel(), + device=result.env_ids.device, + ) + except Exception: + if iteration_error is None: + raise + if lifecycle.post_policy_success is None: + lifecycle.post_policy_success = policy_success.clone() + else: + lifecycle.post_policy_success &= policy_success + trace["result_mask"] = policy_success.detach().cpu().tolist() + if isinstance(port, SegmentPostPolicyMetadataPort): + try: + trace["result"] = port.post_policy_metadata( + policy, + segment=segment, + ) + except Exception: + if iteration_error is None: + raise + traces.append( + _json_safe_copy( + trace, + field_name=f"post-policy {policy_index} metadata", + ) + ) + + def _segment_validator( + self, + segment: Any, + lifecycle: _SegmentLifecycle, + ) -> Callable[[], torch.Tensor]: + """Create a demo-boundary validator including runtime row success.""" + + def validate() -> torch.Tensor: + if not lifecycle.complete or lifecycle.result is None: + raise RuntimeError( + f"Segment {segment.segment_id!r} cannot be validated before its " + "action iterable is exhausted." + ) + if lifecycle.validation is not None: + return lifecycle.validation.clone() + result = lifecycle.result + accepted = result.success_mask.clone() + runtime_success = result.success_mask.clone() + eligible_before = ( + torch.ones_like(accepted) + if self._eligible_mask is None + else self._eligible_mask.clone() + ) + if self._eligible_mask is not None: + accepted &= self._eligible_mask + if lifecycle.post_policy_success is not None: + accepted &= lifecycle.post_policy_success + validators = tuple(segment.validators) + if validators and self._validator_port is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} declares validators, but no " + "SegmentValidatorPort was installed." + ) + validator_traces: list[dict[str, Any]] = [] + for validator_index, validator in enumerate(validators): + assert self._validator_port is not None + value = self._validator_port.validate(validator, segment=segment) + validator_result = _normalize_validation( + value, + batch_size=result.env_ids.numel(), + device=result.env_ids.device, + ) + accepted &= validator_result + cfg = getattr(validator, "cfg", None) + trace: dict[str, Any] = { + "validator_index": validator_index, + "kind": getattr(cfg, "kind", type(validator).__name__), + "source_path": list(getattr(validator, "source_path", ())), + "result_mask": validator_result.detach().cpu().tolist(), + "result": None, + } + port = self._validator_port + if isinstance(port, SegmentValidatorMetadataPort): + trace["result"] = port.validator_metadata( + validator, + segment=segment, + ) + validator_traces.append( + _json_safe_copy( + trace, + field_name=f"validator {validator_index} metadata", + ) + ) + lifecycle.metadata["validation"] = _json_safe_copy( + { + "env_ids": result.env_ids.detach().cpu().tolist(), + "runtime_success_mask": runtime_success.detach().cpu().tolist(), + "eligible_mask_before_validation": eligible_before.detach() + .cpu() + .tolist(), + "post_policy_success_mask": ( + None + if lifecycle.post_policy_success is None + else lifecycle.post_policy_success.detach().cpu().tolist() + ), + "validators": validator_traces, + "accepted_mask": accepted.detach().cpu().tolist(), + }, + field_name="segment validation metadata", + ) + self._retain_eligible_rows(accepted) + lifecycle.validation = accepted.clone() + return accepted.clone() + + return validate + + +__all__ = [ + "AcceptedRuntimeCommandObserver", + "AtomicDemoBridge", + "BufferedGymCommandSink", + "CompiledProgramPort", + "CurrentQposProvider", + "DemoBridgeError", + "EnvironmentStepClock", + "EnvironmentStepTimingError", + "GymPlanningObservationProvider", + "JointPositionGymTransportEncoder", + "ParallelCommandSafetyValidator", + "RuntimeCommandFrameEncoder", + "RuntimeTransportActionEncoder", + "SegmentPostPolicyMetadataPort", + "SegmentPostPolicyPort", + "SegmentPostPolicyResultPort", + "SegmentValidatorMetadataPort", + "SegmentValidatorPort", + "SequentialSkillRuntimePort", + "UnsupportedRuntimeTransportError", +] diff --git a/embodichain/lab/gym/envs/expert_program/cfg.py b/embodichain/lab/gym/envs/expert_program/cfg.py new file mode 100644 index 000000000..1e9b43278 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/cfg.py @@ -0,0 +1,857 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Typed configuration values for declarative Expert Programs.""" + +from __future__ import annotations + +import math +import re +from dataclasses import MISSING, field +from typing import TypeAlias + +from embodichain.utils import configclass + +EXPERT_PROGRAM_SCHEMA_VERSION = 1 +"""Stable sequential Expert Program schema version.""" + +EXPERT_PROGRAM_SCHEMA_VERSION_V2 = 2 +"""Schema version adding deterministic ``Parallel`` and ``Barrier`` nodes.""" + +SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS = ( + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, +) +"""Exact schema revisions accepted by the strict decoder.""" + +MAX_REPEAT_COUNT = 1_000 +"""Maximum repeat count accepted by one Expert Program repeat node.""" + +MAX_EXPANDED_CALLS = 10_000 +"""Maximum statically expanded semantic calls in one Expert Program.""" + +MAX_PROGRAM_DEPTH = 64 +"""Maximum nesting depth of a supported Expert Program AST.""" + +MAX_PROGRAM_NODES = 10_000 +"""Maximum number of stored nodes in a supported Expert Program AST.""" + +MAX_DECLARATIVE_DEPTH = 32 +"""Maximum nesting depth of a registered-call declarative payload.""" + +MAX_DECLARATIVE_NODES = 10_000 +"""Maximum number of values in a registered-call declarative payload.""" + +_REGISTERED_CALL_ID_PATTERN = re.compile(r"[a-z][a-z0-9_]*(?:\.[a-z][a-z0-9_]*)+") +_ENV_TRAVERSAL_PATTERN = re.compile( + r"(?:\$?(?:env|environment)(?:\.[A-Za-z_][A-Za-z0-9_]*)+|" + r"\$\{(?:env|environment)(?:\.[A-Za-z_][A-Za-z0-9_]*)+\})" +) +_FORBIDDEN_DECLARATIVE_KEYS = frozenset( + { + "__import__", + "attribute_path", + "callable", + "environment_path", + "env_path", + "eval", + "exec", + "expression", + "getattr", + "import", + "module", + "python", + } +) + +DeclarativeCfgValue: TypeAlias = ( + None + | bool + | int + | float + | str + | tuple["DeclarativeCfgValue", ...] + | dict[str, "DeclarativeCfgValue"] +) +"""Executable-free value accepted by a registered semantic call config.""" + + +def _validate_identifier(value: object, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _validate_kind(value: object, *, expected: str, field_name: str) -> None: + """Require one exact discriminator value.""" + if type(value) is not str or value != expected: + raise ValueError(f"{field_name} must be exactly {expected!r}.") + + +def _validate_number(value: object, *, field_name: str) -> float: + """Return one finite number while rejecting bool values.""" + if type(value) not in (int, float): + raise TypeError(f"{field_name} must be an int or float.") + try: + normalized = float(value) + except OverflowError as error: + raise ValueError(f"{field_name} must be finite.") from error + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + return normalized + + +def _validate_resources(value: object, *, field_name: str) -> dict[str, str]: + """Own one strict slot-to-resource mapping.""" + if type(value) is not dict: + raise TypeError(f"{field_name} must be an exact dict.") + resources: dict[str, str] = {} + for slot_id, resource_id in value.items(): + resources[ + _validate_identifier(slot_id, field_name=f"{field_name} slot IDs") + ] = _validate_identifier( + resource_id, + field_name=f"{field_name} resource IDs", + ) + return resources + + +def _validate_declarative_string(value: str, *, path: str) -> str: + """Reject strings that request executable or environment traversal behavior.""" + stripped = value.strip() + lowered = stripped.lower() + forbidden_prefixes = ( + "__import__(", + "eval(", + "exec(", + "import ", + "from ", + ) + if lowered.startswith(forbidden_prefixes): + raise ValueError(f"{path} contains an executable import/eval expression.") + if _ENV_TRAVERSAL_PATTERN.fullmatch(stripped) is not None: + raise ValueError(f"{path} contains dotted environment attribute traversal.") + return value + + +def _snapshot_declarative_value( + value: object, + *, + path: str, + _active: set[int] | None = None, + _budget: list[int] | None = None, + _depth: int = 0, +) -> DeclarativeCfgValue: + """Validate and own one bounded executable-free declarative value.""" + active = set() if _active is None else _active + budget = [MAX_DECLARATIVE_NODES] if _budget is None else _budget + if _depth > MAX_DECLARATIVE_DEPTH: + raise ValueError( + f"{path} exceeds declarative depth limit {MAX_DECLARATIVE_DEPTH}." + ) + budget[0] -= 1 + if budget[0] < 0: + raise ValueError( + f"{path} exceeds declarative node limit {MAX_DECLARATIVE_NODES}." + ) + if value is None or type(value) in (bool, int): + return value # type: ignore[return-value] + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{path} contains a non-finite float.") + return value + if type(value) is str: + return _validate_declarative_string(value, path=path) + if type(value) in (list, tuple): + identity = id(value) + if identity in active: + raise ValueError(f"{path} contains a cyclic sequence.") + active.add(identity) + try: + return tuple( + _snapshot_declarative_value( + item, + path=f"{path}[{index}]", + _active=active, + _budget=budget, + _depth=_depth + 1, + ) + for index, item in enumerate(value) + ) + finally: + active.remove(identity) + if type(value) is dict: + identity = id(value) + if identity in active: + raise ValueError(f"{path} contains a cyclic mapping.") + active.add(identity) + try: + result: dict[str, DeclarativeCfgValue] = {} + for key, item in value.items(): + if type(key) is not str: + raise TypeError(f"{path} keys must be exact strings.") + if key.lower() in _FORBIDDEN_DECLARATIVE_KEYS: + raise ValueError( + f"{path}.{key} requests forbidden executable behavior." + ) + result[key] = _snapshot_declarative_value( + item, + path=f"{path}.{key}", + _active=active, + _budget=budget, + _depth=_depth + 1, + ) + return result + finally: + active.remove(identity) + raise TypeError( + f"{path} contains non-declarative {type(value).__name__}; callables, " + "classes, modules, tensors, and live objects are not allowed." + ) + + +@configclass +class ExpertProgramIntegrationCfg: + """Static integration references selected by one Expert Program.""" + + robot_profile: str = MISSING + scene_registry: str = MISSING + runtime_preset: str = MISSING + + def __post_init__(self) -> None: + """Validate stable integration identifiers.""" + _validate_identifier(self.robot_profile, field_name="robot_profile") + _validate_identifier(self.scene_registry, field_name="scene_registry") + _validate_identifier(self.runtime_preset, field_name="runtime_preset") + + +@configclass +class PoseCfg: + """One declarative Cartesian pose using a WXYZ quaternion.""" + + position: tuple[float, float, float] = MISSING + quaternion_wxyz: tuple[float, float, float, float] = MISSING + + def __post_init__(self) -> None: + """Validate pose shape, finiteness, and quaternion magnitude.""" + if type(self.position) not in (list, tuple) or len(self.position) != 3: + raise ValueError("position must contain exactly three numbers.") + if ( + type(self.quaternion_wxyz) not in (list, tuple) + or len(self.quaternion_wxyz) != 4 + ): + raise ValueError("quaternion_wxyz must contain exactly four numbers.") + position = tuple( + _validate_number(value, field_name=f"position[{index}]") + for index, value in enumerate(self.position) + ) + quaternion = tuple( + _validate_number(value, field_name=f"quaternion_wxyz[{index}]") + for index, value in enumerate(self.quaternion_wxyz) + ) + norm = math.sqrt(sum(value * value for value in quaternion)) + if norm <= 1.0e-12: + raise ValueError("quaternion_wxyz must have non-zero magnitude.") + self.position = position # type: ignore[assignment] + self.quaternion_wxyz = quaternion # type: ignore[assignment] + + +@configclass +class TargetRefCfg: + """Reference to one top-level typed target provider.""" + + target: str = MISSING + kind: str = "target_ref" + + def __post_init__(self) -> None: + """Validate the target identifier and discriminator.""" + _validate_identifier(self.target, field_name="target") + _validate_kind(self.kind, expected="target_ref", field_name="kind") + + +@configclass +class CyclicPoseTargetCfg: + """Finite pose values selected cyclically by the enclosing repeat index.""" + + values: tuple[PoseCfg, ...] = MISSING + kind: str = "cyclic_pose" + + def __post_init__(self) -> None: + """Validate a non-empty owned pose sequence.""" + _validate_kind(self.kind, expected="cyclic_pose", field_name="kind") + if type(self.values) not in (list, tuple) or not self.values: + raise ValueError("values must contain at least one PoseCfg.") + values = tuple(self.values) + if not all(type(value) is PoseCfg for value in values): + raise TypeError("values must contain exact PoseCfg values.") + self.values = values # type: ignore[assignment] + + +TargetCfg: TypeAlias = CyclicPoseTargetCfg + + +@configclass +class PickCfg: + """Declarative request to acquire one registered object.""" + + object: str = MISSING + grasp: str | None = None + resources: dict[str, str] = field(default_factory=dict) + kind: str = "pick" + + def __post_init__(self) -> None: + """Validate object, optional affordance, resources, and kind.""" + _validate_identifier(self.object, field_name="object") + if self.grasp is not None: + _validate_identifier(self.grasp, field_name="grasp") + self.resources = _validate_resources(self.resources, field_name="resources") + _validate_kind(self.kind, expected="pick", field_name="kind") + + +@configclass +class PlaceCfg: + """Declarative request to place one held object at one destination.""" + + object: str = MISSING + at: TargetRefCfg | None = None + on: str | None = None + inside: str | None = None + resources: dict[str, str] = field(default_factory=dict) + kind: str = "place" + + def __post_init__(self) -> None: + """Require exactly one typed destination.""" + _validate_identifier(self.object, field_name="object") + selected = sum(value is not None for value in (self.at, self.on, self.inside)) + if selected != 1: + raise ValueError("Place requires exactly one of at, on, or inside.") + if self.at is not None and type(self.at) is not TargetRefCfg: + raise TypeError("at must be exactly TargetRefCfg or None.") + if self.on is not None: + _validate_identifier(self.on, field_name="on") + if self.inside is not None: + _validate_identifier(self.inside, field_name="inside") + self.resources = _validate_resources(self.resources, field_name="resources") + _validate_kind(self.kind, expected="place", field_name="kind") + + +@configclass +class HandOverCfg: + """Declarative request to transfer one held object between resources.""" + + object: str = MISSING + receiver: str | None = None + final_target: TargetRefCfg | None = None + resources: dict[str, str] = field(default_factory=dict) + kind: str = "hand_over" + + def __post_init__(self) -> None: + """Validate object, destination resource, and optional target.""" + _validate_identifier(self.object, field_name="object") + if self.receiver is not None: + _validate_identifier(self.receiver, field_name="receiver") + if ( + self.final_target is not None + and type(self.final_target) is not TargetRefCfg + ): + raise TypeError("final_target must be exactly TargetRefCfg or None.") + resources = _validate_resources(self.resources, field_name="resources") + if self.receiver is not None: + selected = resources.get("destination") + if selected is not None and selected != self.receiver: + raise ValueError("receiver conflicts with resources['destination'].") + resources["destination"] = self.receiver + self.resources = resources + _validate_kind(self.kind, expected="hand_over", field_name="kind") + + +@configclass +class OperateArticulationCfg: + """Declarative request to operate one articulated joint through a handle.""" + + articulation: str = MISSING + handle: str | None = None + target: str | None = None + target_position: float | None = None + target_displacement: float | None = None + resources: dict[str, str] = field(default_factory=dict) + kind: str = "operate_articulation" + + def __post_init__(self) -> None: + """Require one named target or one complete explicit target pair.""" + _validate_identifier(self.articulation, field_name="articulation") + if self.handle is not None: + _validate_identifier(self.handle, field_name="handle") + named = self.target is not None + explicit_position = self.target_position is not None + explicit_displacement = self.target_displacement is not None + if named: + _validate_identifier(self.target, field_name="target") + if explicit_position or explicit_displacement: + raise ValueError( + "target is mutually exclusive with target_position and " + "target_displacement." + ) + elif not (explicit_position and explicit_displacement): + raise ValueError( + "OperateArticulation requires either target or the explicit " + "target_position and target_displacement pair." + ) + else: + self.target_position = _validate_number( + self.target_position, + field_name="target_position", + ) + self.target_displacement = _validate_number( + self.target_displacement, + field_name="target_displacement", + ) + self.resources = _validate_resources(self.resources, field_name="resources") + _validate_kind( + self.kind, + expected="operate_articulation", + field_name="kind", + ) + + +@configclass +class RegisteredSemanticCallCfg: + """Safe declarative payload for one catalog-registered semantic call.""" + + call_id: str = MISSING + schema_version: int = EXPERT_PROGRAM_SCHEMA_VERSION + arguments: dict[str, DeclarativeCfgValue] = field(default_factory=dict) + resources: dict[str, str] = field(default_factory=dict) + kind: str = "registered" + + def __post_init__(self) -> None: + """Validate versioned ID and recursively executable-free arguments.""" + _validate_identifier(self.call_id, field_name="call_id") + if _REGISTERED_CALL_ID_PATTERN.fullmatch(self.call_id) is None: + raise ValueError( + "call_id must contain two or more lowercase identifier segments " + "separated by single dots." + ) + if type(self.schema_version) is not int or self.schema_version != 1: + raise ValueError("Registered call schema_version must be exactly 1.") + if type(self.arguments) is not dict: + raise TypeError("arguments must be an exact dict.") + arguments = _snapshot_declarative_value( + self.arguments, + path="arguments", + ) + assert type(arguments) is dict + self.arguments = arguments + self.resources = _validate_resources(self.resources, field_name="resources") + _validate_kind(self.kind, expected="registered", field_name="kind") + + +SemanticCallCfg: TypeAlias = ( + PickCfg + | PlaceCfg + | HandOverCfg + | OperateArticulationCfg + | RegisteredSemanticCallCfg +) + + +@configclass +class WaitStablePostCfg: + """Wait for one registered entity to satisfy a named stability preset.""" + + entity: str = MISSING + preset: str = "rigid_object" + kind: str = "wait_stable" + + def __post_init__(self) -> None: + """Validate entity, preset, and discriminator.""" + _validate_identifier(self.entity, field_name="entity") + _validate_identifier(self.preset, field_name="preset") + _validate_kind(self.kind, expected="wait_stable", field_name="kind") + + +PostPolicyCfg: TypeAlias = WaitStablePostCfg + + +@configclass +class ObjectNearTargetValidatorCfg: + """Validate an object's position against one resolved target.""" + + object: str = MISSING + target: str = MISSING + position_tolerance: float = 0.03 + kind: str = "object_near_target" + + def __post_init__(self) -> None: + """Validate reference IDs and a positive finite tolerance.""" + _validate_identifier(self.object, field_name="object") + _validate_identifier(self.target, field_name="target") + tolerance = _validate_number( + self.position_tolerance, + field_name="position_tolerance", + ) + if tolerance <= 0.0: + raise ValueError("position_tolerance must be positive.") + self.position_tolerance = tolerance + _validate_kind( + self.kind, + expected="object_near_target", + field_name="kind", + ) + + +ValidatorCfg: TypeAlias = ObjectNearTargetValidatorCfg + + +@configclass +class InvokeCfg: + """Invoke exactly one semantic call at the current program boundary.""" + + call: SemanticCallCfg = MISSING + kind: str = "invoke" + + def __post_init__(self) -> None: + """Validate the semantic-call union and discriminator.""" + if type(self.call) not in _SEMANTIC_CALL_TYPES: + raise TypeError("call must be an exact SemanticCallCfg value.") + _validate_kind(self.kind, expected="invoke", field_name="kind") + + +@configclass +class BarrierCfg: + """Explicit synchronization boundary owned by one parallel node.""" + + name: str = "join" + timeout_steps: int = 1_000 + failure_policy: str = "fail_fast" + kind: str = "barrier" + + def __post_init__(self) -> None: + """Validate deterministic timeout and cancellation semantics.""" + _validate_kind(self.kind, expected="barrier", field_name="kind") + _validate_identifier(self.name, field_name="name") + if type(self.timeout_steps) is not int or self.timeout_steps <= 0: + raise ValueError("timeout_steps must be a positive integer.") + if self.failure_policy != "fail_fast": + raise ValueError("failure_policy must be exactly 'fail_fast'.") + + +@configclass +class SequenceCfg: + """Execute one non-empty ordered tuple of program nodes.""" + + items: tuple[ProgramNodeCfg, ...] = MISSING + kind: str = "sequence" + + def __post_init__(self) -> None: + """Validate ordered child nodes and discriminator.""" + _validate_kind(self.kind, expected="sequence", field_name="kind") + if type(self.items) not in (list, tuple) or not self.items: + raise ValueError("items must contain at least one program node.") + items = tuple(self.items) + if not all(type(item) in _PROGRAM_NODE_TYPES for item in items): + raise TypeError("items must contain exact ProgramNodeCfg values.") + self.items = items # type: ignore[assignment] + + +@configclass +class RepeatCfg: + """Repeat one child node a finite validated number of times.""" + + count: int = MISSING + body: ProgramNodeCfg = MISSING + kind: str = "repeat" + + def __post_init__(self) -> None: + """Validate a bounded positive repeat and its child node.""" + if type(self.count) is not int or not 1 <= self.count <= MAX_REPEAT_COUNT: + raise ValueError(f"count must be an integer in [1, {MAX_REPEAT_COUNT}].") + if type(self.body) not in _PROGRAM_NODE_TYPES: + raise TypeError("body must be an exact ProgramNodeCfg value.") + _validate_kind(self.kind, expected="repeat", field_name="kind") + + +@configclass +class SegmentCfg: + """Logical program transaction with post-policies and validators.""" + + name: str = MISSING + steps: ProgramNodeCfg = MISSING + post: tuple[PostPolicyCfg, ...] = field(default_factory=tuple) + validators: tuple[ValidatorCfg, ...] = field(default_factory=tuple) + kind: str = "segment" + + def __post_init__(self) -> None: + """Validate the segment boundary and its declarative hooks.""" + _validate_identifier(self.name, field_name="name") + if type(self.steps) not in _PROGRAM_NODE_TYPES: + raise TypeError("steps must be an exact ProgramNodeCfg value.") + if type(self.post) not in (list, tuple): + raise TypeError("post must be a list or tuple.") + if type(self.validators) not in (list, tuple): + raise TypeError("validators must be a list or tuple.") + post = tuple(self.post) + validators = tuple(self.validators) + if not all(type(value) in _POST_POLICY_TYPES for value in post): + raise TypeError("post must contain exact PostPolicyCfg values.") + if not all(type(value) in _VALIDATOR_TYPES for value in validators): + raise TypeError("validators must contain exact ValidatorCfg values.") + self.post = post # type: ignore[assignment] + self.validators = validators # type: ignore[assignment] + _validate_kind(self.kind, expected="segment", field_name="kind") + + +@configclass +class ParallelCfg: + """Execute two or more branches concurrently and join at one barrier.""" + + branches: tuple[ProgramNodeCfg, ...] = MISSING + barrier: BarrierCfg = MISSING + kind: str = "parallel" + + def __post_init__(self) -> None: + """Validate branch ownership and an explicit synchronization node.""" + _validate_kind(self.kind, expected="parallel", field_name="kind") + if type(self.branches) not in (list, tuple) or len(self.branches) < 2: + raise ValueError("branches must contain at least two program nodes.") + branches = tuple(self.branches) + if not all(type(branch) in _PROGRAM_NODE_TYPES for branch in branches): + raise TypeError("branches must contain exact ProgramNodeCfg values.") + if any(type(branch) in (ParallelCfg, BarrierCfg) for branch in branches): + raise ValueError( + "Nested Parallel and standalone Barrier branches are forbidden." + ) + if type(self.barrier) is not BarrierCfg: + raise TypeError("barrier must be exactly BarrierCfg.") + self.branches = branches # type: ignore[assignment] + + +ProgramNodeCfg: TypeAlias = ( + SequenceCfg | RepeatCfg | SegmentCfg | InvokeCfg | ParallelCfg | BarrierCfg +) + +_SEMANTIC_CALL_TYPES = ( + PickCfg, + PlaceCfg, + HandOverCfg, + OperateArticulationCfg, + RegisteredSemanticCallCfg, +) +_POST_POLICY_TYPES = (WaitStablePostCfg,) +_VALIDATOR_TYPES = (ObjectNearTargetValidatorCfg,) +_PROGRAM_NODE_TYPES = ( + SequenceCfg, + RepeatCfg, + SegmentCfg, + InvokeCfg, + ParallelCfg, + BarrierCfg, +) + + +def _validate_target_reference(target: str, targets: dict[str, TargetCfg]) -> None: + """Require one target reference to exist in the top-level registry.""" + if target not in targets: + raise ValueError(f"Unknown target reference {target!r}.") + + +def _validate_program( + node: ProgramNodeCfg, + *, + targets: dict[str, TargetCfg], + depth: int, + budget: list[int], + schema_version: int, + inside_parallel: bool = False, +) -> int: + """Validate references and return the statically expanded call count.""" + if depth > MAX_PROGRAM_DEPTH: + raise ValueError(f"Program exceeds depth limit {MAX_PROGRAM_DEPTH}.") + budget[0] -= 1 + if budget[0] < 0: + raise ValueError(f"Program exceeds node limit {MAX_PROGRAM_NODES}.") + if type(node) is InvokeCfg: + call = node.call + if type(call) is PlaceCfg and call.at is not None: + _validate_target_reference(call.at.target, targets) + if type(call) is HandOverCfg and call.final_target is not None: + _validate_target_reference(call.final_target.target, targets) + return 1 + if type(node) is BarrierCfg: + if schema_version < EXPERT_PROGRAM_SCHEMA_VERSION_V2: + raise ValueError("Barrier requires Expert Program schema version 2.") + if not inside_parallel: + raise ValueError("Barrier nodes may only be owned by Parallel.") + return 0 + if type(node) is SequenceCfg: + expanded = sum( + _validate_program( + child, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=inside_parallel, + ) + for child in node.items + ) + elif type(node) is RepeatCfg: + expanded = node.count * _validate_program( + node.body, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=inside_parallel, + ) + elif type(node) is SegmentCfg: + if inside_parallel: + raise ValueError( + "Parallel branches may contain only Invoke, Sequence, and Repeat " + "nodes; wrap the Parallel node in one Segment instead." + ) + for validator in node.validators: + _validate_target_reference(validator.target, targets) + expanded = _validate_program( + node.steps, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=inside_parallel, + ) + elif type(node) is ParallelCfg: + if schema_version < EXPERT_PROGRAM_SCHEMA_VERSION_V2: + raise ValueError("Parallel requires Expert Program schema version 2.") + if inside_parallel: + raise ValueError("Nested Parallel nodes are forbidden in schema version 2.") + branch_counts = tuple( + _validate_program( + branch, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=True, + ) + for branch in node.branches + ) + if any(count <= 0 for count in branch_counts): + raise ValueError("Every Parallel branch must contain a semantic call.") + _validate_program( + node.barrier, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=True, + ) + expanded = sum(branch_counts) + else: # pragma: no cover - exact construction prevents this branch + raise TypeError("program must contain exact ProgramNodeCfg values.") + if expanded > MAX_EXPANDED_CALLS: + raise ValueError( + f"Program expands to more than {MAX_EXPANDED_CALLS} semantic calls." + ) + return expanded + + +@configclass +class ExpertProgramCfg: + """Strict, versioned, executable-free Expert Program configuration.""" + + schema_version: int = MISSING + program_id: str = MISSING + integration: ExpertProgramIntegrationCfg = MISSING + program: ProgramNodeCfg = MISSING + targets: dict[str, TargetCfg] = field(default_factory=dict) + + def __post_init__(self) -> None: + """Validate the complete static configuration and target graph.""" + if ( + type(self.schema_version) is not int + or self.schema_version not in SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS + ): + raise ValueError( + "schema_version must be one of " + f"{SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS}." + ) + _validate_identifier(self.program_id, field_name="program_id") + if type(self.integration) is not ExpertProgramIntegrationCfg: + raise TypeError("integration must be ExpertProgramIntegrationCfg.") + if type(self.targets) is not dict: + raise TypeError("targets must be an exact dict.") + targets: dict[str, TargetCfg] = {} + for target_id, target in self.targets.items(): + normalized_id = _validate_identifier( + target_id, + field_name="target IDs", + ) + if type(target) is not CyclicPoseTargetCfg: + raise TypeError("targets must contain exact TargetCfg values.") + targets[normalized_id] = target + if type(self.program) not in _PROGRAM_NODE_TYPES: + raise TypeError("program must be an exact ProgramNodeCfg value.") + expanded = _validate_program( + self.program, + targets=targets, + depth=0, + budget=[MAX_PROGRAM_NODES], + schema_version=self.schema_version, + ) + if expanded <= 0: + raise ValueError("program must contain at least one semantic call.") + self.targets = targets + + +__all__ = [ + "BarrierCfg", + "CyclicPoseTargetCfg", + "DeclarativeCfgValue", + "EXPERT_PROGRAM_SCHEMA_VERSION", + "EXPERT_PROGRAM_SCHEMA_VERSION_V2", + "ExpertProgramCfg", + "ExpertProgramIntegrationCfg", + "HandOverCfg", + "InvokeCfg", + "MAX_DECLARATIVE_DEPTH", + "MAX_DECLARATIVE_NODES", + "MAX_EXPANDED_CALLS", + "MAX_PROGRAM_DEPTH", + "MAX_PROGRAM_NODES", + "MAX_REPEAT_COUNT", + "ObjectNearTargetValidatorCfg", + "OperateArticulationCfg", + "ParallelCfg", + "PickCfg", + "PlaceCfg", + "PoseCfg", + "PostPolicyCfg", + "ProgramNodeCfg", + "RegisteredSemanticCallCfg", + "RepeatCfg", + "SegmentCfg", + "SemanticCallCfg", + "SequenceCfg", + "SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS", + "TargetCfg", + "TargetRefCfg", + "ValidatorCfg", + "WaitStablePostCfg", +] diff --git a/embodichain/lab/gym/envs/expert_program/compiler.py b/embodichain/lab/gym/envs/expert_program/compiler.py new file mode 100644 index 000000000..cc12cea0d --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/compiler.py @@ -0,0 +1,1912 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Provider-free compilation and lazy expansion of Expert Program ASTs.""" + +from __future__ import annotations + +from collections.abc import Iterator, Mapping +from dataclasses import dataclass, field +from types import MappingProxyType +from typing import Protocol, runtime_checkable + +from embodichain.lab.sim.skills.calls import ( + DeclarativeValue, + HandOver, + OperateArticulation, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, + SemanticPose, +) +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + +from .cfg import ( + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, + MAX_EXPANDED_CALLS, + MAX_REPEAT_COUNT, + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS, + BarrierCfg, + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + OperateArticulationCfg, + ParallelCfg, + PickCfg, + PlaceCfg, + PoseCfg, + ProgramNodeCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + WaitStablePostCfg, +) +from .decoder import ConfigPath, ExpertProgramConfigError, render_config_path + +_SEMANTIC_CALL_TYPES = ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, +) +_SCENE_REF_TYPES = ( + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, +) + + +class ExpertProgramCompileError(ExpertProgramConfigError): + """Raised when a validated AST cannot lower to canonical semantic calls.""" + + +@runtime_checkable +class ExpertProgramSceneResolver(Protocol): + """Provider-free typed resolver for canonical static scene references.""" + + def resolve( + self, + reference: str, + *, + expected_types: tuple[type[SceneEntityRef], ...], + path: ConfigPath, + ) -> SceneEntityRef: + """Resolve one canonical or aliased ID without observing scene state.""" + + +def _copy_scene_ref(reference: SceneEntityRef) -> SceneEntityRef: + """Return one independent exact typed scene reference.""" + if type(reference) not in _SCENE_REF_TYPES: + raise TypeError(f"Unsupported scene reference {type(reference).__name__}.") + return type(reference)(reference.entity_id) + + +class SceneRegistryProgramResolver: + """Provider-free static resolver snapshotted from one SceneRegistry. + + The resolver copies only canonical typed references and aliases. It does not + retain registrations, state providers, geometry providers, or the registry + itself, so compilation cannot observe dynamic scene state. + """ + + def __init__(self, registry: SceneRegistry) -> None: + """Snapshot the registry's static identity table. + + Args: + registry: Authoritative registry used only for static identity data. + """ + if type(registry) is not SceneRegistry: + raise TypeError("registry must be exactly SceneRegistry.") + references = { + reference.entity_id: _copy_scene_ref(reference) + for reference in registry.entity_refs + } + self._references = MappingProxyType(references) + self._aliases = MappingProxyType(dict(registry.aliases)) + + @property + def canonical_references(self) -> Mapping[str, SceneEntityRef]: + """Return an independent canonical typed-reference mapping.""" + return MappingProxyType( + { + entity_id: _copy_scene_ref(reference) + for entity_id, reference in self._references.items() + } + ) + + def resolve( + self, + reference: str, + *, + expected_types: tuple[type[SceneEntityRef], ...], + path: ConfigPath, + ) -> SceneEntityRef: + """Resolve one ID or alias through the snapshotted type table.""" + if ( + type(reference) is not str + or not reference + or reference != reference.strip() + ): + raise ExpertProgramCompileError( + "invalid_scene_reference", + path, + "Scene references must be non-empty strings without outer whitespace.", + ) + if ( + type(expected_types) is not tuple + or not expected_types + or not all( + isinstance(expected_type, type) + and issubclass(expected_type, SceneEntityRef) + for expected_type in expected_types + ) + ): + raise TypeError( + "expected_types must be a non-empty tuple of SceneEntityRef types." + ) + canonical_id = self._aliases.get(reference, reference) + resolved = self._references.get(canonical_id) + if resolved is None: + raise ExpertProgramCompileError( + "unknown_scene_reference", + path, + f"Unknown scene reference {reference!r}.", + ) + if type(resolved) not in expected_types: + expected_names = tuple(value.__name__ for value in expected_types) + raise ExpertProgramCompileError( + "scene_reference_type_mismatch", + path, + f"Scene reference {reference!r} resolves to " + f"{type(resolved).__name__}, expected one of {expected_names}.", + ) + return _copy_scene_ref(resolved) + + +@dataclass(frozen=True, slots=True) +class CompiledRepeatFrame: + """One lexical repeat occurrence in a compiled call or segment path.""" + + path: ConfigPath + iteration_index: int + count: int + + def __post_init__(self) -> None: + if type(self.path) is not tuple: + raise TypeError("path must be a ConfigPath tuple.") + if type(self.iteration_index) is not int or not 0 <= self.iteration_index: + raise ValueError("iteration_index must be a non-negative integer.") + if type(self.count) is not int or self.count <= 0: + raise ValueError("count must be a positive integer.") + if self.iteration_index >= self.count: + raise ValueError("iteration_index must be smaller than count.") + + +@dataclass(frozen=True, slots=True) +class CompiledTargetSelection: + """Deterministic cyclic-target selection metadata for one occurrence.""" + + target_id: str + value_index: int + repeat_path: ConfigPath | None + repeat_iteration_index: int | None + + def __post_init__(self) -> None: + if type(self.target_id) is not str or not self.target_id: + raise ValueError("target_id must be a non-empty string.") + if type(self.value_index) is not int or self.value_index < 0: + raise ValueError("value_index must be a non-negative integer.") + if (self.repeat_path is None) != (self.repeat_iteration_index is None): + raise ValueError( + "repeat_path and repeat_iteration_index must both be set or unset." + ) + if self.repeat_path is not None and type(self.repeat_path) is not tuple: + raise TypeError("repeat_path must be a ConfigPath tuple or None.") + if self.repeat_iteration_index is not None and ( + type(self.repeat_iteration_index) is not int + or self.repeat_iteration_index < 0 + ): + raise ValueError("repeat_iteration_index must be non-negative or None.") + + +def _snapshot_semantic_call(call: SemanticCallSpec) -> SemanticCallSpec: + """Return one independently owned exact semantic-call value.""" + if type(call) is Pick: + return Pick( + object=_copy_scene_ref(call.object), + grasp=(None if call.grasp is None else _copy_scene_ref(call.grasp)), + resources=dict(call.resources), + ) + if type(call) is Place: + return Place( + object=_copy_scene_ref(call.object), + at=None if call.at is None else call.at.snapshot(), + on=None if call.on is None else _copy_scene_ref(call.on), + inside=None if call.inside is None else _copy_scene_ref(call.inside), + resources=dict(call.resources), + ) + if type(call) is HandOver: + return HandOver( + object=_copy_scene_ref(call.object), + receiver=call.receiver, + final_target=( + None if call.final_target is None else call.final_target.snapshot() + ), + resources=dict(call.resources), + ) + if type(call) is OperateArticulation: + return OperateArticulation( + articulation=_copy_scene_ref(call.articulation), + handle=(None if call.handle is None else _copy_scene_ref(call.handle)), + target=call.target, + target_position=call.target_position, + target_displacement=call.target_displacement, + resources=dict(call.resources), + ) + if type(call) is RegisteredSemanticCall: + return RegisteredSemanticCall( + call_id=call.call_id, + arguments=call.arguments, + resources=dict(call.resources), + ) + raise TypeError("call must be an exact supported SemanticCallSpec value.") + + +@dataclass(frozen=True, slots=True) +class CompiledProgramCall: + """One owned semantic call occurrence emitted by lazy program expansion.""" + + call_index: int + segment_call_index: int + call: SemanticCallSpec + source_path: ConfigPath + repeat_frames: tuple[CompiledRepeatFrame, ...] = () + target_selections: tuple[CompiledTargetSelection, ...] = () + + def __post_init__(self) -> None: + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + if type(self.segment_call_index) is not int or self.segment_call_index < 0: + raise ValueError("segment_call_index must be a non-negative integer.") + if type(self.call) not in _SEMANTIC_CALL_TYPES: + raise TypeError("call must be an exact supported SemanticCallSpec value.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + frames = tuple(self.repeat_frames) + selections = tuple(self.target_selections) + if not all(type(frame) is CompiledRepeatFrame for frame in frames): + raise TypeError("repeat_frames must contain CompiledRepeatFrame values.") + if not all( + type(selection) is CompiledTargetSelection for selection in selections + ): + raise TypeError( + "target_selections must contain CompiledTargetSelection values." + ) + object.__setattr__(self, "call", _snapshot_semantic_call(self.call)) + object.__setattr__(self, "repeat_frames", frames) + object.__setattr__(self, "target_selections", selections) + + +@dataclass(frozen=True, slots=True) +class CompiledPostPolicy: + """Owned post-policy config plus its canonical scene entity and source path.""" + + cfg: WaitStablePostCfg + entity: SceneEntityRef + source_path: ConfigPath + + def __post_init__(self) -> None: + if type(self.cfg) is not WaitStablePostCfg: + raise TypeError("cfg must be exactly WaitStablePostCfg.") + if type(self.entity) not in _SCENE_REF_TYPES: + raise TypeError("entity must be an exact SceneEntityRef value.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + object.__setattr__( + self, + "cfg", + WaitStablePostCfg( + entity=self.cfg.entity, + preset=self.cfg.preset, + kind=self.cfg.kind, + ), + ) + object.__setattr__(self, "entity", _copy_scene_ref(self.entity)) + + +@dataclass(frozen=True, slots=True) +class CompiledProgramValidator: + """Owned validator config with canonical object and resolved target pose.""" + + cfg: ObjectNearTargetValidatorCfg + object: SceneObjectRef + target_pose: SemanticPose + target_selection: CompiledTargetSelection + source_path: ConfigPath + + def __post_init__(self) -> None: + if type(self.cfg) is not ObjectNearTargetValidatorCfg: + raise TypeError("cfg must be exactly ObjectNearTargetValidatorCfg.") + if type(self.object) is not SceneObjectRef: + raise TypeError("object must be exactly SceneObjectRef.") + if type(self.target_pose) is not SemanticPose: + raise TypeError("target_pose must be exactly SemanticPose.") + if type(self.target_selection) is not CompiledTargetSelection: + raise TypeError("target_selection must be exactly CompiledTargetSelection.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + object.__setattr__( + self, + "cfg", + ObjectNearTargetValidatorCfg( + object=self.cfg.object, + target=self.cfg.target, + position_tolerance=self.cfg.position_tolerance, + kind=self.cfg.kind, + ), + ) + object.__setattr__(self, "object", _copy_scene_ref(self.object)) + object.__setattr__(self, "target_pose", self.target_pose.snapshot()) + + +@dataclass(frozen=True, slots=True) +class CompiledBarrier: + """Explicit schema-v2 join semantics for one compiled parallel block.""" + + name: str + timeout_steps: int + failure_policy: str + source_path: ConfigPath + + def __post_init__(self) -> None: + if type(self.name) is not str or not self.name: + raise ValueError("barrier name must be non-empty.") + if type(self.timeout_steps) is not int or self.timeout_steps <= 0: + raise ValueError("barrier timeout_steps must be positive.") + if self.failure_policy != "fail_fast": + raise ValueError("barrier failure_policy must be 'fail_fast'.") + if type(self.source_path) is not tuple: + raise TypeError("barrier source_path must be a ConfigPath tuple.") + + +@dataclass(frozen=True, slots=True) +class CompiledParallelBranch: + """One ordered semantic-call lane inside a parallel block.""" + + branch_index: int + calls: tuple[CompiledProgramCall, ...] + source_path: ConfigPath + + def __post_init__(self) -> None: + if type(self.branch_index) is not int or self.branch_index < 0: + raise ValueError("branch_index must be non-negative.") + calls = tuple(self.calls) + if not calls or not all(type(call) is CompiledProgramCall for call in calls): + raise TypeError("parallel branch calls must be non-empty compiled calls.") + if type(self.source_path) is not tuple: + raise TypeError("parallel branch source_path must be a ConfigPath tuple.") + object.__setattr__(self, "calls", calls) + + +@dataclass(frozen=True, slots=True) +class CompiledParallelBlock: + """Two or more call lanes joined by an explicit deterministic barrier.""" + + branches: tuple[CompiledParallelBranch, ...] + barrier: CompiledBarrier + source_path: ConfigPath + + def __post_init__(self) -> None: + branches = tuple(self.branches) + if len(branches) < 2 or not all( + type(branch) is CompiledParallelBranch for branch in branches + ): + raise TypeError("parallel blocks require at least two compiled branches.") + if tuple(branch.branch_index for branch in branches) != tuple( + range(len(branches)) + ): + raise ValueError("parallel branch indices must be contiguous from zero.") + if type(self.barrier) is not CompiledBarrier: + raise TypeError("barrier must be exactly CompiledBarrier.") + if type(self.source_path) is not tuple: + raise TypeError("parallel source_path must be a ConfigPath tuple.") + object.__setattr__(self, "branches", branches) + + +@dataclass(frozen=True, slots=True) +class CompiledProgramSegment: + """One independent explicit or implicit logical program segment.""" + + segment_index: int + segment_id: str + name: str + calls: tuple[CompiledProgramCall, ...] + source_path: ConfigPath + repeat_frames: tuple[CompiledRepeatFrame, ...] = () + post_policies: tuple[CompiledPostPolicy, ...] = () + validators: tuple[CompiledProgramValidator, ...] = () + parallel_block: CompiledParallelBlock | None = None + implicit: bool = False + + def __post_init__(self) -> None: + if type(self.segment_index) is not int or self.segment_index < 0: + raise ValueError("segment_index must be a non-negative integer.") + for field_name in ("segment_id", "name"): + value = getattr(self, field_name) + if type(value) is not str or not value: + raise ValueError(f"{field_name} must be a non-empty string.") + calls = tuple(self.calls) + if not calls or not all(type(call) is CompiledProgramCall for call in calls): + raise TypeError( + "calls must contain at least one exact CompiledProgramCall." + ) + if tuple(call.segment_call_index for call in calls) != tuple(range(len(calls))): + raise ValueError("segment call indices must be contiguous from zero.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + frames = tuple(self.repeat_frames) + post = tuple(self.post_policies) + validators = tuple(self.validators) + if not all(type(frame) is CompiledRepeatFrame for frame in frames): + raise TypeError("repeat_frames must contain CompiledRepeatFrame values.") + if not all(type(value) is CompiledPostPolicy for value in post): + raise TypeError("post_policies must contain CompiledPostPolicy values.") + if not all(type(value) is CompiledProgramValidator for value in validators): + raise TypeError("validators must contain CompiledProgramValidator values.") + if type(self.implicit) is not bool: + raise TypeError("implicit must be a bool.") + if self.implicit and (post or validators): + raise ValueError( + "Implicit segments cannot own post-policies or validators." + ) + if self.parallel_block is not None: + if type(self.parallel_block) is not CompiledParallelBlock: + raise TypeError("parallel_block must be CompiledParallelBlock or None.") + flattened = tuple( + call for branch in self.parallel_block.branches for call in branch.calls + ) + if flattened != calls: + raise ValueError( + "segment calls must equal parallel branch calls in branch order." + ) + object.__setattr__(self, "calls", calls) + object.__setattr__(self, "repeat_frames", frames) + object.__setattr__(self, "post_policies", post) + object.__setattr__(self, "validators", validators) + + +@dataclass(frozen=True, slots=True) +class CompiledProgramAnalysis: + """One owned canonical semantic-analysis window for a compiled program. + + ``execution_prefix_length`` separates calls that the current segment owns + from downstream calls included only for static state-flow and target + look-ahead. Preflight analyses set the prefix to the complete window. + """ + + analysis_id: str + kind: str + calls: tuple[SemanticCallSpec, ...] + source_path: ConfigPath + segment_indices: tuple[int, ...] + execution_prefix_length: int + + def __post_init__(self) -> None: + if type(self.analysis_id) is not str or not self.analysis_id: + raise ValueError("analysis_id must be a non-empty string.") + if self.kind not in { + "sequential_stretch", + "parallel_branch", + "sequential_suffix", + }: + raise ValueError("kind must identify a supported program analysis.") + calls = tuple(self.calls) + if not calls or not all(type(call) in _SEMANTIC_CALL_TYPES for call in calls): + raise TypeError("calls must contain supported semantic call values.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + indices = tuple(self.segment_indices) + if not indices or any(type(index) is not int or index < 0 for index in indices): + raise ValueError("segment_indices must contain non-negative integers.") + if len(set(indices)) != len(indices) or tuple(sorted(indices)) != indices: + raise ValueError("segment_indices must be unique and ordered.") + if type( + self.execution_prefix_length + ) is not int or not 1 <= self.execution_prefix_length <= len(calls): + raise ValueError( + "execution_prefix_length must select a non-empty prefix of calls." + ) + object.__setattr__( + self, + "calls", + tuple(_snapshot_semantic_call(call) for call in calls), + ) + object.__setattr__(self, "segment_indices", indices) + + +@dataclass(frozen=True, slots=True) +class _CallTemplate: + kind: str + source_path: ConfigPath + object: SceneObjectRef | None = None + grasp: SceneAffordanceRef | None = None + at_target_id: str | None = None + on: SceneObjectRef | SceneAffordanceRef | None = None + inside: SceneObjectRef | SceneAffordanceRef | None = None + receiver: str | None = None + final_target_id: str | None = None + articulation: SceneArticulationRef | None = None + handle: SceneAffordanceRef | None = None + articulation_target: str | None = None + target_position: float | None = None + target_displacement: float | None = None + call_id: str | None = None + arguments: Mapping[str, DeclarativeValue] | None = None + resources: tuple[tuple[str, str], ...] = () + + +@dataclass(frozen=True, slots=True) +class _InvokeTemplate: + call: _CallTemplate + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _SequenceTemplate: + items: tuple[_NodeTemplate, ...] + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _RepeatTemplate: + count: int + body: _NodeTemplate + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _BarrierTemplate: + name: str + timeout_steps: int + failure_policy: str + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _ParallelTemplate: + branches: tuple[_NodeTemplate, ...] + barrier: _BarrierTemplate + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _PostTemplate: + cfg: WaitStablePostCfg + entity: SceneEntityRef + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _ValidatorTemplate: + cfg: ObjectNearTargetValidatorCfg + object: SceneObjectRef + target_id: str + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _SegmentTemplate: + name: str + steps: _NodeTemplate + post: tuple[_PostTemplate, ...] + validators: tuple[_ValidatorTemplate, ...] + source_path: ConfigPath + + +_NodeTemplate = ( + _InvokeTemplate + | _SequenceTemplate + | _RepeatTemplate + | _SegmentTemplate + | _ParallelTemplate + | _BarrierTemplate +) + + +def _contains_parallel(template: _NodeTemplate) -> bool: + """Return whether a compiled subtree owns a parallel block.""" + if type(template) is _ParallelTemplate: + return True + if type(template) is _SequenceTemplate: + return any(_contains_parallel(child) for child in template.items) + if type(template) is _RepeatTemplate: + return _contains_parallel(template.body) + if type(template) is _SegmentTemplate: + return _contains_parallel(template.steps) + return False + + +@dataclass(slots=True) +class _ExpansionState: + segment_index: int = 0 + call_index: int = 0 + + +def _resolve_target( + target_id: str, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + repeat_frames: tuple[CompiledRepeatFrame, ...], +) -> tuple[SemanticPose, CompiledTargetSelection]: + """Select one cyclic target from the nearest lexical repeat frame.""" + values = targets[target_id] + repeat = repeat_frames[-1] if repeat_frames else None + value_index = 0 if repeat is None else repeat.iteration_index % len(values) + selection = CompiledTargetSelection( + target_id=target_id, + value_index=value_index, + repeat_path=None if repeat is None else repeat.path, + repeat_iteration_index=None if repeat is None else repeat.iteration_index, + ) + return values[value_index].snapshot(), selection + + +def _instantiate_call( + template: _CallTemplate, + *, + call_index: int, + segment_call_index: int, + targets: Mapping[str, tuple[SemanticPose, ...]], + repeat_frames: tuple[CompiledRepeatFrame, ...], +) -> CompiledProgramCall: + """Instantiate one semantic call occurrence from static templates.""" + resources = dict(template.resources) + selections: list[CompiledTargetSelection] = [] + if template.kind == "pick": + assert template.object is not None + call: SemanticCallSpec = Pick( + object=_copy_scene_ref(template.object), + grasp=(None if template.grasp is None else _copy_scene_ref(template.grasp)), + resources=resources, + ) + elif template.kind == "place": + assert template.object is not None + at: SemanticPose | None = None + if template.at_target_id is not None: + at, selection = _resolve_target( + template.at_target_id, + targets=targets, + repeat_frames=repeat_frames, + ) + selections.append(selection) + call = Place( + object=_copy_scene_ref(template.object), + at=at, + on=None if template.on is None else _copy_scene_ref(template.on), + inside=( + None if template.inside is None else _copy_scene_ref(template.inside) + ), + resources=resources, + ) + elif template.kind == "hand_over": + assert template.object is not None + final_target: SemanticPose | None = None + if template.final_target_id is not None: + final_target, selection = _resolve_target( + template.final_target_id, + targets=targets, + repeat_frames=repeat_frames, + ) + selections.append(selection) + call = HandOver( + object=_copy_scene_ref(template.object), + receiver=template.receiver, + final_target=final_target, + resources=resources, + ) + elif template.kind == "operate_articulation": + assert template.articulation is not None + call = OperateArticulation( + articulation=_copy_scene_ref(template.articulation), + handle=( + None if template.handle is None else _copy_scene_ref(template.handle) + ), + target=template.articulation_target, + target_position=template.target_position, + target_displacement=template.target_displacement, + resources=resources, + ) + elif template.kind == "registered": + assert template.call_id is not None and template.arguments is not None + call = RegisteredSemanticCall( + call_id=template.call_id, + arguments=template.arguments, + resources=resources, + ) + else: # pragma: no cover - compiler-owned templates prevent this + raise AssertionError(f"Unknown call template {template.kind!r}.") + return CompiledProgramCall( + call_index=call_index, + segment_call_index=segment_call_index, + call=call, + source_path=template.source_path, + repeat_frames=repeat_frames, + target_selections=tuple(selections), + ) + + +def _iter_call_templates( + template: _NodeTemplate, + *, + repeat_frames: tuple[CompiledRepeatFrame, ...], +) -> Iterator[tuple[_CallTemplate, tuple[CompiledRepeatFrame, ...]]]: + """Expand call templates inside one explicit segment without segment splits.""" + if type(template) is _InvokeTemplate: + yield template.call, repeat_frames + elif type(template) is _SequenceTemplate: + for child in template.items: + yield from _iter_call_templates(child, repeat_frames=repeat_frames) + elif type(template) is _RepeatTemplate: + for iteration_index in range(template.count): + frame = CompiledRepeatFrame( + path=template.source_path, + iteration_index=iteration_index, + count=template.count, + ) + yield from _iter_call_templates( + template.body, + repeat_frames=(*repeat_frames, frame), + ) + else: # pragma: no cover - nested segments are rejected during compilation + raise AssertionError("A nested segment reached call-only expansion.") + + +def _instantiate_parallel_block( + template: _ParallelTemplate, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + repeat_frames: tuple[CompiledRepeatFrame, ...], + state: _ExpansionState, +) -> tuple[CompiledParallelBlock, tuple[CompiledProgramCall, ...]]: + """Instantiate branch-local call order without serializing branch semantics.""" + branches: list[CompiledParallelBranch] = [] + flattened: list[CompiledProgramCall] = [] + segment_call_index = 0 + for branch_index, branch_template in enumerate(template.branches): + calls: list[CompiledProgramCall] = [] + for call_template, call_repeat_frames in _iter_call_templates( + branch_template, + repeat_frames=repeat_frames, + ): + call = _instantiate_call( + call_template, + call_index=state.call_index, + segment_call_index=segment_call_index, + targets=targets, + repeat_frames=call_repeat_frames, + ) + calls.append(call) + flattened.append(call) + state.call_index += 1 + segment_call_index += 1 + branches.append( + CompiledParallelBranch( + branch_index=branch_index, + calls=tuple(calls), + source_path=template.branches[branch_index].source_path, + ) + ) + barrier = CompiledBarrier( + name=template.barrier.name, + timeout_steps=template.barrier.timeout_steps, + failure_policy=template.barrier.failure_policy, + source_path=template.barrier.source_path, + ) + return ( + CompiledParallelBlock( + branches=tuple(branches), + barrier=barrier, + source_path=template.source_path, + ), + tuple(flattened), + ) + + +def _segment_identity( + program_id: str, + *, + source_path: ConfigPath, + repeat_frames: tuple[CompiledRepeatFrame, ...], + implicit: bool, +) -> str: + """Build one deterministic segment identity from lexical occurrence data.""" + repeat_suffix = "".join( + f"@{render_config_path(frame.path)}[{frame.iteration_index}]" + for frame in repeat_frames + ) + boundary = "implicit" if implicit else "segment" + return f"{program_id}:{boundary}:{render_config_path(source_path)}{repeat_suffix}" + + +def _iter_segments( + template: _NodeTemplate, + *, + program_id: str, + targets: Mapping[str, tuple[SemanticPose, ...]], + repeat_frames: tuple[CompiledRepeatFrame, ...], + state: _ExpansionState, +) -> Iterator[CompiledProgramSegment]: + """Lazily expand outer program structure into independent segments.""" + if type(template) is _SequenceTemplate: + for child in template.items: + yield from _iter_segments( + child, + program_id=program_id, + targets=targets, + repeat_frames=repeat_frames, + state=state, + ) + return + if type(template) is _RepeatTemplate: + for iteration_index in range(template.count): + frame = CompiledRepeatFrame( + path=template.source_path, + iteration_index=iteration_index, + count=template.count, + ) + yield from _iter_segments( + template.body, + program_id=program_id, + targets=targets, + repeat_frames=(*repeat_frames, frame), + state=state, + ) + return + if type(template) is _InvokeTemplate: + call = _instantiate_call( + template.call, + call_index=state.call_index, + segment_call_index=0, + targets=targets, + repeat_frames=repeat_frames, + ) + state.call_index += 1 + segment = CompiledProgramSegment( + segment_index=state.segment_index, + segment_id=_segment_identity( + program_id, + source_path=template.source_path, + repeat_frames=repeat_frames, + implicit=True, + ), + name=f"invoke:{call.call.semantic_id}", + calls=(call,), + source_path=template.source_path, + repeat_frames=repeat_frames, + implicit=True, + ) + state.segment_index += 1 + yield segment + return + + if type(template) is _ParallelTemplate: + parallel_block, calls = _instantiate_parallel_block( + template, + targets=targets, + repeat_frames=repeat_frames, + state=state, + ) + segment = CompiledProgramSegment( + segment_index=state.segment_index, + segment_id=_segment_identity( + program_id, + source_path=template.source_path, + repeat_frames=repeat_frames, + implicit=True, + ), + name=f"parallel:{parallel_block.barrier.name}", + calls=calls, + source_path=template.source_path, + repeat_frames=repeat_frames, + parallel_block=parallel_block, + implicit=True, + ) + state.segment_index += 1 + yield segment + return + + assert type(template) is _SegmentTemplate + parallel_block: CompiledParallelBlock | None = None + if type(template.steps) is _ParallelTemplate: + parallel_block, instantiated_calls = _instantiate_parallel_block( + template.steps, + targets=targets, + repeat_frames=repeat_frames, + state=state, + ) + calls = list(instantiated_calls) + else: + calls = [] + for segment_call_index, (call_template, call_repeat_frames) in enumerate( + _iter_call_templates(template.steps, repeat_frames=repeat_frames) + ): + calls.append( + _instantiate_call( + call_template, + call_index=state.call_index, + segment_call_index=segment_call_index, + targets=targets, + repeat_frames=call_repeat_frames, + ) + ) + state.call_index += 1 + post_policies = tuple( + CompiledPostPolicy( + cfg=post.cfg, + entity=post.entity, + source_path=post.source_path, + ) + for post in template.post + ) + validators: list[CompiledProgramValidator] = [] + for validator in template.validators: + target_pose, selection = _resolve_target( + validator.target_id, + targets=targets, + repeat_frames=repeat_frames, + ) + validators.append( + CompiledProgramValidator( + cfg=validator.cfg, + object=validator.object, + target_pose=target_pose, + target_selection=selection, + source_path=validator.source_path, + ) + ) + segment = CompiledProgramSegment( + segment_index=state.segment_index, + segment_id=_segment_identity( + program_id, + source_path=template.source_path, + repeat_frames=repeat_frames, + implicit=False, + ), + name=template.name, + calls=tuple(calls), + source_path=template.source_path, + repeat_frames=repeat_frames, + post_policies=post_policies, + validators=tuple(validators), + parallel_block=parallel_block, + implicit=False, + ) + state.segment_index += 1 + yield segment + + +@dataclass(frozen=True, slots=True, init=False) +class CompiledProgram: + """Owned provider-free program template with lazy deterministic expansion.""" + + schema_version: int + program_id: str + _integration: ExpertProgramIntegrationCfg = field(repr=False, compare=False) + _targets: Mapping[str, tuple[SemanticPose, ...]] = field( + repr=False, + compare=False, + ) + _root: _NodeTemplate = field(repr=False, compare=False) + + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :class:`ExpertProgramCompiler`.""" + del args, kwargs + raise TypeError("CompiledProgram values are created by ExpertProgramCompiler.") + + @classmethod + def _create( + cls, + *, + schema_version: int, + program_id: str, + integration: ExpertProgramIntegrationCfg, + targets: Mapping[str, tuple[SemanticPose, ...]], + root: _NodeTemplate, + ) -> CompiledProgram: + """Create one compiler-owned lazy program template.""" + instance = object.__new__(cls) + object.__setattr__(instance, "schema_version", schema_version) + object.__setattr__(instance, "program_id", program_id) + object.__setattr__(instance, "_integration", integration) + object.__setattr__( + instance, + "_targets", + MappingProxyType( + { + target_id: tuple(pose.snapshot() for pose in values) + for target_id, values in targets.items() + } + ), + ) + object.__setattr__(instance, "_root", root) + return instance + + @property + def integration(self) -> ExpertProgramIntegrationCfg: + """Return an independent integration-selection snapshot.""" + return ExpertProgramIntegrationCfg( + robot_profile=self._integration.robot_profile, + scene_registry=self._integration.scene_registry, + runtime_preset=self._integration.runtime_preset, + ) + + @property + def targets(self) -> Mapping[str, tuple[SemanticPose, ...]]: + """Return independent static target-pose snapshots.""" + return MappingProxyType( + { + target_id: tuple(pose.snapshot() for pose in values) + for target_id, values in self._targets.items() + } + ) + + def iter_segments(self) -> Iterator[CompiledProgramSegment]: + """Lazily expand a fresh deterministic segment stream.""" + return _iter_segments( + self._root, + program_id=self.program_id, + targets=self._targets, + repeat_frames=(), + state=_ExpansionState(), + ) + + def materialize(self) -> MaterializedCompiledProgram: + """Expand the bounded provider-free segment stream exactly once. + + Materialization never observes a scene provider. It also re-enforces + the public expanded-call bound so a configuration mutated after its + initial validation cannot create an unbounded bridge-preflight pass. + + Returns: + Immutable materialized program with deterministic analysis windows. + + Raises: + ExpertProgramCompileError: If expansion exceeds the configured + semantic-call bound. + """ + segments: list[CompiledProgramSegment] = [] + expanded_calls = 0 + for segment in self.iter_segments(): + expanded_calls += len(segment.calls) + if expanded_calls > MAX_EXPANDED_CALLS: + raise ExpertProgramCompileError( + "expanded_call_limit", + segment.source_path, + "Program materialization exceeds the static limit of " + f"{MAX_EXPANDED_CALLS} semantic calls.", + ) + segments.append(segment) + return MaterializedCompiledProgram._create( + schema_version=self.schema_version, + program_id=self.program_id, + integration=self._integration, + segments=tuple(segments), + ) + + def __iter__(self) -> Iterator[CompiledProgramSegment]: + return self.iter_segments() + + +@dataclass(frozen=True, slots=True, init=False) +class MaterializedCompiledProgram: + """Bounded provider-free segment snapshot used by preflight and execution.""" + + schema_version: int + program_id: str + _integration: ExpertProgramIntegrationCfg = field(repr=False, compare=False) + _segments: tuple[CompiledProgramSegment, ...] = field( + repr=False, + compare=False, + ) + + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :meth:`CompiledProgram.materialize`.""" + del args, kwargs + raise TypeError( + "MaterializedCompiledProgram values are created by " + "CompiledProgram.materialize()." + ) + + @classmethod + def _create( + cls, + *, + schema_version: int, + program_id: str, + integration: ExpertProgramIntegrationCfg, + segments: tuple[CompiledProgramSegment, ...], + ) -> MaterializedCompiledProgram: + """Create one compiler-owned materialized program.""" + if type(schema_version) is not int or schema_version < 1: + raise ValueError("schema_version must be a positive integer.") + if type(program_id) is not str or not program_id: + raise ValueError("program_id must be a non-empty string.") + if type(integration) is not ExpertProgramIntegrationCfg: + raise TypeError("integration must be ExpertProgramIntegrationCfg.") + values = tuple(segments) + if not values or not all( + type(segment) is CompiledProgramSegment for segment in values + ): + raise TypeError( + "segments must contain at least one CompiledProgramSegment." + ) + if tuple(segment.segment_index for segment in values) != tuple( + range(len(values)) + ): + raise ValueError("Materialized segment indices must be contiguous.") + flattened_calls = tuple(call for segment in values for call in segment.calls) + if len(flattened_calls) > MAX_EXPANDED_CALLS: + raise ValueError( + f"Materialized program exceeds {MAX_EXPANDED_CALLS} calls." + ) + if tuple(call.call_index for call in flattened_calls) != tuple( + range(len(flattened_calls)) + ): + raise ValueError("Materialized call indices must be contiguous.") + + instance = object.__new__(cls) + object.__setattr__(instance, "schema_version", schema_version) + object.__setattr__(instance, "program_id", program_id) + object.__setattr__( + instance, + "_integration", + ExpertProgramIntegrationCfg( + robot_profile=integration.robot_profile, + scene_registry=integration.scene_registry, + runtime_preset=integration.runtime_preset, + ), + ) + object.__setattr__(instance, "_segments", values) + return instance + + @property + def integration(self) -> ExpertProgramIntegrationCfg: + """Return an independent integration-selection snapshot.""" + return ExpertProgramIntegrationCfg( + robot_profile=self._integration.robot_profile, + scene_registry=self._integration.scene_registry, + runtime_preset=self._integration.runtime_preset, + ) + + @property + def segment_count(self) -> int: + """Return the number of materialized logical segments.""" + return len(self._segments) + + def iter_segments(self) -> Iterator[CompiledProgramSegment]: + """Iterate the already materialized provider-free segments.""" + return iter(self._segments) + + def preflight_analyses(self) -> tuple[CompiledProgramAnalysis, ...]: + """Return full-program analyses split only at parallel barriers. + + Consecutive sequential segments form one static workflow, preserving + their object-state flow and cross-segment target look-ahead. Each + parallel branch is analyzed independently; no state or target inference + crosses the barrier in either direction. + """ + analyses: list[CompiledProgramAnalysis] = [] + stretch: list[CompiledProgramSegment] = [] + + def flush_stretch() -> None: + if not stretch: + return + indices = tuple(segment.segment_index for segment in stretch) + calls = tuple(call.call for segment in stretch for call in segment.calls) + analyses.append( + CompiledProgramAnalysis( + analysis_id=( + f"{self.program_id}:preflight:sequential:" + f"{indices[0]}-{indices[-1]}" + ), + kind="sequential_stretch", + calls=calls, + source_path=stretch[0].source_path, + segment_indices=indices, + execution_prefix_length=len(calls), + ) + ) + stretch.clear() + + for segment in self._segments: + block = segment.parallel_block + if block is None: + stretch.append(segment) + continue + flush_stretch() + for branch in block.branches: + calls = tuple(call.call for call in branch.calls) + analyses.append( + CompiledProgramAnalysis( + analysis_id=( + f"{self.program_id}:preflight:parallel:" + f"{segment.segment_index}:{branch.branch_index}" + ), + kind="parallel_branch", + calls=calls, + source_path=branch.source_path, + segment_indices=(segment.segment_index,), + execution_prefix_length=len(calls), + ) + ) + flush_stretch() + return tuple(analyses) + + def sequential_execution_analysis( + self, + segment_index: int, + ) -> CompiledProgramAnalysis: + """Return current-segment prefix plus downstream sequential look-ahead. + + Args: + segment_index: Index of the sequential segment about to execute. + + Returns: + Analysis beginning at the selected segment and ending immediately + before the next parallel barrier or the end of the program. + + Raises: + IndexError: If ``segment_index`` is outside this program. + ValueError: If the selected segment is itself parallel. + """ + if type(segment_index) is not int: + raise TypeError("segment_index must be an integer.") + if not 0 <= segment_index < len(self._segments): + raise IndexError(f"segment_index {segment_index!r} is outside the program.") + current = self._segments[segment_index] + if current.parallel_block is not None: + raise ValueError("Parallel segments do not have sequential look-ahead.") + window: list[CompiledProgramSegment] = [] + for segment in self._segments[segment_index:]: + if segment.parallel_block is not None: + break + window.append(segment) + calls = tuple(call.call for segment in window for call in segment.calls) + indices = tuple(segment.segment_index for segment in window) + return CompiledProgramAnalysis( + analysis_id=( + f"{self.program_id}:execution:sequential:" f"{indices[0]}-{indices[-1]}" + ), + kind="sequential_suffix", + calls=calls, + source_path=current.source_path, + segment_indices=indices, + execution_prefix_length=len(current.calls), + ) + + def __iter__(self) -> Iterator[CompiledProgramSegment]: + return self.iter_segments() + + +class ExpertProgramCompiler: + """Compile validated Expert Program ASTs through one typed resolver.""" + + def __init__(self, scene_resolver: ExpertProgramSceneResolver) -> None: + """Create one provider-free compiler. + + Args: + scene_resolver: Static typed scene identity resolver. + """ + if not isinstance(scene_resolver, ExpertProgramSceneResolver): + raise TypeError("scene_resolver must implement ExpertProgramSceneResolver.") + self._scene_resolver = scene_resolver + + @classmethod + def from_scene_registry(cls, registry: SceneRegistry) -> ExpertProgramCompiler: + """Create a compiler from a provider-free SceneRegistry identity snapshot.""" + return cls(SceneRegistryProgramResolver(registry)) + + def _resolve_scene( + self, + reference: str, + *, + expected_types: tuple[type[SceneEntityRef], ...], + path: ConfigPath, + ) -> SceneEntityRef: + """Resolve and validate one exact typed canonical scene reference.""" + try: + resolved = self._scene_resolver.resolve( + reference, + expected_types=expected_types, + path=path, + ) + except ExpertProgramConfigError: + raise + except (KeyError, TypeError, ValueError) as exc: + raise ExpertProgramCompileError( + "scene_resolution_failed", + path, + str(exc), + ) from exc + if type(resolved) not in expected_types: + raise ExpertProgramCompileError( + "scene_resolver_contract_violation", + path, + "Scene resolver returned an incompatible typed reference.", + ) + return _copy_scene_ref(resolved) + + @staticmethod + def _target_id( + reference: TargetRefCfg, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + path: ConfigPath, + ) -> str: + """Resolve one statically registered target ID.""" + if type(reference) is not TargetRefCfg or reference.kind != "target_ref": + raise ExpertProgramCompileError( + "invalid_target_reference", + path, + "Expected an exact target_ref configuration.", + ) + if reference.target not in targets: + raise ExpertProgramCompileError( + "unknown_target", + (*path, "target"), + f"Unknown target {reference.target!r}.", + ) + return reference.target + + def _compile_call( + self, + cfg: object, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + path: ConfigPath, + ) -> _CallTemplate: + """Lower one config call into a provider-free canonical template.""" + if type(cfg) is PickCfg: + if cfg.kind != "pick": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'pick'." + ) + object_ref = self._resolve_scene( + cfg.object, + expected_types=(SceneObjectRef,), + path=(*path, "object"), + ) + grasp_ref = ( + None + if cfg.grasp is None + else self._resolve_scene( + cfg.grasp, + expected_types=(SceneAffordanceRef,), + path=(*path, "grasp"), + ) + ) + return _CallTemplate( + kind="pick", + source_path=path, + object=object_ref, + grasp=grasp_ref, + resources=tuple(sorted(cfg.resources.items())), + ) + if type(cfg) is PlaceCfg: + if cfg.kind != "place": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'place'." + ) + object_ref = self._resolve_scene( + cfg.object, + expected_types=(SceneObjectRef,), + path=(*path, "object"), + ) + at_target_id = ( + None + if cfg.at is None + else self._target_id( + cfg.at, + targets=targets, + path=(*path, "at"), + ) + ) + on = ( + None + if cfg.on is None + else self._resolve_scene( + cfg.on, + expected_types=(SceneObjectRef, SceneAffordanceRef), + path=(*path, "on"), + ) + ) + inside = ( + None + if cfg.inside is None + else self._resolve_scene( + cfg.inside, + expected_types=(SceneObjectRef, SceneAffordanceRef), + path=(*path, "inside"), + ) + ) + return _CallTemplate( + kind="place", + source_path=path, + object=object_ref, + at_target_id=at_target_id, + on=on, + inside=inside, + resources=tuple(sorted(cfg.resources.items())), + ) + if type(cfg) is HandOverCfg: + if cfg.kind != "hand_over": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'hand_over'.", + ) + object_ref = self._resolve_scene( + cfg.object, + expected_types=(SceneObjectRef,), + path=(*path, "object"), + ) + final_target_id = ( + None + if cfg.final_target is None + else self._target_id( + cfg.final_target, + targets=targets, + path=(*path, "final_target"), + ) + ) + return _CallTemplate( + kind="hand_over", + source_path=path, + object=object_ref, + receiver=cfg.receiver, + final_target_id=final_target_id, + resources=tuple(sorted(cfg.resources.items())), + ) + if type(cfg) is OperateArticulationCfg: + if cfg.kind != "operate_articulation": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'operate_articulation'.", + ) + articulation = self._resolve_scene( + cfg.articulation, + expected_types=(SceneArticulationRef,), + path=(*path, "articulation"), + ) + handle = ( + None + if cfg.handle is None + else self._resolve_scene( + cfg.handle, + expected_types=(SceneAffordanceRef,), + path=(*path, "handle"), + ) + ) + snapshot = OperateArticulation( + articulation=articulation, + handle=handle, + target=cfg.target, + target_position=cfg.target_position, + target_displacement=cfg.target_displacement, + resources=cfg.resources, + ) + return _CallTemplate( + kind="operate_articulation", + source_path=path, + articulation=snapshot.articulation, + handle=snapshot.handle, + articulation_target=snapshot.target, + target_position=snapshot.target_position, + target_displacement=snapshot.target_displacement, + resources=tuple(sorted(snapshot.resources.items())), + ) + if type(cfg) is RegisteredSemanticCallCfg: + if cfg.kind != "registered": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'registered'.", + ) + if cfg.schema_version != EXPERT_PROGRAM_SCHEMA_VERSION: + raise ExpertProgramCompileError( + "unsupported_registered_schema", + (*path, "schema_version"), + "Registered call schema_version must be exactly 1.", + ) + snapshot = RegisteredSemanticCall( + call_id=cfg.call_id, + arguments=cfg.arguments, + resources=cfg.resources, + ) + return _CallTemplate( + kind="registered", + source_path=path, + call_id=snapshot.call_id, + arguments=snapshot.arguments, + resources=tuple(sorted(snapshot.resources.items())), + ) + raise ExpertProgramCompileError( + "unsupported_call", + path, + f"Unsupported semantic call config {type(cfg).__name__}.", + ) + + def _compile_node( + self, + node: ProgramNodeCfg, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + path: ConfigPath, + inside_segment: bool, + inside_parallel: bool, + ) -> _NodeTemplate: + """Compile static AST structure without expanding repeats.""" + if type(node) is InvokeCfg: + if node.kind != "invoke": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'invoke'." + ) + return _InvokeTemplate( + call=self._compile_call( + node.call, + targets=targets, + path=(*path, "call"), + ), + source_path=path, + ) + if type(node) is SequenceCfg: + if node.kind != "sequence": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'sequence'.", + ) + if not node.items: + raise ExpertProgramCompileError( + "empty_sequence", + (*path, "items"), + "Sequence items must contain at least one program node.", + ) + return _SequenceTemplate( + items=tuple( + self._compile_node( + child, + targets=targets, + path=(*path, "items", index), + inside_segment=inside_segment, + inside_parallel=inside_parallel, + ) + for index, child in enumerate(node.items) + ), + source_path=path, + ) + if type(node) is RepeatCfg: + if node.kind != "repeat": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'repeat'." + ) + if type(node.count) is not int or not 1 <= node.count <= MAX_REPEAT_COUNT: + raise ExpertProgramCompileError( + "invalid_repeat_count", + (*path, "count"), + f"Repeat count must be an integer in [1, {MAX_REPEAT_COUNT}].", + ) + return _RepeatTemplate( + count=node.count, + body=self._compile_node( + node.body, + targets=targets, + path=(*path, "body"), + inside_segment=inside_segment, + inside_parallel=inside_parallel, + ), + source_path=path, + ) + if type(node) is SegmentCfg: + if inside_parallel: + raise ExpertProgramCompileError( + "segment_inside_parallel", + path, + "Parallel branches may contain only Invoke, Sequence, and " + "Repeat nodes; wrap the Parallel node in one Segment instead.", + ) + if inside_segment: + raise ExpertProgramCompileError( + "nested_segment", + path, + "Nested Segment nodes are ambiguous and forbidden.", + ) + if node.kind != "segment": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'segment'." + ) + post: list[_PostTemplate] = [] + for index, cfg in enumerate(node.post): + post_path = (*path, "post", index) + if type(cfg) is not WaitStablePostCfg or cfg.kind != "wait_stable": + raise ExpertProgramCompileError( + "unsupported_post_policy", + post_path, + "Supported schemas accept only exact wait_stable post policies.", + ) + entity = self._resolve_scene( + cfg.entity, + expected_types=_SCENE_REF_TYPES, + path=(*post_path, "entity"), + ) + post.append( + _PostTemplate( + cfg=WaitStablePostCfg( + entity=cfg.entity, + preset=cfg.preset, + kind=cfg.kind, + ), + entity=entity, + source_path=post_path, + ) + ) + validators: list[_ValidatorTemplate] = [] + for index, cfg in enumerate(node.validators): + validator_path = (*path, "validators", index) + if ( + type(cfg) is not ObjectNearTargetValidatorCfg + or cfg.kind != "object_near_target" + ): + raise ExpertProgramCompileError( + "unsupported_validator", + validator_path, + "Supported schemas accept only exact object_near_target " + "validators.", + ) + if cfg.target not in targets: + raise ExpertProgramCompileError( + "unknown_target", + (*validator_path, "target"), + f"Unknown target {cfg.target!r}.", + ) + object_ref = self._resolve_scene( + cfg.object, + expected_types=(SceneObjectRef,), + path=(*validator_path, "object"), + ) + validators.append( + _ValidatorTemplate( + cfg=ObjectNearTargetValidatorCfg( + object=cfg.object, + target=cfg.target, + position_tolerance=cfg.position_tolerance, + kind=cfg.kind, + ), + object=object_ref, + target_id=cfg.target, + source_path=validator_path, + ) + ) + steps = self._compile_node( + node.steps, + targets=targets, + path=(*path, "steps"), + inside_segment=True, + inside_parallel=False, + ) + if type(steps) is not _ParallelTemplate and _contains_parallel(steps): + raise ExpertProgramCompileError( + "mixed_parallel_segment", + (*path, "steps"), + "A Segment may contain either a call-only program or one direct " + "Parallel node, not a mixed sequential/parallel tree.", + ) + return _SegmentTemplate( + name=node.name, + steps=steps, + post=tuple(post), + validators=tuple(validators), + source_path=path, + ) + if type(node) is ParallelCfg: + if inside_parallel: + raise ExpertProgramCompileError( + "nested_parallel", + path, + "Nested Parallel nodes are forbidden in schema version 2.", + ) + if node.kind != "parallel": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'parallel'.", + ) + if len(node.branches) < 2: + raise ExpertProgramCompileError( + "parallel_branch_count", + (*path, "branches"), + "Parallel requires at least two branches.", + ) + if type(node.barrier) is not BarrierCfg: + raise ExpertProgramCompileError( + "parallel_barrier_required", + (*path, "barrier"), + "Parallel.barrier must be an exact BarrierCfg.", + ) + branches = tuple( + self._compile_node( + branch, + targets=targets, + path=(*path, "branches", index), + inside_segment=inside_segment, + inside_parallel=True, + ) + for index, branch in enumerate(node.branches) + ) + if any(_contains_parallel(branch) for branch in branches): + raise ExpertProgramCompileError( + "nested_parallel", + (*path, "branches"), + "Nested Parallel nodes are forbidden in schema version 2.", + ) + barrier = node.barrier + if barrier.kind != "barrier": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "barrier", "kind"), + "Expected 'barrier'.", + ) + if barrier.failure_policy != "fail_fast": + raise ExpertProgramCompileError( + "unsupported_failure_policy", + (*path, "barrier", "failure_policy"), + "Barrier failure_policy must be exactly 'fail_fast'.", + ) + return _ParallelTemplate( + branches=branches, + barrier=_BarrierTemplate( + name=barrier.name, + timeout_steps=barrier.timeout_steps, + failure_policy=barrier.failure_policy, + source_path=(*path, "barrier"), + ), + source_path=path, + ) + if type(node) is BarrierCfg: + raise ExpertProgramCompileError( + "standalone_barrier", + path, + "Barrier nodes may only be owned by Parallel.", + ) + raise ExpertProgramCompileError( + "unsupported_program_node", + path, + f"Unsupported program node {type(node).__name__}.", + ) + + @staticmethod + def _compile_targets( + targets: Mapping[str, CyclicPoseTargetCfg], + ) -> Mapping[str, tuple[SemanticPose, ...]]: + """Compile static pose providers without selecting repeat values.""" + compiled: dict[str, tuple[SemanticPose, ...]] = {} + for target_id, target in targets.items(): + path = ("targets", target_id) + if type(target) is not CyclicPoseTargetCfg or target.kind != "cyclic_pose": + raise ExpertProgramCompileError( + "unsupported_target", + path, + "Supported schemas accept only exact cyclic_pose targets.", + ) + poses: list[SemanticPose] = [] + if not target.values: + raise ExpertProgramCompileError( + "empty_target_values", + (*path, "values"), + "Cyclic target values must contain at least one pose.", + ) + for index, pose in enumerate(target.values): + if type(pose) is not PoseCfg: + raise ExpertProgramCompileError( + "invalid_pose", + (*path, "values", index), + "Target values must be exact PoseCfg values.", + ) + poses.append(SemanticPose(pose.position, pose.quaternion_wxyz)) + compiled[target_id] = tuple(poses) + return MappingProxyType(compiled) + + def compile(self, config: ExpertProgramCfg) -> CompiledProgram: + """Compile one validated AST into a provider-free lazy program. + + Args: + config: Strict, supported-version Expert Program configuration. + + Returns: + Owned static templates whose iteration resolves repeat-local targets + and emits independent logical segments. + + Raises: + ExpertProgramCompileError: If typed scene resolution or AST lowering + fails. + """ + if type(config) is not ExpertProgramCfg: + raise TypeError("config must be exactly ExpertProgramCfg.") + if config.schema_version not in SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS: + raise ExpertProgramCompileError( + "unsupported_schema_version", + ("schema_version",), + "Supported Expert Program schema versions are " + f"{SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS}.", + ) + targets = self._compile_targets(config.targets) + root = self._compile_node( + config.program, + targets=targets, + path=("program",), + inside_segment=False, + inside_parallel=False, + ) + integration = ExpertProgramIntegrationCfg( + robot_profile=config.integration.robot_profile, + scene_registry=config.integration.scene_registry, + runtime_preset=config.integration.runtime_preset, + ) + return CompiledProgram._create( + schema_version=config.schema_version, + program_id=config.program_id, + integration=integration, + targets=targets, + root=root, + ) + + +__all__ = [ + "CompiledBarrier", + "CompiledParallelBlock", + "CompiledParallelBranch", + "CompiledPostPolicy", + "CompiledProgram", + "CompiledProgramAnalysis", + "CompiledProgramCall", + "CompiledProgramSegment", + "CompiledProgramValidator", + "CompiledRepeatFrame", + "CompiledTargetSelection", + "ExpertProgramCompileError", + "ExpertProgramCompiler", + "ExpertProgramSceneResolver", + "MaterializedCompiledProgram", + "SceneRegistryProgramResolver", +] diff --git a/embodichain/lab/gym/envs/expert_program/decoder.py b/embodichain/lab/gym/envs/expert_program/decoder.py new file mode 100644 index 000000000..966f9be8e --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/decoder.py @@ -0,0 +1,1361 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Strict JSON/YAML-value decoder for Expert Program schema versions 1 and 2.""" + +from __future__ import annotations + +import math +import re +from collections.abc import Callable, Mapping +from copy import deepcopy +from typing import Literal, Protocol, TypeAlias, runtime_checkable + +from .cfg import ( + BarrierCfg, + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, + MAX_PROGRAM_DEPTH, + MAX_REPEAT_COUNT, + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + OperateArticulationCfg, + ParallelCfg, + PickCfg, + PlaceCfg, + PoseCfg, + PostPolicyCfg, + ProgramNodeCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SemanticCallCfg, + SequenceCfg, + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS, + TargetCfg, + TargetRefCfg, + ValidatorCfg, + WaitStablePostCfg, +) + +ConfigPathPart: TypeAlias = str | int +ConfigPath: TypeAlias = tuple[ConfigPathPart, ...] +SceneReferenceRole: TypeAlias = Literal[ + "entity", + "object", + "articulation", + "affordance", + "object_or_affordance", +] + +_MAX_INPUT_DEPTH = 128 +_MAX_INPUT_NODES = 100_000 +_ENV_TRAVERSAL_PATTERN = re.compile( + r"(?:\$?(?:env|environment)(?:\.[A-Za-z_][A-Za-z0-9_]*)+|" + r"\$\{(?:env|environment)(?:\.[A-Za-z_][A-Za-z0-9_]*)+\})" +) +_FORBIDDEN_KEYS = frozenset( + { + "__import__", + "attribute_path", + "callable", + "environment_path", + "env_path", + "eval", + "exec", + "expression", + "getattr", + "import", + "module", + "python", + } +) + + +def render_config_path(path: ConfigPath) -> str: + """Render one configuration path using JSONPath-like notation. + + Args: + path: Tuple of mapping keys and sequence indices. + + Returns: + Stable human-readable path beginning at ``$``. + """ + rendered = "$" + for part in path: + if type(part) is int: + rendered += f"[{part}]" + elif re.fullmatch(r"[A-Za-z_][A-Za-z0-9_]*", part) is not None: + rendered += f".{part}" + else: + rendered += f"[{part!r}]" + return rendered + + +class ExpertProgramConfigError(ValueError): + """Base pathful diagnostic for Expert Program configuration failures.""" + + def __init__(self, code: str, path: ConfigPath, message: str) -> None: + """Create one stable pathful diagnostic. + + Args: + code: Machine-readable failure code. + path: Exact configuration location. + message: Human-readable explanation. + """ + self.code = code + self.path = tuple(path) + self.message = message + super().__init__(f"{render_config_path(self.path)}: {message} [{code}]") + + +class ExpertProgramDecodeError(ExpertProgramConfigError): + """Raised when untrusted data does not match a supported strict schema.""" + + +class ExpertProgramValidationError(ExpertProgramConfigError): + """Raised when an explicit static integration context rejects a reference.""" + + +@runtime_checkable +class ExpertProgramValidationContext(Protocol): + """Provider-free static validation boundary for external references. + + Implementations may resolve profile, scene, preset, catalog, affordance, and + resource IDs, but must not observe simulation state, construct planners, or + execute calls. + """ + + def validate_integration( + self, + integration: ExpertProgramIntegrationCfg, + *, + path: ConfigPath, + ) -> None: + """Validate integration references at ``path``.""" + + def validate_semantic_call( + self, + call: SemanticCallCfg, + *, + path: ConfigPath, + ) -> None: + """Validate catalog identity, schema revision, and resource overrides.""" + + def validate_scene_reference( + self, + reference: str, + *, + role: SceneReferenceRole, + path: ConfigPath, + ) -> None: + """Validate one canonical scene reference with its semantic role.""" + + def validate_post_policy( + self, + policy: PostPolicyCfg, + *, + path: ConfigPath, + ) -> None: + """Validate a post-policy kind and its named preset.""" + + def validate_validator( + self, + validator: ValidatorCfg, + *, + path: ConfigPath, + ) -> None: + """Validate one registered segment-validator contract.""" + + +def _error(code: str, path: ConfigPath, message: str) -> ExpertProgramDecodeError: + """Build one decoder diagnostic.""" + return ExpertProgramDecodeError(code, path, message) + + +def _clone_untrusted_value( + value: object, + *, + path: ConfigPath, + active: set[int], + budget: list[int], + depth: int, +) -> object: + """Own and validate one bounded JSON-compatible value tree.""" + if depth > _MAX_INPUT_DEPTH: + raise _error( + "input_too_deep", + path, + f"Input exceeds nesting depth limit {_MAX_INPUT_DEPTH}.", + ) + budget[0] -= 1 + if budget[0] < 0: + raise _error( + "input_too_large", + path, + f"Input exceeds node limit {_MAX_INPUT_NODES}.", + ) + if value is None or type(value) in (bool, int): + return value + if type(value) is float: + if not math.isfinite(value): + raise _error("non_finite_number", path, "Floats must be finite.") + return value + if type(value) is str: + stripped = value.strip() + lowered = stripped.lower() + if lowered.startswith(("__import__(", "eval(", "exec(", "import ", "from ")): + raise _error( + "executable_expression", + path, + "Imports, eval, exec, and executable expressions are forbidden.", + ) + if _ENV_TRAVERSAL_PATTERN.fullmatch(stripped) is not None: + raise _error( + "environment_traversal", + path, + "Dotted environment attribute traversal is forbidden.", + ) + return value + if type(value) is list: + identity = id(value) + if identity in active: + raise _error("cyclic_input", path, "Input contains a cyclic list.") + active.add(identity) + try: + return [ + _clone_untrusted_value( + item, + path=(*path, index), + active=active, + budget=budget, + depth=depth + 1, + ) + for index, item in enumerate(value) + ] + finally: + active.remove(identity) + if type(value) is dict: + identity = id(value) + if identity in active: + raise _error("cyclic_input", path, "Input contains a cyclic mapping.") + active.add(identity) + try: + result: dict[str, object] = {} + for key, item in value.items(): + if type(key) is not str: + raise _error( + "invalid_mapping_key", + path, + "Mapping keys must be exact strings.", + ) + if key.lower() in _FORBIDDEN_KEYS: + raise _error( + "forbidden_construct", + (*path, key), + f"Field {key!r} requests executable or traversal behavior.", + ) + result[key] = _clone_untrusted_value( + item, + path=(*path, key), + active=active, + budget=budget, + depth=depth + 1, + ) + return result + finally: + active.remove(identity) + raise _error( + "non_declarative_value", + path, + f"{type(value).__name__} is not JSON-compatible declarative data; " + "callables, classes, modules, tensors, and live objects are forbidden.", + ) + + +def _expect_mapping(value: object, *, path: ConfigPath) -> dict[str, object]: + """Require one exact mapping.""" + if type(value) is not dict: + raise _error("expected_mapping", path, "Expected an object mapping.") + return value + + +def _expect_list(value: object, *, path: ConfigPath) -> list[object]: + """Require one exact JSON list.""" + if type(value) is not list: + raise _error("expected_list", path, "Expected a list.") + return value + + +def _validate_fields( + value: dict[str, object], + *, + allowed: frozenset[str], + required: frozenset[str], + path: ConfigPath, +) -> None: + """Reject unknown fields and report the first missing required field.""" + unknown = sorted(set(value).difference(allowed)) + if unknown: + field_name = unknown[0] + raise _error( + "unknown_field", + (*path, field_name), + f"Unknown field {field_name!r}; allowed fields are {sorted(allowed)}.", + ) + missing = sorted(required.difference(value)) + if missing: + field_name = missing[0] + raise _error( + "missing_field", + (*path, field_name), + f"Missing required field {field_name!r}.", + ) + + +def _expect_identifier(value: object, *, path: ConfigPath) -> str: + """Require one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise _error( + "invalid_identifier", + path, + "Expected a non-empty string without outer whitespace.", + ) + return value + + +def _expect_discriminator( + value: dict[str, object], + *, + path: ConfigPath, + supported: tuple[str, ...], +) -> str: + """Read one required exact string discriminator.""" + if "kind" not in value: + raise _error( + "missing_discriminator", + (*path, "kind"), + "Missing required discriminator 'kind'.", + ) + kind = value["kind"] + if type(kind) is not str or kind not in supported: + raise _error( + "unknown_discriminator", + (*path, "kind"), + f"Unsupported discriminator {kind!r}; expected one of {supported}.", + ) + return kind + + +def _decode_resources(value: object, *, path: ConfigPath) -> dict[str, str]: + """Decode one strict slot-to-resource mapping.""" + mapping = _expect_mapping(value, path=path) + return { + _expect_identifier(slot_id, path=(*path, slot_id)): _expect_identifier( + resource_id, + path=(*path, slot_id), + ) + for slot_id, resource_id in mapping.items() + } + + +def _construct( + constructor: Callable[..., object], + *, + path: ConfigPath, + **kwargs: object, +) -> object: + """Construct one config value and wrap invariant failures pathfully.""" + try: + return constructor(**kwargs) + except ExpertProgramConfigError: + raise + except (TypeError, ValueError) as exc: + raise _error("invalid_value", path, str(exc)) from exc + + +def _decode_pose(value: object, *, path: ConfigPath) -> PoseCfg: + """Decode one finite pose value.""" + mapping = _expect_mapping(value, path=path) + _validate_fields( + mapping, + allowed=frozenset({"position", "quaternion_wxyz"}), + required=frozenset({"position", "quaternion_wxyz"}), + path=path, + ) + position_values = _expect_list(mapping["position"], path=(*path, "position")) + quaternion_values = _expect_list( + mapping["quaternion_wxyz"], + path=(*path, "quaternion_wxyz"), + ) + if len(position_values) != 3: + raise _error( + "invalid_pose_shape", + (*path, "position"), + "position must contain exactly three numbers.", + ) + if len(quaternion_values) != 4: + raise _error( + "invalid_pose_shape", + (*path, "quaternion_wxyz"), + "quaternion_wxyz must contain exactly four numbers.", + ) + for name, values in ( + ("position", position_values), + ("quaternion_wxyz", quaternion_values), + ): + for index, number in enumerate(values): + if type(number) not in (int, float): + raise _error( + "invalid_number", + (*path, name, index), + "Pose components must be finite numbers, not bool values.", + ) + return _construct( + PoseCfg, + path=path, + position=tuple(position_values), + quaternion_wxyz=tuple(quaternion_values), + ) # type: ignore[return-value] + + +def _decode_target(value: object, *, path: ConfigPath) -> TargetCfg: + """Decode one target provider shared by the supported schema versions.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator( + mapping, + path=path, + supported=("cyclic_pose",), + ) + assert kind == "cyclic_pose" + _validate_fields( + mapping, + allowed=frozenset({"kind", "values"}), + required=frozenset({"kind", "values"}), + path=path, + ) + values = tuple( + _decode_pose(item, path=(*path, "values", index)) + for index, item in enumerate( + _expect_list(mapping["values"], path=(*path, "values")) + ) + ) + return _construct( + CyclicPoseTargetCfg, + path=path, + kind=kind, + values=values, + ) # type: ignore[return-value] + + +def _decode_target_ref( + value: object, + *, + path: ConfigPath, + target_ids: frozenset[str], +) -> TargetRefCfg: + """Decode and statically resolve one target reference.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator(mapping, path=path, supported=("target_ref",)) + _validate_fields( + mapping, + allowed=frozenset({"kind", "target"}), + required=frozenset({"kind", "target"}), + path=path, + ) + target = _expect_identifier(mapping["target"], path=(*path, "target")) + if target not in target_ids: + raise _error( + "unknown_target", + (*path, "target"), + f"Unknown target {target!r}; available targets are {sorted(target_ids)}.", + ) + return _construct( + TargetRefCfg, + path=path, + kind=kind, + target=target, + ) # type: ignore[return-value] + + +def _decode_call( + value: object, + *, + path: ConfigPath, + target_ids: frozenset[str], +) -> SemanticCallCfg: + """Decode one discriminated semantic call.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator( + mapping, + path=path, + supported=( + "pick", + "place", + "hand_over", + "operate_articulation", + "registered", + ), + ) + resources = _decode_resources( + mapping.get("resources", {}), + path=(*path, "resources"), + ) + if kind == "pick": + _validate_fields( + mapping, + allowed=frozenset({"kind", "object", "grasp", "resources"}), + required=frozenset({"kind", "object"}), + path=path, + ) + grasp = mapping.get("grasp") + if grasp is not None: + grasp = _expect_identifier(grasp, path=(*path, "grasp")) + return _construct( + PickCfg, + path=path, + kind=kind, + object=_expect_identifier(mapping["object"], path=(*path, "object")), + grasp=grasp, + resources=resources, + ) # type: ignore[return-value] + if kind == "place": + _validate_fields( + mapping, + allowed=frozenset({"kind", "object", "at", "on", "inside", "resources"}), + required=frozenset({"kind", "object"}), + path=path, + ) + at = ( + None + if mapping.get("at") is None + else _decode_target_ref( + mapping["at"], + path=(*path, "at"), + target_ids=target_ids, + ) + ) + on = mapping.get("on") + inside = mapping.get("inside") + if on is not None: + on = _expect_identifier(on, path=(*path, "on")) + if inside is not None: + inside = _expect_identifier(inside, path=(*path, "inside")) + return _construct( + PlaceCfg, + path=path, + kind=kind, + object=_expect_identifier(mapping["object"], path=(*path, "object")), + at=at, + on=on, + inside=inside, + resources=resources, + ) # type: ignore[return-value] + if kind == "hand_over": + _validate_fields( + mapping, + allowed=frozenset( + {"kind", "object", "receiver", "final_target", "resources"} + ), + required=frozenset({"kind", "object"}), + path=path, + ) + receiver = mapping.get("receiver") + if receiver is not None: + receiver = _expect_identifier(receiver, path=(*path, "receiver")) + final_target = ( + None + if mapping.get("final_target") is None + else _decode_target_ref( + mapping["final_target"], + path=(*path, "final_target"), + target_ids=target_ids, + ) + ) + return _construct( + HandOverCfg, + path=path, + kind=kind, + object=_expect_identifier(mapping["object"], path=(*path, "object")), + receiver=receiver, + final_target=final_target, + resources=resources, + ) # type: ignore[return-value] + if kind == "operate_articulation": + _validate_fields( + mapping, + allowed=frozenset( + { + "kind", + "articulation", + "handle", + "target", + "target_position", + "target_displacement", + "resources", + } + ), + required=frozenset({"kind", "articulation"}), + path=path, + ) + handle = mapping.get("handle") + target = mapping.get("target") + if handle is not None: + handle = _expect_identifier(handle, path=(*path, "handle")) + if target is not None: + target = _expect_identifier(target, path=(*path, "target")) + target_position = mapping.get("target_position") + target_displacement = mapping.get("target_displacement") + for field_name, value in ( + ("target_position", target_position), + ("target_displacement", target_displacement), + ): + if value is not None and type(value) not in (int, float): + raise _error( + "invalid_number", + (*path, field_name), + f"{field_name} must be a finite number, not bool.", + ) + named = target is not None + explicit_position = target_position is not None + explicit_displacement = target_displacement is not None + if named and (explicit_position or explicit_displacement): + raise _error( + "conflicting_articulation_target", + path, + "target is mutually exclusive with target_position and " + "target_displacement.", + ) + if not named and not (explicit_position and explicit_displacement): + raise _error( + "incomplete_articulation_target", + path, + "Specify target or both target_position and target_displacement.", + ) + return _construct( + OperateArticulationCfg, + path=path, + kind=kind, + articulation=_expect_identifier( + mapping["articulation"], + path=(*path, "articulation"), + ), + handle=handle, + target=target, + target_position=target_position, + target_displacement=target_displacement, + resources=resources, + ) # type: ignore[return-value] + + _validate_fields( + mapping, + allowed=frozenset( + {"kind", "call_id", "schema_version", "arguments", "resources"} + ), + required=frozenset({"kind", "call_id", "schema_version"}), + path=path, + ) + arguments = _expect_mapping( + mapping.get("arguments", {}), + path=(*path, "arguments"), + ) + schema_version = mapping["schema_version"] + if type(schema_version) is not int or schema_version != 1: + raise _error( + "invalid_schema_version", + (*path, "schema_version"), + "Registered call schema_version must be exactly 1.", + ) + return _construct( + RegisteredSemanticCallCfg, + path=path, + kind=kind, + call_id=_expect_identifier(mapping["call_id"], path=(*path, "call_id")), + schema_version=schema_version, + arguments=arguments, + resources=resources, + ) # type: ignore[return-value] + + +def _decode_post_policy(value: object, *, path: ConfigPath) -> PostPolicyCfg: + """Decode one segment post-policy shared by the supported schemas.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator(mapping, path=path, supported=("wait_stable",)) + _validate_fields( + mapping, + allowed=frozenset({"kind", "entity", "preset"}), + required=frozenset({"kind", "entity"}), + path=path, + ) + return _construct( + WaitStablePostCfg, + path=path, + kind=kind, + entity=_expect_identifier(mapping["entity"], path=(*path, "entity")), + preset=_expect_identifier( + mapping.get("preset", "rigid_object"), + path=(*path, "preset"), + ), + ) # type: ignore[return-value] + + +def _decode_validator( + value: object, + *, + path: ConfigPath, + target_ids: frozenset[str], +) -> ValidatorCfg: + """Decode one segment validator shared by the supported schemas.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator( + mapping, + path=path, + supported=("object_near_target",), + ) + _validate_fields( + mapping, + allowed=frozenset({"kind", "object", "target", "position_tolerance"}), + required=frozenset({"kind", "object", "target"}), + path=path, + ) + target = _expect_identifier(mapping["target"], path=(*path, "target")) + if target not in target_ids: + raise _error( + "unknown_target", + (*path, "target"), + f"Unknown target {target!r}; available targets are {sorted(target_ids)}.", + ) + tolerance = mapping.get("position_tolerance", 0.03) + if type(tolerance) not in (int, float): + raise _error( + "invalid_number", + (*path, "position_tolerance"), + "position_tolerance must be a finite number, not bool.", + ) + return _construct( + ObjectNearTargetValidatorCfg, + path=path, + kind=kind, + object=_expect_identifier(mapping["object"], path=(*path, "object")), + target=target, + position_tolerance=tolerance, + ) # type: ignore[return-value] + + +def _decode_program_node( + value: object, + *, + path: ConfigPath, + target_ids: frozenset[str], + depth: int, + schema_version: int, +) -> ProgramNodeCfg: + """Recursively decode one bounded versioned program node.""" + if depth > MAX_PROGRAM_DEPTH: + raise _error( + "program_too_deep", + path, + "Program AST exceeds the configured nesting depth.", + ) + mapping = _expect_mapping(value, path=path) + supported_kinds = ["sequence", "repeat", "segment", "invoke"] + if schema_version >= EXPERT_PROGRAM_SCHEMA_VERSION_V2: + supported_kinds.extend(("parallel", "barrier")) + kind = _expect_discriminator( + mapping, + path=path, + supported=tuple(supported_kinds), + ) + if kind == "sequence": + _validate_fields( + mapping, + allowed=frozenset({"kind", "items"}), + required=frozenset({"kind", "items"}), + path=path, + ) + items = tuple( + _decode_program_node( + item, + path=(*path, "items", index), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ) + for index, item in enumerate( + _expect_list(mapping["items"], path=(*path, "items")) + ) + ) + return _construct( + SequenceCfg, + path=path, + kind=kind, + items=items, + ) # type: ignore[return-value] + if kind == "repeat": + _validate_fields( + mapping, + allowed=frozenset({"kind", "count", "body"}), + required=frozenset({"kind", "count", "body"}), + path=path, + ) + count = mapping["count"] + if type(count) is not int or not 1 <= count <= MAX_REPEAT_COUNT: + raise _error( + "invalid_repeat_count", + (*path, "count"), + f"Repeat count must be an integer in [1, {MAX_REPEAT_COUNT}].", + ) + return _construct( + RepeatCfg, + path=path, + kind=kind, + count=count, + body=_decode_program_node( + mapping["body"], + path=(*path, "body"), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ), + ) # type: ignore[return-value] + if kind == "segment": + _validate_fields( + mapping, + allowed=frozenset({"kind", "name", "steps", "post", "validators"}), + required=frozenset({"kind", "name", "steps"}), + path=path, + ) + post = tuple( + _decode_post_policy(item, path=(*path, "post", index)) + for index, item in enumerate( + _expect_list(mapping.get("post", []), path=(*path, "post")) + ) + ) + validators = tuple( + _decode_validator( + item, + path=(*path, "validators", index), + target_ids=target_ids, + ) + for index, item in enumerate( + _expect_list( + mapping.get("validators", []), + path=(*path, "validators"), + ) + ) + ) + return _construct( + SegmentCfg, + path=path, + kind=kind, + name=_expect_identifier(mapping["name"], path=(*path, "name")), + steps=_decode_program_node( + mapping["steps"], + path=(*path, "steps"), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ), + post=post, + validators=validators, + ) # type: ignore[return-value] + + if kind == "parallel": + _validate_fields( + mapping, + allowed=frozenset({"kind", "branches", "barrier"}), + required=frozenset({"kind", "branches", "barrier"}), + path=path, + ) + branches_values = _expect_list( + mapping["branches"], + path=(*path, "branches"), + ) + if len(branches_values) < 2: + raise _error( + "parallel_branch_count", + (*path, "branches"), + "Parallel requires at least two branches.", + ) + barrier = _decode_program_node( + mapping["barrier"], + path=(*path, "barrier"), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ) + if type(barrier) is not BarrierCfg: + raise _error( + "parallel_barrier_required", + (*path, "barrier"), + "Parallel.barrier must be an explicit barrier node.", + ) + return _construct( + ParallelCfg, + path=path, + kind=kind, + branches=tuple( + _decode_program_node( + branch, + path=(*path, "branches", index), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ) + for index, branch in enumerate(branches_values) + ), + barrier=barrier, + ) # type: ignore[return-value] + if kind == "barrier": + _validate_fields( + mapping, + allowed=frozenset({"kind", "name", "timeout_steps", "failure_policy"}), + required=frozenset({"kind", "name"}), + path=path, + ) + timeout_steps = mapping.get("timeout_steps", 1_000) + if type(timeout_steps) is not int or timeout_steps <= 0: + raise _error( + "invalid_barrier_timeout", + (*path, "timeout_steps"), + "Barrier timeout_steps must be a positive integer.", + ) + failure_policy = mapping.get("failure_policy", "fail_fast") + if failure_policy != "fail_fast": + raise _error( + "unsupported_failure_policy", + (*path, "failure_policy"), + "Barrier failure_policy must be exactly 'fail_fast'.", + ) + return _construct( + BarrierCfg, + path=path, + kind=kind, + name=_expect_identifier(mapping["name"], path=(*path, "name")), + timeout_steps=timeout_steps, + failure_policy=failure_policy, + ) # type: ignore[return-value] + + _validate_fields( + mapping, + allowed=frozenset({"kind", "call"}), + required=frozenset({"kind", "call"}), + path=path, + ) + return _construct( + InvokeCfg, + path=path, + kind=kind, + call=_decode_call( + mapping["call"], + path=(*path, "call"), + target_ids=target_ids, + ), + ) # type: ignore[return-value] + + +def _walk_program( + node: ProgramNodeCfg, + *, + path: ConfigPath, +) -> list[tuple[ProgramNodeCfg, ConfigPath]]: + """Return deterministic node/path pairs for static context validation.""" + values = [(node, path)] + if type(node) is SequenceCfg: + for index, child in enumerate(node.items): + values.extend(_walk_program(child, path=(*path, "items", index))) + elif type(node) is RepeatCfg: + values.extend(_walk_program(node.body, path=(*path, "body"))) + elif type(node) is SegmentCfg: + values.extend(_walk_program(node.steps, path=(*path, "steps"))) + elif type(node) is ParallelCfg: + for index, branch in enumerate(node.branches): + values.extend(_walk_program(branch, path=(*path, "branches", index))) + values.extend(_walk_program(node.barrier, path=(*path, "barrier"))) + return values + + +def _call_context( + callback: Callable[..., None], + *args: object, + path: ConfigPath, + **kwargs: object, +) -> None: + """Call one static validation hook and preserve pathful failures.""" + try: + callback(*args, path=path, **kwargs) + except ExpertProgramConfigError: + raise + except (KeyError, TypeError, ValueError) as exc: + raise ExpertProgramValidationError( + "reference_validation_failed", + path, + str(exc), + ) from exc + + +def _validate_decoded_semantic_call( + call: SemanticCallCfg, + context: ExpertProgramValidationContext, + *, + path: ConfigPath, +) -> None: + """Validate one decoded call against provider-free catalog and scene ports.""" + _call_context(context.validate_semantic_call, call, path=path) + if type(call) in (PickCfg, PlaceCfg, HandOverCfg): + _call_context( + context.validate_scene_reference, + call.object, + role="object", + path=(*path, "object"), + ) + if type(call) is PickCfg and call.grasp is not None: + _call_context( + context.validate_scene_reference, + call.grasp, + role="affordance", + path=(*path, "grasp"), + ) + if type(call) is PlaceCfg: + for field_name in ("on", "inside"): + reference = getattr(call, field_name) + if reference is not None: + _call_context( + context.validate_scene_reference, + reference, + role="object_or_affordance", + path=(*path, field_name), + ) + if type(call) is OperateArticulationCfg: + _call_context( + context.validate_scene_reference, + call.articulation, + role="articulation", + path=(*path, "articulation"), + ) + if call.handle is not None: + _call_context( + context.validate_scene_reference, + call.handle, + role="affordance", + path=(*path, "handle"), + ) + + +def decode_semantic_call( + data: object, + *, + target_ids: frozenset[str] = frozenset(), + validation_context: ExpertProgramValidationContext | None = None, + path: ConfigPath = ("call",), +) -> SemanticCallCfg: + """Decode one untrusted canonical semantic-call payload. + + Args: + data: Exact JSON-compatible call mapping produced by a trusted parser. + target_ids: Target IDs available to ``at`` or ``final_target`` refs. + validation_context: Optional provider-free catalog and scene validator. + path: Diagnostic root used when the call is embedded in another schema. + + Returns: + Fully owned and strictly validated canonical semantic-call config. + + Raises: + ExpertProgramDecodeError: If the payload violates the call schema. + ExpertProgramValidationError: If provider-free validation rejects it. + """ + if type(target_ids) is not frozenset or not all( + type(target_id) is str and target_id and target_id == target_id.strip() + for target_id in target_ids + ): + raise TypeError("target_ids must be a frozenset of exact identifiers.") + if type(path) is not tuple: + raise TypeError("path must be a ConfigPath tuple.") + owned = _clone_untrusted_value( + data, + path=path, + active=set(), + budget=[_MAX_INPUT_NODES], + depth=0, + ) + call = _decode_call(owned, path=path, target_ids=target_ids) + if validation_context is not None: + if not isinstance(validation_context, ExpertProgramValidationContext): + raise TypeError( + "validation_context must implement " "ExpertProgramValidationContext." + ) + _validate_decoded_semantic_call(call, validation_context, path=path) + return call + + +def encode_semantic_call(call: SemanticCallCfg) -> dict[str, object]: + """Encode one canonical semantic-call config as owned JSON-safe values. + + Args: + call: Exact supported semantic-call config. + + Returns: + Deterministic mapping accepted by :func:`decode_semantic_call`. + """ + if type(call) not in ( + PickCfg, + PlaceCfg, + HandOverCfg, + OperateArticulationCfg, + RegisteredSemanticCallCfg, + ): + raise TypeError("call must be an exact SemanticCallCfg value.") + result: dict[str, object] = {"kind": call.kind} + if type(call) is PickCfg: + result["object"] = call.object + if call.grasp is not None: + result["grasp"] = call.grasp + elif type(call) is PlaceCfg: + result["object"] = call.object + if call.at is not None: + result["at"] = {"kind": call.at.kind, "target": call.at.target} + if call.on is not None: + result["on"] = call.on + if call.inside is not None: + result["inside"] = call.inside + elif type(call) is HandOverCfg: + result["object"] = call.object + if call.receiver is not None: + result["receiver"] = call.receiver + if call.final_target is not None: + result["final_target"] = { + "kind": call.final_target.kind, + "target": call.final_target.target, + } + elif type(call) is OperateArticulationCfg: + result["articulation"] = call.articulation + if call.handle is not None: + result["handle"] = call.handle + if call.target is not None: + result["target"] = call.target + else: + result["target_position"] = call.target_position + result["target_displacement"] = call.target_displacement + else: + assert type(call) is RegisteredSemanticCallCfg + result["call_id"] = call.call_id + result["schema_version"] = call.schema_version + result["arguments"] = _encode_declarative_value(call.arguments) + if call.resources: + result["resources"] = dict(call.resources) + return result + + +def _encode_declarative_value(value: object) -> object: + """Convert an owned config snapshot back to exact JSON-compatible values.""" + if value is None or type(value) in (bool, int, float, str): + return value + if isinstance(value, Mapping): + return { + str(key): _encode_declarative_value(nested) for key, nested in value.items() + } + if isinstance(value, (tuple, list)): + return [_encode_declarative_value(nested) for nested in value] + return deepcopy(value) + + +def validate_expert_program( + config: ExpertProgramCfg, + context: ExpertProgramValidationContext, +) -> None: + """Resolve external references without observing or executing an environment. + + Args: + config: Fully decoded and internally validated Expert Program. + context: Provider-free static integration/catalog/scene validator. + + Raises: + TypeError: If either argument has the wrong contract. + ExpertProgramValidationError: If an external reference is unavailable. + """ + if type(config) is not ExpertProgramCfg: + raise TypeError("config must be exactly ExpertProgramCfg.") + if not isinstance(context, ExpertProgramValidationContext): + raise TypeError( + "context must implement ExpertProgramValidationContext exactly." + ) + _call_context( + context.validate_integration, + config.integration, + path=("integration",), + ) + for node, path in _walk_program(config.program, path=("program",)): + if type(node) is InvokeCfg: + call = node.call + call_path = (*path, "call") + _validate_decoded_semantic_call(call, context, path=call_path) + elif type(node) is SegmentCfg: + for index, post in enumerate(node.post): + post_path = (*path, "post", index) + _call_context( + context.validate_post_policy, + post, + path=post_path, + ) + _call_context( + context.validate_scene_reference, + post.entity, + role="entity", + path=(*post_path, "entity"), + ) + for index, validator in enumerate(node.validators): + validator_path = (*path, "validators", index) + _call_context( + context.validate_validator, + validator, + path=validator_path, + ) + _call_context( + context.validate_scene_reference, + validator.object, + role="object", + path=(*validator_path, "object"), + ) + + +def decode_expert_program( + data: object, + *, + validation_context: ExpertProgramValidationContext | None = None, +) -> ExpertProgramCfg: + """Decode untrusted JSON/YAML-shaped values into strict versioned config. + + Schema versions 1 and 2 are supported. Version 2 adds deterministic + parallel blocks with explicit barriers while preserving the Version 1 + sequential nodes and semantic calls. + + Args: + data: Exact JSON-compatible mapping produced by a trusted parser. + validation_context: Optional provider-free static reference validator. + + Returns: + Fully owned and internally validated Expert Program configuration. + + Raises: + ExpertProgramDecodeError: If data is unsafe or violates the schema. + ExpertProgramValidationError: If an explicit context rejects a reference. + """ + owned = _clone_untrusted_value( + data, + path=(), + active=set(), + budget=[_MAX_INPUT_NODES], + depth=0, + ) + mapping = _expect_mapping(owned, path=()) + _validate_fields( + mapping, + allowed=frozenset( + {"schema_version", "program_id", "integration", "targets", "program"} + ), + required=frozenset( + {"schema_version", "program_id", "integration", "targets", "program"} + ), + path=(), + ) + schema_version = mapping["schema_version"] + if ( + type(schema_version) is not int + or schema_version not in SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS + ): + raise _error( + "unsupported_schema_version", + ("schema_version",), + "Supported schema versions are " + f"{list(SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS)}.", + ) + + integration_mapping = _expect_mapping( + mapping["integration"], + path=("integration",), + ) + _validate_fields( + integration_mapping, + allowed=frozenset({"robot_profile", "scene_registry", "runtime_preset"}), + required=frozenset({"robot_profile", "scene_registry", "runtime_preset"}), + path=("integration",), + ) + integration = _construct( + ExpertProgramIntegrationCfg, + path=("integration",), + robot_profile=_expect_identifier( + integration_mapping["robot_profile"], + path=("integration", "robot_profile"), + ), + scene_registry=_expect_identifier( + integration_mapping["scene_registry"], + path=("integration", "scene_registry"), + ), + runtime_preset=_expect_identifier( + integration_mapping["runtime_preset"], + path=("integration", "runtime_preset"), + ), + ) + + target_mapping = _expect_mapping(mapping["targets"], path=("targets",)) + targets: dict[str, TargetCfg] = {} + for target_id, target_value in target_mapping.items(): + normalized_id = _expect_identifier(target_id, path=("targets", target_id)) + targets[normalized_id] = _decode_target( + target_value, + path=("targets", normalized_id), + ) + target_ids = frozenset(targets) + program = _decode_program_node( + mapping["program"], + path=("program",), + target_ids=target_ids, + depth=0, + schema_version=schema_version, + ) + config = _construct( + ExpertProgramCfg, + path=(), + schema_version=schema_version, + program_id=_expect_identifier(mapping["program_id"], path=("program_id",)), + integration=integration, + targets=targets, + program=program, + ) + assert type(config) is ExpertProgramCfg + if validation_context is not None: + validate_expert_program(config, validation_context) + return config + + +__all__ = [ + "ConfigPath", + "ConfigPathPart", + "ExpertProgramConfigError", + "ExpertProgramDecodeError", + "ExpertProgramValidationContext", + "ExpertProgramValidationError", + "SceneReferenceRole", + "decode_expert_program", + "decode_semantic_call", + "encode_semantic_call", + "render_config_path", + "validate_expert_program", +] diff --git a/embodichain/lab/gym/envs/expert_program/environment.py b/embodichain/lab/gym/envs/expert_program/environment.py new file mode 100644 index 000000000..d0167a935 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/environment.py @@ -0,0 +1,843 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Explicit production assembly for environment-backed Expert Programs. + +The adapter in this module is deliberately strict. It does not scan a +simulation, infer robot resources, or manufacture task semantics from naming +conventions. An environment supplies one typed factory that owns all live +provider choices; the adapter validates those declarations and wires the +shared semantic compiler, runtime, and Gym bridge. +""" + +from __future__ import annotations + +from collections.abc import Iterable, Mapping +from dataclasses import dataclass +import math +from typing import Protocol, runtime_checkable + +from embodichain.lab.sim.atomic_actions.engine import AtomicActionEngine +from embodichain.lab.sim.atomic_actions.runner import ( + ExecutionRunnerCfg, + ObservationProvider, +) +from embodichain.lab.sim.skills.calls import ( + SemanticCallCatalog, + SemanticCallSpec, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.compiler import ( + HandOverPoseProvider, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticSkillCompiler, +) +from embodichain.lab.sim.skills.effects import EffectMonitorRegistry +from embodichain.lab.sim.skills.evidence import ( + EffectEvidenceCollector, + EffectEvidenceProvider, + EffectEvidenceProviderRegistry, +) +from embodichain.lab.sim.skills.integration import ( + SceneManifest, + SemanticIntegrationManifest, +) +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, + analyze_parallel_branches, +) +from embodichain.lab.sim.skills.profiles import ( + ResourceEndpoint, + ResourceEndpointAdapter, + RobotSkillProfile, +) +from embodichain.lab.sim.skills.runtime import SkillRuntime +from embodichain.lab.sim.skills.scene import SceneRegistry + +from .bridge import ( + AcceptedRuntimeCommandObserver, + AtomicDemoBridge, + BufferedGymCommandSink, + CurrentQposProvider, + DemoBridgeError, + EnvironmentStepClock, + RuntimeCommandFrameEncoder, + RuntimeTransportActionEncoder, + SegmentPostPolicyPort, + SegmentValidatorPort, +) +from .cfg import ExpertProgramCfg, ExpertProgramIntegrationCfg +from .compiler import ( + CompiledProgram, + ExpertProgramCompiler, + MaterializedCompiledProgram, +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate one stable integration identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +@runtime_checkable +class PlanningObservationPort( + ObservationProvider, + CurrentQposProvider, + Protocol, +): + """Combined observation and full-qpos port required by the Gym runtime.""" + + +@runtime_checkable +class ExpertProgramEnvironmentFactory(Protocol): + """Environment-owned factories for one explicit semantic integration. + + Implementations normally live in reusable robot/task integration modules, + not in individual task motion planners. Every method is passed the exact + objects selected earlier in the assembly so a factory cannot silently bind + a different scene, robot profile, or engine. + """ + + @property + def scene_registry_id(self) -> str: + """Return the configuration ID selecting this scene declaration. + + Returns: + Stable scene-registry identifier. + """ + + @property + def robot_profile_id(self) -> str: + """Return the configuration ID selecting this robot profile. + + Returns: + Stable robot-profile identifier. + """ + + def create_scene_registry(self) -> SceneRegistry: + """Create the authoritative explicitly registered live scene. + + Returns: + Fresh registry containing only explicitly selected entities. + """ + + def create_robot_skill_profile(self) -> RobotSkillProfile: + """Create the authoritative declarative robot skill profile. + + Returns: + Profile whose ID matches :attr:`robot_profile_id`. + """ + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + """Create an engine for exactly ``profile`` and its motion backend. + + Args: + profile: Profile selected and validated by the adapter. + + Returns: + Atomic engine connected to the environment's robot and planner. + """ + + def create_planning_observation_provider( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + clock: EnvironmentStepClock, + ) -> PlanningObservationPort: + """Create fresh planning observations and aligned full-qpos reads. + + Args: + scene_registry: Exact registry selected for this runtime. + engine: Exact atomic engine selected for this runtime. + clock: Shared environment-step execution clock. + + Returns: + Combined planning-observation and qpos provider. + """ + + def create_effect_evidence_providers( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> Iterable[EffectEvidenceProvider]: + """Create exact-version providers used by semantic effect monitors. + + Args: + scene_registry: Exact registry selected for this runtime. + engine: Exact atomic engine selected for this runtime. + observation_provider: Shared planning observation provider. + + Returns: + Explicit provider set; an empty iterable is permitted. + """ + + +@runtime_checkable +class AcceptedRuntimeCommandObserverFactory(Protocol): + """Optional factory capability for runtime-local accepted-command state. + + The observer is created from the exact observation provider used by the + runtime so command-derived evidence cannot leak across bridge instances or + bind to a different simulation batch. + """ + + def create_accepted_runtime_command_observer( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> AcceptedRuntimeCommandObserver: + """Return the observer shared by the command sink and evidence ports.""" + + +@dataclass(frozen=True, slots=True) +class ExpertProgramRuntimeAssembly: + """Auditable result of one fresh environment runtime assembly. + + Attributes: + integration: Owned integration-selection snapshot. + scene_registry: Authoritative live scene registry. + robot_profile: Declarative robot resource profile. + manifest: Static scene/profile/call integration manifest. + engine: Bound atomic action engine. + compiler: Bound semantic skill compiler. + observation_provider: Shared planning and full-qpos provider. + evidence_collector: Exact-version semantic evidence collector. + clock: Shared environment-step clock. + command_encoder: Runtime-frame to Gym-action encoder. + command_sink: Buffered Gym command sink. + accepted_command_observer: Optional transactional command-state owner. + runtime: Nonblocking semantic skill runtime. + """ + + integration: ExpertProgramIntegrationCfg + scene_registry: SceneRegistry + robot_profile: RobotSkillProfile + manifest: SemanticIntegrationManifest + engine: AtomicActionEngine + compiler: SemanticSkillCompiler + observation_provider: PlanningObservationPort + evidence_collector: EffectEvidenceCollector + clock: EnvironmentStepClock + command_encoder: RuntimeCommandFrameEncoder + command_sink: BufferedGymCommandSink + accepted_command_observer: AcceptedRuntimeCommandObserver | None + runtime: SkillRuntime + + +@runtime_checkable +class SkillRuntimeAssemblyPort(Protocol): + """Narrow factory port for frontends that need one canonical runtime.""" + + def assemble_runtime( + self, + integration: ExpertProgramIntegrationCfg, + ) -> ExpertProgramRuntimeAssembly: + """Create a fresh runtime assembly for an exact integration selection.""" + + +@dataclass(frozen=True, slots=True) +class _ExpertProgramSemanticAssembly: + """Observation-free semantic components prepared for program preflight.""" + + integration: ExpertProgramIntegrationCfg + scene_registry: SceneRegistry + robot_profile: RobotSkillProfile + manifest: SemanticIntegrationManifest + engine: AtomicActionEngine + compiler: SemanticSkillCompiler + + +class ExpertProgramEnvironmentAdapter: + """Compile and run Expert Programs through explicit environment factories. + + Args: + factory: Environment-owned live-provider and engine factory. + step_dt: Authoritative Gym control cadence in seconds. + call_catalog: Optional immutable semantic call catalog. The built-in + catalog is used when omitted. + endpoint_adapters: Optional custom robot endpoint adapters. + registered_lowerers: Explicit lowerers for registered semantic calls. + relation_grounders: Explicit relation-target grounding providers. + handover_pose_providers: Explicit embodiment hand-over providers. + effect_monitor_registry: Optional exact-version monitor registry. + runtime_transports: Additional runtime-command-to-Gym encoders. + runner_cfg: Optional execution-runner policy. + post_policy_port: Optional environment post-policy executor. + validator_port: Optional environment segment validator. + parallel_safety_validator: Optional authoritative parallel safety gate. + + A call to :meth:`compile` snapshots only scene identities. A call to + :meth:`assemble_runtime` creates a fresh live runtime, which makes reset and + episode ownership explicit and avoids retaining providers in compiled data. + """ + + def __init__( + self, + factory: ExpertProgramEnvironmentFactory, + *, + step_dt: float, + call_catalog: SemanticCallCatalog | None = None, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), + relation_grounders: Iterable[RelationTargetGrounder] = (), + handover_pose_providers: Iterable[HandOverPoseProvider] = (), + effect_monitor_registry: EffectMonitorRegistry | None = None, + runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), + runner_cfg: ExecutionRunnerCfg | None = None, + post_policy_port: SegmentPostPolicyPort | None = None, + validator_port: SegmentValidatorPort | None = None, + parallel_safety_validator: ParallelCommandSafetyValidator | None = None, + ) -> None: + if not isinstance(factory, ExpertProgramEnvironmentFactory): + raise TypeError("factory must implement ExpertProgramEnvironmentFactory.") + if not isinstance(step_dt, (int, float)) or isinstance(step_dt, bool): + raise TypeError("step_dt must be a real number.") + if not math.isfinite(float(step_dt)) or float(step_dt) <= 0.0: + raise ValueError("step_dt must be finite and positive.") + scene_registry_id = _validate_identifier( + factory.scene_registry_id, + field_name="factory.scene_registry_id", + ) + robot_profile_id = _validate_identifier( + factory.robot_profile_id, + field_name="factory.robot_profile_id", + ) + selected_catalog = call_catalog or builtin_semantic_call_catalog() + if type(selected_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog or None.") + if endpoint_adapters is not None and not isinstance(endpoint_adapters, Mapping): + raise TypeError("endpoint_adapters must be a mapping or None.") + if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") + if post_policy_port is not None and not isinstance( + post_policy_port, + SegmentPostPolicyPort, + ): + raise TypeError( + "post_policy_port must implement SegmentPostPolicyPort or be None." + ) + if validator_port is not None and not isinstance( + validator_port, + SegmentValidatorPort, + ): + raise TypeError( + "validator_port must implement SegmentValidatorPort or be None." + ) + if parallel_safety_validator is not None and not isinstance( + parallel_safety_validator, + ParallelCommandSafetyValidator, + ): + raise TypeError( + "parallel_safety_validator must implement " + "ParallelCommandSafetyValidator or be None." + ) + + self._factory = factory + self._scene_registry_id = scene_registry_id + self._robot_profile_id = robot_profile_id + self._step_dt = float(step_dt) + self._call_catalog = selected_catalog + self._endpoint_adapters = ( + None if endpoint_adapters is None else dict(endpoint_adapters) + ) + self._registered_lowerers = tuple(registered_lowerers) + self._relation_grounders = tuple(relation_grounders) + self._handover_pose_providers = tuple(handover_pose_providers) + self._effect_monitor_registry = effect_monitor_registry + self._runtime_transports = tuple(runtime_transports) + self._runner_cfg = runner_cfg + self._post_policy_port = post_policy_port + self._validator_port = validator_port + self._parallel_safety_validator = parallel_safety_validator + + @property + def scene_registry_id(self) -> str: + """Return the exact scene integration ID accepted by this adapter. + + Returns: + Stable scene-registry identifier. + """ + return self._scene_registry_id + + @property + def robot_profile_id(self) -> str: + """Return the exact robot profile ID accepted by this adapter. + + Returns: + Stable robot-profile identifier. + """ + return self._robot_profile_id + + @property + def step_dt(self) -> float: + """Return the authoritative environment-step cadence. + + Returns: + Positive control step duration in seconds. + """ + return self._step_dt + + def compile(self, program: ExpertProgramCfg) -> CompiledProgram: + """Compile one program after exact integration-selection validation. + + Args: + program: Strict declarative program configuration. + + Returns: + Provider-free lazily expanded compiled program. + """ + if type(program) is not ExpertProgramCfg: + raise TypeError("program must be exactly ExpertProgramCfg.") + self._validate_selection(program.integration) + registry = self._create_scene_registry() + return ExpertProgramCompiler.from_scene_registry(registry).compile(program) + + def assemble_runtime( + self, + integration: ExpertProgramIntegrationCfg, + ) -> ExpertProgramRuntimeAssembly: + """Create a fresh fully connected semantic runtime. + + Args: + integration: Exact scene, profile, and runtime-preset selection. + + Returns: + Owned assembly containing every validated runtime boundary. + """ + semantic = self._assemble_semantic_components(integration) + return self._assemble_execution_runtime(semantic) + + def _assemble_semantic_components( + self, + integration: ExpertProgramIntegrationCfg, + ) -> _ExpertProgramSemanticAssembly: + """Bind compiler dependencies without observation or evidence ports.""" + self._validate_selection(integration) + registry = self._create_scene_registry() + current_profile_id = _validate_identifier( + self._factory.robot_profile_id, + field_name="factory.robot_profile_id", + ) + if current_profile_id != self._robot_profile_id: + raise ValueError( + "Factory robot profile declaration drifted: expected " + f"{self._robot_profile_id!r}, got {current_profile_id!r}." + ) + profile = self._factory.create_robot_skill_profile() + if type(profile) is not RobotSkillProfile: + raise TypeError( + "create_robot_skill_profile() must return exactly RobotSkillProfile." + ) + if profile.profile_id != self._robot_profile_id: + raise ValueError( + "Factory robot profile declaration drifted: expected " + f"{self._robot_profile_id!r}, got {profile.profile_id!r}." + ) + + engine = self._factory.create_atomic_action_engine(profile) + if not isinstance(engine, AtomicActionEngine): + raise TypeError( + "create_atomic_action_engine() must return an AtomicActionEngine." + ) + + manifest = self._create_manifest( + registry, + profile, + runtime_preset=integration.runtime_preset, + ) + bound = manifest.bind( + registry, + engine, + endpoint_adapters=self._endpoint_adapters, + ) + compiler = SemanticSkillCompiler( + bound, + registered_lowerers=self._registered_lowerers, + relation_grounders=self._relation_grounders, + handover_pose_providers=self._handover_pose_providers, + effect_monitor_registry=self._effect_monitor_registry, + ) + + selection = ExpertProgramIntegrationCfg( + robot_profile=integration.robot_profile, + scene_registry=integration.scene_registry, + runtime_preset=integration.runtime_preset, + ) + return _ExpertProgramSemanticAssembly( + integration=selection, + scene_registry=registry, + robot_profile=profile, + manifest=manifest, + engine=engine, + compiler=compiler, + ) + + def _assemble_execution_runtime( + self, + semantic: _ExpertProgramSemanticAssembly, + ) -> ExpertProgramRuntimeAssembly: + """Attach live observation, evidence, command, and runtime boundaries.""" + if type(semantic) is not _ExpertProgramSemanticAssembly: + raise TypeError("semantic must be exactly _ExpertProgramSemanticAssembly.") + + clock = EnvironmentStepClock(self._step_dt) + observation_provider = self._factory.create_planning_observation_provider( + scene_registry=semantic.scene_registry, + engine=semantic.engine, + clock=clock, + ) + if not isinstance(observation_provider, PlanningObservationPort): + raise TypeError( + "create_planning_observation_provider() must return a port " + "implementing both ObservationProvider and CurrentQposProvider." + ) + providers = self._factory.create_effect_evidence_providers( + scene_registry=semantic.scene_registry, + engine=semantic.engine, + observation_provider=observation_provider, + ) + if isinstance(providers, (str, bytes)): + raise TypeError( + "create_effect_evidence_providers() must return an iterable of " + "EffectEvidenceProvider values." + ) + try: + provider_values = tuple(providers) + except TypeError as exc: + raise TypeError( + "create_effect_evidence_providers() must return an iterable of " + "EffectEvidenceProvider values." + ) from exc + evidence_collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry(provider_values) + ) + command_encoder = RuntimeCommandFrameEncoder( + observation_provider, + transports=self._runtime_transports, + ) + accepted_command_observer: AcceptedRuntimeCommandObserver | None = None + if isinstance(self._factory, AcceptedRuntimeCommandObserverFactory): + accepted_command_observer = ( + self._factory.create_accepted_runtime_command_observer( + scene_registry=semantic.scene_registry, + engine=semantic.engine, + observation_provider=observation_provider, + ) + ) + if not isinstance( + accepted_command_observer, + AcceptedRuntimeCommandObserver, + ): + raise TypeError( + "create_accepted_runtime_command_observer() must return an " + "AcceptedRuntimeCommandObserver." + ) + command_sink = BufferedGymCommandSink( + command_encoder, + clock, + accepted_command_observer=accepted_command_observer, + ) + runtime = SkillRuntime.from_components( + semantic.compiler, + observation_provider, + command_sink, + evidence_collector, + clock=clock, + runner_cfg=self._runner_cfg, + ) + return ExpertProgramRuntimeAssembly( + integration=semantic.integration, + scene_registry=semantic.scene_registry, + robot_profile=semantic.robot_profile, + manifest=semantic.manifest, + engine=semantic.engine, + compiler=semantic.compiler, + observation_provider=observation_provider, + evidence_collector=evidence_collector, + clock=clock, + command_encoder=command_encoder, + command_sink=command_sink, + accepted_command_observer=accepted_command_observer, + runtime=runtime, + ) + + def create_bridge(self, program: CompiledProgram) -> AtomicDemoBridge: + """Create a fresh Gym bridge for one provider-free compiled program. + + Args: + program: Program compiled for this adapter's exact integration IDs. + + Returns: + Lazy bridge sharing one newly assembled runtime, clock, and sink. + """ + if type(program) is not CompiledProgram: + raise TypeError("program must be exactly CompiledProgram.") + materialized = program.materialize() + self._validate_selection(materialized.integration) + self._preflight_program_surfaces(materialized) + semantic = self._assemble_semantic_components(materialized.integration) + self._preflight_program(materialized, semantic.compiler) + assembly = self._assemble_execution_runtime(semantic) + return AtomicDemoBridge( + materialized, + assembly.runtime, + assembly.command_sink, + assembly.clock, + post_policy_port=self._post_policy_port, + validator_port=self._validator_port, + parallel_safety_validator=self._parallel_safety_validator, + ) + + def _preflight_program_surfaces( + self, + program: MaterializedCompiledProgram, + ) -> None: + """Validate every segment hook without live observation or action.""" + if type(program) is not MaterializedCompiledProgram: + raise TypeError("program must be exactly MaterializedCompiledProgram.") + for segment in program.iter_segments(): + if segment.post_policies and self._post_policy_port is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} declares post-policies, but no " + "SegmentPostPolicyPort was installed." + ) + for policy in segment.post_policies: + assert self._post_policy_port is not None + self._post_policy_port.validate_policy(policy, segment=segment) + if segment.validators and self._validator_port is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} declares validators, but no " + "SegmentValidatorPort was installed." + ) + for validator in segment.validators: + assert self._validator_port is not None + self._validator_port.validate_validator( + validator, + segment=segment, + ) + + def _preflight_program( + self, + program: MaterializedCompiledProgram, + compiler: SemanticSkillCompiler, + ) -> None: + """Analyze every program workflow before any physical action can run. + + Sequential stretches retain cross-segment state flow and target + look-ahead. A parallel barrier cuts that flow; each branch is checked + independently through the same canonical semantic compiler used by the + runtime. This boundary materializes no observations and starts no + execution session. + """ + if type(program) is not MaterializedCompiledProgram: + raise TypeError("program must be exactly MaterializedCompiledProgram.") + if not isinstance(compiler, SemanticSkillCompiler): + raise TypeError("compiler must be a SemanticSkillCompiler.") + analyses = program.preflight_analyses() + if any(analysis.kind == "parallel_branch" for analysis in analyses) and ( + self._parallel_safety_validator is None + ): + raise ValueError( + "Expert Programs containing parallel blocks require an explicit " + "ParallelCommandSafetyValidator before bridge creation." + ) + index = 0 + while index < len(analyses): + analysis = analyses[index] + if analysis.kind != "parallel_branch": + compiler.analyze( + analysis.calls, + workflow_id=analysis.analysis_id, + path=analysis.source_path, + ) + index += 1 + continue + segment_index = analysis.segment_indices[0] + branches: dict[str, tuple[SemanticCallSpec, ...]] = {} + branch_paths: dict[str, tuple[str | int, ...]] = {} + while index < len(analyses): + branch = analyses[index] + if branch.kind != "parallel_branch" or branch.segment_indices != ( + segment_index, + ): + break + branch_id = f"branch_{len(branches)}" + branches[branch_id] = branch.calls + branch_paths[branch_id] = branch.source_path + index += 1 + analyze_parallel_branches( + compiler, + branches, + workflow_id=( + f"{program.program_id}:preflight:parallel:{segment_index}" + ), + branch_paths=branch_paths, + ) + + def _validate_selection( + self, + integration: ExpertProgramIntegrationCfg, + ) -> None: + """Reject an integration selection owned by another adapter.""" + if type(integration) is not ExpertProgramIntegrationCfg: + raise TypeError("integration must be exactly ExpertProgramIntegrationCfg.") + current_scene_id = _validate_identifier( + self._factory.scene_registry_id, + field_name="factory.scene_registry_id", + ) + current_profile_id = _validate_identifier( + self._factory.robot_profile_id, + field_name="factory.robot_profile_id", + ) + if current_scene_id != self._scene_registry_id: + raise ValueError( + "Factory scene registry declaration drifted: expected " + f"{self._scene_registry_id!r}, got {current_scene_id!r}." + ) + if current_profile_id != self._robot_profile_id: + raise ValueError( + "Factory robot profile declaration drifted: expected " + f"{self._robot_profile_id!r}, got {current_profile_id!r}." + ) + if integration.scene_registry != self._scene_registry_id: + raise ValueError( + f"Expert Program selects scene_registry " + f"{integration.scene_registry!r}, but this environment exposes " + f"only {self._scene_registry_id!r}." + ) + if integration.robot_profile != self._robot_profile_id: + raise ValueError( + f"Expert Program selects robot_profile " + f"{integration.robot_profile!r}, but this environment exposes " + f"only {self._robot_profile_id!r}." + ) + + def _create_scene_registry(self) -> SceneRegistry: + """Create and validate one exact live scene registry.""" + current_id = _validate_identifier( + self._factory.scene_registry_id, + field_name="factory.scene_registry_id", + ) + if current_id != self._scene_registry_id: + raise ValueError( + "Factory scene registry declaration drifted: expected " + f"{self._scene_registry_id!r}, got {current_id!r}." + ) + registry = self._factory.create_scene_registry() + if type(registry) is not SceneRegistry: + raise TypeError( + "create_scene_registry() must return exactly SceneRegistry." + ) + return registry + + def _create_manifest( + self, + registry: SceneRegistry, + profile: RobotSkillProfile, + *, + runtime_preset: str, + ) -> SemanticIntegrationManifest: + """Create one static manifest from exact selected declarations.""" + return SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=self._call_catalog, + runtime_preset=runtime_preset, + ) + + +class ExpertProgramEnvironmentMixin: + """Delegate environment hooks to one reusable explicit adapter. + + Environment classes place this mixin before their normal environment base + and implement only :attr:`expert_program_adapter`. Motion generation and + runtime stepping remain in shared components. + """ + + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the environment-owned reusable adapter. + + Returns: + Exact shared Expert Program environment adapter. + """ + raise NotImplementedError( + "Expert Program environments must expose expert_program_adapter." + ) + + def compile_expert_program( + self, + program: ExpertProgramCfg, + ) -> CompiledProgram: + """Delegate provider-free compilation to the explicit adapter. + + Args: + program: Strict declarative program configuration. + + Returns: + Provider-free compiled program. + """ + return self._checked_expert_program_adapter().compile(program) + + def create_expert_program_bridge( + self, + program: CompiledProgram, + ) -> AtomicDemoBridge: + """Delegate live runtime and Gym bridge assembly to the adapter. + + Args: + program: Provider-free compiled program. + + Returns: + Fresh lazy Gym bridge. + """ + return self._checked_expert_program_adapter().create_bridge(program) + + def _checked_expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the exact adapter or fail before any provider is touched.""" + adapter = self.expert_program_adapter + if type(adapter) is not ExpertProgramEnvironmentAdapter: + raise TypeError( + "expert_program_adapter must be exactly " + "ExpertProgramEnvironmentAdapter." + ) + return adapter + + +__all__ = [ + "AcceptedRuntimeCommandObserverFactory", + "ExpertProgramEnvironmentAdapter", + "ExpertProgramEnvironmentFactory", + "ExpertProgramEnvironmentMixin", + "ExpertProgramRuntimeAssembly", + "PlanningObservationPort", + "SkillRuntimeAssemblyPort", +] diff --git a/embodichain/lab/gym/envs/expert_program/loader.py b/embodichain/lab/gym/envs/expert_program/loader.py new file mode 100644 index 000000000..f47e60940 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/loader.py @@ -0,0 +1,337 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Safe file and strict JSON loading for declarative Expert Programs.""" + +from __future__ import annotations + +import json +import math +import os +from pathlib import Path + +import yaml + +from .cfg import ExpertProgramCfg +from .decoder import ( + ExpertProgramDecodeError, + ExpertProgramValidationContext, + decode_expert_program, +) + +__all__ = [ + "MAX_EXPERT_PROGRAM_BYTES", + "load_expert_program", + "loads_expert_program_json", + "parse_expert_program_json", +] + +MAX_EXPERT_PROGRAM_BYTES = 4 * 1024 * 1024 +"""Maximum serialized Expert Program size accepted by the file loader.""" + + +class _StrictJsonValueError(ValueError): + """Carry one stable strict-JSON failure into the public decode boundary.""" + + def __init__(self, code: str, message: str) -> None: + self.code = code + self.message = message + super().__init__(message) + + +def _reject_duplicate_json_keys( + pairs: list[tuple[str, object]], +) -> dict[str, object]: + """Build a JSON mapping while rejecting ambiguous duplicate keys.""" + mapping: dict[str, object] = {} + for key, value in pairs: + if key in mapping: + raise _StrictJsonValueError( + "duplicate_json_key", + f"Duplicate JSON key {key!r}.", + ) + mapping[key] = value + return mapping + + +def _reject_non_finite_json_constant(token: str) -> object: + """Reject the non-standard NaN and Infinity JSON constants.""" + raise _StrictJsonValueError( + "non_finite_number", + f"Non-finite JSON number {token!r} is forbidden.", + ) + + +def _parse_finite_json_float(token: str) -> float: + """Parse one JSON float while rejecting overflow to infinity.""" + value = float(token) + if not math.isfinite(value): + raise _StrictJsonValueError( + "non_finite_number", + f"JSON number {token!r} is not finite.", + ) + return value + + +def _validate_decoded_json_unicode(value: object) -> None: + """Reject decoded JSON strings that cannot be represented as UTF-8.""" + if type(value) is str: + try: + value.encode("utf-8") + except UnicodeEncodeError as error: + raise _StrictJsonValueError( + "invalid_utf8", + "Expert Program JSON contains an unpaired Unicode surrogate.", + ) from error + return + if type(value) is list: + for item in value: + _validate_decoded_json_unicode(item) + return + if type(value) is dict: + for key, item in value.items(): + _validate_decoded_json_unicode(key) + _validate_decoded_json_unicode(item) + + +def _loads_strict_json_value( + text: str, + *, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> object: + """Parse one bounded JSON document into exact JSON-compatible values.""" + + if type(text) is not str: + raise TypeError("text must be exactly str.") + if type(max_bytes) is not int: + raise TypeError("max_bytes must be exactly int.") + if max_bytes <= 0: + raise ValueError("max_bytes must be positive.") + try: + payload = text.encode("utf-8") + except UnicodeEncodeError as error: + raise ExpertProgramDecodeError( + "invalid_utf8", + (), + "Expert Program JSON must be valid UTF-8 text.", + ) from error + if len(payload) > max_bytes: + raise ExpertProgramDecodeError( + "input_too_large", + (), + f"Expert Program JSON exceeds the {max_bytes}-byte input limit.", + ) + try: + value = json.loads( + text, + object_pairs_hook=_reject_duplicate_json_keys, + parse_constant=_reject_non_finite_json_constant, + parse_float=_parse_finite_json_float, + ) + _validate_decoded_json_unicode(value) + return value + except _StrictJsonValueError as error: + raise ExpertProgramDecodeError(error.code, (), error.message) from error + except json.JSONDecodeError as error: + raise ExpertProgramDecodeError( + "invalid_json", + (), + "Invalid Expert Program JSON at " + f"line {error.lineno}, column {error.colno}.", + ) from error + except RecursionError as error: + raise ExpertProgramDecodeError( + "input_too_deep", + (), + "Expert Program JSON exceeds the parser nesting limit.", + ) from error + except ValueError as error: + raise ExpertProgramDecodeError( + "invalid_json", + (), + "Expert Program JSON contains an invalid numeric value.", + ) from error + + +def parse_expert_program_json( + text: str, + *, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> dict[str, object]: + """Parse one bounded Expert Program JSON object without decoding its schema. + + This parse-only boundary lets a host-controlled frontend inspect or inject + fields before calling :func:`decode_expert_program`. It rejects duplicate + keys, non-finite numbers, trailing content, invalid Unicode, excessive + nesting, oversized UTF-8 input, and non-object top-level values. It does + not validate the Expert Program schema. + + Args: + text: Untrusted JSON document text. + max_bytes: Maximum accepted UTF-8 encoded input size. + + Returns: + Exact JSON object mapping ready for explicit schema decoding. + + Raises: + TypeError: If ``text`` or ``max_bytes`` has the wrong exact type. + ValueError: If ``max_bytes`` is not positive. + ExpertProgramDecodeError: If strict JSON parsing fails. + """ + value = _loads_strict_json_value(text, max_bytes=max_bytes) + if type(value) is not dict: + raise ExpertProgramDecodeError( + "expected_mapping", + (), + "Expected an object mapping.", + ) + return value + + +def loads_expert_program_json( + text: str, + *, + validation_context: ExpertProgramValidationContext | None = None, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> ExpertProgramCfg: + """Strictly parse and decode one untrusted Expert Program JSON document. + + The input must be one plain JSON document. Markdown fences, trailing text, + multiple documents, duplicate keys, non-finite numbers, and oversized input + are rejected before the existing Expert Program decoder is called. + + Args: + text: Untrusted JSON response text. + validation_context: Optional provider-free static reference validator. + max_bytes: Maximum UTF-8 encoded response size. + + Returns: + Fully owned and internally validated Expert Program configuration. + + Raises: + TypeError: If ``text`` or ``max_bytes`` has the wrong exact type. + ValueError: If ``max_bytes`` is not positive. + ExpertProgramDecodeError: If parsing or strict decoding fails. + """ + data = parse_expert_program_json(text, max_bytes=max_bytes) + return decode_expert_program(data, validation_context=validation_context) + + +class _UniqueKeySafeLoader(yaml.SafeLoader): + """YAML safe loader that also rejects ambiguous duplicate keys.""" + + +def _construct_unique_yaml_mapping( + loader: _UniqueKeySafeLoader, + node: yaml.MappingNode, + deep: bool = False, +) -> dict[object, object]: + """Construct one YAML mapping with unique, hashable keys.""" + loader.flatten_mapping(node) + mapping: dict[object, object] = {} + for key_node, value_node in node.value: + key = loader.construct_object(key_node, deep=deep) + try: + duplicate = key in mapping + except TypeError as error: + raise yaml.constructor.ConstructorError( + "while constructing a mapping", + node.start_mark, + "found an unhashable key", + key_node.start_mark, + ) from error + if duplicate: + raise yaml.constructor.ConstructorError( + "while constructing a mapping", + node.start_mark, + f"found duplicate key {key!r}", + key_node.start_mark, + ) + mapping[key] = loader.construct_object(value_node, deep=deep) + return mapping + + +_UniqueKeySafeLoader.add_constructor( + yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG, + _construct_unique_yaml_mapping, +) + + +def load_expert_program( + path: str | os.PathLike[str], + *, + base_dir: str | os.PathLike[str] | None = None, + validation_context: ExpertProgramValidationContext | None = None, +) -> ExpertProgramCfg: + """Safely load and strictly decode one JSON or YAML Expert Program file. + + Relative paths are resolved from ``base_dir`` when provided. Otherwise, + they retain normal :class:`pathlib.Path` semantics and therefore resolve + from the process working directory when opened. + + Args: + path: JSON, YAML, or YML file to load. + base_dir: Optional directory used to resolve a relative ``path``. + validation_context: Optional provider-free static reference validator + applied after decoding either serialized format. + + Returns: + An owned, validated Expert Program configuration. + + Raises: + FileNotFoundError: If the resolved path is not a regular file. + ValueError: If the file is too large, has an unsupported extension, or + contains ambiguous or invalid serialized data. + ExpertProgramValidationError: If ``validation_context`` rejects an + external reference. + UnicodeDecodeError: If the file is not valid UTF-8. + """ + program_path = Path(path).expanduser() + if base_dir is not None and not program_path.is_absolute(): + program_path = Path(base_dir).expanduser() / program_path + if not program_path.is_file(): + raise FileNotFoundError(f"Expert Program path is not a file: {program_path}.") + suffix = program_path.suffix.lower() + if suffix not in {".json", ".yaml", ".yml"}: + raise ValueError( + "Expert Program must use a .json, .yaml, or .yml extension; " + f"got {program_path.name!r}." + ) + + payload = program_path.read_bytes() + if len(payload) > MAX_EXPERT_PROGRAM_BYTES: + raise ExpertProgramDecodeError( + "input_too_large", + (), + "Expert Program exceeds the " + f"{MAX_EXPERT_PROGRAM_BYTES}-byte input limit.", + ) + text = payload.decode("utf-8") + if suffix == ".json": + return loads_expert_program_json( + text, + validation_context=validation_context, + ) + try: + data = yaml.load(text, Loader=_UniqueKeySafeLoader) + except yaml.YAMLError as error: + raise ValueError( + f"Invalid Expert Program YAML in {program_path}: {error}" + ) from error + return decode_expert_program( + data, + validation_context=validation_context, + ) diff --git a/embodichain/lab/gym/envs/expert_program/simulation.py b/embodichain/lab/gym/envs/expert_program/simulation.py new file mode 100644 index 000000000..5317dc091 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation.py @@ -0,0 +1,1240 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Explicit simulation bindings for declarative Expert Programs. + +The values in this module bridge task-owned, executable-free declarations to +the existing :class:`SceneRegistry` and :class:`RobotSkillProfile` contracts. +They deliberately do not scan the simulation or infer semantic capabilities +from names. Every simulation entity, articulation member, control part, and +semantic command is selected explicitly and validated while the binding is +built. +""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass, field, replace +import math +from types import MappingProxyType +from typing import Any, Protocol, runtime_checkable, TYPE_CHECKING + +import torch + +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, + ControlPartCommandProfile, + EntityState, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + ResourceEndpoint, + ResourceBinding, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + GRASP_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneDynamics, + SceneEntityRegistration, + SceneGeometryProvider, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) +from embodichain.toolkits.graspkit.pg_grasp import GraspGeneratorCfg +from embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker import ( + GripperCollisionCfg, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.sim_manager import SimulationManager + + +_IDENTITY_POSE = ( + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, +) + + +def _identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _optional_identifier(value: str | None, *, field_name: str) -> str | None: + """Validate one optional identifier.""" + if value is not None: + _identifier(value, field_name=field_name) + return value + + +def _identifier_tuple( + values: tuple[str, ...], + *, + field_name: str, +) -> tuple[str, ...]: + """Own a duplicate-free tuple of exact identifiers.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must be an iterable of identifiers.") + normalized = tuple(values) + for value in normalized: + _identifier(value, field_name=field_name) + if len(set(normalized)) != len(normalized): + raise ValueError(f"{field_name} must contain unique identifiers.") + return normalized + + +def _finite(value: float, *, field_name: str) -> float: + """Return one finite non-boolean float.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a finite number.") + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + return normalized + + +def _pose_tuple( + values: tuple[float, ...], + *, + field_name: str, +) -> tuple[float, ...]: + """Own and validate one flattened SE(3) matrix.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must contain 16 finite numbers.") + normalized = tuple( + _finite(value, field_name=f"{field_name}[{index}]") + for index, value in enumerate(values) + ) + if len(normalized) != 16: + raise ValueError(f"{field_name} must contain exactly 16 numbers.") + pose = torch.tensor(normalized, dtype=torch.float64).reshape(4, 4) + bottom = torch.tensor((0.0, 0.0, 0.0, 1.0), dtype=torch.float64) + if not torch.allclose(pose[3], bottom, atol=1.0e-6, rtol=0.0): + raise ValueError(f"{field_name} must have bottom row [0, 0, 0, 1].") + rotation = pose[:3, :3] + if not torch.allclose( + rotation.T @ rotation, + torch.eye(3, dtype=torch.float64), + atol=1.0e-6, + rtol=0.0, + ) or not torch.isclose( + torch.linalg.det(rotation), + torch.tensor(1.0, dtype=torch.float64), + atol=1.0e-6, + rtol=0.0, + ): + raise ValueError(f"{field_name} must contain a proper SE(3) rotation.") + return normalized + + +def _pose_tensor(values: tuple[float, ...]) -> torch.Tensor: + """Materialize an owned float32 pose matrix.""" + return torch.tensor(values, dtype=torch.float32).reshape(4, 4) + + +def _validate_scene_classification( + dynamics: SceneDynamics, + collision_role: SceneCollisionRole, +) -> None: + """Validate exact scene-enum values.""" + if not isinstance(dynamics, SceneDynamics): + raise TypeError("dynamics must be a SceneDynamics value.") + if not isinstance(collision_role, SceneCollisionRole): + raise TypeError("collision_role must be a SceneCollisionRole value.") + + +@dataclass(frozen=True, slots=True) +class SimulationRigidObjectBinding: + """Explicit binding for one simulation rigid object.""" + + entity_id: str + simulation_uid: str + aliases: tuple[str, ...] = () + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + semantic_type: str | None = None + default_grasp_affordance: str | None = None + geometry_provider: SceneGeometryProvider | None = None + + def __post_init__(self) -> None: + _identifier(self.entity_id, field_name="entity_id") + _identifier(self.simulation_uid, field_name="simulation_uid") + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + _validate_scene_classification(self.dynamics, self.collision_role) + _optional_identifier(self.semantic_type, field_name="semantic_type") + _optional_identifier( + self.default_grasp_affordance, + field_name="default_grasp_affordance", + ) + + +@dataclass(frozen=True, slots=True) +class SimulationArticulationBinding: + """Explicit binding for one simulation articulation.""" + + entity_id: str + simulation_uid: str + aliases: tuple[str, ...] = () + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + semantic_type: str | None = None + default_operation_affordance: str | None = None + geometry_provider: SceneGeometryProvider | None = None + + def __post_init__(self) -> None: + _identifier(self.entity_id, field_name="entity_id") + _identifier(self.simulation_uid, field_name="simulation_uid") + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + _validate_scene_classification(self.dynamics, self.collision_role) + _optional_identifier(self.semantic_type, field_name="semantic_type") + _optional_identifier( + self.default_operation_affordance, + field_name="default_operation_affordance", + ) + + +@dataclass(frozen=True, slots=True) +class SimulationArticulationLinkBinding: + """Explicit canonical link backed by one native articulation link.""" + + entity_id: str + articulation_id: str + native_link_name: str + aliases: tuple[str, ...] = () + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + semantic_type: str | None = None + + def __post_init__(self) -> None: + _identifier(self.entity_id, field_name="entity_id") + _identifier(self.articulation_id, field_name="articulation_id") + _identifier(self.native_link_name, field_name="native_link_name") + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + if not isinstance(self.dynamics, SceneDynamics): + raise TypeError("dynamics must be a SceneDynamics value.") + _optional_identifier(self.semantic_type, field_name="semantic_type") + + +@dataclass(frozen=True, slots=True) +class AntipodalGraspAffordanceBinding: + """Build one antipodal grasp affordance from a selected rigid-object mesh.""" + + entity_id: str + object_id: str + native_name: str + revision: str + aliases: tuple[str, ...] = () + relative_pose: tuple[float, ...] = _IDENTITY_POSE + mesh_env_id: int = 0 + generator_cfg: GraspGeneratorCfg | None = None + gripper_collision_cfg: GripperCollisionCfg | None = None + force_reannotate: bool = False + + def __post_init__(self) -> None: + for field_name in ("entity_id", "object_id", "native_name", "revision"): + _identifier(getattr(self, field_name), field_name=field_name) + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + object.__setattr__( + self, + "relative_pose", + _pose_tuple(self.relative_pose, field_name="relative_pose"), + ) + if ( + isinstance(self.mesh_env_id, bool) + or not isinstance(self.mesh_env_id, int) + or self.mesh_env_id < 0 + ): + raise ValueError("mesh_env_id must be a non-negative integer.") + if self.generator_cfg is not None and not isinstance( + self.generator_cfg, + GraspGeneratorCfg, + ): + raise TypeError("generator_cfg must be GraspGeneratorCfg or None.") + if self.gripper_collision_cfg is not None and not isinstance( + self.gripper_collision_cfg, + GripperCollisionCfg, + ): + raise TypeError( + "gripper_collision_cfg must be GripperCollisionCfg or None." + ) + if not isinstance(self.force_reannotate, bool): + raise TypeError("force_reannotate must be a bool.") + object.__setattr__(self, "generator_cfg", deepcopy(self.generator_cfg)) + object.__setattr__( + self, + "gripper_collision_cfg", + deepcopy(self.gripper_collision_cfg), + ) + + +@dataclass(frozen=True, slots=True) +class ArticulationOperationTargetBinding: + """Declarative named target for one articulation operation.""" + + target_position: float + displacement: float + + def __post_init__(self) -> None: + object.__setattr__( + self, + "target_position", + _finite(self.target_position, field_name="target_position"), + ) + object.__setattr__( + self, + "displacement", + _finite(self.displacement, field_name="displacement"), + ) + + def build(self) -> ArticulationOperationTarget: + """Build the existing atomic-action target value.""" + return ArticulationOperationTarget( + target_position=self.target_position, + displacement=self.displacement, + ) + + +@dataclass(frozen=True, slots=True) +class ArticulationOperationAffordanceBinding: + """Bind one handle operation to an explicit native link and joint.""" + + entity_id: str + articulation_id: str + link_id: str + joint_id: str + revision: str + semantic_targets: Mapping[str, ArticulationOperationTargetBinding] + aliases: tuple[str, ...] = () + handle_pose_offset: tuple[float, ...] = _IDENTITY_POSE + approach_offset: tuple[float, ...] = _IDENTITY_POSE + contact_offset: tuple[float, ...] = _IDENTITY_POSE + operation_offset: tuple[float, ...] = _IDENTITY_POSE + retract_offset: tuple[float, ...] = _IDENTITY_POSE + operation_axis: tuple[float, float, float] = (1.0, 0.0, 0.0) + position_scale: float = 1.0 + + def __post_init__(self) -> None: + for field_name in ( + "entity_id", + "articulation_id", + "link_id", + "joint_id", + "revision", + ): + _identifier(getattr(self, field_name), field_name=field_name) + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + for field_name in ( + "handle_pose_offset", + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + object.__setattr__( + self, + field_name, + _pose_tuple(getattr(self, field_name), field_name=field_name), + ) + axis = tuple( + _finite(value, field_name=f"operation_axis[{index}]") + for index, value in enumerate(self.operation_axis) + ) + if len(axis) != 3 or math.sqrt(sum(value * value for value in axis)) <= 0.0: + raise ValueError("operation_axis must contain three non-zero values.") + object.__setattr__(self, "operation_axis", axis) + position_scale = _finite(self.position_scale, field_name="position_scale") + if position_scale <= 0.0: + raise ValueError("position_scale must be positive.") + object.__setattr__(self, "position_scale", position_scale) + if not isinstance(self.semantic_targets, Mapping): + raise TypeError("semantic_targets must be a mapping.") + targets: dict[str, ArticulationOperationTargetBinding] = {} + for target_id, target in self.semantic_targets.items(): + _identifier(target_id, field_name="semantic target IDs") + if type(target) is not ArticulationOperationTargetBinding: + raise TypeError( + "semantic_targets values must be exact " + "ArticulationOperationTargetBinding values." + ) + targets[target_id] = target + object.__setattr__(self, "semantic_targets", MappingProxyType(targets)) + + +@dataclass(frozen=True, slots=True) +class _SimulationArticulationLinkStateProvider: + """Read one selected native link pose with an optional local offset.""" + + articulation: Any + native_link_name: str + local_offset: torch.Tensor = field(repr=False) + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp + getter = getattr(self.articulation, "get_link_pose", None) + if not callable(getter): + raise TypeError("Simulation articulation must provide get_link_pose().") + pose = getter( + self.native_link_name, + env_ids=env_ids.detach().to("cpu").tolist(), + to_matrix=True, + ) + if not isinstance(pose, torch.Tensor): + raise TypeError( + "Simulation articulation get_link_pose() must return a tensor." + ) + offset = self.local_offset.to(device=pose.device, dtype=pose.dtype) + return EntityState(torch.matmul(pose, offset)) + + +def _require_native_entity( + simulation: SimulationManager, + *, + getter_name: str, + registry_id: str, + simulation_uid: str, +) -> Any: + """Resolve one explicitly selected native entity or fail closed.""" + getter = getattr(simulation, getter_name, None) + if not callable(getter): + raise TypeError(f"simulation must provide {getter_name}().") + entity = getter(simulation_uid) + if entity is None: + raise KeyError( + f"Simulation UID {simulation_uid!r} selected for registry entity " + f"{registry_id!r} was not found." + ) + return entity + + +def _native_names(entity: Any, *, attribute: str, owner: str) -> tuple[str, ...]: + """Read and validate one existing native-name collection.""" + values = getattr(entity, attribute, None) + if values is None: + raise TypeError(f"{owner} must expose {attribute}.") + if isinstance(values, (str, bytes)): + raise TypeError(f"{owner}.{attribute} must be an iterable of names.") + try: + names = tuple(values) + except TypeError as exc: + raise TypeError(f"{owner}.{attribute} must be an iterable of names.") from exc + for name in names: + _identifier(name, field_name=f"{owner}.{attribute}") + if len(set(names)) != len(names): + raise ValueError(f"{owner}.{attribute} must contain unique names.") + return names + + +def _mesh_tensor( + entity: Any, + *, + getter_name: str, + mesh_env_id: int, + vertices: bool, +) -> torch.Tensor: + """Read one explicitly selected mesh row with strict shape validation.""" + getter = getattr(entity, getter_name, None) + if not callable(getter): + raise TypeError(f"Simulation rigid object must provide {getter_name}().") + if vertices: + value = getter(env_ids=[mesh_env_id], scale=True) + else: + value = getter(env_ids=[mesh_env_id]) + if not isinstance(value, torch.Tensor): + raise TypeError( + f"Simulation rigid object {getter_name}() must return a tensor." + ) + if value.dim() != 3 or value.shape[0] != 1 or value.shape[2] != 3: + raise ValueError( + f"Simulation rigid object {getter_name}() must return shape (1, N, 3)." + ) + selected = value[0].detach().clone() + if selected.shape[0] == 0: + raise ValueError(f"Simulation rigid object {getter_name}() returned no data.") + if vertices: + if not selected.is_floating_point() or not torch.isfinite(selected).all(): + raise ValueError("Antipodal mesh vertices must be finite floating values.") + elif selected.dtype == torch.bool or selected.is_floating_point(): + raise TypeError("Antipodal mesh triangles must use an integer dtype.") + return selected + + +def _antipodal_affordance( + binding: AntipodalGraspAffordanceBinding, + entity: Any, +) -> AntipodalAffordance: + """Build and validate one owned antipodal affordance payload.""" + vertices = _mesh_tensor( + entity, + getter_name="get_vertices", + mesh_env_id=binding.mesh_env_id, + vertices=True, + ) + triangles = _mesh_tensor( + entity, + getter_name="get_triangles", + mesh_env_id=binding.mesh_env_id, + vertices=False, + ) + if bool((triangles < 0).any()) or int(triangles.max().item()) >= vertices.shape[0]: + raise ValueError("Antipodal mesh triangles reference invalid vertex indices.") + return AntipodalAffordance( + mesh_vertices=vertices, + mesh_triangles=triangles, + generator_cfg=deepcopy(binding.generator_cfg), + gripper_collision_cfg=deepcopy(binding.gripper_collision_cfg), + force_reannotate=binding.force_reannotate, + ) + + +@dataclass(frozen=True, slots=True) +class SimulationSceneBinding: + """Build one authoritative registry from explicit simulation bindings.""" + + registry_id: str + rigid_objects: tuple[SimulationRigidObjectBinding, ...] = () + articulations: tuple[SimulationArticulationBinding, ...] = () + links: tuple[SimulationArticulationLinkBinding, ...] = () + antipodal_grasps: tuple[AntipodalGraspAffordanceBinding, ...] = () + articulation_operations: tuple[ArticulationOperationAffordanceBinding, ...] = () + collision_world_mode: SceneCollisionWorldMode | None = None + + def __post_init__(self) -> None: + _identifier(self.registry_id, field_name="registry_id") + expected_types = { + "rigid_objects": SimulationRigidObjectBinding, + "articulations": SimulationArticulationBinding, + "links": SimulationArticulationLinkBinding, + "antipodal_grasps": AntipodalGraspAffordanceBinding, + "articulation_operations": ArticulationOperationAffordanceBinding, + } + all_ids: list[str] = [] + for field_name, expected_type in expected_types.items(): + values = tuple(getattr(self, field_name)) + if not all(type(value) is expected_type for value in values): + raise TypeError( + f"{field_name} must contain exact {expected_type.__name__} values." + ) + object.__setattr__(self, field_name, values) + all_ids.extend(value.entity_id for value in values) + duplicates = sorted( + entity_id for entity_id in set(all_ids) if all_ids.count(entity_id) > 1 + ) + if duplicates: + raise ValueError(f"Scene binding entity IDs must be unique: {duplicates}.") + if self.collision_world_mode is not None and not isinstance( + self.collision_world_mode, + SceneCollisionWorldMode, + ): + raise TypeError( + "collision_world_mode must be SceneCollisionWorldMode or None." + ) + + def build(self, simulation: SimulationManager) -> SceneRegistry: + """Build the existing authoritative scene registry. + + Args: + simulation: Live simulation used only for explicitly named lookups. + + Returns: + Immutable registry with typed roots, links, and affordances. + """ + objects = {item.entity_id: item for item in self.rigid_objects} + articulations = {item.entity_id: item for item in self.articulations} + geometry = { + item.entity_id: item.geometry_provider + for item in (*self.rigid_objects, *self.articulations) + if item.geometry_provider is not None + } + roles = { + item.entity_id: item.collision_role + for item in (*self.rigid_objects, *self.articulations) + } + base = SceneRegistry.from_simulation( + simulation, + rigid_objects={ + item.entity_id: item.simulation_uid for item in self.rigid_objects + }, + articulations={ + item.entity_id: item.simulation_uid for item in self.articulations + }, + collision_roles=roles, + geometry_providers=geometry, + collision_world_mode=self.collision_world_mode, + ) + + registrations: list[SceneEntityRegistration] = [] + for registration in base.registrations: + entity_id = registration.ref.entity_id + if isinstance(registration.ref, SceneObjectRef): + binding = objects[entity_id] + defaults = ( + {} + if binding.default_grasp_affordance is None + else { + GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( + binding.default_grasp_affordance + ) + } + ) + else: + binding = articulations[entity_id] + defaults = ( + {} + if binding.default_operation_affordance is None + else { + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( + SceneAffordanceRef(binding.default_operation_affordance) + ) + } + ) + registrations.append( + replace( + registration, + aliases=(*registration.aliases, *binding.aliases), + dynamics=binding.dynamics, + semantic_type=binding.semantic_type, + default_affordances=defaults, + ) + ) + + native_articulations: dict[str, Any] = {} + links: dict[str, SimulationArticulationLinkBinding] = {} + for binding in self.links: + articulation_binding = articulations.get(binding.articulation_id) + if articulation_binding is None: + raise KeyError( + f"Link {binding.entity_id!r} references unbound articulation " + f"{binding.articulation_id!r}." + ) + articulation = native_articulations.setdefault( + binding.articulation_id, + _require_native_entity( + simulation, + getter_name="get_articulation", + registry_id=binding.articulation_id, + simulation_uid=articulation_binding.simulation_uid, + ), + ) + native_links = _native_names( + articulation, + attribute="link_names", + owner=f"articulation {binding.articulation_id!r}", + ) + if binding.native_link_name not in native_links: + raise KeyError( + f"Native link {binding.native_link_name!r} selected for " + f"{binding.entity_id!r} was not found; available links are " + f"{sorted(native_links)}." + ) + links[binding.entity_id] = binding + registrations.append( + SceneEntityRegistration( + ref=SceneLinkRef(binding.entity_id), + state_provider=_SimulationArticulationLinkStateProvider( + articulation, + binding.native_link_name, + _pose_tensor(_IDENTITY_POSE), + ), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=binding.native_link_name, + dynamics=binding.dynamics, + semantic_type=binding.semantic_type, + ) + ) + + native_objects: dict[str, Any] = {} + for binding in self.antipodal_grasps: + object_binding = objects.get(binding.object_id) + if object_binding is None: + raise KeyError( + f"Grasp affordance {binding.entity_id!r} references unbound " + f"object {binding.object_id!r}." + ) + entity = native_objects.setdefault( + binding.object_id, + _require_native_entity( + simulation, + getter_name="get_rigid_object", + registry_id=binding.object_id, + simulation_uid=object_binding.simulation_uid, + ), + ) + registrations.append( + SceneEntityRegistration( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneObjectRef(binding.object_id), + native_name=binding.native_name, + affordance=_antipodal_affordance(binding, entity), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision=binding.revision, + relative_pose=_pose_tensor(binding.relative_pose), + ) + ) + + for binding in self.articulation_operations: + articulation_binding = articulations.get(binding.articulation_id) + if articulation_binding is None: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound articulation {binding.articulation_id!r}." + ) + link_binding = links.get(binding.link_id) + if link_binding is None: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound link {binding.link_id!r}." + ) + if link_binding.articulation_id != binding.articulation_id: + raise ValueError( + f"Operation affordance {binding.entity_id!r} and link " + f"{binding.link_id!r} select different articulations." + ) + articulation = native_articulations[binding.articulation_id] + native_joints = _native_names( + articulation, + attribute="joint_names", + owner=f"articulation {binding.articulation_id!r}", + ) + if binding.joint_id not in native_joints: + raise KeyError( + f"Native joint {binding.joint_id!r} selected for " + f"{binding.entity_id!r} was not found; available joints are " + f"{sorted(native_joints)}." + ) + payload = ArticulationOperationAffordance( + joint_id=binding.joint_id, + approach_offset=_pose_tensor(binding.approach_offset), + contact_offset=_pose_tensor(binding.contact_offset), + operation_offset=_pose_tensor(binding.operation_offset), + retract_offset=_pose_tensor(binding.retract_offset), + operation_axis=torch.tensor( + binding.operation_axis, + dtype=torch.float32, + ), + position_scale=binding.position_scale, + semantic_targets={ + target_id: target.build() + for target_id, target in binding.semantic_targets.items() + }, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneAffordanceRef(binding.entity_id), + state_provider=_SimulationArticulationLinkStateProvider( + articulation, + link_binding.native_link_name, + _pose_tensor(binding.handle_pose_offset), + ), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=link_binding.native_link_name, + affordance=payload, + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision=binding.revision, + ) + ) + + return SceneRegistry( + registrations, + collision_world_mode=self.collision_world_mode, + ) + + +@dataclass(frozen=True, slots=True) +class ControlPartCommandPreset: + """Named one-dimensional joint commands for one exact control part.""" + + preset_id: str + control_part: str + commands: Mapping[str, tuple[float, ...]] + + def __post_init__(self) -> None: + _identifier(self.preset_id, field_name="preset_id") + _identifier(self.control_part, field_name="control_part") + if not isinstance(self.commands, Mapping): + raise TypeError("commands must be a mapping.") + commands: dict[str, tuple[float, ...]] = {} + for command_id, positions in self.commands.items(): + _identifier(command_id, field_name="command IDs") + if isinstance(positions, (str, bytes)): + raise TypeError("command positions must be an iterable of numbers.") + normalized = tuple( + _finite(value, field_name=f"commands[{command_id!r}][{index}]") + for index, value in enumerate(positions) + ) + if not normalized: + raise ValueError("command positions must not be empty.") + commands[command_id] = normalized + object.__setattr__(self, "commands", MappingProxyType(commands)) + + def build(self, *, control_dof: int) -> ControlPartCommandProfile: + """Build a command profile after validating the native control width.""" + for command_id, positions in self.commands.items(): + if len(positions) != control_dof: + raise ValueError( + f"Command {command_id!r} in preset {self.preset_id!r} has " + f"{len(positions)} positions, but control part " + f"{self.control_part!r} has {control_dof} joints." + ) + return ControlPartCommandProfile.joint_positions( + **{ + command_id: torch.tensor(positions, dtype=torch.float32) + for command_id, positions in self.commands.items() + } + ) + + +def _require_control_part_dof(robot: Robot, control_part: str) -> int: + """Validate one native joint-backed control part and return its width.""" + control_parts = getattr(robot, "control_parts", None) + if not isinstance(control_parts, Mapping): + raise TypeError("robot must expose a control_parts mapping.") + get_joint_ids = getattr(robot, "get_joint_ids", None) + if not callable(get_joint_ids): + raise TypeError("robot must provide get_joint_ids().") + if control_part not in control_parts: + raise KeyError( + f"Robot control part {control_part!r} was not found; available " + f"control parts are {sorted(str(key) for key in control_parts)}." + ) + joint_ids = tuple(get_joint_ids(name=control_part)) + if not joint_ids: + raise ValueError(f"Robot control part {control_part!r} contains no joints.") + if not all( + isinstance(joint_id, int) and not isinstance(joint_id, bool) and joint_id >= 0 + for joint_id in joint_ids + ): + raise ValueError( + f"Robot control part {control_part!r} returned invalid joint IDs." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ValueError( + f"Robot control part {control_part!r} contains duplicate joint IDs." + ) + return len(joint_ids) + + +@runtime_checkable +class SimulationResourceEndpointBinding(Protocol): + """Build one typed resource endpoint from an explicitly selected robot. + + Implementations are reusable robot-integration declarations. They may + validate embodiment-specific controller surfaces, but must only return an + owned :class:`ResourceEndpoint`; live controller handles remain in the + endpoint adapter and runtime transport. + """ + + @property + def endpoint_id(self) -> str: + """Return the stable endpoint ID within its containing resource.""" + + def build(self, robot: Robot) -> ResourceEndpoint: + """Build and validate one endpoint declaration for ``robot``.""" + + +@runtime_checkable +class SimulationRobotResourceBinding(Protocol): + """Build one leaf or composite resource in the robot resource DAG.""" + + @property + def resource_id(self) -> str: + """Return the stable resource ID.""" + + @property + def members(self) -> tuple[str, ...]: + """Return explicitly declared child resource IDs.""" + + def build(self, robot: Robot) -> RobotResource: + """Build and validate one owned robot resource declaration.""" + + +@dataclass(frozen=True, slots=True) +class RobotResourceBinding: + """Generic simulation binding for arbitrary typed resource endpoints. + + This is the direct configuration path for mobile bases, whole-body + controllers, tools, and other non-joint transports. Endpoint-specific + validation remains in the registered :class:`ResourceEndpointAdapter`; + this value owns the declaration and preserves the resource DAG exactly. + """ + + resource_id: str + endpoints: Mapping[str, ResourceEndpoint] = field(default_factory=dict) + members: tuple[str, ...] = () + + def __post_init__(self) -> None: + resource = RobotResource( + resource_id=self.resource_id, + endpoints=self.endpoints, + members=self.members, + ) + object.__setattr__(self, "endpoints", resource.endpoints) + object.__setattr__(self, "members", resource.members) + + def build(self, robot: Robot) -> RobotResource: + """Build an independently owned resource without assuming robot joints.""" + del robot + return RobotResource( + resource_id=self.resource_id, + endpoints=self.endpoints, + members=self.members, + ) + + +@dataclass(frozen=True, slots=True) +class ControlPartEndpointBinding: + """Profile endpoint backed by one explicit robot control part.""" + + endpoint_id: str + control_part: str + capabilities: frozenset[str] + command_preset: str | None = None + + def __post_init__(self) -> None: + _identifier(self.endpoint_id, field_name="endpoint_id") + _identifier(self.control_part, field_name="control_part") + if isinstance(self.capabilities, (str, bytes)): + raise TypeError("capabilities must be an iterable of identifiers.") + capabilities = frozenset(self.capabilities) + for capability in capabilities: + _identifier(capability, field_name="capabilities") + object.__setattr__(self, "capabilities", capabilities) + _optional_identifier(self.command_preset, field_name="command_preset") + + def build(self, robot: Robot) -> ResourceEndpoint: + """Build a joint-backed endpoint after native control-part validation.""" + _require_control_part_dof(robot, self.control_part) + return ControlPartEndpoint( + control_part=self.control_part, + command_profile=self.command_preset, + capabilities=self.capabilities, + ) + + +@dataclass(frozen=True, slots=True) +class ControlPartResourceBinding: + """Joint-backed robot resource containing control-part endpoints.""" + + resource_id: str + endpoints: tuple[ControlPartEndpointBinding, ...] = () + members: tuple[str, ...] = () + + def __post_init__(self) -> None: + _identifier(self.resource_id, field_name="resource_id") + endpoints = tuple(self.endpoints) + if not all( + type(endpoint) is ControlPartEndpointBinding for endpoint in endpoints + ): + raise TypeError( + "endpoints must contain exact ControlPartEndpointBinding values." + ) + endpoint_ids = [endpoint.endpoint_id for endpoint in endpoints] + if len(set(endpoint_ids)) != len(endpoint_ids): + raise ValueError("endpoint_id values must be unique within a resource.") + object.__setattr__(self, "endpoints", endpoints) + object.__setattr__( + self, + "members", + _identifier_tuple(self.members, field_name="members"), + ) + + def build(self, robot: Robot) -> RobotResource: + """Build a resource containing strictly validated control-part endpoints.""" + endpoints: dict[str, ResourceEndpoint] = {} + for binding in self.endpoints: + endpoint = binding.build(robot) + if type(endpoint) is not ControlPartEndpoint: + raise TypeError( + "ControlPartEndpointBinding.build() must return exactly " + "ControlPartEndpoint." + ) + endpoints[binding.endpoint_id] = endpoint + return RobotResource( + resource_id=self.resource_id, + endpoints=endpoints, + members=self.members, + ) + + +def _owned_nested_identifier_mapping( + values: Mapping[str, Mapping[str, str]], + *, + field_name: str, +) -> Mapping[str, Mapping[str, str]]: + """Own a strict two-level identifier mapping.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + outer: dict[str, Mapping[str, str]] = {} + for key, nested in values.items(): + _identifier(key, field_name=f"{field_name} keys") + if not isinstance(nested, Mapping): + raise TypeError(f"{field_name}[{key!r}] must be a mapping.") + normalized: dict[str, str] = {} + for nested_key, nested_value in nested.items(): + _identifier(nested_key, field_name=f"{field_name} slot IDs") + _identifier(nested_value, field_name=f"{field_name} resource IDs") + normalized[nested_key] = nested_value + outer[key] = MappingProxyType(normalized) + return MappingProxyType(outer) + + +def _owned_identifier_mapping( + values: Mapping[str, str], + *, + field_name: str, +) -> Mapping[str, str]: + """Own one strict identifier mapping.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + normalized: dict[str, str] = {} + for key, value in values.items(): + _identifier(key, field_name=f"{field_name} keys") + _identifier(value, field_name=f"{field_name} values") + normalized[key] = value + return MappingProxyType(normalized) + + +@dataclass(frozen=True, slots=True) +class SimulationRobotSkillProfileBinding: + """Build a profile from typed resources with strict native validation.""" + + profile_id: str + resources: tuple[SimulationRobotResourceBinding, ...] + command_presets: tuple[ControlPartCommandPreset, ...] = () + defaults: Mapping[str, Mapping[str, str]] = field(default_factory=dict) + presets: tuple[SkillPolicyPreset, ...] = () + default_preset: str | None = None + skill_presets: Mapping[str, str] = field(default_factory=dict) + grounding_providers: Mapping[str, str] = field(default_factory=dict) + + def __post_init__(self) -> None: + _identifier(self.profile_id, field_name="profile_id") + resources = tuple(self.resources) + if not all( + isinstance(resource, SimulationRobotResourceBinding) + for resource in resources + ): + raise TypeError("resources must implement SimulationRobotResourceBinding.") + for resource in resources: + _identifier(resource.resource_id, field_name="resource_id") + _identifier_tuple(resource.members, field_name="resource members") + resource_ids = [resource.resource_id for resource in resources] + if len(set(resource_ids)) != len(resource_ids): + raise ValueError("resource_id values must be unique.") + object.__setattr__(self, "resources", resources) + command_presets = tuple(self.command_presets) + if not all( + type(preset) is ControlPartCommandPreset for preset in command_presets + ): + raise TypeError( + "command_presets must contain exact ControlPartCommandPreset values." + ) + command_preset_ids = [preset.preset_id for preset in command_presets] + if len(set(command_preset_ids)) != len(command_preset_ids): + raise ValueError("command preset IDs must be unique.") + object.__setattr__(self, "command_presets", command_presets) + object.__setattr__( + self, + "defaults", + _owned_nested_identifier_mapping(self.defaults, field_name="defaults"), + ) + presets = tuple(self.presets) + if not all(type(preset) is SkillPolicyPreset for preset in presets): + raise TypeError("presets must contain exact SkillPolicyPreset values.") + preset_ids = [preset.preset_id for preset in presets] + if len(set(preset_ids)) != len(preset_ids): + raise ValueError("policy preset IDs must be unique.") + object.__setattr__(self, "presets", presets) + _optional_identifier(self.default_preset, field_name="default_preset") + object.__setattr__( + self, + "skill_presets", + _owned_identifier_mapping( + self.skill_presets, + field_name="skill_presets", + ), + ) + object.__setattr__( + self, + "grounding_providers", + _owned_identifier_mapping( + self.grounding_providers, + field_name="grounding_providers", + ), + ) + + def build(self, robot: Robot) -> RobotSkillProfile: + """Build the existing profile after validating every typed resource. + + Args: + robot: Live robot selected by the simulation factory. + + Returns: + Reusable, engine-independent robot skill profile. + """ + control_dofs: dict[str, int] = {} + + def require_control_part(control_part: str) -> int: + if control_part not in control_dofs: + control_dofs[control_part] = _require_control_part_dof( + robot, + control_part, + ) + return control_dofs[control_part] + + command_presets = {preset.preset_id: preset for preset in self.command_presets} + command_profiles: dict[str, ControlPartCommandProfile] = {} + for preset in self.command_presets: + command_profiles[preset.preset_id] = preset.build( + control_dof=require_control_part(preset.control_part) + ) + + resources: dict[str, RobotResource] = {} + for resource_binding in self.resources: + resource = resource_binding.build(robot) + if type(resource) is not RobotResource: + raise TypeError( + f"Resource binding {resource_binding.resource_id!r} must build " + "exactly RobotResource." + ) + if resource.resource_id != resource_binding.resource_id: + raise ValueError( + f"Resource binding {resource_binding.resource_id!r} built " + f"resource ID {resource.resource_id!r}." + ) + if resource.members != tuple(resource_binding.members): + raise ValueError( + f"Resource binding {resource_binding.resource_id!r} changed its " + "declared resource members while building." + ) + for endpoint_id, endpoint in resource.endpoints.items(): + if not isinstance(endpoint, ControlPartEndpoint): + continue + require_control_part(endpoint.control_part) + profile_id = ( + endpoint.control_part + if endpoint.command_profile is None + else endpoint.command_profile + ) + command_preset = command_presets.get(profile_id) + if endpoint.command_profile is not None and command_preset is None: + raise KeyError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} " + "references unknown command " + f"preset {profile_id!r}." + ) + if ( + command_preset is not None + and command_preset.control_part != endpoint.control_part + ): + raise ValueError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} uses " + "control part " + f"{endpoint.control_part!r}, but command preset " + f"{profile_id!r} targets " + f"{command_preset.control_part!r}." + ) + resources[resource.resource_id] = resource + + return RobotSkillProfile( + profile_id=self.profile_id, + resources=resources, + command_profiles=command_profiles, + defaults={ + skill_id: ResourceBinding(resources=bindings) + for skill_id, bindings in self.defaults.items() + }, + presets={preset.preset_id: preset for preset in self.presets}, + default_preset=self.default_preset, + skill_presets=self.skill_presets, + grounding_providers=self.grounding_providers, + ) + + +__all__ = [ + "AntipodalGraspAffordanceBinding", + "ArticulationOperationAffordanceBinding", + "ArticulationOperationTargetBinding", + "ControlPartCommandPreset", + "ControlPartEndpointBinding", + "ControlPartResourceBinding", + "RobotResourceBinding", + "SimulationArticulationBinding", + "SimulationArticulationLinkBinding", + "SimulationRigidObjectBinding", + "SimulationResourceEndpointBinding", + "SimulationRobotResourceBinding", + "SimulationRobotSkillProfileBinding", + "SimulationSceneBinding", +] diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py new file mode 100644 index 000000000..b70104d45 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -0,0 +1,1202 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Production simulation assembly for Gym-backed Expert Programs. + +This module owns the reusable live wiring between declarative simulation +bindings and :class:`ExpertProgramEnvironmentAdapter`. A task declares a +scene binding and a robot profile binding; this factory constructs the motion +generator, atomic-action engine, planning observation port, effect-evidence +providers, and segment-policy port without task-local motion code. + +The resulting runtime is intentionally Gym-only. Its buffered command sink +must remain attached to :class:`AtomicDemoBridge`, which advances the shared +clock only after an ordinary ``env.step()`` consumes a yielded command. It is +therefore not a ``SkillRuntimeProvider`` for synchronous ``AtomicSkills`` use. +""" + +from __future__ import annotations + +from collections.abc import Callable, Iterable, Mapping +from copy import deepcopy +import math +from typing import Any, Protocol, TYPE_CHECKING + +import torch + +from embodichain.lab.gym.envs.settling import DynamicSettleMonitorCfg +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + EntityState, + ObservedArticulationJointState, + PlanningContext, + RobotObservation, + SceneProvider, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.control import ( + GRASP_COMMAND, + OPEN_COMMAND, + ControlPartCommandProfile, + JointPositionCommand, +) +from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + JointPositionPayload, + RuntimeCommandFrame, +) +from embodichain.lab.sim.planners import ( + BasePlannerCfg, + MotionGenCfg, + MotionGenerator, + ToppraPlannerCfg, +) +from embodichain.lab.sim.skills.calls import SemanticCallCatalog +from embodichain.lab.sim.skills.compiler import ( + HandOverPoseProvider, + RegisteredSemanticLowerer, + RelationTargetGrounder, +) +from embodichain.lab.sim.skills.effects import ( + ControlPartEvidenceAddress, + EffectMonitorRegistry, +) +from embodichain.lab.sim.skills.evidence import ( + BinaryEffectEvidenceQuery, + BinaryObservationCallback, + BinaryEffectObservation, + ControlPartRobotEvidenceSource, + ControlPartSimulationEvidenceProvider, + EffectEvidenceCollectionContext, + EffectEvidenceProvider, + ScalarObservationCallback, + SceneArticulationEvidenceProvider, +) +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, +) +from embodichain.lab.sim.skills.profiles import ( + ResourceEndpoint, + ResourceEndpointAdapter, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import RegistrySceneProvider, SceneRegistry + +from .bridge import ( + AcceptedRuntimeCommandObserver, + EnvironmentStepClock, + GymPlanningObservationProvider, + RuntimeTransportActionEncoder, +) +from .environment import ( + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentFactory, + PlanningObservationPort, +) +from .simulation import ( + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, +) +from .simulation_policies import SimulationSegmentPolicyPort + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.sim_manager import SimulationManager + + +MotionGeneratorFactory = Callable[[], MotionGenerator] +"""Zero-argument factory that must return one fresh motion generator.""" + + +class ControlCommandStateEvidenceTracker(AcceptedRuntimeCommandObserver): + """Track row-local open/grasp state from accepted semantic commands. + + This is the explicit lightweight evidence option selected for simulations + that do not expose a typed contact sensor. It does not claim physical + contact by itself: the built-in effect contract still conjuncts this + binary command state with live object-to-endpoint pose evidence. State is + updated only after the complete command frame has been encoded and accepted + by :class:`BufferedGymCommandSink`. + + Args: + control_profiles: Exact semantic command profiles installed in the + atomic engine, keyed by concrete control-part name. + env_ids: Stable full simulation batch correlation IDs. + """ + + def __init__( + self, + control_profiles: Mapping[str, ControlPartCommandProfile], + env_ids: torch.Tensor, + ) -> None: + if not isinstance(control_profiles, Mapping): + raise TypeError("control_profiles must be a mapping.") + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError( + "env_ids must be a non-empty one-dimensional int64 tensor." + ) + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + + commands: dict[ + str, + tuple[JointPositionCommand, JointPositionCommand], + ] = {} + for control_part, profile in control_profiles.items(): + if type(control_part) is not str or not control_part: + raise ValueError("control_profiles keys must be non-empty strings.") + if not isinstance(profile, ControlPartCommandProfile): + raise TypeError( + "control_profiles values must be ControlPartCommandProfile values." + ) + open_command = profile.commands.get(OPEN_COMMAND) + grasp_command = profile.commands.get(GRASP_COMMAND) + if open_command is None and grasp_command is None: + continue + if not isinstance(open_command, JointPositionCommand) or not isinstance( + grasp_command, + JointPositionCommand, + ): + raise TypeError( + f"Control part {control_part!r} must define both open and grasp " + "as JointPositionCommand values for command-state evidence." + ) + if open_command.equivalent_to(grasp_command): + raise ValueError( + f"Control part {control_part!r} has indistinguishable open and " + "grasp commands." + ) + commands[control_part] = ( + open_command.snapshot(), + grasp_command.snapshot(), + ) + + self._commands = commands + self._env_ids = env_ids.clone() + self._row_by_env_id = { + int(env_id): row + for row, env_id in enumerate(env_ids.detach().cpu().tolist()) + } + batch_size = int(env_ids.numel()) + self._values = { + control_part: torch.zeros( + batch_size, + dtype=torch.bool, + device=env_ids.device, + ) + for control_part in commands + } + self._valid = { + control_part: torch.zeros_like(values) + for control_part, values in self._values.items() + } + + @property + def tracked_control_parts(self) -> tuple[str, ...]: + """Return control parts with exact open/grasp semantic commands.""" + return tuple(self._commands) + + def accepted(self, command: RuntimeCommandFrame) -> None: + """Commit exact open/grasp states for active rows in an accepted frame.""" + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") + rows = self._rows(command.env_ids) + for endpoint_command in command.commands: + target = endpoint_command.target + payload = endpoint_command.payload + if not isinstance(target, JointPositionTarget) or not isinstance( + payload, + JointPositionPayload, + ): + continue + semantic_commands = self._commands.get(target.control_part) + if semantic_commands is None: + continue + open_command, grasp_command = semantic_commands + open_positions = open_command.resolve( + num_envs=command.batch_size, + control_dof=payload.dof, + device=payload.device, + dtype=payload.positions.dtype, + ) + grasp_positions = grasp_command.resolve( + num_envs=command.batch_size, + control_dof=payload.dof, + device=payload.device, + dtype=payload.positions.dtype, + ) + is_open = torch.isclose( + payload.positions, + open_positions, + rtol=0.0, + atol=1.0e-7, + ).all(dim=1) + is_grasp = torch.isclose( + payload.positions, + grasp_positions, + rtol=0.0, + atol=1.0e-7, + ).all(dim=1) + if bool((is_open & is_grasp).any().item()): + raise ValueError( + "An accepted row matched both open and grasp commands." + ) + recognized = command.active_mask & (is_open | is_grasp) + if not bool(recognized.any().item()): + continue + destination_rows = torch.tensor( + rows, + dtype=torch.long, + device=self._env_ids.device, + ) + selected_rows = destination_rows[recognized.to(destination_rows.device)] + values = self._values[target.control_part] + valid = self._valid[target.control_part] + values[selected_rows] = is_grasp[recognized].to(values.device) + valid[selected_rows] = True + + def cancelled(self, targets: tuple[RuntimeEndpointTarget, ...]) -> None: + """Invalidate every row owned by cancelled control-part targets.""" + if not isinstance(targets, tuple) or not all( + isinstance(target, RuntimeEndpointTarget) for target in targets + ): + raise TypeError("targets must contain RuntimeEndpointTarget values.") + for target in targets: + if isinstance(target, JointPositionTarget): + self._clear_control_part(target.control_part) + + def discarded(self) -> None: + """Invalidate all command-derived state after a fail-closed discard.""" + for control_part in self._commands: + self._clear_control_part(control_part) + + def observe( + self, + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectObservation: + """Return selected command-state rows for one typed binary query.""" + if type(query) is not BinaryEffectEvidenceQuery: + raise TypeError("query must be exactly BinaryEffectEvidenceQuery.") + if type(context) is not EffectEvidenceCollectionContext: + raise TypeError("context must be exactly EffectEvidenceCollectionContext.") + address = query.source.address + if type(address) is not ControlPartEvidenceAddress: + raise TypeError( + "Command-state evidence requires ControlPartEvidenceAddress." + ) + rows = self._rows(context.env_ids) + values = self._values.get(address.control_part) + valid = self._valid.get(address.control_part) + if values is None or valid is None: + missing = torch.zeros( + context.env_ids.numel(), + dtype=torch.bool, + device=context.env_ids.device, + ) + return BinaryEffectObservation( + values=missing, + valid=missing, + acquisition_errors=( + f"Control part {address.control_part!r} has no exact open/grasp " + "command-state profile.", + ) + * int(context.env_ids.numel()), + ) + indices = torch.tensor(rows, dtype=torch.long, device=values.device) + selected_values = values.index_select(0, indices).to(context.env_ids.device) + selected_valid = valid.index_select(0, indices).to(context.env_ids.device) + errors = tuple( + ( + None + if bool(row_valid) + else "No accepted open/grasp command has established this row's state." + ) + for row_valid in selected_valid.detach().cpu().tolist() + ) + return BinaryEffectObservation( + values=selected_values, + valid=selected_valid, + acquisition_errors=errors, + ) + + def __call__( + self, + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectObservation: + """Delegate callback use to :meth:`observe`.""" + return self.observe(query, context) + + def _rows(self, env_ids: torch.Tensor) -> tuple[int, ...]: + """Resolve stable correlation IDs to full simulation row indices.""" + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError( + "env_ids must be a non-empty one-dimensional int64 tensor." + ) + if env_ids.device != self._env_ids.device: + raise ValueError("env_ids must share the tracker device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + try: + return tuple( + self._row_by_env_id[int(env_id)] + for env_id in env_ids.detach().cpu().tolist() + ) + except KeyError as exc: + raise ValueError( + f"Environment ID {int(exc.args[0])} is absent from tracker env_ids." + ) from exc + + def _clear_control_part(self, control_part: str) -> None: + """Fail-closed reset one tracked control part when present.""" + values = self._values.get(control_part) + valid = self._valid.get(control_part) + if values is not None and valid is not None: + values.zero_() + valid.zero_() + + +class SimulationExpertProgramEnvironment(Protocol): + """Minimal Gym environment surface used by the simulation factory.""" + + sim: SimulationManager + robot: Robot + + @property + def step_dt(self) -> float: + """Return the authoritative Gym control cadence in seconds.""" + + +def _positive_finite(value: float, *, field_name: str) -> float: + """Validate one positive finite real number.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a real number.") + normalized = float(value) + if not math.isfinite(normalized) or normalized <= 0.0: + raise ValueError(f"{field_name} must be finite and positive.") + return normalized + + +def _non_negative_finite(value: float, *, field_name: str) -> float: + """Validate one non-negative finite real number.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a real number.") + normalized = float(value) + if not math.isfinite(normalized) or normalized < 0.0: + raise ValueError(f"{field_name} must be finite and non-negative.") + return normalized + + +def _robot_uid(robot: Robot) -> str: + """Return one strict live robot UID.""" + uid = getattr(robot, "uid", None) + if type(uid) is not str or not uid or uid != uid.strip(): + raise ValueError( + "robot.uid must be a non-empty string without outer whitespace." + ) + return uid + + +def _full_robot_tensor( + robot: Robot, + getter_name: str, + *, + required: bool, + reference: torch.Tensor | None = None, +) -> torch.Tensor | None: + """Read and validate one full-robot floating state tensor.""" + getter = getattr(robot, getter_name, None) + if not callable(getter): + if required: + raise TypeError(f"robot must provide {getter_name}().") + return None + value = getter() + if not isinstance(value, torch.Tensor): + raise TypeError(f"robot.{getter_name}() must return a torch.Tensor.") + if not value.is_floating_point() or value.dim() != 2: + raise ValueError( + f"robot.{getter_name}() must return floating shape (B, robot_dof)." + ) + if value.shape[0] == 0 or value.shape[1] == 0: + raise ValueError(f"robot.{getter_name}() dimensions must be non-zero.") + if reference is not None and ( + value.shape != reference.shape or value.device != reference.device + ): + raise ValueError( + f"robot.{getter_name}() must match robot.get_qpos() shape and device." + ) + if not bool(torch.isfinite(value).all().item()): + raise ValueError(f"robot.{getter_name}() must contain only finite values.") + return value.clone() + + +class SharedTickSceneProvider(SceneProvider): + """Share one immutable scene snapshot across consumers in the same tick. + + ``RegistrySceneProvider`` is stateful: every call observes native entities + and updates material-change baselines. Planning observations and multiple + evidence providers can legitimately request the same timestamp. This + wrapper always delegates one full-batch request per tick, then returns the + exact snapshot or an owned ordered-row projection to later consumers. + """ + + def __init__( + self, + delegate: RegistrySceneProvider, + full_env_ids: torch.Tensor, + ) -> None: + if type(delegate) is not RegistrySceneProvider: + raise TypeError("delegate must be exactly RegistrySceneProvider.") + if ( + not isinstance(full_env_ids, torch.Tensor) + or full_env_ids.dtype != torch.long + or full_env_ids.dim() != 1 + or full_env_ids.numel() == 0 + ): + raise ValueError("full_env_ids must be a non-empty 1D int64 tensor.") + if torch.unique(full_env_ids).numel() != full_env_ids.numel(): + raise ValueError("full_env_ids must be unique.") + self._delegate = delegate + self._full_env_ids = full_env_ids.clone() + self._row_by_env_id = { + int(env_id): row + for row, env_id in enumerate(full_env_ids.detach().cpu().tolist()) + } + self._timestamp: float | None = None + self._snapshot: SceneSnapshot | None = None + + @property + def delegate(self) -> RegistrySceneProvider: + """Return the authoritative stateful registry provider.""" + return self._delegate + + @property + def collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical dynamic collision IDs from the delegate.""" + return self._delegate.collision_entity_ids + + @property + def full_env_ids(self) -> torch.Tensor: + """Return the authoritative full simulation batch order.""" + return self._full_env_ids.clone() + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + """Return the single shared snapshot for ``timestamp`` and ``env_ids``.""" + if isinstance(timestamp, bool) or not isinstance(timestamp, (int, float)): + raise TypeError("timestamp must be a real number.") + normalized_timestamp = float(timestamp) + if not math.isfinite(normalized_timestamp) or normalized_timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError("env_ids must be a non-empty 1D int64 tensor.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + if env_ids.device != self._full_env_ids.device: + raise ValueError("env_ids must share the full simulation batch device.") + try: + rows = tuple( + self._row_by_env_id[int(env_id)] + for env_id in env_ids.detach().cpu().tolist() + ) + except KeyError as exc: + raise ValueError( + f"Environment ID {int(exc.args[0])} is absent from full_env_ids." + ) from exc + + if self._timestamp is not None: + if normalized_timestamp < self._timestamp: + raise ValueError("Shared scene snapshot timestamps must be monotonic.") + if normalized_timestamp == self._timestamp: + assert self._snapshot is not None + return self._select_rows(self._snapshot, rows) + + snapshot = self._delegate.snapshot( + timestamp=normalized_timestamp, + env_ids=self._full_env_ids.clone(), + ) + if not isinstance(snapshot, SceneSnapshot): + raise TypeError( + "RegistrySceneProvider.snapshot() must return SceneSnapshot." + ) + if snapshot.timestamp != normalized_timestamp: + raise ValueError("Scene snapshot timestamp must match the requested tick.") + self._timestamp = normalized_timestamp + self._snapshot = snapshot + return self._select_rows(snapshot, rows) + + def _select_rows( + self, + snapshot: SceneSnapshot, + rows: tuple[int, ...], + ) -> SceneSnapshot: + """Project one cached full-batch snapshot to an ordered row subset.""" + full_size = int(self._full_env_ids.numel()) + if rows == tuple(range(full_size)): + return snapshot + entities: dict[str, EntityState] = {} + for entity_id, state in snapshot.entities.items(): + pose = state.pose + if pose.dim() == 3: + if pose.shape[0] != full_size: + raise ValueError( + f"Scene entity {entity_id!r} batch does not match " + "full_env_ids." + ) + index = torch.tensor(rows, dtype=torch.long, device=pose.device) + pose = pose.index_select(0, index) + entities[entity_id] = EntityState(pose, confidence=state.confidence) + + articulation_joints: dict[tuple[str, str], ObservedArticulationJointState] = {} + for address, state in snapshot.articulation_joints.items(): + position = state.position + valid = state.valid_mask + if position.dim() == 2: + if position.shape[0] != full_size: + raise ValueError( + f"Scene articulation joint {address!r} batch does not " + "match full_env_ids." + ) + index = torch.tensor(rows, dtype=torch.long, device=position.device) + position = position.index_select(0, index) + if valid is not None: + valid = valid.index_select(0, index.to(valid.device)) + articulation_joints[address] = ObservedArticulationJointState( + position, + valid, + ) + + revisions = snapshot.collision_world_revisions(full_size) + return SceneSnapshot( + timestamp=snapshot.timestamp, + version=snapshot.version, + entities=entities, + collision_world_revision=tuple(revisions[row] for row in rows), + collision_entity_ids=snapshot.collision_entity_ids, + articulation_joints=articulation_joints, + ) + + +class SimulationPlanningObservationProvider(GymPlanningObservationProvider): + """Gym planning observations backed by live robot and shared scene state.""" + + def __init__( + self, + robot: Robot, + scene_provider: SharedTickSceneProvider, + clock: EnvironmentStepClock, + env_ids: torch.Tensor, + command_state_tracker: ControlCommandStateEvidenceTracker, + *, + owner_token: object, + ) -> None: + if type(scene_provider) is not SharedTickSceneProvider: + raise TypeError("scene_provider must be exactly SharedTickSceneProvider.") + if type(clock) is not EnvironmentStepClock: + raise TypeError("clock must be exactly EnvironmentStepClock.") + qpos = _full_robot_tensor(robot, "get_qpos", required=True) + assert qpos is not None + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.shape != (qpos.shape[0],) + ): + raise ValueError("env_ids must be int64 with one ID per robot row.") + if env_ids.device != qpos.device: + raise ValueError("env_ids and robot qpos must share a device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + if type(command_state_tracker) is not ControlCommandStateEvidenceTracker: + raise TypeError( + "command_state_tracker must be exactly " + "ControlCommandStateEvidenceTracker." + ) + self._robot = robot + self._scene_provider = scene_provider + self._clock = clock + self._env_ids = env_ids.clone() + self._command_state_tracker = command_state_tracker + self._owner_token = owner_token + super().__init__(self._capture) + + @property + def scene_provider(self) -> SharedTickSceneProvider: + """Return the snapshot-sharing scene provider used by evidence ports.""" + return self._scene_provider + + @property + def env_ids(self) -> torch.Tensor: + """Return stable ordered simulation row IDs.""" + return self._env_ids.clone() + + @property + def command_state_tracker(self) -> ControlCommandStateEvidenceTracker: + """Return the runtime-local accepted-command evidence owner.""" + return self._command_state_tracker + + def is_owned_by(self, owner_token: object) -> bool: + """Return whether this provider belongs to one factory instance.""" + return self._owner_token is owner_token + + def _capture(self, task_state: TaskState) -> PlanningContext: + """Capture one synchronized robot and scene observation.""" + qpos = _full_robot_tensor(self._robot, "get_qpos", required=True) + assert qpos is not None + if ( + qpos.shape[0] != self._env_ids.numel() + or qpos.device != self._env_ids.device + ): + raise ValueError("Robot batch shape or device changed after assembly.") + qvel = _full_robot_tensor( + self._robot, + "get_qvel", + required=False, + reference=qpos, + ) + if qvel is None: + qvel = torch.zeros_like(qpos) + qeffort = _full_robot_tensor( + self._robot, + "get_qf", + required=False, + reference=qpos, + ) + timestamp = self._clock.now() + scene = self._scene_provider.snapshot( + timestamp=timestamp, + env_ids=self._env_ids.clone(), + ) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=qvel, + qeffort=qeffort, + ), + task=task_state, + scene=scene, + env_ids=self._env_ids, + control_dt=self._clock.step_dt, + ) + + +class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): + """Build every live Expert Program component from explicit declarations. + + Args: + simulation: Exact live simulation that owns ``robot`` and scene UIDs. + robot: Exact robot selected for planning and evidence acquisition. + scene_binding: Canonical-to-native scene declaration. + robot_profile_binding: Typed robot resource and policy declaration. + step_dt: Authoritative Gym control cadence. + planner_cfg: Explicit planner configuration. ``None`` selects TOPPRA + for ``robot.uid``. + motion_generator_factory: Optional fresh-generator factory. It is + mutually exclusive with ``planner_cfg`` and intended for custom + planners and isolated tests. + endpoint_adapters: Explicit adapters for non-built-in resource endpoint + types. + settle_presets: Optional named segment settling policies. + translation_threshold: Material scene translation threshold. + rotation_threshold: Material scene rotation threshold. + contact_observer: Optional raw contact evidence callback. + constraint_observer: Optional raw constraint evidence callback. + force_observer: Optional raw force evidence callback. + wrench_observer: Optional raw wrench evidence callback. + + 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. + """ + + def __init__( + self, + simulation: SimulationManager, + robot: Robot, + scene_binding: SimulationSceneBinding, + robot_profile_binding: SimulationRobotSkillProfileBinding, + *, + step_dt: float, + planner_cfg: BasePlannerCfg | None = None, + motion_generator_factory: MotionGeneratorFactory | None = None, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + contact_observer: BinaryObservationCallback | None = None, + constraint_observer: BinaryObservationCallback | None = None, + force_observer: ScalarObservationCallback | None = None, + wrench_observer: ScalarObservationCallback | None = None, + ) -> None: + if type(scene_binding) is not SimulationSceneBinding: + raise TypeError("scene_binding must be exactly SimulationSceneBinding.") + if type(robot_profile_binding) is not SimulationRobotSkillProfileBinding: + raise TypeError( + "robot_profile_binding must be exactly " + "SimulationRobotSkillProfileBinding." + ) + if planner_cfg is not None and motion_generator_factory is not None: + raise ValueError( + "planner_cfg and motion_generator_factory are mutually exclusive." + ) + if planner_cfg is not None and not isinstance(planner_cfg, BasePlannerCfg): + raise TypeError("planner_cfg must be a BasePlannerCfg or None.") + if motion_generator_factory is not None and not callable( + motion_generator_factory + ): + raise TypeError("motion_generator_factory must be callable or None.") + if endpoint_adapters is not None and not isinstance(endpoint_adapters, Mapping): + raise TypeError("endpoint_adapters must be a mapping or None.") + for name, callback in ( + ("contact_observer", contact_observer), + ("constraint_observer", constraint_observer), + ("force_observer", force_observer), + ("wrench_observer", wrench_observer), + ): + if callback is not None and not callable(callback): + raise TypeError(f"{name} must be callable or None.") + + robot_uid = _robot_uid(robot) + get_robot = getattr(simulation, "get_robot", None) + if not callable(get_robot): + raise TypeError("simulation must provide get_robot().") + if get_robot(robot_uid) is not robot: + raise ValueError( + f"simulation.get_robot({robot_uid!r}) must return the exact " + "selected robot." + ) + selected_planner_cfg = deepcopy(planner_cfg) + if ( + selected_planner_cfg is not None + and selected_planner_cfg.robot_uid != robot_uid + ): + raise ValueError( + f"planner_cfg.robot_uid must equal selected robot UID {robot_uid!r}." + ) + + self._simulation = simulation + self._robot = robot + self._scene_binding = scene_binding + self._robot_profile_binding = robot_profile_binding + self._step_dt = _positive_finite(step_dt, field_name="step_dt") + self._planner_cfg = selected_planner_cfg + self._motion_generator_factory = motion_generator_factory + self._endpoint_adapters = ( + None if endpoint_adapters is None else dict(endpoint_adapters) + ) + self._translation_threshold = _non_negative_finite( + translation_threshold, + field_name="translation_threshold", + ) + self._rotation_threshold = _non_negative_finite( + rotation_threshold, + field_name="rotation_threshold", + ) + self._contact_observer = contact_observer + self._constraint_observer = constraint_observer + self._force_observer = force_observer + self._wrench_observer = wrench_observer + self._owner_token = object() + + qpos = _full_robot_tensor(robot, "get_qpos", required=True) + assert qpos is not None + self._env_ids = torch.arange( + qpos.shape[0], + dtype=torch.long, + device=qpos.device, + ) + self._segment_policy_port = SimulationSegmentPolicyPort( + simulation, + robot, + scene_binding, + settle_presets=settle_presets, + env_ids=self._env_ids, + ) + + @classmethod + def from_environment( + cls, + environment: SimulationExpertProgramEnvironment, + *, + scene_binding: SimulationSceneBinding, + robot_profile_binding: SimulationRobotSkillProfileBinding, + planner_cfg: BasePlannerCfg | None = None, + motion_generator_factory: MotionGeneratorFactory | None = None, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + contact_observer: BinaryObservationCallback | None = None, + constraint_observer: BinaryObservationCallback | None = None, + force_observer: ScalarObservationCallback | None = None, + wrench_observer: ScalarObservationCallback | None = None, + ) -> SimulationExpertProgramFactory: + """Create a factory from the explicit standard Gym environment surface.""" + simulation = getattr(environment, "sim", None) + robot = getattr(environment, "robot", None) + try: + step_dt = environment.step_dt + except AttributeError as exc: + raise TypeError("environment must expose step_dt.") from exc + if simulation is None or robot is None: + raise TypeError("environment must expose non-None sim and robot values.") + return cls( + simulation, + robot, + scene_binding, + robot_profile_binding, + step_dt=step_dt, + planner_cfg=planner_cfg, + motion_generator_factory=motion_generator_factory, + endpoint_adapters=endpoint_adapters, + settle_presets=settle_presets, + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + contact_observer=contact_observer, + constraint_observer=constraint_observer, + force_observer=force_observer, + wrench_observer=wrench_observer, + ) + + @property + def scene_registry_id(self) -> str: + """Return the exact configured scene-registry ID.""" + return self._scene_binding.registry_id + + @property + def robot_profile_id(self) -> str: + """Return the exact configured robot-profile ID.""" + return self._robot_profile_binding.profile_id + + @property + def step_dt(self) -> float: + """Return the authoritative Gym control cadence.""" + return self._step_dt + + @property + def segment_policy_port(self) -> SimulationSegmentPolicyPort: + """Return the shared simulation post-policy and validator port.""" + return self._segment_policy_port + + @property + def endpoint_adapters( + self, + ) -> Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None: + """Return an owned copy of installed custom endpoint adapters.""" + return ( + None if self._endpoint_adapters is None else dict(self._endpoint_adapters) + ) + + def create_scene_registry(self) -> SceneRegistry: + """Build one fresh authoritative registry from explicit bindings.""" + return self._scene_binding.build(self._simulation) + + def create_robot_skill_profile(self) -> RobotSkillProfile: + """Build the declarative profile without embedding Gym cadence.""" + return self._robot_profile_binding.build(self._robot) + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + """Create a fresh engine around the selected planner and exact profile.""" + if not isinstance(profile, RobotSkillProfile): + raise TypeError("profile must be a RobotSkillProfile.") + if profile.profile_id != self.robot_profile_id: + raise ValueError( + f"profile ID must be {self.robot_profile_id!r}, got " + f"{profile.profile_id!r}." + ) + motion_generator = self._create_motion_generator() + if motion_generator.robot is not self._robot: + raise ValueError( + "Motion generator must own the exact robot selected by the factory." + ) + return AtomicActionEngine( + motion_generator, + skill_profile=profile, + endpoint_adapters=self._endpoint_adapters, + ) + + def create_planning_observation_provider( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + clock: EnvironmentStepClock, + ) -> PlanningObservationPort: + """Create one planning port and planner-validated shared scene provider.""" + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not self._robot: + raise ValueError("engine must own the exact factory robot.") + if type(clock) is not EnvironmentStepClock: + raise TypeError("clock must be exactly EnvironmentStepClock.") + if clock.step_dt != self._step_dt: + raise ValueError("clock.step_dt must equal the factory Gym cadence.") + provider = scene_registry.make_planning_scene_provider( + engine.motion_generator, + batch_size=int(self._env_ids.numel()), + translation_threshold=self._translation_threshold, + rotation_threshold=self._rotation_threshold, + ) + shared = SharedTickSceneProvider(provider, self._env_ids) + command_state_tracker = ControlCommandStateEvidenceTracker( + engine.control_profiles, + self._env_ids, + ) + return SimulationPlanningObservationProvider( + self._robot, + shared, + clock, + self._env_ids, + command_state_tracker, + owner_token=self._owner_token, + ) + + def create_effect_evidence_providers( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> Iterable[EffectEvidenceProvider]: + """Create built-in control-part and articulation evidence providers.""" + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not self._robot: + raise ValueError("engine must own the exact factory robot.") + if type(observation_provider) is not SimulationPlanningObservationProvider: + raise TypeError( + "observation_provider must be exactly " + "SimulationPlanningObservationProvider." + ) + if not observation_provider.is_owned_by(self._owner_token): + raise ValueError("observation_provider belongs to another factory.") + scene_provider = observation_provider.scene_provider + command_state_tracker = observation_provider.command_state_tracker + contact_observer = self._contact_observer or command_state_tracker + constraint_observer = self._constraint_observer or command_state_tracker + providers: list[EffectEvidenceProvider] = [] + if isinstance(self._robot, ControlPartRobotEvidenceSource): + providers.append( + ControlPartSimulationEvidenceProvider( + self._robot, + scene_provider=scene_provider, + contact_observer=contact_observer, + constraint_observer=constraint_observer, + force_observer=self._force_observer, + wrench_observer=self._wrench_observer, + ) + ) + providers.append( + SceneArticulationEvidenceProvider(scene_provider=scene_provider) + ) + return tuple(providers) + + def create_accepted_runtime_command_observer( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> AcceptedRuntimeCommandObserver: + """Return the tracker already shared with this runtime's evidence ports.""" + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not self._robot: + raise ValueError("engine must own the exact factory robot.") + if type(observation_provider) is not SimulationPlanningObservationProvider: + raise TypeError( + "observation_provider must be exactly " + "SimulationPlanningObservationProvider." + ) + if not observation_provider.is_owned_by(self._owner_token): + raise ValueError("observation_provider belongs to another factory.") + return observation_provider.command_state_tracker + + def create_adapter( + self, + *, + call_catalog: SemanticCallCatalog | None = None, + registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), + relation_grounders: Iterable[RelationTargetGrounder] = (), + handover_pose_providers: Iterable[HandOverPoseProvider] = (), + effect_monitor_registry: EffectMonitorRegistry | None = None, + runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), + runner_cfg: ExecutionRunnerCfg | None = None, + parallel_safety_validator: ParallelCommandSafetyValidator | None = None, + ) -> ExpertProgramEnvironmentAdapter: + """Create the exact Gym adapter with shared simulation policy ports.""" + return ExpertProgramEnvironmentAdapter( + self, + step_dt=self._step_dt, + call_catalog=call_catalog, + endpoint_adapters=self._endpoint_adapters, + registered_lowerers=registered_lowerers, + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + effect_monitor_registry=effect_monitor_registry, + runtime_transports=runtime_transports, + runner_cfg=runner_cfg, + post_policy_port=self._segment_policy_port, + validator_port=self._segment_policy_port, + parallel_safety_validator=parallel_safety_validator, + ) + + def _create_motion_generator(self) -> MotionGenerator: + """Create and validate one exact motion generator.""" + if self._motion_generator_factory is not None: + generator = self._motion_generator_factory() + else: + planner_cfg = ( + ToppraPlannerCfg(robot_uid=_robot_uid(self._robot)) + if self._planner_cfg is None + else deepcopy(self._planner_cfg) + ) + generator = MotionGenerator(MotionGenCfg(planner_cfg=planner_cfg)) + if not isinstance(generator, MotionGenerator): + raise TypeError( + "motion_generator_factory must return a MotionGenerator instance." + ) + return generator + + +def create_simulation_expert_program_adapter( + environment: SimulationExpertProgramEnvironment, + *, + scene_binding: SimulationSceneBinding, + robot_profile_binding: SimulationRobotSkillProfileBinding, + planner_cfg: BasePlannerCfg | None = None, + motion_generator_factory: MotionGeneratorFactory | None = None, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + relation_grounders: Iterable[RelationTargetGrounder] = (), + handover_pose_providers: Iterable[HandOverPoseProvider] = (), + runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), + settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + contact_observer: BinaryObservationCallback | None = None, + constraint_observer: BinaryObservationCallback | None = None, + force_observer: ScalarObservationCallback | None = None, + wrench_observer: ScalarObservationCallback | None = None, + parallel_safety_validator: ParallelCommandSafetyValidator | None = None, +) -> ExpertProgramEnvironmentAdapter: + """Create a complete production adapter from one standard Gym environment. + + This is the intended task-side one-line integration. Relation-target + grounders and embodiment-owned handover pose providers are explicit and + default to empty collections, so calls that require an uninstalled provider + remain fail-closed during program preflight. Advanced callers can retain + :class:`SimulationExpertProgramFactory` and call ``create_adapter`` directly + to install registered semantic lowerers or custom monitors. Custom endpoint + adapters and their matching Gym runtime transports are accepted here so a + non-joint endpoint remains executable through the one-line path. + + Args: + environment: Standard Gym simulation environment exposing ``sim``, + ``robot``, and ``step_dt``. + scene_binding: Authoritative typed scene declaration. + robot_profile_binding: Typed robot resource and policy declaration. + planner_cfg: Optional planner configuration owned by the factory. + motion_generator_factory: Optional factory for one fresh motion generator. + endpoint_adapters: Optional exact-type custom endpoint adapters. + relation_grounders: Explicit typed relation-target grounders. + handover_pose_providers: Explicit embodiment-owned handover pose providers. + runtime_transports: Additional runtime-command-to-Gym encoders. + settle_presets: Optional named dynamic-settling policies. + translation_threshold: Scene translation revision threshold. + rotation_threshold: Scene rotation revision threshold. + contact_observer: Optional raw contact evidence callback. + constraint_observer: Optional raw constraint evidence callback. + force_observer: Optional raw force evidence callback. + wrench_observer: Optional raw wrench evidence callback. + parallel_safety_validator: Optional authoritative parallel-command gate. + + Returns: + Complete production Expert Program environment adapter. + """ + factory = SimulationExpertProgramFactory.from_environment( + environment, + scene_binding=scene_binding, + robot_profile_binding=robot_profile_binding, + planner_cfg=planner_cfg, + motion_generator_factory=motion_generator_factory, + endpoint_adapters=endpoint_adapters, + settle_presets=settle_presets, + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + contact_observer=contact_observer, + constraint_observer=constraint_observer, + force_observer=force_observer, + wrench_observer=wrench_observer, + ) + return factory.create_adapter( + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + runtime_transports=runtime_transports, + parallel_safety_validator=parallel_safety_validator, + ) + + +__all__ = [ + "ControlCommandStateEvidenceTracker", + "MotionGeneratorFactory", + "SharedTickSceneProvider", + "SimulationExpertProgramEnvironment", + "SimulationExpertProgramFactory", + "SimulationPlanningObservationProvider", + "create_simulation_expert_program_adapter", +] diff --git a/embodichain/lab/gym/envs/expert_program/simulation_policies.py b/embodichain/lab/gym/envs/expert_program/simulation_policies.py new file mode 100644 index 000000000..c18dace2c --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation_policies.py @@ -0,0 +1,715 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Simulation-backed Expert Program post-policies and validators. + +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`. +""" + +from __future__ import annotations + +from collections.abc import Iterator, Mapping +from copy import deepcopy +from dataclasses import dataclass +import math +from types import MappingProxyType +from typing import Any, TYPE_CHECKING + +import torch + +from embodichain.lab.gym.envs.settling import ( + DynamicSettleMonitor, + DynamicSettleMonitorCfg, + DynamicSettleSample, + DynamicSettleState, +) + +from .compiler import ( + CompiledPostPolicy, + CompiledProgramSegment, + CompiledProgramValidator, +) +from .simulation import SimulationSceneBinding + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.sim_manager import SimulationManager + + +@dataclass(frozen=True, slots=True) +class _SimulationSettleTarget: + """One canonical entity resolved through an explicit native binding.""" + + canonical_id: str + kind: str + native_entity: Any + + +def _default_settle_presets() -> Mapping[str, DynamicSettleMonitorCfg]: + """Return independently owned built-in post-policy presets.""" + return MappingProxyType( + { + "rigid_object": DynamicSettleMonitorCfg( + linear_velocity_threshold=0.03, + angular_velocity_threshold=0.20, + min_steps=10, + max_steps=240, + check_interval_steps=2, + required_stable_checks=3, + ) + } + ) + + +def _json_speed_values(value: torch.Tensor) -> list[float | None]: + """Convert speed evidence to finite JSON numbers or explicit unknowns.""" + return [ + float(item) if math.isfinite(float(item)) else None + for item in value.detach().cpu().tolist() + ] + + +class SimulationSegmentPolicyPort: + """Execute built-in segment policies against explicitly bound simulation data. + + Args: + simulation: Live simulation used only for UIDs declared in + ``scene_binding``. + robot: Live robot used to produce controller-safe full-qpos holds. + scene_binding: Exact canonical-to-native scene declaration. + settle_presets: Named settling policies. ``None`` installs the shared + ``rigid_object`` preset. + env_ids: Optional stable logical row IDs. They describe correlation, + not simulator row indices; simulator rows remain ordered exactly as + returned by the robot and bound entities. + + The same instance implements both ``SegmentPostPolicyPort`` and + ``SegmentValidatorPort``. Unknown policy types, presets, canonical IDs, or + native entities fail before an action is emitted. + """ + + def __init__( + self, + simulation: SimulationManager, + robot: Robot, + scene_binding: SimulationSceneBinding, + *, + settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, + env_ids: torch.Tensor | None = None, + ) -> None: + if type(scene_binding) is not SimulationSceneBinding: + raise TypeError("scene_binding must be exactly SimulationSceneBinding.") + qpos = self._read_robot_qpos(robot) + if env_ids is None: + env_ids = torch.arange( + qpos.shape[0], + dtype=torch.long, + device=qpos.device, + ) + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor or None.") + if env_ids.dtype != torch.long or env_ids.shape != (qpos.shape[0],): + raise ValueError("env_ids must be int64 with one ID per simulator row.") + if env_ids.device != qpos.device: + raise ValueError("env_ids and robot qpos must share a device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must contain unique values.") + + selected_presets = ( + _default_settle_presets() if settle_presets is None else settle_presets + ) + if not isinstance(selected_presets, Mapping) or not selected_presets: + raise ValueError("settle_presets must be a non-empty mapping.") + normalized_presets: dict[str, DynamicSettleMonitorCfg] = {} + for preset_id, cfg in selected_presets.items(): + if ( + type(preset_id) is not str + or not preset_id + or preset_id != preset_id.strip() + ): + raise ValueError( + "Settle preset IDs must be non-empty strings without outer " + "whitespace." + ) + if not isinstance(cfg, DynamicSettleMonitorCfg): + raise TypeError( + "settle_presets values must be DynamicSettleMonitorCfg values." + ) + normalized_presets[preset_id] = cfg.snapshot() + + self._simulation = simulation + self._robot = robot + self._scene_binding = scene_binding + self._env_ids = env_ids.clone() + self._row_indices = torch.arange( + qpos.shape[0], + dtype=torch.long, + device=qpos.device, + ) + self._settle_presets = MappingProxyType(normalized_presets) + self._settle_targets, self._rigid_objects = self._resolve_native_entities() + self._post_policy_results: dict[int, dict[str, object]] = {} + self._post_policy_success: dict[int, torch.Tensor] = {} + self._validator_results: dict[int, dict[str, object]] = {} + + @property + def settle_preset_ids(self) -> tuple[str, ...]: + """Return installed post-policy preset IDs in declaration order.""" + return tuple(self._settle_presets) + + def validate_policy( + self, + policy: Any, + *, + segment: Any, + ) -> None: + """Validate one post-policy against static bindings without observation. + + This method reads only the compiled declaration, installed preset + table, and entities resolved when the port was constructed. It never + samples velocity or qpos and never emits a controller action. + """ + if type(policy) is not CompiledPostPolicy: + raise TypeError("policy must be exactly CompiledPostPolicy.") + self._validate_segment_membership(segment, policy, kind="post policy") + if policy.cfg.kind != "wait_stable": + raise ValueError( + f"Unsupported compiled post-policy kind {policy.cfg.kind!r}." + ) + if policy.cfg.preset not in self._settle_presets: + raise KeyError( + f"Unknown settle preset {policy.cfg.preset!r}; available presets " + f"are {sorted(self._settle_presets)}." + ) + entity_id = policy.entity.entity_id + target = self._settle_targets.get(entity_id) + if target is None: + raise KeyError( + f"Canonical settle entity {entity_id!r} has no explicit native " + "dynamic binding." + ) + if target.kind == "rigid_object" and bool( + getattr(target.native_entity, "is_non_dynamic", False) + ): + raise ValueError( + f"Canonical settle entity {entity_id!r} is static or kinematic." + ) + + def actions( + self, + policy: Any, + *, + segment: Any, + active_mask: torch.Tensor, + ) -> Iterator[torch.Tensor]: + """Yield full-qpos hold actions until active rows settle or time out. + + Args: + policy: Exact compiled ``wait_stable`` policy. + segment: Exact segment that owns ``policy``. + active_mask: Rows that remain eligible after runtime execution and + preceding post-policies. Inactive rows are held safely but do + not participate in settling, timeout, or success results. + + Yields: + Fresh full-qpos hold commands consumed by ordinary ``env.step()``. + + Timeout is a normal row-local result boundary. Timed-out rows are + exposed through :meth:`post_policy_result` and + :meth:`post_policy_metadata`; no batch-level exception is raised. + """ + self.validate_policy(policy, segment=segment) + active_mask = self._validate_active_mask(active_mask) + preset = self._settle_presets[policy.cfg.preset] + entity_id = policy.entity.entity_id + target = self._settle_targets[entity_id] + + result_key = id(policy) + self._post_policy_results.pop(result_key, None) + self._post_policy_success.pop(result_key, None) + if not bool(active_mask.any().item()): + self._post_policy_success[result_key] = active_mask.clone() + self._post_policy_results[result_key] = { + "kind": policy.cfg.kind, + "entity_id": entity_id, + "preset": policy.cfg.preset, + "source_path": list(policy.source_path), + "status": "skipped", + "active_mask": active_mask.detach().cpu().tolist(), + "thresholds": self._settle_threshold_metadata(preset), + "state": self._empty_settle_state_metadata(active_mask), + } + return + + active_rows = self._row_indices[active_mask] + monitor = DynamicSettleMonitor(preset, self._env_ids[active_mask]) + elapsed_steps = 0 + while True: + state = monitor.observe( + (self._measure_settle_target(target, row_indices=active_rows),), + elapsed_steps=elapsed_steps, + ) + settled_mask = torch.zeros_like(active_mask) + settled_mask[active_mask] = state.settled_mask + self._post_policy_results[result_key] = { + "kind": policy.cfg.kind, + "entity_id": entity_id, + "preset": policy.cfg.preset, + "source_path": list(policy.source_path), + "active_mask": active_mask.detach().cpu().tolist(), + "status": ( + "settled" + if bool(state.settled_mask.all().item()) + else ( + "timed_out" + if bool(state.timeout_mask.any().item()) + else "running" + ) + ), + "thresholds": self._settle_threshold_metadata(preset), + "state": self._expand_settle_state_metadata(state, active_mask), + } + self._post_policy_success[result_key] = settled_mask + if bool(state.settled_mask.all().item()): + return + if bool(state.timeout_mask.any().item()): + return + yield self._read_robot_qpos(self._robot) + elapsed_steps += 1 + + def post_policy_result( + self, + policy: Any, + *, + segment: Any, + ) -> torch.Tensor: + """Return the latest independently owned per-row settling result.""" + if type(policy) is not CompiledPostPolicy: + raise TypeError("policy must be exactly CompiledPostPolicy.") + self._validate_segment_membership(segment, policy, kind="post policy") + result = self._post_policy_success.get(id(policy)) + if result is None: + raise RuntimeError("Post-policy result is unavailable before execution.") + return result.clone() + + def post_policy_metadata( + self, + policy: Any, + *, + segment: Any, + ) -> Mapping[str, object]: + """Return the latest JSON-safe settling trace for one policy. + + The trace is available after the policy generator has started. A + terminal trace has status ``"settled"``, ``"timed_out"``, or + ``"skipped"`` when no rows remain active; an early demo interruption + intentionally retains the latest ``"running"`` snapshot for diagnosis. + """ + if type(policy) is not CompiledPostPolicy: + raise TypeError("policy must be exactly CompiledPostPolicy.") + self._validate_segment_membership(segment, policy, kind="post policy") + metadata = self._post_policy_results.get(id(policy)) + if metadata is None: + raise RuntimeError("Post-policy metadata is unavailable before execution.") + return deepcopy(metadata) + + def validate_validator( + self, + validator: Any, + *, + segment: Any, + ) -> None: + """Validate one validator against static bindings without observation.""" + if type(validator) is not CompiledProgramValidator: + raise TypeError("validator must be exactly CompiledProgramValidator.") + self._validate_segment_membership(segment, validator, kind="validator") + if validator.cfg.kind != "object_near_target": + raise ValueError( + f"Unsupported compiled validator kind {validator.cfg.kind!r}." + ) + entity_id = validator.object.entity_id + if entity_id not in self._rigid_objects: + raise KeyError( + f"Canonical validator object {entity_id!r} has no explicit rigid-" + "object binding." + ) + + def validate(self, validator: Any, *, segment: Any) -> torch.Tensor: + """Observe an explicitly bound rigid object against a world target. + + Args: + validator: Exact compiled ``object_near_target`` validator. + segment: Exact segment that owns ``validator``. + + Returns: + Boolean tensor with one result per simulation row. + """ + self.validate_validator(validator, segment=segment) + entity_id = validator.object.entity_id + entity = self._rigid_objects[entity_id] + pose = self._read_pose(entity, entity_id=entity_id) + current_position = pose[:, :3, 3] + target_position = validator.target_pose.position.to( + device=current_position.device, + dtype=current_position.dtype, + ) + if target_position.dim() == 1: + target_position = target_position.unsqueeze(0).expand_as(current_position) + elif target_position.shape != current_position.shape: + raise ValueError( + "Validator target batch must be unbatched or match simulator rows." + ) + error = torch.linalg.vector_norm(current_position - target_position, dim=1) + accepted = torch.isfinite(error) & ( + error <= float(validator.cfg.position_tolerance) + ) + self._validator_results[id(validator)] = { + "kind": validator.cfg.kind, + "object_id": entity_id, + "target_id": validator.target_selection.target_id, + "target_value_index": validator.target_selection.value_index, + "source_path": list(validator.source_path), + "position_tolerance": float(validator.cfg.position_tolerance), + "env_ids": self._env_ids.detach().cpu().tolist(), + "object_position": current_position.detach().cpu().tolist(), + "target_position": target_position.detach().cpu().tolist(), + "position_error": error.detach().cpu().tolist(), + "accepted_mask": accepted.detach().cpu().tolist(), + } + return accepted + + def validator_metadata( + self, + validator: Any, + *, + segment: Any, + ) -> Mapping[str, object]: + """Return an owned JSON-safe trace for one completed validator.""" + if type(validator) is not CompiledProgramValidator: + raise TypeError("validator must be exactly CompiledProgramValidator.") + self._validate_segment_membership(segment, validator, kind="validator") + metadata = self._validator_results.get(id(validator)) + if metadata is None: + raise RuntimeError("Validator metadata is unavailable before validation.") + return deepcopy(metadata) + + @staticmethod + def _read_robot_qpos(robot: Robot) -> torch.Tensor: + """Capture one finite full-robot position batch.""" + get_qpos = getattr(robot, "get_qpos", None) + if not callable(get_qpos): + raise TypeError("robot must provide get_qpos().") + qpos = get_qpos() + if ( + not isinstance(qpos, torch.Tensor) + or not qpos.is_floating_point() + or qpos.dim() != 2 + or qpos.shape[0] == 0 + or qpos.shape[1] == 0 + ): + raise ValueError("robot.get_qpos() must return floating shape (B, J).") + if not bool(torch.isfinite(qpos).all().item()): + raise ValueError("robot.get_qpos() must contain finite values.") + return qpos.clone() + + 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): + raise TypeError("active_mask must be a torch.Tensor.") + if active_mask.dtype != torch.bool or active_mask.shape != self._env_ids.shape: + raise ValueError( + "active_mask must be bool with one value per simulator row." + ) + if active_mask.device != self._env_ids.device: + raise ValueError("active_mask and env_ids must share a device.") + return active_mask.clone() + + @staticmethod + def _settle_threshold_metadata( + preset: DynamicSettleMonitorCfg, + ) -> dict[str, float | int]: + """Serialize one settling preset without exposing mutable state.""" + return { + "linear_velocity": float(preset.linear_velocity_threshold), + "angular_velocity": float(preset.angular_velocity_threshold), + "min_steps": preset.min_steps, + "max_steps": preset.max_steps, + "check_interval_steps": preset.check_interval_steps, + "required_stable_checks": preset.required_stable_checks, + } + + def _empty_settle_state_metadata( + self, + active_mask: torch.Tensor, + ) -> dict[str, object]: + """Return a full-batch trace for a policy with no eligible rows.""" + batch_size = self._env_ids.numel() + return { + "elapsed_steps": 0, + "observation_count": 0, + "env_ids": self._env_ids.detach().cpu().tolist(), + "active_mask": active_mask.detach().cpu().tolist(), + "stable_counts": [0] * batch_size, + "settled_mask": [False] * batch_size, + "timeout_mask": [False] * batch_size, + "checked": False, + "max_linear_speed": [None] * batch_size, + "max_angular_speed": [None] * batch_size, + } + + def _expand_settle_state_metadata( + self, + state: DynamicSettleState, + active_mask: torch.Tensor, + ) -> dict[str, object]: + """Expand active-row monitor state to the stable full-batch ordering.""" + stable_counts = torch.zeros_like(self._env_ids) + settled_mask = torch.zeros_like(active_mask) + timeout_mask = torch.zeros_like(active_mask) + max_linear_speed = torch.full( + active_mask.shape, + float("inf"), + dtype=state.max_linear_speed.dtype, + device=active_mask.device, + ) + max_angular_speed = torch.full_like(max_linear_speed, float("inf")) + stable_counts[active_mask] = state.stable_counts + settled_mask[active_mask] = state.settled_mask + timeout_mask[active_mask] = state.timeout_mask + max_linear_speed[active_mask] = state.max_linear_speed + max_angular_speed[active_mask] = state.max_angular_speed + return { + "elapsed_steps": state.elapsed_steps, + "observation_count": state.observation_count, + "env_ids": self._env_ids.detach().cpu().tolist(), + "active_mask": active_mask.detach().cpu().tolist(), + "stable_counts": stable_counts.detach().cpu().tolist(), + "settled_mask": settled_mask.detach().cpu().tolist(), + "timeout_mask": timeout_mask.detach().cpu().tolist(), + "checked": state.checked, + "max_linear_speed": _json_speed_values(max_linear_speed), + "max_angular_speed": _json_speed_values(max_angular_speed), + } + + @staticmethod + def _validate_segment_membership( + segment: Any, + member: CompiledPostPolicy | CompiledProgramValidator, + *, + kind: str, + ) -> None: + """Require the supplied compiled value to belong to the exact segment.""" + if type(segment) is not CompiledProgramSegment: + raise TypeError("segment must be exactly CompiledProgramSegment.") + values = ( + segment.post_policies + if type(member) is CompiledPostPolicy + else segment.validators + ) + if not any(value is member for value in values): + raise ValueError( + f"Compiled {kind} does not belong to the supplied segment." + ) + + def _resolve_native_entities( + self, + ) -> tuple[ + Mapping[str, _SimulationSettleTarget], + Mapping[str, Any], + ]: + """Resolve only explicitly declared canonical/native pairs.""" + settle_targets: dict[str, _SimulationSettleTarget] = {} + rigid_objects: dict[str, Any] = {} + articulation_targets: dict[str, _SimulationSettleTarget] = {} + + for binding in self._scene_binding.rigid_objects: + entity = self._require_native( + "get_rigid_object", + canonical_id=binding.entity_id, + simulation_uid=binding.simulation_uid, + ) + target = _SimulationSettleTarget( + binding.entity_id, + "rigid_object", + entity, + ) + settle_targets[binding.entity_id] = target + rigid_objects[binding.entity_id] = entity + + for binding in self._scene_binding.articulations: + entity = self._require_native( + "get_articulation", + canonical_id=binding.entity_id, + simulation_uid=binding.simulation_uid, + ) + target = _SimulationSettleTarget( + binding.entity_id, + "articulation", + entity, + ) + settle_targets[binding.entity_id] = target + articulation_targets[binding.entity_id] = target + + for binding in self._scene_binding.links: + settle_targets[binding.entity_id] = self._require_parent_target( + articulation_targets, + child_id=binding.entity_id, + parent_id=binding.articulation_id, + ) + for binding in self._scene_binding.antipodal_grasps: + parent = settle_targets.get(binding.object_id) + if parent is None or parent.kind != "rigid_object": + raise KeyError( + f"Affordance {binding.entity_id!r} references unavailable rigid " + f"object {binding.object_id!r}." + ) + settle_targets[binding.entity_id] = parent + for binding in self._scene_binding.articulation_operations: + settle_targets[binding.entity_id] = self._require_parent_target( + articulation_targets, + child_id=binding.entity_id, + parent_id=binding.articulation_id, + ) + return MappingProxyType(settle_targets), MappingProxyType(rigid_objects) + + @staticmethod + def _require_parent_target( + targets: Mapping[str, _SimulationSettleTarget], + *, + child_id: str, + parent_id: str, + ) -> _SimulationSettleTarget: + """Resolve a child to one explicitly declared articulation root.""" + target = targets.get(parent_id) + if target is None: + raise KeyError( + f"Canonical entity {child_id!r} references unavailable parent " + f"{parent_id!r}." + ) + return target + + def _require_native( + self, + getter_name: str, + *, + canonical_id: str, + simulation_uid: str, + ) -> Any: + """Resolve one explicitly selected native simulation entity.""" + getter = getattr(self._simulation, getter_name, None) + if not callable(getter): + raise TypeError(f"simulation must provide {getter_name}().") + entity = getter(simulation_uid) + if entity is None: + raise KeyError( + f"Native entity {simulation_uid!r} selected for canonical entity " + f"{canonical_id!r} was not found." + ) + return entity + + def _measure_settle_target( + self, + target: _SimulationSettleTarget, + *, + row_indices: torch.Tensor, + ) -> DynamicSettleSample: + """Measure physical bodies for explicitly selected simulator rows.""" + if target.kind == "articulation": + body_data = getattr(target.native_entity, "body_data", None) + velocity = getattr(body_data, "body_link_vel", None) + if not isinstance(velocity, torch.Tensor): + raise RuntimeError( + f"Articulation settle target {target.canonical_id!r} has no " + "body_link_vel tensor." + ) + selected = velocity.index_select(0, row_indices.to(velocity.device)) + if selected.dim() != 3 or selected.shape[-1] != 6: + raise ValueError( + "Articulation body_link_vel must have shape (B, N, 6)." + ) + linear_velocity = selected[..., :3] + angular_velocity = selected[..., 3:] + else: + body_data = getattr(target.native_entity, "body_data", None) + linear_velocity = getattr(body_data, "lin_vel", None) + angular_velocity = getattr(body_data, "ang_vel", None) + if not isinstance(linear_velocity, torch.Tensor) or not isinstance( + angular_velocity, + torch.Tensor, + ): + raise RuntimeError( + f"Rigid settle target {target.canonical_id!r} has no linear/" + "angular velocity tensors." + ) + rows = row_indices.to(linear_velocity.device) + linear_velocity = linear_velocity.index_select(0, rows) + angular_velocity = angular_velocity.index_select( + 0, + row_indices.to(angular_velocity.device), + ) + if ( + linear_velocity.shape != angular_velocity.shape + or linear_velocity.dim() < 2 + or linear_velocity.shape[-1] != 3 + ): + raise ValueError( + "Rigid body velocities must have equal shape (B, ..., 3)." + ) + + linear_speed = torch.linalg.vector_norm(linear_velocity, dim=-1).reshape( + row_indices.numel(), + -1, + ) + angular_speed = torch.linalg.vector_norm(angular_velocity, dim=-1).reshape( + row_indices.numel(), + -1, + ) + device = self._env_ids.device + return DynamicSettleSample( + entity_id=target.canonical_id, + linear_speed=linear_speed.to(device=device), + angular_speed=angular_speed.to(device=device), + ) + + def _read_pose(self, entity: Any, *, entity_id: str) -> torch.Tensor: + """Read one rigid-object pose batch in simulator row order.""" + getter = getattr(entity, "get_local_pose", None) + if not callable(getter): + raise TypeError( + f"Native rigid object for {entity_id!r} must provide " + "get_local_pose()." + ) + pose = getter(to_matrix=True) + if not isinstance(pose, torch.Tensor) or not pose.is_floating_point(): + raise TypeError("get_local_pose(to_matrix=True) must return a tensor.") + batch_size = int(self._env_ids.numel()) + if pose.shape == (4, 4): + pose = pose.unsqueeze(0).expand(batch_size, -1, -1) + elif pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Rigid object {entity_id!r} pose must have shape " + f"({batch_size}, 4, 4)." + ) + if not bool(torch.isfinite(pose).all().item()): + raise ValueError(f"Rigid object {entity_id!r} pose must be finite.") + return pose.clone() + + +__all__ = ["SimulationSegmentPolicyPort"] diff --git a/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py b/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py index b857f1078..9418d6182 100644 --- a/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py +++ b/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py @@ -18,14 +18,17 @@ from __future__ import annotations -import math from collections.abc import Sequence -from numbers import Real from typing import TYPE_CHECKING, Literal import torch from embodichain.lab.gym.envs.managers.cfg import SceneEntityCfg +from embodichain.lab.gym.envs.settling import ( + DynamicSettleMonitor, + DynamicSettleMonitorCfg, + DynamicSettleSample, +) from embodichain.lab.sim.objects import Articulation, RigidObject, RigidObjectGroup from embodichain.utils import logger @@ -37,7 +40,6 @@ _DynamicEntity = RigidObject | RigidObjectGroup | Articulation _SettleEntity = tuple[str, SceneEntityCfg, _DynamicEntity] -_SpeedSample = tuple[str, torch.Tensor, torch.Tensor] def _validate_settle_parameters( @@ -49,48 +51,21 @@ def _validate_settle_parameters( required_stable_checks: int, timeout_behavior: str, allow_partial_envs: bool, -) -> None: - """Validate dynamic-object settle parameters.""" - for name, value in ( - ("min_steps", min_steps), - ("max_steps", max_steps), - ("check_interval_steps", check_interval_steps), - ("required_stable_checks", required_stable_checks), - ): - if isinstance(value, bool) or not isinstance(value, int): - raise TypeError(f"{name} must be an integer, got {type(value).__name__}.") - - if min_steps < 0: - raise ValueError("min_steps must be non-negative.") - if max_steps < min_steps: - raise ValueError("max_steps must be greater than or equal to min_steps.") - if check_interval_steps < 1: - raise ValueError("check_interval_steps must be at least 1.") - if required_stable_checks < 1: - raise ValueError("required_stable_checks must be at least 1.") - - for name, value in ( - ("linear_velocity_threshold", linear_velocity_threshold), - ("angular_velocity_threshold", angular_velocity_threshold), - ): - if isinstance(value, bool) or not isinstance(value, Real): - raise TypeError(f"{name} must be a real number.") - if not math.isfinite(float(value)) or value < 0: - raise ValueError(f"{name} must be finite and non-negative.") - - available_checks = ( - 1 + (max_steps - min_steps + check_interval_steps - 1) // check_interval_steps +) -> DynamicSettleMonitorCfg: + """Validate parameters and return the reusable monitor policy.""" + cfg = DynamicSettleMonitorCfg( + linear_velocity_threshold=linear_velocity_threshold, + angular_velocity_threshold=angular_velocity_threshold, + min_steps=min_steps, + max_steps=max_steps, + check_interval_steps=check_interval_steps, + required_stable_checks=required_stable_checks, ) - if required_stable_checks > available_checks: - raise ValueError( - "required_stable_checks cannot be reached within the configured " - f"step budget; at most {available_checks} checks are possible." - ) - if timeout_behavior not in ("warn", "raise"): raise ValueError("timeout_behavior must be either 'warn' or 'raise'.") if not isinstance(allow_partial_envs, bool): raise TypeError("allow_partial_envs must be a boolean.") + return cfg def _normalize_settle_env_ids( @@ -210,9 +185,9 @@ def _resolve_settle_entities( def _measure_settle_speeds( entities: Sequence[_SettleEntity], env_ids: torch.Tensor, -) -> list[_SpeedSample]: +) -> list[DynamicSettleSample]: """Measure per-body linear and angular speeds for selected environments.""" - samples: list[_SpeedSample] = [] + samples: list[DynamicSettleSample] = [] for kind, entity_cfg, entity in entities: if kind == "articulation": velocity = entity.body_data.body_link_vel[env_ids] @@ -238,29 +213,35 @@ def _measure_settle_speeds( angular_speed = torch.linalg.vector_norm(angular_velocity, dim=-1).reshape( env_ids.numel(), -1 ) - samples.append((entity_cfg.uid, linear_speed, angular_speed)) + samples.append( + DynamicSettleSample( + entity_id=entity_cfg.uid, + linear_speed=linear_speed, + angular_speed=angular_speed, + ) + ) return samples def _settle_samples_are_stable( - samples: Sequence[_SpeedSample], + samples: Sequence[DynamicSettleSample], linear_velocity_threshold: float, angular_velocity_threshold: float, ) -> bool: """Return whether every measured body is finite and below both thresholds.""" stable = [] - for _, linear_speed, angular_speed in samples: + for sample in samples: stable.append( - torch.isfinite(linear_speed) - & torch.isfinite(angular_speed) - & (linear_speed <= linear_velocity_threshold) - & (angular_speed <= angular_velocity_threshold) + torch.isfinite(sample.linear_speed) + & torch.isfinite(sample.angular_speed) + & (sample.linear_speed <= linear_velocity_threshold) + & (sample.angular_speed <= angular_velocity_threshold) ) return bool(torch.cat([value.reshape(-1) for value in stable]).all().item()) def _format_settle_timeout( - samples: Sequence[_SpeedSample], + samples: Sequence[DynamicSettleSample], env_ids: torch.Tensor, linear_velocity_threshold: float, angular_velocity_threshold: float, @@ -272,7 +253,9 @@ def _format_settle_timeout( unsettled: list[str] = [] all_linear_speeds: list[torch.Tensor] = [] all_angular_speeds: list[torch.Tensor] = [] - for uid, linear_speed, angular_speed in samples: + for sample in samples: + linear_speed = sample.linear_speed + angular_speed = sample.angular_speed stable = ( torch.isfinite(linear_speed) & torch.isfinite(angular_speed) @@ -282,7 +265,7 @@ def _format_settle_timeout( unsettled_mask = ~stable.all(dim=1) if bool(unsettled_mask.any().item()): unsettled_env_ids = env_ids[unsettled_mask].detach().cpu().tolist() - unsettled.append(f"{uid}(env_ids={unsettled_env_ids})") + unsettled.append(f"{sample.entity_id}(env_ids={unsettled_env_ids})") all_linear_speeds.append(linear_speed.reshape(-1)) all_angular_speeds.append(angular_speed.reshape(-1)) @@ -364,7 +347,7 @@ def wait_for_dynamic_objects_to_settle( TypeError: If a parameter or entity configuration has the wrong type. ValueError: If parameters, targets, or environment selection are invalid. """ - _validate_settle_parameters( + monitor_cfg = _validate_settle_parameters( linear_velocity_threshold=linear_velocity_threshold, angular_velocity_threshold=angular_velocity_threshold, min_steps=min_steps, @@ -395,20 +378,14 @@ def wait_for_dynamic_objects_to_settle( env.sim.update(step=min_steps) step_count = min_steps - stable_checks = 0 - samples: list[_SpeedSample] + monitor = DynamicSettleMonitor(monitor_cfg, target_env_ids) + samples: list[DynamicSettleSample] + settle_state = None while True: samples = _measure_settle_speeds(entities, target_env_ids) - if _settle_samples_are_stable( - samples, - linear_velocity_threshold=linear_velocity_threshold, - angular_velocity_threshold=angular_velocity_threshold, - ): - stable_checks += 1 - if stable_checks >= required_stable_checks: - return - else: - stable_checks = 0 + settle_state = monitor.observe(samples, elapsed_steps=step_count) + if bool(settle_state.settled_mask.all().item()): + return if step_count >= max_steps: break @@ -422,7 +399,7 @@ def wait_for_dynamic_objects_to_settle( linear_velocity_threshold=linear_velocity_threshold, angular_velocity_threshold=angular_velocity_threshold, max_steps=max_steps, - stable_checks=stable_checks, + stable_checks=int(settle_state.stable_counts.min().item()), required_stable_checks=required_stable_checks, ) if timeout_behavior == "raise": diff --git a/embodichain/lab/gym/envs/settling.py b/embodichain/lab/gym/envs/settling.py new file mode 100644 index 000000000..39d5be171 --- /dev/null +++ b/embodichain/lab/gym/envs/settling.py @@ -0,0 +1,374 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Reusable per-environment dynamic-settling state machine.""" + +from __future__ import annotations + +import math +from collections.abc import Sequence +from dataclasses import dataclass +from numbers import Real + +import torch + +from embodichain.utils import configclass + + +@configclass +class DynamicSettleMonitorCfg: + """Threshold and cadence policy for :class:`DynamicSettleMonitor`. + + The monitor never advances an environment. Callers own the stepping path + and provide raw velocity samples after the configured minimum/cadence. + This lets reset events and demonstration post-policies share exactly the + same state transition rules while using different stepping ports. + """ + + linear_velocity_threshold: float = 0.03 + """Maximum stable linear speed in metres per second.""" + + angular_velocity_threshold: float = 0.20 + """Maximum stable angular speed in radians per second.""" + + min_steps: int = 10 + """Minimum number of environment steps before the first check.""" + + max_steps: int = 240 + """Maximum elapsed environment steps before unresolved rows time out.""" + + check_interval_steps: int = 2 + """Minimum number of steps between independent evidence checks.""" + + required_stable_checks: int = 3 + """Consecutive stable checks required independently for each row.""" + + def __post_init__(self) -> None: + for name in ( + "min_steps", + "max_steps", + "check_interval_steps", + "required_stable_checks", + ): + value = getattr(self, name) + if isinstance(value, bool) or not isinstance(value, int): + raise TypeError(f"{name} must be an integer.") + if self.min_steps < 0: + raise ValueError("min_steps must be non-negative.") + if self.max_steps < self.min_steps: + raise ValueError("max_steps must be greater than or equal to min_steps.") + if self.check_interval_steps < 1: + raise ValueError("check_interval_steps must be at least 1.") + if self.required_stable_checks < 1: + raise ValueError("required_stable_checks must be at least 1.") + for name in ( + "linear_velocity_threshold", + "angular_velocity_threshold", + ): + value = getattr(self, name) + if isinstance(value, bool) or not isinstance(value, Real): + raise TypeError(f"{name} must be a real number.") + if not math.isfinite(float(value)) or float(value) < 0.0: + raise ValueError(f"{name} must be finite and non-negative.") + available_checks = ( + 1 + + (self.max_steps - self.min_steps + self.check_interval_steps - 1) + // self.check_interval_steps + ) + if self.required_stable_checks > available_checks: + raise ValueError( + "required_stable_checks cannot be reached within the configured " + f"step budget; at most {available_checks} checks are possible." + ) + + def snapshot(self) -> DynamicSettleMonitorCfg: + """Return an independently owned configuration value.""" + return DynamicSettleMonitorCfg( + linear_velocity_threshold=self.linear_velocity_threshold, + angular_velocity_threshold=self.angular_velocity_threshold, + min_steps=self.min_steps, + max_steps=self.max_steps, + check_interval_steps=self.check_interval_steps, + required_stable_checks=self.required_stable_checks, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class DynamicSettleSample: + """Raw per-body speed evidence for one registered scene entity. + + Args: + entity_id: Stable entity identifier used in metadata and diagnostics. + linear_speed: Per-row body speeds with shape ``(B, N)``. + angular_speed: Per-row body speeds with shape ``(B, N)``. + """ + + entity_id: str + linear_speed: torch.Tensor + angular_speed: torch.Tensor + + def __post_init__(self) -> None: + if ( + type(self.entity_id) is not str + or not self.entity_id + or self.entity_id != self.entity_id.strip() + ): + raise ValueError( + "entity_id must be a non-empty string without outer whitespace." + ) + for name in ("linear_speed", "angular_speed"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if not value.is_floating_point() or value.dim() != 2: + raise ValueError(f"{name} must be a floating tensor with shape (B, N).") + if value.shape[0] == 0 or value.shape[1] == 0: + raise ValueError(f"{name} must contain at least one row and body.") + if self.linear_speed.shape != self.angular_speed.shape: + raise ValueError("linear_speed and angular_speed must have equal shapes.") + if self.linear_speed.device != self.angular_speed.device: + raise ValueError("linear_speed and angular_speed must share a device.") + object.__setattr__(self, "linear_speed", self.linear_speed.clone()) + object.__setattr__(self, "angular_speed", self.angular_speed.clone()) + + def snapshot(self) -> DynamicSettleSample: + """Return an independently owned raw evidence sample.""" + return DynamicSettleSample( + entity_id=self.entity_id, + linear_speed=self.linear_speed, + angular_speed=self.angular_speed, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class DynamicSettleState: + """Owned state emitted after one monitor observation.""" + + env_ids: torch.Tensor + elapsed_steps: int + observation_count: int + checked: bool + stable_counts: torch.Tensor + settled_mask: torch.Tensor + timeout_mask: torch.Tensor + max_linear_speed: torch.Tensor + max_angular_speed: torch.Tensor + + def __post_init__(self) -> None: + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if self.env_ids.dtype != torch.long or self.env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if self.env_ids.numel() == 0: + raise ValueError("env_ids must contain at least one row.") + if type(self.elapsed_steps) is not int or self.elapsed_steps < 0: + raise ValueError("elapsed_steps must be a non-negative integer.") + if type(self.observation_count) is not int or self.observation_count < 0: + raise ValueError("observation_count must be a non-negative integer.") + if type(self.checked) is not bool: + raise TypeError("checked must be a bool.") + row_count = self.env_ids.numel() + for name, dtype in ( + ("stable_counts", torch.long), + ("settled_mask", torch.bool), + ("timeout_mask", torch.bool), + ): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != dtype or value.shape != (row_count,): + raise ValueError(f"{name} must have shape (B,) and dtype {dtype}.") + if value.device != self.env_ids.device: + raise ValueError(f"{name} and env_ids must share a device.") + if (self.settled_mask & self.timeout_mask).any(): + raise ValueError("settled_mask and timeout_mask must not overlap.") + for name in ("max_linear_speed", "max_angular_speed"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if not value.is_floating_point() or value.shape != (row_count,): + raise ValueError(f"{name} must be a floating tensor with shape (B,).") + if value.device != self.env_ids.device: + raise ValueError(f"{name} and env_ids must share a device.") + for name in ( + "env_ids", + "stable_counts", + "settled_mask", + "timeout_mask", + "max_linear_speed", + "max_angular_speed", + ): + object.__setattr__(self, name, getattr(self, name).clone()) + + @property + def complete(self) -> bool: + """Whether every row has either settled or timed out.""" + return bool((self.settled_mask | self.timeout_mask).all().item()) + + def to_metadata(self) -> dict[str, object]: + """Return deterministic, JSON-compatible post-policy metadata.""" + return { + "elapsed_steps": self.elapsed_steps, + "observation_count": self.observation_count, + "env_ids": self.env_ids.detach().to("cpu").tolist(), + "stable_counts": self.stable_counts.detach().to("cpu").tolist(), + "settled_mask": self.settled_mask.detach().to("cpu").tolist(), + "timeout_mask": self.timeout_mask.detach().to("cpu").tolist(), + "max_linear_speed": self.max_linear_speed.detach().to("cpu").tolist(), + "max_angular_speed": self.max_angular_speed.detach().to("cpu").tolist(), + } + + +class DynamicSettleMonitor: + """Track settling independently for stable environment IDs. + + Duplicate observations at the same ``elapsed_steps`` value are idempotent. + Regressing step counters are rejected, and a jump across multiple cadence + boundaries counts as one fresh observation rather than replaying one sample. + """ + + def __init__( + self, + cfg: DynamicSettleMonitorCfg, + env_ids: torch.Tensor, + ) -> None: + if not isinstance(cfg, DynamicSettleMonitorCfg): + raise TypeError("cfg must be a DynamicSettleMonitorCfg.") + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if env_ids.numel() == 0 or torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must contain unique environment IDs.") + self.cfg = cfg.snapshot() + self._env_ids = env_ids.clone() + self._stable_counts = torch.zeros_like(env_ids) + self._settled = torch.zeros_like(env_ids, dtype=torch.bool) + self._timeout = torch.zeros_like(env_ids, dtype=torch.bool) + self._max_linear = torch.full( + env_ids.shape, + float("inf"), + dtype=torch.float32, + device=env_ids.device, + ) + self._max_angular = self._max_linear.clone() + self._last_elapsed_steps = -1 + self._last_checked_steps = -1 + self._observation_count = 0 + + @property + def env_ids(self) -> torch.Tensor: + """Return the stable row IDs owned by this monitor.""" + return self._env_ids.clone() + + def observe( + self, + samples: Sequence[DynamicSettleSample], + *, + elapsed_steps: int, + ) -> DynamicSettleState: + """Consume one raw speed observation when the configured cadence is due. + + Args: + samples: One speed sample per monitored entity. + elapsed_steps: Steps advanced by the caller since post-policy start. + + Returns: + Per-row stable, settled, timeout, and velocity metadata. + """ + if type(elapsed_steps) is not int or elapsed_steps < 0: + raise ValueError("elapsed_steps must be a non-negative integer.") + if elapsed_steps < self._last_elapsed_steps: + raise ValueError("elapsed_steps must be monotonic.") + normalized = tuple(samples) + if not normalized or not all( + isinstance(sample, DynamicSettleSample) for sample in normalized + ): + raise ValueError("samples must contain DynamicSettleSample values.") + if len({sample.entity_id for sample in normalized}) != len(normalized): + raise ValueError("samples must use unique entity IDs.") + for sample in normalized: + if sample.linear_speed.shape[0] != self._env_ids.numel(): + raise ValueError("Every sample batch must match env_ids length.") + if sample.linear_speed.device != self._env_ids.device: + raise ValueError("Samples and env_ids must share a device.") + + duplicate = elapsed_steps == self._last_elapsed_steps + due = elapsed_steps >= self.cfg.min_steps and ( + self._last_checked_steps < 0 + or elapsed_steps - self._last_checked_steps >= self.cfg.check_interval_steps + or elapsed_steps >= self.cfg.max_steps + ) + checked = due and not duplicate and not self._timeout.all() + if checked: + linear = torch.cat([sample.linear_speed for sample in normalized], dim=1) + angular = torch.cat([sample.angular_speed for sample in normalized], dim=1) + finite = torch.isfinite(linear).all(dim=1) & torch.isfinite(angular).all( + dim=1 + ) + self._max_linear = torch.where( + torch.isfinite(linear), linear, torch.full_like(linear, float("inf")) + ).amax(dim=1) + self._max_angular = torch.where( + torch.isfinite(angular), + angular, + torch.full_like(angular, float("inf")), + ).amax(dim=1) + stable = ( + finite + & (self._max_linear <= self.cfg.linear_velocity_threshold) + & (self._max_angular <= self.cfg.angular_velocity_threshold) + ) + active = ~self._settled & ~self._timeout + self._stable_counts = torch.where( + active & stable, + self._stable_counts + 1, + torch.where( + active, torch.zeros_like(self._stable_counts), self._stable_counts + ), + ) + self._settled |= active & ( + self._stable_counts >= self.cfg.required_stable_checks + ) + self._observation_count += 1 + self._last_checked_steps = elapsed_steps + + if elapsed_steps >= self.cfg.max_steps: + self._timeout |= ~self._settled + self._last_elapsed_steps = elapsed_steps + return self._state(elapsed_steps=elapsed_steps, checked=checked) + + def _state(self, *, elapsed_steps: int, checked: bool) -> DynamicSettleState: + """Build an owned state snapshot.""" + return DynamicSettleState( + env_ids=self._env_ids, + elapsed_steps=elapsed_steps, + observation_count=self._observation_count, + checked=checked, + stable_counts=self._stable_counts, + settled_mask=self._settled, + timeout_mask=self._timeout, + max_linear_speed=self._max_linear, + max_angular_speed=self._max_angular, + ) + + +__all__ = [ + "DynamicSettleMonitor", + "DynamicSettleMonitorCfg", + "DynamicSettleSample", + "DynamicSettleState", +] diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index cf3c1086b..e524b6765 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -17,6 +17,7 @@ from __future__ import annotations import os +from pathlib import Path import numpy as np import torch import dexsim @@ -393,13 +394,22 @@ def cat_tensor_with_ids( return out -def config_to_cfg(config: dict, manager_modules: list = None) -> "EmbodiedEnvCfg": +def config_to_cfg( + config: dict, + manager_modules: list | None = None, + *, + source_path: str | os.PathLike[str] | None = None, +) -> "EmbodiedEnvCfg": """Parser configuration file into cfgs for env initialization. Args: config (dict): The configuration dictionary containing robot, sensor, light, background, and interactive objects. manager_modules (list): List of module paths for dataset, event, observation, and reward managers. If not provided, uses default module paths. + source_path: Optional path of the Gym configuration source file. A + relative top-level ``expert_program_path`` is resolved from this + file's directory. Without it, relative paths use the current + working directory. Returns: EmbodiedEnvCfg: A configuration object for initializing the environment. @@ -446,6 +456,30 @@ class ComponentCfg: if key not in config: log_error(f"Missing required config key: {key}") + if "expert_program_path" in config: + expert_program_path = config["expert_program_path"] + if type(expert_program_path) is not str: + raise TypeError("expert_program_path must be an exact string.") + if ( + not expert_program_path + or expert_program_path != expert_program_path.strip() + ): + raise ValueError( + "expert_program_path must be a non-empty string without outer " + "whitespace." + ) + from embodichain.lab.gym.envs.expert_program.loader import ( + load_expert_program, + ) + + expert_program_base_dir = ( + None if source_path is None else Path(source_path).expanduser().parent + ) + env_cfg.expert_program = load_expert_program( + expert_program_path, + base_dir=expert_program_base_dir, + ) + env_cfg.max_episode_steps = config.get("max_episode_steps", 300) env_cfg.num_envs = config.get("num_envs", 1) @@ -1021,16 +1055,20 @@ def build_env_cfg_from_args( tuple[EmbodiedEnvCfg, dict, dict]: A tuple containing the environment configuration object, the merged gym configuration dictionary, and the action configuration dictionary. """ + from embodichain.utils.config_paths import resolve_config_path from embodichain.utils.utility import load_config from embodichain.lab.gym.envs import EmbodiedEnvCfg - gym_config = load_config(args.gym_config) + gym_config_source_path = resolve_config_path(args.gym_config) + gym_config = load_config(gym_config_source_path) gym_config = merge_args_with_gym_config(args, gym_config) if gym_config_modifier is not None: gym_config_modifier(gym_config) cfg: EmbodiedEnvCfg = config_to_cfg( - gym_config, manager_modules=get_manager_modules() + gym_config, + manager_modules=get_manager_modules(), + source_path=gym_config_source_path, ) cfg.filter_visual_rand = args.filter_visual_rand cfg.filter_dataset_saving = args.filter_dataset_saving diff --git a/embodichain/lab/scripts/run_env.py b/embodichain/lab/scripts/run_env.py index 880d040fc..8c99f098f 100644 --- a/embodichain/lab/scripts/run_env.py +++ b/embodichain/lab/scripts/run_env.py @@ -17,6 +17,7 @@ from __future__ import annotations import argparse +import json import os import select import sys @@ -31,6 +32,9 @@ import tqdm from embodichain.lab.gym.envs.demo import DemoEpisodeResult, execute_demo_episode +from embodichain.lab.gym.envs.expert_program.loader import ( + load_expert_program as _load_expert_program, +) from embodichain.lab.gym.envs.wrapper import ReplayWrapper from embodichain.lab.gym.utils.gym_utils import ( add_env_launcher_args_to_parser, @@ -289,6 +293,16 @@ def generate_function( f"Episode {time_id} attempt {attempt}/{max_attempts} failed: " f"{result.terminal_reason}. Discarding {result.length} frames." ) + if debug_mode: + log_warning( + "Failed demo trace: " + + json.dumps( + result.to_metadata(), + allow_nan=False, + sort_keys=True, + separators=(",", ":"), + ) + ) return False @@ -741,6 +755,18 @@ def _create_parser() -> argparse.ArgumentParser: add_env_launcher_args_to_parser(parser, require_gym_config=True) parser.set_defaults(viser_image_fps=None) + parser.add_argument( + "--expert-program", + type=str, + default=None, + help="Path to a declarative Expert Program (.json, .yaml, or .yml).", + ) + parser.add_argument( + "--debug-mode", + action="store_true", + help="Log the structured trace for each failed demo attempt.", + ) + parser.add_argument( "--replay", action="store_true", @@ -832,6 +858,9 @@ def cli(argv: Sequence[str] | None = None) -> None: execute_init_hooks() env_cfg, gym_config, action_config = build_env_cfg_from_args(args) + expert_program_path = getattr(args, "expert_program", None) + if expert_program_path is not None: + env_cfg.expert_program = _load_expert_program(expert_program_path) if args.replay and args.replay_mode == "control": log_info("Dataset saving disabled for control replay mode.", color="green") diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index a877644b2..0c9d90547 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -1229,6 +1229,7 @@ def observe(self, task_state: TaskState) -> PlanningContext: task=task_state, scene=context.scene, env_ids=context.env_ids, + control_dt=context.control_dt, ) @@ -1765,6 +1766,7 @@ def _observe_for_grounding(self) -> PlanningContext: task=self._task_state, scene=context.scene, env_ids=context.env_ids, + control_dt=context.control_dt, ) if normalized.batch_size != self._task_state.batch_size: raise ValueError( diff --git a/embodichain/utils/__init__.py b/embodichain/utils/__init__.py index f3dd6ba62..fd680446c 100644 --- a/embodichain/utils/__init__.py +++ b/embodichain/utils/__init__.py @@ -20,6 +20,15 @@ """ from .configclass import configclass, is_configclass +from .config_paths import resolve_config_path + +__all__ = [ + "GLOBAL_SEED", + "configclass", + "is_configclass", + "resolve_config_path", + "set_seed", +] GLOBAL_SEED = 1024 diff --git a/embodichain/utils/config_paths.py b/embodichain/utils/config_paths.py new file mode 100644 index 000000000..e97346c98 --- /dev/null +++ b/embodichain/utils/config_paths.py @@ -0,0 +1,56 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Stable path resolution for user and packaged configuration files.""" + +from __future__ import annotations + +from pathlib import Path + +__all__ = ["resolve_config_path"] + + +def resolve_config_path(path: str | Path) -> Path: + """Resolve one configuration path without opening the target file. + + Existing, absolute, and ordinary relative paths preserve their normal + filesystem meaning. Repository-style paths below + ``embodichain_tasks/configs`` are redirected to the packaged task-config + resource so the same configuration reference works from an installed + wheel. + + Args: + path: User path or repository-style official-task configuration path. + + Returns: + Expanded filesystem path, resolved through the packaged task resource + only when the input uses the official-task configuration prefix. + + Raises: + TypeError: If ``path`` is not path-like. + """ + resolved_path = Path(path).expanduser() + if resolved_path.exists() or resolved_path.is_absolute(): + return resolved_path + + task_prefix = ("embodichain_tasks", "configs") + if resolved_path.parts[: len(task_prefix)] != task_prefix: + return resolved_path + + from embodichain_tasks.configs import get_config_path + + relative_path = Path(*resolved_path.parts[len(task_prefix) :]) + return get_config_path(relative_path) diff --git a/embodichain/utils/utility.py b/embodichain/utils/utility.py index 2c6b3cd1d..e44c5633f 100644 --- a/embodichain/utils/utility.py +++ b/embodichain/utils/utility.py @@ -31,6 +31,7 @@ from pathlib import Path from typing import Any, Dict, List, Tuple, Callable +from embodichain.utils.config_paths import resolve_config_path as _resolve_config_path from embodichain.utils.string import callable_to_string @@ -375,22 +376,6 @@ def _config_format_from_path(path: str | Path) -> str: ) -def _resolve_config_path(path: str | Path) -> Path: - """Resolve repository-style official-task paths from an installed wheel.""" - resolved_path = Path(path).expanduser() - if resolved_path.exists() or resolved_path.is_absolute(): - return resolved_path - - task_prefix = ("embodichain_tasks", "configs") - if resolved_path.parts[: len(task_prefix)] != task_prefix: - return resolved_path - - from embodichain_tasks.configs import get_config_path - - relative_path = Path(*resolved_path.parts[len(task_prefix) :]) - return get_config_path(relative_path) - - def load_config(path: str | Path) -> Dict[str, Any]: """Load a gym or agent config file into a dictionary. diff --git a/tests/gym/envs/expert_program/test_articulation_program.py b/tests/gym/envs/expert_program/test_articulation_program.py new file mode 100644 index 000000000..6629724cc --- /dev/null +++ b/tests/gym/envs/expert_program/test_articulation_program.py @@ -0,0 +1,185 @@ +# ---------------------------------------------------------------------------- +# 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 declarative articulation calls in Expert Programs.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramCfg, + ExpertProgramCompiler, + ExpertProgramDecodeError, + ExpertProgramIntegrationCfg, + InvokeCfg, + OperateArticulationCfg, + decode_expert_program, +) +from embodichain.lab.sim.atomic_actions import Affordance, EntityState +from embodichain.lab.sim.skills.calls import OperateArticulation +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRegistration, + SceneRegistry, +) + + +class _NeverObserveProvider: + """Reject state observation during static program compilation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + raise AssertionError("Compilation must not observe providers.") + + +def _payload(call: dict[str, object]) -> dict[str, object]: + return { + "schema_version": 1, + "program_id": "open_drawer", + "integration": { + "robot_profile": "manipulator", + "scene_registry": "scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": {"kind": "invoke", "call": call}, + } + + +def _compiler() -> ExpertProgramCompiler: + provider = _NeverObserveProvider() + drawer = SceneArticulationRef("drawer") + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=drawer, state_provider=provider), + SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + parent=drawer, + native_name="handle", + relative_pose=torch.eye(4), + affordance=Affordance(), + ), + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def _integration() -> ExpertProgramIntegrationCfg: + return ExpertProgramIntegrationCfg( + robot_profile="manipulator", + scene_registry="scene", + runtime_preset="safe", + ) + + +def test_decoder_accepts_named_and_explicit_articulation_targets() -> None: + named = decode_expert_program( + _payload( + { + "kind": "operate_articulation", + "articulation": "drawer", + "handle": "drawer_handle", + "target": "open", + "resources": {"primary": "right_arm"}, + } + ) + ) + explicit = decode_expert_program( + _payload( + { + "kind": "operate_articulation", + "articulation": "drawer", + "target_position": 0.42, + "target_displacement": 0.40, + } + ) + ) + + assert type(named.program) is InvokeCfg + assert named.program.call == OperateArticulationCfg( + articulation="drawer", + handle="drawer_handle", + target="open", + resources={"primary": "right_arm"}, + ) + assert type(explicit.program) is InvokeCfg + assert explicit.program.call == OperateArticulationCfg( + articulation="drawer", + target_position=0.42, + target_displacement=0.40, + ) + + +@pytest.mark.parametrize( + ("fields", "code"), + ( + ({"target": "open", "target_position": 0.4}, "conflicting_articulation_target"), + ({"target_position": 0.4}, "incomplete_articulation_target"), + ({"target_displacement": 0.2}, "incomplete_articulation_target"), + ({"target_position": True, "target_displacement": 0.2}, "invalid_number"), + ), +) +def test_decoder_rejects_ambiguous_or_incomplete_articulation_targets( + fields: dict[str, object], + code: str, +) -> None: + call = { + "kind": "operate_articulation", + "articulation": "drawer", + **fields, + } + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(_payload(call)) + + assert error.value.code == code + + +def test_compiler_preserves_typed_articulation_call_without_observation() -> None: + config = ExpertProgramCfg( + schema_version=1, + program_id="open_drawer", + integration=_integration(), + targets={}, + program=InvokeCfg( + call=OperateArticulationCfg( + articulation="drawer", + handle="drawer_handle", + target="open", + ) + ), + ) + + segment = tuple(_compiler().compile(config))[0] + call = segment.calls[0].call + + assert type(call) is OperateArticulation + assert call.articulation == SceneArticulationRef("drawer") + assert call.handle == SceneAffordanceRef("drawer_handle") + assert call.target == "open" + assert call.target_position is None + assert call.target_displacement is None + + +__all__: list[str] = [] diff --git a/tests/gym/envs/expert_program/test_bridge.py b/tests/gym/envs/expert_program/test_bridge.py new file mode 100644 index 000000000..9389fd5a9 --- /dev/null +++ b/tests/gym/envs/expert_program/test_bridge.py @@ -0,0 +1,2049 @@ +# ---------------------------------------------------------------------------- +# 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 + +from dataclasses import dataclass +import json +from typing import Any + +import pytest +import torch + +from embodichain.lab.gym.envs.demo import ProcessedEnvAction, execute_demo_episode +from embodichain.lab.gym.envs.expert_program.bridge import ( + AtomicDemoBridge, + BufferedGymCommandSink, + DemoBridgeError, + EnvironmentStepClock, + EnvironmentStepTimingError, + GymPlanningObservationProvider, + RuntimeCommandFrameEncoder, + UnsupportedRuntimeTransportError, +) +import embodichain.lab.gym.envs.expert_program.bridge as bridge_module +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.execution import ( + ExecutionEvent, + ExecutionEventKind, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + RuntimeCommandPayload, +) +from embodichain.lab.sim.atomic_actions.state import ( + PlanningContext, + RobotObservation, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy +from embodichain.lab.sim.skills.runtime import SkillResult, SkillStatus +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelLaneCommandSink, + ParallelRuntimeBranch, + ParallelSkillResult, + ParallelSkillRuntime, +) +from embodichain.lab.sim.skills.profiles import ResourceClaim + +STEP_DT = 0.02 +BATCH_SIZE = 2 +ROBOT_DOF = 5 + + +class _QposProvider: + """Return an owned fixed full-qpos snapshot.""" + + def __init__(self, qpos: torch.Tensor) -> None: + self.qpos = qpos.clone() + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + assert env_ids.numel() == self.qpos.shape[0] + return self.qpos.clone() + + +def _context( + *, + qpos: torch.Tensor | None = None, + env_ids: torch.Tensor | None = None, +) -> PlanningContext: + qpos = torch.zeros(BATCH_SIZE, ROBOT_DOF) if qpos is None else qpos + env_ids = torch.tensor([7, 3], dtype=torch.long) if env_ids is None else env_ids + return PlanningContext( + robot=RobotObservation( + timestamp=0.0, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=TaskState.empty(qpos.shape[0], qpos.device), + scene=SceneSnapshot.empty(), + env_ids=env_ids, + ) + + +def _joint_frame( + *, + duration: float, + active_mask: torch.Tensor | None = None, + positions: torch.Tensor | None = None, +) -> RuntimeCommandFrame: + active_mask = torch.tensor([True, True]) if active_mask is None else active_mask + positions = ( + torch.tensor([[10.0, 30.0], [11.0, 31.0]]) if positions is None else positions + ) + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget( + control_part="arm", + joint_ids=(1, 3), + ), + payload=JointPositionPayload(positions=positions), + ), + ), + active_mask=active_mask, + env_ids=torch.tensor([7, 3], dtype=torch.long), + hold_duration=torch.full((BATCH_SIZE,), duration), + ) + + +@dataclass(frozen=True, slots=True) +class _DummyTarget(RuntimeEndpointTarget): + """Test-only non-joint runtime target.""" + + name: str + + @property + def transport_id(self) -> str: + return "test.transport" + + @property + def target_id(self) -> str: + return self.name + + def snapshot(self) -> _DummyTarget: + return _DummyTarget(self.name) + + +@dataclass(frozen=True, slots=True, eq=False) +class _DummyPayload(RuntimeCommandPayload): + """Test-only scalar payload.""" + + values: torch.Tensor + + def __post_init__(self) -> None: + object.__setattr__(self, "values", self.values.clone()) + + @property + def batch_size(self) -> int: + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + return self.values.device + + @property + def transport_id(self) -> str: + return "test.transport" + + def snapshot(self) -> _DummyPayload: + return _DummyPayload(self.values) + + +class _DummyTransportEncoder: + """Test registration proving the frame encoder is transport-extensible.""" + + @property + def transport_id(self) -> str: + return "test.transport" + + def encode( + self, + command: EndpointCommand, + *, + base_action: Any, + active_mask: torch.Tensor, + ) -> Any: + assert isinstance(command.payload, _DummyPayload) + action = base_action.clone() + action[active_mask, 0] = command.payload.values[active_mask] + return action + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: Any, + context: PlanningContext, + ) -> Any: + del targets, context + return base_action.clone() + + +class _RecordingAcceptedCommandObserver: + """Record transactional sink notifications and optional callback failures.""" + + def __init__( + self, + *, + fail_accept: bool = False, + fail_cancel: bool = False, + ) -> None: + self.fail_accept = fail_accept + self.fail_cancel = fail_cancel + self.sink: BufferedGymCommandSink | None = None + self.accepted_frames: list[RuntimeCommandFrame] = [] + self.accepted_pending_counts: list[int] = [] + self.cancelled_targets: list[tuple[RuntimeEndpointTarget, ...]] = [] + self.cancelled_pending_counts: list[int] = [] + self.discard_count = 0 + + def accepted(self, command: RuntimeCommandFrame) -> None: + """Record acceptance after observing the sink's committed buffer.""" + if self.sink is None: + raise AssertionError("Observer sink must be assigned before use.") + self.accepted_pending_counts.append(self.sink.pending_count) + self.accepted_frames.append(command) + if self.fail_accept: + raise RuntimeError("observer rejected accepted command") + + def cancelled(self, targets: tuple[RuntimeEndpointTarget, ...]) -> None: + """Record owned cancellation targets.""" + if self.sink is None: + raise AssertionError("Observer sink must be assigned before use.") + self.cancelled_pending_counts.append(self.sink.pending_count) + self.cancelled_targets.append(targets) + if self.fail_cancel: + raise RuntimeError("observer rejected cancellation") + + def discarded(self) -> None: + """Record one fail-closed observer reset.""" + self.discard_count += 1 + + +def _dummy_frame() -> RuntimeCommandFrame: + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target=_DummyTarget("base"), + payload=_DummyPayload(torch.tensor([4.0, 5.0])), + ), + ), + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([7, 3], dtype=torch.long), + hold_duration=torch.full((BATCH_SIZE,), STEP_DT), + ) + + +@dataclass(frozen=True, slots=True) +class _FakeCompiledCall: + call_index: int + call: object + + +@dataclass(frozen=True, slots=True) +class _FakeSegment: + segment_index: int = 0 + segment_id: str = "segment-0" + name: str = "pick-and-place" + calls: tuple[_FakeCompiledCall, ...] = ( + _FakeCompiledCall(0, "pick"), + _FakeCompiledCall(1, "place"), + ) + source_path: tuple[object, ...] = ("program", "steps", 0) + post_policies: tuple[object, ...] = () + validators: tuple[object, ...] = () + parallel_block: object | None = None + implicit: bool = False + + +@dataclass(frozen=True, slots=True) +class _FakeParallelBranch: + branch_index: int + calls: tuple[_FakeCompiledCall, ...] + + +@dataclass(frozen=True, slots=True) +class _FakeBarrier: + timeout_steps: int = 17 + failure_policy: str = "fail_fast" + + +@dataclass(frozen=True, slots=True) +class _FakeParallelBlock: + branches: tuple[_FakeParallelBranch, ...] + barrier: _FakeBarrier = _FakeBarrier() + + +@dataclass(frozen=True, slots=True) +class _FakeProgramAnalysis: + calls: tuple[object, ...] + execution_prefix_length: int + + +class _FakeProgram: + schema_version = 1 + program_id = "demo-program" + + def __init__(self, *segments: _FakeSegment) -> None: + self.segments = segments + + def iter_segments(self): + yield from self.segments + + def sequential_execution_analysis( + self, + segment_index: int, + ) -> _FakeProgramAnalysis: + current = self.segments[segment_index] + if current.parallel_block is not None: + raise ValueError("Parallel segments have no sequential analysis.") + calls: list[object] = [] + for segment in self.segments[segment_index:]: + if segment.parallel_block is not None: + break + calls.extend(compiled.call for compiled in segment.calls) + return _FakeProgramAnalysis(tuple(calls), len(current.calls)) + + +def _skill_result( + status: SkillStatus, + *, + wait_duration: float = 0.0, + workflow_id: str = "demo-program/segment-0", +) -> SkillResult: + env_ids = torch.tensor([7, 3], dtype=torch.long) + eligible = torch.ones(BATCH_SIZE, dtype=torch.bool) + success = ( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.COMPLETED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ) + failure = ( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.FAILED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ) + if status is SkillStatus.FAILED: + eligible = torch.zeros_like(eligible) + return SkillResult( + status=status, + workflow_id=workflow_id, + current_call_index=0 if status is SkillStatus.RUNNING else None, + env_ids=env_ids, + success_mask=success, + failure_mask=failure, + cancelled_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + eligible_mask=eligible, + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + wait_duration=wait_duration, + ) + + +class _FakeRuntime: + """Clock-aware nonblocking runtime used to test the Gym boundary only.""" + + def __init__( + self, + sink: BufferedGymCommandSink, + clock: EnvironmentStepClock, + frame: RuntimeCommandFrame, + ) -> None: + self.sink = sink + self.clock = clock + self.frame = frame + self._status = SkillStatus.IDLE + self._result = _skill_result(SkillStatus.IDLE) + self._due_at = 0.0 + self._sent = False + self.start_count = 0 + self.step_count = 0 + self.cancel_count = 0 + self.calls: tuple[object, ...] = () + self.execution_prefix_lengths: list[int | None] = [] + self.eligible_masks: list[torch.Tensor | None] = [] + self.adopted_states: list[TaskState] = [] + + @property + def result(self) -> SkillResult: + return self._result + + @property + def status(self) -> SkillStatus: + return self._status + + def start( + self, + *calls: object, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + self.start_count += 1 + self.calls = tuple(calls[0]) if len(calls) == 1 else calls + self.execution_prefix_lengths.append(execution_prefix_length) + self.eligible_masks.append( + None if eligible_mask is None else eligible_mask.clone() + ) + self._sent = False + self._due_at = 0.0 + self._status = SkillStatus.RUNNING + self._result = _skill_result( + SkillStatus.RUNNING, + workflow_id=workflow_id, + ) + return self._result + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + self.adopted_states.append(task_state) + return self._result + + def step(self) -> SkillResult: + self.step_count += 1 + if not self._sent: + self.sink.send(self.frame, timeout=1.0) + self._sent = True + self._due_at = self.clock.now() + float( + self.frame.hold_duration.max().item() + ) + remaining = max(self._due_at - self.clock.now(), 0.0) + if remaining > 1.0e-9: + self._result = _skill_result( + SkillStatus.RUNNING, + wait_duration=remaining, + workflow_id=self._result.workflow_id or "semantic_workflow", + ) + return self._result + self._status = SkillStatus.COMPLETED + self._result = _skill_result( + SkillStatus.COMPLETED, + workflow_id=self._result.workflow_id or "semantic_workflow", + ) + return self._result + + def cancel(self, reason: str) -> SkillResult: + del reason + self.cancel_count += 1 + self.sink.cancel(self.frame.targets, timeout=1.0) + self.sink.hold(self.frame.targets, _context(), timeout=1.0) + self._status = SkillStatus.CANCELLED + self._result = SkillResult( + status=SkillStatus.CANCELLED, + workflow_id=self._result.workflow_id, + current_call_index=None, + env_ids=torch.tensor([7, 3], dtype=torch.long), + success_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + failure_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + cancelled_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + eligible_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + ) + return self._result + + +class _StartFailingRuntime(_FakeRuntime): + """Fail semantic preflight before accepting any controller command.""" + + def start( + self, + *calls: object, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + del calls, workflow_id, eligible_mask, execution_prefix_length + self.start_count += 1 + raise RuntimeError("semantic runtime preflight failed") + + +class _TerminalHoldRuntime(_FakeRuntime): + """Emit a terminal safe hold after one consumed command.""" + + def step(self) -> SkillResult: + self.step_count += 1 + if not self._sent: + self.sink.send(self.frame, timeout=1.0) + self._sent = True + self._status = SkillStatus.RUNNING + self._result = _skill_result( + SkillStatus.RUNNING, + workflow_id=self._result.workflow_id or "semantic_workflow", + ) + return self._result + self.sink.hold(self.frame.targets, _context(), timeout=1.0) + self._status = SkillStatus.COMPLETED + self._result = _skill_result( + SkillStatus.COMPLETED, + workflow_id=self._result.workflow_id or "semantic_workflow", + ) + return self._result + + +class _TerminalFailedRuntime(_FakeRuntime): + """Fail terminally during planning without accepting a command.""" + + def start( + self, + *calls: object, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + running = super().start( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + ) + failed_mask = torch.ones(BATCH_SIZE, dtype=torch.bool) + self._status = SkillStatus.FAILED + self._result = SkillResult( + status=SkillStatus.FAILED, + workflow_id=running.workflow_id, + current_call_index=None, + env_ids=running.env_ids, + success_mask=torch.zeros_like(failed_mask), + failure_mask=failed_mask, + cancelled_mask=torch.zeros_like(failed_mask), + eligible_mask=torch.zeros_like(failed_mask), + task_state=running.task_state, + events=( + ExecutionEvent( + kind=ExecutionEventKind.ACTION_PLANNING_FAILED, + timestamp=self.clock.now(), + skill_id="operate_articulation", + invocation_id="open-drawer-call", + invocation_revision=0, + invocation_index=0, + env_mask=failed_mask, + message="Articulation motion phase 'operate' failed.", + ), + ), + message="Motion planning failed before the first command.", + ) + return self._result + + +class _PartialSuccessRuntime(_FakeRuntime): + """Complete the workflow while retaining one failed environment row.""" + + def step(self) -> SkillResult: + result = super().step() + if result.status is SkillStatus.COMPLETED: + active_mask = torch.tensor([True, False]) + self._result = SkillResult( + status=SkillStatus.COMPLETED, + workflow_id=result.workflow_id, + current_call_index=None, + env_ids=result.env_ids, + success_mask=active_mask, + failure_mask=~active_mask, + cancelled_mask=torch.zeros_like(active_mask), + eligible_mask=active_mask, + task_state=result.task_state, + ) + return self._result + + +def _parallel_result( + status: SkillStatus, + *, + wait_duration: float = 0.0, +) -> ParallelSkillResult: + env_ids = torch.tensor([7, 3], dtype=torch.long) + terminal = status in { + SkillStatus.COMPLETED, + SkillStatus.FAILED, + SkillStatus.CANCELLED, + } + return ParallelSkillResult( + status=status, + env_ids=env_ids, + success_mask=( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.COMPLETED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ), + failure_mask=( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.FAILED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ), + cancelled_mask=( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.CANCELLED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ), + pending_mask=( + torch.zeros(BATCH_SIZE, dtype=torch.bool) + if terminal + else torch.ones(BATCH_SIZE, dtype=torch.bool) + ), + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + branch_results={}, + elapsed_steps=0, + command_count=0, + wait_duration=wait_duration, + ) + + +class _FakeParallelRuntime: + """One-grid-frame parallel coordinator used at the bridge boundary.""" + + def __init__( + self, + sink: BufferedGymCommandSink, + clock: EnvironmentStepClock, + ) -> None: + self.sink = sink + self.clock = clock + self._result = _parallel_result(SkillStatus.IDLE) + self._sent = False + self._due_at = 0.0 + self.eligible_mask: torch.Tensor | None = None + + @property + def result(self) -> ParallelSkillResult: + return self._result + + def start( + self, + *, + workflow_id: str = "parallel_workflow", + eligible_mask: torch.Tensor | None = None, + ) -> ParallelSkillResult: + del workflow_id + self.eligible_mask = None if eligible_mask is None else eligible_mask.clone() + self._result = _parallel_result(SkillStatus.RUNNING) + return self._result + + def step(self) -> ParallelSkillResult: + if not self._sent: + self.sink.send(_joint_frame(duration=STEP_DT), timeout=1.0) + self._sent = True + self._due_at = self.clock.now() + STEP_DT + remaining = max(self._due_at - self.clock.now(), 0.0) + self._result = ( + _parallel_result(SkillStatus.RUNNING, wait_duration=remaining) + if remaining > 1.0e-9 + else _parallel_result(SkillStatus.COMPLETED) + ) + return self._result + + def cancel(self, reason: str) -> ParallelSkillResult: + del reason + self._result = _parallel_result(SkillStatus.CANCELLED) + return self._result + + +class _GridLaneRuntime: + """Small branch runtime used with the real parallel coordinator and sink.""" + + def __init__( + self, + sink: ParallelLaneCommandSink, + script: tuple[tuple[SkillStatus, RuntimeCommandFrame | None], ...], + ) -> None: + self.sink = sink + self.script = script + self.step_count = 0 + self._result = _skill_result(SkillStatus.IDLE) + + @property + def result(self) -> SkillResult: + return self._result + + def start( + self, + *calls: object, + workflow_id: str, + eligible_mask: torch.Tensor | None = None, + ) -> SkillResult: + del calls, eligible_mask + self._result = _skill_result(SkillStatus.RUNNING, workflow_id=workflow_id) + return self._result + + def step(self) -> SkillResult: + status, frame = self.script[min(self.step_count, len(self.script) - 1)] + self.step_count += 1 + if frame is not None: + self.sink.send(frame, timeout=1.0) + if status is not SkillStatus.RUNNING: + last_frame = frame or self.sink.last_frame + assert last_frame is not None + self.sink.hold(last_frame.targets, _context(), timeout=1.0) + self._result = _skill_result( + status, workflow_id=self._result.workflow_id or "lane" + ) + return self._result + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + del env_mask, reason + return self._result + + def cancel(self, reason: str) -> SkillResult: + del reason + last_frame = self.sink.last_frame + if last_frame is not None: + self.sink.cancel(last_frame.targets, timeout=1.0) + self.sink.hold(last_frame.targets, _context(), timeout=1.0) + self._result = SkillResult( + status=SkillStatus.CANCELLED, + workflow_id=self._result.workflow_id, + current_call_index=None, + env_ids=torch.tensor([7, 3], dtype=torch.long), + success_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + failure_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + cancelled_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + eligible_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + ) + return self._result + + +def _grid_frame( + control_part: str, + joint_id: int, + value: float, +) -> RuntimeCommandFrame: + return RuntimeCommandFrame( + commands=( + EndpointCommand( + JointPositionTarget(control_part, (joint_id,)), + JointPositionPayload( + torch.full((BATCH_SIZE, 1), value, dtype=torch.float32) + ), + ), + ), + active_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + env_ids=torch.tensor([7, 3], dtype=torch.long), + hold_duration=torch.full((BATCH_SIZE,), STEP_DT), + ) + + +class _PostPolicyPort: + def __init__(self, action: torch.Tensor) -> None: + self.action = action + self.seen: list[object] = [] + self.active_masks: list[torch.Tensor] = [] + + def validate_policy(self, policy: object, *, segment: object) -> None: + del policy, segment + + def actions( + self, + policy: object, + *, + segment: object, + active_mask: torch.Tensor, + ): + del segment + self.seen.append(policy) + self.active_masks.append(active_mask.clone()) + yield self.action + + +class _ValidatorPort: + def __init__(self, result: torch.Tensor) -> None: + self.result = result + self.seen: list[object] = [] + + def validate_validator(self, validator: object, *, segment: object) -> None: + del validator, segment + + def validate(self, validator: object, *, segment: object) -> torch.Tensor: + del segment + self.seen.append(validator) + return self.result + + +class _MetadataPostPolicyPort(_PostPolicyPort): + """Post-policy test port exposing a deterministic result trace.""" + + def post_policy_metadata( + self, + policy: object, + *, + segment: object, + ) -> dict[str, object]: + del policy, segment + return { + "status": "timed_out", + "state": { + "elapsed_steps": 1, + "settled_mask": [True, False], + "timeout_mask": [False, True], + }, + } + + def post_policy_result( + self, + policy: object, + *, + segment: object, + ) -> torch.Tensor: + del policy, segment + return torch.tensor([True, False]) + + +class _MetadataValidatorPort(_ValidatorPort): + """Validator test port exposing observed error metadata.""" + + def validator_metadata( + self, + validator: object, + *, + segment: object, + ) -> dict[str, object]: + del validator, segment + return {"position_error": [0.01, 0.10]} + + +class _FailingPostPolicyPort: + """Raise from lazy policy iteration after the runtime reached a safe hold.""" + + def validate_policy(self, policy: object, *, segment: object) -> None: + del policy, segment + + def actions( + self, + policy: object, + *, + segment: object, + active_mask: torch.Tensor, + ): + del policy, segment, active_mask + if False: + yield torch.empty(0) + raise RuntimeError("post-policy observation failed") + + +class _AcceptParallelSafety: + """Test-only authoritative gate that accepts the supplied merged frame.""" + + def validate( + self, + *, + branch_frames: dict[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + assert branch_frames + assert isinstance(merged_frame, RuntimeCommandFrame) + + +def _bridge( + *, + duration: float, + segment: _FakeSegment | None = None, + post_policy_port: object | None = None, + validator_port: object | None = None, + parallel_safety_validator: object | None = None, +) -> tuple[AtomicDemoBridge, _FakeRuntime, EnvironmentStepClock]: + clock = EnvironmentStepClock(STEP_DT) + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)) + ) + sink = BufferedGymCommandSink(encoder, clock) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=duration)) + bridge = AtomicDemoBridge( + _FakeProgram(_FakeSegment() if segment is None else segment), + runtime, + sink, + clock, + post_policy_port=post_policy_port, + validator_port=validator_port, + parallel_safety_validator=parallel_safety_validator, + ) + return bridge, runtime, clock + + +def test_environment_step_clock_advances_only_explicitly() -> None: + clock = EnvironmentStepClock(STEP_DT) + + assert clock.now() == 0.0 + assert clock.steps_for_duration(3 * STEP_DT) == 3 + with pytest.raises(EnvironmentStepTimingError, match="not an integer multiple"): + clock.steps_for_duration(0.03) + with pytest.raises(RuntimeError, match="cannot sleep"): + clock.sleep(STEP_DT) + assert clock.now() == 0.0 + + clock.advance_after_env_step() + assert clock.step_index == 1 + assert clock.now() == pytest.approx(STEP_DT) + + +def test_observation_provider_reorders_qpos_by_stable_env_id() -> None: + context = _context( + qpos=torch.tensor([[7.0, 7.1, 7.2, 7.3, 7.4], [3.0, 3.1, 3.2, 3.3, 3.4]]) + ) + provider = GymPlanningObservationProvider(lambda task_state: context) + + observed = provider.observe(context.task) + reordered = provider.current_qpos(torch.tensor([3, 7], dtype=torch.long)) + + assert observed is context + assert torch.equal(reordered[0], context.robot.qpos[1]) + assert torch.equal(reordered[1], context.robot.qpos[0]) + + +def test_joint_encoder_emits_full_qpos_and_holds_inactive_rows() -> None: + qpos = torch.arange(BATCH_SIZE * ROBOT_DOF, dtype=torch.float32).reshape( + BATCH_SIZE, ROBOT_DOF + ) + encoder = RuntimeCommandFrameEncoder(_QposProvider(qpos)) + frame = _joint_frame( + duration=STEP_DT, + active_mask=torch.tensor([True, False]), + ) + + action = encoder.encode(frame) + + assert isinstance(action, torch.Tensor) + assert action.shape == qpos.shape + assert torch.equal(action[0, torch.tensor([1, 3])], torch.tensor([10.0, 30.0])) + assert torch.equal(action[0, torch.tensor([0, 2, 4])], qpos[0, [0, 2, 4]]) + assert torch.equal(action[1], qpos[1]) + + +def test_frame_encoder_supports_registered_future_transport() -> None: + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)) + ) + frame = _dummy_frame() + with pytest.raises(UnsupportedRuntimeTransportError, match="test.transport"): + encoder.encode(frame) + + encoder.register_transport(_DummyTransportEncoder()) + action = encoder.encode(frame) + + assert isinstance(action, torch.Tensor) + assert action[0, 0].item() == 4.0 + assert action[1, 0].item() == 0.0 + + +def test_buffered_sink_rejects_off_grid_frame_before_buffering() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + + with pytest.raises(EnvironmentStepTimingError, match="hold_duration"): + sink.send(_joint_frame(duration=0.03), timeout=1.0) + + assert sink.pending_count == 0 + assert clock.step_index == 0 + + +def test_buffered_sink_buffers_command_hold_and_cancel_without_stepping() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + frame = _joint_frame(duration=STEP_DT) + + acknowledgement = sink.send(frame, timeout=1.0) + assert acknowledgement.accepted + assert sink.pending_count == 1 + action = sink.pop() + assert isinstance(action, ProcessedEnvAction) + assert action.metadata["bridge_action_kind"] == "runtime_command" + assert clock.step_index == 0 + + sink.hold(frame.targets, _context(), timeout=1.0) + assert sink.pending_count == 1 + sink.cancel(frame.targets, timeout=1.0) + assert sink.pending_count == 0 + assert clock.step_index == 0 + + +def test_buffered_sink_notifies_observer_only_after_successful_buffering() -> None: + """Observer acceptance follows encoding and owns a command snapshot.""" + clock = EnvironmentStepClock(STEP_DT) + observer = _RecordingAcceptedCommandObserver() + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + accepted_command_observer=observer, + ) + observer.sink = sink + frame = _joint_frame(duration=STEP_DT) + + sink.send(frame, timeout=1.0) + + assert observer.accepted_pending_counts == [1] + assert len(observer.accepted_frames) == 1 + observed = observer.accepted_frames[0] + assert observed is not frame + assert torch.equal(observed.env_ids, frame.env_ids) + assert observed.env_ids.data_ptr() != frame.env_ids.data_ptr() + + +def test_buffered_sink_does_not_notify_observer_when_encoding_fails() -> None: + """A frame that never reaches the buffer cannot establish evidence.""" + clock = EnvironmentStepClock(STEP_DT) + observer = _RecordingAcceptedCommandObserver() + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + accepted_command_observer=observer, + ) + observer.sink = sink + + with pytest.raises(UnsupportedRuntimeTransportError, match="test.transport"): + sink.send(_dummy_frame(), timeout=1.0) + + assert sink.pending_count == 0 + assert observer.accepted_frames == [] + assert observer.discard_count == 0 + + +def test_buffered_sink_rolls_back_buffer_when_observer_rejects_acceptance() -> None: + """Observer failure atomically clears the pending action and evidence state.""" + clock = EnvironmentStepClock(STEP_DT) + observer = _RecordingAcceptedCommandObserver(fail_accept=True) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + accepted_command_observer=observer, + ) + observer.sink = sink + + with pytest.raises(RuntimeError, match="observer rejected accepted command"): + sink.send(_joint_frame(duration=STEP_DT), timeout=1.0) + + assert observer.accepted_pending_counts == [1] + assert sink.pending_count == 0 + assert observer.discard_count == 1 + + +def test_buffered_sink_notifies_observer_on_cancel_and_explicit_discard() -> None: + """Cancel is target-scoped while a local discard resets all evidence.""" + clock = EnvironmentStepClock(STEP_DT) + observer = _RecordingAcceptedCommandObserver() + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + accepted_command_observer=observer, + ) + observer.sink = sink + frame = _joint_frame(duration=STEP_DT) + sink.send(frame, timeout=1.0) + + sink.cancel(frame.targets, timeout=1.0) + + assert sink.pending_count == 0 + assert observer.cancelled_pending_counts == [0] + assert len(observer.cancelled_targets) == 1 + assert observer.cancelled_targets[0][0].address_fingerprint == ( + frame.targets[0].address_fingerprint + ) + assert observer.discard_count == 0 + + sink.send(frame, timeout=1.0) + sink.discard_pending() + assert sink.pending_count == 0 + assert observer.discard_count == 1 + + +def test_atomic_demo_bridge_is_lazy_and_waits_with_hold_actions() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + + demo_segment = next(bridge.iter_segments()) + assert runtime.start_count == 0 + with pytest.raises(RuntimeError, match="before its action iterable"): + demo_segment.validator() + + actions = iter(demo_segment.actions) + command = next(actions) + assert runtime.start_count == 1 + assert runtime.calls == ("pick", "place") + assert clock.step_index == 0 + assert command.metadata["bridge_action_kind"] == "runtime_command" + assert command.metadata["environment_step"] == 0 + + wait_hold = next(actions) + assert clock.step_index == 1 + assert wait_hold.metadata["bridge_action_kind"] == "runtime_wait_hold" + assert torch.equal(wait_hold.value, command.value) + + with pytest.raises(StopIteration): + next(actions) + assert clock.step_index == 2 + assert runtime.status is SkillStatus.COMPLETED + assert runtime.cancel_count == 0 + assert demo_segment.validator().tolist() == [True, True] + + +def test_closing_without_abort_handshake_fails_loudly_and_does_not_ack() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + actions = iter(next(bridge.iter_segments()).actions) + + next(actions) + with pytest.raises(DemoBridgeError, match="abort_actions"): + actions.close() + + assert clock.step_index == 0 + assert runtime.cancel_count == 1 + + +def test_abort_handshake_discards_unconsumed_command_and_yields_safe_hold() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + segment = next(bridge.iter_segments()) + actions = iter(segment.actions) + + command = next(actions) + assert command.metadata["bridge_action_kind"] == "runtime_command" + assert segment.abort_actions is not None + emergency = iter(segment.abort_actions("operator stop", last_action_consumed=False)) + hold = next(emergency) + + assert hold.metadata["bridge_action_kind"] == "runtime_abort_safe_hold" + assert clock.step_index == 0 + with pytest.raises(StopIteration): + next(emergency) + assert clock.step_index == 1 + assert runtime.cancel_count == 1 + assert runtime.sink.pending_count == 0 + assert segment.metadata["runtime"]["status"] == "cancelled" + assert segment.metadata["runtime"]["masks"]["cancelled"] == [True, True] + with pytest.raises(RuntimeError, match="already started"): + next( + iter( + segment.abort_actions( + "duplicate stop", + last_action_consumed=False, + ) + ) + ) + actions.close() + + +def test_abort_handshake_acknowledges_consumed_command_exactly_once() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + segment = next(bridge.iter_segments()) + actions = iter(segment.actions) + + next(actions) + assert segment.abort_actions is not None + emergency = iter( + segment.abort_actions("environment failure", last_action_consumed=True) + ) + hold = next(emergency) + + assert clock.step_index == 1 + assert hold.metadata["bridge_action_kind"] == "runtime_abort_safe_hold" + with pytest.raises(StopIteration): + next(emergency) + assert clock.step_index == 2 + assert runtime.cancel_count == 1 + actions.close() + + +def test_abort_replays_unconsumed_terminal_safe_hold_without_recancelling() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _TerminalHoldRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + bridge = AtomicDemoBridge(_FakeProgram(_FakeSegment()), runtime, sink, clock) + segment = next(bridge.iter_segments()) + actions = iter(segment.actions) + + command = next(actions) + assert command.metadata["bridge_action_kind"] == "runtime_command" + terminal_hold = next(actions) + assert terminal_hold.metadata["bridge_action_kind"] == "runtime_safe_hold" + assert runtime.status is SkillStatus.COMPLETED + assert clock.step_index == 1 + + assert segment.abort_actions is not None + emergency = iter( + segment.abort_actions("stop before hold", last_action_consumed=False) + ) + replay = next(emergency) + assert replay.metadata["bridge_action_kind"] == "runtime_abort_safe_hold" + assert torch.equal(replay.value, terminal_hold.value) + with pytest.raises(StopIteration): + next(emergency) + + assert clock.step_index == 2 + assert runtime.cancel_count == 0 + assert sink.pending_count == 0 + actions.close() + + +def test_post_policy_interruption_replays_last_runtime_safe_hold() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _TerminalHoldRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + post_policy = object() + segment_spec = _FakeSegment(post_policies=(post_policy,)) + bridge = AtomicDemoBridge( + _FakeProgram(segment_spec), + runtime, + sink, + clock, + post_policy_port=_PostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)), + ) + segment = next(bridge.iter_segments()) + actions = iter(segment.actions) + + assert next(actions).metadata["bridge_action_kind"] == "runtime_command" + assert next(actions).metadata["bridge_action_kind"] == "runtime_safe_hold" + post_action = next(actions) + assert post_action.metadata["bridge_action_kind"] == "program_post_policy" + assert clock.step_index == 2 + + assert segment.abort_actions is not None + emergency = iter( + segment.abort_actions("post policy stop", last_action_consumed=False) + ) + replay = next(emergency) + assert replay.metadata["bridge_action_kind"] == "runtime_abort_safe_hold" + assert torch.equal(replay.value, torch.zeros(BATCH_SIZE, ROBOT_DOF)) + with pytest.raises(StopIteration): + next(emergency) + + assert clock.step_index == 3 + assert runtime.cancel_count == 0 + actions.close() + + +class _BridgeExecutorEnv: + """Minimal demo executor proving abort actions cross the Gym boundary.""" + + def __init__( + self, + bridge: AtomicDemoBridge, + *, + fail_first_mask: torch.Tensor | None = None, + raise_first_step: bool = False, + ) -> None: + self.bridge = bridge + self.fail_first_mask = ( + torch.zeros(BATCH_SIZE, dtype=torch.bool) + if fail_first_mask is None + else fail_first_mask.clone() + ) + self.raise_first_step = raise_first_step + self.num_envs = BATCH_SIZE + self.steps: list[ProcessedEnvAction] = [] + self._demo_no_auto_reset = False + + @property + def unwrapped(self) -> _BridgeExecutorEnv: + return self + + def create_demo_segments(self): + return self.bridge.iter_segments() + + def step(self, action: ProcessedEnvAction): + assert isinstance(action, ProcessedEnvAction) + self.steps.append(action.snapshot()) + if self.raise_first_step and len(self.steps) == 1: + raise RuntimeError("simulated environment failure") + failed = ( + self.fail_first_mask + if len(self.steps) == 1 + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ) + return ( + None, + torch.zeros(BATCH_SIZE), + torch.zeros(BATCH_SIZE, dtype=torch.bool), + torch.zeros(BATCH_SIZE, dtype=torch.bool), + {"fail": failed}, + ) + + def _mask_demo_action( + self, + action: ProcessedEnvAction, + active_mask: tuple[bool, ...], + ) -> ProcessedEnvAction: + del active_mask + return action.snapshot() + + +def test_zero_command_terminal_runtime_failure_preserves_trace_and_validates_once() -> ( + None +): + validator = object() + first = _FakeSegment( + calls=(_FakeCompiledCall(0, "operate_articulation"),), + validators=(validator,), + ) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="must-not-start", + calls=(_FakeCompiledCall(1, "place"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _TerminalFailedRuntime( + sink, + clock, + _joint_frame(duration=STEP_DT), + ) + validator_port = _ValidatorPort(torch.ones(BATCH_SIZE, dtype=torch.bool)) + bridge = AtomicDemoBridge( + _FakeProgram(first, second), + runtime, + sink, + clock, + validator_port=validator_port, + ) + env = _BridgeExecutorEnv(bridge) + + result = execute_demo_episode(env) + + assert env.steps == [] + assert clock.step_index == 0 + assert runtime.start_count == 1 + assert runtime.step_count == 0 + assert validator_port.seen == [validator] + assert not result.completed + assert result.terminal_reason == "segment_validation_failed" + assert len(result.segments) == 1 + segment_result = result.segments[0] + assert segment_result.failure_reason == "segment_validation_failed" + runtime_trace = segment_result.metadata["runtime"] + assert runtime_trace["status"] == "failed" + assert ( + runtime_trace["message"] == "Motion planning failed before the first command." + ) + assert runtime_trace["events"] == [ + { + "kind": "action_planning_failed", + "timestamp": 0.0, + "skill_id": "operate_articulation", + "invocation_id": "open-drawer-call", + "invocation_revision": 0, + "invocation_index": 0, + "env_mask": [True, True], + "message": "Articulation motion phase 'operate' failed.", + } + ] + assert segment_result.metadata["validation"] == { + "env_ids": [7, 3], + "runtime_success_mask": [False, False], + "eligible_mask_before_validation": [False, False], + "post_policy_success_mask": None, + "validators": [ + { + "validator_index": 0, + "kind": "object", + "source_path": [], + "result_mask": [True, True], + "result": None, + } + ], + "accepted_mask": [False, False], + } + + +def test_sequential_start_failure_before_first_command_preserves_cause() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _StartFailingRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + bridge = AtomicDemoBridge(_FakeProgram(_FakeSegment()), runtime, sink, clock) + env = _BridgeExecutorEnv(bridge) + + with pytest.raises(RuntimeError, match="action generation") as error: + execute_demo_episode(env) + + assert isinstance(error.value.__cause__, RuntimeError) + assert str(error.value.__cause__) == "semantic runtime preflight failed" + assert env.steps == [] + assert clock.step_index == 0 + assert runtime.start_count == 1 + assert runtime.cancel_count == 0 + assert sink.pending_count == 0 + + +def test_parallel_construction_failure_before_first_command_preserves_cause( + monkeypatch: pytest.MonkeyPatch, +) -> None: + block = _FakeParallelBlock( + branches=( + _FakeParallelBranch(0, (_FakeCompiledCall(0, "left"),)), + _FakeParallelBranch(1, (_FakeCompiledCall(1, "right"),)), + ) + ) + segment = _FakeSegment(parallel_block=block) + bridge, runtime, clock = _bridge( + duration=STEP_DT, + segment=segment, + parallel_safety_validator=_AcceptParallelSafety(), + ) + env = _BridgeExecutorEnv(bridge) + + def fail_construction(*args: object, **kwargs: object) -> _FakeParallelRuntime: + del args, kwargs + raise RuntimeError("parallel runtime construction failed") + + monkeypatch.setattr(bridge_module, "SkillRuntime", _FakeRuntime) + monkeypatch.setattr( + ParallelSkillRuntime, + "from_template", + classmethod(fail_construction), + ) + + with pytest.raises(RuntimeError, match="action generation") as error: + execute_demo_episode(env) + + assert isinstance(error.value.__cause__, RuntimeError) + assert str(error.value.__cause__) == "parallel runtime construction failed" + assert env.steps == [] + assert clock.step_index == 0 + assert runtime.start_count == 0 + assert runtime.cancel_count == 0 + assert runtime.sink.pending_count == 0 + + +def test_post_policy_timeout_is_row_local_and_preserved_in_segment_result() -> None: + segment = _FakeSegment(post_policies=(object(),)) + bridge, runtime, clock = _bridge( + duration=STEP_DT, + segment=segment, + post_policy_port=_MetadataPostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)), + ) + env = _BridgeExecutorEnv(bridge) + + result = execute_demo_episode(env) + + assert result.segments[0].successes == (True, False) + assert result.segments[0].failure_reasons == ( + None, + "segment_validation_failed", + ) + assert result.segments[0].metadata["post_policies"][0]["result_mask"] == [ + True, + False, + ] + assert result.segments[0].metadata["validation"]["accepted_mask"] == [ + True, + False, + ] + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "program_post_policy", + ] + assert runtime.cancel_count == 0 + assert clock.step_index == 2 + + +def test_post_policy_generator_error_replays_safe_hold_before_propagating() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _TerminalHoldRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + segment_spec = _FakeSegment(post_policies=(object(),)) + bridge = AtomicDemoBridge( + _FakeProgram(segment_spec), + runtime, + sink, + clock, + post_policy_port=_FailingPostPolicyPort(), + ) + env = _BridgeExecutorEnv(bridge) + + with pytest.raises(RuntimeError, match="action generation") as error: + execute_demo_episode(env) + + assert isinstance(error.value.__cause__, RuntimeError) + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "runtime_safe_hold", + "runtime_abort_safe_hold", + ] + assert runtime.cancel_count == 0 + assert sink.pending_count == 0 + assert clock.step_index == 3 + + +def test_demo_executor_pre_step_stop_consumes_only_abort_hold() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv(bridge) + checks = iter((False, True)) + + result = execute_demo_episode(env, should_stop=lambda: next(checks, True)) + + assert result.terminal_reason == "interrupted" + assert result.length == 1 + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_abort_safe_hold" + ] + assert clock.step_index == 1 + assert runtime.cancel_count == 1 + assert runtime.sink.pending_count == 0 + + +def test_demo_executor_post_step_failure_acknowledges_then_safe_stops() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv( + bridge, + fail_first_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + ) + + result = execute_demo_episode(env) + + assert result.terminal_reason == "failure" + assert result.length == 2 + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "runtime_abort_safe_hold", + ] + assert clock.step_index == 2 + assert runtime.cancel_count == 1 + assert runtime.sink.pending_count == 0 + + +def test_demo_executor_safe_stops_when_regular_env_step_raises() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv(bridge, raise_first_step=True) + + with pytest.raises(RuntimeError, match="emergency safe-stop"): + execute_demo_episode(env) + + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "runtime_abort_safe_hold", + ] + assert clock.step_index == 1 + assert runtime.cancel_count == 1 + assert runtime.sink.pending_count == 0 + + +def test_row_independent_partial_failure_does_not_abort_healthy_peer() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv( + bridge, + fail_first_mask=torch.tensor([True, False]), + ) + + result = execute_demo_episode(env) + + assert result.lengths == (1, 2) + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "runtime_wait_hold", + ] + assert clock.step_index == 2 + assert runtime.cancel_count == 0 + assert runtime.sink.pending_count == 0 + + +def test_post_policy_and_validator_ports_stay_at_demo_boundary() -> None: + post_policy = object() + validator = object() + segment = _FakeSegment( + post_policies=(post_policy,), + validators=(validator,), + ) + post_port = _PostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)) + validator_port = _ValidatorPort(torch.tensor([True, False])) + bridge, _, clock = _bridge( + duration=STEP_DT, + segment=segment, + post_policy_port=post_port, + validator_port=validator_port, + ) + demo_segment = next(bridge.iter_segments()) + actions = iter(demo_segment.actions) + + runtime_action = next(actions) + assert runtime_action.metadata["bridge_action_kind"] == "runtime_command" + post_action = next(actions) + assert clock.step_index == 1 + assert post_action.metadata["bridge_action_kind"] == "program_post_policy" + with pytest.raises(StopIteration): + next(actions) + + assert clock.step_index == 2 + assert post_port.seen == [post_policy] + assert len(post_port.active_masks) == 1 + assert post_port.active_masks[0].tolist() == [True, True] + assert demo_segment.validator().tolist() == [True, False] + assert validator_port.seen == [validator] + + +def test_post_policy_receives_only_rows_surviving_partial_runtime_failure() -> None: + """Post-policy completion cannot be blocked by a runtime-failed row.""" + post_policy = object() + segment = _FakeSegment(post_policies=(post_policy,)) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _PartialSuccessRuntime( + sink, + clock, + _joint_frame(duration=STEP_DT), + ) + post_port = _PostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)) + bridge = AtomicDemoBridge( + _FakeProgram(segment), + runtime, + sink, + clock, + post_policy_port=post_port, + ) + demo_segment = next(bridge.iter_segments()) + + tuple(demo_segment.actions) + + assert len(post_port.active_masks) == 1 + assert post_port.active_masks[0].tolist() == [True, False] + assert demo_segment.metadata["post_policies"][0]["result_mask"] == [True, False] + assert demo_segment.validator().tolist() == [True, False] + + +def test_later_post_policy_receives_only_rows_passing_prior_policy() -> None: + """Sequential post-policies monotonically narrow their active cohort.""" + segment = _FakeSegment(post_policies=(object(), object())) + post_port = _MetadataPostPolicyPort( + torch.ones(BATCH_SIZE, ROBOT_DOF), + ) + bridge, _, _ = _bridge( + duration=STEP_DT, + segment=segment, + post_policy_port=post_port, + ) + + tuple(next(bridge.iter_segments()).actions) + + assert [mask.tolist() for mask in post_port.active_masks] == [ + [True, True], + [True, False], + ] + + +def test_segment_lifecycle_metadata_records_runtime_post_and_validation() -> None: + post_policy = object() + validator = object() + segment = _FakeSegment( + post_policies=(post_policy,), + validators=(validator,), + ) + bridge, _, _ = _bridge( + duration=STEP_DT, + segment=segment, + post_policy_port=_MetadataPostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)), + validator_port=_MetadataValidatorPort(torch.tensor([True, False])), + ) + demo_segment = next(bridge.iter_segments()) + + tuple(demo_segment.actions) + assert demo_segment.metadata["runtime"]["status"] == "completed" + assert demo_segment.metadata["post_policies"] == [ + { + "policy_index": 0, + "kind": "object", + "source_path": [], + "result_mask": [True, False], + "result": { + "status": "timed_out", + "state": { + "elapsed_steps": 1, + "settled_mask": [True, False], + "timeout_mask": [False, True], + }, + }, + } + ] + + assert demo_segment.validator().tolist() == [True, False] + assert demo_segment.metadata["validation"] == { + "env_ids": [7, 3], + "runtime_success_mask": [True, True], + "eligible_mask_before_validation": [True, True], + "post_policy_success_mask": [True, False], + "validators": [ + { + "validator_index": 0, + "kind": "object", + "source_path": [], + "result_mask": [True, False], + "result": {"position_error": [0.01, 0.1]}, + } + ], + "accepted_mask": [True, False], + } + json.dumps(demo_segment.metadata, allow_nan=False, sort_keys=True) + + +def test_declared_post_policy_requires_explicit_port() -> None: + segment = _FakeSegment(post_policies=(object(),)) + bridge, _, _ = _bridge(duration=STEP_DT, segment=segment) + + with pytest.raises(DemoBridgeError, match="no SegmentPostPolicyPort"): + tuple(next(bridge.iter_segments()).actions) + + +def test_bridge_marks_segments_row_independent_and_retains_failed_rows() -> None: + first_validator = object() + first = _FakeSegment(validators=(first_validator,)) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="place-next", + calls=(_FakeCompiledCall(2, "place-next"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + bridge = AtomicDemoBridge( + _FakeProgram(first, second), + runtime, + sink, + clock, + validator_port=_ValidatorPort(torch.tensor([True, False])), + ) + segments = iter(bridge.iter_segments()) + + first_demo = next(segments) + assert first_demo.failure_policy == "row_independent" + tuple(first_demo.actions) + assert first_demo.validator().tolist() == [True, False] + + second_demo = next(segments) + tuple(second_demo.actions) + assert runtime.eligible_masks[0] is None + assert runtime.eligible_masks[1].tolist() == [True, False] + assert second_demo.validator().tolist() == [True, False] + + +def test_bridge_refuses_next_segment_when_validator_was_skipped() -> None: + first = _FakeSegment(calls=(_FakeCompiledCall(0, "pick"),)) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="place", + calls=(_FakeCompiledCall(1, "place"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + segments = iter(AtomicDemoBridge(_FakeProgram(first, second), runtime, sink, clock)) + + first_demo = next(segments) + tuple(first_demo.actions) + + with pytest.raises(DemoBridgeError, match="validator must be called"): + next(segments) + assert runtime.start_count == 1 + + +def test_demo_executor_consumes_validation_before_requesting_next_segment() -> None: + first_validator = object() + first = _FakeSegment( + calls=(_FakeCompiledCall(0, "pick"),), + validators=(first_validator,), + ) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="place", + calls=(_FakeCompiledCall(1, "place"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + validator_port = _ValidatorPort(torch.ones(BATCH_SIZE, dtype=torch.bool)) + bridge = AtomicDemoBridge( + _FakeProgram(first, second), + runtime, + sink, + clock, + validator_port=validator_port, + ) + + result = execute_demo_episode(_BridgeExecutorEnv(bridge)) + + assert result.completed + assert len(result.segments) == 2 + assert [segment.success for segment in result.segments] == [True, True] + assert runtime.start_count == 2 + assert validator_port.seen == [first_validator] + + +def test_sequential_segment_analyzes_downstream_calls_but_executes_own_prefix() -> None: + first = _FakeSegment( + calls=(_FakeCompiledCall(0, "pick"),), + ) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="place", + calls=(_FakeCompiledCall(1, "place"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + bridge = AtomicDemoBridge(_FakeProgram(first, second), runtime, sink, clock) + segments = iter(bridge.iter_segments()) + + first_demo = next(segments) + tuple(first_demo.actions) + assert first_demo.validator().tolist() == [True, True] + + assert runtime.calls == ("pick", "place") + assert runtime.execution_prefix_lengths == [1] + + tuple(next(segments).actions) + assert runtime.calls == ("place",) + assert runtime.execution_prefix_lengths == [1, 1] + + +def test_parallel_segment_preserves_branches_barrier_and_adopts_state( + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_calls = (_FakeCompiledCall(0, "left-pick"),) + right_calls = ( + _FakeCompiledCall(1, "right-pick"), + _FakeCompiledCall(2, "right-place"), + ) + block = _FakeParallelBlock( + branches=( + _FakeParallelBranch(0, left_calls), + _FakeParallelBranch(1, right_calls), + ) + ) + segment = _FakeSegment( + calls=left_calls + right_calls, + parallel_block=block, + ) + safety_validator = _AcceptParallelSafety() + bridge, runtime, clock = _bridge( + duration=STEP_DT, + segment=segment, + parallel_safety_validator=safety_validator, + ) + captured: dict[str, Any] = {} + fake_parallel = _FakeParallelRuntime(runtime.sink, clock) + + def from_template( + cls: type[ParallelSkillRuntime], + template_runtime: object, + branch_calls: dict[str, tuple[object, ...]], + command_sink: object, + timing_policy: object, + supplied_safety_validator: object, + *, + timeout_steps: int, + failure_policy: str, + workflow_id: str, + branch_paths: dict[str, tuple[object, ...]], + ) -> _FakeParallelRuntime: + del cls + captured.update( + { + "template_runtime": template_runtime, + "branch_calls": branch_calls, + "command_sink": command_sink, + "timing_policy": timing_policy, + "safety_validator": supplied_safety_validator, + "timeout_steps": timeout_steps, + "failure_policy": failure_policy, + "workflow_id": workflow_id, + "branch_paths": branch_paths, + } + ) + return fake_parallel + + monkeypatch.setattr(bridge_module, "SkillRuntime", _FakeRuntime) + monkeypatch.setattr( + ParallelSkillRuntime, + "from_template", + classmethod(from_template), + ) + + demo_segment = next(bridge.iter_segments()) + actions = tuple(demo_segment.actions) + + assert len(actions) == 1 + assert captured["template_runtime"] is runtime + assert captured["command_sink"] is runtime.sink + assert captured["branch_calls"] == { + "branch_0": ("left-pick",), + "branch_1": ("right-pick", "right-place"), + } + assert captured["timing_policy"].step_dt == STEP_DT + assert captured["safety_validator"] is safety_validator + assert captured["timeout_steps"] == 17 + assert captured["failure_policy"] == "fail_fast" + assert captured["workflow_id"].endswith(":parallel_analysis") + assert captured["branch_paths"] == { + "branch_0": segment.source_path, + "branch_1": segment.source_path, + } + assert len(runtime.adopted_states) == 1 + assert demo_segment.failure_policy == "row_independent" + assert demo_segment.metadata["runtime"]["kind"] == "parallel_skill_result" + assert demo_segment.metadata["runtime"]["status"] == "completed" + assert demo_segment.metadata["runtime"]["masks"]["success"] == [True, True] + assert demo_segment.validator().tolist() == [True, True] + + +def test_real_parallel_coordinator_buffers_one_ordered_gym_action_per_step() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + left_sink = ParallelLaneCommandSink() + right_sink = ParallelLaneCommandSink() + left_runtime = _GridLaneRuntime( + left_sink, + ( + (SkillStatus.RUNNING, _grid_frame("left_arm", 0, 1.0)), + (SkillStatus.COMPLETED, None), + ), + ) + right_runtime = _GridLaneRuntime( + right_sink, + ( + (SkillStatus.RUNNING, _grid_frame("right_arm", 1, 2.0)), + (SkillStatus.RUNNING, _grid_frame("right_arm", 1, 3.0)), + (SkillStatus.COMPLETED, None), + ), + ) + runtime = ParallelSkillRuntime( + ( + ParallelRuntimeBranch( + "left", + (RegisteredSemanticCall("test.left"),), + ResourceClaim(frozenset({"left_arm"}), (0,)), + left_runtime, + left_sink, + ), + ParallelRuntimeBranch( + "right", + (RegisteredSemanticCall("test.right"),), + ResourceClaim(frozenset({"right_arm"}), (1,)), + right_runtime, + right_sink, + ), + ), + sink, + clock, + ParallelTimingPolicy(STEP_DT), + _AcceptParallelSafety(), + timeout_steps=8, + ) + + runtime.start() + first = runtime.step() + assert first.status is SkillStatus.RUNNING + assert sink.pending_count == 1 + first_action = sink.pop() + assert first_action.metadata["bridge_action_kind"] == "runtime_command" + assert torch.equal(first_action.value[:, 0], torch.ones(BATCH_SIZE)) + assert torch.equal(first_action.value[:, 1], torch.full((BATCH_SIZE,), 2.0)) + clock.advance_after_env_step() + + padded = runtime.step() + assert padded.status is SkillStatus.RUNNING + assert sink.pending_count == 1 + padding_action = sink.pop() + assert padding_action.metadata["bridge_action_kind"] == "runtime_safe_hold" + assert torch.equal(padding_action.value, torch.zeros(BATCH_SIZE, ROBOT_DOF)) + lane_steps = (left_runtime.step_count, right_runtime.step_count) + clock.advance_after_env_step() + + deferred = runtime.step() + assert deferred.status is SkillStatus.RUNNING + assert sink.pending_count == 1 + deferred_action = sink.pop() + assert deferred_action.metadata["bridge_action_kind"] == "runtime_command" + assert torch.equal(deferred_action.value[:, 0], torch.zeros(BATCH_SIZE)) + assert torch.equal( + deferred_action.value[:, 1], + torch.full((BATCH_SIZE,), 3.0), + ) + assert (left_runtime.step_count, right_runtime.step_count) == lane_steps + clock.advance_after_env_step() + + completed = runtime.step() + assert completed.status is SkillStatus.COMPLETED + assert sink.pending_count == 1 + terminal_hold = sink.pop() + assert terminal_hold.metadata["bridge_action_kind"] == "runtime_safe_hold" + assert completed.command_count == 2 + clock.advance_after_env_step() + assert clock.step_index == 4 + + +def test_parallel_segment_fails_closed_without_safety_validator() -> None: + block = _FakeParallelBlock( + branches=( + _FakeParallelBranch(0, (_FakeCompiledCall(0, "left"),)), + _FakeParallelBranch(1, (_FakeCompiledCall(1, "right"),)), + ) + ) + segment = _FakeSegment(parallel_block=block) + bridge, runtime, _ = _bridge(duration=STEP_DT, segment=segment) + + with pytest.raises(DemoBridgeError, match="requires an explicit"): + tuple(next(bridge.iter_segments()).actions) + + assert runtime.start_count == 0 diff --git a/tests/gym/envs/expert_program/test_cfg.py b/tests/gym/envs/expert_program/test_cfg.py new file mode 100644 index 000000000..a504f371c --- /dev/null +++ b/tests/gym/envs/expert_program/test_cfg.py @@ -0,0 +1,206 @@ +# ---------------------------------------------------------------------------- +# 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 typed Expert Program configuration values.""" + +from __future__ import annotations + +import math + +import pytest + +from embodichain.lab.gym.envs.expert_program import ( + MAX_REPEAT_COUNT, + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + PickCfg, + PlaceCfg, + PoseCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + WaitStablePostCfg, +) +from embodichain.utils.configclass import is_configclass + + +def _integration() -> ExpertProgramIntegrationCfg: + """Return one valid provider-free integration selection.""" + return ExpertProgramIntegrationCfg( + robot_profile="auto", + scene_registry="env", + runtime_preset="safe", + ) + + +def _pick_invoke() -> InvokeCfg: + """Return one minimal semantic invocation.""" + return InvokeCfg(call=PickCfg(object="cube")) + + +def test_every_public_schema_value_uses_configclass() -> None: + classes = ( + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + PoseCfg, + TargetRefCfg, + CyclicPoseTargetCfg, + PickCfg, + PlaceCfg, + HandOverCfg, + RegisteredSemanticCallCfg, + WaitStablePostCfg, + ObjectNearTargetValidatorCfg, + InvokeCfg, + SequenceCfg, + RepeatCfg, + SegmentCfg, + ) + + assert all(is_configclass(cls) for cls in classes) + + +def test_call_configs_own_resources_and_registered_payloads() -> None: + resources = {"primary": "left_actor"} + arguments = {"waypoints": [1, {"enabled": True}]} + pick = PickCfg(object="cube", resources=resources) + registered = RegisteredSemanticCallCfg( + call_id="example.inspect", + arguments=arguments, + ) + + resources["primary"] = "right_actor" + arguments["waypoints"][1]["enabled"] = False + + assert pick.resources == {"primary": "left_actor"} + assert registered.arguments == { + "waypoints": (1, {"enabled": True}), + } + + +@pytest.mark.parametrize("count", [False, 0, -1, MAX_REPEAT_COUNT + 1]) +def test_repeat_rejects_non_positive_non_integer_or_excessive_count( + count: object, +) -> None: + with pytest.raises(ValueError, match="count must be an integer"): + RepeatCfg(count=count, body=_pick_invoke()) + + +def test_program_rejects_nested_repeat_expansion_above_static_budget() -> None: + nested = RepeatCfg( + count=MAX_REPEAT_COUNT, + body=RepeatCfg(count=MAX_REPEAT_COUNT, body=_pick_invoke()), + ) + + with pytest.raises(ValueError, match="expands to more than"): + ExpertProgramCfg( + schema_version=1, + program_id="too_large", + integration=_integration(), + targets={}, + program=nested, + ) + + +@pytest.mark.parametrize( + ("kwargs", "message"), + [ + ({}, "exactly one"), + ( + { + "at": TargetRefCfg(target="drop"), + "on": "tray", + }, + "exactly one", + ), + ], +) +def test_place_requires_exactly_one_typed_destination( + kwargs: dict[str, object], + message: str, +) -> None: + with pytest.raises(ValueError, match=message): + PlaceCfg(object="cube", **kwargs) + + +def test_programmatic_config_rejects_unknown_target_reference() -> None: + program = InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="missing"), + ) + ) + + with pytest.raises(ValueError, match="Unknown target reference 'missing'"): + ExpertProgramCfg( + schema_version=1, + program_id="missing_target", + integration=_integration(), + targets={}, + program=program, + ) + + +@pytest.mark.parametrize( + "arguments", + [ + {"callback": lambda: None}, + {"eval": "1 + 1"}, + {"source": "env.robot.control_parts"}, + {"bad": math.inf}, + ], +) +def test_registered_call_rejects_executable_or_non_declarative_payload( + arguments: dict[str, object], +) -> None: + with pytest.raises((TypeError, ValueError)): + RegisteredSemanticCallCfg( + call_id="example.inspect", + arguments=arguments, + ) + + +def test_pose_rejects_zero_quaternion() -> None: + with pytest.raises(ValueError, match="non-zero magnitude"): + PoseCfg( + position=(0.0, 0.0, 0.0), + quaternion_wxyz=(0.0, 0.0, 0.0, 0.0), + ) + + +def test_segment_owns_post_policy_and_validator_sequences() -> None: + post = [WaitStablePostCfg(entity="cube")] + validators = [ObjectNearTargetValidatorCfg(object="cube", target="drop_pose")] + segment = SegmentCfg( + name="move_cube", + steps=SequenceCfg(items=(_pick_invoke(),)), + post=post, + validators=validators, + ) + + post.clear() + validators.clear() + + assert segment.post == (WaitStablePostCfg(entity="cube"),) + assert segment.validators == ( + ObjectNearTargetValidatorCfg(object="cube", target="drop_pose"), + ) diff --git a/tests/gym/envs/expert_program/test_compiler.py b/tests/gym/envs/expert_program/test_compiler.py new file mode 100644 index 000000000..f3a94abc8 --- /dev/null +++ b/tests/gym/envs/expert_program/test_compiler.py @@ -0,0 +1,549 @@ +# ---------------------------------------------------------------------------- +# 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 provider-free Expert Program compilation and lazy expansion.""" + +from __future__ import annotations + +from itertools import islice + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramCompileError, + ExpertProgramCompiler, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + MaterializedCompiledProgram, + ObjectNearTargetValidatorCfg, + PickCfg, + PlaceCfg, + PoseCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + WaitStablePostCfg, +) +from embodichain.lab.sim.atomic_actions import Affordance, EntityState +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, + SemanticPose, +) +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRegistration, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + + +class _NeverObserveProvider: + """Record and reject every attempted dynamic scene observation.""" + + def __init__(self) -> None: + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + raise AssertionError("Expert Program compilation must not observe state.") + + +def _scene_registry() -> tuple[SceneRegistry, _NeverObserveProvider]: + """Return static identities backed by a provider that must stay unused.""" + provider = _NeverObserveProvider() + cube = SceneObjectRef("cube") + tray = SceneObjectRef("tray") + arm = SceneArticulationRef("arm") + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=cube, + state_provider=provider, + aliases=("sim_cube",), + ), + SceneEntityRegistration(ref=tray, state_provider=provider), + SceneEntityRegistration(ref=arm, state_provider=provider), + SceneEntityRegistration( + ref=SceneLinkRef("arm_tcp"), + state_provider=provider, + parent=arm, + native_name="tcp", + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube_grasp"), + aliases=("legacy_grasp",), + parent=cube, + native_name="grasp", + affordance=Affordance(), + relative_pose=torch.eye(4), + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("tray_top"), + parent=tray, + native_name="top", + affordance=Affordance(), + relative_pose=torch.eye(4), + ), + ) + ) + return registry, provider + + +def _integration() -> ExpertProgramIntegrationCfg: + """Return one static integration selection.""" + return ExpertProgramIntegrationCfg( + robot_profile="auto", + scene_registry="env", + runtime_preset="safe", + ) + + +def _pose(x: float, y: float = 0.0, z: float = 0.2) -> PoseCfg: + """Build one target pose with an identity WXYZ quaternion.""" + return PoseCfg( + position=(x, y, z), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ) + + +def _program( + node: InvokeCfg | SequenceCfg | RepeatCfg | SegmentCfg, + *, + targets: dict[str, CyclicPoseTargetCfg] | None = None, + program_id: str = "test_program", +) -> ExpertProgramCfg: + """Build one valid Version 1 program around a supplied node.""" + return ExpertProgramCfg( + schema_version=1, + program_id=program_id, + integration=_integration(), + program=node, + targets={} if targets is None else targets, + ) + + +def _assert_pose_equal(actual: SemanticPose, expected: SemanticPose) -> None: + """Compare owned pose tensor values.""" + assert torch.allclose(actual.position, expected.position) + assert torch.allclose(actual.quaternion_wxyz, expected.quaternion_wxyz) + + +def _assert_semantic_call_equal( + actual: SemanticCallSpec, + expected: SemanticCallSpec, +) -> None: + """Compare exact semantic call values whose public classes use eq=False.""" + assert type(actual) is type(expected) + assert dict(actual.resources) == dict(expected.resources) + if type(actual) is Pick and type(expected) is Pick: + assert actual.object == expected.object + assert actual.grasp == expected.grasp + elif type(actual) is Place and type(expected) is Place: + assert actual.object == expected.object + assert actual.on == expected.on + assert actual.inside == expected.inside + assert (actual.at is None) == (expected.at is None) + if actual.at is not None and expected.at is not None: + _assert_pose_equal(actual.at, expected.at) + elif type(actual) is HandOver and type(expected) is HandOver: + assert actual.object == expected.object + assert actual.receiver == expected.receiver + assert (actual.final_target is None) == (expected.final_target is None) + if actual.final_target is not None and expected.final_target is not None: + _assert_pose_equal(actual.final_target, expected.final_target) + elif ( + type(actual) is RegisteredSemanticCall + and type(expected) is RegisteredSemanticCall + ): + assert actual.call_id == expected.call_id + assert actual.arguments == expected.arguments + else: # pragma: no cover - exact supported union is exhausted above + raise AssertionError(f"Unsupported call type {type(actual).__name__}.") + + +def test_compiler_matches_direct_python_semantic_calls_and_sequence_order() -> None: + registry, provider = _scene_registry() + target = _pose(0.5, 0.1) + config = _program( + SequenceCfg( + items=( + InvokeCfg( + call=PickCfg( + object="sim_cube", + grasp="legacy_grasp", + resources={"primary": "left_actor"}, + ) + ), + InvokeCfg(call=PlaceCfg(object="sim_cube", on="tray_top")), + InvokeCfg( + call=HandOverCfg( + object="sim_cube", + receiver="right_actor", + final_target=TargetRefCfg(target="handover_pose"), + ) + ), + InvokeCfg( + call=RegisteredSemanticCallCfg( + call_id="example.inspect", + arguments={ + "labels": ["front", "back"], + "options": {"confidence": 0.9}, + }, + ) + ), + ) + ), + targets={"handover_pose": CyclicPoseTargetCfg(values=(target,))}, + ) + + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + segments = list(compiled) + + expected = ( + Pick( + object=SceneObjectRef("cube"), + grasp=SceneAffordanceRef("cube_grasp"), + resources={"primary": "left_actor"}, + ), + Place( + object=SceneObjectRef("cube"), + on=SceneAffordanceRef("tray_top"), + ), + HandOver( + object=SceneObjectRef("cube"), + receiver="right_actor", + final_target=SemanticPose(target.position, target.quaternion_wxyz), + ), + RegisteredSemanticCall( + call_id="example.inspect", + arguments={ + "labels": ("front", "back"), + "options": {"confidence": 0.9}, + }, + ), + ) + assert len(segments) == len(expected) + assert all(segment.implicit for segment in segments) + assert [segment.segment_index for segment in segments] == list(range(4)) + assert [segment.calls[0].call_index for segment in segments] == list(range(4)) + assert len({segment.segment_id for segment in segments}) == 4 + for segment, expected_call in zip(segments, expected, strict=True): + _assert_semantic_call_equal(segment.calls[0].call, expected_call) + assert provider.calls == 0 + + +def test_repeat_expands_independent_segments_with_cyclic_targets() -> None: + registry, provider = _scene_registry() + poses = (_pose(0.45, -0.2), _pose(0.45, 0.0), _pose(0.45, 0.2)) + body = SegmentCfg( + name="move_cube", + steps=SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="cube")), + InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop_pose"), + ) + ), + ) + ), + post=(WaitStablePostCfg(entity="cube"),), + validators=(ObjectNearTargetValidatorCfg(object="cube", target="drop_pose"),), + ) + config = _program( + RepeatCfg(count=3, body=body), + targets={"drop_pose": CyclicPoseTargetCfg(values=poses)}, + program_id="repeated_cube", + ) + + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + segments = list(compiled) + second_pass = list(compiled) + + assert [segment.segment_id for segment in segments] == [ + segment.segment_id for segment in second_pass + ] + assert len(segments) == 3 + assert len({segment.segment_id for segment in segments}) == 3 + assert [segment.segment_index for segment in segments] == [0, 1, 2] + assert [call.call_index for segment in segments for call in segment.calls] == list( + range(6) + ) + assert all(not segment.implicit for segment in segments) + assert all(segment is not other for segment, other in zip(segments, second_pass)) + for index, (segment, pose) in enumerate(zip(segments, poses, strict=True)): + assert len(segment.repeat_frames) == 1 + assert segment.repeat_frames[0].path == ("program",) + assert segment.repeat_frames[0].iteration_index == index + assert segment.repeat_frames[0].count == 3 + place = segment.calls[1] + assert type(place.call) is Place + assert place.call.at is not None + _assert_pose_equal( + place.call.at, + SemanticPose(pose.position, pose.quaternion_wxyz), + ) + assert place.target_selections[0].value_index == index + validator = segment.validators[0] + _assert_pose_equal(validator.target_pose, place.call.at) + assert validator.target_selection == place.target_selections[0] + assert segment.post_policies[0].entity == SceneObjectRef("cube") + assert provider.calls == 0 + + +def test_repeat_expansion_is_lazy_and_never_observes_scene_providers() -> None: + registry, provider = _scene_registry() + config = _program( + RepeatCfg( + count=1_000, + body=InvokeCfg(call=PickCfg(object="sim_cube")), + ) + ) + + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + iterator = iter(compiled) + + assert provider.calls == 0 + first_two = list(islice(iterator, 2)) + assert [segment.segment_index for segment in first_two] == [0, 1] + assert [segment.repeat_frames[0].iteration_index for segment in first_two] == [ + 0, + 1, + ] + assert provider.calls == 0 + + +def test_materialized_program_builds_cross_segment_analysis_windows_provider_free() -> ( + None +): + registry, provider = _scene_registry() + config = _program( + SequenceCfg( + items=( + SegmentCfg( + name="pick", + steps=InvokeCfg(call=PickCfg(object="cube")), + ), + SegmentCfg( + name="place", + steps=InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop_pose"), + ) + ), + ), + ) + ), + targets={"drop_pose": CyclicPoseTargetCfg(values=(_pose(0.5),))}, + ) + + materialized = ( + ExpertProgramCompiler.from_scene_registry(registry) + .compile(config) + .materialize() + ) + preflight = materialized.preflight_analyses() + execution = materialized.sequential_execution_analysis(0) + + assert type(materialized) is MaterializedCompiledProgram + assert materialized.segment_count == 2 + assert len(tuple(materialized.iter_segments())) == 2 + assert len(preflight) == 1 + assert preflight[0].kind == "sequential_stretch" + assert [type(call) for call in preflight[0].calls] == [Pick, Place] + assert execution.kind == "sequential_suffix" + assert execution.execution_prefix_length == 1 + assert [type(call) for call in execution.calls] == [Pick, Place] + assert provider.calls == 0 + + +def test_materialization_rechecks_expanded_call_bound_after_config_mutation() -> None: + registry, provider = _scene_registry() + inner = RepeatCfg(count=1, body=InvokeCfg(call=PickCfg(object="cube"))) + config = _program(RepeatCfg(count=1, body=inner)) + assert type(config.program) is RepeatCfg + assert type(config.program.body) is RepeatCfg + config.program.count = 1_000 + config.program.body.count = 1_000 + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + + with pytest.raises(ExpertProgramCompileError) as error: + compiled.materialize() + + assert error.value.code == "expanded_call_limit" + assert provider.calls == 0 + + +def test_wait_stable_accepts_every_canonical_scene_entity_subtype() -> None: + registry, provider = _scene_registry() + config = _program( + SegmentCfg( + name="link_settle", + steps=InvokeCfg(call=PickCfg(object="cube")), + post=(WaitStablePostCfg(entity="arm_tcp"),), + ) + ) + + segment = next( + iter(ExpertProgramCompiler.from_scene_registry(registry).compile(config)) + ) + + assert segment.post_policies[0].entity == SceneLinkRef("arm_tcp") + assert provider.calls == 0 + + +def test_compiled_program_owns_source_and_each_emitted_mutable_config() -> None: + registry, _ = _scene_registry() + target = CyclicPoseTargetCfg(values=(_pose(0.4), _pose(0.5))) + registered = RegisteredSemanticCallCfg( + call_id="example.inspect", + arguments={"settings": {"enabled": True}}, + ) + repeat = RepeatCfg( + count=2, + body=SegmentCfg( + name="inspect_and_place", + steps=SequenceCfg( + items=( + InvokeCfg(call=registered), + InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop_pose"), + ) + ), + ) + ), + post=(WaitStablePostCfg(entity="cube", preset="rigid_object"),), + validators=( + ObjectNearTargetValidatorCfg( + object="cube", + target="drop_pose", + position_tolerance=0.03, + ), + ), + ), + ) + config = _program(repeat, targets={"drop_pose": target}) + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + + compiled_source = config.program + assert type(compiled_source) is RepeatCfg + source_segment = compiled_source.body + assert type(source_segment) is SegmentCfg + source_steps = source_segment.steps + assert type(source_steps) is SequenceCfg + source_registered = source_steps.items[0].call + assert type(source_registered) is RegisteredSemanticCallCfg + compiled_source.count = 1 + config.targets["drop_pose"].values = (_pose(9.0),) + source_registered.arguments["settings"]["enabled"] = False + source_segment.post[0].preset = "changed" + source_segment.validators[0].position_tolerance = 9.0 + + first_pass = list(compiled) + assert len(first_pass) == 2 + first_registered = first_pass[0].calls[0].call + assert type(first_registered) is RegisteredSemanticCall + assert first_registered.arguments["settings"]["enabled"] is True + first_place = first_pass[0].calls[1].call + assert type(first_place) is Place and first_place.at is not None + assert first_place.at.position[0].item() == pytest.approx(0.4) + assert first_pass[0].post_policies[0].cfg.preset == "rigid_object" + assert first_pass[0].validators[0].cfg.position_tolerance == pytest.approx(0.03) + + first_pass[0].post_policies[0].cfg.preset = "mutated_output" + first_pass[0].validators[0].cfg.position_tolerance = 8.0 + exposed_position = compiled.targets["drop_pose"][0].position + exposed_position[0] = -10.0 + + second_pass = list(compiled) + assert second_pass[0].post_policies[0].cfg.preset == "rigid_object" + assert second_pass[0].validators[0].cfg.position_tolerance == pytest.approx(0.03) + assert compiled.targets["drop_pose"][0].position[0].item() == pytest.approx(0.4) + + +def test_compiler_rejects_nested_segment_at_exact_path() -> None: + registry, _ = _scene_registry() + config = _program( + SegmentCfg( + name="outer", + steps=SegmentCfg( + name="inner", + steps=InvokeCfg(call=PickCfg(object="cube")), + ), + ) + ) + + with pytest.raises(ExpertProgramCompileError) as error: + ExpertProgramCompiler.from_scene_registry(registry).compile(config) + + assert error.value.code == "nested_segment" + assert error.value.path == ("program", "steps") + + +def test_compiler_reports_typed_scene_mismatch_at_reference_site() -> None: + registry, _ = _scene_registry() + config = _program( + InvokeCfg(call=PickCfg(object="tray_top")), + ) + + with pytest.raises(ExpertProgramCompileError) as error: + ExpertProgramCompiler.from_scene_registry(registry).compile(config) + + assert error.value.code == "scene_reference_type_mismatch" + assert error.value.path == ("program", "call", "object") + + +def test_compiler_rechecks_mutated_repeat_and_target_bounds() -> None: + registry, _ = _scene_registry() + repeat = RepeatCfg(count=1, body=InvokeCfg(call=PickCfg(object="cube"))) + target = CyclicPoseTargetCfg(values=(_pose(0.4),)) + config = _program(repeat, targets={"drop_pose": target}) + assert type(config.program) is RepeatCfg + config.program.count = 0 + + with pytest.raises(ExpertProgramCompileError) as repeat_error: + ExpertProgramCompiler.from_scene_registry(registry).compile(config) + assert repeat_error.value.code == "invalid_repeat_count" + assert repeat_error.value.path == ("program", "count") + + config.program.count = 1 + config.targets["drop_pose"].values = () + with pytest.raises(ExpertProgramCompileError) as target_error: + ExpertProgramCompiler.from_scene_registry(registry).compile(config) + assert target_error.value.code == "empty_target_values" + assert target_error.value.path == ("targets", "drop_pose", "values") diff --git a/tests/gym/envs/expert_program/test_completion_metadata.py b/tests/gym/envs/expert_program/test_completion_metadata.py new file mode 100644 index 000000000..eb8eda4cd --- /dev/null +++ b/tests/gym/envs/expert_program/test_completion_metadata.py @@ -0,0 +1,502 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Completion-trace audit across semantic execution and the Gym bridge.""" + +from __future__ import annotations + +from dataclasses import dataclass +import json +from types import SimpleNamespace +from typing import ClassVar +from unittest.mock import Mock + +import torch + +from embodichain.lab.gym.envs.expert_program.bridge import ( + AtomicDemoBridge, + BufferedGymCommandSink, + EnvironmentStepClock, + RuntimeCommandFrameEncoder, +) +from embodichain.lab.sim.atomic_actions import ( + ActionInvocation, + ActionOptions, + ActionPlan, + AtomicAction, + AtomicActionEngine, + EndpointCommand, + EntityState, + JointPositionPayload, + JointPositionTarget, + MotionPolicy, + PlannerDiagnostics, + PlanningContext, + RecoveryPolicy, + ResolvedActionRequest, + RobotObservation, + RuntimeCommandFrame, + SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + TaskState, + TimedCommandSequence, +) +from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.runtime import SkillRuntime, SkillStatus +from embodichain.lab.sim.skills.scene import SceneRegistry + +STEP_DT = 0.02 +BATCH_SIZE = 2 +ROBOT_DOF = 5 +ENV_IDS = torch.tensor([7, 3], dtype=torch.long) +INITIAL_SCENE_VERSION = 41 +REPLANNED_SCENE_VERSION = 42 +INITIAL_COLLISION_REVISIONS = (5, 7) +REPLANNED_COLLISION_REVISIONS = (6, 8) + + +@dataclass(frozen=True, slots=True) +class _TraceGoal: + """Test goal for a deterministic two-phase runtime command sequence.""" + + goal_kind: ClassVar[str] = "completion_trace" + + +class _TraceAction(AtomicAction[_TraceGoal, ActionOptions]): + """Emit named segments and preserve distinct diagnostics on every replan.""" + + skill_id: ClassVar[str] = "completion_trace" + GoalType: ClassVar[type] = _TraceGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=(SkillEndpointRequirement(endpoint_id="motion"),), + ), + ) + ) + + def __init__(self) -> None: + super().__init__() + self.plan_count = 0 + + def _scene_dependencies( + self, + request: ResolvedActionRequest[_TraceGoal, ActionOptions], + ) -> tuple[str, ...]: + del request + return ("trace_target",) + + def _plan( + self, + request: ResolvedActionRequest[_TraceGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + self.require_goal(request) + generation = self.plan_count + self.plan_count += 1 + target = request.binding.endpoint( + "primary", + "motion", + ).require_target(JointPositionTarget) + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target=target, + payload=JointPositionPayload( + torch.full( + (context.batch_size, len(target.joint_ids)), + float(generation + phase_index + 1), + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + ), + ), + ), + active_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + env_ids=context.env_ids, + hold_duration=torch.full( + (context.batch_size,), + STEP_DT, + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ), + ) + for phase_index in range(2) + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence(frames, context.env_ids), + replannable=True, + diagnostics=PlannerDiagnostics( + backend="completion_trace_planner", + messages=(f"installed generation {generation}",), + metadata={ + "generation": generation, + "quality": {"accepted": True, "score": generation + 0.25}, + }, + ), + segment_lengths={"approach": 1, "commit": 1}, + scene_dependency_monitor_until={"trace_target": 2}, + ) + + +class _TraceObservationProvider: + """Move one scene dependency after the first installed command frame.""" + + def __init__(self, clock: EnvironmentStepClock) -> None: + self.clock = clock + self.calls = 0 + + def observe(self, task_state: TaskState) -> PlanningContext: + self.calls += 1 + replanned_scene = self.calls >= 2 + pose = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + if replanned_scene: + pose[:, 0, 3] = 0.25 + qpos = torch.zeros(BATCH_SIZE, ROBOT_DOF) + timestamp = self.clock.now() + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=task_state, + scene=SceneSnapshot( + timestamp=timestamp, + version=( + REPLANNED_SCENE_VERSION + if replanned_scene + else INITIAL_SCENE_VERSION + ), + entities={"trace_target": EntityState(pose)}, + collision_world_revision=( + REPLANNED_COLLISION_REVISIONS + if replanned_scene + else INITIAL_COLLISION_REVISIONS + ), + ), + env_ids=ENV_IDS, + ) + + +class _StaticQposProvider: + """Supply full robot state to the bridge's transport encoder.""" + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + assert torch.equal(env_ids, ENV_IDS) + return torch.zeros(BATCH_SIZE, ROBOT_DOF) + + +class _UnusedEvidenceCollector: + """Satisfy the runtime port; this action declares no physical effect.""" + + def collect( + self, + spec: object, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> dict[str, object]: + del spec, timestamp, observation_revision, env_ids + raise AssertionError("The completion trace must not request effect evidence.") + + +@dataclass(frozen=True, slots=True) +class _TraceWorkflow: + """Minimal analyzed workflow retained by the production runtime.""" + + workflow_id: str + calls: tuple[RegisteredSemanticCall, ...] + + +@dataclass(frozen=True, slots=True) +class _TraceIntegration: + """Production engine and registry exposed through the compiler boundary.""" + + engine: AtomicActionEngine + scene_registry: SceneRegistry + + +@dataclass(frozen=True, slots=True) +class _TraceGroundedCall: + """One grounded invocation with no external effect-verification boundary.""" + + analyzed: object + invocation: ActionInvocation + eligible_mask: torch.Tensor + effect_spec: None = None + effect_monitor: None = None + + +class _TraceCompiler(SemanticSkillCompiler): + """Keep semantic boundaries real while making lowering deterministic.""" + + def __init__(self, engine: AtomicActionEngine) -> None: + self._trace_integration = _TraceIntegration(engine, SceneRegistry()) + + @property + def integration(self) -> _TraceIntegration: + return self._trace_integration + + def analyze( + self, + calls: tuple[RegisteredSemanticCall, ...], + *, + workflow_id: str = "semantic_workflow", + path: tuple[object, ...] = ("workflow",), + ) -> _TraceWorkflow: + del path + return _TraceWorkflow(workflow_id, tuple(calls)) + + def ground( + self, + workflow: _TraceWorkflow, + call_index: int, + context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, + revision: int = 0, + path: tuple[object, ...] = ("workflow",), + ) -> _TraceGroundedCall: + del context, path + assert eligible_mask is not None + binding = self.integration.engine.bind_control_parts( + _TraceAction.skill_id, + {"primary": {"motion": "arm"}}, + ) + invocation = ActionInvocation( + skill_id=_TraceAction.skill_id, + goal=_TraceGoal(), + binding=binding, + motion_policy=MotionPolicy( + strategy="ik_interp", + sample_count=9, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + max_action_retries=1, + action_timeout=1.0, + ), + invocation_id=f"{workflow.workflow_id}:{call_index}", + revision=revision, + ) + analyzed = SimpleNamespace( + bound=SimpleNamespace( + robot_profile=SimpleNamespace(profile_id="completion_trace_robot"), + preset=SimpleNamespace( + preset_id="completion_trace_preset", + schema_version=1, + motion_policy=invocation.motion_policy, + recovery_policy=invocation.recovery_policy, + ), + ) + ) + return _TraceGroundedCall( + analyzed=analyzed, + invocation=invocation, + eligible_mask=eligible_mask.clone(), + ) + + +@dataclass(frozen=True, slots=True) +class _CompiledCall: + """Program-owned semantic call and stable call index.""" + + call_index: int + call: RegisteredSemanticCall + + +@dataclass(frozen=True, slots=True) +class _CompiledSegment: + """One logical program segment consumed by the production bridge.""" + + calls: tuple[_CompiledCall, ...] + segment_index: int = 0 + segment_id: str = "completion-segment" + name: str = "completion-audit" + source_path: tuple[object, ...] = ("program", "steps", 0) + post_policies: tuple[object, ...] = () + validators: tuple[object, ...] = () + parallel_block: None = None + implicit: bool = False + + +@dataclass(frozen=True, slots=True) +class _ProgramAnalysis: + """Sequential look-ahead window selected for one bridge segment.""" + + calls: tuple[RegisteredSemanticCall, ...] + execution_prefix_length: int + + +class _CompiledProgram: + """Single-segment compiled-program port for the completion audit.""" + + schema_version = 2 + program_id = "completion-audit-program" + + def __init__(self, segment: _CompiledSegment) -> None: + self.segment = segment + + def iter_segments(self): + yield self.segment + + def sequential_execution_analysis(self, segment_index: int) -> _ProgramAnalysis: + assert segment_index == self.segment.segment_index + return _ProgramAnalysis( + tuple(compiled.call for compiled in self.segment.calls), + len(self.segment.calls), + ) + + +def _runtime_and_bridge() -> tuple[AtomicDemoBridge, _TraceAction]: + """Assemble real execution/runtime/bridge layers around deterministic ports.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = ROBOT_DOF + robot.control_parts = {"arm": object()} + robot.get_joint_ids.return_value = (1, 3) + robot.get_qpos.return_value = torch.zeros(BATCH_SIZE, ROBOT_DOF) + robot.get_qvel.return_value = torch.zeros(BATCH_SIZE, ROBOT_DOF) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "completion_trace_planner" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _TraceAction() + engine.register(action) + + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_StaticQposProvider()), + clock, + ) + runtime = SkillRuntime.from_components( + _TraceCompiler(engine), + _TraceObservationProvider(clock), + sink, + _UnusedEvidenceCollector(), + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + clock=clock, + ) + call = RegisteredSemanticCall(call_id="audit.completion_metadata") + segment = _CompiledSegment((_CompiledCall(0, call),)) + bridge = AtomicDemoBridge(_CompiledProgram(segment), runtime, sink, clock) + return bridge, action + + +def test_completion_trace_preserves_every_plan_generation_as_json_metadata() -> None: + """A real scene replan remains complete after SkillResult and bridge snapshots.""" + bridge, action = _runtime_and_bridge() + demo_segment = next(bridge.iter_segments()) + + emitted_actions = tuple(demo_segment.actions) + accepted = demo_segment.validator() + metadata = demo_segment.metadata + + serialized = json.dumps(metadata, allow_nan=False, sort_keys=True) + assert json.loads(serialized) == metadata + assert emitted_actions + assert accepted.tolist() == [True, True] + assert action.plan_count == 2 + + assert metadata["expert_program_schema_version"] == 2 + assert metadata["expert_program_id"] == "completion-audit-program" + assert metadata["program_segment_id"] == "completion-segment" + assert metadata["program_segment_index"] == 0 + assert metadata["program_segment_source_path"] == ["program", "steps", 0] + assert metadata["program_segment_implicit"] is False + assert metadata["semantic_call_indices"] == [0] + assert metadata["post_policy_count"] == 0 + assert metadata["validator_count"] == 0 + assert metadata["parallel"] is False + assert metadata["validation"]["accepted_mask"] == [True, True] + + runtime_trace = metadata["runtime"] + assert runtime_trace["kind"] == "skill_result" + assert runtime_trace["status"] == SkillStatus.COMPLETED.value + call_trace = runtime_trace["calls"][0] + assert call_trace["active_plan_attempt_generation"] == 1 + attempts = call_trace["plan_attempts"] + assert [attempt["attempt_generation"] for attempt in attempts] == [0, 1] + assert [attempt["trigger"] for attempt in attempts] == [ + "action_planned", + "replanned", + ] + assert [attempt["planned_scene_version"] for attempt in attempts] == [ + INITIAL_SCENE_VERSION, + REPLANNED_SCENE_VERSION, + ] + assert [attempt["planned_collision_world_revision"] for attempt in attempts] == [ + list(INITIAL_COLLISION_REVISIONS), + list(REPLANNED_COLLISION_REVISIONS), + ] + assert all( + attempt["scene_dependency_monitor_until"] == {"trace_target": 2} + for attempt in attempts + ) + assert all( + attempt["trajectory_segments"] + == [ + {"name": "approach", "start": 0, "stop": 1, "waypoint_count": 1}, + {"name": "commit", "start": 1, "stop": 2, "waypoint_count": 1}, + ] + for attempt in attempts + ) + assert [attempt["recovery_counters"] for attempt in attempts] == [ + {"action_retries": [0, 0], "replans": [0, 0]}, + {"action_retries": [0, 0], "replans": [1, 1]}, + ] + assert [attempt["planner_diagnostics"] for attempt in attempts] == [ + { + "backend": "completion_trace_planner", + "messages": ["installed generation 0"], + "metadata": { + "generation": 0, + "quality": {"accepted": True, "score": 0.25}, + }, + }, + { + "backend": "completion_trace_planner", + "messages": ["installed generation 1"], + "metadata": { + "generation": 1, + "quality": {"accepted": True, "score": 1.25}, + }, + }, + ] + event_kinds = [event["kind"] for event in call_trace["events"]] + assert runtime_trace["events"] == call_trace["events"] + assert "dynamic_goal_changed" in event_kinds + assert "replanned" in event_kinds + assert event_kinds[-3:] == [ + "trajectory_completed", + "action_completed", + "session_completed", + ] diff --git a/tests/gym/envs/expert_program/test_decoder.py b/tests/gym/envs/expert_program/test_decoder.py new file mode 100644 index 000000000..d21458dc6 --- /dev/null +++ b/tests/gym/envs/expert_program/test_decoder.py @@ -0,0 +1,596 @@ +# ---------------------------------------------------------------------------- +# 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 strict Expert Program Version 1 decoding.""" + +from __future__ import annotations + +from copy import deepcopy + +import pytest + +from embodichain.lab.gym.envs.expert_program import ( + MAX_REPEAT_COUNT, + ConfigPath, + ExpertProgramDecodeError, + ExpertProgramIntegrationCfg, + ExpertProgramValidationError, + HandOverCfg, + PickCfg, + PlaceCfg, + PostPolicyCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SceneReferenceRole, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + ValidatorCfg, + decode_expert_program, + decode_semantic_call, + encode_semantic_call, + render_config_path, +) + + +def _program_data() -> dict[str, object]: + """Return the repeated-cube Version 1 example as plain JSON values.""" + return { + "schema_version": 1, + "program_id": "repeated_cube_pick_place", + "integration": { + "robot_profile": "auto", + "scene_registry": "env", + "runtime_preset": "safe", + }, + "targets": { + "drop_pose": { + "kind": "cyclic_pose", + "values": [ + { + "position": [0.45, -0.20, 0.20], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + { + "position": [0.45, 0.00, 0.20], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + { + "position": [0.45, 0.20, 0.20], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + ], + } + }, + "program": { + "kind": "repeat", + "count": 3, + "body": { + "kind": "segment", + "name": "move_cube", + "steps": { + "kind": "sequence", + "items": [ + { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "at": { + "kind": "target_ref", + "target": "drop_pose", + }, + }, + }, + ], + }, + "post": [ + { + "kind": "wait_stable", + "entity": "cube", + "preset": "rigid_object", + } + ], + "validators": [ + { + "kind": "object_near_target", + "object": "cube", + "target": "drop_pose", + "position_tolerance": 0.03, + } + ], + }, + }, + } + + +def _invoke(call: dict[str, object]) -> dict[str, object]: + """Wrap one call mapping in an invoke node.""" + return {"kind": "invoke", "call": call} + + +@pytest.mark.parametrize( + "payload", + ( + { + "kind": "pick", + "object": "cube", + "grasp": "cube_grasp", + "resources": {"primary": "left"}, + }, + { + "kind": "place", + "object": "cube", + "at": {"kind": "target_ref", "target": "drop_pose"}, + }, + { + "kind": "hand_over", + "object": "cube", + "receiver": "right", + "final_target": {"kind": "target_ref", "target": "drop_pose"}, + }, + { + "kind": "operate_articulation", + "articulation": "drawer", + "handle": "drawer_handle", + "target": "open", + }, + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 1, + "arguments": {"labels": ["front", "back"]}, + }, + ), +) +def test_public_semantic_call_codec_round_trips_exact_schema( + payload: dict[str, object], +) -> None: + call = decode_semantic_call( + payload, + target_ids=frozenset({"drop_pose"}), + ) + encoded = encode_semantic_call(call) + round_trip = decode_semantic_call( + encoded, + target_ids=frozenset({"drop_pose"}), + ) + + assert type(round_trip) is type(call) + assert encode_semantic_call(round_trip) == encoded + + +def test_public_semantic_call_decoder_owns_input_and_validates_context() -> None: + payload = {"kind": "pick", "object": "cube", "grasp": "cube_grasp"} + context = _StaticValidationContext() + + call = decode_semantic_call(payload, validation_context=context) + payload["object"] = "changed" + + assert type(call) is PickCfg + assert call.object == "cube" + assert ("call",) in context.validated_paths + assert ("call", "object") in context.validated_paths + + +def test_decoder_builds_owned_repeated_cube_ast() -> None: + data = _program_data() + + config = decode_expert_program(data) + data["program"]["count"] = 99 + data["targets"]["drop_pose"]["values"][0]["position"][0] = -1.0 + + assert type(config.program) is RepeatCfg + assert config.program.count == 3 + assert type(config.program.body) is SegmentCfg + assert type(config.program.body.steps) is SequenceCfg + place = config.program.body.steps.items[1].call + assert type(place) is PlaceCfg + assert place.at == TargetRefCfg(target="drop_pose") + assert config.targets["drop_pose"].values[0].position[0] == pytest.approx(0.45) + + +def test_decoder_supports_every_version_one_semantic_call() -> None: + data = _program_data() + data["program"] = { + "kind": "sequence", + "items": [ + _invoke( + { + "kind": "pick", + "object": "cube", + "grasp": "cube_grasp", + "resources": {"primary": "left_actor"}, + } + ), + _invoke({"kind": "place", "object": "cube", "on": "tray_top"}), + _invoke( + { + "kind": "hand_over", + "object": "cube", + "receiver": "right_actor", + "final_target": { + "kind": "target_ref", + "target": "drop_pose", + }, + } + ), + _invoke( + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 1, + "arguments": { + "labels": ["front", "back"], + "options": {"confidence": 0.9}, + }, + } + ), + ], + } + + config = decode_expert_program(data) + assert [type(node.call) for node in config.program.items] == [ + PickCfg, + PlaceCfg, + HandOverCfg, + RegisteredSemanticCallCfg, + ] + handover = config.program.items[2].call + assert handover.resources == {"destination": "right_actor"} + registered = config.program.items[3].call + assert registered.arguments == { + "labels": ("front", "back"), + "options": {"confidence": 0.9}, + } + + +@pytest.mark.parametrize( + ("mutate", "expected_path"), + [ + ( + lambda data: data.update({"unexpected": True}), + "$.unexpected", + ), + ( + lambda data: data["program"]["body"].update({"unexpected": True}), + "$.program.body.unexpected", + ), + ( + lambda data: data["program"]["body"]["steps"]["items"][0]["call"].update( + {"unexpected": True} + ), + "$.program.body.steps.items[0].call.unexpected", + ), + ], +) +def test_decoder_rejects_unknown_fields_with_complete_path( + mutate: object, + expected_path: str, +) -> None: + data = _program_data() + mutate(data) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "unknown_field" + assert render_config_path(error.value.path) == expected_path + + +def test_decoder_reports_missing_required_field_at_exact_path() -> None: + data = _program_data() + del data["integration"]["runtime_preset"] + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "missing_field" + assert render_config_path(error.value.path) == "$.integration.runtime_preset" + + +@pytest.mark.parametrize( + ("value", "code"), + [ + (None, "missing_discriminator"), + ("parallel", "unknown_discriminator"), + ], +) +def test_decoder_rejects_missing_or_reserved_program_discriminator( + value: str | None, + code: str, +) -> None: + data = _program_data() + if value is None: + del data["program"]["kind"] + else: + data["program"]["kind"] = value + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == code + assert error.value.path == ("program", "kind") + + +@pytest.mark.parametrize( + ("mutate", "expected_path"), + [ + ( + lambda data: data["targets"]["drop_pose"].update({"kind": "pose"}), + "$.targets.drop_pose.kind", + ), + ( + lambda data: data["program"]["body"]["steps"]["items"][0]["call"].update( + {"kind": "move"} + ), + "$.program.body.steps.items[0].call.kind", + ), + ( + lambda data: data["program"]["body"]["steps"]["items"][1]["call"][ + "at" + ].update({"kind": "env_ref"}), + "$.program.body.steps.items[1].call.at.kind", + ), + ( + lambda data: data["program"]["body"]["post"][0].update({"kind": "sleep"}), + "$.program.body.post[0].kind", + ), + ( + lambda data: data["program"]["body"]["validators"][0].update( + {"kind": "python"} + ), + "$.program.body.validators[0].kind", + ), + ], +) +def test_every_union_rejects_unknown_discriminator_at_exact_path( + mutate: object, + expected_path: str, +) -> None: + data = _program_data() + mutate(data) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "unknown_discriminator" + assert render_config_path(error.value.path) == expected_path + + +@pytest.mark.parametrize("schema_version", [False, 0, 3, "1"]) +def test_decoder_rejects_unsupported_top_level_schema_version( + schema_version: object, +) -> None: + data = _program_data() + data["schema_version"] = schema_version + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "unsupported_schema_version" + assert error.value.path == ("schema_version",) + + +def test_decoder_reports_unknown_target_at_reference_site() -> None: + data = _program_data() + data["program"]["body"]["steps"]["items"][1]["call"]["at"]["target"] = "missing" + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "unknown_target" + assert render_config_path(error.value.path) == ( + "$.program.body.steps.items[1].call.at.target" + ) + + +@pytest.mark.parametrize("count", [False, 0, MAX_REPEAT_COUNT + 1]) +def test_decoder_rejects_unbounded_or_invalid_repeat_count(count: object) -> None: + data = _program_data() + data["program"]["count"] = count + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "invalid_repeat_count" + assert render_config_path(error.value.path) == "$.program.count" + + +def test_registered_call_schema_version_error_reports_version_field() -> None: + data = _program_data() + data["program"] = _invoke( + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 2, + } + ) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "invalid_schema_version" + assert render_config_path(error.value.path) == "$.program.call.schema_version" + + +@pytest.mark.parametrize( + ("arguments", "code", "suffix"), + [ + ({"eval": "1 + 1"}, "forbidden_construct", ".arguments.eval"), + ( + {"source": "env.robot.control_parts"}, + "environment_traversal", + ".arguments.source", + ), + ( + {"source": "eval(1 + 1)"}, + "executable_expression", + ".arguments.source", + ), + ({"callback": lambda: None}, "non_declarative_value", ".arguments.callback"), + ({"live": object()}, "non_declarative_value", ".arguments.live"), + ], +) +def test_decoder_rejects_executable_traversal_or_live_registered_payload( + arguments: dict[str, object], + code: str, + suffix: str, +) -> None: + data = _program_data() + data["program"] = _invoke( + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 1, + "arguments": arguments, + } + ) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == code + assert render_config_path(error.value.path).endswith(suffix) + + +def test_decoder_rejects_cyclic_input_before_ast_recursion() -> None: + data = _program_data() + cyclic: dict[str, object] = {} + cyclic["self"] = cyclic + data["program"] = _invoke( + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 1, + "arguments": cyclic, + } + ) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "cyclic_input" + + +class _StaticValidationContext: + """Small provider-free reference catalog used by decoder tests.""" + + def __init__( + self, + *, + calls: set[str] | None = None, + scene: set[str] | None = None, + ) -> None: + self.calls = {"pick", "place", "hand_over"} if calls is None else calls + self.scene = {"cube", "cube_grasp", "tray_top"} if scene is None else scene + self.validated_paths: list[ConfigPath] = [] + + def validate_integration( + self, + integration: ExpertProgramIntegrationCfg, + *, + path: ConfigPath, + ) -> None: + if integration.robot_profile != "auto": + raise KeyError(integration.robot_profile) + self.validated_paths.append(path) + + def validate_semantic_call( + self, + call: object, + *, + path: ConfigPath, + ) -> None: + semantic_id = ( + call.call_id if type(call) is RegisteredSemanticCallCfg else call.kind + ) + if semantic_id not in self.calls: + raise KeyError(semantic_id) + self.validated_paths.append(path) + + def validate_scene_reference( + self, + reference: str, + *, + role: SceneReferenceRole, + path: ConfigPath, + ) -> None: + del role + if reference not in self.scene: + raise KeyError(reference) + self.validated_paths.append(path) + + def validate_post_policy( + self, + policy: PostPolicyCfg, + *, + path: ConfigPath, + ) -> None: + if policy.kind != "wait_stable" or policy.preset != "rigid_object": + raise KeyError(policy.preset) + self.validated_paths.append(path) + + def validate_validator( + self, + validator: ValidatorCfg, + *, + path: ConfigPath, + ) -> None: + if validator.kind != "object_near_target": + raise KeyError(validator.kind) + self.validated_paths.append(path) + + +def test_decoder_runs_explicit_provider_free_validation_context() -> None: + data = _program_data() + context = _StaticValidationContext() + + config = decode_expert_program(data, validation_context=context) + + assert config.program_id == "repeated_cube_pick_place" + assert ("integration",) in context.validated_paths + assert ("program", "body", "steps", "items", 0, "call") in (context.validated_paths) + assert ("program", "body", "post", 0, "entity") in (context.validated_paths) + + +def test_validation_context_failure_is_wrapped_at_exact_reference_path() -> None: + data = _program_data() + context = _StaticValidationContext(scene={"cube"}) + data["program"]["body"]["steps"]["items"][0]["call"]["grasp"] = "missing_grasp" + + with pytest.raises(ExpertProgramValidationError) as error: + decode_expert_program(data, validation_context=context) + + assert error.value.code == "reference_validation_failed" + assert render_config_path(error.value.path) == ( + "$.program.body.steps.items[0].call.grasp" + ) + + +def test_decoder_does_not_mutate_caller_input_on_failure() -> None: + data = _program_data() + data["program"]["unexpected"] = True + before = deepcopy(data) + + with pytest.raises(ExpertProgramDecodeError): + decode_expert_program(data) + + assert data == before diff --git a/tests/gym/envs/expert_program/test_environment.py b/tests/gym/envs/expert_program/test_environment.py new file mode 100644 index 000000000..b1be4a6a0 --- /dev/null +++ b/tests/gym/envs/expert_program/test_environment.py @@ -0,0 +1,949 @@ +# ---------------------------------------------------------------------------- +# 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 reusable environment-backed Expert Program assembly.""" + +from __future__ import annotations + +from collections import Counter +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program.bridge import ( + AtomicDemoBridge, + DemoBridgeError, + EnvironmentStepClock, + GymPlanningObservationProvider, +) +from embodichain.lab.gym.envs.expert_program.cfg import ( + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, + BarrierCfg, + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + OperateArticulationCfg, + ParallelCfg, + PickCfg, + PlaceCfg, + PoseCfg, + ProgramNodeCfg, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + WaitStablePostCfg, +) +from embodichain.lab.gym.envs.expert_program.environment import ( + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + PlanningObservationPort, + SkillRuntimeAssemblyPort, +) +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + EntityState, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + MotionPolicy, + PlanningContext, + RobotObservation, + TaskState, +) +from embodichain.lab.sim.skills import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + GRASP_AFFORDANCE_CAPABILITY, + ControlPartEndpoint, + RobotResource, + RobotSkillProfile, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.evidence import EffectEvidenceProvider +from embodichain.lab.sim.skills.integration import SemanticValidationError + +_BATCH_SIZE = 2 +_ROBOT_DOF = 2 +_STEP_DT = 0.02 + + +class _PoseProvider: + """Return a stable owned pose for the fake environment scene.""" + + def __init__(self) -> None: + self._pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + """Return rows aligned to the requested environment IDs.""" + del timestamp + self.calls += 1 + return EntityState(self._pose.index_select(0, env_ids)) + + +class _GeometryProvider: + """Return one opaque planner-facing geometry descriptor.""" + + def get_geometry(self) -> object: + return object() + + +def _scene_registry( + *, + dynamic_collision: bool = False, + pose_provider: _PoseProvider | None = None, +) -> SceneRegistry: + """Build an explicitly named object and default grasp affordance.""" + cube = SceneObjectRef("cube") + grasp = SceneAffordanceRef("cube_grasp") + selected_pose_provider = _PoseProvider() if pose_provider is None else pose_provider + return SceneRegistry( + ( + SceneEntityRegistration( + ref=cube, + state_provider=selected_pose_provider, + semantic_type="cube", + default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp}, + geometry_provider=(_GeometryProvider() if dynamic_collision else None), + collision_role=( + SceneCollisionRole.DYNAMIC + if dynamic_collision + else SceneCollisionRole.NONE + ), + ), + SceneEntityRegistration( + ref=grasp, + parent=cube, + native_name="grasp", + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="grasp-v1", + relative_pose=torch.eye(4), + ), + ), + collision_world_mode=( + SceneCollisionWorldMode.PER_ENV if dynamic_collision else None + ), + ) + + +def _robot_profile( + profile_id: str = "fake_robot", + *, + safe_motion_policy: MotionPolicy | None = None, +) -> RobotSkillProfile: + """Build the declarative resource graph used by the fake backend.""" + return RobotSkillProfile( + profile_id=profile_id, + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + "grasp": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor((0.0,)), + grasp=torch.tensor((1.0,)), + ) + }, + presets={ + "safe": SkillPolicyPreset( + "safe", + motion_policy=safe_motion_policy, + ) + }, + default_preset="safe", + ) + + +def _parallel_articulation_scene_registry() -> SceneRegistry: + """Build one drawer whose exact joint key is statically discoverable.""" + drawer = SceneArticulationRef("drawer") + handle = SceneAffordanceRef("drawer_handle") + return SceneRegistry( + ( + SceneEntityRegistration( + ref=drawer, + state_provider=_PoseProvider(), + semantic_type="drawer", + default_affordances={ + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: handle + }, + ), + SceneEntityRegistration( + ref=handle, + state_provider=_PoseProvider(), + parent=drawer, + native_name="handle", + affordance=ArticulationOperationAffordance( + joint_id="drawer_slide", + operation_axis=torch.tensor((1.0, 0.0, 0.0)), + semantic_targets={ + "open": ArticulationOperationTarget( + target_position=0.4, + displacement=0.35, + ) + }, + ), + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision="drawer-operation-v1", + ), + ) + ) + + +def _parallel_articulation_profile() -> RobotSkillProfile: + """Build two physically disjoint resources that can address one drawer.""" + + def resource(resource_id: str) -> RobotResource: + return RobotResource( + resource_id=resource_id, + endpoints={ + "motion": ControlPartEndpoint( + control_part=f"{resource_id}_arm", + capabilities=frozenset( + {CARTESIAN_POSE_CAPABILITY, JOINT_POSITION_CAPABILITY} + ), + ), + "interaction": ControlPartEndpoint( + control_part=f"{resource_id}_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + + return RobotSkillProfile( + profile_id="parallel_articulation_robot", + resources={ + "left": resource("left"), + "right": resource("right"), + }, + command_profiles={ + hand: ControlPartCommandProfile.joint_positions( + open=torch.tensor((0.0,)), + grasp=torch.tensor((1.0,)), + ) + for hand in ("left_hand", "right_hand") + }, + presets={"safe": SkillPolicyPreset("safe")}, + default_preset="safe", + ) + + +def _parallel_articulation_engine( + profile: RobotSkillProfile, +) -> AtomicActionEngine: + """Build the disjoint four-control-part engine used only for preflight.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 4 + robot.control_parts = { + "left_arm": object(), + "left_hand": object(), + "right_arm": object(), + "right_hand": object(), + } + robot.get_qpos.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + robot.get_qvel.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + joint_ids = { + "left_arm": [0], + "left_hand": [1], + "right_arm": [2], + "right_hand": [3], + } + robot.get_joint_ids.side_effect = lambda name: joint_ids[name] + robot.get_solver.return_value = object() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "fake_planner" + return AtomicActionEngine(generator, skill_profile=profile) + + +def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: + """Build a CPU-only engine around a minimal typed robot surface.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = _ROBOT_DOF + robot.control_parts = {"arm": object(), "hand": object()} + robot.get_qpos.return_value = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) + robot.get_qvel.return_value = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) + robot.get_joint_ids.side_effect = lambda name: {"arm": [0], "hand": [1]}[name] + robot.get_solver.return_value = object() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "fake_planner" + return AtomicActionEngine(generator, skill_profile=profile) + + +class _FakeEnvironmentFactory: + """Count every explicit factory boundary used by the production adapter.""" + + scene_registry_id = "fake_scene" + robot_profile_id = "fake_robot" + + def __init__(self, *, returned_profile_id: str = "fake_robot") -> None: + self.returned_profile_id = returned_profile_id + self.calls: Counter[str] = Counter() + self.observation_samples = 0 + + def create_scene_registry(self) -> SceneRegistry: + """Create a fresh live registry.""" + self.calls["scene"] += 1 + return _scene_registry() + + def create_robot_skill_profile(self) -> RobotSkillProfile: + """Create the configured robot profile.""" + self.calls["profile"] += 1 + return _robot_profile(self.returned_profile_id) + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + """Create an engine for exactly the supplied profile.""" + self.calls["engine"] += 1 + return _engine(profile) + + def create_planning_observation_provider( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + clock: EnvironmentStepClock, + ) -> GymPlanningObservationProvider: + """Create a callback-backed Gym observation port.""" + self.calls["observation"] += 1 + scene_provider = scene_registry.make_scene_provider(batch_size=_BATCH_SIZE) + + def capture(task_state: TaskState) -> PlanningContext: + self.observation_samples += 1 + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + timestamp = clock.now() + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=engine.robot.get_qpos(), + qvel=engine.robot.get_qvel(), + ), + task=task_state, + scene=scene_provider.snapshot( + timestamp=timestamp, + env_ids=env_ids, + ), + env_ids=env_ids, + ) + + return GymPlanningObservationProvider(capture) + + def create_effect_evidence_providers( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> tuple[EffectEvidenceProvider, ...]: + """Return the fake environment's explicit evidence-provider set.""" + del scene_registry, engine, observation_provider + self.calls["evidence"] += 1 + return () + + +class _ParallelArticulationFactory(_FakeEnvironmentFactory): + """Expose two robot resources and one shared articulation write target.""" + + scene_registry_id = "parallel_articulation_scene" + robot_profile_id = "parallel_articulation_robot" + + def create_scene_registry(self) -> SceneRegistry: + self.calls["scene"] += 1 + return _parallel_articulation_scene_registry() + + def create_robot_skill_profile(self) -> RobotSkillProfile: + self.calls["profile"] += 1 + return _parallel_articulation_profile() + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + self.calls["engine"] += 1 + return _parallel_articulation_engine(profile) + + +class _DynamicCollisionFactory(_FakeEnvironmentFactory): + """Expose a safe dynamic scene backed by an unsupported planner.""" + + def __init__(self) -> None: + super().__init__() + self.pose_provider = _PoseProvider() + self.last_engine: AtomicActionEngine | None = None + + def create_scene_registry(self) -> SceneRegistry: + self.calls["scene"] += 1 + return _scene_registry( + dynamic_collision=True, + pose_provider=self.pose_provider, + ) + + def create_robot_skill_profile(self) -> RobotSkillProfile: + self.calls["profile"] += 1 + return _robot_profile( + safe_motion_policy=MotionPolicy(strategy="motion_gen"), + ) + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + self.calls["engine"] += 1 + engine = _engine(profile) + engine.motion_generator.supports_dynamic_collision_world = False + self.last_engine = engine + return engine + + +class _FakeDeclarativeEnvironment(ExpertProgramEnvironmentMixin): + """Environment surface requiring no task-level motion implementation.""" + + def __init__(self, adapter: ExpertProgramEnvironmentAdapter) -> None: + self._adapter = adapter + + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the reusable environment adapter.""" + return self._adapter + + +class _AcceptParallelSafety: + """Accept test-only merged commands after static preflight succeeds.""" + + def validate( + self, + *, + branch_frames: object, + merged_frame: object, + ) -> None: + del branch_frames, merged_frame + + +class _PresetCheckingPostPolicyPort: + """Pure test port that rejects policies outside its preset table.""" + + def __init__(self, preset_ids: tuple[str, ...]) -> None: + self._preset_ids = frozenset(preset_ids) + self.validated_presets: list[str] = [] + + def validate_policy(self, policy: object, *, segment: object) -> None: + del segment + cfg = getattr(policy, "cfg") + preset = getattr(cfg, "preset") + self.validated_presets.append(preset) + if preset not in self._preset_ids: + raise KeyError(f"Unknown settle preset {preset!r}.") + + def actions( + self, + policy: object, + *, + segment: object, + active_mask: torch.Tensor, + ) -> tuple[torch.Tensor, ...]: + del policy, segment, active_mask + raise AssertionError("Preflight must not request post-policy actions.") + + +def _program( + *, + robot_profile: str = "fake_robot", + scene_registry: str = "fake_scene", + runtime_preset: str = "safe", + schema_version: int = EXPERT_PROGRAM_SCHEMA_VERSION, + node: ProgramNodeCfg | None = None, + targets: dict[str, CyclicPoseTargetCfg] | None = None, +) -> ExpertProgramCfg: + """Build one minimal declarative pick program.""" + return ExpertProgramCfg( + schema_version=schema_version, + program_id="fake_pick", + integration=ExpertProgramIntegrationCfg( + robot_profile=robot_profile, + scene_registry=scene_registry, + runtime_preset=runtime_preset, + ), + program=(InvokeCfg(call=PickCfg(object="cube")) if node is None else node), + targets={} if targets is None else targets, + ) + + +def _program_with_later_parallel_conflict() -> ExpertProgramCfg: + """Build an early sequential call followed by conflicting branch claims.""" + return _program( + schema_version=EXPERT_PROGRAM_SCHEMA_VERSION_V2, + node=SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="cube")), + ParallelCfg( + branches=( + InvokeCfg(call=PickCfg(object="cube")), + InvokeCfg(call=PickCfg(object="cube")), + ), + barrier=BarrierCfg(name="conflicting_join"), + ), + ) + ), + ) + + +def _program_with_later_segment_hooks( + *, + post: tuple[WaitStablePostCfg, ...] = (), + validators: tuple[ObjectNearTargetValidatorCfg, ...] = (), +) -> ExpertProgramCfg: + """Build a valid pick/place flow whose hooks live on the later segment.""" + return _program( + node=SequenceCfg( + items=( + SegmentCfg( + name="pick", + steps=InvokeCfg(call=PickCfg(object="cube")), + ), + SegmentCfg( + name="place", + steps=InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop"), + ) + ), + post=post, + validators=validators, + ), + ) + ), + targets={ + "drop": CyclicPoseTargetCfg( + values=( + PoseCfg( + position=(0.4, 0.1, 0.2), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ), + ) + ) + }, + ) + + +def _parallel_articulation_program() -> ExpertProgramCfg: + """Operate one joint from disjoint resources in two parallel branches.""" + return ExpertProgramCfg( + schema_version=EXPERT_PROGRAM_SCHEMA_VERSION_V2, + program_id="conflicting_drawer_operations", + integration=ExpertProgramIntegrationCfg( + robot_profile="parallel_articulation_robot", + scene_registry="parallel_articulation_scene", + runtime_preset="safe", + ), + targets={}, + program=ParallelCfg( + branches=tuple( + InvokeCfg( + call=OperateArticulationCfg( + articulation="drawer", + target="open", + resources={"primary": resource_id}, + ) + ) + for resource_id in ("left", "right") + ), + barrier=BarrierCfg(name="drawer_join"), + ), + ) + + +def test_mixin_compiles_and_assembles_bridge_without_task_motion_code() -> None: + """One adapter property implements both EmbodiedEnv integration hooks.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + env = _FakeDeclarativeEnvironment(adapter) + + assert isinstance(adapter, SkillRuntimeAssemblyPort) + + compiled = env.compile_expert_program(_program()) + + assert factory.calls == Counter(scene=1) + bridge = env.create_expert_program_bridge(compiled) + assert isinstance(bridge, AtomicDemoBridge) + assert factory.calls == Counter( + scene=2, + profile=1, + engine=1, + observation=1, + evidence=1, + ) + segment_iterator = bridge.iter_segments() + segment = next(segment_iterator) + assert segment.name == "invoke:pick" + assert segment.failure_policy == "row_independent" + segment_iterator.close() + + +def test_later_sequential_resource_error_fails_before_observation_or_action() -> None: + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile( + _program( + node=SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="cube")), + InvokeCfg( + call=PickCfg( + object="cube", + resources={"primary": "missing"}, + ) + ), + ) + ) + ) + ) + + with pytest.raises(SemanticValidationError) as error: + adapter.create_bridge(compiled) + + assert error.value.diagnostic.code == "unknown_resource" + assert factory.calls == Counter(scene=2, profile=1, engine=1) + assert factory.observation_samples == 0 + + +def test_later_post_policy_requires_port_before_runtime_assembly() -> None: + """A later hook cannot defer its missing-port error until segment execution.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile( + _program_with_later_segment_hooks( + post=(WaitStablePostCfg(entity="cube", preset="fast"),), + ) + ) + + with pytest.raises(DemoBridgeError, match="SegmentPostPolicyPort"): + adapter.create_bridge(compiled) + + assert factory.calls == Counter(scene=1) + assert factory.observation_samples == 0 + + +def test_later_validator_requires_port_before_runtime_assembly() -> None: + """A later validator must have an installed pure-validation boundary.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile( + _program_with_later_segment_hooks( + validators=( + ObjectNearTargetValidatorCfg( + object="cube", + target="drop", + ), + ), + ) + ) + + with pytest.raises(DemoBridgeError, match="SegmentValidatorPort"): + adapter.create_bridge(compiled) + + assert factory.calls == Counter(scene=1) + assert factory.observation_samples == 0 + + +def test_later_unknown_settle_preset_fails_during_pure_preflight() -> None: + """Every declared preset is checked before semantic or live runtime assembly.""" + factory = _FakeEnvironmentFactory() + post_policy_port = _PresetCheckingPostPolicyPort(("fast",)) + adapter = ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + post_policy_port=post_policy_port, + ) + compiled = adapter.compile( + _program_with_later_segment_hooks( + post=(WaitStablePostCfg(entity="cube", preset="missing"),), + ) + ) + + with pytest.raises(KeyError, match="Unknown settle preset 'missing'"): + adapter.create_bridge(compiled) + + assert post_policy_port.validated_presets == ["missing"] + assert factory.calls == Counter(scene=1) + assert factory.observation_samples == 0 + + +def test_preflight_preserves_pick_target_lookahead_across_explicit_segments( + monkeypatch: pytest.MonkeyPatch, +) -> None: + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + config = _program( + node=SequenceCfg( + items=( + SegmentCfg( + name="pick", + steps=InvokeCfg(call=PickCfg(object="cube")), + ), + SegmentCfg( + name="place", + steps=InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop"), + ) + ), + ), + ) + ), + targets={ + "drop": CyclicPoseTargetCfg( + values=( + PoseCfg( + position=(0.4, 0.1, 0.2), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ), + ) + ) + }, + ) + workflows: list[object] = [] + original_analyze = SemanticSkillCompiler.analyze + + def record_analyze( + compiler: SemanticSkillCompiler, + calls: object, + **kwargs: object, + ) -> object: + workflow = original_analyze(compiler, calls, **kwargs) + workflows.append(workflow) + return workflow + + monkeypatch.setattr(SemanticSkillCompiler, "analyze", record_analyze) + + adapter.create_bridge(adapter.compile(config)) + + assert len(workflows) == 1 + workflow = workflows[0] + assert len(workflow.calls) == 2 # type: ignore[attr-defined] + downstream = workflow.calls[0].downstream_object_targets # type: ignore[attr-defined] + assert len(downstream) == 1 + assert downstream[0].pose is not None + torch.testing.assert_close( + downstream[0].pose.position, + torch.tensor((0.4, 0.1, 0.2)), + ) + assert factory.observation_samples == 0 + + +def test_parallel_program_requires_safety_validator_before_first_action() -> None: + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile(_program_with_later_parallel_conflict()) + + with pytest.raises(ValueError, match="ParallelCommandSafetyValidator"): + adapter.create_bridge(compiled) + + assert factory.observation_samples == 0 + + +def test_later_parallel_claim_conflict_fails_during_whole_program_preflight() -> None: + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + parallel_safety_validator=_AcceptParallelSafety(), + ) + compiled = adapter.compile(_program_with_later_parallel_conflict()) + + with pytest.raises(ValueError, match="overlapping resource claims"): + adapter.create_bridge(compiled) + + assert factory.observation_samples == 0 + + +def test_parallel_symbolic_write_conflict_fails_before_observation_or_action() -> None: + factory = _ParallelArticulationFactory() + adapter = ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + parallel_safety_validator=_AcceptParallelSafety(), + ) + compiled = adapter.compile(_parallel_articulation_program()) + materialized = compiled.materialize() + parallel_block = tuple(materialized.iter_segments())[0].parallel_block + assert parallel_block is not None + expected_path = parallel_block.branches[1].source_path + + with pytest.raises(SemanticValidationError) as error: + adapter.create_bridge(compiled) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "parallel_symbolic_write_conflict" + assert diagnostic.path == expected_path + assert "articulation_joint['drawer', 'drawer_slide']" in diagnostic.message + assert factory.observation_samples == 0 + + +def test_runtime_assembly_shares_exact_bound_components() -> None: + """Compiler, runtime, clock, sink, scene, and profile form one ownership graph.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + + assembly = adapter.assemble_runtime(_program().integration) + + assert assembly.compiler.integration.scene_registry is assembly.scene_registry + assert assembly.compiler.integration.manifest is assembly.manifest + assert assembly.compiler.integration.engine is assembly.engine + assert assembly.compiler.integration.manifest.runtime_preset == "safe" + assert assembly.compiler.integration.robot_profile.engine is assembly.engine + assert assembly.runtime.compiler is assembly.compiler + assert assembly.runtime.clock is assembly.clock + assert assembly.command_sink.clock is assembly.clock + assert assembly.clock.step_dt == pytest.approx(_STEP_DT) + assert assembly.evidence_collector.registry.providers == {} + + +def test_safe_dynamic_collision_fails_before_observation_planning_or_command() -> None: + factory = _DynamicCollisionFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile(_program(runtime_preset="safe")) + + with pytest.raises(SemanticValidationError) as error: + adapter.create_bridge(compiled) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "safe_dynamic_collision_unsupported" + assert diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "motion_policy", + "dynamic_collision_mode", + ) + assert factory.calls == Counter(scene=2, profile=1, engine=1) + assert factory.pose_provider.calls == 0 + assert factory.observation_samples == 0 + assert factory.last_engine is not None + factory.last_engine.motion_generator.generate.assert_not_called() + + +@pytest.mark.parametrize( + ("field", "value", "match"), + ( + ("robot_profile", "other_robot", "selects robot_profile"), + ("scene_registry", "other_scene", "selects scene_registry"), + ), +) +def test_integration_id_mismatch_fails_before_live_factory_access( + field: str, + value: str, + match: str, +) -> None: + """Static selection drift never reaches simulation or motion factories.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + options = {field: value} + + with pytest.raises(ValueError, match=match): + adapter.compile(_program(**options)) + + assert factory.calls == Counter() + + +def test_robot_profile_factory_drift_fails_before_engine_creation() -> None: + """The declared profile ID must match the concrete factory output.""" + factory = _FakeEnvironmentFactory(returned_profile_id="different_robot") + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + + with pytest.raises(ValueError, match="profile declaration drifted"): + adapter.assemble_runtime(_program().integration) + + assert factory.calls == Counter(scene=1, profile=1) + + +def test_factory_selection_declaration_drift_fails_before_live_access() -> None: + """A mutable factory cannot silently change IDs after adapter creation.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + factory.scene_registry_id = "changed_scene" + + with pytest.raises(ValueError, match="scene registry declaration drifted"): + adapter.compile(_program()) + + assert factory.calls == Counter() + + +def test_unknown_runtime_preset_fails_during_manifest_assembly() -> None: + """Runtime preset names are validated against the selected robot profile.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + + with pytest.raises(ValueError, match="Unknown runtime preset"): + adapter.assemble_runtime(_program(runtime_preset="unregistered").integration) + + +def test_factory_protocol_is_required() -> None: + """Loose objects cannot enter the production assembly boundary.""" + with pytest.raises(TypeError, match="ExpertProgramEnvironmentFactory"): + ExpertProgramEnvironmentAdapter(object(), step_dt=_STEP_DT) diff --git a/tests/gym/envs/expert_program/test_loader.py b/tests/gym/envs/expert_program/test_loader.py new file mode 100644 index 000000000..4ef5c4bb6 --- /dev/null +++ b/tests/gym/envs/expert_program/test_loader.py @@ -0,0 +1,252 @@ +# ---------------------------------------------------------------------------- +# 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 strict serialized Expert Program loading.""" + +from __future__ import annotations + +import json +from pathlib import Path + +import pytest +import yaml + +from embodichain.lab.gym.envs.expert_program import ( + ConfigPath, + ExpertProgramDecodeError, + ExpertProgramValidationError, + InvokeCfg, + PickCfg, + load_expert_program, + loads_expert_program_json, + parse_expert_program_json, +) + + +def _program_data(*, schema_version: int = 1) -> dict[str, object]: + """Return one minimal complete Expert Program JSON value.""" + program: dict[str, object] = { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + } + if schema_version == 2: + program = { + "kind": "parallel", + "branches": [ + program, + { + "kind": "invoke", + "call": {"kind": "pick", "object": "other_cube"}, + }, + ], + "barrier": { + "kind": "barrier", + "name": "both_picked", + "timeout_steps": 20, + "failure_policy": "fail_fast", + }, + } + return { + "schema_version": schema_version, + "program_id": "loader_pick", + "integration": { + "robot_profile": "test_robot", + "scene_registry": "test_scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": program, + } + + +def _program_json() -> str: + """Serialize the minimal program using standards-compliant JSON.""" + return json.dumps(_program_data()) + + +class _RejectingValidationContext: + """Reject integration references after recording their exact path.""" + + def __init__(self) -> None: + self.integration_paths: list[ConfigPath] = [] + + def validate_integration( + self, + integration: object, + *, + path: ConfigPath, + ) -> None: + del integration + self.integration_paths.append(path) + raise KeyError("unavailable integration") + + def validate_semantic_call(self, call: object, *, path: ConfigPath) -> None: + del call, path + + def validate_scene_reference( + self, + reference: str, + *, + role: str, + path: ConfigPath, + ) -> None: + del reference, role, path + + def validate_post_policy(self, policy: object, *, path: ConfigPath) -> None: + del policy, path + + def validate_validator(self, validator: object, *, path: ConfigPath) -> None: + del validator, path + + +def test_parse_expert_program_json_preserves_predecode_mapping() -> None: + value = parse_expert_program_json('{"host_integration_pending": [true, null, 3.5]}') + + assert value == {"host_integration_pending": [True, None, 3.5]} + + +@pytest.mark.parametrize("response", ["[]", "null", '"program"']) +def test_parse_expert_program_json_requires_top_level_mapping(response: str) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + parse_expert_program_json(response) + + assert error.value.code == "expected_mapping" + assert error.value.path == () + + +def test_loads_expert_program_json_decodes_one_plain_document() -> None: + config = loads_expert_program_json(f"\n{_program_json()}\t") + + assert type(config.program) is InvokeCfg + assert type(config.program.call) is PickCfg + assert config.program.call.object == "cube" + + +@pytest.mark.parametrize("suffix", [".json", ".yaml"]) +@pytest.mark.parametrize("schema_version", [1, 2]) +def test_load_expert_program_forwards_validation_context_for_each_format( + tmp_path: Path, + suffix: str, + schema_version: int, +) -> None: + data = _program_data(schema_version=schema_version) + serialized = json.dumps(data) if suffix == ".json" else yaml.safe_dump(data) + path = tmp_path / f"program{suffix}" + path.write_text(serialized, encoding="utf-8") + context = _RejectingValidationContext() + + with pytest.raises(ExpertProgramValidationError) as error: + load_expert_program(path, validation_context=context) + + assert error.value.code == "reference_validation_failed" + assert error.value.path == ("integration",) + assert context.integration_paths == [("integration",)] + + +def test_loads_expert_program_json_rejects_nested_duplicate_keys() -> None: + duplicate = _program_json().replace( + '"object": "cube"', + '"object": "cube", "object": "other"', + ) + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(duplicate) + + assert error.value.code == "duplicate_json_key" + + +@pytest.mark.parametrize( + "invalid_response", + [ + "```json\n{}\n```", + f"{_program_json()} trailing text", + f"{_program_json()} {_program_json()}", + ], +) +def test_loads_expert_program_json_requires_one_unfenced_document( + invalid_response: str, +) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(invalid_response) + + assert error.value.code == "invalid_json" + + +@pytest.mark.parametrize("number", ["NaN", "Infinity", "-Infinity", "1e400"]) +def test_loads_expert_program_json_rejects_non_finite_numbers(number: str) -> None: + response = f'{{"value": {number}}}' + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(response) + + assert error.value.code == "non_finite_number" + + +def test_loads_expert_program_json_enforces_utf8_byte_limit() -> None: + response = _program_json() + too_small = len(response.encode("utf-8")) - 1 + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(response, max_bytes=too_small) + + assert error.value.code == "input_too_large" + + +def test_loads_expert_program_json_normalizes_invalid_utf8_text() -> None: + response = "\ud800" + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(response) + + assert error.value.code == "invalid_utf8" + + +def test_loads_expert_program_json_rejects_escaped_unpaired_surrogate() -> None: + response = _program_json().replace("loader_pick", r"\ud800") + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(response) + + assert error.value.code == "invalid_utf8" + + +def test_loads_expert_program_json_accepts_escaped_surrogate_pair() -> None: + response = _program_json().replace("loader_pick", r"\ud83d\ude00") + + config = loads_expert_program_json(response) + + assert config.program_id == "😀" + + +def test_loads_expert_program_json_normalizes_oversized_integer() -> None: + data = _program_data() + data["targets"] = { + "goal": { + "kind": "cyclic_pose", + "values": [ + { + "position": [10**400, 0, 0], + "quaternion_wxyz": [1, 0, 0, 0], + } + ], + } + } + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(json.dumps(data)) + + assert error.value.code == "invalid_value" + assert error.value.path == ("targets", "goal", "values", 0) diff --git a/tests/gym/envs/expert_program/test_parallel_compiler.py b/tests/gym/envs/expert_program/test_parallel_compiler.py new file mode 100644 index 000000000..84c678c3e --- /dev/null +++ b/tests/gym/envs/expert_program/test_parallel_compiler.py @@ -0,0 +1,221 @@ +# ---------------------------------------------------------------------------- +# 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 provider-free schema-v2 parallel program compilation.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + BarrierCfg, + ExpertProgramCfg, + ExpertProgramCompileError, + ExpertProgramCompiler, + ExpertProgramIntegrationCfg, + InvokeCfg, + ParallelCfg, + PickCfg, + RepeatCfg, + SegmentCfg, + SequenceCfg, +) +from embodichain.lab.sim.atomic_actions import EntityState +from embodichain.lab.sim.skills.calls import Pick +from embodichain.lab.sim.skills.scene import ( + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + + +class _NeverObserveProvider: + """Reject dynamic observation during provider-free compilation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + raise AssertionError("Compilation must not observe the scene.") + + +def _compiler() -> ExpertProgramCompiler: + provider = _NeverObserveProvider() + registry = SceneRegistry( + tuple( + SceneEntityRegistration( + ref=SceneObjectRef(entity_id), + state_provider=provider, + ) + for entity_id in ("left_cube", "right_cube") + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def _integration() -> ExpertProgramIntegrationCfg: + return ExpertProgramIntegrationCfg( + robot_profile="dual_arm", + scene_registry="scene", + runtime_preset="safe", + ) + + +def _parallel() -> ParallelCfg: + return ParallelCfg( + branches=( + SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="left_cube")), + InvokeCfg(call=PickCfg(object="left_cube")), + ) + ), + RepeatCfg( + count=2, + body=InvokeCfg(call=PickCfg(object="right_cube")), + ), + ), + barrier=BarrierCfg( + name="both_arms_done", + timeout_steps=240, + failure_policy="fail_fast", + ), + ) + + +def _config(program: ParallelCfg | SegmentCfg | SequenceCfg) -> ExpertProgramCfg: + return ExpertProgramCfg( + schema_version=2, + program_id="parallel_pick", + integration=_integration(), + program=program, + targets={}, + ) + + +def test_parallel_compiles_independent_ordered_lanes_and_explicit_join() -> None: + segment = tuple(_compiler().compile(_config(_parallel())))[0] + + assert segment.implicit + assert segment.name == "parallel:both_arms_done" + assert segment.parallel_block is not None + block = segment.parallel_block + assert block.barrier.name == "both_arms_done" + assert block.barrier.timeout_steps == 240 + assert block.barrier.failure_policy == "fail_fast" + assert tuple(branch.branch_index for branch in block.branches) == (0, 1) + assert tuple(len(branch.calls) for branch in block.branches) == (2, 2) + assert tuple(call.call_index for call in segment.calls) == (0, 1, 2, 3) + assert tuple(call.segment_call_index for call in segment.calls) == (0, 1, 2, 3) + assert segment.calls == tuple( + call for branch in block.branches for call in branch.calls + ) + assert all(type(call.call) is Pick for call in segment.calls) + assert tuple(call.call.object.entity_id for call in block.branches[0].calls) == ( + "left_cube", + "left_cube", + ) + assert tuple(call.call.object.entity_id for call in block.branches[1].calls) == ( + "right_cube", + "right_cube", + ) + assert tuple( + frame.iteration_index + for call in block.branches[1].calls + for frame in call.repeat_frames + ) == (0, 1) + + +def test_segment_may_wrap_one_parallel_block() -> None: + segment = tuple( + _compiler().compile(_config(SegmentCfg(name="dual_pick", steps=_parallel()))) + )[0] + + assert not segment.implicit + assert segment.name == "dual_pick" + assert segment.parallel_block is not None + assert len(segment.calls) == 4 + + +def test_materialized_analysis_stops_sequential_lookahead_at_parallel_barriers() -> ( + None +): + config = _config( + SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="left_cube")), + _parallel(), + InvokeCfg(call=PickCfg(object="left_cube")), + ) + ) + ) + + program = _compiler().compile(config).materialize() + analyses = program.preflight_analyses() + + assert [analysis.kind for analysis in analyses] == [ + "sequential_stretch", + "parallel_branch", + "parallel_branch", + "sequential_stretch", + ] + assert [analysis.segment_indices for analysis in analyses] == [ + (0,), + (1,), + (1,), + (2,), + ] + assert program.sequential_execution_analysis(0).segment_indices == (0,) + assert program.sequential_execution_analysis(2).segment_indices == (2,) + with pytest.raises(ValueError, match="Parallel segments"): + program.sequential_execution_analysis(1) + + +def test_parallel_branch_rejects_segment_owned_lifecycle() -> None: + invoke = InvokeCfg(call=PickCfg(object="left_cube")) + parallel = ParallelCfg( + branches=( + SegmentCfg(name="branch", steps=invoke), + invoke, + ), + barrier=BarrierCfg(name="join"), + ) + + with pytest.raises(ValueError, match="wrap the Parallel node in one Segment"): + _config(parallel) + + +def test_segment_rejects_mixed_sequential_and_parallel_tree() -> None: + invoke = InvokeCfg(call=PickCfg(object="left_cube")) + config = _config( + SegmentCfg( + name="ambiguous_boundary", + steps=SequenceCfg(items=(invoke, _parallel())), + ) + ) + + with pytest.raises( + ExpertProgramCompileError, + match="either a call-only program or one direct Parallel", + ): + _compiler().compile(config) + + +__all__: list[str] = [] diff --git a/tests/gym/envs/expert_program/test_parallel_schema.py b/tests/gym/envs/expert_program/test_parallel_schema.py new file mode 100644 index 000000000..b34ffcb69 --- /dev/null +++ b/tests/gym/envs/expert_program/test_parallel_schema.py @@ -0,0 +1,137 @@ +# ---------------------------------------------------------------------------- +# 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 Expert Program schema Version 2 parallel nodes.""" + +from __future__ import annotations + +import pytest + +from embodichain.lab.gym.envs.expert_program.cfg import ( + BarrierCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + InvokeCfg, + ParallelCfg, + PickCfg, +) +from embodichain.lab.gym.envs.expert_program.decoder import ( + ExpertProgramDecodeError, + decode_expert_program, +) + + +def _payload(*, schema_version: int = 2) -> dict[str, object]: + return { + "schema_version": schema_version, + "program_id": "parallel_pick", + "integration": { + "robot_profile": "dual_arm", + "scene_registry": "scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": { + "kind": "parallel", + "branches": [ + { + "kind": "invoke", + "call": {"kind": "pick", "object": "left_cube"}, + }, + { + "kind": "invoke", + "call": {"kind": "pick", "object": "right_cube"}, + }, + ], + "barrier": { + "kind": "barrier", + "name": "both_picked", + "timeout_steps": 200, + "failure_policy": "fail_fast", + }, + }, + } + + +def test_decode_schema_v2_parallel_with_explicit_barrier() -> None: + config = decode_expert_program(_payload()) + + assert config.schema_version == 2 + assert type(config.program) is ParallelCfg + assert len(config.program.branches) == 2 + assert config.program.barrier == BarrierCfg( + name="both_picked", + timeout_steps=200, + failure_policy="fail_fast", + ) + + +def test_schema_v1_rejects_parallel_discriminator() -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(_payload(schema_version=1)) + + assert error.value.code == "unknown_discriminator" + assert error.value.path == ("program", "kind") + + +def test_parallel_requires_two_branches_and_explicit_barrier() -> None: + payload = _payload() + program = payload["program"] + assert type(program) is dict + program["branches"] = program["branches"][:1] # type: ignore[index] + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(payload) + assert error.value.code == "parallel_branch_count" + + payload = _payload() + program = payload["program"] + assert type(program) is dict + del program["barrier"] + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(payload) + assert error.value.code == "missing_field" + assert error.value.path == ("program", "barrier") + + +def test_barrier_is_not_valid_as_a_standalone_program() -> None: + with pytest.raises(ValueError, match="only be owned by Parallel"): + ExpertProgramCfg( + schema_version=2, + program_id="invalid_barrier", + integration=ExpertProgramIntegrationCfg( + robot_profile="profile", + scene_registry="scene", + runtime_preset="safe", + ), + targets={}, + program=BarrierCfg(name="orphan"), + ) + + +def test_parallel_cfg_rejects_nested_parallel() -> None: + invoke = InvokeCfg(call=PickCfg(object="cube")) + nested = ParallelCfg( + branches=(invoke, invoke), + barrier=BarrierCfg(name="inner"), + ) + with pytest.raises(ValueError, match="Nested Parallel"): + ParallelCfg( + branches=(nested, invoke), + barrier=BarrierCfg(name="outer"), + ) + + +__all__: list[str] = [] diff --git a/tests/gym/envs/expert_program/test_simulation.py b/tests/gym/envs/expert_program/test_simulation.py new file mode 100644 index 000000000..652df4702 --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation.py @@ -0,0 +1,436 @@ +# ---------------------------------------------------------------------------- +# 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 explicit Expert Program simulation bindings.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program.simulation import ( + AntipodalGraspAffordanceBinding, + ArticulationOperationAffordanceBinding, + ArticulationOperationTargetBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + RobotResourceBinding, + SimulationArticulationBinding, + SimulationArticulationLinkBinding, + SimulationRigidObjectBinding, + SimulationResourceEndpointBinding, + SimulationRobotResourceBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, +) +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + ArticulationOperationAffordance, + CARTESIAN_POSE_CAPABILITY, + GRASP_CAPABILITY, +) +from embodichain.lab.sim.skills import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + GRASP_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneArticulationRef, + SceneDynamics, + SceneLinkRef, + SceneObjectRef, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.profiles import ResourceEndpoint + +_BATCH_SIZE = 2 +_OPEN_TARGET = 0.42 +_OPEN_DISPLACEMENT = 0.4 + + +class _RigidObject: + """Minimal selected rigid object with a batched triangle mesh.""" + + def __init__(self) -> None: + self.pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + self.vertices = torch.tensor( + ( + (0.0, 0.0, 0.0), + (1.0, 0.0, 0.0), + (0.0, 1.0, 0.0), + ), + dtype=torch.float32, + ).repeat(_BATCH_SIZE, 1, 1) + self.triangles = torch.tensor( + (((0, 1, 2),),), + dtype=torch.int32, + ).repeat(_BATCH_SIZE, 1, 1) + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix is True + return self.pose + + def get_vertices( + self, + env_ids: list[int], + *, + scale: bool, + ) -> torch.Tensor: + assert scale is True + return self.vertices[env_ids] + + def get_triangles(self, env_ids: list[int]) -> torch.Tensor: + return self.triangles[env_ids] + + +class _Articulation: + """Minimal articulation exposing exact joint and link lookup surfaces.""" + + joint_names = ("drawer_slide",) + link_names = ("drawer_handle",) + + def __init__(self) -> None: + self.pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + self.qpos = torch.tensor(((0.1,), (0.2,)), dtype=torch.float32) + self.link_pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + self.link_pose[:, 0, 3] = torch.tensor((0.3, 0.4)) + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix is True + return self.pose + + def get_qpos(self, *, target: bool) -> torch.Tensor: + assert target is False + return self.qpos + + def get_link_pose( + self, + link_name: str, + *, + env_ids: list[int], + to_matrix: bool, + ) -> torch.Tensor: + assert link_name == "drawer_handle" + assert to_matrix is True + return self.link_pose[env_ids] + + +class _Simulation: + """Explicit native-UID lookup fixture.""" + + def __init__(self) -> None: + self.rigid_object = _RigidObject() + self.articulation = _Articulation() + + def get_rigid_object(self, uid: str) -> _RigidObject | None: + return self.rigid_object if uid == "native_cube" else None + + def get_articulation(self, uid: str) -> _Articulation | None: + return self.articulation if uid == "native_drawer" else None + + +class _Robot: + """Minimal robot control-part lookup fixture.""" + + control_parts = {"arm": object(), "hand": object()} + + def get_joint_ids(self, *, name: str) -> list[int]: + return {"arm": [0, 1], "hand": [2]}[name] + + +@dataclass(frozen=True, slots=True) +class _MobileEndpoint(ResourceEndpoint): + """Test-only non-joint endpoint declaration.""" + + controller_id: str + + +def _scene_binding() -> SimulationSceneBinding: + """Build one cube-and-drawer binding using only typed declarations.""" + return SimulationSceneBinding( + registry_id="tabletop", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="native_cube", + aliases=("perceived_cube",), + dynamics=SceneDynamics.DYNAMIC, + semantic_type="cube", + default_grasp_affordance="cube_grasp", + ), + ), + articulations=( + SimulationArticulationBinding( + entity_id="drawer", + simulation_uid="native_drawer", + semantic_type="drawer", + default_operation_affordance="drawer_handle_operation", + ), + ), + links=( + SimulationArticulationLinkBinding( + entity_id="drawer_handle_link", + articulation_id="drawer", + native_link_name="drawer_handle", + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id="cube_grasp", + object_id="cube", + native_name="mesh_antipodal", + revision="cube-grasp-v1", + ), + ), + articulation_operations=( + ArticulationOperationAffordanceBinding( + entity_id="drawer_handle_operation", + articulation_id="drawer", + link_id="drawer_handle_link", + joint_id="drawer_slide", + revision="drawer-operation-v1", + semantic_targets={ + "open": ArticulationOperationTargetBinding( + target_position=_OPEN_TARGET, + displacement=_OPEN_DISPLACEMENT, + ) + }, + ), + ), + ) + + +def _profile_binding() -> SimulationRobotSkillProfileBinding: + """Build one manipulation profile declaration.""" + return SimulationRobotSkillProfileBinding( + profile_id="test_robot", + resources=( + ControlPartResourceBinding( + resource_id="manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + ControlPartEndpointBinding( + endpoint_id="grasp", + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="parallel_gripper", + ), + ), + ), + ), + command_presets=( + ControlPartCommandPreset( + preset_id="parallel_gripper", + control_part="hand", + commands={"open": (0.0,), "grasp": (1.0,)}, + ), + ), + defaults={"pick_up": {"primary": "manipulator"}}, + presets=(SkillPolicyPreset("safe"),), + default_preset="safe", + ) + + +def test_scene_binding_builds_existing_registry_contracts() -> None: + simulation = _Simulation() + + registry = _scene_binding().build(simulation) # type: ignore[arg-type] + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor((0, 1), dtype=torch.long), + ) + + assert registry.resolve("perceived_cube") == SceneObjectRef("cube") + assert registry.resolve("drawer") == SceneArticulationRef("drawer") + assert registry.resolve("drawer_handle_link") == SceneLinkRef("drawer_handle_link") + grasp_ref = registry.resolve_affordance( + "cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + assert grasp_ref == SceneAffordanceRef("cube_grasp") + grasp = registry.lookup(grasp_ref).affordance + assert isinstance(grasp, AntipodalAffordance) + assert grasp.mesh_vertices is not None and grasp.mesh_vertices.shape == (3, 3) + operation_ref = registry.resolve_affordance( + "drawer", + capability=ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ) + operation = registry.lookup(operation_ref).affordance + assert isinstance(operation, ArticulationOperationAffordance) + assert operation.joint_id == "drawer_slide" + assert operation.semantic_targets["open"].target_position == pytest.approx( + _OPEN_TARGET + ) + assert torch.equal( + snapshot.articulation_joints[("drawer", "drawer_slide")].position, + simulation.articulation.qpos, + ) + assert torch.equal( + snapshot.entities["drawer_handle_operation"].pose, + simulation.articulation.link_pose, + ) + + +def test_scene_binding_fails_closed_on_missing_native_entity() -> None: + binding = _scene_binding() + missing = replace( + binding.rigid_objects[0], + simulation_uid="missing_cube", + ) + + with pytest.raises(KeyError, match="missing_cube"): + replace(binding, rigid_objects=(missing,)).build( # type: ignore[arg-type] + _Simulation() + ) + + +def test_scene_binding_fails_closed_on_missing_native_link() -> None: + binding = _scene_binding() + missing = replace(binding.links[0], native_link_name="missing_handle") + + with pytest.raises(KeyError, match="missing_handle"): + replace(binding, links=(missing,)).build( # type: ignore[arg-type] + _Simulation() + ) + + +def test_scene_binding_fails_closed_on_missing_native_joint() -> None: + binding = _scene_binding() + missing = replace( + binding.articulation_operations[0], + joint_id="missing_joint", + ) + + with pytest.raises(KeyError, match="missing_joint"): + replace(binding, articulation_operations=(missing,)).build( # type: ignore[arg-type] + _Simulation() + ) + + +def test_robot_profile_binding_builds_existing_profile_contracts() -> None: + profile = _profile_binding().build(_Robot()) # type: ignore[arg-type] + + resource = profile.resources["manipulator"] + motion = resource.endpoints["motion"] + grasp = resource.endpoints["grasp"] + assert motion.control_part == "arm" + assert motion.capabilities == frozenset({CARTESIAN_POSE_CAPABILITY}) + assert grasp.control_part == "hand" + assert grasp.command_profile == "parallel_gripper" + command = profile.command_profiles["parallel_gripper"].commands["grasp"] + assert torch.equal(command.positions, torch.tensor((1.0,))) + assert profile.defaults["pick_up"].resources == {"primary": "manipulator"} + + +def test_generic_resource_binding_owns_arbitrary_typed_endpoint() -> None: + endpoint = _MobileEndpoint( + controller_id="base_controller", + capabilities=frozenset({"motion.base.velocity"}), + ) + binding = RobotResourceBinding( + resource_id="mobile_base", + endpoints={"motion": endpoint}, + ) + + assert isinstance(binding, SimulationRobotResourceBinding) + resource = binding.build(object()) # type: ignore[arg-type] + built_endpoint = resource.endpoints["motion"] + + assert isinstance(built_endpoint, _MobileEndpoint) + assert built_endpoint is not endpoint + assert built_endpoint.controller_id == "base_controller" + assert resource.members == () + + +def test_control_part_endpoint_binding_implements_public_build_protocol() -> None: + binding = ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ) + + assert isinstance(binding, SimulationResourceEndpointBinding) + + +def test_whole_body_control_part_remains_supported_and_strict() -> None: + class WholeBodyRobot: + control_parts = {"whole_body": object()} + + def get_joint_ids(self, *, name: str) -> list[int]: + assert name == "whole_body" + return [0, 1, 2, 3] + + binding = SimulationRobotSkillProfileBinding( + profile_id="whole_body_robot", + resources=( + ControlPartResourceBinding( + resource_id="body", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="whole_body", + capabilities=frozenset({"motion.whole_body"}), + ), + ), + ), + ), + ) + + profile = binding.build(WholeBodyRobot()) # type: ignore[arg-type] + endpoint = profile.resources["body"].endpoints["motion"] + + assert endpoint.control_part == "whole_body" + assert endpoint.capabilities == frozenset({"motion.whole_body"}) + + +def test_robot_profile_binding_fails_closed_on_missing_control_part() -> None: + binding = _profile_binding() + resource = binding.resources[0] + missing_endpoint = replace( + resource.endpoints[0], + control_part="missing_arm", + ) + + with pytest.raises(KeyError, match="missing_arm"): + replace( + binding, + resources=( + replace( + resource, + endpoints=(missing_endpoint, resource.endpoints[1]), + ), + ), + ).build( + _Robot() + ) # type: ignore[arg-type] + + +def test_robot_profile_binding_rejects_wrong_command_width() -> None: + binding = _profile_binding() + invalid = replace( + binding.command_presets[0], + commands={"open": (0.0, 0.0), "grasp": (1.0, 1.0)}, + ) + + with pytest.raises(ValueError, match="has 2 positions.*has 1 joints"): + replace(binding, command_presets=(invalid,)).build( # type: ignore[arg-type] + _Robot() + ) diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py new file mode 100644 index 000000000..48c37b8f6 --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -0,0 +1,1899 @@ +# ---------------------------------------------------------------------------- +# 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 reusable simulation-backed Expert Program assembly.""" + +from __future__ import annotations + +import ast +from collections.abc import Mapping, Sequence +from dataclasses import dataclass, fields, is_dataclass +import inspect +import textwrap +from types import MethodType, SimpleNamespace +from typing import Any, ClassVar +from unittest.mock import MagicMock + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + AntipodalGraspAffordanceBinding, + ControlCommandStateEvidenceTracker, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramCompiler, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + ExpertProgramEnvironmentAdapter, + ExpertProgramRuntimeAssembly, + HandOverCfg, + InvokeCfg, + RobotResourceBinding, + SharedTickSceneProvider, + SimulationExpertProgramFactory, + SimulationPlanningObservationProvider, + SimulationRigidObjectBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, + decode_expert_program, +) +from embodichain.lab.gym.envs.expert_program.bridge import EnvironmentStepClock +from embodichain.lab.sim.atomic_actions import ( + ActionInvocation, + Affordance, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + CommandAcknowledgement, + EntityState, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + HeldObjectState, + MotionPolicy, + ObservedArticulationJointState, + PlanningContext, + StateDelta, + TaskState, + TimedTrajectory, +) +from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.bindings import RuntimeEndpointTarget +from embodichain.lab.sim.atomic_actions.control import ControlPartCommandProfile +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, +) +from embodichain.lab.sim.planners import MotionGenerator +from embodichain.lab.sim.skills import ( + AtomicSkills, + BoundSemanticCall, + EndpointResolution, + HandOver, + HandOverPoseProvider, + HandOverPoseTargets, + Pick, + Place, + RelationTargetGrounder, + ResourceEndpoint, + ResourceEndpointAdapter, + SceneArticulationRef, + SceneEntityRegistration, + SceneRegistry, + SemanticCallSpec, + SemanticObjectTarget, + SemanticPose, + SemanticRelationTarget, + SemanticValidationError, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.effects import ( + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + BinaryEffectClause, + BinaryEvidenceKind, + ControlPartEvidenceAddress, + EffectEvidenceSourceRef, + HeldObjectRelation, + HeldObjectStateExpectation, +) +from embodichain.lab.sim.skills.evidence import ( + BinaryEffectEvidenceQuery, + BinaryEffectEvidenceBatch, + BinaryEffectObservation, + EffectEvidenceCollectionContext, + PoseRelationEvidenceBatch, +) +from embodichain.lab.sim.skills.runtime import ( + SkillEffectTrace, + SkillResult, + SkillRuntime, + SkillStatus, +) +from embodichain.lab.sim.skills.scene import SceneObjectRef + +_BATCH_SIZE = 3 +_ROBOT_DOF = 2 +_STEP_DT = 0.04 +_TRACKER_ENV_IDS = torch.tensor((7, 3, 11), dtype=torch.long) +_HAND_OPEN_POSITION = 0.0 +_HAND_GRASP_POSITION = 0.8 +_HAND_INTERMEDIATE_POSITION = 0.4 +_DUAL_ROBOT_DOF = 4 +_RELEASE_SEPARATION = 0.2 +_DIRECT_PLACE_TARGET = SemanticPose( + position=(0.0, 0.0, 0.0), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), +) +_QUICKSTART_MAX_LINES = 15 + + +def _command_state_tracker() -> ControlCommandStateEvidenceTracker: + """Build a three-row tracker for one semantic gripper profile.""" + profile = ControlPartCommandProfile.joint_positions( + open=torch.tensor((_HAND_OPEN_POSITION,)), + grasp=torch.tensor((_HAND_GRASP_POSITION,)), + ) + return ControlCommandStateEvidenceTracker( + {"hand": profile}, + _TRACKER_ENV_IDS, + ) + + +def _hand_command_frame( + *, + env_ids: tuple[int, ...], + positions: tuple[float, ...], + active: tuple[bool, ...] | None = None, +) -> RuntimeCommandFrame: + """Build one row-addressed semantic hand command frame.""" + batch_size = len(env_ids) + if len(positions) != batch_size: + raise ValueError("positions must have one value per environment ID.") + if active is None: + active = (True,) * batch_size + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("hand", (1,)), + payload=JointPositionPayload( + torch.tensor(positions, dtype=torch.float32).unsqueeze(1) + ), + ), + ), + active_mask=torch.tensor(active, dtype=torch.bool), + env_ids=torch.tensor(env_ids, dtype=torch.long), + hold_duration=torch.full((batch_size,), _STEP_DT), + ) + + +def _hand_state_observation( + tracker: ControlCommandStateEvidenceTracker, + *env_ids: int, +) -> BinaryEffectObservation: + """Observe command-state evidence in an explicit stable-ID order.""" + expectation = HeldObjectStateExpectation( + expectation_id="held-cube", + relation=HeldObjectRelation.ATTACHED, + object_id="cube", + slot_id="primary", + resource_id="manipulator", + task_state_key="held-cube", + ) + query = BinaryEffectEvidenceQuery( + BinaryEffectClause( + clause_id="hand-constraint", + expectation_id=expectation.expectation_id, + source=EffectEvidenceSourceRef( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress("hand", CONSTRAINT_EFFECT_CHANNEL), + ), + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + expected=True, + ), + expectation, + ) + context = EffectEvidenceCollectionContext( + timestamp=0.0, + observation_revision=0, + env_ids=torch.tensor(env_ids, dtype=torch.long), + ) + return tracker.observe(query, context) + + +class _CountingEntityProvider: + """Return row-addressed poses and record every native acquisition.""" + + def __init__(self) -> None: + self.calls: list[tuple[float, torch.Tensor]] = [] + + def observe(self, *, timestamp: float, env_ids: torch.Tensor) -> EntityState: + """Return one distinct x translation for each environment ID.""" + self.calls.append((timestamp, env_ids.clone())) + pose = torch.eye(4).repeat(env_ids.numel(), 1, 1) + pose[:, 0, 3] = env_ids.to(dtype=pose.dtype) + return EntityState(pose) + + +class _CountingJointProvider: + """Return row-addressed articulation state and record acquisitions.""" + + def __init__(self) -> None: + self.calls: list[tuple[float, torch.Tensor]] = [] + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, ObservedArticulationJointState]: + """Return one scalar joint position per environment ID.""" + self.calls.append((timestamp, env_ids.clone())) + position = env_ids.to(dtype=torch.float32).unsqueeze(1) + return { + "slide": ObservedArticulationJointState( + position, + torch.ones(env_ids.numel(), dtype=torch.bool), + ) + } + + +def _shared_scene_provider() -> tuple[ + SharedTickSceneProvider, + _CountingEntityProvider, + _CountingJointProvider, +]: + """Build one full-batch registry provider with observable acquisitions.""" + entity_provider = _CountingEntityProvider() + joint_provider = _CountingJointProvider() + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=entity_provider, + joint_state_provider=joint_provider, + ), + ) + ) + delegate = registry.make_scene_provider(batch_size=_BATCH_SIZE) + full_env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + return ( + SharedTickSceneProvider(delegate, full_env_ids), + entity_provider, + joint_provider, + ) + + +def test_shared_tick_scene_provider_projects_partial_rows_without_resampling() -> None: + """Planning full batch and evidence subsets share one native acquisition.""" + provider, entity_provider, joint_provider = _shared_scene_provider() + full_env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + + full = provider.snapshot(timestamp=0.0, env_ids=full_env_ids) + subset = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor((2, 0), dtype=torch.long), + ) + + assert len(entity_provider.calls) == 1 + assert len(joint_provider.calls) == 1 + assert torch.equal(entity_provider.calls[0][1], full_env_ids) + assert full.entities["drawer"].pose[:, 0, 3].tolist() == [0.0, 1.0, 2.0] + assert subset.entities["drawer"].pose[:, 0, 3].tolist() == [2.0, 0.0] + joint = subset.articulation_joints[("drawer", "slide")] + assert joint.position[:, 0].tolist() == [2.0, 0.0] + assert joint.valid_mask is not None and joint.valid_mask.tolist() == [True, True] + assert subset.collision_world_revision == (0, 0) + + +def test_shared_tick_scene_provider_captures_full_batch_when_subset_arrives_first() -> ( + None +): + """A partial first consumer cannot poison the delegate's stable batch.""" + provider, entity_provider, joint_provider = _shared_scene_provider() + requested = torch.tensor((1,), dtype=torch.long) + + first = provider.snapshot(timestamp=0.0, env_ids=requested) + second = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor((2, 1), dtype=torch.long), + ) + + expected_full = torch.arange(_BATCH_SIZE, dtype=torch.long) + assert torch.equal(entity_provider.calls[0][1], expected_full) + assert torch.equal(joint_provider.calls[0][1], expected_full) + assert len(entity_provider.calls) == 1 + assert first.entities["drawer"].pose[:, 0, 3].tolist() == [1.0] + assert second.entities["drawer"].pose[:, 0, 3].tolist() == [2.0, 1.0] + + +def test_shared_tick_scene_provider_rejects_unknown_or_regressing_rows() -> None: + """Unknown correlations and time regressions fail before native sampling.""" + provider, entity_provider, _ = _shared_scene_provider() + provider.snapshot( + timestamp=0.5, + env_ids=torch.tensor((0, 2), dtype=torch.long), + ) + + with pytest.raises(ValueError, match="absent from full_env_ids"): + provider.snapshot( + timestamp=0.5, + env_ids=torch.tensor((3,), dtype=torch.long), + ) + with pytest.raises(ValueError, match="monotonic"): + provider.snapshot( + timestamp=0.4, + env_ids=torch.tensor((0,), dtype=torch.long), + ) + + assert len(entity_provider.calls) == 1 + + +def test_command_state_tracker_correlates_open_and_grasp_across_subsets() -> None: + """Stable IDs, not subset row positions, own accepted gripper state.""" + tracker = _command_state_tracker() + + tracker.accepted( + _hand_command_frame( + env_ids=(3, 7), + positions=(_HAND_GRASP_POSITION, _HAND_OPEN_POSITION), + ) + ) + observation = _hand_state_observation(tracker, 7, 11, 3) + + assert tracker.tracked_control_parts == ("hand",) + assert observation.values.tolist() == [False, False, True] + assert observation.valid is not None + assert observation.valid.tolist() == [True, False, True] + assert observation.acquisition_errors[0] is None + assert observation.acquisition_errors[1] is not None + assert observation.acquisition_errors[2] is None + + +def test_command_state_tracker_preserves_intermediate_and_inactive_rows() -> None: + """Unrecognized targets and inactive rows cannot overwrite prior evidence.""" + tracker = _command_state_tracker() + tracker.accepted( + _hand_command_frame( + env_ids=(7, 3), + positions=(_HAND_OPEN_POSITION, _HAND_GRASP_POSITION), + ) + ) + + tracker.accepted( + _hand_command_frame( + env_ids=(3, 7), + positions=( + _HAND_INTERMEDIATE_POSITION, + _HAND_INTERMEDIATE_POSITION, + ), + ) + ) + after_intermediate = _hand_state_observation(tracker, 3, 7) + assert after_intermediate.values.tolist() == [True, False] + assert after_intermediate.valid is not None + assert after_intermediate.valid.tolist() == [True, True] + + tracker.accepted( + _hand_command_frame( + env_ids=(3, 7), + positions=(_HAND_OPEN_POSITION, _HAND_GRASP_POSITION), + active=(False, True), + ) + ) + after_inactive_row = _hand_state_observation(tracker, 3, 7) + assert after_inactive_row.values.tolist() == [True, True] + assert after_inactive_row.valid is not None + assert after_inactive_row.valid.tolist() == [True, True] + + +def test_command_state_tracker_cancel_invalidates_target_state() -> None: + """Cancelling a hand destination invalidates every correlated hand row.""" + tracker = _command_state_tracker() + frame = _hand_command_frame( + env_ids=(7, 3), + positions=(_HAND_OPEN_POSITION, _HAND_GRASP_POSITION), + ) + tracker.accepted(frame) + + tracker.cancelled(frame.targets) + observation = _hand_state_observation(tracker, 3, 7) + + assert observation.values.tolist() == [False, False] + assert observation.valid is not None + assert observation.valid.tolist() == [False, False] + assert all(error is not None for error in observation.acquisition_errors) + + +def test_command_state_tracker_discard_invalidates_all_state() -> None: + """A fail-closed sink discard removes every accepted row state.""" + tracker = _command_state_tracker() + tracker.accepted( + _hand_command_frame( + env_ids=(11, 3), + positions=(_HAND_GRASP_POSITION, _HAND_OPEN_POSITION), + ) + ) + + tracker.discarded() + observation = _hand_state_observation(tracker, 11, 3) + + assert observation.values.tolist() == [False, False] + assert observation.valid is not None + assert observation.valid.tolist() == [False, False] + + +def test_command_state_tracker_rejects_unknown_environment_ids() -> None: + """Unknown correlation IDs fail before tracker state can be mutated or read.""" + tracker = _command_state_tracker() + + with pytest.raises(ValueError, match="absent from tracker env_ids"): + tracker.accepted( + _hand_command_frame( + env_ids=(99,), + positions=(_HAND_GRASP_POSITION,), + ) + ) + with pytest.raises(ValueError, match="absent from tracker env_ids"): + _hand_state_observation(tracker, 99) + + observation = _hand_state_observation(tracker, 7, 3, 11) + assert observation.valid is not None + assert observation.valid.tolist() == [False, False, False] + + +class _Robot: + """Minimal typed robot surface used by the production factory.""" + + uid = "robot" + device = torch.device("cpu") + dof = _ROBOT_DOF + control_parts = {"arm": ("joint_0",)} + + def __init__(self) -> None: + self.qpos = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) + + def get_qpos( + self, + name: str | None = None, + target: bool = False, + ) -> torch.Tensor: + """Return full or control-part positions.""" + del target + return self.qpos if name is None else self.qpos[:, :1] + + def get_qvel( + self, + name: str | None = None, + target: bool = False, + ) -> torch.Tensor: + """Return zero measured velocities.""" + return torch.zeros_like(self.get_qpos(name=name, target=target)) + + def get_qf(self, name: str | None = None) -> torch.Tensor: + """Return zero measured effort.""" + return torch.zeros_like(self.get_qpos(name=name)) + + def get_joint_ids(self, name: str) -> list[int]: + """Resolve the only declared control part.""" + if name != "arm": + raise KeyError(name) + return [0] + + def get_solver(self, name: str) -> object: + """Return a configured solver marker for Cartesian capability.""" + if name != "arm": + raise KeyError(name) + return object() + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: list[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + """Return identity endpoint poses for evidence adapter validation.""" + del name, env_ids + if not to_matrix: + raise ValueError("Tests require matrix FK output.") + return torch.eye(4).repeat(qpos.shape[0], 1, 1) + + +class _EvidenceRobot(_Robot): + """Joint-backed arm and hand with a mutable measured endpoint pose.""" + + control_parts = { + "arm": ("joint_0",), + "hand": ("joint_1",), + } + + def __init__(self) -> None: + super().__init__() + self.endpoint_pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + + def get_qpos( + self, + name: str | None = None, + target: bool = False, + ) -> torch.Tensor: + """Return the full state or the selected control-part state.""" + del target + if name is None: + return self.qpos + joint_id = self.get_joint_ids(name)[0] + return self.qpos[:, joint_id : joint_id + 1] + + def get_joint_ids(self, name: str) -> list[int]: + """Resolve the disjoint arm and hand joints.""" + if name == "arm": + return [0] + if name == "hand": + return [1] + raise KeyError(name) + + def get_solver(self, name: str) -> object: + """Return the configured arm solver marker.""" + if name != "arm": + raise KeyError(name) + return object() + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: list[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + """Return the live arm endpoint pose for requested simulator rows.""" + del qpos + if name != "arm" or not to_matrix: + raise ValueError("Evidence FK requires the arm matrix pose.") + rows = list(range(_BATCH_SIZE)) if env_ids is None else env_ids + return self.endpoint_pose[rows].clone() + + +class _DualRobot(_Robot): + """Four-part dual-arm robot used for provider-aware helper preflight.""" + + uid = "dual_robot" + dof = _DUAL_ROBOT_DOF + control_parts = { + "left_arm": ("left_arm_joint",), + "left_hand": ("left_hand_joint",), + "right_arm": ("right_arm_joint",), + "right_hand": ("right_hand_joint",), + } + _joint_ids = { + "left_arm": (0,), + "left_hand": (1,), + "right_arm": (2,), + "right_hand": (3,), + } + + def __init__(self) -> None: + self.qpos = torch.zeros(_BATCH_SIZE, self.dof) + + def get_qpos( + self, + name: str | None = None, + target: bool = False, + ) -> torch.Tensor: + """Return full state or the selected one-joint control part.""" + del target + if name is None: + return self.qpos + return self.qpos[:, list(self._joint_ids[name])] + + def get_joint_ids(self, name: str) -> list[int]: + """Resolve one disjoint arm or hand joint.""" + return list(self._joint_ids[name]) + + def get_solver(self, name: str) -> object: + """Return configured solver markers for both motion endpoints.""" + if name not in {"left_arm", "right_arm"}: + raise KeyError(name) + return object() + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: list[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + """Return identity arm endpoint poses for runtime assembly checks.""" + del env_ids + if name not in {"left_arm", "right_arm"} or not to_matrix: + raise ValueError("Dual-arm evidence requires an arm matrix pose.") + return torch.eye(4).repeat(qpos.shape[0], 1, 1) + + +class _RigidObject: + """Mutable batched rigid object with the mesh surface required by binding.""" + + def __init__(self) -> None: + self.pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + + def get_local_pose(self, *, to_matrix: bool = False) -> torch.Tensor: + """Return the current measured object pose.""" + if not to_matrix: + raise ValueError("Tests require matrix object poses.") + return self.pose.clone() + + def get_vertices( + self, + *, + env_ids: list[int], + scale: bool = True, + ) -> torch.Tensor: + """Return one minimal triangular mesh per requested row.""" + del scale + vertices = torch.tensor(((0.0, 0.0, 0.0), (0.04, 0.0, 0.0), (0.0, 0.04, 0.0))) + return vertices.unsqueeze(0).repeat(len(env_ids), 1, 1) + + def get_triangles(self, *, env_ids: list[int]) -> torch.Tensor: + """Return one valid triangle per requested row.""" + return ( + torch.tensor(((0, 1, 2),), dtype=torch.long) + .unsqueeze(0) + .repeat(len(env_ids), 1, 1) + ) + + +class _ForwardedRelationGrounder(RelationTargetGrounder): + """Sentinel relation grounder installed only to prove helper forwarding.""" + + capability: ClassVar[str] = "test.place_relation" + affordance_type: ClassVar[type[Affordance]] = Affordance + affordance_revision: ClassVar[str] = "test-v1" + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> torch.Tensor: + """Return a direct identity target when explicitly exercised.""" + del relation, affordance, context + return torch.eye(4) + + +class _ForwardedHandOverPoseProvider(HandOverPoseProvider): + """Sentinel embodiment provider installed only through the standard helper.""" + + provider_id: ClassVar[str] = "test.handover_pose" + + def __init__(self) -> None: + self.calls = 0 + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Return owned direct targets without embedding task-side motion code.""" + del call, context, bound + self.calls += 1 + pose = SemanticPose( + position=(0.0, 0.0, 0.5), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ) + return HandOverPoseTargets( + middle=SemanticObjectTarget(pose=pose), + final=SemanticObjectTarget(pose=pose), + ) + + +@dataclass(frozen=True, slots=True) +class _MobileEndpoint(ResourceEndpoint): + """Non-joint endpoint used by the standard simulation factory test.""" + + controller_id: str + + +@dataclass(frozen=True, slots=True) +class _MobileTarget(RuntimeEndpointTarget): + """Runtime destination for the test mobile controller.""" + + controller_id: str + + @property + def transport_id(self) -> str: + """Return the matching test Gym transport ID.""" + return "test.mobile_velocity" + + @property + def target_id(self) -> str: + """Return the selected controller ID.""" + return self.controller_id + + +class _MobileEndpointAdapter(ResourceEndpointAdapter): + """Resolve a mobile endpoint without consulting robot control parts.""" + + adapter_id: ClassVar[str] = "test.mobile_velocity" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _MobileEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + """Resolve one exclusive controller claim.""" + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_MobileEndpointAdapter requires _MobileEndpoint.") + return EndpointResolution( + runtime_target=_MobileTarget(endpoint.controller_id), + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +class _MobileTransportEncoder: + """Minimal Gym encoder registered for the custom mobile target.""" + + @property + def transport_id(self) -> str: + """Return the custom mobile transport ID.""" + return "test.mobile_velocity" + + def encode( + self, + command: EndpointCommand, + *, + base_action: Any, + active_mask: torch.Tensor, + ) -> Any: + """Preserve the base action in this assembly-only test transport.""" + del command, active_mask + return base_action.clone() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: Any, + context: Any, + ) -> Any: + """Preserve the base action for a mobile safe hold.""" + del targets, context + return base_action.clone() + + +class _MobileRobot: + """Full-state robot fixture with no control-parts or joint-ID surface.""" + + uid = "mobile_robot" + device = torch.device("cpu") + dof = _ROBOT_DOF + + def __init__(self) -> None: + self.qpos = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) + + def get_qpos(self) -> torch.Tensor: + """Return the full controller hold state.""" + return self.qpos + + def get_qvel(self) -> torch.Tensor: + """Return the full measured velocity state.""" + return torch.zeros_like(self.qpos) + + def get_qf(self) -> torch.Tensor: + """Return the full measured effort state.""" + return torch.zeros_like(self.qpos) + + +class _Simulation: + """Minimal simulation registry for one exact robot.""" + + def __init__( + self, + robot: _Robot, + rigid_objects: dict[str, _RigidObject] | None = None, + ) -> None: + self.robot = robot + self.rigid_objects = {} if rigid_objects is None else dict(rigid_objects) + + def get_robot(self, uid: str) -> _Robot | None: + """Resolve the selected robot UID.""" + return self.robot if uid == self.robot.uid else None + + def get_rigid_object(self, uid: str) -> _RigidObject | None: + """Resolve one explicitly registered rigid-object UID.""" + return self.rigid_objects.get(uid) + + +def _profile_binding() -> SimulationRobotSkillProfileBinding: + """Build one motion-only profile with an intentionally wrong cadence.""" + return SimulationRobotSkillProfileBinding( + profile_id="robot_profile", + resources=( + ControlPartResourceBinding( + resource_id="manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + ), + ), + ), + presets=( + SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(sample_count=17), + ), + ), + default_preset="safe", + ) + + +def _handover_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare two disjoint manipulators and one selected pose provider ID.""" + motion_capabilities = frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ) + 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}_hand_commands", + ), + ), + ) + for side in ("left", "right") + ) + command_presets = tuple( + ControlPartCommandPreset( + preset_id=f"{side}_hand_commands", + control_part=f"{side}_hand", + commands={ + "open": (_HAND_OPEN_POSITION,), + "grasp": (_HAND_GRASP_POSITION,), + }, + ) + for side in ("left", "right") + ) + return SimulationRobotSkillProfileBinding( + profile_id="handover_profile", + resources=resources, + command_presets=command_presets, + defaults={ + "hand_over": {"source": "left", "destination": "right"}, + }, + presets=(SkillPolicyPreset("safe"),), + default_preset="safe", + grounding_providers={ + "hand_over": _ForwardedHandOverPoseProvider.provider_id, + }, + ) + + +def _handover_helper_inputs() -> tuple[ + SimpleNamespace, + SimulationSceneBinding, + SimulationRobotSkillProfileBinding, +]: + """Build standard-helper inputs for one provider-aware HandOver program.""" + robot = _DualRobot() + cube = _RigidObject() + simulation = _Simulation(robot, {"cube_native": cube}) + environment = SimpleNamespace( + sim=simulation, + robot=robot, + step_dt=_STEP_DT, + ) + scene_binding = SimulationSceneBinding( + registry_id="handover_scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="cube_native", + default_grasp_affordance="cube_grasp", + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id="cube_grasp", + object_id="cube", + native_name="body", + revision="1", + ), + ), + ) + return environment, scene_binding, _handover_profile_binding() + + +def _handover_program() -> ExpertProgramCfg: + """Build one external-held-state HandOver call for static preflight.""" + return ExpertProgramCfg( + schema_version=1, + program_id="handover_preflight", + integration=ExpertProgramIntegrationCfg( + robot_profile="handover_profile", + scene_registry="handover_scene", + runtime_preset="safe", + ), + program=InvokeCfg(call=HandOverCfg(object="cube")), + ) + + +def _motion_generator(robot: _Robot) -> MotionGenerator: + """Build a type-checkable motion-generator test double.""" + generator = MagicMock(spec=MotionGenerator) + generator.robot = robot + generator.device = robot.device + generator.planner = SimpleNamespace(cfg=SimpleNamespace(planner_type="test")) + generator.collision_world_info = None + return generator + + +def _factory() -> tuple[SimulationExpertProgramFactory, _Robot]: + """Create one production factory around CPU-only test doubles.""" + robot = _Robot() + simulation = _Simulation(robot) + return ( + SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + SimulationSceneBinding(registry_id="scene"), + _profile_binding(), + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ), + robot, + ) + + +def _evidence_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare one manipulation resource with exact open/grasp semantics.""" + motion_capabilities = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ) + return SimulationRobotSkillProfileBinding( + profile_id="evidence_profile", + resources=( + ControlPartResourceBinding( + resource_id="manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=motion_capabilities, + ), + ControlPartEndpointBinding( + endpoint_id="grasp", + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="hand_commands", + ), + ), + ), + ), + command_presets=( + ControlPartCommandPreset( + preset_id="hand_commands", + control_part="hand", + commands={ + "open": (_HAND_OPEN_POSITION,), + "grasp": (_HAND_GRASP_POSITION,), + }, + ), + ), + defaults={ + "pick_up": {"primary": "manipulator"}, + "place": {"primary": "manipulator"}, + }, + presets=(SkillPolicyPreset("evidence"),), + default_preset="evidence", + ) + + +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 + relation = torch.eye(4).repeat(context.batch_size, 1, 1) + held = HeldObjectState( + semantics=goal.semantics, + object_to_eef=relation, + grasp_xpos=relation, + ) + return action.build_plan( + request, + context, + success=torch.ones(context.batch_size, dtype=torch.bool), + trajectory=TimedTrajectory.from_uniform_step( + trajectory, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + expected_effects=StateDelta( + held_object_updates={"manipulator": held}, + ), + replannable=False, + 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 + return action.build_plan( + request, + context, + success=torch.ones(context.batch_size, dtype=torch.bool), + trajectory=TimedTrajectory.from_uniform_step( + trajectory, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + expected_effects=StateDelta( + held_object_updates={"manipulator": None}, + ), + replannable=False, + ) + + +def _evidence_runtime() -> tuple[ + ExpertProgramRuntimeAssembly, + _EvidenceRobot, + _RigidObject, +]: + """Assemble the production Pick/Place evidence chain on CPU fixtures.""" + robot = _EvidenceRobot() + cube = _RigidObject() + simulation = _Simulation(robot, {"cube_native": cube}) + scene_binding = SimulationSceneBinding( + registry_id="evidence_scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="cube_native", + default_grasp_affordance="cube_grasp", + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id="cube_grasp", + object_id="cube", + native_name="body", + revision="1", + ), + ), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + scene_binding, + _evidence_profile_binding(), + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ) + adapter = factory.create_adapter( + runner_cfg=ExecutionRunnerCfg( + minimum_cycle_time=0.0, + hold_on_completion=False, + ) + ) + assembly = adapter.assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="evidence_profile", + scene_registry="evidence_scene", + runtime_preset="evidence", + ) + ) + pick_action = assembly.engine.actions["pick_up"] + place_action = assembly.engine.actions["place"] + pick_action._plan = MethodType(_pick_evidence_plan, pick_action) + place_action._plan = MethodType(_place_evidence_plan, place_action) + return assembly, robot, cube + + +def _consume_buffered_action( + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, +) -> None: + """Apply one accepted Gym action and advance the authoritative clock.""" + processed = assembly.command_sink.pop() + if not isinstance(processed.value, torch.Tensor): + raise TypeError("Joint-backed evidence actions must be tensors.") + robot.qpos = processed.value.clone() + assembly.clock.advance_after_env_step() + + +def _accept_hand_command( + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, + position: float, +) -> None: + """Accept and consume one semantic hand command through the Gym sink.""" + assert assembly.command_sink.pending_count == 0 + frame = _hand_command_frame( + env_ids=tuple(range(_BATCH_SIZE)), + positions=(position,) * _BATCH_SIZE, + ) + acknowledgement = assembly.command_sink.send(frame, timeout=1.0) + assert acknowledgement.accepted + _consume_buffered_action(assembly, robot) + + +def _sample_effect( + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, + *, + expected_trace_count: int, + advance_clock: bool = True, +) -> tuple[Any, SkillEffectTrace]: + """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] + + +class _SynchronousEvidenceClock: + """Advance the fixture's simulation clock during standalone facade waits.""" + + def __init__(self, clock: EnvironmentStepClock) -> None: + self._clock = clock + + def now(self) -> float: + """Return the simulation fixture's authoritative time.""" + return self._clock.now() + + def sleep(self, duration: float) -> None: + """Advance the exact number of fixture ticks requested by the runner.""" + steps = self._clock.steps_for_duration(duration) + if steps: + self._clock.advance_after_env_step(steps) + + +class _ImmediateEvidenceCommandSink: + """Apply accepted endpoint frames immediately for standalone CPU execution.""" + + def __init__( + self, + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, + cube: _RigidObject, + ) -> None: + self._encoder = assembly.command_encoder + self._observer = assembly.accepted_command_observer + self._clock = assembly.clock + self._robot = robot + self._cube = cube + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply one command and publish its accepted semantic hand state.""" + assert timeout > 0.0 + action = self._encoder.encode(command) + if not isinstance(action, torch.Tensor): + raise TypeError("The CPU quickstart fixture requires tensor actions.") + self._robot.qpos = action.clone() + if self._observer is None: + raise RuntimeError("The evidence fixture requires an accepted observer.") + self._observer.accepted(command.snapshot()) + if torch.allclose( + action[:, 1], + torch.full_like(action[:, 1], _HAND_OPEN_POSITION), + ): + self._cube.pose[:, 0, 3] = _RELEASE_SEPARATION + self._clock.advance_after_env_step() + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply the encoder's observed-position hold immediately.""" + assert timeout > 0.0 + action = self._encoder.encode_hold(targets, context) + if not isinstance(action, torch.Tensor): + raise TypeError("The CPU quickstart fixture requires tensor actions.") + self._robot.qpos = action.clone() + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Clear accepted command evidence for cancelled destinations.""" + assert timeout > 0.0 + if self._observer is not None: + self._observer.cancelled(targets) + return CommandAcknowledgement.accepted_ack() + + +class _QuickstartRuntimeProvider: + """Explicit provider used by the public ``AtomicSkills.from_env`` path.""" + + def __init__(self, runtime: SkillRuntime) -> None: + self._runtime = runtime + self.presets: list[str] = [] + + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + """Return the configured canonical runtime and record preset selection.""" + self.presets.append(preset) + return self._runtime + + +def _quickstart_runtime_provider() -> _QuickstartRuntimeProvider: + """Build a synchronous provider from the shared production CPU fixture.""" + assembly, robot, cube = _evidence_runtime() + runtime = SkillRuntime.from_components( + assembly.compiler, + assembly.observation_provider, + _ImmediateEvidenceCommandSink(assembly, robot, cube), + assembly.evidence_collector, + clock=_SynchronousEvidenceClock(assembly.clock), + runner_cfg=ExecutionRunnerCfg( + minimum_cycle_time=0.0, + hold_on_completion=False, + ), + ) + return _QuickstartRuntimeProvider(runtime) + + +def _documented_pick_place_quickstart( + runtime_provider: _QuickstartRuntimeProvider, +) -> SkillResult: + """Run the application-facing quickstart, excluding scene construction.""" + skills = AtomicSkills.from_env(runtime_provider, preset="evidence") + cube = skills.scene.object("cube") + return skills.run( + Pick(object=cube), + Place(object=cube, at=_DIRECT_PLACE_TARGET), + ) + + +def _python_pick_place_calls() -> tuple[SemanticCallSpec, ...]: + """Return the application-facing calls used by both acceptance paths.""" + cube = SceneObjectRef("cube") + return ( + Pick(object=cube), + Place(object=cube, at=_DIRECT_PLACE_TARGET), + ) + + +def _decoded_pick_place_calls( + registry: SceneRegistry, +) -> tuple[SemanticCallSpec, ...]: + """Decode and provider-free compile the config equivalent of Python calls.""" + config = decode_expert_program( + { + "schema_version": 1, + "program_id": "pick_place_equivalence", + "integration": { + "robot_profile": "evidence_profile", + "scene_registry": "evidence_scene", + "runtime_preset": "evidence", + }, + "targets": { + "place_target": { + "kind": "cyclic_pose", + "values": [ + { + "position": _DIRECT_PLACE_TARGET.position.tolist(), + "quaternion_wxyz": ( + _DIRECT_PLACE_TARGET.quaternion_wxyz.tolist() + ), + } + ], + } + }, + "program": { + "kind": "sequence", + "items": [ + { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "at": { + "kind": "target_ref", + "target": "place_target", + }, + }, + }, + ], + }, + } + ) + program = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + return tuple( + compiled_call.call for segment in program for compiled_call in segment.calls + ) + + +def _capture_grounded_invocations( + monkeypatch: pytest.MonkeyPatch, + assembly: ExpertProgramRuntimeAssembly, +) -> list[ActionInvocation[Any, Any]]: + """Record the production compiler's final lowering without replacing it.""" + invocations: list[ActionInvocation[Any, Any]] = [] + ground = assembly.compiler.ground + + def recording_ground(*args: Any, **kwargs: Any) -> Any: + grounded = ground(*args, **kwargs) + invocations.append(grounded.invocation) + return grounded + + monkeypatch.setattr(assembly.compiler, "ground", recording_ground) + return invocations + + +def _run_evidence_pick_place( + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, + cube: _RigidObject, + calls: tuple[SemanticCallSpec, ...], +) -> tuple[SkillResult, HeldObjectState]: + """Drive one happy-path workflow through accepted commands and live evidence.""" + result = assembly.runtime.start(calls, workflow_id="pick_place_equivalence") + verified_pick: HeldObjectState | None = None + for _ in range(32): + while assembly.command_sink.pending_count: + _consume_buffered_action(assembly, robot) + if result.terminal: + break + if result.current_call_index == 1: + if verified_pick is None: + verified_pick = result.task_state.get_held_object("manipulator") + cube.pose[:, 0, 3] = _RELEASE_SEPARATION + assembly.clock.advance_after_env_step() + result = assembly.runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert verified_pick is not None + assert result.task_state.get_held_object("manipulator") is None + return result, verified_pick + + +def _assert_typed_equivalent( + actual: object, + expected: object, + *, + path: str = "value", +) -> None: + """Compare nested typed compiler output, including owned tensor values.""" + assert type(actual) is type(expected), path + if isinstance(actual, torch.Tensor): + assert isinstance(expected, torch.Tensor) + torch.testing.assert_close(actual, expected) + return + if isinstance(actual, Mapping): + assert isinstance(expected, Mapping) + assert tuple(actual) == tuple(expected) + for key in actual: + _assert_typed_equivalent( + actual[key], + expected[key], + path=f"{path}[{key!r}]", + ) + return + if isinstance(actual, Sequence) and not isinstance(actual, (str, bytes)): + assert isinstance(expected, Sequence) + assert len(actual) == len(expected) + for index, (actual_item, expected_item) in enumerate( + zip(actual, expected, strict=True) + ): + _assert_typed_equivalent( + actual_item, + expected_item, + path=f"{path}[{index}]", + ) + return + if is_dataclass(actual) and not isinstance(actual, type): + assert is_dataclass(expected) and not isinstance(expected, type) + for data_field in fields(actual): + _assert_typed_equivalent( + getattr(actual, data_field.name), + getattr(expected, data_field.name), + path=f"{path}.{data_field.name}", + ) + return + assert actual == expected, path + + +def _assert_invocation_equivalent( + actual: ActionInvocation[Any, Any], + expected: ActionInvocation[Any, Any], +) -> None: + """Compare semantic lowering while ignoring engine-instance owner UUIDs.""" + assert actual.skill_id == expected.skill_id + assert actual.invocation_id == expected.invocation_id + assert actual.revision == expected.revision + _assert_typed_equivalent(actual.goal, expected.goal, path="invocation.goal") + _assert_typed_equivalent( + actual.binding.endpoints, + expected.binding.endpoints, + path="invocation.binding.endpoints", + ) + _assert_typed_equivalent( + actual.motion_policy, + expected.motion_policy, + path="invocation.motion_policy", + ) + _assert_typed_equivalent( + actual.recovery_policy, + expected.recovery_policy, + path="invocation.recovery_policy", + ) + _assert_typed_equivalent( + actual.skill_options, + expected.skill_options, + path="invocation.skill_options", + ) + _assert_typed_equivalent( + actual.control_overrides, + expected.control_overrides, + path="invocation.control_overrides", + ) + + +def test_simulation_factory_preserves_declarative_motion_policy() -> None: + """Gym cadence stays on observations rather than mutating motion policy.""" + factory, _ = _factory() + + profile = factory.create_robot_skill_profile() + + assert profile.presets["safe"].motion_policy.sample_count == 17 + + +def test_decoded_program_and_python_calls_share_invocations_and_results( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Both frontends reach equivalent core invocations and verified state.""" + python_assembly, python_robot, python_cube = _evidence_runtime() + config_assembly, config_robot, config_cube = _evidence_runtime() + python_invocations = _capture_grounded_invocations(monkeypatch, python_assembly) + config_invocations = _capture_grounded_invocations(monkeypatch, config_assembly) + + python_result, python_held = _run_evidence_pick_place( + python_assembly, + python_robot, + python_cube, + _python_pick_place_calls(), + ) + config_result, config_held = _run_evidence_pick_place( + config_assembly, + config_robot, + config_cube, + _decoded_pick_place_calls(config_assembly.scene_registry), + ) + + assert len(python_invocations) == len(config_invocations) == 2 + for python_invocation, config_invocation in zip( + python_invocations, + config_invocations, + strict=True, + ): + _assert_invocation_equivalent(python_invocation, config_invocation) + _assert_typed_equivalent(python_held, config_held) + _assert_typed_equivalent(python_result, config_result) + + +def test_atomic_skills_from_env_runs_documented_pick_place_quickstart() -> None: + """The small public facade executes without exposing core motion plumbing.""" + provider = _quickstart_runtime_provider() + + result = _documented_pick_place_quickstart(provider) + + source = textwrap.dedent(inspect.getsource(_documented_pick_place_quickstart)) + function = ast.parse(source).body[0] + assert isinstance(function, ast.FunctionDef) + executable = function.body[1:] # Exclude the helper's docstring. + assert executable[-1].end_lineno is not None + assert executable[-1].end_lineno - executable[0].lineno + 1 <= ( + _QUICKSTART_MAX_LINES + ) + identifiers = { + identifier + for node in ast.walk(function) + for identifier in ( + node.id if isinstance(node, ast.Name) else None, + node.attr if isinstance(node, ast.Attribute) else None, + ) + if identifier is not None + } + assert identifiers.isdisjoint( + { + "qpos", + "matrix", + "planner", + "session", + "MotionGenerator", + "PlanningContext", + "ExecutionSession", + } + ) + assert provider.presets == ["evidence"] + assert result.status is SkillStatus.COMPLETED + assert result.success_mask.tolist() == [True] * _BATCH_SIZE + assert [call.semantic_id for call in result.calls] == ["pick", "place"] + assert result.task_state.get_held_object("manipulator") is None + + +def test_simulation_factory_builds_shared_observation_and_evidence_ports() -> None: + """Observation and both built-in evidence providers share one scene source.""" + factory, robot = _factory() + registry = factory.create_scene_registry() + profile = factory.create_robot_skill_profile() + engine = factory.create_atomic_action_engine(profile) + clock = EnvironmentStepClock(_STEP_DT) + + observation = factory.create_planning_observation_provider( + scene_registry=registry, + engine=engine, + clock=clock, + ) + assert type(observation) is SimulationPlanningObservationProvider + context = observation.observe(TaskState.empty(_BATCH_SIZE, robot.device)) + providers = tuple( + factory.create_effect_evidence_providers( + scene_registry=registry, + engine=engine, + observation_provider=observation, + ) + ) + accepted_command_observer = factory.create_accepted_runtime_command_observer( + scene_registry=registry, + engine=engine, + observation_provider=observation, + ) + + assert context.robot.timestamp == pytest.approx(0.0) + assert torch.equal(observation.current_qpos(context.env_ids), robot.qpos) + assert accepted_command_observer is observation.command_state_tracker + assert len(providers) == 2 + assert all( + getattr(provider, "_scene_provider") is observation.scene_provider + for provider in providers + ) + + +def test_simulation_factory_returns_exact_environment_adapter() -> None: + """The convenience path remains compatible with the exact-type mixin check.""" + factory, _ = _factory() + + adapter = factory.create_adapter() + + assert type(adapter) is ExpertProgramEnvironmentAdapter + assert adapter.step_dt == pytest.approx(_STEP_DT) + assert factory.segment_policy_port is not None + + +def test_simulation_helper_forwards_semantic_grounding_extensions() -> None: + """Both explicit grounding seams reach the runtime compiler unchanged.""" + robot = _Robot() + environment = SimpleNamespace( + sim=_Simulation(robot), + robot=robot, + step_dt=_STEP_DT, + ) + relation_grounder = _ForwardedRelationGrounder() + handover_provider = _ForwardedHandOverPoseProvider() + adapter = create_simulation_expert_program_adapter( + environment, # type: ignore[arg-type] + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + motion_generator_factory=lambda: _motion_generator(robot), + relation_grounders=(relation_grounder,), + handover_pose_providers=(handover_provider,), + ) + + assembly = adapter.assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + ) + + assert tuple(assembly.compiler.relation_grounders.values()) == (relation_grounder,) + assert tuple(assembly.compiler.handover_pose_providers.values()) == ( + handover_provider, + ) + + +def test_simulation_helper_handover_preflight_is_fail_closed_by_default() -> None: + """Selecting a provider ID does not infer or auto-install an implementation.""" + environment, scene_binding, profile_binding = _handover_helper_inputs() + robot = environment.robot + adapter = create_simulation_expert_program_adapter( + environment, # type: ignore[arg-type] + scene_binding=scene_binding, + robot_profile_binding=profile_binding, + motion_generator_factory=lambda: _motion_generator(robot), + ) + compiled = adapter.compile(_handover_program()) + + with pytest.raises(SemanticValidationError) as error: + adapter.create_bridge(compiled) + + assert error.value.diagnostic.code == "handover_grounding_provider_not_installed" + + +def test_simulation_helper_forwards_handover_provider_to_preflight() -> None: + """An explicitly supplied embodiment provider satisfies standard preflight.""" + environment, scene_binding, profile_binding = _handover_helper_inputs() + robot = environment.robot + provider = _ForwardedHandOverPoseProvider() + adapter = create_simulation_expert_program_adapter( + environment, # type: ignore[arg-type] + scene_binding=scene_binding, + robot_profile_binding=profile_binding, + motion_generator_factory=lambda: _motion_generator(robot), + handover_pose_providers=(provider,), + ) + + bridge = adapter.create_bridge(adapter.compile(_handover_program())) + + assert bridge is not None + assert provider.calls == 0 + + +def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joints() -> ( + None +): + """The one-line factory path supports a custom non-joint controller.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + profile_binding = SimulationRobotSkillProfileBinding( + profile_id="mobile_profile", + resources=( + RobotResourceBinding( + resource_id="mobile_base", + endpoints={ + "motion": _MobileEndpoint( + controller_id="base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ), + ), + presets=(SkillPolicyPreset("runtime"),), + default_preset="runtime", + ) + environment = SimpleNamespace( + sim=simulation, + robot=robot, + step_dt=_STEP_DT, + ) + + adapter = create_simulation_expert_program_adapter( + environment, # type: ignore[arg-type] + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=profile_binding, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + endpoint_adapters={_MobileEndpoint: _MobileEndpointAdapter()}, + runtime_transports=(_MobileTransportEncoder(),), + ) + assembly = adapter.assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) + + endpoint = assembly.robot_profile.resources["mobile_base"].endpoints["motion"] + assert isinstance(endpoint, _MobileEndpoint) + assert "test.mobile_velocity" in assembly.command_encoder.transport_ids + assert assembly.engine.skill_profile is not None + resolved = assembly.engine.skill_profile.resources["mobile_base"] + assert isinstance(resolved.endpoints["motion"].runtime_target, _MobileTarget) + assert resolved.claim.claim_tokens == frozenset({"controller:base_velocity"}) + + +def test_pick_place_effects_require_accepted_hand_state_and_live_pose() -> None: + """Production Pick/Place evidence stays conjunctive through runtime traces.""" + assembly, robot, cube = _evidence_runtime() + assert type(assembly.accepted_command_observer) is ( + ControlCommandStateEvidenceTracker + ) + cube.pose[:, 0, 3] = 0.2 + result = assembly.runtime.start( + ( + Pick(object=SceneObjectRef("cube")), + Place( + object=SceneObjectRef("cube"), + at=SemanticPose( + position=(0.0, 0.0, 0.0), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ), + ), + ), + workflow_id="production_evidence_chain", + ) + assert result.status is SkillStatus.RUNNING + + result = assembly.runtime.step() + assert assembly.command_sink.pending_count == 1 + assert len(result.effects) == 0 + _consume_buffered_action(assembly, robot) + result = assembly.runtime.step() + assert len(result.effects) == 0 + while assembly.command_sink.pending_count: + _consume_buffered_action(assembly, robot) + + result, pick_pose_missing = _sample_effect( + assembly, + robot, + expected_trace_count=1, + ) + pick_pose = pick_pose_missing.evidence["destination.pose"] + pick_constraint = pick_pose_missing.evidence["destination.constraint"] + assert type(pick_pose) is PoseRelationEvidenceBatch + assert type(pick_constraint) is BinaryEffectEvidenceBatch + assert pick_pose.object_to_endpoint[:, 0, 3].tolist() == pytest.approx( + [-0.2] * _BATCH_SIZE + ) + assert pick_constraint.values.tolist() == [True] * _BATCH_SIZE + assert pick_constraint.valid.tolist() == [True] * _BATCH_SIZE + assert not pick_pose_missing.success_mask.any() + + cube.pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + assembly.command_sink.discard_pending() + result, pick_command_missing = _sample_effect( + assembly, + robot, + expected_trace_count=2, + ) + pick_pose = pick_command_missing.evidence["destination.pose"] + pick_constraint = pick_command_missing.evidence["destination.constraint"] + torch.testing.assert_close( + pick_pose.object_to_endpoint, + torch.eye(4).repeat(_BATCH_SIZE, 1, 1), + ) + assert pick_constraint.valid.tolist() == [False] * _BATCH_SIZE + assert not pick_command_missing.success_mask.any() + + _accept_hand_command(assembly, robot, _HAND_GRASP_POSITION) + result, pick_first_complete_sample = _sample_effect( + assembly, + robot, + expected_trace_count=3, + advance_clock=False, + ) + assert not pick_first_complete_sample.success_mask.any() + result, pick_success = _sample_effect( + assembly, + robot, + expected_trace_count=4, + ) + assert pick_success.call_index == 0 + assert pick_success.effect_spec.semantic_id == "pick" + assert pick_success.success_mask.tolist() == [True] * _BATCH_SIZE + assert ( + pick_success.evidence["destination.constraint"].values.tolist() + == [True] * _BATCH_SIZE + ) + assert result.task_state.get_held_object("manipulator") is not None + assert result.current_call_index == 1 + + result = assembly.runtime.step() + assert assembly.command_sink.pending_count == 1 + assert len(result.effects) == 4 + _consume_buffered_action(assembly, robot) + result = assembly.runtime.step() + assert len(result.effects) == 4 + while assembly.command_sink.pending_count: + _consume_buffered_action(assembly, robot) + + result, place_pose_missing = _sample_effect( + assembly, + robot, + expected_trace_count=5, + ) + place_pose = place_pose_missing.evidence["source.pose"] + place_constraint = place_pose_missing.evidence["source.constraint"] + assert type(place_pose) is PoseRelationEvidenceBatch + assert type(place_constraint) is BinaryEffectEvidenceBatch + torch.testing.assert_close( + place_pose.object_to_endpoint, + torch.eye(4).repeat(_BATCH_SIZE, 1, 1), + ) + assert place_constraint.values.tolist() == [False] * _BATCH_SIZE + assert place_constraint.valid.tolist() == [True] * _BATCH_SIZE + assert not place_pose_missing.success_mask.any() + + cube.pose[:, 0, 3] = 0.2 + assembly.command_sink.discard_pending() + result, place_command_missing = _sample_effect( + assembly, + robot, + expected_trace_count=6, + ) + place_pose = place_command_missing.evidence["source.pose"] + place_constraint = place_command_missing.evidence["source.constraint"] + assert place_pose.object_to_endpoint[:, 0, 3].tolist() == pytest.approx( + [-0.2] * _BATCH_SIZE + ) + assert place_constraint.valid.tolist() == [False] * _BATCH_SIZE + assert not place_command_missing.success_mask.any() + + _accept_hand_command(assembly, robot, _HAND_OPEN_POSITION) + result, place_first_complete_sample = _sample_effect( + assembly, + robot, + expected_trace_count=7, + advance_clock=False, + ) + assert not place_first_complete_sample.success_mask.any() + result, place_success = _sample_effect( + assembly, + robot, + expected_trace_count=8, + ) + assert result.status is SkillStatus.COMPLETED + assert place_success.call_index == 1 + assert place_success.effect_spec.semantic_id == "place" + assert place_success.success_mask.tolist() == [True] * _BATCH_SIZE + assert ( + place_success.evidence["source.constraint"].values.tolist() + == [False] * _BATCH_SIZE + ) + assert result.task_state.get_held_object("manipulator") is None + assert [len(call.effects) for call in result.calls] == [4, 4] + assert assembly.command_sink.accepted_action_count >= 4 diff --git a/tests/gym/envs/expert_program/test_simulation_policies.py b/tests/gym/envs/expert_program/test_simulation_policies.py new file mode 100644 index 000000000..51b3bea13 --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation_policies.py @@ -0,0 +1,451 @@ +# ---------------------------------------------------------------------------- +# 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 explicit simulation-backed segment policies.""" + +from __future__ import annotations + +import json +from types import SimpleNamespace + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramCompiler, + SimulationRigidObjectBinding, + SimulationSceneBinding, + decode_expert_program, +) +from embodichain.lab.gym.envs.expert_program.bridge import ( + SegmentPostPolicyMetadataPort, + SegmentPostPolicyPort, + SegmentPostPolicyResultPort, + SegmentValidatorMetadataPort, + SegmentValidatorPort, +) +from embodichain.lab.gym.envs.expert_program.simulation_policies import ( + SimulationSegmentPolicyPort, +) +from embodichain.lab.gym.envs.settling import DynamicSettleMonitorCfg +from embodichain.lab.sim.atomic_actions import EntityState +from embodichain.lab.sim.skills.scene import ( + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + + +class _StaticStateProvider: + """Provide an inert object state for provider-free compilation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp + return EntityState( + torch.eye(4, device=env_ids.device).expand(env_ids.numel(), -1, -1) + ) + + +class _RigidObject: + """Small live rigid-object double with mutable velocities and poses.""" + + def __init__(self, positions: torch.Tensor) -> None: + batch_size = positions.shape[0] + self.is_non_dynamic = False + self.pose_reads = 0 + self.body_data = SimpleNamespace( + lin_vel=torch.zeros(batch_size, 3), + ang_vel=torch.zeros(batch_size, 3), + ) + self._pose = torch.eye(4).expand(batch_size, -1, -1).clone() + self._pose[:, :3, 3] = positions + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix + self.pose_reads += 1 + return self._pose.clone() + + +class _Robot: + """Full-qpos source used by post-policy hold actions.""" + + def __init__(self, qpos: torch.Tensor) -> None: + self.qpos = qpos + self.qpos_reads = 0 + + def get_qpos(self) -> torch.Tensor: + self.qpos_reads += 1 + return self.qpos.clone() + + +class _Simulation: + """Resolve only one explicitly selected native rigid object.""" + + def __init__(self, entity: _RigidObject) -> None: + self.entity = entity + + def get_rigid_object(self, uid: str) -> _RigidObject | None: + return self.entity if uid == "native_cube" else None + + def get_articulation(self, uid: str) -> None: + del uid + return None + + +def _compiled_segment(*, settle_preset: str = "fast"): + """Compile one segment containing both supported policy types.""" + payload = { + "schema_version": 1, + "program_id": "policy_test", + "integration": { + "robot_profile": "test_robot", + "scene_registry": "test_scene", + "runtime_preset": "safe", + }, + "targets": { + "drop": { + "kind": "cyclic_pose", + "values": [ + { + "position": [0.0, 0.0, 0.0], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + } + ], + } + }, + "program": { + "kind": "segment", + "name": "place", + "steps": { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "at": {"kind": "target_ref", "target": "drop"}, + }, + }, + "post": [ + { + "kind": "wait_stable", + "entity": "cube", + "preset": settle_preset, + } + ], + "validators": [ + { + "kind": "object_near_target", + "object": "cube", + "target": "drop", + "position_tolerance": 0.05, + } + ], + }, + } + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StaticStateProvider(), + ), + ) + ) + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile( + decode_expert_program(payload) + ) + return next(compiled.iter_segments()) + + +def _port( + positions: torch.Tensor, + *, + preset: DynamicSettleMonitorCfg | 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]])) + port = SimulationSegmentPolicyPort( + _Simulation(entity), + robot, + SimulationSceneBinding( + registry_id="test_scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="native_cube", + ), + ), + ), + settle_presets={ + "fast": preset + or DynamicSettleMonitorCfg( + min_steps=0, + max_steps=3, + check_interval_steps=1, + required_stable_checks=2, + ) + }, + ) + return port, entity, robot + + +def test_port_implements_both_bridge_policy_protocols() -> None: + """One shared instance serves post-policy and validator boundaries.""" + port, _, _ = _port(torch.zeros(2, 3)) + + assert isinstance(port, SegmentPostPolicyPort) + assert isinstance(port, SegmentPostPolicyMetadataPort) + assert isinstance(port, SegmentPostPolicyResultPort) + assert isinstance(port, SegmentValidatorPort) + assert isinstance(port, SegmentValidatorMetadataPort) + assert port.settle_preset_ids == ("fast",) + + +def test_pure_preflight_validates_hooks_without_reading_live_state() -> None: + """Static hook validation emits no hold and samples no pose or qpos.""" + segment = _compiled_segment() + port, entity, robot = _port(torch.zeros(2, 3)) + + port.validate_policy(segment.post_policies[0], segment=segment) + port.validate_validator(segment.validators[0], segment=segment) + + assert robot.qpos_reads == 1 + assert entity.pose_reads == 0 + + +def test_pure_preflight_rejects_unknown_settle_preset_without_observation() -> None: + """An unknown preset fails before policy iteration can sample live state.""" + segment = _compiled_segment(settle_preset="missing") + port, entity, robot = _port(torch.zeros(2, 3)) + + with pytest.raises(KeyError, match="Unknown settle preset 'missing'"): + port.validate_policy(segment.post_policies[0], segment=segment) + + assert robot.qpos_reads == 1 + 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.""" + segment = _compiled_segment() + port, _, robot = _port(torch.zeros(2, 3)) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + first = next(actions) + assert torch.equal(first, robot.qpos) + first.fill_(99.0) + with pytest.raises(StopIteration): + next(actions) + assert torch.equal(robot.qpos, torch.tensor([[1.0, 2.0], [3.0, 4.0]])) + + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + assert metadata["status"] == "settled" + assert metadata["preset"] == "fast" + assert metadata["thresholds"] == { + "linear_velocity": 0.03, + "angular_velocity": 0.2, + "min_steps": 0, + "max_steps": 3, + "check_interval_steps": 1, + "required_stable_checks": 2, + } + assert metadata["state"]["elapsed_steps"] == 1 + assert metadata["state"]["settled_mask"] == [True, True] + assert metadata["state"]["timeout_mask"] == [False, False] + assert metadata["state"]["max_linear_speed"] == [0.0, 0.0] + assert port.post_policy_result( + segment.post_policies[0], + segment=segment, + ).tolist() == [True, 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() + port, entity, _ = _port(torch.zeros(2, 3)) + entity.body_data.lin_vel[1, 0] = 1.0 + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + assert sum(1 for _ in (next(actions), next(actions), next(actions))) == 3 + with pytest.raises(StopIteration): + next(actions) + + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + assert metadata["status"] == "timed_out" + assert metadata["state"]["elapsed_steps"] == 3 + assert metadata["state"]["settled_mask"] == [True, False] + assert metadata["state"]["timeout_mask"] == [False, True] + assert metadata["state"]["max_linear_speed"] == [0.0, 1.0] + assert port.post_policy_result( + segment.post_policies[0], + segment=segment, + ).tolist() == [True, False] + + +def test_in_progress_settling_metadata_uses_json_null_for_unchecked_speeds() -> None: + segment = _compiled_segment() + port, _, _ = _port( + torch.zeros(2, 3), + preset=DynamicSettleMonitorCfg( + min_steps=2, + max_steps=4, + check_interval_steps=1, + required_stable_checks=1, + ), + ) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + next(actions) + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + actions.close() + + assert metadata["status"] == "running" + assert metadata["state"]["max_linear_speed"] == [None, None] + assert metadata["state"]["max_angular_speed"] == [None, None] + json.dumps(metadata, allow_nan=False, sort_keys=True) + + +def test_wait_stable_excludes_inactive_moving_row_from_completion() -> None: + """A failed runtime row cannot block or pass a later settling policy.""" + segment = _compiled_segment() + port, entity, _ = _port(torch.zeros(2, 3)) + entity.body_data.lin_vel[1, 0] = 1.0 + active_mask = torch.tensor([True, False]) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=active_mask, + ) + + assert sum(1 for _ in actions) == 1 + + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + assert metadata["status"] == "settled" + assert metadata["active_mask"] == [True, False] + assert metadata["state"]["active_mask"] == [True, False] + assert metadata["state"]["settled_mask"] == [True, False] + assert metadata["state"]["timeout_mask"] == [False, False] + assert metadata["state"]["max_linear_speed"] == [0.0, None] + assert port.post_policy_result( + segment.post_policies[0], + segment=segment, + ).tolist() == [True, False] + + +def test_wait_stable_skips_when_no_rows_remain_active() -> None: + """An empty active cohort completes without an environment hold.""" + segment = _compiled_segment() + port, _, _ = _port(torch.zeros(2, 3)) + + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.zeros(2, dtype=torch.bool), + ) + + assert tuple(actions) == () + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + assert metadata["status"] == "skipped" + assert metadata["state"]["settled_mask"] == [False, False] + assert metadata["state"]["timeout_mask"] == [False, False] + assert port.post_policy_result( + segment.post_policies[0], + segment=segment, + ).tolist() == [False, False] + + +def test_object_near_target_validates_rows_independently() -> None: + """The validator compares explicit native object poses row by row.""" + segment = _compiled_segment() + port, _, _ = _port(torch.tensor([[0.01, 0.0, 0.0], [0.20, 0.0, 0.0]])) + + result = port.validate(segment.validators[0], segment=segment) + + assert result.dtype == torch.bool + assert result.tolist() == [True, False] + metadata = port.validator_metadata(segment.validators[0], segment=segment) + assert metadata["kind"] == "object_near_target" + assert metadata["object_id"] == "cube" + assert metadata["target_id"] == "drop" + assert metadata["position_tolerance"] == 0.05 + assert metadata["position_error"] == pytest.approx([0.01, 0.20]) + assert metadata["accepted_mask"] == [True, False] + + +def test_policy_port_rejects_unbound_native_entities_and_foreign_members() -> None: + """Bindings and compiled segment ownership are exact fail-closed boundaries.""" + binding = SimulationSceneBinding( + registry_id="test_scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="missing", + simulation_uid="unknown", + ), + ), + ) + robot = _Robot(torch.zeros(2, 2)) + with pytest.raises(KeyError, match="unknown"): + SimulationSegmentPolicyPort( + _Simulation(_RigidObject(torch.zeros(2, 3))), + robot, + binding, + ) + + segment = _compiled_segment() + other = _compiled_segment() + port, _, _ = _port(torch.zeros(2, 3)) + with pytest.raises(ValueError, match="does not belong"): + tuple( + port.actions( + other.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + ) + + +__all__: list[str] = [] diff --git a/tests/gym/envs/test_demo.py b/tests/gym/envs/test_demo.py index 1cbfcb29e..b0ef3a740 100644 --- a/tests/gym/envs/test_demo.py +++ b/tests/gym/envs/test_demo.py @@ -20,15 +20,99 @@ import threading from typing import Any +from unittest.mock import Mock import pytest import torch from tensordict import TensorDict -from embodichain.lab.gym.envs.demo import DemoSegment, execute_demo_episode +from embodichain.lab.gym.envs.demo import ( + DemoSegment, + DemoSegmentResult, + ProcessedEnvAction, + execute_demo_episode, +) from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv +def test_processed_env_action_owns_value_and_metadata() -> None: + value = torch.tensor([[1.0, 2.0]]) + metadata = {"semantic_id": "pick", "segments": ["approach"]} + + action = ProcessedEnvAction(value=value, metadata=metadata) + value.zero_() + metadata["segments"].append("close") + snapshot = action.snapshot() + + assert action.value.tolist() == [[1.0, 2.0]] + assert dict(action.metadata) == { + "semantic_id": "pick", + "segments": ["approach"], + } + assert snapshot is not action + assert snapshot.value is not action.value + + +def test_demo_segment_result_owns_json_safe_lifecycle_metadata() -> None: + metadata = { + "runtime": {"status": "completed"}, + "validation": {"accepted_mask": [True, False]}, + } + result = DemoSegmentResult( + segment_id=0, + name="place", + start_step=0, + end_step=2, + success=False, + metadata=metadata, + ) + + metadata["runtime"]["status"] = "mutated" + exported = result.to_metadata() + exported["metadata"]["validation"]["accepted_mask"][0] = False + + assert result.metadata["runtime"]["status"] == "completed" + assert result.metadata["validation"]["accepted_mask"] == [True, False] + + +def test_demo_segment_result_rejects_non_json_metadata() -> None: + with pytest.raises(TypeError, match="non-JSON value Tensor"): + DemoSegmentResult( + segment_id=0, + name="place", + start_step=0, + end_step=1, + success=True, + metadata={"mask": torch.tensor([True])}, + ) + + +def test_embodied_env_skips_preprocessing_for_processed_action() -> None: + env = object.__new__(EmbodiedEnv) + env._num_envs = 2 + env._traj_buffer = None + env.action_manager = Mock() + env._demo_no_auto_reset = False + action = ProcessedEnvAction(value=torch.ones(2, 3)) + + normalized = env._normalize_demo_action(action) + processed = env._preprocess_action(normalized) + + assert isinstance(normalized, ProcessedEnvAction) + assert normalized is not action + assert torch.equal(processed, action.value) + env.action_manager.process_action.assert_not_called() + + +def test_embodied_env_validates_processed_action_batch_size() -> None: + env = object.__new__(EmbodiedEnv) + env._num_envs = 2 + action = ProcessedEnvAction(value=torch.ones(1, 3)) + + with pytest.raises(ValueError, match="batch size"): + env._normalize_demo_action(action) + + class _SegmentedEnv: """Small environment stub that supports lazy two-segment planning.""" @@ -94,6 +178,142 @@ def test_execute_demo_episode_runs_lazy_segments_as_one_episode() -> None: assert not env._demo_no_auto_reset +class _LifecycleMetadataEnv: + """Populate one shared metadata mapping at lazy lifecycle boundaries.""" + + def __init__(self) -> None: + self.num_envs = 1 + self.lifecycle = {"runtime": None, "validation": None} + + def create_demo_segments(self): + def actions(): + yield 1 + self.lifecycle["runtime"] = {"status": "completed"} + + def validate() -> bool: + self.lifecycle["validation"] = {"accepted_mask": [True]} + return True + + return ( + DemoSegment( + actions=actions(), + name="lifecycle", + metadata=self.lifecycle, + validator=validate, + ), + ) + + def step(self, action: int): + del action + return ( + None, + torch.zeros(1), + torch.zeros(1, dtype=torch.bool), + torch.zeros(1, dtype=torch.bool), + {"success": torch.tensor([True])}, + ) + + def is_task_success(self) -> torch.Tensor: + return torch.tensor([True]) + + +class _EmptySuccessfulSegmentEnv: + """Expose an empty ordinary segment whose callbacks otherwise succeed.""" + + num_envs = 1 + + def __init__(self) -> None: + self.validator_calls = 0 + self.step_calls = 0 + + def create_demo_segments(self): + return ( + DemoSegment( + actions=(), + name="empty", + validator=self._validate, + ), + ) + + def _validate(self) -> bool: + self.validator_calls += 1 + return True + + def step(self, action: object): + del action + self.step_calls += 1 + raise AssertionError("An empty segment must not call env.step().") + + @staticmethod + def is_task_success() -> torch.Tensor: + return torch.tensor([True]) + + +def test_execute_demo_episode_snapshots_finalized_lifecycle_metadata() -> None: + env = _LifecycleMetadataEnv() + + result = execute_demo_episode(env) + env.lifecycle["runtime"]["status"] = "mutated" + + assert result.segments[0].metadata == { + "runtime": {"status": "completed"}, + "validation": {"accepted_mask": [True]}, + } + + +def test_empty_ordinary_segment_keeps_existing_empty_segment_guard() -> None: + env = _EmptySuccessfulSegmentEnv() + + result = execute_demo_episode(env) + + assert env.step_calls == 0 + assert env.validator_calls == 0 + assert not result.completed + assert result.terminal_reason == "empty_segment" + assert result.segments[0].failure_reason == "empty_segment" + + +class _GeneratorFailureEnv: + """Raise between lazy actions and expose an emergency hold callback.""" + + def __init__(self) -> None: + self.num_envs = 1 + self.actions: list[int] = [] + self.abort_calls: list[tuple[str, bool]] = [] + + def create_demo_segments(self): + def actions(): + yield 1 + raise ValueError("planner stream failed") + + def abort(reason: str, *, last_action_consumed: bool): + self.abort_calls.append((reason, last_action_consumed)) + yield 0 + + return (DemoSegment(actions=actions(), abort_actions=abort),) + + def step(self, action: int): + self.actions.append(action) + return ( + None, + torch.zeros(1), + torch.zeros(1, dtype=torch.bool), + torch.zeros(1, dtype=torch.bool), + {}, + ) + + +def test_action_generator_failure_safe_stops_before_propagating() -> None: + env = _GeneratorFailureEnv() + + with pytest.raises(RuntimeError, match="action generation") as error: + execute_demo_episode(env) + + assert isinstance(error.value.__cause__, ValueError) + assert env.actions == [1, 0] + assert env.abort_calls == [("action_generation_failed", True)] + + class _TerminatingEnv(_SegmentedEnv): def create_demo_segments(self): return (DemoSegment(actions=(1, 2, 3), name="pick"),) @@ -213,6 +433,30 @@ def test_vector_failure_aborts_peer_and_preserves_per_env_reason() -> None: assert result.lengths == (2, 2) +class _RowIndependentFailureEnv(_VectorFailureEnv): + def create_demo_segments(self): + return ( + DemoSegment( + actions=(1, 2, 3), + name="shared", + failure_policy="row_independent", + ), + ) + + +def test_row_independent_failure_freezes_only_failed_environment() -> None: + env = _RowIndependentFailureEnv() + + result = execute_demo_episode(env) + + assert env.actions == [1, 2, 3] + assert env.masked_actions == [(3, (False, True))] + assert result.completed_by_env == (False, True) + assert result.terminal_reasons == ("failure", "success") + assert result.success == (False, True) + assert result.lengths == (2, 3) + + class _ValidatedSegmentEnv(_SegmentedEnv): def __init__(self, validation: bool) -> None: super().__init__() @@ -388,6 +632,35 @@ def test_validator_batch_abort_has_consistent_peer_status() -> None: assert result.segments[0].failure_reason == "segment_validation_failed" +class _RowIndependentValidatorEnv(_VectorValidatorEnv): + def create_demo_segments(self): + return ( + DemoSegment( + actions=(1,), + name="validated", + validator=lambda: torch.tensor([True, False]), + failure_policy="row_independent", + ), + ) + + +def test_row_independent_validator_keeps_accepted_peer_active() -> None: + result = execute_demo_episode(_RowIndependentValidatorEnv()) + + assert result.segments[0].successes == (True, False) + assert result.segments[0].failure_reasons == ( + None, + "segment_validation_failed", + ) + assert result.completed_by_env == (True, False) + assert result.terminal_reasons == ("success", "segment_validation_failed") + + +def test_demo_segment_rejects_unknown_failure_policy() -> None: + with pytest.raises(ValueError, match="failure_policy"): + DemoSegment(actions=(1,), failure_policy="continue") + + class _CancellationEnv(_ValidatedSegmentEnv): def __init__(self) -> None: super().__init__(validation=True) diff --git a/tests/gym/envs/test_embodied_env_expert_program.py b/tests/gym/envs/test_embodied_env_expert_program.py new file mode 100644 index 000000000..fc7892b9f --- /dev/null +++ b/tests/gym/envs/test_embodied_env_expert_program.py @@ -0,0 +1,92 @@ +# ---------------------------------------------------------------------------- +# 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 explicit Expert Program environment integration hooks.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from embodichain.lab.gym.envs.demo import DemoSegment +from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv, EmbodiedEnvCfg + + +class _FakeBridge: + """Minimal bridge protocol used by the environment adapter test.""" + + def __init__(self, segment: DemoSegment) -> None: + self._segment = segment + self.iteration_count = 0 + + def iter_segments(self): + """Yield the configured segment lazily.""" + self.iteration_count += 1 + yield self._segment + + +class _DeclarativeEnv(EmbodiedEnv): + """Environment stub with explicit compiler and bridge factories.""" + + def compile_expert_program(self, program): + self.compiled_input = program + return self.compiled_program + + def create_expert_program_bridge(self, program): + self.bridge_input = program + return self.bridge + + +def _uninitialized_env(cls: type[EmbodiedEnv], expert_program: object) -> EmbodiedEnv: + """Create an environment instance without starting simulation.""" + env = object.__new__(cls) + env.cfg = SimpleNamespace(expert_program=expert_program) + return env + + +def test_embodied_env_cfg_disables_expert_program_by_default() -> None: + """Declarative execution remains an explicit opt-in configuration.""" + cfg = EmbodiedEnvCfg() + + assert cfg.expert_program is None + + +def test_create_demo_segments_uses_explicit_compiler_and_bridge_hooks() -> None: + """Configured programs flow through provider and runtime factories lazily.""" + program = object() + compiled_program = object() + expected_segment = DemoSegment(actions=(), name="declarative") + bridge = _FakeBridge(expected_segment) + env = _uninitialized_env(_DeclarativeEnv, program) + env.compiled_program = compiled_program + env.bridge = bridge + + segments = env.create_demo_segments(debug_mode=True) + + assert bridge.iteration_count == 0 + assert tuple(segments) == (expected_segment,) + assert bridge.iteration_count == 1 + assert env.compiled_input is program + assert env.bridge_input is compiled_program + + +def test_configured_program_requires_explicit_scene_provider_hook() -> None: + """The base environment never guesses a live scene provider.""" + env = _uninitialized_env(EmbodiedEnv, object()) + + with pytest.raises(NotImplementedError, match="explicit scene resolver"): + env.create_demo_segments() diff --git a/tests/gym/envs/test_settling.py b/tests/gym/envs/test_settling.py new file mode 100644 index 000000000..d1476b842 --- /dev/null +++ b/tests/gym/envs/test_settling.py @@ -0,0 +1,145 @@ +# ---------------------------------------------------------------------------- +# 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 pytest +import torch + +from embodichain.lab.gym.envs.settling import ( + DynamicSettleMonitor, + DynamicSettleMonitorCfg, + DynamicSettleSample, +) + + +def _sample( + linear: tuple[float, ...], angular: tuple[float, ...] +) -> DynamicSettleSample: + return DynamicSettleSample( + entity_id="cube", + linear_speed=torch.tensor(linear, dtype=torch.float32).unsqueeze(1), + angular_speed=torch.tensor(angular, dtype=torch.float32).unsqueeze(1), + ) + + +def test_settle_monitor_tracks_rows_independently_and_owns_metadata() -> None: + env_ids = torch.tensor([4, 9], dtype=torch.long) + monitor = DynamicSettleMonitor( + DynamicSettleMonitorCfg( + min_steps=1, + max_steps=5, + check_interval_steps=1, + required_stable_checks=2, + ), + env_ids, + ) + + first = monitor.observe((_sample((0.0, 1.0), (0.0, 1.0)),), elapsed_steps=1) + second = monitor.observe((_sample((0.0, 1.0), (0.0, 1.0)),), elapsed_steps=2) + third = monitor.observe((_sample((0.0, 0.0), (0.0, 0.0)),), elapsed_steps=3) + final = monitor.observe((_sample((0.0, 0.0), (0.0, 0.0)),), elapsed_steps=4) + + assert first.stable_counts.tolist() == [1, 0] + assert second.settled_mask.tolist() == [True, False] + assert third.stable_counts.tolist() == [2, 1] + assert final.settled_mask.tolist() == [True, True] + assert final.timeout_mask.tolist() == [False, False] + assert final.complete is True + metadata = final.to_metadata() + assert metadata["env_ids"] == [4, 9] + assert metadata["settled_mask"] == [True, True] + + env_ids[0] = 100 + assert monitor.env_ids.tolist() == [4, 9] + + +def test_settle_monitor_duplicate_observation_is_idempotent() -> None: + monitor = DynamicSettleMonitor( + DynamicSettleMonitorCfg( + min_steps=0, + max_steps=3, + check_interval_steps=1, + required_stable_checks=2, + ), + torch.tensor([0], dtype=torch.long), + ) + + first = monitor.observe((_sample((0.0,), (0.0,)),), elapsed_steps=0) + duplicate = monitor.observe((_sample((0.0,), (0.0,)),), elapsed_steps=0) + second = monitor.observe((_sample((0.0,), (0.0,)),), elapsed_steps=1) + + assert first.checked is True + assert duplicate.checked is False + assert duplicate.stable_counts.tolist() == [1] + assert second.settled_mask.tolist() == [True] + assert second.observation_count == 2 + + +def test_settle_monitor_marks_only_unresolved_rows_timed_out() -> None: + monitor = DynamicSettleMonitor( + DynamicSettleMonitorCfg( + min_steps=0, + max_steps=1, + check_interval_steps=1, + required_stable_checks=1, + ), + torch.tensor([0, 1], dtype=torch.long), + ) + + state = monitor.observe((_sample((0.0, 1.0), (0.0, 1.0)),), elapsed_steps=1) + + assert state.settled_mask.tolist() == [True, False] + assert state.timeout_mask.tolist() == [False, True] + assert state.complete is True + + +@pytest.mark.parametrize( + ("kwargs", "match"), + ( + ({"min_steps": -1}, "min_steps"), + ({"max_steps": 1, "min_steps": 2}, "max_steps"), + ({"check_interval_steps": 0}, "check_interval_steps"), + ({"linear_velocity_threshold": float("nan")}, "linear_velocity_threshold"), + ( + {"min_steps": 0, "max_steps": 0, "required_stable_checks": 2}, + "cannot be reached", + ), + ), +) +def test_settle_monitor_cfg_rejects_invalid_values( + kwargs: dict[str, object], match: str +) -> None: + with pytest.raises((TypeError, ValueError), match=match): + DynamicSettleMonitorCfg(**kwargs) + + +def test_settle_monitor_rejects_regressing_steps_and_incomplete_samples() -> None: + monitor = DynamicSettleMonitor( + DynamicSettleMonitorCfg( + min_steps=0, + max_steps=2, + required_stable_checks=1, + ), + torch.tensor([0], dtype=torch.long), + ) + sample = _sample((1.0,), (1.0,)) + monitor.observe((sample,), elapsed_steps=1) + + with pytest.raises(ValueError, match="monotonic"): + monitor.observe((sample,), elapsed_steps=0) + with pytest.raises(ValueError, match="contain DynamicSettleSample"): + monitor.observe((), elapsed_steps=2) diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index 12d46874b..db3119281 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -509,6 +509,37 @@ def test_different_max_episode_steps(): class TestConfigToCfgFromFile: + @staticmethod + def _minimal_gym_config() -> dict[str, object]: + """Return a minimal config that reaches the generic parser.""" + return { + "id": "EmbodiedEnv-v1", + "env": {}, + "robot": { + "class_type": "URRobot", + "robot_type": "ur5", + "uid": "TestUR5", + }, + } + + @staticmethod + def _expert_program_payload() -> dict[str, object]: + """Return one minimal strict Expert Program payload.""" + return { + "schema_version": 1, + "program_id": "configured_pick", + "integration": { + "robot_profile": "default_robot", + "scene_registry": "default_scene", + "runtime_preset": "default_runtime", + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + } + def test_robot_class_type_preserves_ur_variant(self): config = { "id": "EmbodiedEnv-v1", @@ -532,6 +563,107 @@ def test_robot_class_type_preserves_ur_variant(self): "uid": "TestUR5", } + def test_expert_program_path_is_resolved_from_gym_config_source( + self, + tmp_path, + ) -> None: + """A serialized program path is relative to its Gym config file.""" + gym_dir = tmp_path / "gym" / "task" + program_dir = tmp_path / "expert_program" + gym_dir.mkdir(parents=True) + program_dir.mkdir() + gym_path = gym_dir / "gym_config.json" + program_path = program_dir / "program.yaml" + save_config(program_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = "../../expert_program/program.yaml" + + cfg = config_to_cfg( + config, + manager_modules=DEFAULT_MANAGER_MODULES, + source_path=gym_path, + ) + + assert cfg.expert_program.program_id == "configured_pick" + assert cfg.expert_program.integration.scene_registry == "default_scene" + + def test_build_env_cfg_loads_source_relative_expert_program( + self, + tmp_path, + ) -> None: + """The normal file launcher attaches the decoded program before init.""" + gym_dir = tmp_path / "gym" + program_dir = tmp_path / "programs" + gym_dir.mkdir() + program_dir.mkdir() + gym_path = gym_dir / "gym_config.json" + program_path = program_dir / "program.json" + save_config(program_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = "../programs/program.json" + save_config(gym_path, config) + args = argparse.Namespace( + gym_config=str(gym_path), + num_envs=1, + device="cpu", + headless=True, + renderer=None, + gpu_id=0, + arena_space=2.0, + max_episodes=None, + filter_visual_rand=False, + filter_dataset_saving=False, + preview=False, + action_config=None, + ) + + cfg, _, _ = build_env_cfg_from_args(args) + + assert cfg.expert_program.program_id == "configured_pick" + + def test_config_to_cfg_uses_cwd_without_source_path( + self, + tmp_path, + monkeypatch, + ) -> None: + """Dictionary-only callers retain explicit current-directory semantics.""" + program_path = tmp_path / "program.yaml" + save_config(program_path, self._expert_program_payload()) + monkeypatch.chdir(tmp_path) + config = self._minimal_gym_config() + config["expert_program_path"] = "program.yaml" + + cfg = config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + assert cfg.expert_program.program_id == "configured_pick" + + @pytest.mark.parametrize("value", [None, True, 1, {}, "", " program.yaml"]) + def test_expert_program_path_rejects_ambiguous_values( + self, + value, + ) -> None: + """The path field never accepts coercion, null, or outer whitespace.""" + config = self._minimal_gym_config() + config["expert_program_path"] = value + + with pytest.raises((TypeError, ValueError), match="expert_program_path"): + config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + def test_expert_program_path_missing_file_fails_before_environment_init( + self, + tmp_path, + ) -> None: + """A configured program must exist when the Gym config is decoded.""" + config = self._minimal_gym_config() + config["expert_program_path"] = "missing.yaml" + + with pytest.raises(FileNotFoundError, match="missing.yaml"): + config_to_cfg( + config, + manager_modules=DEFAULT_MANAGER_MODULES, + source_path=tmp_path / "gym_config.json", + ) + def test_yaml_gym_config_parses_to_cfg(self, tmp_path): config = { "id": "EmbodiedEnv-v1", diff --git a/tests/lab/scripts/test_run_env.py b/tests/lab/scripts/test_run_env.py index 0c495a4f5..788a89f9b 100644 --- a/tests/lab/scripts/test_run_env.py +++ b/tests/lab/scripts/test_run_env.py @@ -16,6 +16,7 @@ from __future__ import annotations +import json from types import SimpleNamespace from unittest.mock import MagicMock @@ -27,6 +28,7 @@ from embodichain.lab.scripts import run_env from embodichain.lab.scripts.run_env import ( _create_parser, + _load_expert_program, _run_replay_control_loop, generate_function, ) @@ -40,6 +42,24 @@ VISER_POLL_INTERVAL = 0.05 +def _expert_program_payload() -> dict[str, object]: + """Return one minimal strict Expert Program payload.""" + return { + "schema_version": 1, + "program_id": "cli_pick", + "integration": { + "robot_profile": "default_robot", + "scene_registry": "default_scene", + "runtime_preset": "default_runtime", + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + } + + class _LegacyProgressEnv: num_envs = 1 @@ -123,6 +143,86 @@ def test_run_env_preserves_configured_viser_image_fps() -> None: assert merged["visualization"]["sensor_image_fps"] == configured_fps +def test_run_env_parser_accepts_expert_program_path() -> None: + """The declarative program is an explicit, opt-in CLI input.""" + program_path = "program.yaml" + + args = _create_parser().parse_args( + ["--gym_config", GYM_CONFIG_PATH, "--expert-program", program_path] + ) + + assert args.expert_program == program_path + + +def test_run_env_parser_accepts_debug_trace_mode() -> None: + """Failed Expert Program attempts can expose their structured trace.""" + args = _create_parser().parse_args( + ["--gym_config", GYM_CONFIG_PATH, "--debug-mode"] + ) + + assert args.debug_mode is True + + +@pytest.mark.parametrize("suffix", [".json", ".yaml", ".yml"]) +def test_load_expert_program_safely_decodes_supported_files( + tmp_path, + suffix: str, +) -> None: + """JSON and safe YAML inputs share the same strict schema decoder.""" + path = tmp_path / f"program{suffix}" + payload = _expert_program_payload() + if suffix == ".json": + serialized = json.dumps(payload) + else: + import yaml + + serialized = yaml.safe_dump(payload) + path.write_text(serialized, encoding="utf-8") + + program = _load_expert_program(path) + + assert program.program_id == "cli_pick" + assert program.integration.scene_registry == "default_scene" + + +@pytest.mark.parametrize( + ("filename", "serialized", "message"), + [ + ( + "program.json", + '{"schema_version": 1, "schema_version": 1}', + "Duplicate JSON key", + ), + ( + "program.yaml", + "schema_version: 1\nschema_version: 1\n", + "found duplicate key", + ), + ], +) +def test_load_expert_program_rejects_duplicate_mapping_keys( + tmp_path, + filename: str, + serialized: str, + message: str, +) -> None: + """Ambiguous duplicate keys are rejected before schema decoding.""" + path = tmp_path / filename + path.write_text(serialized, encoding="utf-8") + + with pytest.raises(ValueError, match=message): + _load_expert_program(path) + + +def test_load_expert_program_rejects_unsupported_file_extension(tmp_path) -> None: + """Only explicit JSON and YAML file formats are accepted.""" + path = tmp_path / "program.toml" + path.write_text("schema_version = 1", encoding="utf-8") + + with pytest.raises(ValueError, match=".json, .yaml, or .yml"): + _load_expert_program(path) + + def test_replay_restores_wrapper_state_without_closing_caller_env(monkeypatch) -> None: """Replay leaves the environment close to its CLI owner.""" env = MagicMock() @@ -230,6 +330,34 @@ def test_generate_function_discards_retry_then_commits_once(monkeypatch) -> None assert env.reset_options == [{"save_data": False}, None] +def test_generate_function_logs_failed_trace_in_debug_mode(monkeypatch) -> None: + """Debug retries expose the owned structured episode trace.""" + env = _ResetTrackingEnv() + result = _episode_result(success=False, reason="segment_validation_failed") + warnings: list[str] = [] + monkeypatch.setattr( + "embodichain.lab.scripts.run_env.execute_demo_episode", + lambda *args, **kwargs: result, + ) + monkeypatch.setattr( + "embodichain.lab.scripts.run_env.log_warning", + warnings.append, + ) + + generated = generate_function( + env, + max_attempts=1, + reset_before=False, + debug_mode=True, + ) + + assert not generated + debug_trace = next( + message for message in warnings if "Failed demo trace" in message + ) + assert '"terminal_reason":"segment_validation_failed"' in debug_trace + + def test_generate_function_commits_failed_episode_when_configured(monkeypatch) -> None: """A recorded task failure is a persisted result when explicitly enabled.""" env = _ResetTrackingEnv(save_failed_episodes=True) @@ -421,6 +549,49 @@ def test_cli_aborts_before_closing_environment_once(monkeypatch) -> None: assert env.events == [abort_event, abort_event, ("close", None)] +def test_cli_injects_decoded_expert_program_before_environment_creation( + monkeypatch, +) -> None: + """The CLI attaches the strict program config to the environment config.""" + env = _LifecycleTrackingEnv() + env_cfg = SimpleNamespace(expert_program=None) + decoded_program = object() + args = SimpleNamespace( + replay=False, + replay_mode="kinematic", + preview=True, + expert_program="program.yaml", + ) + parser = MagicMock() + parser.parse_args.return_value = args + make = MagicMock(return_value=env) + + monkeypatch.setattr(run_env, "_create_parser", lambda: parser) + monkeypatch.setattr(run_env, "discover_task_packages", lambda: None) + monkeypatch.setattr(run_env, "execute_init_hooks", lambda: None) + monkeypatch.setattr( + run_env, + "build_env_cfg_from_args", + lambda parsed_args: (env_cfg, {"id": GYM_ID}, {}), + ) + monkeypatch.setattr( + run_env, + "_load_expert_program", + MagicMock(return_value=decoded_program), + ) + monkeypatch.setattr(run_env.gymnasium, "make", make) + monkeypatch.setattr(run_env, "main", lambda *args, **kwargs: None) + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: None, + ) + + run_env.cli([]) + + assert env_cfg.expert_program is decoded_program + make.assert_called_once_with(id=GYM_ID, cfg=env_cfg) + + def test_close_durability_failure_is_not_swallowed() -> None: """A failed recorder barrier makes the runner fail after aborting pending data.""" env = _LifecycleTrackingEnv() diff --git a/tests/utils/test_config_paths.py b/tests/utils/test_config_paths.py new file mode 100644 index 000000000..b05c9fe04 --- /dev/null +++ b/tests/utils/test_config_paths.py @@ -0,0 +1,68 @@ +# ---------------------------------------------------------------------------- +# 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 stable configuration-path resolution.""" + +from __future__ import annotations + +from pathlib import Path + +import pytest + +from embodichain.utils import resolve_config_path as exported_resolve_config_path +from embodichain.utils.config_paths import resolve_config_path + + +def test_resolve_config_path_preserves_existing_path(tmp_path: Path) -> None: + config_path = tmp_path / "config.yaml" + config_path.write_text("id: Test-v0\n", encoding="utf-8") + + assert resolve_config_path(config_path) == config_path + + +def test_resolve_config_path_is_exported_from_utils_package() -> None: + assert exported_resolve_config_path is resolve_config_path + + +def test_resolve_config_path_preserves_ordinary_relative_path( + tmp_path: Path, + monkeypatch, +) -> None: + monkeypatch.chdir(tmp_path) + + assert resolve_config_path("local/config.yaml") == Path("local/config.yaml") + + +def test_resolve_config_path_redirects_packaged_task_config( + tmp_path: Path, + monkeypatch, +) -> None: + monkeypatch.chdir(tmp_path) + + resolved = resolve_config_path("embodichain_tasks/configs/gym/cobotmagic.json") + + assert resolved.is_file() + assert resolved.name == "cobotmagic.json" + + +def test_resolve_config_path_rejects_packaged_path_escape( + tmp_path: Path, + monkeypatch, +) -> None: + monkeypatch.chdir(tmp_path) + + with pytest.raises(ValueError, match="stay within the package"): + resolve_config_path("embodichain_tasks/configs/../VERSION") From d6be5c197f1674daf2403ee68f35ace1bd52fc41 Mon Sep 17 00:00:00 2001 From: skywhite1024 <129768272+skywhite1024@users.noreply.github.com> Date: Fri, 21 Aug 2026 17:24:01 +0800 Subject: [PATCH 08/29] refactor(expert-program): remove handover receiver alias --- embodichain/lab/gym/envs/expert_program/cfg.py | 11 +---------- embodichain/lab/gym/envs/expert_program/compiler.py | 4 ---- embodichain/lab/gym/envs/expert_program/decoder.py | 10 +--------- tests/gym/envs/expert_program/test_compiler.py | 5 ++--- tests/gym/envs/expert_program/test_decoder.py | 4 ++-- 5 files changed, 6 insertions(+), 28 deletions(-) diff --git a/embodichain/lab/gym/envs/expert_program/cfg.py b/embodichain/lab/gym/envs/expert_program/cfg.py index 1e9b43278..04959ce1d 100644 --- a/embodichain/lab/gym/envs/expert_program/cfg.py +++ b/embodichain/lab/gym/envs/expert_program/cfg.py @@ -356,7 +356,6 @@ class HandOverCfg: """Declarative request to transfer one held object between resources.""" object: str = MISSING - receiver: str | None = None final_target: TargetRefCfg | None = None resources: dict[str, str] = field(default_factory=dict) kind: str = "hand_over" @@ -364,20 +363,12 @@ class HandOverCfg: def __post_init__(self) -> None: """Validate object, destination resource, and optional target.""" _validate_identifier(self.object, field_name="object") - if self.receiver is not None: - _validate_identifier(self.receiver, field_name="receiver") if ( self.final_target is not None and type(self.final_target) is not TargetRefCfg ): raise TypeError("final_target must be exactly TargetRefCfg or None.") - resources = _validate_resources(self.resources, field_name="resources") - if self.receiver is not None: - selected = resources.get("destination") - if selected is not None and selected != self.receiver: - raise ValueError("receiver conflicts with resources['destination'].") - resources["destination"] = self.receiver - self.resources = resources + self.resources = _validate_resources(self.resources, field_name="resources") _validate_kind(self.kind, expected="hand_over", field_name="kind") diff --git a/embodichain/lab/gym/envs/expert_program/compiler.py b/embodichain/lab/gym/envs/expert_program/compiler.py index cc12cea0d..ca01f6663 100644 --- a/embodichain/lab/gym/envs/expert_program/compiler.py +++ b/embodichain/lab/gym/envs/expert_program/compiler.py @@ -258,7 +258,6 @@ def _snapshot_semantic_call(call: SemanticCallSpec) -> SemanticCallSpec: if type(call) is HandOver: return HandOver( object=_copy_scene_ref(call.object), - receiver=call.receiver, final_target=( None if call.final_target is None else call.final_target.snapshot() ), @@ -563,7 +562,6 @@ class _CallTemplate: at_target_id: str | None = None on: SceneObjectRef | SceneAffordanceRef | None = None inside: SceneObjectRef | SceneAffordanceRef | None = None - receiver: str | None = None final_target_id: str | None = None articulation: SceneArticulationRef | None = None handle: SceneAffordanceRef | None = None @@ -730,7 +728,6 @@ def _instantiate_call( selections.append(selection) call = HandOver( object=_copy_scene_ref(template.object), - receiver=template.receiver, final_target=final_target, resources=resources, ) @@ -1493,7 +1490,6 @@ def _compile_call( kind="hand_over", source_path=path, object=object_ref, - receiver=cfg.receiver, final_target_id=final_target_id, resources=tuple(sorted(cfg.resources.items())), ) diff --git a/embodichain/lab/gym/envs/expert_program/decoder.py b/embodichain/lab/gym/envs/expert_program/decoder.py index 966f9be8e..4b7e78b4e 100644 --- a/embodichain/lab/gym/envs/expert_program/decoder.py +++ b/embodichain/lab/gym/envs/expert_program/decoder.py @@ -571,15 +571,10 @@ def _decode_call( if kind == "hand_over": _validate_fields( mapping, - allowed=frozenset( - {"kind", "object", "receiver", "final_target", "resources"} - ), + allowed=frozenset({"kind", "object", "final_target", "resources"}), required=frozenset({"kind", "object"}), path=path, ) - receiver = mapping.get("receiver") - if receiver is not None: - receiver = _expect_identifier(receiver, path=(*path, "receiver")) final_target = ( None if mapping.get("final_target") is None @@ -594,7 +589,6 @@ def _decode_call( path=path, kind=kind, object=_expect_identifier(mapping["object"], path=(*path, "object")), - receiver=receiver, final_target=final_target, resources=resources, ) # type: ignore[return-value] @@ -1137,8 +1131,6 @@ def encode_semantic_call(call: SemanticCallCfg) -> dict[str, object]: result["inside"] = call.inside elif type(call) is HandOverCfg: result["object"] = call.object - if call.receiver is not None: - result["receiver"] = call.receiver if call.final_target is not None: result["final_target"] = { "kind": call.final_target.kind, diff --git a/tests/gym/envs/expert_program/test_compiler.py b/tests/gym/envs/expert_program/test_compiler.py index f3a94abc8..afed14046 100644 --- a/tests/gym/envs/expert_program/test_compiler.py +++ b/tests/gym/envs/expert_program/test_compiler.py @@ -178,7 +178,6 @@ def _assert_semantic_call_equal( _assert_pose_equal(actual.at, expected.at) elif type(actual) is HandOver and type(expected) is HandOver: assert actual.object == expected.object - assert actual.receiver == expected.receiver assert (actual.final_target is None) == (expected.final_target is None) if actual.final_target is not None and expected.final_target is not None: _assert_pose_equal(actual.final_target, expected.final_target) @@ -209,7 +208,7 @@ def test_compiler_matches_direct_python_semantic_calls_and_sequence_order() -> N InvokeCfg( call=HandOverCfg( object="sim_cube", - receiver="right_actor", + resources={"destination": "right_actor"}, final_target=TargetRefCfg(target="handover_pose"), ) ), @@ -242,7 +241,7 @@ def test_compiler_matches_direct_python_semantic_calls_and_sequence_order() -> N ), HandOver( object=SceneObjectRef("cube"), - receiver="right_actor", + resources={"destination": "right_actor"}, final_target=SemanticPose(target.position, target.quaternion_wxyz), ), RegisteredSemanticCall( diff --git a/tests/gym/envs/expert_program/test_decoder.py b/tests/gym/envs/expert_program/test_decoder.py index d21458dc6..a2997f0cc 100644 --- a/tests/gym/envs/expert_program/test_decoder.py +++ b/tests/gym/envs/expert_program/test_decoder.py @@ -143,7 +143,7 @@ def _invoke(call: dict[str, object]) -> dict[str, object]: { "kind": "hand_over", "object": "cube", - "receiver": "right", + "resources": {"destination": "right"}, "final_target": {"kind": "target_ref", "target": "drop_pose"}, }, { @@ -225,7 +225,7 @@ def test_decoder_supports_every_version_one_semantic_call() -> None: { "kind": "hand_over", "object": "cube", - "receiver": "right_actor", + "resources": {"destination": "right_actor"}, "final_target": { "kind": "target_ref", "target": "drop_pose", From 826454d0ab3a1fc9d3acbc2eba40a28c085ef2fd Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:20:24 +0800 Subject: [PATCH 09/29] feat(agents): add strict expert program frontend --- ...mbodichain.lab.gym.envs.expert_program.rst | 11 +- docs/source/api_reference/public_api.rst | 10 + embodichain/agents/__init__.py | 21 + embodichain/agents/mllm/__init__.py | 29 ++ embodichain/agents/mllm/expert_program.py | 260 +++++++++++ tests/agents/mllm/test_expert_program.py | 433 ++++++++++++++++++ .../test_simulation_environment.py | 200 +++++--- 7 files changed, 898 insertions(+), 66 deletions(-) create mode 100644 embodichain/agents/__init__.py create mode 100644 embodichain/agents/mllm/__init__.py create mode 100644 embodichain/agents/mllm/expert_program.py create mode 100644 tests/agents/mllm/test_expert_program.py diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst index eb8e473f4..a3fb7e654 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst @@ -137,9 +137,16 @@ and 2. Version 2 adds deterministic parallel blocks with explicit barriers. .. autofunction:: decode_expert_program -.. autofunction:: decode_semantic_call +MLLM frontend +------------- -.. autofunction:: encode_semantic_call +The MLLM frontend intentionally accepts only the constrained schema version 1 +surface. Trusted host code remains responsible for authoring version 2 +parallel structure and the integration selection. + +.. autofunction:: embodichain.agents.mllm.decode_mllm_expert_program + +.. autofunction:: embodichain.agents.mllm.compile_mllm_expert_program Compilation and environment integration --------------------------------------- diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index cb0d0fecf..fb558394f 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -9,6 +9,16 @@ covered by a more focused API-reference page. Prefer curated pages for APIs that need deeper explanations or examples. Sphinx obtains signatures and summaries here from the canonical Python docstrings. +embodichain.agents.mllm.expert_program +-------------------------------------- + +.. currentmodule:: embodichain.agents.mllm.expert_program + +.. autosummary:: + + compile_mllm_expert_program + decode_mllm_expert_program + embodichain.data.assets.planner_assets -------------------------------------- diff --git a/embodichain/agents/__init__.py b/embodichain/agents/__init__.py new file mode 100644 index 000000000..071c9ac48 --- /dev/null +++ b/embodichain/agents/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Agent-facing frontends built on EmbodiChain's typed runtime contracts.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/embodichain/agents/mllm/__init__.py b/embodichain/agents/mllm/__init__.py new file mode 100644 index 000000000..607c022d9 --- /dev/null +++ b/embodichain/agents/mllm/__init__.py @@ -0,0 +1,29 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Multimodal-model frontends for typed EmbodiChain agent contracts.""" + +from __future__ import annotations + +from .expert_program import ( + compile_mllm_expert_program, + decode_mllm_expert_program, +) + +__all__ = [ + "compile_mllm_expert_program", + "decode_mllm_expert_program", +] diff --git a/embodichain/agents/mllm/expert_program.py b/embodichain/agents/mllm/expert_program.py new file mode 100644 index 000000000..a54304d78 --- /dev/null +++ b/embodichain/agents/mllm/expert_program.py @@ -0,0 +1,260 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Strict MLLM frontend for declarative Expert Program JSON responses.""" + +from __future__ import annotations + +from collections.abc import Iterator + +from embodichain.lab.gym.envs.expert_program.cfg import ( + EXPERT_PROGRAM_SCHEMA_VERSION, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + OperateArticulationCfg, + PickCfg, + PlaceCfg, + ProgramNodeCfg, + RepeatCfg, + SegmentCfg, + SemanticCallCfg, + SequenceCfg, +) +from embodichain.lab.gym.envs.expert_program.compiler import CompiledProgram +from embodichain.lab.gym.envs.expert_program.decoder import ( + ConfigPath, + ExpertProgramDecodeError, + ExpertProgramValidationContext, + decode_expert_program, + validate_expert_program, +) +from embodichain.lab.gym.envs.expert_program.environment import ( + ExpertProgramEnvironmentAdapter, +) +from embodichain.lab.gym.envs.expert_program.loader import ( + MAX_EXPERT_PROGRAM_BYTES, + parse_expert_program_json, +) + +__all__ = [ + "compile_mllm_expert_program", + "decode_mllm_expert_program", +] + +_CURATED_CALL_TYPES = ( + PickCfg, + PlaceCfg, + HandOverCfg, + OperateArticulationCfg, +) + + +def _iter_calls( + node: ProgramNodeCfg, + *, + path: ConfigPath, +) -> Iterator[tuple[SemanticCallCfg, ConfigPath]]: + """Yield every semantic call and its decoder-compatible source path.""" + if type(node) is InvokeCfg: + yield node.call, (*path, "call") + return + if type(node) is SequenceCfg: + for index, child in enumerate(node.items): + yield from _iter_calls(child, path=(*path, "items", index)) + return + if type(node) is RepeatCfg: + yield from _iter_calls(node.body, path=(*path, "body")) + return + if type(node) is SegmentCfg: + yield from _iter_calls(node.steps, path=(*path, "steps")) + return + raise ExpertProgramDecodeError( + "mllm_program_node_not_allowed", + (*path, "kind"), + "The MLLM frontend permits only Version 1 sequential program nodes.", + ) + + +def _value_at_path(value: object, path: ConfigPath) -> object: + """Return a raw decoded JSON value at one already validated config path.""" + current = value + for part in path: + if type(part) is int: + if type(current) is not list or not 0 <= part < len(current): + raise ExpertProgramDecodeError( + "mllm_payload_mismatch", + path, + "Decoded model payload no longer matches the canonical program.", + ) + current = current[part] + else: + if type(current) is not dict or part not in current: + raise ExpertProgramDecodeError( + "mllm_payload_mismatch", + path, + "Decoded model payload no longer matches the canonical program.", + ) + current = current[part] + return current + + +def _validate_mllm_policy( + config: ExpertProgramCfg, + *, + raw_payload: dict[str, object], +) -> None: + """Apply the narrow agent-facing policy after canonical decoding.""" + if config.schema_version != EXPERT_PROGRAM_SCHEMA_VERSION: + raise ExpertProgramDecodeError( + "mllm_schema_version_not_allowed", + ("schema_version",), + "The MLLM frontend permits only Expert Program schema Version 1.", + ) + for call, path in _iter_calls(config.program, path=("program",)): + if type(call) not in _CURATED_CALL_TYPES: + raise ExpertProgramDecodeError( + "mllm_call_not_allowed", + (*path, "kind"), + "The MLLM frontend permits only curated pick, place, hand_over, " + "and operate_articulation calls.", + ) + raw_call = _value_at_path(raw_payload, path) + if type(raw_call) is not dict: + raise ExpertProgramDecodeError( + "mllm_payload_mismatch", + path, + "Decoded model payload no longer matches the canonical program.", + ) + raw_resources = raw_call.get("resources", {}) + if type(raw_resources) is dict and raw_resources: + raise ExpertProgramDecodeError( + "mllm_resource_override_not_allowed", + (*path, "resources"), + "MLLM responses cannot override robot resource bindings.", + ) + if type(call) is HandOverCfg and call.receiver is not None: + raise ExpertProgramDecodeError( + "mllm_resource_override_not_allowed", + (*path, "receiver"), + "MLLM responses cannot select a hand-over receiver resource.", + ) + if type(call) is OperateArticulationCfg and call.target is None: + raise ExpertProgramDecodeError( + "mllm_articulation_target_not_allowed", + (*path, "target_position"), + "MLLM articulation calls must select a host-declared named target.", + ) + if call.resources: + raise ExpertProgramDecodeError( + "mllm_resource_override_not_allowed", + (*path, "resources"), + "MLLM responses cannot override robot resource bindings.", + ) + + +def decode_mllm_expert_program( + response: str, + *, + integration: ExpertProgramIntegrationCfg, + validation_context: ExpertProgramValidationContext | None = None, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> ExpertProgramCfg: + """Decode one untrusted model response into the canonical program config. + + The model response is a single plain JSON object containing + ``schema_version``, ``program_id``, ``targets``, and ``program``. The trusted + host supplies ``integration``; a response attempting to select its own + integration is rejected rather than silently overwritten. Version 1 curated + calls are the only admitted semantic surface, and robot resource overrides + are forbidden. + + Args: + response: Untrusted model response containing one plain JSON document. + integration: Host-owned scene, robot-profile, and runtime-preset choice. + validation_context: Optional provider-free static reference validator. + max_bytes: Maximum UTF-8 encoded response size. + + Returns: + An owned canonical :class:`ExpertProgramCfg`. + + Raises: + TypeError: If ``integration`` is not an exact integration config. + ExpertProgramDecodeError: If JSON, schema, or MLLM policy validation + fails. + """ + if type(integration) is not ExpertProgramIntegrationCfg: + raise TypeError("integration must be exactly ExpertProgramIntegrationCfg.") + data = parse_expert_program_json(response, max_bytes=max_bytes) + if "integration" in data: + raise ExpertProgramDecodeError( + "model_controlled_integration", + ("integration",), + "MLLM responses cannot select an integration; the host injects it.", + ) + payload = dict(data) + payload["integration"] = { + "robot_profile": integration.robot_profile, + "scene_registry": integration.scene_registry, + "runtime_preset": integration.runtime_preset, + } + config = decode_expert_program(payload) + _validate_mllm_policy(config, raw_payload=payload) + if validation_context is not None: + validate_expert_program(config, validation_context) + return config + + +def compile_mllm_expert_program( + response: str, + *, + adapter: ExpertProgramEnvironmentAdapter, + integration: ExpertProgramIntegrationCfg, + validation_context: ExpertProgramValidationContext | None = None, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> CompiledProgram: + """Decode and compile a model response through the existing environment path. + + This function introduces no MLLM-specific compiler. It delegates the owned + config to :meth:`ExpertProgramEnvironmentAdapter.compile`, which performs the + canonical scene resolution and Expert Program lowering used by every other + frontend. + + Args: + response: Untrusted model response containing one plain JSON document. + adapter: Existing trusted Expert Program environment adapter. + integration: Host-owned scene, robot-profile, and runtime-preset choice. + validation_context: Optional provider-free static reference validator. + max_bytes: Maximum UTF-8 encoded response size. + + Returns: + Provider-free program produced by the existing Expert Program compiler. + + Raises: + TypeError: If ``adapter`` or ``integration`` has the wrong exact type. + ExpertProgramDecodeError: If JSON, schema, or MLLM policy validation + fails. + """ + if type(adapter) is not ExpertProgramEnvironmentAdapter: + raise TypeError("adapter must be exactly ExpertProgramEnvironmentAdapter.") + config = decode_mllm_expert_program( + response, + integration=integration, + validation_context=validation_context, + max_bytes=max_bytes, + ) + return adapter.compile(config) diff --git a/tests/agents/mllm/test_expert_program.py b/tests/agents/mllm/test_expert_program.py new file mode 100644 index 000000000..12542e6eb --- /dev/null +++ b/tests/agents/mllm/test_expert_program.py @@ -0,0 +1,433 @@ +# ---------------------------------------------------------------------------- +# 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 strict MLLM Expert Program frontend.""" + +from __future__ import annotations + +from collections.abc import Iterable +import json + +import pytest + +from embodichain.agents.mllm import ( + compile_mllm_expert_program, + decode_mllm_expert_program, +) +from embodichain.lab.gym.envs.expert_program import ( + CompiledProgram, + EnvironmentStepClock, + ExpertProgramCompileError, + ExpertProgramDecodeError, + ExpertProgramEnvironmentAdapter, + ExpertProgramIntegrationCfg, + PlanningObservationPort, + decode_expert_program, +) +from embodichain.lab.sim.atomic_actions import AtomicActionEngine, EntityState +from embodichain.lab.sim.skills import ( + EffectEvidenceProvider, + Pick, + RobotSkillProfile, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + + +def _integration() -> ExpertProgramIntegrationCfg: + """Return the exact trusted integration selected by the host.""" + return ExpertProgramIntegrationCfg( + robot_profile="test_robot", + scene_registry="test_scene", + runtime_preset="safe", + ) + + +def _invoke(call: dict[str, object]) -> dict[str, object]: + """Wrap one semantic call in an Expert Program invoke node.""" + return {"kind": "invoke", "call": call} + + +def _model_data( + call: dict[str, object] | None = None, + *, + schema_version: int = 1, + program: dict[str, object] | None = None, +) -> dict[str, object]: + """Build the integration-free JSON envelope exposed to the model.""" + if call is None: + call = {"kind": "pick", "object": "cube"} + return { + "schema_version": schema_version, + "program_id": "model_program", + "targets": {}, + "program": _invoke(call) if program is None else program, + } + + +def _model_json( + call: dict[str, object] | None = None, + *, + schema_version: int = 1, + program: dict[str, object] | None = None, +) -> str: + """Serialize one integration-free model response.""" + return json.dumps( + _model_data( + call, + schema_version=schema_version, + program=program, + ) + ) + + +class _UnusedStateProvider: + """Satisfy the static scene contract without allowing live observation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: object, + ) -> EntityState: + """Fail if provider-free compilation accidentally observes the scene.""" + del timestamp, env_ids + raise AssertionError("Provider-free compilation must not observe the scene.") + + +class _CompileOnlyFactory: + """Expose only the scene snapshot needed by adapter compilation.""" + + scene_registry_id = "test_scene" + robot_profile_id = "test_robot" + + def __init__(self) -> None: + self.scene_registry_calls = 0 + + def create_scene_registry(self) -> SceneRegistry: + """Return one canonical object registration and count compilation.""" + self.scene_registry_calls += 1 + return SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_UnusedStateProvider(), + semantic_type="cube", + ), + ) + ) + + def create_robot_skill_profile(self) -> RobotSkillProfile: + """Reject runtime assembly in this compile-only test factory.""" + raise AssertionError("MLLM frontend compilation must not assemble a runtime.") + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + """Reject engine creation in this compile-only test factory.""" + del profile + raise AssertionError("MLLM frontend compilation must not create an engine.") + + def create_planning_observation_provider( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + clock: EnvironmentStepClock, + ) -> PlanningObservationPort: + """Reject observation-port creation during provider-free compilation.""" + del scene_registry, engine, clock + raise AssertionError("MLLM frontend compilation must not create live ports.") + + def create_effect_evidence_providers( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> Iterable[EffectEvidenceProvider]: + """Reject evidence-provider creation during provider-free compilation.""" + del scene_registry, engine, observation_provider + raise AssertionError("MLLM frontend compilation must not create live ports.") + + +def _adapter(factory: _CompileOnlyFactory) -> ExpertProgramEnvironmentAdapter: + """Create the existing production adapter around the compile-only factory.""" + return ExpertProgramEnvironmentAdapter(factory, step_dt=0.02) + + +def test_decoder_injects_exact_host_integration() -> None: + config = decode_mllm_expert_program( + _model_json(), + integration=_integration(), + ) + + assert config.integration.robot_profile == "test_robot" + assert config.integration.scene_registry == "test_scene" + assert config.integration.runtime_preset == "safe" + + +def test_decoder_rejects_model_controlled_integration() -> None: + response = _model_data() + response["integration"] = { + "robot_profile": "attacker_robot", + "scene_registry": "attacker_scene", + "runtime_preset": "unsafe", + } + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + json.dumps(response), + integration=_integration(), + ) + + assert error.value.code == "model_controlled_integration" + assert error.value.path == ("integration",) + + +def test_decoder_rejects_version_two_parallel_program() -> None: + parallel = { + "kind": "parallel", + "branches": [ + _invoke({"kind": "pick", "object": "cube"}), + _invoke({"kind": "pick", "object": "cube"}), + ], + "barrier": {"kind": "barrier", "name": "join"}, + } + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json(schema_version=2, program=parallel), + integration=_integration(), + ) + + assert error.value.code == "mllm_schema_version_not_allowed" + assert error.value.path == ("schema_version",) + + +def test_decoder_rejects_registered_semantic_calls() -> None: + registered = { + "kind": "registered", + "call_id": "vendor.inspect", + "schema_version": 1, + "arguments": {}, + } + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json(registered), + integration=_integration(), + ) + + assert error.value.code == "mllm_call_not_allowed" + assert error.value.path == ("program", "call", "kind") + + +@pytest.mark.parametrize( + "call", + [ + {"kind": "pick", "object": "cube", "resources": {"primary": "left"}}, + { + "kind": "place", + "object": "cube", + "on": "tray", + "resources": {"primary": "left"}, + }, + { + "kind": "hand_over", + "object": "cube", + "receiver": "right", + "resources": {"destination": "right"}, + }, + { + "kind": "operate_articulation", + "articulation": "drawer", + "target": "open", + "resources": {"primary": "left"}, + }, + ], +) +def test_decoder_rejects_explicit_nonempty_resource_overrides( + call: dict[str, object], +) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json(call), + integration=_integration(), + ) + + assert error.value.code == "mllm_resource_override_not_allowed" + assert error.value.path == ("program", "call", "resources") + + +def test_decoder_allows_explicit_empty_resources() -> None: + config = decode_mllm_expert_program( + _model_json({"kind": "pick", "object": "cube", "resources": {}}), + integration=_integration(), + ) + + assert config.program.call.resources == {} # type: ignore[union-attr] + + +def test_decoder_rejects_handover_receiver_resource_selection() -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json( + { + "kind": "hand_over", + "object": "cube", + "receiver": "right", + } + ), + integration=_integration(), + ) + + assert error.value.code == "mllm_resource_override_not_allowed" + assert error.value.path == ("program", "call", "receiver") + + +def test_decoder_rejects_explicit_articulation_motion_target() -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json( + { + "kind": "operate_articulation", + "articulation": "drawer", + "target_position": 1_000_000.0, + "target_displacement": 1_000_000.0, + } + ), + integration=_integration(), + ) + + assert error.value.code == "mllm_articulation_target_not_allowed" + assert error.value.path == ("program", "call", "target_position") + + +def test_decoder_allows_named_articulation_target() -> None: + config = decode_mllm_expert_program( + _model_json( + { + "kind": "operate_articulation", + "articulation": "drawer", + "target": "open", + } + ), + integration=_integration(), + ) + + assert config.program.call.target == "open" # type: ignore[union-attr] + + +@pytest.mark.parametrize( + ("call", "code"), + [ + ( + {"kind": "pick", "object": "env.robot.control_parts"}, + "environment_traversal", + ), + ({"kind": "pick", "object": "eval(1 + 1)"}, "executable_expression"), + ], +) +def test_decoder_reuses_executable_free_value_validation( + call: dict[str, object], + code: str, +) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json(call), + integration=_integration(), + ) + + assert error.value.code == code + + +@pytest.mark.parametrize( + ("response", "code"), + [ + ("```json\n{}\n```", "invalid_json"), + ('{"schema_version": 1, "schema_version": 1}', "duplicate_json_key"), + ('{"schema_version": NaN}', "non_finite_number"), + ('{"schema_version": 1e400}', "non_finite_number"), + ], +) +def test_decoder_propagates_strict_json_failures(response: str, code: str) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program(response, integration=_integration()) + + assert error.value.code == code + + +def test_compile_frontend_reuses_existing_adapter_and_compiler() -> None: + factory = _CompileOnlyFactory() + adapter = _adapter(factory) + response = _model_json() + + model_compiled = compile_mllm_expert_program( + response, + adapter=adapter, + integration=_integration(), + ) + direct_data = _model_data() + direct_data["integration"] = { + "robot_profile": "test_robot", + "scene_registry": "test_scene", + "runtime_preset": "safe", + } + direct_compiled = adapter.compile(decode_expert_program(direct_data)) + + model_call = list(model_compiled)[0].calls[0].call + direct_call = list(direct_compiled)[0].calls[0].call + assert type(model_compiled) is CompiledProgram + assert type(model_call) is Pick + assert type(direct_call) is Pick + assert model_call.object.entity_id == direct_call.object.entity_id == "cube" + assert factory.scene_registry_calls == 2 + + +def test_policy_failure_does_not_touch_adapter_or_runtime() -> None: + factory = _CompileOnlyFactory() + adapter = _adapter(factory) + registered = { + "kind": "registered", + "call_id": "vendor.inspect", + "schema_version": 1, + } + + with pytest.raises(ExpertProgramDecodeError): + compile_mllm_expert_program( + _model_json(registered), + adapter=adapter, + integration=_integration(), + ) + + assert factory.scene_registry_calls == 0 + + +def test_compile_frontend_rejects_unknown_scene_reference() -> None: + factory = _CompileOnlyFactory() + + with pytest.raises(ExpertProgramCompileError) as error: + compile_mllm_expert_program( + _model_json({"kind": "pick", "object": "missing"}), + adapter=_adapter(factory), + integration=_integration(), + ) + + assert error.value.code == "unknown_scene_reference" + assert error.value.path == ("program", "call", "object") diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 48c37b8f6..3bd98e348 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -22,6 +22,7 @@ from collections.abc import Mapping, Sequence from dataclasses import dataclass, fields, is_dataclass import inspect +import json import textwrap from types import MethodType, SimpleNamespace from typing import Any, ClassVar @@ -30,8 +31,10 @@ import pytest import torch +from embodichain.agents.mllm import compile_mllm_expert_program from embodichain.lab.gym.envs.expert_program import ( AntipodalGraspAffordanceBinding, + CompiledProgram, ControlCommandStateEvidenceTracker, ControlPartCommandPreset, ControlPartEndpointBinding, @@ -1077,12 +1080,22 @@ def _place_evidence_plan(action: Any, request: Any, context: Any) -> Any: ) -def _evidence_runtime() -> tuple[ +def _evidence_integration() -> ExpertProgramIntegrationCfg: + """Return the host-owned integration shared by all frontend paths.""" + return ExpertProgramIntegrationCfg( + robot_profile="evidence_profile", + scene_registry="evidence_scene", + runtime_preset="evidence", + ) + + +def _evidence_adapter_runtime() -> tuple[ + ExpertProgramEnvironmentAdapter, ExpertProgramRuntimeAssembly, _EvidenceRobot, _RigidObject, ]: - """Assemble the production Pick/Place evidence chain on CPU fixtures.""" + """Assemble the production adapter and Pick/Place evidence chain.""" robot = _EvidenceRobot() cube = _RigidObject() simulation = _Simulation(robot, {"cube_native": cube}) @@ -1118,17 +1131,21 @@ def _evidence_runtime() -> tuple[ hold_on_completion=False, ) ) - assembly = adapter.assemble_runtime( - ExpertProgramIntegrationCfg( - robot_profile="evidence_profile", - scene_registry="evidence_scene", - runtime_preset="evidence", - ) - ) + assembly = adapter.assemble_runtime(_evidence_integration()) pick_action = assembly.engine.actions["pick_up"] place_action = assembly.engine.actions["place"] pick_action._plan = MethodType(_pick_evidence_plan, pick_action) place_action._plan = MethodType(_place_evidence_plan, place_action) + return adapter, assembly, robot, cube + + +def _evidence_runtime() -> tuple[ + ExpertProgramRuntimeAssembly, + _EvidenceRobot, + _RigidObject, +]: + """Assemble the production Pick/Place evidence chain on CPU fixtures.""" + _, assembly, robot, cube = _evidence_adapter_runtime() return assembly, robot, cube @@ -1311,60 +1328,79 @@ def _python_pick_place_calls() -> tuple[SemanticCallSpec, ...]: ) -def _decoded_pick_place_calls( - registry: SceneRegistry, -) -> tuple[SemanticCallSpec, ...]: - """Decode and provider-free compile the config equivalent of Python calls.""" - config = decode_expert_program( - { - "schema_version": 1, - "program_id": "pick_place_equivalence", - "integration": { - "robot_profile": "evidence_profile", - "scene_registry": "evidence_scene", - "runtime_preset": "evidence", - }, - "targets": { - "place_target": { - "kind": "cyclic_pose", - "values": [ - { - "position": _DIRECT_PLACE_TARGET.position.tolist(), - "quaternion_wxyz": ( - _DIRECT_PLACE_TARGET.quaternion_wxyz.tolist() - ), - } - ], - } - }, - "program": { - "kind": "sequence", - "items": [ - { - "kind": "invoke", - "call": {"kind": "pick", "object": "cube"}, - }, +def _pick_place_program_data() -> dict[str, object]: + """Return the integration-free program shared with the MLLM frontend.""" + return { + "schema_version": 1, + "program_id": "pick_place_equivalence", + "targets": { + "place_target": { + "kind": "cyclic_pose", + "values": [ { - "kind": "invoke", - "call": { - "kind": "place", - "object": "cube", - "at": { - "kind": "target_ref", - "target": "place_target", - }, + "position": _DIRECT_PLACE_TARGET.position.tolist(), + "quaternion_wxyz": ( + _DIRECT_PLACE_TARGET.quaternion_wxyz.tolist() + ), + } + ], + } + }, + "program": { + "kind": "sequence", + "items": [ + { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "at": { + "kind": "target_ref", + "target": "place_target", }, }, - ], - }, - } - ) - program = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + }, + ], + }, + } + + +def _compiled_program_calls(program: CompiledProgram) -> tuple[SemanticCallSpec, ...]: + """Flatten one provider-free compiled program into semantic calls.""" return tuple( compiled_call.call for segment in program for compiled_call in segment.calls ) +def _decoded_pick_place_calls( + adapter: ExpertProgramEnvironmentAdapter, +) -> tuple[SemanticCallSpec, ...]: + """Decode and compile the config equivalent of the Python calls.""" + data = _pick_place_program_data() + data["integration"] = { + "robot_profile": "evidence_profile", + "scene_registry": "evidence_scene", + "runtime_preset": "evidence", + } + return _compiled_program_calls(adapter.compile(decode_expert_program(data))) + + +def _mllm_pick_place_calls( + adapter: ExpertProgramEnvironmentAdapter, +) -> tuple[SemanticCallSpec, ...]: + """Compile the same program through the strict MLLM frontend.""" + program = compile_mllm_expert_program( + json.dumps(_pick_place_program_data()), + adapter=adapter, + integration=_evidence_integration(), + ) + return _compiled_program_calls(program) + + def _capture_grounded_invocations( monkeypatch: pytest.MonkeyPatch, assembly: ExpertProgramRuntimeAssembly, @@ -1387,9 +1423,12 @@ def _run_evidence_pick_place( robot: _EvidenceRobot, cube: _RigidObject, calls: tuple[SemanticCallSpec, ...], + *, + skills: AtomicSkills | None = None, ) -> tuple[SkillResult, HeldObjectState]: """Drive one happy-path workflow through accepted commands and live evidence.""" - result = assembly.runtime.start(calls, workflow_id="pick_place_equivalence") + entry = assembly.runtime if skills is None else skills + result = entry.start(calls, workflow_id="pick_place_equivalence") verified_pick: HeldObjectState | None = None for _ in range(32): while assembly.command_sink.pending_count: @@ -1401,7 +1440,7 @@ def _run_evidence_pick_place( verified_pick = result.task_state.get_held_object("manipulator") cube.pose[:, 0, 3] = _RELEASE_SEPARATION assembly.clock.advance_after_env_step() - result = assembly.runtime.step() + result = entry.step() assert result.status is SkillStatus.COMPLETED assert verified_pick is not None @@ -1500,37 +1539,70 @@ def test_simulation_factory_preserves_declarative_motion_policy() -> None: assert profile.presets["safe"].motion_policy.sample_count == 17 -def test_decoded_program_and_python_calls_share_invocations_and_results( +def test_mllm_config_and_atomic_skills_share_invocations_and_verified_results( monkeypatch: pytest.MonkeyPatch, ) -> None: - """Both frontends reach equivalent core invocations and verified state.""" - python_assembly, python_robot, python_cube = _evidence_runtime() - config_assembly, config_robot, config_cube = _evidence_runtime() + """All public frontends reach equivalent invocations and verified state.""" + ( + _, + python_assembly, + python_robot, + python_cube, + ) = _evidence_adapter_runtime() + ( + config_adapter, + config_assembly, + config_robot, + config_cube, + ) = _evidence_adapter_runtime() + ( + mllm_adapter, + mllm_assembly, + mllm_robot, + mllm_cube, + ) = _evidence_adapter_runtime() python_invocations = _capture_grounded_invocations(monkeypatch, python_assembly) config_invocations = _capture_grounded_invocations(monkeypatch, config_assembly) + mllm_invocations = _capture_grounded_invocations(monkeypatch, mllm_assembly) + runtime_provider = _QuickstartRuntimeProvider(python_assembly.runtime) + python_skills = AtomicSkills.from_env(runtime_provider, preset="evidence") python_result, python_held = _run_evidence_pick_place( python_assembly, python_robot, python_cube, _python_pick_place_calls(), + skills=python_skills, ) config_result, config_held = _run_evidence_pick_place( config_assembly, config_robot, config_cube, - _decoded_pick_place_calls(config_assembly.scene_registry), + _decoded_pick_place_calls(config_adapter), + ) + mllm_result, mllm_held = _run_evidence_pick_place( + mllm_assembly, + mllm_robot, + mllm_cube, + _mllm_pick_place_calls(mllm_adapter), ) - assert len(python_invocations) == len(config_invocations) == 2 - for python_invocation, config_invocation in zip( + assert runtime_provider.presets == ["evidence"] + assert ( + len(python_invocations) == len(config_invocations) == len(mllm_invocations) == 2 + ) + for python_invocation, config_invocation, mllm_invocation in zip( python_invocations, config_invocations, + mllm_invocations, strict=True, ): _assert_invocation_equivalent(python_invocation, config_invocation) + _assert_invocation_equivalent(python_invocation, mllm_invocation) _assert_typed_equivalent(python_held, config_held) + _assert_typed_equivalent(python_held, mllm_held) _assert_typed_equivalent(python_result, config_result) + _assert_typed_equivalent(python_result, mllm_result) def test_atomic_skills_from_env_runs_documented_pick_place_quickstart() -> None: From 8bf6a50f9a3cc71785e3d331c66ef9e58e373ff1 Mon Sep 17 00:00:00 2001 From: skywhite1024 <129768272+skywhite1024@users.noreply.github.com> Date: Fri, 21 Aug 2026 17:24:01 +0800 Subject: [PATCH 10/29] test(agents): reject removed handover receiver alias --- tests/agents/mllm/test_expert_program.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/tests/agents/mllm/test_expert_program.py b/tests/agents/mllm/test_expert_program.py index 12542e6eb..1a45b4f65 100644 --- a/tests/agents/mllm/test_expert_program.py +++ b/tests/agents/mllm/test_expert_program.py @@ -251,7 +251,6 @@ def test_decoder_rejects_registered_semantic_calls() -> None: { "kind": "hand_over", "object": "cube", - "receiver": "right", "resources": {"destination": "right"}, }, { @@ -284,7 +283,7 @@ def test_decoder_allows_explicit_empty_resources() -> None: assert config.program.call.resources == {} # type: ignore[union-attr] -def test_decoder_rejects_handover_receiver_resource_selection() -> None: +def test_decoder_rejects_removed_handover_receiver_alias() -> None: with pytest.raises(ExpertProgramDecodeError) as error: decode_mllm_expert_program( _model_json( @@ -297,7 +296,7 @@ def test_decoder_rejects_handover_receiver_resource_selection() -> None: integration=_integration(), ) - assert error.value.code == "mllm_resource_override_not_allowed" + assert error.value.code == "unknown_field" assert error.value.path == ("program", "call", "receiver") From eeb6f834282fb067668d1954aa7b076086c0e1ef Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:23:28 +0800 Subject: [PATCH 11/29] feat(tasks): add declarative expert program vertical slices --- agent_context/MAP.yaml | 68 +- .../topics/atomic-actions/atomic-actions.md | 174 ++++- .../design/declarative_expert_program_plan.md | 156 ++++- .../sim/atomic_actions/builtin_actions.md | 63 +- .../overview/sim/atomic_actions/index.md | 24 +- .../atomic_actions/robot_skill_profiles.md | 85 ++- docs/source/overview/sim/index.rst | 3 + docs/source/tutorial/atomic_actions.rst | 9 + .../repeated_cube_pick_place.yaml | 46 ++ .../expert_program/tableware/open_drawer.json | 23 + .../gym/multi_segments/cube_pick_place.json | 33 +- .../gym/open_drawer/cobot_magic_3cam.json | 1 + .../multi_segments/cube_pick_place.py | 647 +++++------------- .../tableware/open_drawer.py | 400 +++++------ .../test_task_vertical_slices.py | 625 +++++++++++++++++ .../test_multi_segments_cube_pick_place.py | 323 +++++---- tests/gym/envs/tasks/test_open_drawer.py | 306 +++++++++ tests/test_expert_program_package_data.py | 196 ++++++ 18 files changed, 2289 insertions(+), 893 deletions(-) create mode 100644 embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml create mode 100644 embodichain_tasks/configs/expert_program/tableware/open_drawer.json create mode 100644 tests/gym/envs/expert_program/test_task_vertical_slices.py create mode 100644 tests/gym/envs/tasks/test_open_drawer.py create mode 100644 tests/test_expert_program_package_data.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 9ed83f5d1..1374d7740 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -541,10 +541,10 @@ topics: - resource graph - resource DAG - semantic skill catalog - - semantic skill compiler - - semantic call - - semantic workflow - - semantic integration manifest + - semantic skill runtime + - expert program + - declarative expert program + - atomic demo bridge - capability binding - AtomicAction - ActionInvocation @@ -564,6 +564,26 @@ topics: - ExecutionSession - EffectVerificationRequest - EffectVerificationResult + - attempt_generation + - SemanticEffectSpec + - EffectMonitorRef + - EffectMonitorRegistry + - EffectMonitorDecision + - PoseRelationEvidenceBatch + - relation hysteresis + - SkillRuntime + - SkillResult + - AtomicSkills + - SemanticCallSpec + - SemanticSkillCompiler + - ExpertProgramCfg + - ExpertProgramCompiler + - AtomicDemoBridge + - BufferedGymCommandSink + - ControlCommandStateEvidenceTracker + - DynamicSettleMonitor + - ParallelSkillRuntime + - program segment metadata - eligible_mask - deactivate_rows - effect verification deadline @@ -634,33 +654,8 @@ topics: - ResolvedRobotResource - ResolvedSkillBinding - SkillPolicyPreset - - SkillPolicyPreset.required_planner - - SemanticCallSpec - - SemanticPose - - Pick - - Place - - HandOver - - RegisteredSemanticCall - - SemanticCallCatalog - - SceneManifest - - SemanticIntegrationManifest - - BoundSemanticIntegration - - SemanticSkillCompiler - - SkillRuntime - - SkillResult - - ParallelSkillRuntime - - AtomicSkills - - SemanticWorkflow - - SemanticLowering - - GroundedSemanticCall - - RelationTargetGrounder - - HandOverPoseProvider - - SemanticDiagnostic - - SemanticValidationError - - affordance_capabilities - - default_affordances - - grounding_providers - - skill_catalog_revision + - effect_monitors + - semantic effect monitor - binding_contract - engine.skills - skill_profile @@ -747,13 +742,22 @@ topics: - embodichain/lab/sim/atomic_actions/primitives/ - embodichain/lab/sim/atomic_actions/__init__.py - embodichain/lab/sim/skills/scene.py + - embodichain/lab/sim/skills/calls.py + - embodichain/lab/sim/skills/compiler.py + - embodichain/lab/sim/skills/effects.py + - embodichain/lab/sim/skills/evidence.py + - embodichain/lab/sim/skills/integration.py + - embodichain/lab/sim/skills/runtime.py + - embodichain/lab/sim/skills/parallel.py + - embodichain/lab/sim/skills/parallel_runtime.py - embodichain/lab/sim/skills/profiles.py - embodichain/lab/sim/skills/calls.py - embodichain/lab/sim/skills/integration.py - embodichain/lab/sim/skills/compiler.py - embodichain/lab/sim/skills/runtime.py - embodichain/lab/sim/skills/__init__.py - - scripts/tutorials/semantic_skill/ + - embodichain/lab/gym/envs/expert_program/ + - embodichain/lab/gym/envs/settling.py related_topics: - simulation-system - motion-planning diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 57e36f25e..04e2ed834 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -183,7 +183,8 @@ Binding and policy authority is split deliberately: - the `RobotSkillProfile` owns the resource DAG, capability declarations, complete per-skill default `ResourceBinding` values, semantic command profiles keyed by generic profile IDs, and named `SkillPolicyPreset` - snapshots; endpoint declarations or adapters select those profile IDs; + snapshots that also select exact semantic-effect monitors; endpoint + declarations or adapters select those profile IDs; - the bound robot owns actual control-part membership and joint IDs, and its configured solver is checked for known solver-backed capabilities; - endpoint adapters own controller-specific validation, physical claims, and @@ -222,10 +223,12 @@ match the engine's configured planner. IDs, and adapter-defined `claim_tokens`. Claims conflict when any category overlaps, so a `whole_body` composite conflicts with a contained arm even when their endpoint or control-part names differ. This is deterministic conflict -metadata only: there is no resource lease manager, parallel scheduler, -or concurrency guarantee yet. Dynamic execution can dispatch multiple -endpoint commands in one synchronized frame, but that does not imply resource -scheduling or safe parallelism. A custom mobile/base or whole-body endpoint is +metadata only: a `ResourceClaim` by itself is not a resource lease manager, +parallel scheduler, or concurrency guarantee. The separate explicit +`ParallelSkillRuntime` described below coordinates analyzed branch lanes and +still requires an authoritative safety validator. Dynamic execution can +dispatch multiple endpoint commands in one synchronized frame, but that alone +does not imply resource scheduling or safe parallelism. A custom mobile/base or whole-body endpoint is executable only when its adapter supplies a target, the action emits a matching runtime payload, and the target's transport is registered with the `EndpointCommandRouter`. Successful binding or a non-conflicting claim alone is @@ -545,6 +548,42 @@ effect_result = EffectVerificationResult( result = runner.step(effect_result=effect_result) ``` +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 +selects the built-in `builtin.composite_effect@1` monitor for `pick`, +`place`, and `hand_over`; an explicit empty mapping disables the default and +makes analysis of those curated calls fail with `missing_effect_monitor`. +`SemanticIntegrationManifest` rejects monitor keys absent from its call +catalog. `SemanticSkillCompiler.analyze()` resolves the exact factory and +validates monitor parameters without observing scene providers or constructing +stateful monitors. + +Grounding creates an immutable `SemanticEffectSpec` and an independent monitor +for the call. The spec separates typed symbolic state expectations from typed +physical clauses. Pick declares an attached destination, Place a detached +source with an owned pre-effect pose baseline, and HandOver both. Endpoint +adapters publish immutable `EffectEvidenceSourceRef` values and a logical +`task_state_key`; evidence routes use `EffectEvidenceAddress`, never the +command-only `RuntimeEndpointTarget`. This keeps motion, mobile, whole-body, +articulation, and custom controller transports extensible without treating a +control part as symbolic state identity. + +Providers emit raw `PoseRelationEvidenceBatch`, `BinaryEffectEvidenceBatch`, +`ScalarEffectEvidenceBatch`, or `JointStateEvidenceBatch` values with stable +environment IDs, per-row validity/acquisition diagnostics, timestamps, and +observation revisions. Providers do not apply policy thresholds. The composite +monitor evaluates clauses as a conjunction per state expectation, applies +pose/force/joint hysteresis, treats invalid rows as unresolved, and reports +explicit contradictory evidence as failure. It never uses `TaskState` as +physical proof. The `SkillRuntime` adapter validates the decision, attaches +only the current verification ID, and returns an exact +`EffectVerificationResult` in the same due observation cycle. Request shrink +within one `attempt_generation` preserves remaining-row hysteresis; installing +a retry/replan/revision increments the generation and resets it. Evidence at +the exact deadline is allowed; evidence after it is rejected and normal runner +timeout/recovery remains authoritative. + 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. @@ -662,6 +701,130 @@ live observation fails. Environment IDs must remain stable and ordered for the entire session; robot and scene timestamps and scene versions must be monotonic. Collision-world revisions must also remain monotonic per environment. +## Semantic runtime and Expert Programs + +`embodichain.lab.sim.skills` is the semantic frontend over the core contracts. +`Pick`, `Place`, `HandOver`, `OperateArticulation`, and registered extension +calls are immutable, robot-independent intent values. `SemanticSkillCompiler` +performs provider-free workflow analysis first, then grounds exactly one call +from a fresh `PlanningContext`. It resolves the authoritative `SceneRegistry`, +profile resource binding and preset, downstream target look-ahead, typed goal, +effect specification, and effect monitor before producing one +`ActionInvocation`. + +`SkillRuntime` owns the shared call barrier and persistent verified `TaskState`. +Every call creates exactly one one-invocation `ExecutionSession` and re-observes +before the next call. Eligibility, success, failure, cancellation, recovery, +and effect state are row-local; active rows share the call boundary. The +runtime exposes non-blocking `start()`/`step()` and synchronous `run()` over the +same path. `AtomicSkills` is a convenience facade. `AtomicSkills.from_env()` +accepts only an explicit `SkillRuntimeProvider` and never scans arbitrary +environment attributes; Gym demo environments use the lazy bridge below so +commands cannot bypass `env.step()`. + +`embodichain.lab.gym.envs.expert_program` owns strict declarative programs. +Schema version 1 supports bounded `Sequence`, `Repeat`, `Segment`, and `Invoke`; +version 2 adds deterministic `Parallel` branches and explicit `Barrier` nodes. +The decoder rejects unknown fields/discriminators, duplicate serialized keys, +unsupported versions, executable values, dotted environment traversal, +unbounded expansion, and invalid registry/catalog references before runtime. +JSON and YAML files are loaded with `load_expert_program()`. A Gym config can +select one with `expert_program_path`, resolved relative to that config file. + +`ExpertProgramCompiler` expands program/demo segments lazily while preserving +typed target selections, post-policies, validators, and parallel blocks. +`AtomicDemoBridge` assembles each segment around the canonical runtime and a +buffered command sink. A `ProcessedEnvAction` marks controller-ready output so +the action manager does not transform it twice, but every command and +post-policy hold still passes through ordinary `env.step()`. `BaseEnv.step_dt` +is authoritative; frame durations must be integral multiples of that cadence. +Parallel lanes are aligned on that strict grid and shorter lanes repeat their +last safe target as hold padding; fractional frames are rejected rather than +implicitly resampled. Early generator termination performs the bridge's +explicit cancel-then-hold handshake before the iterator is closed. + +Bridge creation materializes the bounded segment stream and performs +provider-aware semantic preflight before the first command is emitted. +Sequential stretches analyze their remaining downstream calls together, so a +Pick retains target look-ahead across logical segment boundaries; an explicit +parallel block is a conservative look-ahead barrier. Runtime grounding remains +just-in-time against the latest observation. Relation Place calls require an +exact typed/versioned `RelationTargetGrounder`, and HandOver requires the +profile-selected `HandOverPoseProvider`; neither provider is inferred from +names. + +The production simulation path is +`create_simulation_expert_program_adapter(environment, scene_binding=..., +robot_profile_binding=...)`. `SimulationSceneBinding` declares canonical/native +scene data, while `SimulationRobotSkillProfileBinding` declares reusable robot +resources, capabilities, commands, defaults, and presets. The factory creates +the registry, profile, motion generator, engine, shared-tick observation/evidence +port, command encoder, runtime, and segment policy port. Task classes combine an +external declarative program with typed scene/profile integration declarations +and install the returned adapter; they do not assemble skill trajectories. + +`SimulationRobotSkillProfileBinding` accepts generic `RobotResourceBinding` +declarations containing arbitrary typed `ResourceEndpoint` values; +`ControlPartResourceBinding` is the joint-backed convenience. Mobile-base, +whole-body, and non-joint integrations install a matching +`ResourceEndpointAdapter` and `RuntimeTransportActionEncoder` through the same +standard simulation factory. Task-level Expert Programs remain unchanged. This +is an extension seam rather than built-in locomotion: current curated semantic +skills do not consume the example base/whole-body capabilities. A reusable +production capability also installs its semantic descriptor/lowerer, atomic +skill, payload, safe-state transport behavior, and effect integration as +applicable. + +The standard Gym encoder currently composes custom transports over a full-qpos +hold and the standard simulation factory owns a `MotionGenerator`. A robot may +omit named control parts, but a truly jointless or natively structured mobile +controller still needs a reusable base-action composition/provider +integration. That integration must not add base- or whole-body-shaped fields to +the generic resource, binding, runner, or router contracts. + +Task vertical slices may keep typed profile bindings locally during API +stabilization, but repeated use should promote them into an embodiment-owned +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. + +When no explicit contact or constraint callback is installed, simulation grasp +and release evidence combines the live object-to-endpoint pose relation with +`ControlCommandStateEvidenceTracker`. The tracker changes row-local state only +after an exact profile-owned `open` or `grasp` command is successfully encoded +and buffered. Intermediate commands and inactive rows retain prior state; +cancel, discard, or observer failure invalidates affected evidence. Stable +`env_ids`, not simulator array assumptions, correlate full and subset batches. +This command state is evidence of accepted controller intent, not physical +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. + +Runtime and demo results expose deterministic JSON-safe metadata. Call traces +include invocation identity, masks, command counts, execution/recovery events, +plan-attempt trajectory segments, scene/collision revisions and dependencies, +plus effect decisions and monitor evidence. Segment metadata adds post-policy +settling and validator results. Trajectory segments are trace ranges inside an +atomic plan and never own separate recovery, effect, or timeout state. + +Parallel execution is an explicit schema/runtime layer rather than a second +atomic scheduler. Static analysis rejects overlapping `ResourceClaim` values. +Independent lane runtimes share one clock and barrier, command frames are +merged only after destination/claim/safety validation, failure handling is +row-local, and verified `StateDelta` values merge deterministically at the +barrier. Parallel execution also requires an authoritative +`ParallelCommandSafetyValidator`; resource disjointness alone is never promoted +to physical-safety evidence, and a missing validator fails closed. Schema +version 2 intentionally uses strict task-state key-level merge conflicts; +mask-aware same-key branch merges are not part of this version. + ## Parameter ownership Goal dataclasses carry only semantic task intent. They do not carry robot part @@ -768,6 +931,7 @@ on their resolved endpoint. | `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | | `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | | `hand_over` | `GraspGoal` | `source.motion`, `source.grasp`, `destination.motion`, `destination.grasp` | +| `operate_articulation` | `OperateArticulationGoal` | `primary.motion`, `primary.interaction` | `PressAffordance`, `SlideAffordance`, and `TwistAffordance` contain only target-local geometry and interaction semantics. Their goals own an explicit diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 5f536c1b6..5bd8caa50 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,9 +1,11 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: implementation in progress; Phase 0, PR1, PR2A, and PR2B are - complete on `main`, and PR2C is implemented on this feature branch -- Baseline: `main@dbc6553f11d23a5ab738282fbcde1a7214fca783` -- Last updated: 2026-08-19 +- Status: core contracts are implemented through Phase 7 on stacked feature + branches. 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` +- Last updated: 2026-08-11 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), [#474](https://github.com/DexForce/EmbodiChain/issues/474) - Related implementation: @@ -47,11 +49,13 @@ same layer and run through one runtime built on `ExecutionRunner`. The target authoring cost is: -- a new task that uses existing semantic capabilities: scene configuration, - Expert Program configuration, and optionally a declarative validator; +- a new task that uses existing semantic capabilities: Expert Program + configuration plus typed scene/profile integration declarations, and + optionally a declarative validator, with no task-specific motion code; - a new robot: one reusable `RobotSkillProfile`, not task-specific motion code; -- a genuinely new physical interaction: one reusable semantic skill/compiler/ - monitor implementation, after which tasks use it from configuration. +- a genuinely new physical interaction: one reusable capability bundle + containing its semantic skill/compiler/monitor and controller integration as + applicable, after which tasks select it through program and integration data. This design preserves the core direction of #471. Issue #474 changes the middle of the architecture: ordinary configuration must describe semantic @@ -93,12 +97,12 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is updated against committed `main@dbc6553f`. PR #517 simplified the -atomic-action core, PR #487 landed the complete Phase 1 foundation, and PR #523 -simplified that foundation without changing its contracts. The scene registry -and robot skill profile APIs are available, but official task environments have -not adopted them yet; that rollout starts only after the semantic compiler and -runtime exist. +This plan is updated against committed `main@bcccb787` after PRs #475 and #476. The +implementation series is stacked from that baseline: PR1 is complete on +`refactor/atomic-actions-phase0`, PR2A is implemented by +`feat/atomic-action-pr2a-scene-registry`, and PR2B is implemented by +`feat/atomic-action-pr2b-robot-skill-profile`. These status statements do not +imply that the stacked changes have landed on `main`. | Capability | Current main | Design consequence | |---|---|---| @@ -886,7 +890,7 @@ PR2A SceneRegistry PR2B RobotSkillProfile | | | v | PR2C Runtime Endpoints - | (in progress) + | (implemented) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -1041,10 +1045,28 @@ Deliverables: same-slot endpoint disjointness for future conflict analysis, without claiming safe parallel execution. -The profile API can represent mobile-base and whole-body resources today. A -new endpoint kind still needs one shared adapter and a compatible shared atomic -skill before the current core can execute it; adding tasks that reuse that -capability then remains configuration-only. +The profile and endpoint-runtime APIs can represent mobile-base and whole-body +resources today, and the generic paths are covered by whole-body joint and +custom planar-velocity tests. They are extension seams, not built-in navigation +or whole-body behavior: no current curated semantic skill consumes the example +`motion.base.*` or `motion.whole_body` capabilities. A production shared +capability still needs its semantic descriptor/lowerer, atomic skill, payload, +endpoint adapter, transport, and effect integration as applicable. Once that +reusable bundle exists, another task supplies an Expert Program plus typed +scene/profile integration declarations without task-specific motion code. + +The standard Gym bridge currently composes every custom transport action over +a full-qpos hold and the standard simulation factory owns a +`MotionGenerator`. This supports robots without named control parts, but a +truly jointless or natively structured mobile controller still needs a reusable +base-action composition/provider integration. That extension must not add +base- or whole-body-shaped fields to the generic resource, binding, runner, or +router contracts. + +The current task vertical slices still construct their typed profile bindings +from task modules. Promoting stable bindings into an embodiment-owned profile +catalog is rollout packaging needed for cross-task reuse; it does not require a +new resource or runtime contract. PR2A and PR2B landed together through PR #487. That foundation does not migrate official tasks; the repeated-cube vertical slice opts in only after the @@ -1106,8 +1128,21 @@ diagnostic, robot capabilities resolve bindings/presets without task-owned motion code, and generic resolved endpoints can reach their registered runtime transports without adding arm/tool-specific core paths. +Implementation status: when `safe` is reachable and the registry declares +dynamic collision entities, binding rejects an unsupported active planner +before observation or planning. Linking produces an effective +`DynamicCollisionMode.REQUIRED` preset snapshot without mutating the profile's +source preset. This preflight coverage does not replace the remaining +end-to-end dynamic-obstacle recovery simulation. + ### Phase 2: semantic facade and compiler +Implementation status: the semantic facade, provider-free linking, canonical +compiler, bounded program preflight, and cross-segment sequential look-ahead are +implemented. Relation placement remains an exact typed integration capability; +a reusable production support-surface/container affordance and grounder are +follow-up work rather than inferred behavior. + Deliverables: - `SemanticCallSpec`, object-centric `Pick`, `Place`, and `HandOver`; @@ -1124,6 +1159,16 @@ effect verifier. ### Phase 3: canonical runtime and effects +Implementation status: core contracts are implemented in the current stack. +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. + Deliverables: - `SkillRuntime` wrapping `ExecutionRunner` for sync and step-wise use; @@ -1140,6 +1185,12 @@ compiler/runtime code and produce equivalent results. ### Phase 4: demo integration primitives +Implementation status: implemented. The bridge uses buffered runtime commands and +an environment-step clock, dynamic settling is shared with reset behavior, and +JSON-safe lifecycle metadata covers every installed plan attempt, named +trajectory segment, effect decision/evidence, recovery event, scene/collision +revision, post-policy outcome, and validator result. + Deliverables: - expose the existing named plan trajectory segments through optional demo @@ -1156,13 +1207,23 @@ effect, or trace integration contains a hard-coded trajectory index. ### Phase 5: Expert Program version 1 and repeated-cube vertical slice +Implementation status: configuration and task migration are implemented. The +strict decoder/loader, lazy compiler, environment/CLI integration, shared +simulation factory, and three-segment cube program are implemented. The task +combines declarative program configuration with typed scene/profile integration +declarations and installs the shared adapter without overriding task motion +generation. A supported-simulation run has completed the first physical +Pick/Place/settle/validator cycle; completing all three cycles remains an +acceptance item while thresholds are calibrated. + Deliverables: - strict `@configclass` schema and versioned decoder; - `Sequence`, bounded `Repeat`, `Segment`, and `Invoke`; - registered targets, post-policies, and validators; - `EmbodiedEnvCfg` and CLI integration with legacy fallback; -- configuration-only migration of repeated cube pick/place. +- motion-code-free migration of repeated cube pick/place using a declarative + program and typed scene/profile integration declarations. Exit criteria: @@ -1177,6 +1238,13 @@ Exit criteria: ### Phase 6: sequential skill coverage and articulated interaction +Implementation status: the articulation path and task migration are +implemented. Articulation/link/operation-affordance registration, +`OperateArticulation`, typed joint-state effects/evidence, and the declarative +Open Drawer program with typed integration declarations use the same +compiler/runtime path as pick/place. Its supported-simulation physical run now +completes and reaches the configured drawer joint target. + Deliverables: - articulation/link/affordance registry integration; @@ -1191,11 +1259,22 @@ trajectories in task code. ### Phase 7: parallel execution and PourWater +Implementation status: the schema/runtime contracts and fail-closed safety +boundary are implemented. Schema +version 2 provides explicit parallel branches and barriers; static resource +conflict analysis, shared-clock lane coordination, deterministic hold padding, +transport/safety validation, row-local failure and cancellation, timeouts, and +deterministic state merge are covered by tests. A production simulation safety +validator and parallel physical integration remain pending. The PourWater task +migration is outside the current scope because it would require modifying +Action Bank code. + Deliverables: - `Parallel` and explicit `Barrier` nodes in a new schema version; - robot-resource conflict analysis; -- deterministic trajectory alignment/resampling policy; +- deterministic strict-step-grid alignment with hold padding; fractional frame + durations are rejected rather than implicitly resampled; - synchronization and timeout behavior; - deterministic per-environment `StateDelta` merge rules; - PourWater migration from its Action Bank subclass. @@ -1205,6 +1284,11 @@ tests pass before the legacy task is switched. ### Phase 8: rollout, documentation, and deprecation +Implementation status: partial. The canonical semantic/Expert Program documentation, +project-development context, task vertical slices, and public integration +guidance are included in this stack. Metrics, migrations that touch Action +Bank, and any deprecation proposal remain explicitly separate follow-up work. + Deliverables: - semantic quickstart and advanced-core integration guide; @@ -1267,11 +1351,15 @@ independent of adoption of the new path. The design is complete when all of the following hold: -- [ ] A versioned Expert Program is fully validated before execution and cannot +- [x] A reachable `safe` preset in a dynamic-collision scene resolves to + `DynamicCollisionMode.REQUIRED` and rejects an unsupported active planner + before observation, planning, or command emission without mutating the + profile configuration. +- [x] A versioned Expert Program is fully validated before execution and cannot evaluate arbitrary code or traverse environment attributes by string. -- [ ] Python, configuration, and future MLLM calls share one semantic compiler, +- [x] Python, configuration, and MLLM calls share one semantic compiler, typed atomic-action core, and runtime. -- [ ] A common new task using existing semantic skills needs no task-specific +- [x] A common new task using existing semantic skills needs no task-specific motion-generation code. - [x] Robot capability binding is expressed through generic participant resources and endpoints, so mobile-base and whole-body skills do not @@ -1279,14 +1367,14 @@ The design is complete when all of the following hold: - [x] Runtime binding, command framing, routing, and safe stop are endpoint generic; joint trajectories remain an optional planning/feedback artifact rather than the only runtime carrier. -- [ ] Each scene entity is registered once under an authoritative registry ID +- [x] Each scene entity is registered once under an authoritative registry ID across semantics, observation, affordance, and collision handling; simulation `uid` values are legacy aliases only. -- [ ] The default pick/place path does not expose raw qpos, grasp/EEF matrix +- [x] The default pick/place path does not expose raw qpos, grasp/EEF matrix math, planner construction, session plumbing, or custom verification. -- [ ] Automatic grasping tracks target revisions and receives downstream object +- [x] Automatic grasping tracks target revisions and receives downstream object goals without caller duplication. -- [ ] `Place` is object-centric and consumes verified held-object state. +- [x] `Place` is object-centric and consumes verified held-object state. - [ ] Built-in grasp, release, handover, and supported articulation effect monitors work in simulation. - [x] Repeated sub-threshold motion eventually publishes the correct scene @@ -1294,20 +1382,20 @@ The design is complete when all of the following hold: - [x] Custom actions have a documented and tested intentional hard-break migration from overriding `plan()` to implementing `_plan()`; no compatibility adapter is required. -- [ ] Version 1 creates exactly one one-invocation `ExecutionSession` for each +- [x] Version 1 creates exactly one one-invocation `ExecutionSession` for each semantic call and re-observes before lowering the next call. -- [ ] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass +- [x] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass through `env.step()`. -- [ ] No program post-policy, effect, or tracing integration depends on +- [x] No program post-policy, effect, or tracing integration depends on hard-coded waypoint indices. - [ ] Repeated cube pick/place completes at least three lazy, independently observed program/demo segments with settle/effect/validation metadata. -- [ ] Version 1 uses one shared program/call barrier while per-environment task +- [x] Version 1 uses one shared program/call barrier while per-environment task state, effects, recovery, eligibility, success, and failure remain independent. -- [ ] Advanced users retain typed goals, invocations, policies, providers, +- [x] Advanced users retain typed goals, invocations, policies, providers, sessions, and planners as escape hatches. -- [ ] Parallel resource conflicts, synchronization, timing, cancellation, and +- [x] Parallel resource conflicts, synchronization, timing, cancellation, and state merging are tested before PourWater migration. - [ ] Action Bank remains usable until feature parity and a deprecation window are documented. diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index e766bd68a..2a767bf37 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -160,14 +160,13 @@ The animations below are the focused simulator demos under | `move_end_effector` | `EndEffectorPoseGoal` | `primary.motion` | none | none | none | | `move_joints` | `JointPositionGoal` | `primary.motion` | named target only: command matching `target` on `primary.motion` | none | none | | `pick_up` | `GraspGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | semantic object/entity | attach object to the `primary.motion` target | -| `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | object held exclusively by the `primary.motion` target | preserve attachment | -| `place` | `PlaceGoal`, `AssembleGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | any active attachment must be exclusive to `primary.motion`; `AssembleGoal` requires one | detach object | -| `press` | `PressGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | `PressAffordance` + target pose | open-loop motion; application verifies contact/actuation | -| `slide` | `SlideGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | `SlideAffordance` + link pose | open-loop motion; application verifies joint travel/grasp | -| `twist` | `TwistGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | `TwistAffordance` + target pose | open-loop motion; application verifies joint travel/grasp | -| `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | both grasp endpoints: `open`, `grasp` | semantic object/entity | attach the shared object to both motion targets | -| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | `placing.grasp`: `open`, `grasp`; `support.grasp`: `grasp` | two distinct objects, each held exclusively by its motion target | optionally detach placing object; preserve support attachment | -| `hand_over` | `GraspGoal` | `source.motion`, `source.grasp`, `destination.motion`, `destination.grasp` | both grasp endpoints: `open`, `grasp` | object held exclusively by the source motion target | transfer attachment to the destination motion target | +| `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | object held by the `primary.motion` target | preserve attachment | +| `place` | `PlaceGoal`, `AssembleGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | `AssembleGoal` requires an object held by the `primary.motion` target; ordinary `PlaceGoal` has no planner-enforced attachment precondition | detach object | +| `press` | `PressGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | none | none | +| `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | both grasp endpoints: `open`, `grasp` | semantic object/entity | create coordinated attachment; clear individual attachments | +| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | `placing.grasp`: `open`, `grasp`; `support.grasp`: `grasp` | one individually held object per motion target | optionally detach placing object; preserve support attachment | +| `hand_over` | `GraspGoal` | `source.motion`, `source.grasp`, `destination.motion`, `destination.grasp` | both grasp endpoints: `open`, `grasp` | object held by the source motion target | transfer attachment to the destination motion target | +| `operate_articulation` | `OperateArticulationGoal` | `primary.motion`, `primary.interaction` | `primary.interaction`: `open`, `grasp` | registered articulation and handle operation affordance | update and physically verify the target articulation joint position | ### Participant slot meanings @@ -391,6 +390,13 @@ same tensor for grasp sampling, upright adjustment, and `object_to_eef`, and automatically records the ID as a scene dependency. An explicit ID never falls back to a live simulation entity when the snapshot entry is missing. +The object dependency is monitored only while the `approach` segment is active. +Its exclusive cutoff is `close.start`: object motion observed before that frame +invalidates the plan, while motion from gripper closure and lift does not. After +the cutoff, every object-pose change is ignored by scene recovery, including an +external disturbance, so the accepted `grasp` command and live +object-to-endpoint effect evidence become the authoritative completion check. + `PickUp` requires typed `open` and `grasp` commands on `primary.grasp`. Important `PickUpOptions` fields: @@ -742,6 +748,47 @@ dual-arm `strategy="motion_gen"` path. **Example:** `scripts/tutorials/atomic_action/hand_over.py` +(builtin-operate-articulation)= + +## `OperateArticulation` + +Runs one reusable **approach -> engage -> operate -> release -> retract** +interaction for a drawer, slider, or another handle-driven articulation. + +| Contract | Value | +|---|---| +| Skill ID | `operate_articulation` | +| Goal | `OperateArticulationGoal(articulation_id, joint_id, geometry, source_position, target_position, target_displacement)` | +| Binding contract | disjoint `primary.motion` and `primary.interaction` endpoints | +| Required commands | `primary.interaction`: `open`, `grasp` | +| Effect | `ArticulationJointState[(articulation_id, joint_id)] = target_position` | +| Verification | explicit joint-state evidence is required before committing the effect | + +The first-class semantic call takes an articulation reference, an optional +handle affordance reference, and either a named target or an explicit +`target_position` plus `target_displacement` pair. The pair is intentionally +not inferred from simulator state: the core scene snapshot contains entity +poses, not articulation qpos. + +`ArticulationOperationAffordance` owns the joint ID, approach/contact/ +operation/retract offsets, operation axis, position scale, and optional named +position/displacement pairs. At every JIT grounding boundary the compiler +reads the latest registered handle pose and derives all four end-effector +poses. The displacement is measured from that observed handle pose. A named +target also supplies both its absolute joint postcondition and its explicit +handle-relative displacement. + +The grounded semantic effect uses an `ArticulationJointStateExpectation` and a +`JointStateEffectClause` addressed by canonical articulation and joint IDs. +Planning success alone never commits the symbolic joint state. + +The handle scene dependency has an exclusive cutoff at `operate.start`. Motion +before engagement can still invalidate and replan the trajectory; motion after +that boundary is expected to be caused by the operation and is not classified +as target drift. The joint-state effect monitor remains authoritative for +completion, and the cutoff also ignores unrelated external handle motion after +the operation starts. + ## Running the demos Every focused script is interactive by default. Add `--auto_play` to skip diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index f1a920345..f63527335 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -7,6 +7,7 @@ builtin_actions robot_skill_profiles +expert_programs ``` ```{currentmodule} embodichain.lab.sim.atomic_actions @@ -88,7 +89,8 @@ The boundary is deliberate: | Scene observation | Registry-derived `SceneProvider` | Captures canonical ordered entities plus monotonic global or per-environment collision-world revisions | | Scheduling and controller lifecycle | `ExecutionRunner` | Observes only when due, dispatches timed commands, records acknowledgements, and performs safe stop | | Robot/simulator I/O | `ObservationProvider`, `EndpointCommandRouter`, `EndpointCommandTransport`, and `ExecutionClock` adapters | Isolates observation, per-controller command transport, and time/physics advancement from planning and session state | -| Physical-effect verification | Application observer | Verifies grasp, release, handover, and other symbolic effects | +| Physical-effect evidence | Backend provider or application adapter | Acquires typed pose/contact/controller evidence without applying policy thresholds | +| Effect decision and correlation | `EffectMonitor` plus the semantic runtime adapter, or an application verifier on the direct-core path | Interprets evidence, attaches the current request ID, and reports grasp, release, handover, or other symbolic effects | `ExecutionRunner.step()` is non-blocking. Its convenience `run_until_blocked()` loop waits or advances simulation through an injected @@ -832,6 +834,26 @@ its `effect_result`: schedule another call using `wait_duration`, re-read the current request, and submit a result for that current ID. Partial resolution and row deactivation can also replace the request before the delayed result arrives. +The semantic layer provides a reusable verifier kernel for the curated +`Pick`, `Place`, and `HandOver` calls. A +{class}`~embodichain.lab.sim.skills.SemanticEffectSpec` binds the canonical +object and expected attach/detach relations to concrete runtime endpoints. Its +fresh per-call {class}`~embodichain.lab.sim.skills.EffectMonitor` consumes +backend-neutral {class}`~embodichain.lab.sim.skills.PoseRelationEvidenceBatch` +values and returns an uncorrelated +{class}`~embodichain.lab.sim.skills.EffectMonitorDecision`. The semantic runtime +must validate that decision, attach the *current* request ID, and pass the +result to the runner in the same due observation cycle. + +This split is deliberate: the evidence provider owns physical observation, +the monitor owns thresholds and hysteresis, and `ExecutionSession` remains the +only owner of deadlines, retries, partial-row commits, and verified +`TaskState`. A request-mask shrink keeps monitor history for remaining rows via +`attempt_generation`; a replacement plan or retry increments that generation +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. + ## Action Agent integration An MLLM should not construct `ActionInvocation` by copying arbitrary JSON into diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md index 66488542d..cbddb478b 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -80,7 +80,10 @@ from embodichain.lab.sim.atomic_actions import ( MotionPolicy, ) from embodichain.lab.sim.skills import ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, ControlPartEndpoint, + EffectMonitorRef, ResourceBinding, RobotResource, RobotSkillProfile, @@ -138,12 +141,32 @@ profile = RobotSkillProfile( "default": SkillPolicyPreset( preset_id="default", motion_policy=MotionPolicy(strategy="ik_interp"), + effect_monitors={ + semantic_id: EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + { + "attached_translation_threshold": 0.02, + "detached_translation_threshold": 0.05, + "consecutive_samples": 2, + }, + ) + for semantic_id in ("pick", "place", "hand_over") + }, ), }, default_preset="default", ) ``` +During binding, each resolved endpoint also receives a logical +`task_state_key` and immutable, channel-keyed `effect_sources`. By default the +logical key is the selected resource ID, so the `motion` and `grasp` endpoints +of `left_participant` share one symbolic held-object state even though they use +different control parts. An effect source contains an `EffectEvidenceAddress`; +it is intentionally separate from the endpoint's command-only +`RuntimeEndpointTarget`. + Every `ControlPartEndpoint.control_part` must be a key in `robot.control_parts`. A composite endpoint may reuse a member's control part, but all joints controlled directly by the composite must already be covered by @@ -311,6 +334,56 @@ endpoint subtype and adapter when controller semantics differ. An adapter may set `requires_command_profile=True` when a missing generic command-profile ID must make profile binding fail immediately. +The standard Expert Program simulation declaration accepts these endpoints +directly for robots that expose the normal full-state/qpos action base; a task +does not need a custom runtime factory solely to register the endpoint and Gym +transport: + +```python +profile = SimulationRobotSkillProfileBinding( + profile_id="mobile_v1", + resources=( + RobotResourceBinding( + resource_id="mobile_base", + endpoints={ + "motion": MobileVelocityEndpoint( + controller_id="base_controller", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ), + ), +) + +adapter = create_simulation_expert_program_adapter( + env, + scene_binding=scene_binding, + robot_profile_binding=profile, + endpoint_adapters={MobileVelocityEndpoint: MobileVelocityEndpointAdapter()}, + runtime_transports=(MobileVelocityGymEncoder(),), +) +``` + +`RobotResourceBinding` snapshots arbitrary typed `ResourceEndpoint` values. +`ControlPartResourceBinding` remains the stricter joint-backed convenience and +continues to validate native control parts, joint IDs, and command-preset +widths. + +Endpoint registration is not a navigation or whole-body planner. Existing +built-in semantic skills do not consume the example base/whole-body +capabilities. A reusable capability must also install its semantic descriptor +and lowerer, atomic planner, command payload, safe-state transport behavior, and +effect integration as applicable. The current standard Gym encoder composes +custom transports over a full-qpos hold and the standard simulation factory +owns a `MotionGenerator`; a truly jointless or natively structured controller +therefore needs a reusable base-action composition/provider integration. This +does not require base- or whole-body-specific fields in the generic profile or +runtime core. + +Task vertical slices may declare a typed profile binding locally while the API +stabilizes. Repeated use should move that binding into an embodiment-owned +profile catalog so new tasks select it instead of redefining robot data. + A resolved action binding is keyed only by the skill-local `(slot_id, endpoint_id)` pair. A reusable non-joint capability supplies a matching {class}`~embodichain.lab.sim.atomic_actions.RuntimeCommandPayload`, a @@ -325,11 +398,13 @@ code. ```{important} `ResourceClaim` combines leaf IDs, concrete joint IDs, and adapter claim tokens. -It and explicit disjoint constraints detect physical overlap for binding and -future scheduling work. They do not enable parallel action execution. The -runtime does not merge concurrent endpoint-command streams. Joint-backed plans -may retain a full-robot trajectory for feedback and offline compilation, but -runtime dispatch is scoped to the endpoints in each command frame. +It and explicit disjoint constraints detect physical overlap for binding. A +claim alone does not enable or prove safe parallel action execution. The +separate explicit `ParallelSkillRuntime` can coordinate disjoint branch lanes, +but it merges command frames only through an authoritative +`ParallelCommandSafetyValidator`. Joint-backed plans may retain a full-robot +trajectory for feedback and offline compilation, while runtime dispatch remains +scoped to the endpoints in each command frame. ``` See {doc}`index` for the direct atomic-action core and diff --git a/docs/source/overview/sim/index.rst b/docs/source/overview/sim/index.rst index dcd93d472..6127e4634 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -150,6 +150,9 @@ Choosing Where to Start - Use :doc:`atomic_actions/robot_skill_profiles` when semantic skills should resolve robot resources and policy presets from reusable embodiment configuration. +- Use :doc:`atomic_actions/expert_programs` when a task should declare semantic + calls, settling, validation, or parallel barriers from JSON/YAML without + implementing task-local motion generation. - Use :doc:`atomic actions ` when building scripted manipulation from reusable motion primitives. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index e96c1e910..88b2584c1 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -491,6 +491,15 @@ create a scene dependency. An ``ActionPlan`` may bound each dependency with ``approach`` as that boundary; joint tracking and collision-world revision checks are unaffected. +An action may give selected dependencies an exclusive waypoint cutoff through +``ActionPlan.scene_dependency_monitor_until``. A dependency is monitored while +the current waypoint index is smaller than its cutoff; ``0`` disables monitoring +from the start, and an omitted dependency remains monitored for the whole +action. Reaching the cutoff ignores every later pose change, not only motion +caused by the skill. Built-in ``PickUp`` uses ``close.start`` for the grasped +object, and ``OperateArticulation`` uses ``operate.start`` for the handle; their +physical effect monitors are authoritative after those boundaries. + Task-state effects ------------------ diff --git a/embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml b/embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml new file mode 100644 index 000000000..107236109 --- /dev/null +++ b/embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml @@ -0,0 +1,46 @@ +schema_version: 1 +program_id: repeated_cube_pick_place + +integration: + robot_profile: ur5_parallel_gripper_v1 + scene_registry: multi_segments_cube_v1 + runtime_preset: safe + +targets: + drop_pose: + kind: cyclic_pose + values: + - position: [-0.40, 0.48, 0.10] + quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + - position: [-0.42, -0.08, 0.10] + quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + +program: + kind: repeat + count: 3 + body: + kind: segment + name: move_cube + steps: + kind: sequence + items: + - kind: invoke + call: + kind: pick + object: cube + - kind: invoke + call: + kind: place + object: cube + at: + kind: target_ref + target: drop_pose + post: + - kind: wait_stable + entity: cube + preset: rigid_object + validators: + - kind: object_near_target + object: cube + target: drop_pose + position_tolerance: 0.12 diff --git a/embodichain_tasks/configs/expert_program/tableware/open_drawer.json b/embodichain_tasks/configs/expert_program/tableware/open_drawer.json new file mode 100644 index 000000000..9bd54f210 --- /dev/null +++ b/embodichain_tasks/configs/expert_program/tableware/open_drawer.json @@ -0,0 +1,23 @@ +{ + "schema_version": 1, + "program_id": "open_drawer", + "integration": { + "robot_profile": "cobot_magic_right_manipulator_v1", + "scene_registry": "open_drawer_v1", + "runtime_preset": "safe" + }, + "targets": {}, + "program": { + "kind": "segment", + "name": "open_drawer", + "steps": { + "kind": "invoke", + "call": { + "kind": "operate_articulation", + "articulation": "drawer", + "handle": "drawer_handle", + "target": "open" + } + } + } +} diff --git a/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json index 6543d8fa1..32cf15513 100644 --- a/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json +++ b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json @@ -1,5 +1,6 @@ { "id": "MultiSegmentsCubePickPlace-v1", + "expert_program_path": "../../expert_program/multi_segments/repeated_cube_pick_place.yaml", "max_episodes": 1, "max_episode_steps": 1200, "num_envs": 1, @@ -9,6 +10,24 @@ }, "env": { "sim_steps_per_control": 4, + "events": { + "settle_cube_on_reset": { + "func": "wait_for_dynamic_objects_to_settle", + "mode": "reset", + "params": { + "entity_cfgs": [ + { + "uid": "cube" + } + ], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise" + } + } + }, "dataset": { "lerobot": { "func": "LeRobotRecorder", @@ -32,20 +51,8 @@ } }, "extensions": { - "num_cycles": 3, - "place_positions": [ - [-0.40, 0.48, 0.10], - [-0.42, -0.08, 0.10] - ], "grasp_samples": 10000, - "force_reannotate": false, - "grasp_hold_steps": 45, - "settle_min_steps": 15, - "settle_max_steps": 80, - "settle_stable_steps": 5, - "linear_velocity_threshold": 0.03, - "angular_velocity_threshold": 0.20, - "place_position_tolerance": 0.12 + "force_reannotate": false } }, "robot": { diff --git a/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json b/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json index 60ab001f8..100fc9c21 100644 --- a/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json +++ b/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json @@ -1,5 +1,6 @@ { "id": "OpenDrawer-v1", + "expert_program_path": "../../expert_program/tableware/open_drawer.json", "max_episodes": 3, "max_episode_steps": 300, "env": { 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 0638ffbdc..6965c6f95 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -14,25 +14,46 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Repeated cube pick-and-place task using lazy demonstration segments. +"""Declarative repeated cube pick-and-place environment. -Each segment plans one complete ``PickUp -> Place -> settle`` cycle. The outer -segment generator resumes only after the previous segment has executed and its -free-falling cube has settled. Consequently, the next pickup always plans from -the cube pose currently measured in simulation instead of a pose predicted -before the episode started. +The task declares its simulation identities and robot resources, while the +packaged Expert Program defines the three semantic pick/place cycles. Shared +Expert Program components own motion generation, execution, settling, and +validation; extending the cycle count or destinations requires config only. """ from __future__ import annotations -from collections.abc import Iterable, Sequence -from functools import partial -from typing import TYPE_CHECKING, Any +from pathlib import Path +from typing import Any -import torch - -from embodichain.lab.gym.envs import DemoSegment, EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +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.envs.expert_program import ( + AntipodalGraspAffordanceBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramCfg, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + SimulationRigidObjectBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, + load_expert_program, +) 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, + RecoveryPolicy, +) from embodichain.lab.sim.cfg import ( LightCfg, RigidBodyAttributesCfg, @@ -40,21 +61,28 @@ ) from embodichain.lab.sim.robots import URRobotCfg from embodichain.lab.sim.shapes import CubeCfg -from embodichain.utils import logger - -if TYPE_CHECKING: - from embodichain.lab.sim.atomic_actions import AtomicActionEngine, ObjectSemantics - from embodichain.lab.sim.objects import RigidObject +from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics +from embodichain.lab.sim.skills.profiles import SkillPolicyPreset +from embodichain.toolkits.graspkit.pg_grasp import ( + AntipodalSamplerCfg, + GraspGeneratorCfg, + GripperCollisionCfg, +) +from embodichain_tasks.configs import get_config_path -__all__ = ["MultiSegmentsCubePickPlaceEnv"] +__all__ = [ + "MultiSegmentsCubePickPlaceEnv", + "create_cube_robot_profile_binding", + "create_cube_scene_binding", +] CUBE_UID = "cube" CUBE_SIZE = 0.05 -DEFAULT_NUM_CYCLES = 3 -DEFAULT_GRASP_HOLD_STEPS = 45 -DEFAULT_PLACE_POSITIONS = ( - (-0.40, 0.48, 0.10), - (-0.42, -0.08, 0.10), +CUBE_SCENE_REGISTRY_ID = "multi_segments_cube_v1" +CUBE_ROBOT_PROFILE_ID = "ur5_parallel_gripper_v1" +CUBE_GRASP_AFFORDANCE_ID = "cube_antipodal_grasp" +CUBE_EXPERT_PROGRAM_PATH = Path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" ) GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" @@ -64,11 +92,12 @@ GRIPPER_FINGER_LENGTH = 0.12 GRIPPER_ROOT_Z_WIDTH = 0.096 GRIPPER_Y_THICKNESS = 0.040 -DEFAULT_GRIPPER_CLOSE_QPOS = 0.024 +GRIPPER_OPEN_QPOS = 0.0 +GRIPPER_GRASP_QPOS = 0.024 def _create_default_robot_cfg() -> URRobotCfg: - """Create the UR5 and parallel-gripper setup used by atomic-action demos.""" + """Create the UR5 scene embodiment used by the declarative task.""" return URRobotCfg.from_dict( { "robot_type": "ur5", @@ -81,19 +110,11 @@ def _create_default_robot_cfg() -> URRobotCfg: }, ], }, - "control_parts": { - "hand": [GRIPPER_HAND_JOINT_PATTERN], - }, + "control_parts": {"hand": [GRIPPER_HAND_JOINT_PATTERN]}, "drive_pros": { - "stiffness": { - GRIPPER_HAND_JOINT_PATTERN: 1e3, - }, - "damping": { - GRIPPER_HAND_JOINT_PATTERN: 1e2, - }, - "max_effort": { - GRIPPER_HAND_JOINT_PATTERN: 1e4, - }, + "stiffness": {GRIPPER_HAND_JOINT_PATTERN: 1e3}, + "damping": {GRIPPER_HAND_JOINT_PATTERN: 1e2}, + "max_effort": {GRIPPER_HAND_JOINT_PATTERN: 1e4}, }, "solver_cfg": { "arm": { @@ -110,8 +131,13 @@ def _create_default_robot_cfg() -> URRobotCfg: ) +def _load_default_expert_program() -> ExpertProgramCfg: + """Decode the packaged semantic program for direct instantiation.""" + return load_expert_program(get_config_path(CUBE_EXPERT_PROGRAM_PATH)) + + def _create_default_env_cfg() -> EmbodiedEnvCfg: - """Create a directly-instantiable default task configuration.""" + """Create a directly-instantiable task configuration.""" cfg = EmbodiedEnvCfg() cfg.max_episode_steps = 1200 cfg.robot = _create_default_robot_cfg() @@ -142,458 +168,153 @@ def _create_default_env_cfg() -> EmbodiedEnvCfg: ) ] cfg.extensions = { - "num_cycles": DEFAULT_NUM_CYCLES, - "place_positions": [list(position) for position in DEFAULT_PLACE_POSITIONS], "grasp_samples": 10000, "force_reannotate": False, - "grasp_hold_steps": DEFAULT_GRASP_HOLD_STEPS, - "settle_min_steps": 15, - "settle_max_steps": 80, - "settle_stable_steps": 5, - "linear_velocity_threshold": 0.03, - "angular_velocity_threshold": 0.20, - "place_position_tolerance": 0.12, } - return cfg - - -@register_env("MultiSegmentsCubePickPlace-v1", max_episode_steps=1200) -class MultiSegmentsCubePickPlaceEnv(EmbodiedEnv): - """Repeatedly pick up and freely place one cube. - - The demonstration planner is intentionally lazy. It yields one complete - pick/place cycle at a time, waits for that cycle to execute and settle, and - only then reads the cube pose and plans the following cycle. - """ - - PICK_SAMPLE_INTERVAL = 120 - PLACE_SAMPLE_INTERVAL = 120 - HAND_INTERP_STEPS = 12 - - def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs: Any) -> None: - if cfg is None: - cfg = _create_default_env_cfg() - - extensions = getattr(cfg, "extensions", {}) or {} - self.num_cycles = int(extensions.get("num_cycles", DEFAULT_NUM_CYCLES)) - self.place_positions = self._validate_place_positions( - extensions.get("place_positions", DEFAULT_PLACE_POSITIONS) - ) - self.grasp_samples = int(extensions.get("grasp_samples", 10000)) - self.force_reannotate = bool(extensions.get("force_reannotate", False)) - self.grasp_hold_steps = int( - extensions.get("grasp_hold_steps", DEFAULT_GRASP_HOLD_STEPS) - ) - self.settle_min_steps = int(extensions.get("settle_min_steps", 15)) - self.settle_max_steps = int(extensions.get("settle_max_steps", 80)) - self.settle_stable_steps = int(extensions.get("settle_stable_steps", 5)) - self.linear_velocity_threshold = float( - extensions.get("linear_velocity_threshold", 0.03) - ) - self.angular_velocity_threshold = float( - extensions.get("angular_velocity_threshold", 0.20) - ) - self.place_position_tolerance = float( - extensions.get("place_position_tolerance", 0.12) - ) - self._validate_settings() - - super().__init__(cfg, **kwargs) - - # ``EmbodiedEnv`` exposes extension values as instance attributes. - # Re-normalize them because that binding intentionally preserves the - # JSON-native list/scalar types supplied by the launcher. - self.num_cycles = int(self.num_cycles) - self.place_positions = self._validate_place_positions(self.place_positions) - self.grasp_samples = int(self.grasp_samples) - self.force_reannotate = bool(self.force_reannotate) - self.grasp_hold_steps = int(self.grasp_hold_steps) - self.settle_min_steps = int(self.settle_min_steps) - self.settle_max_steps = int(self.settle_max_steps) - self.settle_stable_steps = int(self.settle_stable_steps) - self.linear_velocity_threshold = float(self.linear_velocity_threshold) - self.angular_velocity_threshold = float(self.angular_velocity_threshold) - self.place_position_tolerance = float(self.place_position_tolerance) - self._validate_settings() - - cube = self.sim.get_rigid_object(CUBE_UID) - if cube is None: - raise RuntimeError(f"Task requires a rigid object with uid {CUBE_UID!r}.") - self._cube: RigidObject = cube - self._completed_cycles = 0 - self._planned_cycle_count = 0 - self._last_target_position: torch.Tensor | None = None - self._initialize_atomic_actions() - - @staticmethod - def _validate_place_positions( - positions: Sequence[Sequence[float]], - ) -> tuple[tuple[float, float, float], ...]: - """Validate and normalize release positions from task configuration.""" - normalized = tuple( - tuple(float(value) for value in position) for position in positions - ) - if not normalized or any(len(position) != 3 for position in normalized): - raise ValueError("place_positions must contain at least one XYZ position.") - return normalized - - def _validate_settings(self) -> None: - """Validate task settings before allocating a simulation.""" - if self.num_cycles < 1: - raise ValueError("num_cycles must be at least 1.") - if self.grasp_samples < 1: - raise ValueError("grasp_samples must be at least 1.") - if self.grasp_hold_steps < 0: - raise ValueError("grasp_hold_steps must be non-negative.") - if not 0 <= self.settle_min_steps <= self.settle_max_steps: - raise ValueError( - "settle_min_steps must be non-negative and no larger than " - "settle_max_steps." - ) - if self.settle_stable_steps < 1: - raise ValueError("settle_stable_steps must be at least 1.") - if self.linear_velocity_threshold < 0 or self.angular_velocity_threshold < 0: - raise ValueError("Velocity thresholds must be non-negative.") - if self.place_position_tolerance <= 0: - raise ValueError("place_position_tolerance must be positive.") - - def _initialize_atomic_actions(self) -> None: - """Create the motion generator, action engine and cube semantics.""" - from embodichain.lab.sim.atomic_actions import ( - AtomicActionEngine, - ControlPartCommandProfile, - ) - from embodichain.lab.sim.planners import ( - MotionGenCfg, - MotionGenerator, - ToppraPlannerCfg, - ) - - hand_limits = self.robot.get_qpos_limits(name="hand")[0].to( - device=self.device, dtype=torch.float32 - ) - hand_open_qpos = hand_limits[:, 0] - hand_close_qpos = torch.clamp( - torch.full_like(hand_limits[:, 1], DEFAULT_GRIPPER_CLOSE_QPOS), - min=hand_limits[:, 0], - max=hand_limits[:, 1], - ) - motion_generator = MotionGenerator( - cfg=MotionGenCfg(planner_cfg=ToppraPlannerCfg(robot_uid=self.robot.uid)) - ) - self._action_engine: AtomicActionEngine = AtomicActionEngine( - motion_generator, - control_profiles={ - "hand": ControlPartCommandProfile.joint_positions( - open=hand_open_qpos, - grasp=hand_close_qpos, - ) + cfg.events = { + "settle_cube_on_reset": EventCfg( + func=wait_for_dynamic_objects_to_settle, + mode="reset", + params={ + "entity_cfgs": [SceneEntityCfg(uid=CUBE_UID)], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise", }, ) - self._cube_semantics: ObjectSemantics = self._create_cube_semantics() + } + cfg.expert_program = _load_default_expert_program() + return cfg - def _create_cube_semantics(self) -> ObjectSemantics: - """Create reusable antipodal semantics for the task cube.""" - from embodichain.lab.sim.atomic_actions import ( - AntipodalAffordance, - ObjectSemantics, - ) - from embodichain.toolkits.graspkit.pg_grasp.antipodal_generator import ( - AntipodalSamplerCfg, - GraspGeneratorCfg, - ) - from embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker import ( - GripperCollisionCfg, - ) - vertices = self._cube.get_vertices(env_ids=[0], scale=True)[0] - triangles = self._cube.get_triangles(env_ids=[0])[0] - return ObjectSemantics( - label=CUBE_UID, - geometry={}, - affordance=AntipodalAffordance( - mesh_vertices=vertices, - mesh_triangles=triangles, - 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, - ), +def create_cube_scene_binding( + *, + grasp_samples: int = 10000, + force_reannotate: bool = False, +) -> SimulationSceneBinding: + """Declare the cube and its exact 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=CUBE_SCENE_REGISTRY_ID, + rigid_objects=( + SimulationRigidObjectBinding( + entity_id=CUBE_UID, + simulation_uid=CUBE_UID, + dynamics=SceneDynamics.DYNAMIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="cube", + default_grasp_affordance=CUBE_GRASP_AFFORDANCE_ID, + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id=CUBE_GRASP_AFFORDANCE_ID, + object_id=CUBE_UID, + native_name="cube_mesh_antipodal", + revision="cube-antipodal-v1", generator_cfg=GraspGeneratorCfg( viser_port=11801, antipodal_sampler_cfg=AntipodalSamplerCfg( - n_sample=self.grasp_samples, + n_sample=grasp_samples, max_length=GRIPPER_MAX_OPEN_WIDTH, min_length=0.005, ), is_partial_annotate=False, is_filter_ground_collision=False, ), - force_reannotate=self.force_reannotate, - ), - entity=self._cube, - ) - - def create_demo_segments( - self, *, num_cycles: int | None = None, **kwargs: Any - ) -> Iterable[DemoSegment]: - """Lazily plan repeated cube pick-and-place segments. - - Args: - num_cycles: Optional per-rollout override for the configured cycle count. - **kwargs: Reserved for future expert-planning options. - - Yields: - One :class:`DemoSegment` for every pickup/place cycle. - """ - del kwargs - cycle_count = self.num_cycles if num_cycles is None else int(num_cycles) - if cycle_count < 1: - raise ValueError("num_cycles must be at least 1.") - - self._completed_cycles = 0 - self._planned_cycle_count = cycle_count - self._last_target_position = None - for cycle_index in range(cycle_count): - target_position = torch.tensor( - self.place_positions[cycle_index % len(self.place_positions)], - dtype=torch.float32, - device=self.device, - ) - plan_success, actions, source_pose = self._plan_pick_place_cycle( - target_position - ) - self._last_target_position = target_position - source_position = source_pose[:, :3, 3].detach().cpu().tolist() - logger.log_info( - f"Planned cube pick/place segment {cycle_index + 1}/{cycle_count} " - f"from {source_position} to {target_position.detach().cpu().tolist()}." - ) - yield DemoSegment( - actions=actions, - name=f"cube_pick_place_{cycle_index + 1}", - target_uid=CUBE_UID, - instruction=( - "Pick up the cube from its current settled pose and freely " - f"place it at target {cycle_index + 1}." - ), - metadata={ - "cycle_index": cycle_index, - "cycle_count": cycle_count, - "planning_success": plan_success.detach().cpu().tolist(), - "planned_source_poses": source_pose.detach().cpu().tolist(), - "target_position": target_position.detach().cpu().tolist(), - "free_fall_settle": True, - }, - validator=partial( - self._validate_cycle, - plan_success.detach().clone(), - target_position.detach().clone(), + 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, ), - ) - # Execution and validation happen while the generator is suspended at - # ``yield``. Advancing to the next iteration therefore means that the - # cube has already reached its new, measured scene pose. - self._completed_cycles = cycle_index + 1 + force_reannotate=force_reannotate, + ), + ), + ) - def _plan_pick_place_cycle( - self, target_position: torch.Tensor - ) -> tuple[torch.Tensor, Iterable[torch.Tensor], torch.Tensor]: - """Plan one pickup/place cycle from the cube's current measured pose.""" - from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, - GraspGoal, - MotionPolicy, - PickUpOptions, - PlaceGoal, - PlaceOptions, - ) - source_pose = self._cube.get_local_pose(to_matrix=True).to( - device=self.device, dtype=torch.float32 - ) - endpoints = { - "primary": { - "motion": "arm", - "grasp": "hand", - } +def create_cube_robot_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare the UR5 arm and parallel-gripper semantic resource.""" + motion_capabilities = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, } - pick_binding = self._action_engine.bind_control_parts( - "pick_up", - endpoints, - ) - place_binding = self._action_engine.bind_control_parts( - "place", - endpoints, - ) - pick_compiled = self._action_engine.compile( - ( - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal(self._cube_semantics), - binding=pick_binding, - motion_policy=MotionPolicy(sample_count=self.PICK_SAMPLE_INTERVAL), - skill_options=PickUpOptions( - pre_grasp_distance=0.15, - lift_height=0.16, - hand_interp_steps=self.HAND_INTERP_STEPS, + ) + return SimulationRobotSkillProfileBinding( + profile_id=CUBE_ROBOT_PROFILE_ID, + resources=( + ControlPartResourceBinding( + resource_id="manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=motion_capabilities, ), - ), - ), - self._action_engine.initial_context(control_dt=self.step_dt), - ) - pick_success = pick_compiled.plan_success - pick_trajectory = pick_compiled.trajectory.positions - picked_context = pick_compiled.projected_context - held = picked_context.get_held_object("arm") - if held is None or not bool(pick_success.all().item()): - trajectory = self._ensure_nonempty_trajectory(pick_trajectory) - return ( - torch.zeros_like(pick_success, dtype=torch.bool), - self._iter_cycle_actions(trajectory, clear_dynamics_step=None), - source_pose, - ) - - pick_trajectory, clear_dynamics_step = self._insert_grasp_hold(pick_trajectory) - desired_cube_pose = source_pose.clone() - desired_cube_pose[:, :3, 3] = target_position.unsqueeze(0).expand( - self.num_envs, -1 - ) - place_eef_pose = torch.bmm(desired_cube_pose, held.object_to_eef) - place_compiled = self._action_engine.compile( - ( - ActionInvocation( - skill_id="place", - goal=PlaceGoal(place_eef_pose), - binding=place_binding, - motion_policy=MotionPolicy(sample_count=self.PLACE_SAMPLE_INTERVAL), - skill_options=PlaceOptions( - lift_height=0.14, - hand_interp_steps=self.HAND_INTERP_STEPS, + ControlPartEndpointBinding( + endpoint_id="grasp", + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="parallel_gripper", ), ), ), - picked_context, - ) - place_success = place_compiled.plan_success - place_trajectory = place_compiled.trajectory.positions - trajectory = self._ensure_nonempty_trajectory( - torch.cat((pick_trajectory, place_trajectory), dim=1) - ) - return ( - pick_success & place_success, - self._iter_cycle_actions(trajectory, clear_dynamics_step), - source_pose, - ) - - def _insert_grasp_hold( - self, pick_trajectory: torch.Tensor - ) -> tuple[torch.Tensor, int]: - """Hold the closed command at the grasp pose before beginning the lift.""" - close_end_step = min( - int(round(self.PICK_SAMPLE_INTERVAL - self.HAND_INTERP_STEPS) * 0.6) - + self.HAND_INTERP_STEPS, - pick_trajectory.shape[1], - ) - if self.grasp_hold_steps == 0: - return pick_trajectory, close_end_step - - grasp_hold = pick_trajectory[:, close_end_step - 1 : close_end_step, :].repeat( - 1, self.grasp_hold_steps, 1 - ) - augmented = torch.cat( - ( - pick_trajectory[:, :close_end_step, :], - grasp_hold, - pick_trajectory[:, close_end_step:, :], + ), + command_presets=( + ControlPartCommandPreset( + preset_id="parallel_gripper", + control_part="hand", + commands={ + "open": (GRIPPER_OPEN_QPOS,), + "grasp": (GRIPPER_GRASP_QPOS,), + }, ), - dim=1, - ) - return augmented, close_end_step + self.grasp_hold_steps - - def _ensure_nonempty_trajectory(self, trajectory: torch.Tensor) -> torch.Tensor: - """Return at least one hold command so planning failure is recordable.""" - if trajectory.shape[1] > 0: - return trajectory - return self.robot.get_qpos().clone().unsqueeze(1) - - def _iter_cycle_actions( - self, - trajectory: torch.Tensor, - clear_dynamics_step: int | None, - ) -> Iterable[torch.Tensor]: - """Replay a planned trajectory, then hold until the cube is stable.""" - for step_index, action in enumerate(trajectory.unbind(dim=1), start=1): - yield action - if clear_dynamics_step is not None and step_index == clear_dynamics_step: - # Match the pickup tutorial: clear residual object velocity just - # after gripper closure and before the lift phase. - self._cube.clear_dynamics() - - hold_action = trajectory[:, -1].clone() - stable_steps = 0 - for settle_step in range(self.settle_max_steps): - yield hold_action - if settle_step + 1 < self.settle_min_steps: - continue - if bool(self._cube_is_stable().all().item()): - stable_steps += 1 - if stable_steps >= self.settle_stable_steps: - break - else: - stable_steps = 0 + ), + defaults={ + "pick_up": {"primary": "manipulator"}, + "place": {"primary": "manipulator"}, + }, + presets=( + SkillPolicyPreset( + "safe", + recovery_policy=RecoveryPolicy(tracking_error_threshold=0.08), + ), + ), + default_preset="safe", + ) - def _cube_is_stable(self) -> torch.Tensor: - """Return whether cube linear and angular speeds are below thresholds.""" - linear_speed = torch.linalg.vector_norm(self._cube.body_data.lin_vel, dim=-1) - angular_speed = torch.linalg.vector_norm(self._cube.body_data.ang_vel, dim=-1) - return (linear_speed <= self.linear_velocity_threshold) & ( - angular_speed <= self.angular_velocity_threshold - ) - def _cube_settled_near(self, target_position: torch.Tensor) -> torch.Tensor: - """Validate that the cube settled near a release target after free fall.""" - cube_position = self._cube.get_local_pose(to_matrix=True)[:, :3, 3] - target_position = target_position.to( - device=cube_position.device, dtype=cube_position.dtype - ) - xy_error = torch.linalg.vector_norm( - cube_position[:, :2] - target_position[None, :2], dim=-1 - ) - valid_height = (cube_position[:, 2] >= -0.01) & ( - cube_position[:, 2] <= target_position[2] + CUBE_SIZE - ) - return ( - (xy_error <= self.place_position_tolerance) - & valid_height - & self._cube_is_stable() - ) +@register_env("MultiSegmentsCubePickPlace-v1", max_episode_steps=1200) +class MultiSegmentsCubePickPlaceEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + """Repeatedly pick and place a cube from a semantic config program.""" - def _validate_cycle( - self, plan_success: torch.Tensor, target_position: torch.Tensor - ) -> torch.Tensor: - """Combine motion-planning and post-free-fall validation.""" - return plan_success.to(device=self.device, dtype=torch.bool) & ( - self._cube_settled_near(target_position) + 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, + scene_binding=create_cube_scene_binding( + grasp_samples=getattr(self, "grasp_samples", 10000), + force_reannotate=getattr(self, "force_reannotate", False), + ), + robot_profile_binding=create_cube_robot_profile_binding(), ) - def is_task_success(self, **kwargs: Any) -> torch.Tensor: - """Return success after all lazy segments have executed and validated. - - Args: - **kwargs: Reserved for task-evaluation options. - - Returns: - One success flag per parallel environment. - """ - del kwargs - if ( - self._planned_cycle_count < 1 - or self._completed_cycles < self._planned_cycle_count - or self._last_target_position is None - ): - return torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) - return self._cube_settled_near(self._last_target_position) + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the shared adapter assembled for this environment.""" + return self._expert_program_adapter diff --git a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py index 3b4cbdc09..ff1166c67 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py +++ b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py @@ -14,232 +14,210 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Expert demonstration environment for opening a drawer.""" +"""Declarative expert environment for opening a sliding drawer. + +The task owns only scene and embodiment declarations. The packaged Expert +Program selects the semantic ``operate_articulation`` skill and its named +``open`` target; shared runtime components generate and execute all motion. +""" from __future__ import annotations from typing import Any -import torch - from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs.expert_program import ( + ArticulationOperationAffordanceBinding, + ArticulationOperationTargetBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + SimulationArticulationBinding, + SimulationArticulationLinkBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, +) from embodichain.lab.gym.utils.registration import register_env -from embodichain.lab.sim.planners import ( - MotionGenCfg, - MotionGenerator, - MotionGenOptions, - MoveType, - PlanResult, - PlanState, - ToppraPlannerCfg, - ToppraPlanOptions, - TrajectorySampleMethod, +from embodichain.lab.sim.atomic_actions import ( + CARTESIAN_POSE_CAPABILITY, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, +) +from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics +from embodichain.lab.sim.skills.profiles import SkillPolicyPreset + +__all__ = [ + "OpenDrawerEnv", + "create_open_drawer_robot_profile_binding", + "create_open_drawer_scene_binding", +] + +DRAWER_SCENE_REGISTRY_ID = "open_drawer_v1" +DRAWER_ROBOT_PROFILE_ID = "cobot_magic_right_manipulator_v1" +DRAWER_UID = "drawer" +DRAWER_HANDLE_LINK_ID = "drawer_handle_link" +DRAWER_HANDLE_AFFORDANCE_ID = "drawer_handle" +DRAWER_NATIVE_HANDLE_LINK = "handle_xpos" +DRAWER_NATIVE_SLIDE_JOINT = "slide_rails" +DRAWER_OPEN_POSITION = 0.11 +DRAWER_OPEN_DISPLACEMENT = 0.11 + +# Rotation from the drawer handle frame to the historical right-arm TCP frame. +_HANDLE_POSE_OFFSET = ( + -0.023958006, + -0.999453075, + -0.022793945, + 0.0, + 0.999712744, + -0.023966955, + 0.000119456, + 0.0, + -0.000665692, + -0.022784535, + 0.999740177, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, ) -from embodichain.lab.sim.utility.action_utils import interpolate_with_nums - -__all__ = ["OpenDrawerEnv"] - - -def _require_plan_positions(result: PlanResult, *, phase: str) -> torch.Tensor: - """Return a successful single-environment trajectory. - - Args: - result: Motion-planning result to validate. - phase: Human-readable planning phase for error reporting. - - Returns: - Joint positions for the task's single environment. - - Raises: - RuntimeError: If planning failed or returned no joint positions. - """ - if not result.is_all_success(): - raise RuntimeError(f"Motion planning failed during {phase}.") - if result.positions is None: - raise RuntimeError( - f"Motion planning returned no joint positions during {phase}." - ) - return result.positions[0] - - -@register_env("OpenDrawer-v1", max_episode_steps=300) -class OpenDrawerEnv(EmbodiedEnv): - """Open a sliding drawer with the right arm of a CobotMagic robot.""" - - def __init__(self, cfg: EmbodiedEnvCfg, **kwargs: Any) -> None: - """Initialize the environment and its TOPPRA motion generator. - Args: - cfg: Declarative environment configuration. - **kwargs: Additional arguments forwarded to :class:`EmbodiedEnv`. - """ - super().__init__(cfg, **kwargs) - self.motion_gen = MotionGenerator( - cfg=MotionGenCfg( - planner_cfg=ToppraPlannerCfg( - robot_uid=self.robot.uid, - ) - ) - ) - self.eef_open = self.robot.get_qpos_limits(name="right_eef")[:, :, 1] - self.eef_close = self.robot.get_qpos_limits(name="right_eef")[:, :, 0] - - def _generate_eef_motion( - self, num_steps: int = 10, *, opening: bool = True - ) -> torch.Tensor: - """Interpolate the right gripper between its closed and open limits. - - Args: - num_steps: Number of trajectory samples. - opening: Whether to open rather than close the gripper. - - Returns: - Gripper joint trajectory with shape ``(num_steps, eef_dof)``. - """ - if num_steps < 2: - raise ValueError("num_steps must be at least 2.") - - current_qpos = self.eef_close if opening else self.eef_open - target_qpos = self.eef_open if opening else self.eef_close - return interpolate_with_nums( - torch.stack([current_qpos, target_qpos], dim=1), - interp_nums=[num_steps - 1], - device=self.device, - ).squeeze(0) - - def create_demo_action_list(self, *args: Any, **kwargs: Any) -> torch.Tensor: - """Generate an expert trajectory that grasps and pulls the drawer handle. - - The demonstration is defined for the single-environment CobotMagic task - configuration and consists of four phases: move to the start pose, - approach the handle, close the gripper, and pull the drawer open. - - Returns: - Joint-position actions with shape ``(num_steps, action_dof)``. - - Raises: - ValueError: If the environment contains more than one arena. - RuntimeError: If any motion-planning phase fails. - """ - if self.num_envs != 1: - raise ValueError( - "OpenDrawerEnv expert demonstrations currently require num_envs=1." - ) - - qpos_start = torch.tensor( - [[0.0, 2.06, -0.75, 0.0, -1.20, 1.6]], - dtype=torch.float32, - device=self.device, - ) - - options_to_start = MotionGenOptions( - control_part="right_arm", - is_interpolate=True, - start_qpos=self.robot.get_qpos("right_arm")[0], - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=50, +def _translation_pose(x: float, y: float, z: float) -> tuple[float, ...]: + """Return a flattened identity-rotation pose with one translation.""" + return ( + 1.0, + 0.0, + 0.0, + x, + 0.0, + 1.0, + 0.0, + y, + 0.0, + 0.0, + 1.0, + z, + 0.0, + 0.0, + 0.0, + 1.0, + ) + + +def create_open_drawer_scene_binding() -> SimulationSceneBinding: + """Declare the exact native drawer identities used by the semantic task.""" + approach = _translation_pose(-0.00442594, -0.00050044, -0.10508996) + contact = _translation_pose(-0.00442594, -0.00050041, 0.00491005) + retract = _translation_pose(-0.00442594, -0.00050044, -0.00508996) + return SimulationSceneBinding( + registry_id=DRAWER_SCENE_REGISTRY_ID, + articulations=( + SimulationArticulationBinding( + entity_id=DRAWER_UID, + simulation_uid=DRAWER_UID, + dynamics=SceneDynamics.DYNAMIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="sliding_drawer", + default_operation_affordance=DRAWER_HANDLE_AFFORDANCE_ID, ), - ) - plan_to_start_result = self.motion_gen.generate( - target_states=[ - PlanState.single(move_type=MoveType.JOINT_MOVE, qpos=qpos_start[0]) - ], - options=options_to_start, - ) - plan_to_start = _require_plan_positions( - plan_to_start_result, phase="move to start" - ) - - xpos_begin = self.robot.compute_fk( - name="right_arm", qpos=qpos_start, to_matrix=True - )[0] - xpos_mid = xpos_begin.clone() - xpos_mid[0, 3] += 0.11 - - options_to_handle = MotionGenOptions( - control_part="right_arm", - is_interpolate=True, - is_linear=True, - start_qpos=qpos_start[0], - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=50, + ), + links=( + SimulationArticulationLinkBinding( + entity_id=DRAWER_HANDLE_LINK_ID, + articulation_id=DRAWER_UID, + native_link_name=DRAWER_NATIVE_HANDLE_LINK, + dynamics=SceneDynamics.DYNAMIC, + semantic_type="drawer_handle_link", ), - ) - plan_to_handle_result = self.motion_gen.generate( - target_states=[ - PlanState.single(move_type=MoveType.EEF_MOVE, xpos=xpos) - for xpos in (xpos_begin, xpos_mid) - ], - options=options_to_handle, - ) - plan_to_handle = _require_plan_positions( - plan_to_handle_result, phase="handle approach" - ) - - options_leave_handle = MotionGenOptions( - control_part="right_arm", - is_interpolate=True, - is_linear=True, - start_qpos=plan_to_handle[-1], - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=50, + ), + articulation_operations=( + ArticulationOperationAffordanceBinding( + entity_id=DRAWER_HANDLE_AFFORDANCE_ID, + articulation_id=DRAWER_UID, + link_id=DRAWER_HANDLE_LINK_ID, + joint_id=DRAWER_NATIVE_SLIDE_JOINT, + revision="open-drawer-v1", + semantic_targets={ + "open": ArticulationOperationTargetBinding( + target_position=DRAWER_OPEN_POSITION, + displacement=DRAWER_OPEN_DISPLACEMENT, + ), + }, + handle_pose_offset=_HANDLE_POSE_OFFSET, + approach_offset=approach, + contact_offset=contact, + operation_offset=contact, + retract_offset=retract, + operation_axis=(0.0, 0.0, -1.0), + position_scale=1.0, ), - ) - plan_leave_handle_result = self.motion_gen.generate( - target_states=[ - PlanState.single(move_type=MoveType.EEF_MOVE, xpos=xpos) - for xpos in (xpos_mid, xpos_begin) - ], - options=options_leave_handle, - ) - plan_leave_handle = _require_plan_positions( - plan_leave_handle_result, phase="drawer pull" - ) - - num_grasp_steps = 20 - eef_grasp_motion = self._generate_eef_motion( - num_steps=num_grasp_steps, opening=False - ) - - len_to_start = plan_to_start.shape[0] - len_to_handle = plan_to_handle.shape[0] - len_leave_handle = plan_leave_handle.shape[0] - total_len = len_to_start + len_to_handle + num_grasp_steps + len_leave_handle - trajectory = torch.zeros( - (total_len, self.robot.dof), - dtype=torch.float32, - device=self.device, - ) - - right_arm_ids = self.robot.get_joint_ids("right_arm") - right_eef_ids = self.robot.get_joint_ids("right_eef") - idx = 0 + ), + ) + + +def create_open_drawer_robot_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare the CobotMagic right-arm and right-gripper skill resource.""" + return SimulationRobotSkillProfileBinding( + profile_id=DRAWER_ROBOT_PROFILE_ID, + resources=( + ControlPartResourceBinding( + resource_id="right_manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="right_arm", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + ControlPartEndpointBinding( + endpoint_id="interaction", + control_part="right_eef", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="right_parallel_gripper", + ), + ), + ), + ), + command_presets=( + ControlPartCommandPreset( + preset_id="right_parallel_gripper", + control_part="right_eef", + commands={ + "open": (0.05, 0.05), + "grasp": (0.0, 0.0), + }, + ), + ), + defaults={ + "operate_articulation": {"primary": "right_manipulator"}, + }, + presets=(SkillPolicyPreset("safe"),), + default_preset="safe", + ) - trajectory[idx : idx + len_to_start, right_arm_ids] = plan_to_start - trajectory[idx : idx + len_to_start, right_eef_ids] = self._generate_eef_motion( - num_steps=len_to_start, opening=True - ) - idx += len_to_start - trajectory[idx : idx + len_to_handle, right_arm_ids] = plan_to_handle - trajectory[idx : idx + len_to_handle, right_eef_ids] = self.eef_open.expand( - len_to_handle, -1 - ) - idx += len_to_handle - - trajectory[idx : idx + num_grasp_steps, right_arm_ids] = ( - plan_to_handle[-1].unsqueeze(0).expand(num_grasp_steps, -1) - ) - trajectory[idx : idx + num_grasp_steps, right_eef_ids] = eef_grasp_motion - idx += num_grasp_steps +@register_env("OpenDrawer-v1", max_episode_steps=300) +class OpenDrawerEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + """Open a drawer through a configured semantic Expert Program.""" - trajectory[idx : idx + len_leave_handle, right_arm_ids] = plan_leave_handle - trajectory[idx : idx + len_leave_handle, right_eef_ids] = self.eef_close.expand( - len_leave_handle, -1 + def __init__(self, cfg: EmbodiedEnvCfg, **kwargs: Any) -> None: + """Initialize the configured scene without task-level motion code.""" + super().__init__(cfg, **kwargs) + self._expert_program_adapter = create_simulation_expert_program_adapter( + self, + scene_binding=create_open_drawer_scene_binding(), + robot_profile_binding=create_open_drawer_robot_profile_binding(), ) - return trajectory[:, self.active_joint_ids] + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the shared adapter assembled for this environment.""" + return self._expert_program_adapter diff --git a/tests/gym/envs/expert_program/test_task_vertical_slices.py b/tests/gym/envs/expert_program/test_task_vertical_slices.py new file mode 100644 index 000000000..af67f89e3 --- /dev/null +++ b/tests/gym/envs/expert_program/test_task_vertical_slices.py @@ -0,0 +1,625 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Configuration and non-physical bridge vertical slices for Expert Programs.""" + +from __future__ import annotations + +from copy import deepcopy +import json +from pathlib import Path + +import pytest +import torch +import yaml + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramCompiler, + decode_expert_program, +) +from embodichain.lab.gym.envs.expert_program.bridge import ( + AtomicDemoBridge, + BufferedGymCommandSink, + EnvironmentStepClock, + RuntimeCommandFrameEncoder, +) +from embodichain.lab.sim.atomic_actions import Affordance, EntityState, TaskState +from embodichain.lab.sim.skills.calls import OperateArticulation, Pick, Place +from embodichain.lab.sim.skills.runtime import SkillResult, SkillStatus +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) +from embodichain_tasks.configs import get_config_path +from embodichain_tasks.multi_segments import cube_pick_place as cube_task +from embodichain_tasks.tableware import open_drawer as drawer_task + +_REPEATED_CUBE_PROGRAM = Path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" +) +_OPEN_DRAWER_PROGRAM = Path("expert_program/tableware/open_drawer.json") +_LIFECYCLE_BATCH_SIZE = 2 +_LIFECYCLE_ROBOT_DOF = 3 +_LIFECYCLE_STEP_DT = 0.02 + + +class _NeverObserveProvider: + """Reject dynamic observations during configuration decoding/compilation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + raise AssertionError("Task configuration compilation must not observe state.") + + +class _FixedQposProvider: + """Return a finite full-qpos hold for the bridge's unused command sink.""" + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + return torch.zeros( + (env_ids.numel(), _LIFECYCLE_ROBOT_DOF), + dtype=torch.float32, + device=env_ids.device, + ) + + +class _FreshObservationPort: + """Issue one distinct observation generation for every segment runtime.""" + + def __init__(self) -> None: + self.generations: list[int] = [] + + def capture(self) -> int: + generation = len(self.generations) + 1 + self.generations.append(generation) + return generation + + +class _CompletedSegmentRuntime: + """Complete each semantic prefix from one freshly captured observation.""" + + def __init__( + self, + observation: _FreshObservationPort, + lifecycle_events: list[tuple[str, int]], + ) -> None: + self._observation = observation + self._lifecycle_events = lifecycle_events + self._status = SkillStatus.IDLE + self._result = self._make_result( + status=SkillStatus.IDLE, + workflow_id=None, + eligible_mask=torch.ones(_LIFECYCLE_BATCH_SIZE, dtype=torch.bool), + generation=0, + ) + self.analysis_window_lengths: list[int] = [] + self.executed_semantic_ids: list[str] = [] + self.eligible_masks: list[torch.Tensor | None] = [] + + @staticmethod + def _make_result( + *, + status: SkillStatus, + workflow_id: str | None, + eligible_mask: torch.Tensor, + generation: int, + ) -> SkillResult: + terminal = status is SkillStatus.COMPLETED + return SkillResult( + status=status, + workflow_id=workflow_id, + current_call_index=None, + env_ids=torch.arange(_LIFECYCLE_BATCH_SIZE, dtype=torch.long), + success_mask=( + eligible_mask.clone() if terminal else torch.zeros_like(eligible_mask) + ), + failure_mask=torch.zeros_like(eligible_mask), + cancelled_mask=torch.zeros_like(eligible_mask), + eligible_mask=eligible_mask, + task_state=TaskState.empty(_LIFECYCLE_BATCH_SIZE, "cpu"), + message=f"observation_generation={generation}", + ) + + @property + def result(self) -> SkillResult: + return self._result + + @property + def status(self) -> SkillStatus: + return self._status + + def start( + self, + *calls: object, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + call_values = tuple(calls[0]) if len(calls) == 1 else tuple(calls) + if execution_prefix_length is None: + raise AssertionError("A packaged sequential segment requires a prefix.") + selected = ( + torch.ones(_LIFECYCLE_BATCH_SIZE, dtype=torch.bool) + if eligible_mask is None + else eligible_mask.clone() + ) + execution_calls = call_values[:execution_prefix_length] + generation = self._observation.capture() + self._lifecycle_events.append(("observe", generation)) + self.analysis_window_lengths.append(len(call_values)) + self.executed_semantic_ids.extend( + str(getattr(call, "semantic_id")) for call in execution_calls + ) + self.eligible_masks.append( + None if eligible_mask is None else eligible_mask.clone() + ) + self._status = SkillStatus.COMPLETED + self._result = self._make_result( + status=SkillStatus.COMPLETED, + workflow_id=workflow_id, + eligible_mask=selected, + generation=generation, + ) + return self._result + + def step(self) -> SkillResult: + raise AssertionError("A terminal fake runtime must not be stepped.") + + def cancel(self, reason: str) -> SkillResult: + raise AssertionError(f"A completed fake runtime cannot be cancelled: {reason}") + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + del task_state + return self._result + + +class _LifecyclePostPolicyPort: + """Run every packaged settle policy and expose deterministic metadata.""" + + def __init__( + self, + observation: _FreshObservationPort, + lifecycle_events: list[tuple[str, int]], + ) -> None: + self._observation = observation + self._lifecycle_events = lifecycle_events + self.active_masks: list[torch.Tensor] = [] + self._metadata: dict[int, dict[str, object]] = {} + + def validate_policy(self, policy: object, *, segment: object) -> None: + del policy, segment + + def actions( + self, + policy: object, + *, + segment: object, + active_mask: torch.Tensor, + ): + segment_index = int(getattr(segment, "segment_index")) + generation = self._observation.generations[-1] + self._lifecycle_events.append(("settle", segment_index)) + self.active_masks.append(active_mask.clone()) + self._metadata[id(policy)] = { + "status": "settled", + "segment_index": segment_index, + "observation_generation": generation, + } + yield torch.zeros( + (_LIFECYCLE_BATCH_SIZE, _LIFECYCLE_ROBOT_DOF), + dtype=torch.float32, + ) + + def post_policy_result( + self, + policy: object, + *, + segment: object, + ) -> torch.Tensor: + del policy, segment + return self.active_masks[-1].clone() + + def post_policy_metadata( + self, + policy: object, + *, + segment: object, + ) -> dict[str, object]: + del segment + return dict(self._metadata[id(policy)]) + + +class _LifecycleValidatorPort: + """Validate every segment and filter one row after the first cycle.""" + + def __init__( + self, + observation: _FreshObservationPort, + lifecycle_events: list[tuple[str, int]], + ) -> None: + self._observation = observation + self._lifecycle_events = lifecycle_events + self._metadata: dict[int, dict[str, object]] = {} + + def validate_validator(self, validator: object, *, segment: object) -> None: + del validator, segment + + def validate(self, validator: object, *, segment: object) -> torch.Tensor: + segment_index = int(getattr(segment, "segment_index")) + generation = self._observation.generations[-1] + self._lifecycle_events.append(("validate", segment_index)) + result = ( + torch.tensor([True, False]) + if segment_index == 0 + else torch.ones(_LIFECYCLE_BATCH_SIZE, dtype=torch.bool) + ) + self._metadata[id(validator)] = { + "segment_index": segment_index, + "observation_generation": generation, + "accepted_mask": result.tolist(), + } + return result + + def validator_metadata( + self, + validator: object, + *, + segment: object, + ) -> dict[str, object]: + del segment + return dict(self._metadata[id(validator)]) + + +def _read_payload(relative_path: Path) -> dict[str, object]: + """Load one packaged JSON/YAML example as inert data.""" + path = get_config_path(relative_path) + if path.suffix == ".json": + payload = json.loads(path.read_text(encoding="utf-8")) + else: + payload = yaml.safe_load(path.read_text(encoding="utf-8")) + assert type(payload) is dict + return payload + + +def _cube_compiler() -> ExpertProgramCompiler: + """Build the smallest typed identity registry needed by the cube program.""" + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_NeverObserveProvider(), + ), + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def _drawer_compiler() -> ExpertProgramCompiler: + """Build typed drawer and handle identities without any motion code.""" + provider = _NeverObserveProvider() + drawer = SceneArticulationRef("drawer") + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=drawer, state_provider=provider), + SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + parent=drawer, + native_name="handle_xpos", + affordance=Affordance(), + relative_pose=torch.eye(4), + ), + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def test_repeated_cube_program_is_three_lazy_semantic_segments() -> None: + """The packaged cube task expands to three independently scoped cycles.""" + config = decode_expert_program(_read_payload(_REPEATED_CUBE_PROGRAM)) + + assert config.integration.scene_registry == cube_task.CUBE_SCENE_REGISTRY_ID + assert config.integration.robot_profile == cube_task.CUBE_ROBOT_PROFILE_ID + + segments = tuple(_cube_compiler().compile(config)) + + assert [segment.name for segment in segments] == ["move_cube"] * 3 + assert [segment.segment_index for segment in segments] == [0, 1, 2] + assert [len(segment.calls) for segment in segments] == [2, 2, 2] + assert all(type(segment.calls[0].call) is Pick for segment in segments) + assert all(type(segment.calls[1].call) is Place for segment in segments) + assert [ + segment.calls[1].target_selections[0].value_index for segment in segments + ] == [0, 1, 0] + assert [ + segment.validators[0].target_selection.value_index for segment in segments + ] == [ + 0, + 1, + 0, + ] + assert all( + segment.post_policies[0].cfg.kind == "wait_stable" for segment in segments + ) + assert all( + segment.validators[0].cfg.position_tolerance == 0.12 for segment in segments + ) + + +def test_packaged_repeated_cube_runs_three_lazy_bridge_lifecycles() -> None: + """The real packaged program owns three ordered observable lifecycles.""" + config = decode_expert_program(_read_payload(_REPEATED_CUBE_PROGRAM)) + compiled = _cube_compiler().compile(config).materialize() + lifecycle_events: list[tuple[str, int]] = [] + observation = _FreshObservationPort() + clock = EnvironmentStepClock(_LIFECYCLE_STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_FixedQposProvider()), + clock, + ) + runtime = _CompletedSegmentRuntime(observation, lifecycle_events) + post_port = _LifecyclePostPolicyPort(observation, lifecycle_events) + validator_port = _LifecycleValidatorPort(observation, lifecycle_events) + bridge = AtomicDemoBridge( + compiled, + runtime, + sink, + clock, + post_policy_port=post_port, + validator_port=validator_port, + ) + + iterator = iter(bridge.iter_segments()) + segment_names: list[str | None] = [] + segment_metadata: list[dict[str, object]] = [] + action_metadata: list[dict[str, object]] = [] + accepted_masks: list[list[bool]] = [] + for segment_index in range(3): + observation_count = len(observation.generations) + demo_segment = next(iterator) + segment_names.append(demo_segment.name) + + # Merely requesting the next lazy segment must not capture live state. + assert len(observation.generations) == observation_count + actions = tuple(demo_segment.actions) + + assert observation.generations == list(range(1, segment_index + 2)) + assert len(actions) == 1 + assert demo_segment.metadata["validation"] is None + action_metadata.append(dict(actions[0].metadata)) + accepted_masks.append(demo_segment.validator().tolist()) + segment_metadata.append(dict(demo_segment.metadata)) + + with pytest.raises(StopIteration): + next(iterator) + + assert segment_names == ["move_cube"] * 3 + assert runtime.analysis_window_lengths == [6, 4, 2] + assert runtime.executed_semantic_ids == ["pick", "place"] * 3 + assert observation.generations == [1, 2, 3] + assert lifecycle_events == [ + ("observe", 1), + ("settle", 0), + ("validate", 0), + ("observe", 2), + ("settle", 1), + ("validate", 1), + ("observe", 3), + ("settle", 2), + ("validate", 2), + ] + assert runtime.eligible_masks[0] is None + assert [mask.tolist() for mask in runtime.eligible_masks[1:]] == [ + [True, False], + [True, False], + ] + assert [mask.tolist() for mask in post_port.active_masks] == [ + [True, True], + [True, False], + [True, False], + ] + assert accepted_masks == [[True, False]] * 3 + + for segment_index, metadata in enumerate(segment_metadata): + eligible_before = [True, True] if segment_index == 0 else [True, False] + validator_result = [True, False] if segment_index == 0 else [True, True] + assert metadata["expert_program_id"] == compiled.program_id + assert metadata["program_segment_index"] == segment_index + assert metadata["semantic_call_indices"] == [ + 2 * segment_index, + 2 * segment_index + 1, + ] + assert metadata["post_policy_count"] == 1 + assert metadata["validator_count"] == 1 + runtime_metadata = metadata["runtime"] + assert isinstance(runtime_metadata, dict) + assert runtime_metadata["message"] == ( + f"observation_generation={segment_index + 1}" + ) + post_policies = metadata["post_policies"] + assert isinstance(post_policies, list) + assert post_policies[0]["kind"] == "wait_stable" + assert post_policies[0]["result_mask"] == eligible_before + assert post_policies[0]["result"] == { + "status": "settled", + "segment_index": segment_index, + "observation_generation": segment_index + 1, + } + validation = metadata["validation"] + assert isinstance(validation, dict) + assert validation["eligible_mask_before_validation"] == eligible_before + assert validation["accepted_mask"] == [True, False] + validators = validation["validators"] + assert validators[0]["kind"] == "object_near_target" + assert validators[0]["result_mask"] == validator_result + assert validators[0]["result"] == { + "segment_index": segment_index, + "observation_generation": segment_index + 1, + "accepted_mask": validator_result, + } + json.dumps(metadata, allow_nan=False, sort_keys=True) + + assert action_metadata[segment_index]["bridge_action_kind"] == ( + "program_post_policy" + ) + assert action_metadata[segment_index]["program_segment_index"] == ( + segment_index + ) + + +def test_cube_variant_extends_by_data_without_motion_generation_code() -> None: + """A fourth destination and cycle require only serialized-data changes.""" + payload = deepcopy(_read_payload(_REPEATED_CUBE_PROGRAM)) + target = payload["targets"]["drop_pose"] + target["values"].extend( + ( + { + "position": [-0.25, -0.20, 0.10], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + { + "position": [-0.25, 0.20, 0.10], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + ) + ) + payload["program"]["count"] = 4 + + segments = tuple(_cube_compiler().compile(decode_expert_program(payload))) + + assert len(segments) == 4 + last_place = segments[-1].calls[-1].call + assert type(last_place) is Place + assert last_place.at is not None + assert last_place.at.position.tolist() == pytest.approx([-0.25, 0.20, 0.10]) + + +def test_open_drawer_program_compiles_to_reusable_articulation_skill() -> None: + """The drawer task supplies a goal and identities, never a trajectory.""" + payload = _read_payload(_OPEN_DRAWER_PROGRAM) + config = decode_expert_program(payload) + + assert config.integration.scene_registry == drawer_task.DRAWER_SCENE_REGISTRY_ID + assert config.integration.robot_profile == drawer_task.DRAWER_ROBOT_PROFILE_ID + + segments = tuple(_drawer_compiler().compile(config)) + + assert len(segments) == 1 + assert segments[0].name == "open_drawer" + assert len(segments[0].calls) == 1 + call = segments[0].calls[0].call + assert type(call) is OperateArticulation + assert call.articulation == SceneArticulationRef("drawer") + assert call.handle == SceneAffordanceRef("drawer_handle") + assert call.target == "open" + assert call.target_position is None + assert call.target_displacement is None + assert dict(call.resources) == {} + + +def test_task_classes_do_not_override_motion_or_demo_generation() -> None: + """Both environments delegate planning and execution to the shared runtime.""" + forbidden_overrides = { + "create_demo_action_list", + "create_demo_segments", + "_generate_eef_motion", + "_initialize_atomic_actions", + "_plan_pick_place_cycle", + } + + for env_type in ( + cube_task.MultiSegmentsCubePickPlaceEnv, + drawer_task.OpenDrawerEnv, + ): + assert forbidden_overrides.isdisjoint(env_type.__dict__) + + +def test_cube_task_declares_scene_and_robot_bindings_without_trajectory_code() -> None: + """Cube integration is an auditable identity/resource declaration.""" + scene = cube_task.create_cube_scene_binding(grasp_samples=32) + profile = cube_task.create_cube_robot_profile_binding() + + assert scene.registry_id == cube_task.CUBE_SCENE_REGISTRY_ID + assert scene.rigid_objects[0].simulation_uid == "cube" + assert scene.rigid_objects[0].collision_role is SceneCollisionRole.NONE + assert scene.rigid_objects[0].default_grasp_affordance == ( + cube_task.CUBE_GRASP_AFFORDANCE_ID + ) + assert scene.antipodal_grasps[0].object_id == "cube" + assert profile.profile_id == cube_task.CUBE_ROBOT_PROFILE_ID + assert dict(profile.defaults) == { + "pick_up": {"primary": "manipulator"}, + "place": {"primary": "manipulator"}, + } + assert profile.command_presets[0].commands["grasp"] == (0.024,) + + +def test_drawer_task_declares_native_link_joint_and_named_target() -> None: + """Drawer operation grounds through explicit native simulation identities.""" + scene = drawer_task.create_open_drawer_scene_binding() + profile = drawer_task.create_open_drawer_robot_profile_binding() + + operation = scene.articulation_operations[0] + assert scene.registry_id == drawer_task.DRAWER_SCENE_REGISTRY_ID + assert scene.articulations[0].collision_role is SceneCollisionRole.NONE + assert scene.links[0].native_link_name == "handle_xpos" + assert operation.joint_id == "slide_rails" + assert operation.operation_axis == (0.0, 0.0, -1.0) + assert operation.semantic_targets["open"].target_position == 0.11 + assert profile.profile_id == drawer_task.DRAWER_ROBOT_PROFILE_ID + assert dict(profile.defaults) == { + "operate_articulation": {"primary": "right_manipulator"} + } + assert profile.command_presets[0].commands == { + "open": (0.05, 0.05), + "grasp": (0.0, 0.0), + } + + +def test_vertical_slice_payloads_expose_no_motion_layer_fields() -> None: + """Official examples remain semantic data without controller/planner knobs.""" + forbidden_fields = { + "action", + "control_part", + "eef", + "joint_ids", + "motion_generator", + "planner", + "qpos", + "sample_count", + "tcp", + "trajectory", + } + + def keys(value: object) -> set[str]: + if type(value) is dict: + return set(value).union(*(keys(item) for item in value.values())) + if type(value) is list: + return set().union(*(keys(item) for item in value)) + return set() + + for path in (_REPEATED_CUBE_PROGRAM, _OPEN_DRAWER_PROGRAM): + assert forbidden_fields.isdisjoint(keys(_read_payload(path))) + + +__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 1c04d6ca7..a54203df9 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 @@ -14,18 +14,19 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Tests for the lazy multi-segment cube pick-and-place task.""" +"""Tests for the declarative multi-segment cube task.""" from __future__ import annotations +import importlib import json from pathlib import Path -from types import MethodType, SimpleNamespace +from types import SimpleNamespace -import pytest import torch from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.gym.envs.expert_program import ExpertProgramEnvironmentMixin from embodichain.lab.gym.utils.gym_utils import config_to_cfg from embodichain.lab.gym.utils.registration import ( REGISTERED_ENVS, @@ -37,125 +38,205 @@ discover_task_packages() from embodichain_tasks.multi_segments.cube_pick_place import ( # noqa: E402 + CUBE_ROBOT_PROFILE_ID, + CUBE_SCENE_REGISTRY_ID, MultiSegmentsCubePickPlaceEnv, + _create_default_env_cfg, + create_cube_robot_profile_binding, ) -class TestMultiSegmentsCubePickPlaceEnv: - """Registration, config, and lazy-planning tests.""" - - def test_registered_and_exported(self) -> None: - """The new task category exports a registered environment.""" - from embodichain_tasks.multi_segments import __all__ - - assert "MultiSegmentsCubePickPlaceEnv" in __all__ - spec = REGISTERED_ENVS["MultiSegmentsCubePickPlace-v1"] - assert spec.cls is MultiSegmentsCubePickPlaceEnv - assert spec.max_episode_steps == 1200 - assert issubclass(MultiSegmentsCubePickPlaceEnv, EmbodiedEnv) - - def test_gym_config_targets_the_registered_task(self) -> None: - """The runnable gym config selects the task and three cycles.""" - config_path = ( - Path(__file__).parents[4] - / "embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json" - ) - config = json.loads(config_path.read_text()) - - assert config["id"] == "MultiSegmentsCubePickPlace-v1" - assert config["env"]["extensions"]["num_cycles"] == 3 - assert config["env"]["extensions"]["grasp_hold_steps"] == 45 - assert len(config["env"]["extensions"]["place_positions"]) == 2 - assert config["rigid_object"][0]["uid"] == "cube" - assert config["robot"]["class_type"] == "URRobot" - assert config["robot"]["robot_type"] == "ur5" - recorder = config["env"]["dataset"]["lerobot"] - assert recorder["func"] == "LeRobotRecorder" - assert recorder["params"]["robot_meta"] == { - "robot_type": "UR5", - "control_freq": 25, - } - assert recorder["params"]["save_path"] == "outputs/lerobot/multi_segments" - - cfg = config_to_cfg(config) - - assert isinstance(cfg.robot, URRobotCfg) - assert cfg.robot.robot_type == "ur5" - assert cfg.robot.control_parts["arm"] == [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - ] - assert cfg.robot.solver_cfg["arm"].ur_type == "ur5" - assert cfg.robot.solver_cfg["arm"].d1 == 0.089159 - - def test_segments_are_planned_lazily_from_updated_scene(self) -> None: - """Requesting the next segment observes the post-execution cube pose.""" - env = object.__new__(MultiSegmentsCubePickPlaceEnv) - env.num_cycles = 3 - env.place_positions = ((1.0, 0.0, 0.1), (2.0, 0.0, 0.1)) - env._completed_cycles = 0 - env._last_target_position = None - env.sim = SimpleNamespace(device=torch.device("cpu")) - env._scene_position_for_test = 0.0 - env._planned_positions_for_test = [] - - def fake_plan( - self: MultiSegmentsCubePickPlaceEnv, target_position: torch.Tensor - ): - source_pose = torch.eye(4).unsqueeze(0) - source_pose[:, 0, 3] = self._scene_position_for_test - self._planned_positions_for_test.append(self._scene_position_for_test) - action = torch.tensor([[self._scene_position_for_test]]) - return torch.ones(1, dtype=torch.bool), (action,), source_pose - - env._plan_pick_place_cycle = MethodType(fake_plan, env) - segments = iter(env.create_demo_segments()) - - first = next(segments) - assert env._planned_positions_for_test == [0.0] - assert first.metadata["planned_source_poses"][0][0][3] == 0.0 - - # In the real executor the first segment actions run while the outer - # generator is suspended. Emulate the resulting free-fall displacement. - list(first.actions) - env._scene_position_for_test = 0.17 - second = next(segments) - assert env._planned_positions_for_test == [0.0, 0.17] - assert second.metadata["planned_source_poses"][0][0][3] == pytest.approx(0.17) - - env._scene_position_for_test = -0.04 - third = next(segments) - assert env._planned_positions_for_test == [0.0, 0.17, -0.04] - assert third.metadata["target_position"] == pytest.approx([1.0, 0.0, 0.1]) - - list(third.actions) - try: - next(segments) - except StopIteration: - pass - else: - raise AssertionError("Expected exactly three demo segments.") - assert env._completed_cycles == 3 - - def test_invalid_positions_are_rejected(self) -> None: - """Every configured placement target must be an XYZ position.""" - with pytest.raises(ValueError, match="XYZ"): - MultiSegmentsCubePickPlaceEnv._validate_place_positions([(1.0, 2.0)]) - - def test_grasp_hold_is_inserted_before_lift(self) -> None: - """The closed grasp waypoint is held before the pickup lift starts.""" - env = object.__new__(MultiSegmentsCubePickPlaceEnv) - env.grasp_hold_steps = 2 - trajectory = torch.arange(120, dtype=torch.float32).reshape(1, 120, 1) - - augmented, clear_step = env._insert_grasp_hold(trajectory) - - assert augmented.shape == (1, 122, 1) - assert clear_step == 78 - assert augmented[0, 75, 0] == 75 - assert augmented[0, 76:78, 0].tolist() == [75, 75] - assert augmented[0, 78, 0] == 76 +def _gym_config_path() -> Path: + """Return the installed-source cube Gym config path.""" + return ( + Path(__file__).parents[4] + / "embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json" + ) + + +def _gym_payload() -> dict[str, object]: + """Load the runnable Gym configuration as inert JSON data.""" + path = _gym_config_path() + payload = json.loads(path.read_text(encoding="utf-8")) + assert type(payload) is dict + return payload + + +def test_registered_task_uses_shared_expert_program_mixin() -> None: + """The task is registered and delegates semantic execution to the mixin.""" + from embodichain_tasks.multi_segments import __all__ + + assert "MultiSegmentsCubePickPlaceEnv" in __all__ + spec = REGISTERED_ENVS["MultiSegmentsCubePickPlace-v1"] + assert spec.cls is MultiSegmentsCubePickPlaceEnv + assert spec.max_episode_steps == 1200 + assert issubclass(MultiSegmentsCubePickPlaceEnv, ExpertProgramEnvironmentMixin) + assert issubclass(MultiSegmentsCubePickPlaceEnv, EmbodiedEnv) + + +def test_gym_config_selects_packaged_expert_program() -> None: + """Normal Gym startup selects the semantic program by a relative path.""" + payload = _gym_payload() + + assert payload["id"] == "MultiSegmentsCubePickPlace-v1" + assert payload["expert_program_path"] == ( + "../../expert_program/multi_segments/repeated_cube_pick_place.yaml" + ) + extensions = payload["env"]["extensions"] + assert extensions == { + "grasp_samples": 10000, + "force_reannotate": False, + } + settle = payload["env"]["events"]["settle_cube_on_reset"] + assert settle["func"] == "wait_for_dynamic_objects_to_settle" + assert settle["mode"] == "reset" + assert settle["params"]["entity_cfgs"] == [{"uid": "cube"}] + + +def test_gym_config_keeps_scene_and_robot_configuration() -> None: + """The migration changes the expert layer, not the physical environment.""" + payload = _gym_payload() + cfg = config_to_cfg(payload, source_path=_gym_config_path()) + + assert isinstance(cfg.robot, URRobotCfg) + assert cfg.robot.robot_type == "ur5" + assert cfg.robot.control_parts["arm"] == [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + ] + assert cfg.robot.control_parts["hand"] == ["gripper_finger1_joint_1"] + assert cfg.rigid_object[0].uid == "cube" + + +def test_direct_default_cfg_loads_the_same_typed_program() -> None: + """Direct Python construction and Gym startup share one packaged program.""" + cfg = _create_default_env_cfg() + + assert cfg.expert_program is not None + assert cfg.expert_program.integration.scene_registry == CUBE_SCENE_REGISTRY_ID + assert cfg.expert_program.integration.robot_profile == CUBE_ROBOT_PROFILE_ID + assert cfg.expert_program.program_id == "repeated_cube_pick_place" + settle = cfg.events["settle_cube_on_reset"] + assert settle.func is not None + assert settle.params["entity_cfgs"][0].uid == "cube" + + +def test_robot_profile_calibrates_physical_tracking_tolerance() -> None: + """The UR5 preset tolerates its measured drive lag without disabling feedback.""" + binding = create_cube_robot_profile_binding() + + assert binding.presets[0].preset_id == "safe" + assert binding.presets[0].recovery_policy.tracking_error_threshold == 0.08 + + +def test_task_initialization_delegates_to_shared_simulation_factory( + monkeypatch, +) -> None: + """Task setup contributes bindings but no task-local motion generator.""" + adapter = object() + captured: dict[str, object] = {} + + def fake_base_init(self, cfg, **kwargs) -> None: + del cfg, kwargs + self.grasp_samples = 48 + self.force_reannotate = True + + 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(MultiSegmentsCubePickPlaceEnv.__module__) + monkeypatch.setattr( + task_module, + "create_simulation_expert_program_adapter", + fake_create_adapter, + ) + + env = MultiSegmentsCubePickPlaceEnv(cfg=object()) + + assert env.expert_program_adapter is adapter + assert captured["environment"] is env + assert ( + captured["scene_binding"] + .antipodal_grasps[0] + .generator_cfg.antipodal_sampler_cfg.n_sample + == 48 + ) + assert captured["scene_binding"].antipodal_grasps[0].force_reannotate is True + assert captured["robot_profile_binding"].profile_id == CUBE_ROBOT_PROFILE_ID + + +def test_task_config_compiles_through_real_simulation_factory( + monkeypatch, +) -> None: + """Packaged config reaches the real adapter with explicitly bound mocks.""" + + class FakeRobot: + uid = "UR5" + + @staticmethod + def get_qpos() -> torch.Tensor: + return torch.zeros((1, 8), dtype=torch.float32) + + class FakeCube: + is_non_dynamic = False + + @staticmethod + def get_vertices(*, env_ids, scale) -> torch.Tensor: + assert env_ids == [0] + assert scale is True + return torch.tensor( + [[[-0.5, -0.5, 0.0], [0.5, -0.5, 0.0], [0.0, 0.5, 0.0]]], + dtype=torch.float32, + ) + + @staticmethod + def get_triangles(*, env_ids) -> torch.Tensor: + assert env_ids == [0] + return torch.tensor([[[0, 1, 2]]], dtype=torch.int64) + + @staticmethod + def get_local_pose(*, to_matrix) -> torch.Tensor: + assert to_matrix is True + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + robot = FakeRobot() + cube = FakeCube() + + class FakeSimulation: + @staticmethod + def get_robot(uid: str): + return robot if uid == "UR5" else None + + @staticmethod + def get_rigid_object(uid: str): + return cube if uid == "cube" else None + + 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 + for name, value in cfg.extensions.items(): + setattr(self, name, value) + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + cfg = _create_default_env_cfg() + + env = MultiSegmentsCubePickPlaceEnv(cfg=cfg) + segments = tuple(env.compile_expert_program(cfg.expert_program)) + + assert len(segments) == 3 + assert [segment.name for segment in segments] == ["move_cube"] * 3 + assert env.expert_program_adapter.scene_registry_id == CUBE_SCENE_REGISTRY_ID + assert env.expert_program_adapter.robot_profile_id == CUBE_ROBOT_PROFILE_ID + + +__all__: list[str] = [] diff --git a/tests/gym/envs/tasks/test_open_drawer.py b/tests/gym/envs/tasks/test_open_drawer.py new file mode 100644 index 000000000..81893c5a0 --- /dev/null +++ b/tests/gym/envs/tasks/test_open_drawer.py @@ -0,0 +1,306 @@ +# ---------------------------------------------------------------------------- +# 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 drawer-opening 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 ExpertProgramEnvironmentMixin +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, +) + +# Trigger official task auto-registration (idempotent). +discover_task_packages() + +from embodichain_tasks.tableware.open_drawer import ( # noqa: E402 + DRAWER_NATIVE_SLIDE_JOINT, + DRAWER_OPEN_POSITION, + DRAWER_ROBOT_PROFILE_ID, + DRAWER_UID, + OpenDrawerEnv, + create_open_drawer_scene_binding, +) + + +def _gym_config_path() -> Path: + """Return the installed-source drawer Gym config path.""" + return ( + Path(__file__).parents[4] + / "embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json" + ) + + +def _gym_payload() -> dict[str, object]: + """Load the drawer 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_drawer_task_uses_shared_expert_program_mixin() -> None: + """The environment delegates all demo generation to the shared runtime.""" + spec = REGISTERED_ENVS["OpenDrawer-v1"] + + assert spec.cls is OpenDrawerEnv + assert issubclass(OpenDrawerEnv, ExpertProgramEnvironmentMixin) + assert issubclass(OpenDrawerEnv, EmbodiedEnv) + assert "create_demo_action_list" not in OpenDrawerEnv.__dict__ + + +def test_drawer_gym_config_selects_packaged_semantic_program() -> None: + """The runnable task config points at the named-target Expert Program.""" + payload = _gym_payload() + + assert payload["id"] == "OpenDrawer-v1" + assert payload["expert_program_path"] == ( + "../../expert_program/tableware/open_drawer.json" + ) + assert payload["env"]["extensions"] == {} + + +def test_drawer_gym_config_preserves_physical_scene() -> None: + """Parsing still creates the CobotMagic robot and native drawer entity.""" + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + + assert cfg.robot.uid == "CobotMagic" + assert cfg.robot.control_parts["right_arm"] == [ + "right_joint1", + "right_joint2", + "right_joint3", + "right_joint4", + "right_joint5", + "right_joint6", + ] + assert cfg.robot.control_parts["right_eef"] == [ + "right_joint7", + "right_joint8", + ] + assert cfg.articulation[0].uid == "drawer" + assert cfg.expert_program is not None + assert cfg.expert_program.program_id == "open_drawer" + + +def test_drawer_affordance_uses_reachable_post_release_retract() -> None: + """The opened drawer retract remains clear of the handle and IK-reachable.""" + operation = create_open_drawer_scene_binding().articulation_operations[0] + contact_z = operation.contact_offset[11] + retract_z = operation.retract_offset[11] + + assert retract_z < contact_z + assert contact_z - retract_z == pytest.approx(0.01) + + +def test_task_initialization_delegates_to_shared_simulation_factory( + monkeypatch, +) -> None: + """Drawer setup contributes declarations but no planner implementation.""" + 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(OpenDrawerEnv.__module__) + monkeypatch.setattr( + task_module, + "create_simulation_expert_program_adapter", + fake_create_adapter, + ) + + env = OpenDrawerEnv(cfg=object()) + + assert env.expert_program_adapter is adapter + assert captured["environment"] is env + assert captured["scene_binding"].links[0].native_link_name == "handle_xpos" + assert captured["robot_profile_binding"].profile_id == DRAWER_ROBOT_PROFILE_ID + + +def test_task_config_compiles_through_real_simulation_factory( + monkeypatch, +) -> None: + """Packaged drawer config reaches the real adapter with explicit mocks.""" + + class FakeRobot: + uid = "CobotMagic" + + @staticmethod + def get_qpos() -> torch.Tensor: + return torch.zeros((1, 16), dtype=torch.float32) + + class FakeDrawer: + link_names = ("outer_box", "inner_box", "handle_xpos") + joint_names = ("slide_rails",) + + @staticmethod + def get_local_pose(*, to_matrix) -> torch.Tensor: + assert to_matrix is True + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + @staticmethod + def get_link_pose(name: str, *, env_ids, to_matrix) -> torch.Tensor: + assert name == "handle_xpos" + assert env_ids == [0] + assert to_matrix is True + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + robot = FakeRobot() + drawer = FakeDrawer() + + class FakeSimulation: + @staticmethod + def get_robot(uid: str): + return robot if uid == "CobotMagic" else None + + @staticmethod + def get_articulation(uid: str): + return drawer if uid == "drawer" else None + + 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) + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + + env = OpenDrawerEnv(cfg=cfg) + segments = tuple(env.compile_expert_program(cfg.expert_program)) + + assert len(segments) == 1 + assert segments[0].name == "open_drawer" + assert env.expert_program_adapter.scene_registry_id == "open_drawer_v1" + assert env.expert_program_adapter.robot_profile_id == DRAWER_ROBOT_PROFILE_ID + + +@pytest.mark.requires_sim +@pytest.mark.slow +def test_real_sim_expert_episode_opens_drawer_with_joint_effect_trace() -> None: + """The packaged program completes against live drawer physics and evidence.""" + import gc + + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + cfg.num_envs = 1 + cfg.sim_cfg = SimulationManagerCfg( + headless=True, + sim_device="cpu", + num_envs=1, + ) + cfg.sensor = [] + cfg.events = None + cfg.observations = None + cfg.dataset = None + cfg.init_rollout_buffer = False + cfg.record_trajectory = False + cfg.filter_dataset_saving = True + + env: OpenDrawerEnv | None = None + try: + env = OpenDrawerEnv(cfg=cfg) + env.reset(seed=0) + + result = execute_demo_episode(env) + + assert result.completed + assert result.all_success + assert result.terminal_reason == "success" + assert len(result.segments) == 1 + segment = result.segments[0] + assert segment.name == "open_drawer" + assert segment.success + + metadata = segment.metadata + runtime = metadata["runtime"] + assert runtime["kind"] == "skill_result" + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + assert len(runtime["calls"]) == 1 + call = runtime["calls"][0] + assert call["semantic_id"] == "operate_articulation" + 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] + + effects = call["effects"] + assert effects + for effect in effects: + assert effect["effect_spec"]["semantic_id"] == "operate_articulation" + evidence = effect["evidence"]["joint.position"] + assert evidence["valid_mask"] == [True] + assert evidence["acquisition_errors"] == [None] + assert evidence["env_ids"] == [0] + final_effect = effects[-1] + assert final_effect["decision"] == { + "success_mask": [True], + "failure_mask": [False], + } + + assert metadata["post_policies"] == [] + assert metadata["validation"] == { + "env_ids": [0], + "runtime_success_mask": [True], + "eligible_mask_before_validation": [True], + "post_policy_success_mask": None, + "validators": [], + "accepted_mask": [True], + } + + drawer = env.sim.get_articulation(DRAWER_UID) + assert drawer is not None + joint_index = drawer.joint_names.index(DRAWER_NATIVE_SLIDE_JOINT) + final_position = float(drawer.get_qpos()[0, joint_index].item()) + joint_tolerance = float( + final_effect["monitor"]["resolved_params"]["joint_success_tolerance"] + ) + assert abs(final_position - DRAWER_OPEN_POSITION) <= joint_tolerance + finally: + if env is not None: + env.close() + SimulationManager.flush_cleanup_queue() + gc.collect() + + +__all__: list[str] = [] diff --git a/tests/test_expert_program_package_data.py b/tests/test_expert_program_package_data.py new file mode 100644 index 000000000..4d695881f --- /dev/null +++ b/tests/test_expert_program_package_data.py @@ -0,0 +1,196 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Focused setuptools coverage for packaged Expert Program resources.""" + +from __future__ import annotations + +import ast +import json +import os +from pathlib import Path +import shutil +import subprocess +import sys +from typing import NamedTuple + +import pytest +from setuptools import Distribution +from setuptools.command.build_py import build_py + +from setup import get_package_dir + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[1] +_SETUP_PATH = _REPOSITORY_ROOT / "setup.py" +_CONFIG_PACKAGE = "embodichain_tasks.configs" +_CONFIG_SOURCE = _REPOSITORY_ROOT / "embodichain_tasks" / "configs" +_PROGRAMS = { + Path("expert_program/multi_segments/repeated_cube_pick_place.yaml"): ( + "repeated_cube_pick_place" + ), + Path("expert_program/tableware/open_drawer.json"): "open_drawer", +} + + +class _StagedConfigPackage(NamedTuple): + """Isolated setuptools output and the setup options that produced it.""" + + build_lib: Path + relative_outputs: frozenset[Path] + package_data: dict[str, list[str]] + include_package_data: bool + + +def _literal_setup_keyword(keyword_name: str) -> object: + """Read one literal keyword from the repository's setup() call.""" + tree = ast.parse(_SETUP_PATH.read_text(encoding="utf-8"), filename=str(_SETUP_PATH)) + setup_calls = tuple( + node + for node in ast.walk(tree) + if isinstance(node, ast.Call) + and isinstance(node.func, ast.Name) + and node.func.id == "setup" + ) + if len(setup_calls) != 1: + raise AssertionError("setup.py must contain exactly one setup() call.") + keywords = { + keyword.arg: keyword.value + for keyword in setup_calls[0].keywords + if keyword.arg is not None + } + if keyword_name not in keywords: + raise AssertionError(f"setup.py does not declare {keyword_name!r}.") + return ast.literal_eval(keywords[keyword_name]) + + +@pytest.fixture +def staged_config_package(tmp_path: Path) -> _StagedConfigPackage: + """Stage only the two official programs through the real build_py command.""" + package_data = _literal_setup_keyword("package_data") + include_package_data = _literal_setup_keyword("include_package_data") + assert type(package_data) is dict + assert type(include_package_data) is bool + + isolated_source = tmp_path / "source" / "embodichain_tasks" / "configs" + isolated_source.mkdir(parents=True) + shutil.copyfile(_CONFIG_SOURCE / "__init__.py", isolated_source / "__init__.py") + for relative_path in _PROGRAMS: + destination = isolated_source / relative_path + destination.parent.mkdir(parents=True, exist_ok=True) + shutil.copyfile(_CONFIG_SOURCE / relative_path, destination) + + build_lib = tmp_path / "build_lib" + distribution = Distribution( + { + "packages": [_CONFIG_PACKAGE], + "package_dir": {_CONFIG_PACKAGE: str(isolated_source)}, + "package_data": package_data, + "include_package_data": include_package_data, + } + ) + distribution.script_name = str(_SETUP_PATH) + command = build_py(distribution) + command.build_lib = str(build_lib) + command.ensure_finalized() + + def reject_manifest_command(command_name: str) -> None: + raise AssertionError( + f"Focused package-data staging must not run {command_name!r}." + ) + + command.run_command = reject_manifest_command + relative_outputs = frozenset( + Path(output).resolve().relative_to(build_lib.resolve()) + for output in command.get_outputs(include_bytecode=False) + ) + command.run() + return _StagedConfigPackage( + build_lib=build_lib, + relative_outputs=relative_outputs, + package_data=package_data, + include_package_data=include_package_data, + ) + + +def test_setup_stages_both_official_expert_program_formats( + staged_config_package: _StagedConfigPackage, +) -> None: + """The actual setup patterns put nested JSON and YAML in wheel staging.""" + assert staged_config_package.include_package_data is False + assert get_package_dir()[_CONFIG_PACKAGE] == "embodichain_tasks/configs" + assert staged_config_package.package_data[_CONFIG_PACKAGE] == [ + "**/*.json", + "**/*.yaml", + "**/*.yml", + ] + expected_outputs = { + Path("embodichain_tasks") / "configs" / relative_path + for relative_path in _PROGRAMS + } + assert expected_outputs <= staged_config_package.relative_outputs + + +def test_staged_programs_decode_through_installed_config_paths( + staged_config_package: _StagedConfigPackage, + tmp_path: Path, +) -> None: + """A clean process resolves and decodes both files from wheel staging.""" + runtime_dir = tmp_path / "runtime" + runtime_dir.mkdir() + expected_ids = { + relative_path.as_posix(): program_id + for relative_path, program_id in _PROGRAMS.items() + } + script = """ +import json +from pathlib import Path +import sys + +import embodichain_tasks.configs as config_package +from embodichain.lab.gym.envs.expert_program import load_expert_program +from embodichain_tasks.configs import get_config_path + +build_lib = Path(sys.argv[1]).resolve() +expected = json.loads(sys.argv[2]) +module_path = Path(config_package.__file__).resolve() +assert module_path.is_relative_to(build_lib), (module_path, build_lib) +decoded = {} +for relative_path, expected_program_id in expected.items(): + resource_path = get_config_path(relative_path).resolve() + assert resource_path.is_relative_to(build_lib), (resource_path, build_lib) + program = load_expert_program(resource_path) + assert program.program_id == expected_program_id + decoded[relative_path] = program.program_id +print(json.dumps(decoded, sort_keys=True)) +""" + environment = os.environ.copy() + environment["PYTHONPATH"] = str(staged_config_package.build_lib) + completed = subprocess.run( + [ + sys.executable, + "-c", + script, + str(staged_config_package.build_lib), + json.dumps(expected_ids, sort_keys=True), + ], + cwd=runtime_dir, + env=environment, + check=True, + capture_output=True, + text=True, + ) + + assert json.loads(completed.stdout) == expected_ids From 872b8a14e93de0187388a2616ea3e61c5be71824 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 14:21:42 +0800 Subject: [PATCH 12/29] feat(benchmark): add expert program rollout validation --- .../design/declarative_expert_program_plan.md | 30 +- docs/design/expert_program_rollout_report.md | 68 + docs/source/api_reference/public_api.rst | 4 + scripts/benchmark/expert_program/__init__.py | 21 + .../benchmark/expert_program/demo_success.py | 1393 +++++++++++++++++ .../tools/expert_program_rollout_report.py | 503 ++++++ tests/benchmark/expert_program/__init__.py | 21 + .../expert_program/test_demo_success.py | 971 ++++++++++++ .../test_demo_success_open_drawer_sim.py | 166 ++ .../test_task_vertical_slices.py | 4 + .../test_expert_program_rollout_report.py | 94 ++ tests/test_expert_program_package_data.py | 8 +- 12 files changed, 3274 insertions(+), 9 deletions(-) create mode 100644 docs/design/expert_program_rollout_report.md create mode 100644 scripts/benchmark/expert_program/__init__.py create mode 100644 scripts/benchmark/expert_program/demo_success.py create mode 100644 scripts/tools/expert_program_rollout_report.py create mode 100644 tests/benchmark/expert_program/__init__.py create mode 100644 tests/benchmark/expert_program/test_demo_success.py create mode 100644 tests/benchmark/expert_program/test_demo_success_open_drawer_sim.py create mode 100644 tests/scripts/tools/test_expert_program_rollout_report.py diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 5bd8caa50..cab412e62 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,9 +1,12 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime - Status: core contracts are implemented through Phase 7 on stacked feature - branches. 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. + branches. A real CUDA/cuRobo dynamic-obstacle recovery gate is landed and + runs conditionally when cuRobo is installed, CUDA is available, and GPU/slow + 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` - Last updated: 2026-08-11 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), @@ -1132,8 +1135,11 @@ Implementation status: when `safe` is reachable and the registry declares dynamic collision entities, binding rejects an unsupported active planner before observation or planning. Linking produces an effective `DynamicCollisionMode.REQUIRED` preset snapshot without mutating the profile's -source preset. This preflight coverage does not replace the remaining -end-to-end dynamic-obstacle recovery simulation. +source preset. A real-simulation gate now covers semantic lowering, CUDA/cuRobo +planning, a post-plan dynamic-obstacle world change, collision-revision-aware +replanning, and successful completion. This is a conditional GPU gate: the +module skips when cuRobo is unavailable or CUDA is unavailable, and pytest runs +it only when GPU and slow tests are explicitly selected. ### Phase 2: semantic facade and compiler @@ -1284,6 +1290,13 @@ tests pass before the legacy task is switched. ### Phase 8: rollout, documentation, and deprecation +The deterministic framework/integration capability matrix and migration-size +snapshot are maintained in +[`expert_program_rollout_report.md`](expert_program_rollout_report.md). Demo +success collection uses the no-retry benchmark harness; real success-rate +claims and gates remain deferred until the repeated Cube threshold contract and +three-cycle physical acceptance are settled. + Implementation status: partial. The canonical semantic/Expert Program documentation, project-development context, task vertical slices, and public integration guidance are included in this stack. Metrics, migrations that touch Action @@ -1344,7 +1357,8 @@ independent of adoption of the new path. - grasp/release/handover effect monitors; - settling success and timeout metadata; - Open Drawer articulation effect; -- GPU-backed dynamic cuRobo coverage where supported; +- conditionally executed real CUDA/cuRobo dynamic-obstacle recovery coverage + where cuRobo and CUDA are available and GPU/slow tests are enabled; - parallel PourWater only after Phase 7 contracts land. ## 14. Acceptance criteria @@ -1355,6 +1369,10 @@ The design is complete when all of the following hold: `DynamicCollisionMode.REQUIRED` and rejects an unsupported active planner before observation, planning, or command emission without mutating the profile configuration. +- [x] On supported CUDA/cuRobo installations, a conditional real-simulation + gate moves a dynamic obstacle after the initial plan, observes the + collision-world change and replan, and reaches the target successfully; + environments without cuRobo or CUDA skip this GPU/slow gate. - [x] A versioned Expert Program is fully validated before execution and cannot evaluate arbitrary code or traverse environment attributes by string. - [x] Python, configuration, and MLLM calls share one semantic compiler, diff --git a/docs/design/expert_program_rollout_report.md b/docs/design/expert_program_rollout_report.md new file mode 100644 index 000000000..6c8228748 --- /dev/null +++ b/docs/design/expert_program_rollout_report.md @@ -0,0 +1,68 @@ +# Declarative Expert Program Rollout Report + +This is a deterministic, static Phase 8 snapshot of checked-in framework and integration code. It does not run simulation, report physical acceptance, or certify production readiness for an embodiment. + +## Framework Contract Matrix + +`framework-tested` describes the reusable framework contract only. A task appears in the matrix below only when its integration/production code is checked in; that code status does not imply physical acceptance. + +| Capability | Framework status | Integration gate | Scope | +| --- | --- | --- | --- | +| Pick + Place(at) | framework-tested | per-embodiment integration | Typed goals, compilation, execution, and terminal effects are covered. | +| Attach/release effect | framework-tested | per-embodiment integration | Effects use accepted commands plus live object-to-endpoint pose evidence. | +| OperateArticulation | framework-tested | per-embodiment integration | Typed articulation goals and execution contracts are covered. | +| Articulation effect | framework-tested | per-embodiment integration | Joint-state terminal effect validation is covered. | +| V1 sequential | framework-tested | per-task integration | Ordered call execution and failure propagation are covered. | +| HandOver | framework-tested | integration-required | No landed task integration is claimed by this report. | +| Place relation (on/inside) | framework-tested | integration-required | Embodiment frames and relation validators must be supplied. | +| Registered call | framework-tested | integration-required | Production registration must declare and validate its concrete contract. | +| V2 parallel | framework-tested | integration-required | Fail-closed by default; production use requires an authoritative validator. | + +Parallel execution remains fail-closed by default. Resource declarations alone do not authorize production concurrency; the selected embodiment must provide an authoritative validator. + +## Checked-in Integration Matrix + +Only the two checked-in vertical slices below are classified as integration/production code. Physical acceptance is tracked separately. + +| Embodiment | Task | Skill contract | Terminal effect | Program schema | Code status | Physical acceptance | +| --- | --- | --- | --- | --- | --- | --- | +| UR5 | Cube Pick + Place | Pick + Place(at) | attach/release | V1 sequential | checked in | pending: one cycle passed; full three-cycle gate remains | +| CobotMagic | Open Drawer | OperateArticulation | articulation effect | V1 sequential | checked in | fixed-seed supported-simulation slow gate; not release-required | + +HandOver, Place relations (`on`/`inside`), Registered calls, and V2 parallel are framework-tested but integration-required. They are intentionally not listed as checked-in integrations. + +Both checked-in environment classes have zero task-local motion or demo-generation overrides; `test_task_classes_do_not_override_motion_or_demo_generation` keeps that structural metric at zero. + +## Migration Size Snapshot + +The baseline is a fixed, manually recorded pre-migration snapshot: Cube is 598 lines / 23912 bytes and Drawer is 245 lines / 8833 bytes. The tool does not inspect Git history. Current values are recomputed only from the four explicit files in the table. + +Baseline identity: Cube uses Git blob `1965563b060d1fc889f03ad13d47655c2edcd99b` and Drawer uses Git blob `3b4cbdc09537098b4f109d46efb8785b88f31ce1` at each task's Python path listed in the current-source column. Blob IDs remain stable across stack rebases. + +Counting rule: `lines` is the number of raw LF (`0x0A`) bytes; `bytes` is the raw on-disk byte length. Counts are summed per task without normalizing encoding or line endings. + +| Task | Baseline lines | Current lines | Line delta | Baseline bytes | Current bytes | Byte delta | Current source files | +| --- | --- | --- | --- | --- | --- | --- | --- | +| Cube | 598 | 366 | -232 (-38.8%) | 23912 | 12448 | -11464 (-47.9%) | `embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py`
`embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml` | +| Drawer | 245 | 246 | +1 (+0.4%) | 8833 | 8391 | -442 (-5.0%) | `embodichain_tasks/embodichain_tasks/tableware/open_drawer.py`
`embodichain_tasks/configs/expert_program/tableware/open_drawer.json` | +| Total | 843 | 612 | -231 (-27.4%) | 32745 | 20839 | -11906 (-36.4%) | the four files above | + +## Demo Success Measurement + +`scripts/benchmark/expert_program/demo_success.py` executes each fixed seed exactly once, always discards the episode buffer, and counts executor exceptions as failed rows. It writes raw JSON plus a three-table Markdown report. Its CLI supports offline raw-JSON re-aggregation and an explicit `--run-simulation` mode that constructs one standard Gym environment from Gym and Expert Program configurations. + +No success-rate result or release gate is checked in yet. Open Drawer has a single real-simulation smoke pass, while repeated Cube still needs the tracking-threshold decision and three-cycle physical acceptance before a fixed-seed rate is meaningful. + +## Drift Check + +Regenerate the checked-in report after an intentional source or capability snapshot change: + +```bash +python scripts/tools/expert_program_rollout_report.py +``` + +CI and local validation can reject stale output without rewriting it: + +```bash +python scripts/tools/expert_program_rollout_report.py --check +``` diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index fb558394f..3ed8179fa 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -2019,6 +2019,8 @@ embodichain_tasks.multi_segments.cube_pick_place .. autosummary:: MultiSegmentsCubePickPlaceEnv + create_cube_robot_profile_binding + create_cube_scene_binding embodichain_tasks.rl -------------------- @@ -2110,6 +2112,8 @@ embodichain_tasks.tableware.open_drawer .. autosummary:: OpenDrawerEnv + create_open_drawer_robot_profile_binding + create_open_drawer_scene_binding embodichain_tasks.tableware.place_object_drawer ----------------------------------------------- diff --git a/scripts/benchmark/expert_program/__init__.py b/scripts/benchmark/expert_program/__init__.py new file mode 100644 index 000000000..57445d243 --- /dev/null +++ b/scripts/benchmark/expert_program/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Expert Program benchmark helpers.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/scripts/benchmark/expert_program/demo_success.py b/scripts/benchmark/expert_program/demo_success.py new file mode 100644 index 000000000..7a9b531fa --- /dev/null +++ b/scripts/benchmark/expert_program/demo_success.py @@ -0,0 +1,1393 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Measure Expert Program demo success without retries. + +The command line can either aggregate an existing raw artifact or construct one +real Gym environment from explicit Gym and Expert Program configurations. Live +runs execute every fixed seed once, discard every episode buffer, then reuse the +same raw JSON and three-table report pipeline as injected programmatic runs. + +Run offline: +``python -m scripts.benchmark.expert_program.demo_success --raw-json RAW`` + +Run live: +``python -m scripts.benchmark.expert_program.demo_success --run-simulation +--gym_config GYM --expert-program PROGRAM --case-id CASE --seeds 0 1 +--raw-json RAW`` +""" + +from __future__ import annotations + +import argparse +from collections import Counter, defaultdict +from collections.abc import Callable, Mapping, Sequence +from copy import deepcopy +from dataclasses import asdict, dataclass +from datetime import datetime, timezone +import json +import math +import os +from pathlib import Path +from statistics import mean +import sys +import time +from typing import Any + +import psutil +import torch +import gymnasium + +from embodichain.lab.gym.envs.demo import ( + DEMO_SCHEMA_VERSION, + DemoEpisodeResult, + execute_demo_episode, +) +from embodichain.lab.gym.envs.expert_program import load_expert_program +from embodichain.lab.gym.utils.gym_utils import ( + add_env_launcher_args_to_parser, + build_env_cfg_from_args, +) +from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, +) + +__all__ = [ + "DEMO_SUCCESS_SCHEMA_VERSION", + "DemoSuccessAggregates", + "DemoSuccessArtifacts", + "DemoSuccessCase", + "DemoSuccessRow", + "DemoSuccessTrial", + "MemorySnapshot", + "aggregate_demo_success_trials", + "capture_memory", + "collect_demo_success_trials", + "load_raw_trials", + "main", + "run_all_benchmarks", + "run_demo_success_benchmark", + "run_gym_demo_success_benchmark", + "write_markdown_report", + "write_raw_trials", +] + +DEMO_SUCCESS_SCHEMA_VERSION = 1 +_BENCHMARK_ID = "expert_program_demo_success" + +_TIME_COLUMNS = ( + "case", + "episodes", + "attempted_rows", + "cost_time_ms", + "mean_episode_ms", + "cpu_delta_mb", + "gpu_delta_mb", + "peak_gpu_mb", +) +_METRIC_COLUMNS = ( + "case", + "attempted", + "successes", + "success_rate", + "terminal_reasons", + "segment_failures", + "segment_failure_breakdown", + "call_failures", + "call_failure_breakdown", + "length_mean", + "length_min", + "length_max", +) +_LEADERBOARD_COLUMNS = ( + "rank", + "case", + "attempted", + "successes", + "overall_success_rate", + "length_mean", + "mean_episode_ms", +) + +EpisodeExecutor = Callable[..., DemoEpisodeResult] +EnvironmentProvider = Callable[["DemoSuccessCase"], Any] +MemorySampler = Callable[..., "MemorySnapshot"] +GymEnvironmentFactory = Callable[[argparse.Namespace, str | Path], Any] +EnvironmentCloser = Callable[[Any], None] + + +def _validate_nonempty_string(value: object, *, field_name: str) -> str: + """Return one exact non-empty string without outer whitespace.""" + if type(value) is not str: + raise TypeError(f"{field_name} must be a string.") + if not value or value != value.strip(): + raise ValueError(f"{field_name} must be non-empty without outer whitespace.") + return value + + +def _add_exception_note(error: BaseException, note: str) -> None: + """Attach an exception note on every supported Python version.""" + add_note = getattr(error, "add_note", None) + if callable(add_note): + add_note(note) + return + notes = list(getattr(error, "__notes__", ())) + notes.append(note) + error.__notes__ = notes + + +def _snapshot_string_tuple( + values: object, + *, + field_name: str, +) -> tuple[str, ...]: + """Validate and snapshot one list-or-tuple of stable string labels.""" + if type(values) not in (list, tuple): + raise TypeError(f"{field_name} must be a list or tuple.") + snapshot = tuple(values) + for index, value in enumerate(snapshot): + _validate_nonempty_string( + value, + field_name=f"{field_name}[{index}]", + ) + return snapshot + + +@dataclass(frozen=True, slots=True) +class DemoSuccessCase: + """One named demo benchmark case and its fixed evaluation seeds. + + Args: + case_id: Stable identity shown in raw artifacts and reports. + seeds: Unique seeds, each executed exactly once in the given order. + """ + + case_id: str + seeds: tuple[int, ...] + + def __post_init__(self) -> None: + _validate_nonempty_string(self.case_id, field_name="case_id") + if type(self.seeds) not in (list, tuple): + raise TypeError("seeds must be a list or tuple.") + owned_seeds = tuple(self.seeds) + if not owned_seeds: + raise ValueError("seeds must contain at least one fixed evaluation seed.") + if any(type(seed) is not int for seed in owned_seeds): + raise TypeError("Every evaluation seed must be an integer.") + if len(set(owned_seeds)) != len(owned_seeds): + raise ValueError("Evaluation seeds must be unique within a case.") + object.__setattr__(self, "seeds", owned_seeds) + + +@dataclass(frozen=True, slots=True) +class MemorySnapshot: + """Current process and PyTorch GPU memory in megabytes. + + Args: + cpu_rss_mb: Current process resident memory. + gpu_allocated_mb: Current PyTorch-allocated GPU memory. + gpu_peak_allocated_mb: Peak PyTorch GPU allocation since the last reset. + """ + + cpu_rss_mb: float + gpu_allocated_mb: float + gpu_peak_allocated_mb: float + + +@dataclass(frozen=True, slots=True) +class DemoSuccessRow: + """Normalized result for one vector-environment row. + + Args: + env_index: Zero-based row index in the vector environment. + success: Whether this row completed the episode successfully. + terminal_reason: Stable terminal-reason label. + length: Recorded row length in environment steps. + segment_failure_reasons: Segment-name-qualified failure keys. + call_failure_keys: Segment/call/status-qualified runtime failure keys. + """ + + env_index: int + success: bool + terminal_reason: str + length: int + segment_failure_reasons: tuple[str, ...] = () + call_failure_keys: tuple[str, ...] = () + + def __post_init__(self) -> None: + if type(self.env_index) is not int: + raise TypeError("env_index must be an integer.") + if self.env_index < 0: + raise ValueError("env_index must be non-negative.") + if type(self.success) is not bool: + raise TypeError("success must be a boolean.") + _validate_nonempty_string( + self.terminal_reason, + field_name="terminal_reason", + ) + if type(self.length) is not int: + raise TypeError("length must be an integer.") + if self.length < 0: + raise ValueError("length must be non-negative.") + object.__setattr__( + self, + "segment_failure_reasons", + _snapshot_string_tuple( + self.segment_failure_reasons, + field_name="segment_failure_reasons", + ), + ) + object.__setattr__( + self, + "call_failure_keys", + _snapshot_string_tuple( + self.call_failure_keys, + field_name="call_failure_keys", + ), + ) + + +@dataclass(frozen=True, slots=True) +class DemoSuccessTrial: + """Raw result for one no-retry seed execution. + + Args: + case_id: Stable benchmark case identity. + seed: Fixed seed executed exactly once. + cost_time_ms: Executor wall-clock duration in milliseconds. + cpu_delta_mb: Process RSS delta across execution. + gpu_delta_mb: PyTorch GPU allocation delta across execution. + peak_gpu_mb: Peak PyTorch GPU allocation during execution. + rows: Normalized per-environment outcomes. + episode_result: Owned JSON-compatible executor metadata. + """ + + case_id: str + seed: int + cost_time_ms: float + cpu_delta_mb: float + gpu_delta_mb: float + peak_gpu_mb: float + rows: tuple[DemoSuccessRow, ...] + episode_result: dict[str, object] + + def __post_init__(self) -> None: + _validate_nonempty_string(self.case_id, field_name="case_id") + if type(self.seed) is not int: + raise TypeError("seed must be an integer.") + numeric_fields = { + "cost_time_ms": self.cost_time_ms, + "cpu_delta_mb": self.cpu_delta_mb, + "gpu_delta_mb": self.gpu_delta_mb, + "peak_gpu_mb": self.peak_gpu_mb, + } + normalized_numeric: dict[str, float] = {} + for field_name, value in numeric_fields.items(): + if type(value) not in (int, float): + raise TypeError(f"{field_name} must be a real number.") + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + normalized_numeric[field_name] = normalized + if ( + normalized_numeric["cost_time_ms"] < 0.0 + or normalized_numeric["peak_gpu_mb"] < 0.0 + ): + raise ValueError("Elapsed time and peak GPU memory cannot be negative.") + if type(self.rows) not in (list, tuple): + raise TypeError("rows must be a list or tuple.") + owned_rows = tuple(self.rows) + if not owned_rows: + raise ValueError("A demo success trial must contain at least one row.") + if not all(type(row) is DemoSuccessRow for row in owned_rows): + raise TypeError("rows must contain exactly DemoSuccessRow values.") + env_indices = tuple(row.env_index for row in owned_rows) + if env_indices != tuple(range(len(owned_rows))): + raise ValueError( + "rows must have unique contiguous env_index values starting at zero." + ) + if type(self.episode_result) is not dict: + raise TypeError("episode_result must be a dictionary.") + owned_result = deepcopy(self.episode_result) + json.dumps(owned_result, allow_nan=False) + for field_name, value in normalized_numeric.items(): + object.__setattr__(self, field_name, value) + object.__setattr__(self, "rows", owned_rows) + object.__setattr__(self, "episode_result", owned_result) + + def to_dict(self) -> dict[str, object]: + """Return a JSON-compatible raw trial mapping. + + Returns: + An independently owned raw trial mapping. + """ + return { + "case_id": self.case_id, + "seed": self.seed, + "cost_time_ms": self.cost_time_ms, + "cpu_delta_mb": self.cpu_delta_mb, + "gpu_delta_mb": self.gpu_delta_mb, + "peak_gpu_mb": self.peak_gpu_mb, + "rows": [asdict(row) for row in self.rows], + "episode_result": deepcopy(self.episode_result), + } + + +@dataclass(frozen=True, slots=True) +class DemoSuccessAggregates: + """The three stable row sets rendered into the Markdown report. + + Args: + time_and_memory: Per-case timing and memory summaries. + success_and_metrics: Per-case success and diagnostic summaries. + leaderboard: All cases ranked by success rate. + """ + + time_and_memory: tuple[dict[str, object], ...] + success_and_metrics: tuple[dict[str, object], ...] + leaderboard: tuple[dict[str, object], ...] + + +@dataclass(frozen=True, slots=True) +class DemoSuccessArtifacts: + """Paths and in-memory results produced by one benchmark run. + + Args: + raw_json_path: Written lossless raw artifact. + report_path: Written three-table Markdown report. + trials: In-memory no-retry trials. + aggregates: In-memory report rows. + """ + + raw_json_path: Path + report_path: Path + trials: tuple[DemoSuccessTrial, ...] + aggregates: DemoSuccessAggregates + + +def capture_memory(*, reset_gpu_peak: bool = False) -> MemorySnapshot: + """Capture CPU RSS and PyTorch GPU allocation. + + Args: + reset_gpu_peak: Reset the PyTorch peak-memory counter before sampling. + + Returns: + Current CPU, GPU, and peak GPU memory in megabytes. + """ + cuda_available = torch.cuda.is_available() + if cuda_available and reset_gpu_peak: + torch.cuda.reset_peak_memory_stats() + cpu_rss_mb = psutil.Process(os.getpid()).memory_info().rss / 1024**2 + gpu_allocated_mb = ( + torch.cuda.memory_allocated() / 1024**2 if cuda_available else 0.0 + ) + gpu_peak_allocated_mb = ( + torch.cuda.max_memory_allocated() / 1024**2 if cuda_available else 0.0 + ) + return MemorySnapshot( + cpu_rss_mb=cpu_rss_mb, + gpu_allocated_mb=gpu_allocated_mb, + gpu_peak_allocated_mb=gpu_peak_allocated_mb, + ) + + +def _vector_or_default( + values: tuple[Any, ...], + *, + row_count: int, + default: Any, + field_name: str, +) -> tuple[Any, ...]: + """Return a validated per-row tuple or broadcast its scalar fallback.""" + if not values: + return tuple(default for _ in range(row_count)) + if len(values) != row_count: + raise ValueError( + f"DemoEpisodeResult.{field_name} has {len(values)} rows; " + f"expected {row_count}." + ) + return values + + +def _normalize_episode_rows(result: DemoEpisodeResult) -> tuple[DemoSuccessRow, ...]: + """Project a batched episode result into independent benchmark rows.""" + row_count = len(result.success) + if row_count == 0: + raise ValueError("DemoEpisodeResult.success must contain at least one row.") + lengths = _vector_or_default( + result.lengths, + row_count=row_count, + default=result.length, + field_name="lengths", + ) + terminal_reasons = _vector_or_default( + result.terminal_reasons, + row_count=row_count, + default=result.terminal_reason, + field_name="terminal_reasons", + ) + failures: list[list[str]] = [[] for _ in range(row_count)] + call_failures: list[list[str]] = [[] for _ in range(row_count)] + for segment in result.segments: + active = _vector_or_default( + segment.active, + row_count=row_count, + default=True, + field_name="segments.active", + ) + successes = _vector_or_default( + segment.successes, + row_count=row_count, + default=segment.success, + field_name="segments.successes", + ) + reasons = _vector_or_default( + segment.failure_reasons, + row_count=row_count, + default=segment.failure_reason, + field_name="segments.failure_reasons", + ) + for env_index in range(row_count): + if not active[env_index]: + continue + reason = reasons[env_index] + if reason is not None: + failures[env_index].append(f"{segment.name}:{reason}") + elif not successes[env_index]: + failures[env_index].append(f"{segment.name}:segment_failed") + runtime = segment.metadata.get("runtime") + if isinstance(runtime, Mapping): + _append_runtime_call_failures( + runtime, + segment_name=segment.name, + row_failures=call_failures, + ) + + return tuple( + DemoSuccessRow( + env_index=env_index, + success=bool(result.success[env_index]), + terminal_reason=str(terminal_reasons[env_index]), + length=int(lengths[env_index]), + segment_failure_reasons=tuple(failures[env_index]), + call_failure_keys=tuple(call_failures[env_index]), + ) + for env_index in range(row_count) + ) + + +def _append_runtime_call_failures( + runtime: Mapping[str, object], + *, + segment_name: str, + row_failures: list[list[str]], + branch_id: str | None = None, +) -> None: + """Attribute canonical runtime call failures to their environment rows.""" + env_ids = runtime.get("env_ids") + calls = runtime.get("calls") + if isinstance(env_ids, list) and isinstance(calls, list): + for call in calls: + if not isinstance(call, Mapping): + continue + semantic_id = call.get("semantic_id") + status = call.get("status") + masks = call.get("masks") + failed = masks.get("failed") if isinstance(masks, Mapping) else None + if ( + not isinstance(semantic_id, str) + or not isinstance(status, str) + or not isinstance(failed, list) + or len(failed) != len(env_ids) + ): + continue + identity = ( + f"{segment_name}:{semantic_id}:{status}" + if branch_id is None + else f"{segment_name}:{branch_id}:{semantic_id}:{status}" + ) + for env_id, is_failed in zip(env_ids, failed): + if ( + type(env_id) is int + and type(is_failed) is bool + and is_failed + and 0 <= env_id < len(row_failures) + ): + row_failures[env_id].append(identity) + + branches = runtime.get("branches") + if isinstance(branches, Mapping): + branch_ids = sorted(key for key in branches if isinstance(key, str)) + for child_branch_id in branch_ids: + branch_runtime = branches[child_branch_id] + if isinstance(branch_runtime, Mapping): + _append_runtime_call_failures( + branch_runtime, + segment_name=segment_name, + row_failures=row_failures, + branch_id=child_branch_id, + ) + + +def _executor_error_trial_rows(env: Any, reason: str) -> tuple[DemoSuccessRow, ...]: + """Return zero-length failed rows for one executor exception.""" + configured_rows = getattr(env, "num_envs", 1) + row_count = ( + configured_rows if type(configured_rows) is int and configured_rows > 0 else 1 + ) + return tuple( + DemoSuccessRow( + env_index=env_index, + success=False, + terminal_reason=reason, + length=0, + ) + for env_index in range(row_count) + ) + + +def _executor_error_metadata( + *, + episode_index: int, + reason: str, + error: Exception, + row_count: int, +) -> dict[str, object]: + """Return raw episode-shaped metadata that preserves one executor error.""" + return { + "schema_version": DEMO_SCHEMA_VERSION, + "episode_index": episode_index, + "length": 0, + "completed": False, + "success": [False] * row_count, + "terminated": [False] * row_count, + "truncated": [False] * row_count, + "terminal_reason": reason, + "segments": [], + "lengths": [0] * row_count, + "completed_by_env": [False] * row_count, + "terminal_reasons": [reason] * row_count, + "executor_error": { + "type": type(error).__name__, + "message": str(error), + }, + } + + +def collect_demo_success_trials( + cases: Sequence[DemoSuccessCase], + env_provider: EnvironmentProvider, + *, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, +) -> tuple[DemoSuccessTrial, ...]: + """Execute every fixed seed once and discard every resulting episode buffer. + + The caller owns environment construction and teardown. The harness performs + one non-committing seeded reset, one executor call, and one mandatory + non-committing discard reset for each seed. Executor exceptions become + failed trials only after that discard succeeds. + + Args: + cases: Named cases with fixed, unique seed sequences. + env_provider: Required environment injection. It is called once per case. + episode_executor: Demo executor, injectable for pure unit tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + + Returns: + Raw per-seed trials in case and seed order. + + Raises: + ValueError: If cases are empty, case IDs are duplicated, or an episode + result is malformed. + TypeError: If ``cases`` contains non-``DemoSuccessCase`` values. + """ + try: + case_values = tuple(cases) + except TypeError as error: + raise TypeError( + "cases must be an iterable of DemoSuccessCase values." + ) from error + if not case_values: + raise ValueError("cases must contain at least one benchmark case.") + if not all(type(case) is DemoSuccessCase for case in case_values): + raise TypeError("cases must contain exactly DemoSuccessCase values.") + case_ids = [case.case_id for case in case_values] + if len(set(case_ids)) != len(case_ids): + raise ValueError("Demo success benchmark case IDs must be unique.") + + trials: list[DemoSuccessTrial] = [] + episode_index = 0 + for case in case_values: + env = env_provider(case) + for seed in case.seeds: + env.reset(seed=seed, options={"save_data": False}) + executor_error: Exception | None = None + body_error: BaseException | None = None + try: + before = memory_sampler(reset_gpu_peak=True) + start = clock() + result: DemoEpisodeResult | None = None + try: + result = episode_executor(env, episode_index=episode_index) + except Exception as error: + executor_error = error + elapsed_ms = (clock() - start) * 1000.0 + after = memory_sampler(reset_gpu_peak=False) + except BaseException as error: + body_error = error + if executor_error is not None: + _add_exception_note( + body_error, + "Episode executor also failed before benchmark measurement " + f"completed: {type(executor_error).__name__}: " + f"{executor_error}", + ) + raise + finally: + try: + env.reset(options={"save_data": False}) + except BaseException as discard_error: + discard_note = ( + "Episode discard also failed: " + f"{type(discard_error).__name__}: {discard_error}" + ) + if body_error is not None: + _add_exception_note(body_error, discard_note) + elif executor_error is not None: + _add_exception_note(executor_error, discard_note) + raise executor_error + else: + raise + + if executor_error is None: + if result is None: + raise RuntimeError("The demo episode executor returned no result.") + rows = _normalize_episode_rows(result) + episode_result = result.to_metadata() + else: + reason = f"executor_error:{type(executor_error).__name__}" + rows = _executor_error_trial_rows(env, reason) + episode_result = _executor_error_metadata( + episode_index=episode_index, + reason=reason, + error=executor_error, + row_count=len(rows), + ) + trials.append( + DemoSuccessTrial( + case_id=case.case_id, + seed=seed, + cost_time_ms=elapsed_ms, + cpu_delta_mb=after.cpu_rss_mb - before.cpu_rss_mb, + gpu_delta_mb=after.gpu_allocated_mb - before.gpu_allocated_mb, + peak_gpu_mb=after.gpu_peak_allocated_mb, + rows=rows, + episode_result=episode_result, + ) + ) + episode_index += 1 + return tuple(trials) + + +def _counter_json(counter: Counter[str]) -> str: + """Render a deterministic compact JSON counter for one Markdown cell.""" + ordered = dict(sorted(counter.items(), key=lambda item: (-item[1], item[0]))) + return json.dumps(ordered, ensure_ascii=False, separators=(",", ":")) + + +def _validate_unique_trials( + trials: Sequence[DemoSuccessTrial], +) -> tuple[DemoSuccessTrial, ...]: + """Snapshot non-empty exact trials and reject duplicate identities.""" + try: + trial_values = tuple(trials) + except TypeError as error: + raise TypeError( + "trials must be an iterable of DemoSuccessTrial values." + ) from error + if not trial_values: + raise ValueError("trials must contain at least one demo success trial.") + if not all(type(trial) is DemoSuccessTrial for trial in trial_values): + raise TypeError("trials must contain exactly DemoSuccessTrial values.") + seen: set[tuple[str, int]] = set() + for trial in trial_values: + identity = (trial.case_id, trial.seed) + if identity in seen: + raise ValueError( + "Duplicate demo success trial for " + f"case_id={trial.case_id!r}, seed={trial.seed}." + ) + seen.add(identity) + return trial_values + + +def aggregate_demo_success_trials( + trials: Sequence[DemoSuccessTrial], +) -> DemoSuccessAggregates: + """Aggregate raw trials by case and rank every represented case. + + Args: + trials: Unique case-and-seed trials. + + Returns: + Stable rows for the three report tables. + + Raises: + ValueError: If trials are empty or a case-and-seed identity occurs more + than once. + TypeError: If ``trials`` contains non-``DemoSuccessTrial`` values. + """ + trial_values = _validate_unique_trials(trials) + grouped: dict[str, list[DemoSuccessTrial]] = defaultdict(list) + for trial in trial_values: + grouped[trial.case_id].append(trial) + + time_rows: list[dict[str, object]] = [] + metric_rows: list[dict[str, object]] = [] + for case_id in sorted(grouped): + case_trials = grouped[case_id] + rows = [row for trial in case_trials for row in trial.rows] + attempted = len(rows) + successes = sum(row.success for row in rows) + lengths = [row.length for row in rows] + terminal_reasons = Counter(row.terminal_reason for row in rows) + segment_reasons = Counter( + reason for row in rows for reason in row.segment_failure_reasons + ) + call_failure_keys = Counter( + key for row in rows for key in row.call_failure_keys + ) + time_rows.append( + { + "case": case_id, + "episodes": len(case_trials), + "attempted_rows": attempted, + "cost_time_ms": sum(trial.cost_time_ms for trial in case_trials), + "mean_episode_ms": mean(trial.cost_time_ms for trial in case_trials), + "cpu_delta_mb": mean(trial.cpu_delta_mb for trial in case_trials), + "gpu_delta_mb": mean(trial.gpu_delta_mb for trial in case_trials), + "peak_gpu_mb": max(trial.peak_gpu_mb for trial in case_trials), + } + ) + metric_rows.append( + { + "case": case_id, + "attempted": attempted, + "successes": successes, + "success_rate": successes / attempted, + "terminal_reasons": _counter_json(terminal_reasons), + "segment_failures": sum(segment_reasons.values()), + "segment_failure_breakdown": _counter_json(segment_reasons), + "call_failures": sum(call_failure_keys.values()), + "call_failure_breakdown": _counter_json(call_failure_keys), + "length_mean": mean(lengths), + "length_min": min(lengths), + "length_max": max(lengths), + } + ) + + time_by_case = {str(row["case"]): row for row in time_rows} + ranked_metrics = sorted( + metric_rows, + key=lambda row: (-float(row["success_rate"]), str(row["case"])), + ) + leaderboard = tuple( + { + "rank": rank, + "case": row["case"], + "attempted": row["attempted"], + "successes": row["successes"], + "overall_success_rate": row["success_rate"], + "length_mean": row["length_mean"], + "mean_episode_ms": time_by_case[str(row["case"])]["mean_episode_ms"], + } + for rank, row in enumerate(ranked_metrics, start=1) + ) + return DemoSuccessAggregates( + time_and_memory=tuple(time_rows), + success_and_metrics=tuple(metric_rows), + leaderboard=leaderboard, + ) + + +def write_raw_trials(path: str | Path, trials: Sequence[DemoSuccessTrial]) -> Path: + """Write lossless per-seed and per-row results to one raw JSON artifact. + + Args: + path: Destination JSON path. + trials: Unique case-and-seed trials. + + Returns: + Written artifact path. + + Raises: + ValueError: If trials are empty or a case-and-seed identity occurs more + than once. + TypeError: If ``trials`` contains non-``DemoSuccessTrial`` values. + """ + trial_values = _validate_unique_trials(trials) + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + payload = { + "schema_version": DEMO_SUCCESS_SCHEMA_VERSION, + "benchmark": _BENCHMARK_ID, + "trials": [trial.to_dict() for trial in trial_values], + } + output.write_text( + json.dumps(payload, indent=2, ensure_ascii=False, allow_nan=False) + "\n", + encoding="utf-8", + ) + return output + + +def _require_mapping(value: object, field_name: str) -> Mapping[str, object]: + """Validate one raw JSON mapping boundary.""" + if not isinstance(value, Mapping): + raise ValueError(f"{field_name} must be a JSON object.") + return value + + +def _load_row(value: object, field_name: str) -> DemoSuccessRow: + """Decode one normalized row from a raw JSON trial.""" + data = _require_mapping(value, field_name) + failures = data.get("segment_failure_reasons") + if not isinstance(failures, list) or not all( + isinstance(reason, str) for reason in failures + ): + raise ValueError(f"{field_name}.segment_failure_reasons must be a string list.") + call_failures = data.get("call_failure_keys") + if not isinstance(call_failures, list) or not all( + isinstance(key, str) for key in call_failures + ): + raise ValueError(f"{field_name}.call_failure_keys must be a string list.") + env_index = data.get("env_index") + success = data.get("success") + terminal_reason = data.get("terminal_reason") + length = data.get("length") + if type(env_index) is not int or env_index < 0: + raise ValueError(f"{field_name}.env_index must be a non-negative integer.") + if type(success) is not bool: + raise ValueError(f"{field_name}.success must be a boolean.") + if not isinstance(terminal_reason, str): + raise ValueError(f"{field_name}.terminal_reason must be a string.") + if type(length) is not int or length < 0: + raise ValueError(f"{field_name}.length must be a non-negative integer.") + return DemoSuccessRow( + env_index=env_index, + success=success, + terminal_reason=terminal_reason, + length=length, + segment_failure_reasons=tuple(failures), + call_failure_keys=tuple(call_failures), + ) + + +def _required_number(data: Mapping[str, object], key: str, field_name: str) -> float: + """Read one finite raw numeric field without accepting booleans.""" + value = data.get(key) + if type(value) not in {int, float} or not math.isfinite(float(value)): + raise ValueError(f"{field_name}.{key} must be a finite number.") + return float(value) + + +def _load_trial(value: object, index: int) -> DemoSuccessTrial: + """Decode one validated trial from a raw JSON artifact.""" + field_name = f"trials[{index}]" + data = _require_mapping(value, field_name) + case_id = data.get("case_id") + seed = data.get("seed") + rows = data.get("rows") + episode_result = data.get("episode_result") + if not isinstance(case_id, str) or not case_id: + raise ValueError(f"{field_name}.case_id must be a non-empty string.") + if type(seed) is not int: + raise ValueError(f"{field_name}.seed must be an integer.") + if not isinstance(rows, list): + raise ValueError(f"{field_name}.rows must be a list.") + episode_mapping = _require_mapping(episode_result, f"{field_name}.episode_result") + return DemoSuccessTrial( + case_id=case_id, + seed=seed, + cost_time_ms=_required_number(data, "cost_time_ms", field_name), + cpu_delta_mb=_required_number(data, "cpu_delta_mb", field_name), + gpu_delta_mb=_required_number(data, "gpu_delta_mb", field_name), + peak_gpu_mb=_required_number(data, "peak_gpu_mb", field_name), + rows=tuple( + _load_row(row, f"{field_name}.rows[{i}]") for i, row in enumerate(rows) + ), + episode_result=dict(episode_mapping), + ) + + +def load_raw_trials(path: str | Path) -> tuple[DemoSuccessTrial, ...]: + """Load a raw artifact for deterministic offline re-aggregation. + + Args: + path: Existing raw JSON artifact. + + Returns: + Validated trials in artifact order. + + Raises: + ValueError: If the artifact schema is invalid or contains no valid trial. + """ + payload = json.loads(Path(path).read_text(encoding="utf-8")) + data = _require_mapping(payload, "raw benchmark") + if data.get("schema_version") != DEMO_SUCCESS_SCHEMA_VERSION: + raise ValueError( + "Unsupported demo success raw schema version: " + f"{data.get('schema_version')!r}." + ) + if data.get("benchmark") != _BENCHMARK_ID: + raise ValueError("Raw JSON is not an Expert Program demo success artifact.") + raw_trials = data.get("trials") + if not isinstance(raw_trials, list): + raise ValueError("raw benchmark.trials must be a list.") + trials = tuple(_load_trial(trial, index) for index, trial in enumerate(raw_trials)) + return _validate_unique_trials(trials) + + +def _format_value(column: str, value: object) -> str: + """Format one Markdown value deterministically.""" + if isinstance(value, float): + if column.endswith("rate"): + return f"{value:.2%}" + return f"{value:.6f}" + return str(value).replace("|", "\\|").replace("\n", " ") + + +def _format_table( + rows: Sequence[Mapping[str, object]], columns: tuple[str, ...] +) -> list[str]: + """Render one Markdown table with a stable schema.""" + lines = [ + "| " + " | ".join(columns) + " |", + "| " + " | ".join("---" for _ in columns) + " |", + ] + lines.extend( + "| " + + " | ".join(_format_value(column, row.get(column)) for column in columns) + + " |" + for row in rows + ) + return lines + + +def write_markdown_report(path: str | Path, aggregates: DemoSuccessAggregates) -> Path: + """Write exactly one report containing exactly the required three tables. + + Args: + path: Destination Markdown path. + aggregates: Rows for timing, success metrics, and leaderboard tables. + + Returns: + Written report path. + """ + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + lines = [ + "# Expert Program Demo Success Benchmark", + "", + f"Generated at: {datetime.now(timezone.utc).isoformat(timespec='seconds')}", + "", + "Each fixed seed is executed once, no failed episode is retried, and all " + "episode buffers are discarded without being committed.", + "", + "## Time & Memory", + "", + ] + lines.extend(_format_table(aggregates.time_and_memory, _TIME_COLUMNS)) + lines.extend(["", "## Success & Other Metrics", ""]) + lines.extend(_format_table(aggregates.success_and_metrics, _METRIC_COLUMNS)) + lines.extend(["", "## Leaderboard", ""]) + lines.extend(_format_table(aggregates.leaderboard, _LEADERBOARD_COLUMNS)) + output.write_text("\n".join(lines) + "\n", encoding="utf-8") + return output + + +def run_demo_success_benchmark( + cases: Sequence[DemoSuccessCase], + env_provider: EnvironmentProvider, + *, + raw_json_path: str | Path, + report_path: str | Path, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, +) -> DemoSuccessArtifacts: + """Collect no-retry trials and write one raw JSON plus one Markdown report. + + Args: + cases: Named cases with fixed, unique seed sequences. + env_provider: Environment injection called once per case. + raw_json_path: Destination for lossless trials. + report_path: Destination for the three-table report. + episode_executor: Demo executor, injectable for tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + + Returns: + Written paths, raw trials, and aggregate rows. + + Raises: + ValueError: If output paths collide or trial identities are invalid. + """ + if Path(raw_json_path).resolve() == Path(report_path).resolve(): + raise ValueError("raw_json_path and report_path must be different files.") + trials = collect_demo_success_trials( + cases, + env_provider, + episode_executor=episode_executor, + clock=clock, + memory_sampler=memory_sampler, + ) + aggregates = aggregate_demo_success_trials(trials) + raw_path = write_raw_trials(raw_json_path, trials) + markdown_path = write_markdown_report(report_path, aggregates) + return DemoSuccessArtifacts( + raw_json_path=raw_path, + report_path=markdown_path, + trials=trials, + aggregates=aggregates, + ) + + +def _create_gym_demo_success_environment( + launcher_args: argparse.Namespace, + expert_program_path: str | Path, +) -> Any: + """Create one configured Gym environment through the standard launcher APIs.""" + gym_config_path = getattr(launcher_args, "gym_config", "") + if not gym_config_path: + raise ValueError("launcher_args.gym_config must select a Gym config file.") + if getattr(launcher_args, "action_config", None) is not None: + raise ValueError( + "--action_config is not supported by the Expert Program benchmark." + ) + + discover_task_packages() + execute_init_hooks() + env_cfg, gym_config, action_config = build_env_cfg_from_args(launcher_args) + if action_config: + raise RuntimeError( + "The Expert Program benchmark environment builder produced an " + "unexpected action configuration." + ) + env_cfg.expert_program = load_expert_program(expert_program_path) + return gymnasium.make(id=gym_config["id"], cfg=env_cfg) + + +def _flush_simulation_cleanup_queue() -> None: + """Flush deferred simulation cleanup after live benchmark work.""" + from embodichain.lab.sim.sim_manager import SimulationManager + + SimulationManager.flush_cleanup_queue() + + +def _close_gym_demo_success_environment(env: Any) -> None: + """Close one benchmark environment without terminating the host process.""" + target = getattr(env, "unwrapped", env) + close = getattr(target, "close", None) + if not callable(close): + raise TypeError("Benchmark environment must expose close().") + + close_error: BaseException | None = None + try: + close(exit_process=False) + except BaseException as error: + close_error = error + + try: + _flush_simulation_cleanup_queue() + except BaseException as error: + if close_error is None: + raise + _add_exception_note( + close_error, + "Simulation cleanup also failed: " f"{type(error).__name__}: {error}", + ) + if close_error is not None: + raise close_error + + +def run_gym_demo_success_benchmark( + case: DemoSuccessCase, + *, + launcher_args: argparse.Namespace, + expert_program_path: str | Path, + raw_json_path: str | Path, + report_path: str | Path, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, + environment_factory: GymEnvironmentFactory | None = None, + environment_closer: EnvironmentCloser | None = None, +) -> DemoSuccessArtifacts: + """Run one configured real-environment benchmark case and close it safely. + + One environment is constructed for the case and reused across its fixed + seeds. The shared harness performs exactly one execution per seed between + non-committing seeded and discard resets. Closing the environment is an + additional abort barrier and never commits an episode. + + Args: + case: Named case and unique fixed evaluation seeds. + launcher_args: Standard environment-launcher arguments containing the + Gym configuration path and simulation overrides. + expert_program_path: Explicit Expert Program JSON/YAML configuration. + raw_json_path: Destination for lossless per-seed results. + report_path: Destination for the three-table Markdown report. + episode_executor: Demo executor, injectable for pure tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + environment_factory: Optional environment construction override. + environment_closer: Optional deterministic close override. + + Returns: + Written artifacts and the in-memory no-retry results. + + Raises: + ValueError: If launcher inputs, output paths, or trials are invalid. + RuntimeError: If environment construction, execution, or cleanup fails. + """ + factory = environment_factory or _create_gym_demo_success_environment + closer = environment_closer or _close_gym_demo_success_environment + try: + env = factory(launcher_args, expert_program_path) + except BaseException as factory_error: + try: + _flush_simulation_cleanup_queue() + except BaseException as cleanup_error: + _add_exception_note( + factory_error, + "Benchmark environment construction cleanup also failed: " + f"{type(cleanup_error).__name__}: {cleanup_error}", + ) + raise + body_error: BaseException | None = None + try: + return run_all_benchmarks( + (case,), + lambda requested_case: env, + raw_json_path=raw_json_path, + report_path=report_path, + episode_executor=episode_executor, + clock=clock, + memory_sampler=memory_sampler, + ) + except BaseException as error: + body_error = error + raise + finally: + try: + closer(env) + except BaseException as cleanup_error: + if body_error is None: + raise + _add_exception_note( + body_error, + "Benchmark environment cleanup also failed: " + f"{type(cleanup_error).__name__}: {cleanup_error}", + ) + + +def run_all_benchmarks( + cases: Sequence[DemoSuccessCase], + env_provider: EnvironmentProvider, + *, + raw_json_path: str | Path, + report_path: str | Path, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, +) -> DemoSuccessArtifacts: + """Run the injected demo benchmark and print its two artifact paths. + + Args: + cases: Named cases with fixed, unique seed sequences. + env_provider: Environment injection called once per case. + raw_json_path: Destination for lossless trials. + report_path: Destination for the three-table report. + episode_executor: Demo executor, injectable for tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + + Returns: + Written paths, raw trials, and aggregate rows. + """ + print("=" * 60) + print("Expert Program Demo Success Benchmark") + print("=" * 60) + artifacts = run_demo_success_benchmark( + cases, + env_provider, + raw_json_path=raw_json_path, + report_path=report_path, + episode_executor=episode_executor, + clock=clock, + memory_sampler=memory_sampler, + ) + print(f"Raw JSON saved: {artifacts.raw_json_path}") + print(f"Markdown report saved: {artifacts.report_path}") + print("=" * 60) + print("Benchmarks complete.") + print("=" * 60) + return artifacts + + +def _build_parser() -> argparse.ArgumentParser: + """Build the offline-aggregation and live-simulation command parser.""" + parser = argparse.ArgumentParser( + description=( + "Run one fixed-seed Expert Program benchmark or aggregate an " + "existing raw JSON artifact." + ) + ) + add_env_launcher_args_to_parser(parser, require_gym_config=False) + parser.set_defaults( + num_envs=None, + renderer=None, + viser_image_fps=None, + ) + parser.add_argument( + "--run-simulation", + action="store_true", + help="Create a Gym environment and collect raw fixed-seed trials.", + ) + parser.add_argument( + "--expert-program", + type=Path, + default=None, + help="Expert Program JSON/YAML file used by --run-simulation.", + ) + parser.add_argument( + "--case-id", + type=str, + default=None, + help="Stable benchmark case identity used by --run-simulation.", + ) + parser.add_argument( + "--seeds", + type=int, + nargs="+", + default=None, + help="Unique fixed seeds, each executed exactly once in the given order.", + ) + parser.add_argument( + "--raw-json", + type=Path, + required=True, + help=( + "Raw JSON destination for --run-simulation, or an existing raw " + "artifact in offline aggregation mode." + ), + ) + parser.add_argument( + "--report", + type=Path, + default=None, + help="Output Markdown path (default: RAW with a .md suffix).", + ) + return parser + + +def _provided_option_strings(argv: Sequence[str]) -> frozenset[str]: + """Return normalized long option names explicitly present in ``argv``.""" + return frozenset(token.split("=", 1)[0] for token in argv if token.startswith("--")) + + +def _validate_cli_mode( + parser: argparse.ArgumentParser, + args: argparse.Namespace, + *, + provided_options: frozenset[str], +) -> None: + """Reject incomplete or mixed live/offline command-line inputs.""" + live_values = { + "--gym_config": args.gym_config, + "--expert-program": args.expert_program, + "--case-id": args.case_id, + "--seeds": args.seeds, + } + if args.run_simulation: + missing = [name for name, value in live_values.items() if not value] + if missing: + parser.error("--run-simulation requires " + ", ".join(missing) + ".") + if args.preview: + parser.error("--preview is not supported by --run-simulation.") + if args.action_config is not None: + parser.error("--action_config is not supported by --run-simulation.") + return + + offline_options = frozenset({"--raw-json", "--report"}) + mixed_options = sorted(provided_options - offline_options) + if mixed_options: + parser.error( + "Offline aggregation accepts only --raw-json and --report; " + "live environment options require --run-simulation: " + + ", ".join(mixed_options) + + "." + ) + + +def main(argv: Sequence[str] | None = None) -> int: + """Run live fixed-seed trials or aggregate existing raw benchmark data. + + Args: + argv: Optional command-line arguments for embedding and tests. + + Returns: + Zero after the report is written. + """ + raw_argv = tuple(sys.argv[1:] if argv is None else argv) + parser = _build_parser() + args = parser.parse_args(raw_argv) + _validate_cli_mode( + parser, + args, + provided_options=_provided_option_strings(raw_argv), + ) + report_path = args.report or args.raw_json.with_suffix(".md") + if args.raw_json.resolve() == report_path.resolve(): + raise ValueError( + "The Markdown report must not overwrite the raw JSON artifact." + ) + if args.run_simulation: + case = DemoSuccessCase( + case_id=args.case_id, + seeds=tuple(args.seeds), + ) + run_gym_demo_success_benchmark( + case, + launcher_args=args, + expert_program_path=args.expert_program, + raw_json_path=args.raw_json, + report_path=report_path, + ) + return 0 + + trials = load_raw_trials(args.raw_json) + write_markdown_report(report_path, aggregate_demo_success_trials(trials)) + print(f"Markdown report saved: {report_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/tools/expert_program_rollout_report.py b/scripts/tools/expert_program_rollout_report.py new file mode 100644 index 000000000..eeff71a75 --- /dev/null +++ b/scripts/tools/expert_program_rollout_report.py @@ -0,0 +1,503 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Render the deterministic declarative Expert Program rollout report.""" + +from __future__ import annotations + +import argparse +from collections.abc import Sequence +from dataclasses import dataclass +from pathlib import Path + +__all__ = [ + "DEFAULT_REPORT_PATH", + "REPOSITORY_ROOT", + "SourceSnapshot", + "TaskSizeMetric", + "build_task_size_metrics", + "main", + "render_report", +] + + +REPOSITORY_ROOT = Path(__file__).resolve().parents[2] +DEFAULT_REPORT_PATH = REPOSITORY_ROOT / "docs/design/expert_program_rollout_report.md" + + +@dataclass(frozen=True) +class SourceSnapshot: + """One source file included in a task migration size snapshot. + + Args: + path: Repository-relative source path. + lines: Raw LF-byte count. + bytes: Raw on-disk byte count. + """ + + path: str + lines: int + bytes: int + + +@dataclass(frozen=True) +class TaskSizeMetric: + """Baseline and current source size for one migrated task. + + Args: + task: Stable task label. + baseline_lines: Recorded pre-migration LF-byte count. + baseline_bytes: Recorded pre-migration byte count. + sources: Explicit current source snapshots. + """ + + task: str + baseline_lines: int + baseline_bytes: int + sources: tuple[SourceSnapshot, ...] + + @property + def current_lines(self) -> int: + """Return the current LF-delimited line count across all source files.""" + return sum(source.lines for source in self.sources) + + @property + def current_bytes(self) -> int: + """Return the current raw byte count across all source files.""" + return sum(source.bytes for source in self.sources) + + +@dataclass(frozen=True) +class _TaskSizeSpec: + """Stable baseline snapshot and explicit current source paths.""" + + task: str + baseline_lines: int + baseline_bytes: int + baseline_blob: str + source_paths: tuple[str, ...] + + +_TASK_SIZE_SPECS = ( + _TaskSizeSpec( + task="Cube", + baseline_lines=598, + baseline_bytes=23_912, + baseline_blob="1965563b060d1fc889f03ad13d47655c2edcd99b", + source_paths=( + "embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py", + "embodichain_tasks/configs/expert_program/multi_segments/" + "repeated_cube_pick_place.yaml", + ), + ), + _TaskSizeSpec( + task="Drawer", + baseline_lines=245, + baseline_bytes=8_833, + baseline_blob="3b4cbdc09537098b4f109d46efb8785b88f31ce1", + source_paths=( + "embodichain_tasks/embodichain_tasks/tableware/open_drawer.py", + "embodichain_tasks/configs/expert_program/tableware/open_drawer.json", + ), + ), +) + + +_FRAMEWORK_CAPABILITIES = ( + ( + "Pick + Place(at)", + "framework-tested", + "per-embodiment integration", + "Typed goals, compilation, execution, and terminal effects are covered.", + ), + ( + "Attach/release effect", + "framework-tested", + "per-embodiment integration", + "Effects use accepted commands plus live object-to-endpoint pose evidence.", + ), + ( + "OperateArticulation", + "framework-tested", + "per-embodiment integration", + "Typed articulation goals and execution contracts are covered.", + ), + ( + "Articulation effect", + "framework-tested", + "per-embodiment integration", + "Joint-state terminal effect validation is covered.", + ), + ( + "V1 sequential", + "framework-tested", + "per-task integration", + "Ordered call execution and failure propagation are covered.", + ), + ( + "HandOver", + "framework-tested", + "integration-required", + "No landed task integration is claimed by this report.", + ), + ( + "Place relation (on/inside)", + "framework-tested", + "integration-required", + "Embodiment frames and relation validators must be supplied.", + ), + ( + "Registered call", + "framework-tested", + "integration-required", + "Production registration must declare and validate its concrete contract.", + ), + ( + "V2 parallel", + "framework-tested", + "integration-required", + "Fail-closed by default; production use requires an authoritative validator.", + ), +) + + +_LANDED_INTEGRATIONS = ( + ( + "UR5", + "Cube Pick + Place", + "Pick + Place(at)", + "attach/release", + "V1 sequential", + "checked in", + "pending: one cycle passed; full three-cycle gate remains", + ), + ( + "CobotMagic", + "Open Drawer", + "OperateArticulation", + "articulation effect", + "V1 sequential", + "checked in", + "fixed-seed supported-simulation slow gate; not release-required", + ), +) + + +def _count_source(repository_root: Path, relative_path: str) -> SourceSnapshot: + """Count raw LF bytes and total bytes for one explicit repository file.""" + data = (repository_root / relative_path).read_bytes() + return SourceSnapshot( + path=relative_path, + lines=data.count(b"\n"), + bytes=len(data), + ) + + +def build_task_size_metrics( + repository_root: str | Path = REPOSITORY_ROOT, +) -> tuple[TaskSizeMetric, ...]: + """Build deterministic migration metrics from the four declared source files. + + Args: + repository_root: EmbodiChain checkout root containing the declared files. + + Returns: + Metrics in the stable order defined by the report specification. + """ + root = Path(repository_root) + return tuple( + TaskSizeMetric( + task=spec.task, + baseline_lines=spec.baseline_lines, + baseline_bytes=spec.baseline_bytes, + sources=tuple( + _count_source(root, relative_path) + for relative_path in spec.source_paths + ), + ) + for spec in _TASK_SIZE_SPECS + ) + + +def _render_table(headers: tuple[str, ...], rows: Sequence[Sequence[str]]) -> list[str]: + """Render a Markdown table with stable column and row ordering.""" + return [ + "| " + " | ".join(headers) + " |", + "| " + " | ".join("---" for _ in headers) + " |", + *("| " + " | ".join(row) + " |" for row in rows), + ] + + +def _format_delta(current: int, baseline: int) -> str: + """Format an absolute and baseline-relative size delta.""" + delta = current - baseline + percentage = delta / baseline * 100.0 + return f"{delta:+d} ({percentage:+.1f}%)" + + +def render_report(metrics: Sequence[TaskSizeMetric]) -> str: + """Render the static rollout snapshot as deterministic Markdown. + + Args: + metrics: Task size metrics, normally from :func:`build_task_size_metrics`. + + Returns: + Complete Markdown document ending with exactly one newline. + """ + if not metrics: + raise ValueError("metrics must contain at least one task snapshot.") + + metric_rows = [] + for metric in metrics: + source_paths = "
".join(f"`{source.path}`" for source in metric.sources) + metric_rows.append( + ( + metric.task, + str(metric.baseline_lines), + str(metric.current_lines), + _format_delta(metric.current_lines, metric.baseline_lines), + str(metric.baseline_bytes), + str(metric.current_bytes), + _format_delta(metric.current_bytes, metric.baseline_bytes), + source_paths, + ) + ) + + total_baseline_lines = sum(metric.baseline_lines for metric in metrics) + total_current_lines = sum(metric.current_lines for metric in metrics) + total_baseline_bytes = sum(metric.baseline_bytes for metric in metrics) + total_current_bytes = sum(metric.current_bytes for metric in metrics) + metric_rows.append( + ( + "Total", + str(total_baseline_lines), + str(total_current_lines), + _format_delta(total_current_lines, total_baseline_lines), + str(total_baseline_bytes), + str(total_current_bytes), + _format_delta(total_current_bytes, total_baseline_bytes), + "the four files above", + ) + ) + + lines = [ + "# Declarative Expert Program Rollout Report", + "", + ( + "This is a deterministic, static Phase 8 snapshot of checked-in " + "framework and integration code. It does not run simulation, report " + "physical acceptance, or certify production readiness for an embodiment." + ), + "", + "## Framework Contract Matrix", + "", + ( + "`framework-tested` describes the reusable framework contract only. A " + "task appears in the matrix below only when its integration/production " + "code is checked in; that code status does not imply physical acceptance." + ), + "", + ] + lines.extend( + _render_table( + ("Capability", "Framework status", "Integration gate", "Scope"), + _FRAMEWORK_CAPABILITIES, + ) + ) + lines.extend( + [ + "", + ( + "Parallel execution remains fail-closed by default. Resource " + "declarations alone do not authorize production concurrency; the " + "selected embodiment must provide an authoritative validator." + ), + "", + "## Checked-in Integration Matrix", + "", + ( + "Only the two checked-in vertical slices below are classified as " + "integration/production code. Physical acceptance is tracked " + "separately." + ), + "", + ] + ) + lines.extend( + _render_table( + ( + "Embodiment", + "Task", + "Skill contract", + "Terminal effect", + "Program schema", + "Code status", + "Physical acceptance", + ), + _LANDED_INTEGRATIONS, + ) + ) + lines.extend( + [ + "", + ( + "HandOver, Place relations (`on`/`inside`), Registered calls, and V2 " + "parallel are framework-tested but integration-required. They are " + "intentionally not listed as checked-in integrations." + ), + "", + ( + "Both checked-in environment classes have zero task-local motion or " + "demo-generation overrides; " + "`test_task_classes_do_not_override_motion_or_demo_generation` " + "keeps that structural metric at zero." + ), + "", + "## Migration Size Snapshot", + "", + ( + "The baseline is a fixed, manually recorded pre-migration snapshot: " + "Cube is 598 lines / 23912 bytes and Drawer is 245 lines / 8833 bytes. " + "The tool does not inspect Git history. Current values are recomputed " + "only from the four explicit files in the table." + ), + "", + ( + "Baseline identity: Cube uses Git blob " + f"`{_TASK_SIZE_SPECS[0].baseline_blob}` and Drawer uses Git blob " + f"`{_TASK_SIZE_SPECS[1].baseline_blob}` at each task's Python path " + "listed in the current-source column. Blob IDs remain stable across " + "stack rebases." + ), + "", + ( + "Counting rule: `lines` is the number of raw LF (`0x0A`) bytes; " + "`bytes` is the raw on-disk byte length. Counts are summed per task " + "without normalizing encoding or line endings." + ), + "", + ] + ) + lines.extend( + _render_table( + ( + "Task", + "Baseline lines", + "Current lines", + "Line delta", + "Baseline bytes", + "Current bytes", + "Byte delta", + "Current source files", + ), + metric_rows, + ) + ) + lines.extend( + [ + "", + "## Demo Success Measurement", + "", + ( + "`scripts/benchmark/expert_program/demo_success.py` executes each " + "fixed seed exactly once, always discards the episode buffer, and " + "counts executor exceptions as failed rows. It writes raw JSON plus " + "a three-table Markdown report. Its CLI supports offline raw-JSON " + "re-aggregation and an explicit `--run-simulation` mode that " + "constructs one standard Gym environment from Gym and Expert " + "Program configurations." + ), + "", + ( + "No success-rate result or release gate is checked in yet. Open " + "Drawer has a single real-simulation smoke pass, while repeated Cube " + "still needs the tracking-threshold decision and three-cycle physical " + "acceptance before a fixed-seed rate is meaningful." + ), + "", + "## Drift Check", + "", + ( + "Regenerate the checked-in report after an intentional source or " + "capability snapshot change:" + ), + "", + "```bash", + "python scripts/tools/expert_program_rollout_report.py", + "```", + "", + "CI and local validation can reject stale output without rewriting it:", + "", + "```bash", + "python scripts/tools/expert_program_rollout_report.py --check", + "```", + ] + ) + return "\n".join(lines) + "\n" + + +def _build_parser() -> argparse.ArgumentParser: + """Create the command-line parser.""" + parser = argparse.ArgumentParser( + description="Generate or check the declarative Expert Program rollout report." + ) + parser.add_argument( + "--check", + action="store_true", + help="Fail when the output file differs from the deterministic render.", + ) + parser.add_argument( + "--output", + type=Path, + default=DEFAULT_REPORT_PATH, + help="Markdown output path (defaults to the checked-in design report).", + ) + return parser + + +def main(argv: Sequence[str] | None = None) -> int: + """Generate the rollout report or check its checked-in representation. + + Args: + argv: Optional command-line argument sequence for tests and embedding. + + Returns: + Zero on success, or one when ``--check`` detects missing or stale output. + """ + args = _build_parser().parse_args(argv) + rendered = render_report(build_task_size_metrics()) + output = args.output + + if args.check: + try: + existing = output.read_text(encoding="utf-8") + except FileNotFoundError: + print(f"rollout report is missing: {output}") + return 1 + if existing != rendered: + print(f"rollout report is stale: {output}") + return 1 + print(f"rollout report is up to date: {output}") + return 0 + + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(rendered, encoding="utf-8") + print(f"wrote rollout report: {output}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/benchmark/expert_program/__init__.py b/tests/benchmark/expert_program/__init__.py new file mode 100644 index 000000000..b1ad75924 --- /dev/null +++ b/tests/benchmark/expert_program/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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 Expert Program benchmarks.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/tests/benchmark/expert_program/test_demo_success.py b/tests/benchmark/expert_program/test_demo_success.py new file mode 100644 index 000000000..535941a15 --- /dev/null +++ b/tests/benchmark/expert_program/test_demo_success.py @@ -0,0 +1,971 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Pure-Python tests for the no-retry demo-success benchmark.""" + +from __future__ import annotations + +import argparse +from collections import deque +import json +from pathlib import Path + +import pytest + +from embodichain.lab.gym.envs.demo import DemoEpisodeResult, DemoSegmentResult +from scripts.benchmark.expert_program import demo_success as demo_success_module +from scripts.benchmark.expert_program.demo_success import ( + DemoSuccessCase, + DemoSuccessRow, + DemoSuccessTrial, + MemorySnapshot, + aggregate_demo_success_trials, + collect_demo_success_trials, + load_raw_trials, + main, + run_all_benchmarks, + run_gym_demo_success_benchmark, + write_markdown_report, + write_raw_trials, +) + + +class _FakeEnv: + """Record benchmark reset calls without creating a simulation.""" + + def __init__(self, num_envs: int = 1) -> None: + self.num_envs = num_envs + self.reset_calls: list[dict[str, object]] = [] + self.seed: int | None = None + + def reset(self, **kwargs: object) -> None: + self.reset_calls.append(dict(kwargs)) + if "seed" in kwargs: + self.seed = int(kwargs["seed"]) + + +class _PostEpisodeDiscardFailureEnv(_FakeEnv): + """Fail the discard reset after allowing the non-committing seed reset.""" + + def __init__(self) -> None: + super().__init__() + self.non_committing_resets = 0 + + def reset(self, **kwargs: object) -> None: + super().reset(**kwargs) + if kwargs.get("options") == {"save_data": False}: + self.non_committing_resets += 1 + if self.non_committing_resets == 2: + raise RuntimeError("synthetic discard failure") + + +class _EpisodeExecutor: + """Return queued demo results and record one call per seed.""" + + def __init__(self, results: list[DemoEpisodeResult]) -> None: + self.results = deque(results) + self.calls: list[tuple[int | None, int]] = [] + + def __call__(self, env: _FakeEnv, *, episode_index: int) -> DemoEpisodeResult: + self.calls.append((env.seed, episode_index)) + return self.results.popleft() + + +def _result( + successes: tuple[bool, ...], + *, + lengths: tuple[int, ...] | None = None, + reasons: tuple[str, ...] | None = None, + segments: tuple[DemoSegmentResult, ...] = (), +) -> DemoEpisodeResult: + """Build a compact batched result with consistent vector metadata.""" + row_count = len(successes) + row_lengths = lengths or tuple(1 for _ in successes) + row_reasons = reasons or tuple( + "success" if success else "task_incomplete" for success in successes + ) + return DemoEpisodeResult( + episode_index=0, + length=max(row_lengths), + completed=all(successes), + success=successes, + terminated=tuple(successes), + truncated=tuple(False for _ in successes), + terminal_reason="success" if all(successes) else "task_incomplete", + segments=segments, + lengths=row_lengths, + completed_by_env=successes, + terminal_reasons=row_reasons, + ) + + +def _clock(values: list[float]): + """Return a deterministic clock backed by the supplied readings.""" + readings = iter(values) + return lambda: next(readings) + + +def _memory_sampler(values: list[MemorySnapshot]): + """Return a deterministic memory sampler backed by supplied snapshots.""" + snapshots = iter(values) + + def sample(*, reset_gpu_peak: bool = False) -> MemorySnapshot: # noqa: ARG001 + return next(snapshots) + + return sample + + +def test_public_case_and_row_types_validate_and_snapshot_inputs() -> None: + seeds = [3, 5] + segment_failures = ["place:timeout"] + call_failures = ["place:place:failed"] + + case = DemoSuccessCase("cube", seeds) # type: ignore[arg-type] + row = DemoSuccessRow( + env_index=0, + success=False, + terminal_reason="timeout", + length=4, + segment_failure_reasons=segment_failures, # type: ignore[arg-type] + call_failure_keys=call_failures, # type: ignore[arg-type] + ) + seeds.append(7) + segment_failures.append("mutated") + call_failures.append("mutated") + + assert case.seeds == (3, 5) + assert row.segment_failure_reasons == ("place:timeout",) + assert row.call_failure_keys == ("place:place:failed",) + with pytest.raises(TypeError, match="case_id must be a string"): + DemoSuccessCase(7, (1,)) # type: ignore[arg-type] + with pytest.raises(TypeError, match="evaluation seed"): + DemoSuccessCase("cube", (True,)) + with pytest.raises(ValueError, match="env_index must be non-negative"): + DemoSuccessRow(-1, False, "timeout", 0) + with pytest.raises(TypeError, match="success must be a boolean"): + DemoSuccessRow(0, 1, "timeout", 0) # type: ignore[arg-type] + + +def test_public_trial_validates_rows_and_owns_nested_inputs() -> None: + row = DemoSuccessRow(0, True, "success", 2) + rows = [row] + episode_result: dict[str, object] = {"success": [True]} + + trial = DemoSuccessTrial( + case_id="cube", + seed=3, + cost_time_ms=1, + cpu_delta_mb=0, + gpu_delta_mb=0, + peak_gpu_mb=0, + rows=rows, # type: ignore[arg-type] + episode_result=episode_result, + ) + rows.clear() + episode_result["success"] = [False] + + assert trial.rows == (row,) + assert trial.cost_time_ms == 1.0 + assert trial.episode_result == {"success": [True]} + with pytest.raises(ValueError, match="unique contiguous env_index"): + DemoSuccessTrial( + "cube", + 3, + 1.0, + 0.0, + 0.0, + 0.0, + (DemoSuccessRow(1, True, "success", 1),), + {}, + ) + with pytest.raises(TypeError, match="exactly DemoSuccessRow"): + DemoSuccessTrial( + "cube", + 3, + 1.0, + 0.0, + 0.0, + 0.0, + (object(),), # type: ignore[arg-type] + {}, + ) + + +def test_each_seed_executes_once_without_retry_and_discards_data() -> None: + env = _FakeEnv() + executor = _EpisodeExecutor( + [_result((False,)), _result((True,)), _result((False,))] + ) + case = DemoSuccessCase(case_id="drawer", seeds=(11, 22, 33)) + memory_values = [MemorySnapshot(100.0, 10.0, 10.0)] * 6 + + trials = collect_demo_success_trials( + [case], + lambda requested: env, + episode_executor=executor, + clock=_clock([0.0, 0.1, 1.0, 1.2, 2.0, 2.3]), + memory_sampler=_memory_sampler(memory_values), + ) + + assert [call[0] for call in executor.calls] == [11, 22, 33] + assert len(trials) == len(case.seeds) + assert env.reset_calls == [ + {"seed": 11, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + {"seed": 22, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + {"seed": 33, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + ] + + +def test_executor_error_is_counted_and_next_seed_still_executes() -> None: + env = _FakeEnv(num_envs=2) + calls: list[int | None] = [] + + def execute( + env: _FakeEnv, *, episode_index: int + ) -> DemoEpisodeResult: # noqa: ARG001 + calls.append(env.seed) + if env.seed == 7: + raise RuntimeError("synthetic execution failure") + return _result((True, True)) + + trials = collect_demo_success_trials( + [DemoSuccessCase("drawer", (7, 8))], + lambda requested: env, + episode_executor=execute, + clock=_clock([0.0, 0.1, 1.0, 1.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 4), + ) + + assert calls == [7, 8] + assert [row.terminal_reason for row in trials[0].rows] == [ + "executor_error:RuntimeError", + "executor_error:RuntimeError", + ] + assert [row.length for row in trials[0].rows] == [0, 0] + assert trials[0].episode_result["executor_error"] == { + "type": "RuntimeError", + "message": "synthetic execution failure", + } + assert all(row.success for row in trials[1].rows) + metric = aggregate_demo_success_trials(trials).success_and_metrics[0] + assert metric["attempted"] == 4 + assert metric["successes"] == 2 + assert metric["success_rate"] == pytest.approx(0.5) + assert env.reset_calls[-1] == {"options": {"save_data": False}} + + +def test_executor_error_remains_primary_when_discard_also_fails() -> None: + env = _PostEpisodeDiscardFailureEnv() + + def execute(env: _FakeEnv, *, episode_index: int) -> DemoEpisodeResult: + del env, episode_index + raise ValueError("synthetic executor failure") + + with pytest.raises(ValueError, match="synthetic executor failure") as error: + collect_demo_success_trials( + [DemoSuccessCase("drawer", (7,))], + lambda requested: env, + episode_executor=execute, + clock=_clock([0.0, 0.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 2), + ) + + assert error.value.__notes__ == [ + "Episode discard also failed: RuntimeError: synthetic discard failure" + ] + assert env.reset_calls == [ + {"seed": 7, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + ] + + +def test_measurement_error_remains_primary_when_discard_also_fails() -> None: + env = _PostEpisodeDiscardFailureEnv() + clock_calls = 0 + + def failing_clock() -> float: + nonlocal clock_calls + clock_calls += 1 + if clock_calls == 2: + raise LookupError("synthetic clock failure") + return 0.0 + + with pytest.raises(LookupError, match="synthetic clock failure") as error: + collect_demo_success_trials( + [DemoSuccessCase("drawer", (7,))], + lambda requested: env, + episode_executor=_EpisodeExecutor([_result((True,))]), + clock=failing_clock, + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)]), + ) + + assert error.value.__notes__ == [ + "Episode discard also failed: RuntimeError: synthetic discard failure" + ] + + +def test_batched_rows_aggregate_success_reasons_failures_and_lengths() -> None: + env = _FakeEnv() + segment = DemoSegmentResult( + segment_id=0, + name="place", + start_step=0, + end_step=5, + success=False, + failure_reason="segment_validation_failed", + active=(True, True), + start_steps=(0, 0), + end_steps=(3, 5), + successes=(True, False), + failure_reasons=(None, "segment_validation_failed"), + ) + executor = _EpisodeExecutor( + [ + _result( + (True, False), + lengths=(3, 5), + reasons=("success", "segment_validation_failed"), + segments=(segment,), + ) + ] + ) + trials = collect_demo_success_trials( + [DemoSuccessCase("batched", (5,))], + lambda requested: env, + episode_executor=executor, + clock=_clock([1.0, 1.25]), + memory_sampler=_memory_sampler( + [ + MemorySnapshot(100.0, 20.0, 20.0), + MemorySnapshot(104.0, 22.0, 25.0), + ] + ), + ) + + metric = aggregate_demo_success_trials(trials).success_and_metrics[0] + + assert metric["attempted"] == 2 + assert metric["successes"] == 1 + assert metric["success_rate"] == pytest.approx(0.5) + assert json.loads(str(metric["terminal_reasons"])) == { + "segment_validation_failed": 1, + "success": 1, + } + assert metric["segment_failures"] == 1 + assert json.loads(str(metric["segment_failure_breakdown"])) == { + "place:segment_validation_failed": 1 + } + assert metric["length_mean"] == pytest.approx(4.0) + + +def test_runtime_call_failures_are_attributed_by_env_and_segment() -> None: + env = _FakeEnv(num_envs=3) + sequential = DemoSegmentResult( + segment_id=0, + name="prepare", + start_step=0, + end_step=1, + success=False, + metadata={ + "runtime": { + "kind": "skill_result", + "env_ids": [0, 1, 2], + "calls": [ + { + "semantic_id": "open", + "status": "failed", + "masks": {"failed": [True, False, False]}, + } + ], + } + }, + active=(True, True, True), + start_steps=(0, 0, 0), + end_steps=(1, 1, 1), + successes=(False, True, True), + failure_reasons=("timeout", None, None), + ) + parallel = DemoSegmentResult( + segment_id=1, + name="transfer", + start_step=1, + end_step=2, + success=False, + metadata={ + "runtime": { + "kind": "parallel_skill_result", + "branches": { + "left": { + "kind": "skill_result", + "env_ids": [0, 2], + "calls": [ + { + "semantic_id": "pick", + "status": "completed", + "masks": {"failed": [False, True]}, + } + ], + }, + "right": { + "kind": "skill_result", + "env_ids": [1], + "calls": [ + { + "semantic_id": "place", + "status": "failed", + "masks": {"failed": [True]}, + } + ], + }, + }, + } + }, + active=(True, True, True), + start_steps=(1, 1, 1), + end_steps=(2, 2, 2), + successes=(True, False, False), + failure_reasons=(None, "collision", "batch_aborted"), + ) + trials = collect_demo_success_trials( + [DemoSuccessCase("runtime", (3,))], + lambda requested: env, + episode_executor=_EpisodeExecutor( + [_result((False, False, False), segments=(sequential, parallel))] + ), + clock=_clock([0.0, 0.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 2), + ) + + metric = aggregate_demo_success_trials(trials).success_and_metrics[0] + + assert metric["call_failures"] == 3 + assert json.loads(str(metric["call_failure_breakdown"])) == { + "prepare:open:failed": 1, + "transfer:left:pick:completed": 1, + "transfer:right:place:failed": 1, + } + assert json.loads(str(metric["segment_failure_breakdown"])) == { + "prepare:timeout": 1, + "transfer:batch_aborted": 1, + "transfer:collision": 1, + } + + +def _single_trial(case_id: str, successes: tuple[bool, ...]): + """Collect one deterministic trial for ranking/report tests.""" + env = _FakeEnv() + return collect_demo_success_trials( + [DemoSuccessCase(case_id, (1,))], + lambda requested: env, + episode_executor=_EpisodeExecutor([_result(successes)]), + clock=_clock([0.0, 0.01]), + memory_sampler=_memory_sampler( + [ + MemorySnapshot(100.0, 0.0, 0.0), + MemorySnapshot(100.0, 0.0, 0.0), + ] + ), + )[0] + + +def test_leaderboard_contains_every_case_with_deterministic_tie_break() -> None: + trials = ( + _single_trial("zeta", (True, False)), + _single_trial("alpha", (True, False)), + _single_trial("winner", (True, True)), + ) + + leaderboard = aggregate_demo_success_trials(trials).leaderboard + + assert [row["case"] for row in leaderboard] == ["winner", "alpha", "zeta"] + assert [row["rank"] for row in leaderboard] == [1, 2, 3] + + +def test_report_contains_exactly_three_tables(tmp_path: Path) -> None: + trials = (_single_trial("case-a", (True,)),) + report = write_markdown_report( + tmp_path / "report.md", aggregate_demo_success_trials(trials) + ) + + text = report.read_text(encoding="utf-8") + + assert text.count("\n## ") == 3 + assert text.count("\n| ---") == 3 + assert "## Time & Memory" in text + assert "## Success & Other Metrics" in text + assert "## Leaderboard" in text + + +def test_raw_json_round_trip_preserves_trials(tmp_path: Path) -> None: + trials = (_single_trial("case-a", (True, False)),) + raw_path = write_raw_trials(tmp_path / "raw.json", trials) + + loaded = load_raw_trials(raw_path) + + assert [trial.to_dict() for trial in loaded] == [ + trial.to_dict() for trial in trials + ] + + +def test_duplicate_case_seed_is_rejected_by_aggregate_write_and_load( + tmp_path: Path, +) -> None: + trial = _single_trial("case-a", (True,)) + duplicates = (trial, trial) + + with pytest.raises(ValueError, match="Duplicate demo success trial"): + aggregate_demo_success_trials(duplicates) + with pytest.raises(ValueError, match="Duplicate demo success trial"): + write_raw_trials(tmp_path / "duplicates.json", duplicates) + + raw_path = write_raw_trials(tmp_path / "raw.json", (trial,)) + payload = json.loads(raw_path.read_text(encoding="utf-8")) + payload["trials"].append(payload["trials"][0]) + raw_path.write_text(json.dumps(payload), encoding="utf-8") + with pytest.raises(ValueError, match="Duplicate demo success trial"): + load_raw_trials(raw_path) + + +def test_zero_case_and_zero_trial_benchmarks_are_rejected(tmp_path: Path) -> None: + with pytest.raises(ValueError, match="at least one benchmark case"): + collect_demo_success_trials((), lambda requested: _FakeEnv()) + with pytest.raises(ValueError, match="at least one demo success trial"): + aggregate_demo_success_trials(()) + with pytest.raises(ValueError, match="at least one demo success trial"): + write_raw_trials(tmp_path / "empty.json", ()) + + empty_raw = tmp_path / "empty-input.json" + empty_raw.write_text( + json.dumps( + { + "schema_version": 1, + "benchmark": "expert_program_demo_success", + "trials": [], + } + ), + encoding="utf-8", + ) + with pytest.raises(ValueError, match="at least one demo success trial"): + load_raw_trials(empty_raw) + + +def test_cli_offline_mode_aggregates_existing_raw_json(tmp_path: Path) -> None: + raw_path = write_raw_trials( + tmp_path / "raw.json", (_single_trial("case-a", (True,)),) + ) + report_path = tmp_path / "offline-report.md" + + exit_code = main(["--raw-json", str(raw_path), "--report", str(report_path)]) + + assert exit_code == 0 + assert report_path.is_file() + assert len(list(tmp_path.glob("*.md"))) == 1 + + +@pytest.mark.parametrize( + "live_args", + ( + ("--preview",), + ("--action_config", "actions.json"), + ("--headless",), + ("--device", "cpu"), + ("--num_envs", "1"), + ("--renderer", "auto"), + ), +) +def test_cli_offline_mode_rejects_explicit_live_options( + tmp_path: Path, + live_args: tuple[str, ...], +) -> None: + with pytest.raises(SystemExit) as error: + main([*live_args, "--raw-json", str(tmp_path / "raw.json")]) + + assert error.value.code == 2 + + +def test_gym_runner_reuses_one_environment_and_shared_no_retry_harness( + tmp_path: Path, +) -> None: + env = _FakeEnv() + launcher_args = argparse.Namespace(gym_config="gym.json", action_config=None) + factory_calls: list[tuple[object, Path]] = [] + closed: list[object] = [] + + def environment_factory(args: object, program_path: str | Path) -> _FakeEnv: + factory_calls.append((args, Path(program_path))) + return env + + artifacts = run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3, 5)), + launcher_args=launcher_args, + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + episode_executor=_EpisodeExecutor([_result((False,)), _result((True,))]), + clock=_clock([0.0, 0.1, 1.0, 1.2]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 4), + environment_factory=environment_factory, + environment_closer=closed.append, + ) + + assert factory_calls == [(launcher_args, tmp_path / "program.yaml")] + assert closed == [env] + assert env.reset_calls == [ + {"seed": 3, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + {"seed": 5, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + ] + assert [trial.seed for trial in artifacts.trials] == [3, 5] + assert artifacts.raw_json_path.is_file() + assert artifacts.report_path.is_file() + + +def test_gym_runner_closes_environment_when_seed_reset_fails(tmp_path: Path) -> None: + class _ResetFailureEnv(_FakeEnv): + def reset(self, **kwargs: object) -> None: + super().reset(**kwargs) + if "seed" in kwargs: + raise RuntimeError("synthetic reset failure") + + env = _ResetFailureEnv() + closed: list[object] = [] + + with pytest.raises(RuntimeError, match="synthetic reset failure"): + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=lambda args, path: env, + environment_closer=closed.append, + ) + + assert closed == [env] + + +def test_gym_runner_flushes_cleanup_without_closing_when_factory_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + factory_error = LookupError("synthetic factory failure") + cleanup_calls: list[str] = [] + close_calls: list[object] = [] + + def fail_factory( + launcher_args: argparse.Namespace, + expert_program_path: str | Path, + ) -> _FakeEnv: + raise factory_error + + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: cleanup_calls.append("flush_cleanup_queue"), + ) + + with pytest.raises(LookupError, match="synthetic factory failure") as error: + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=fail_factory, + environment_closer=close_calls.append, + ) + + assert error.value is factory_error + assert cleanup_calls == ["flush_cleanup_queue"] + assert close_calls == [] + + +def test_gym_runner_preserves_factory_error_when_cleanup_flush_also_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + factory_error = LookupError("synthetic factory failure") + cleanup_calls: list[str] = [] + close_calls: list[object] = [] + + def fail_factory( + launcher_args: argparse.Namespace, + expert_program_path: str | Path, + ) -> _FakeEnv: + raise factory_error + + def fail_cleanup() -> None: + cleanup_calls.append("flush_cleanup_queue") + raise RuntimeError("synthetic cleanup failure") + + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + fail_cleanup, + ) + + with pytest.raises(LookupError, match="synthetic factory failure") as error: + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=fail_factory, + environment_closer=close_calls.append, + ) + + assert error.value is factory_error + assert error.value.__notes__ == [ + "Benchmark environment construction cleanup also failed: " + "RuntimeError: synthetic cleanup failure" + ] + assert cleanup_calls == ["flush_cleanup_queue"] + assert close_calls == [] + + +def test_default_gym_environment_closer_uses_unwrapped_target_and_flushes_cleanup( + monkeypatch: pytest.MonkeyPatch, +) -> None: + calls: list[object] = [] + + class _UnwrappedEnv: + def close(self, *, exit_process: bool) -> None: + calls.append(("close", exit_process)) + + env = argparse.Namespace(unwrapped=_UnwrappedEnv()) + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: calls.append("flush_cleanup_queue"), + ) + + demo_success_module._close_gym_demo_success_environment(env) + + assert calls == [("close", False), "flush_cleanup_queue"] + + +def test_gym_runner_preserves_body_error_when_default_close_also_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + close_calls: list[bool] = [] + cleanup_calls: list[str] = [] + + class _CloseFailureTarget: + def close(self, *, exit_process: bool) -> None: + close_calls.append(exit_process) + raise RuntimeError("synthetic close failure") + + class _BodyFailureEnv(_FakeEnv): + def __init__(self) -> None: + super().__init__() + self.unwrapped = _CloseFailureTarget() + + def reset(self, **kwargs: object) -> None: + super().reset(**kwargs) + if "seed" in kwargs: + raise LookupError("synthetic benchmark body failure") + + env = _BodyFailureEnv() + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: cleanup_calls.append("flush_cleanup_queue"), + ) + + with pytest.raises(LookupError, match="synthetic benchmark body failure") as error: + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=lambda args, path: env, + ) + + assert error.value.__notes__ == [ + "Benchmark environment cleanup also failed: " + "RuntimeError: synthetic close failure" + ] + assert close_calls == [False] + assert cleanup_calls == ["flush_cleanup_queue"] + + +def test_gym_environment_builder_uses_standard_public_config_pipeline( + monkeypatch: pytest.MonkeyPatch, +) -> None: + calls: list[object] = [] + launcher_args = argparse.Namespace( + gym_config="gym.json", + action_config=None, + ) + env_cfg = argparse.Namespace(expert_program=None) + program = object() + env = object() + + monkeypatch.setattr( + demo_success_module, + "discover_task_packages", + lambda: calls.append("discover"), + ) + monkeypatch.setattr( + demo_success_module, + "execute_init_hooks", + lambda: calls.append("hooks"), + ) + + def build(args: argparse.Namespace): + calls.append(("build", args)) + return env_cfg, {"id": "ExpertTask-v1"}, {} + + monkeypatch.setattr(demo_success_module, "build_env_cfg_from_args", build) + monkeypatch.setattr( + demo_success_module, + "load_expert_program", + lambda path: calls.append(("load", path)) or program, + ) + monkeypatch.setattr( + demo_success_module.gymnasium, + "make", + lambda **kwargs: calls.append(("make", kwargs)) or env, + ) + + created = demo_success_module._create_gym_demo_success_environment( + launcher_args, + "program.yaml", + ) + + assert created is env + assert env_cfg.expert_program is program + assert calls == [ + "discover", + "hooks", + ("build", launcher_args), + ("load", "program.yaml"), + ("make", {"id": "ExpertTask-v1", "cfg": env_cfg}), + ] + + +@pytest.mark.parametrize( + "unsupported", + ( + ("--preview",), + ("--action_config", "actions.json"), + ), +) +def test_cli_live_mode_rejects_unsupported_launcher_options( + tmp_path: Path, + unsupported: tuple[str, ...], +) -> None: + with pytest.raises(SystemExit) as error: + main( + [ + "--run-simulation", + "--gym_config", + "gym.json", + "--expert-program", + "program.yaml", + "--case-id", + "cube", + "--seeds", + "7", + "--raw-json", + str(tmp_path / "raw.json"), + *unsupported, + ] + ) + + assert error.value.code == 2 + + +def test_cli_live_mode_dispatches_fixed_seed_case( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + captured: dict[str, object] = {} + + def run(case: DemoSuccessCase, **kwargs: object) -> object: + captured["case"] = case + captured.update(kwargs) + return object() + + monkeypatch.setattr( + demo_success_module, + "run_gym_demo_success_benchmark", + run, + ) + raw_path = tmp_path / "raw.json" + + exit_code = main( + [ + "--run-simulation", + "--gym_config", + "gym.json", + "--expert-program", + "program.yaml", + "--case-id", + "cube", + "--seeds", + "7", + "11", + "--raw-json", + str(raw_path), + ] + ) + + assert exit_code == 0 + assert captured["case"] == DemoSuccessCase("cube", (7, 11)) + assert captured["expert_program_path"] == Path("program.yaml") + assert captured["raw_json_path"] == raw_path + assert captured["report_path"] == raw_path.with_suffix(".md") + launcher_args = captured["launcher_args"] + assert isinstance(launcher_args, argparse.Namespace) + assert launcher_args.gym_config == "gym.json" + assert launcher_args.num_envs is None + assert launcher_args.renderer is None + + +def test_run_all_benchmarks_prints_report_path( + tmp_path: Path, capsys: pytest.CaptureFixture[str] +) -> None: + env = _FakeEnv() + report_path = tmp_path / "report.md" + + artifacts = run_all_benchmarks( + [DemoSuccessCase("case-a", (1,))], + lambda requested: env, + raw_json_path=tmp_path / "raw.json", + report_path=report_path, + episode_executor=_EpisodeExecutor([_result((True,))]), + clock=_clock([0.0, 0.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 2), + ) + + assert artifacts.report_path == report_path + assert f"Markdown report saved: {report_path}" in capsys.readouterr().out diff --git a/tests/benchmark/expert_program/test_demo_success_open_drawer_sim.py b/tests/benchmark/expert_program/test_demo_success_open_drawer_sim.py new file mode 100644 index 000000000..c3af1a94c --- /dev/null +++ b/tests/benchmark/expert_program/test_demo_success_open_drawer_sim.py @@ -0,0 +1,166 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Live OpenDrawer regression coverage for the Expert Program benchmark.""" + +from __future__ import annotations + +import json +from pathlib import Path +import subprocess +import sys + +import pytest + +from embodichain_tasks.configs import get_config_path +from scripts.benchmark.expert_program.demo_success import ( + aggregate_demo_success_trials, + load_raw_trials, +) + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +_OPEN_DRAWER_GYM_CONFIG = get_config_path("gym/open_drawer/cobot_magic_3cam.json") +_OPEN_DRAWER_EXPERT_PROGRAM = get_config_path( + "expert_program/tableware/open_drawer.json" +) +_CASE_ID = "open_drawer_live" +_SEED = 0 +_NUM_ENVS = 1 +_SUBPROCESS_TIMEOUT_SECONDS = 180 +_RUN_PUBLIC_MAIN = ( + "from scripts.benchmark.expert_program.demo_success import main; " + "raise SystemExit(main())" +) + + +def _write_headless_cpu_gym_config(tmp_path: Path) -> Path: + """Write a camera-free copy of the packaged live-physics configuration.""" + payload = json.loads(_OPEN_DRAWER_GYM_CONFIG.read_text(encoding="utf-8")) + if type(payload) is not dict: + raise TypeError("The packaged OpenDrawer Gym config must be a JSON object.") + env_config = payload.get("env") + if type(env_config) is not dict: + raise TypeError("The packaged OpenDrawer env config must be a JSON object.") + + # Cameras and their recording event are orthogonal to drawer physics and make + # this CPU regression unnecessarily renderer-sensitive. + payload["sensor"] = [] + env_config["events"] = {} + env_config["observations"] = {} + env_config["dataset"] = {} + payload["expert_program_path"] = str(_OPEN_DRAWER_EXPERT_PROGRAM) + + output = tmp_path / "open_drawer_headless_cpu.json" + output.write_text( + json.dumps(payload, indent=2, ensure_ascii=False) + "\n", + encoding="utf-8", + ) + return output + + +@pytest.mark.requires_sim +@pytest.mark.slow +def test_live_open_drawer_benchmark_writes_successful_decodable_artifacts( + tmp_path: Path, +) -> None: + """Run one no-retry seed through the public live benchmark entry point.""" + gym_config_path = _write_headless_cpu_gym_config(tmp_path) + raw_path = tmp_path / "open_drawer_raw.json" + report_path = tmp_path / "open_drawer_report.md" + completed = subprocess.run( + [ + sys.executable, + "-c", + _RUN_PUBLIC_MAIN, + "--run-simulation", + "--gym_config", + str(gym_config_path), + "--expert-program", + str(_OPEN_DRAWER_EXPERT_PROGRAM), + "--case-id", + _CASE_ID, + "--seeds", + str(_SEED), + "--raw-json", + str(raw_path), + "--report", + str(report_path), + "--headless", + "--device", + "cpu", + "--num_envs", + str(_NUM_ENVS), + "--filter_dataset_saving", + ], + cwd=_REPOSITORY_ROOT, + capture_output=True, + text=True, + timeout=_SUBPROCESS_TIMEOUT_SECONDS, + check=False, + ) + + # main() returns zero only after the live runner's default closer completes; + # the process boundary also isolates native simulator teardown from pytest. + assert completed.returncode == 0, completed.stdout + completed.stderr + assert f"Raw JSON saved: {raw_path}" in completed.stdout + assert f"Markdown report saved: {report_path}" in completed.stdout + + decoded_trials = load_raw_trials(raw_path) + assert len(decoded_trials) == 1 + trial = decoded_trials[0] + assert trial.case_id == _CASE_ID + assert trial.seed == _SEED + assert len(trial.rows) == _NUM_ENVS + row = trial.rows[0] + assert row.success + assert row.terminal_reason == "success" + assert row.length > 0 + + segments = trial.episode_result["segments"] + assert isinstance(segments, list) + assert len(segments) == 1 + segment = segments[0] + assert isinstance(segment, dict) + assert segment["name"] == "open_drawer" + runtime = segment["metadata"]["runtime"] + assert runtime["kind"] == "skill_result" + assert runtime["status"] == "completed" + calls = runtime["calls"] + assert isinstance(calls, list) + assert len(calls) == 1 + call = calls[0] + assert call["semantic_id"] == "operate_articulation" + assert call["status"] == "completed" + effects = call["effects"] + assert isinstance(effects, list) + assert effects + for effect in effects: + evidence = effect["evidence"]["joint.position"] + assert evidence["valid_mask"] == [True] + assert evidence["acquisition_errors"] == [None] + + aggregates = aggregate_demo_success_trials(decoded_trials) + assert len(aggregates.success_and_metrics) == 1 + metrics = aggregates.success_and_metrics[0] + assert metrics["attempted"] == 1 + assert metrics["successes"] == 1 + assert metrics["success_rate"] == 1.0 + + report = report_path.read_text(encoding="utf-8") + assert report.count("\n## ") == 3 + assert "## Success & Other Metrics" in report + assert "## Leaderboard" in report + assert _CASE_ID in report diff --git a/tests/gym/envs/expert_program/test_task_vertical_slices.py b/tests/gym/envs/expert_program/test_task_vertical_slices.py index af67f89e3..4766acf7a 100644 --- a/tests/gym/envs/expert_program/test_task_vertical_slices.py +++ b/tests/gym/envs/expert_program/test_task_vertical_slices.py @@ -47,7 +47,11 @@ SceneObjectRef, SceneRegistry, ) +from embodichain.lab.gym.utils.registration import discover_task_packages from embodichain_tasks.configs import get_config_path + +discover_task_packages() + from embodichain_tasks.multi_segments import cube_pick_place as cube_task from embodichain_tasks.tableware import open_drawer as drawer_task diff --git a/tests/scripts/tools/test_expert_program_rollout_report.py b/tests/scripts/tools/test_expert_program_rollout_report.py new file mode 100644 index 000000000..260f15a81 --- /dev/null +++ b/tests/scripts/tools/test_expert_program_rollout_report.py @@ -0,0 +1,94 @@ +# ---------------------------------------------------------------------------- +# 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 pytest + +from scripts.tools.expert_program_rollout_report import ( + DEFAULT_REPORT_PATH, + REPOSITORY_ROOT, + build_task_size_metrics, + main, + render_report, +) + +EXPECTED_CURRENT_COUNTS = { + # Each tuple is (raw LF bytes, raw file bytes) for the explicit task pair. + "Cube": (366, 12_448), + "Drawer": (246, 8_391), +} + +EXPECTED_SOURCE_PATHS = { + "Cube": ( + "embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py", + "embodichain_tasks/configs/expert_program/multi_segments/" + "repeated_cube_pick_place.yaml", + ), + "Drawer": ( + "embodichain_tasks/embodichain_tasks/tableware/open_drawer.py", + "embodichain_tasks/configs/expert_program/tableware/open_drawer.json", + ), +} + + +def test_current_counts_use_only_the_four_declared_sources() -> None: + metrics = build_task_size_metrics(REPOSITORY_ROOT) + + actual = { + metric.task: ( + metric.current_lines, + metric.current_bytes, + tuple(source.path for source in metric.sources), + ) + for metric in metrics + } + expected = { + task: (*EXPECTED_CURRENT_COUNTS[task], EXPECTED_SOURCE_PATHS[task]) + for task in EXPECTED_CURRENT_COUNTS + } + assert actual == expected + + +def test_render_is_deterministic() -> None: + metrics = build_task_size_metrics(REPOSITORY_ROOT) + + first = render_report(metrics) + second = render_report(metrics) + + assert first == second + + +def test_render_rejects_empty_metric_snapshot() -> None: + with pytest.raises(ValueError, match="at least one task snapshot"): + render_report(()) + + +def test_checked_in_report_matches_deterministic_render() -> None: + expected = render_report(build_task_size_metrics(REPOSITORY_ROOT)) + + assert DEFAULT_REPORT_PATH.read_text(encoding="utf-8") == expected + + +def test_check_mode_accepts_current_report() -> None: + assert main(["--check"]) == 0 + + +def test_check_mode_rejects_stale_report(tmp_path) -> None: + stale_report = tmp_path / "expert_program_rollout_report.md" + stale_report.write_text("stale\n", encoding="utf-8") + + assert main(["--check", "--output", str(stale_report)]) == 1 diff --git a/tests/test_expert_program_package_data.py b/tests/test_expert_program_package_data.py index 4d695881f..90c747645 100644 --- a/tests/test_expert_program_package_data.py +++ b/tests/test_expert_program_package_data.py @@ -175,9 +175,11 @@ def test_staged_programs_decode_through_installed_config_paths( assert program.program_id == expected_program_id decoded[relative_path] = program.program_id print(json.dumps(decoded, sort_keys=True)) -""" + """ environment = os.environ.copy() - environment["PYTHONPATH"] = str(staged_config_package.build_lib) + environment["PYTHONPATH"] = os.pathsep.join( + (str(staged_config_package.build_lib), str(_REPOSITORY_ROOT)) + ) completed = subprocess.run( [ sys.executable, @@ -193,4 +195,4 @@ def test_staged_programs_decode_through_installed_config_paths( text=True, ) - assert json.loads(completed.stdout) == expected_ids + assert json.loads(completed.stdout.splitlines()[-1]) == expected_ids From 57e47f3e997c8409c34a73161872abb8cba79243 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 16:37:00 +0800 Subject: [PATCH 13/29] refactor(atomic-actions): add typed tracking contracts --- .../embodichain.lab.sim.skills.rst | 1 + docs/source/api_reference/public_api.rst | 89 ++ .../lab/sim/atomic_actions/__init__.py | 84 +- .../lab/sim/atomic_actions/bindings.py | 69 + embodichain/lab/sim/atomic_actions/core.py | 86 +- embodichain/lab/sim/atomic_actions/engine.py | 16 + .../lab/sim/atomic_actions/execution.py | 438 +++--- .../lab/sim/atomic_actions/invocation.py | 13 + embodichain/lab/sim/atomic_actions/plans.py | 194 ++- .../lab/sim/atomic_actions/policies.py | 4 - embodichain/lab/sim/atomic_actions/runtime.py | 40 +- embodichain/lab/sim/atomic_actions/state.py | 32 + .../lab/sim/atomic_actions/tracking.py | 1210 +++++++++++++++++ embodichain/lab/sim/skills/__init__.py | 2 + embodichain/lab/sim/skills/compiler.py | 1 + embodichain/lab/sim/skills/integration.py | 1 + embodichain/lab/sim/skills/profiles.py | 99 +- embodichain/lab/sim/skills/runtime.py | 241 +++- .../test_completion_metadata.py | 7 +- tests/sim/atomic_actions/test_core.py | 222 +-- .../test_endpoint_runtime_e2e.py | 2 + .../sim/atomic_actions/test_engine_per_env.py | 102 +- tests/sim/atomic_actions/test_runner.py | 200 ++- tests/sim/atomic_actions/test_tracking.py | 263 ++++ tests/sim/skills/test_compiler.py | 16 + ...o_semantic_runtime_dynamic_recovery_gpu.py | 6 +- tests/sim/skills/test_profiles.py | 28 + tests/sim/skills/test_runtime.py | 59 +- 28 files changed, 3111 insertions(+), 414 deletions(-) create mode 100644 embodichain/lab/sim/atomic_actions/tracking.py create mode 100644 tests/sim/atomic_actions/test_tracking.py diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst index 6bdb46b30..dfd1cc2dc 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -184,6 +184,7 @@ embodichain.lab.sim.skills SemanticValidationError SemanticWorkflow SkillEndpointBindingTrace + SkillEndpointTrackingChannelTrace SkillFailure SkillRuntimeProvider SkillScene diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 3ed8179fa..4dc60ae8e 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -491,16 +491,26 @@ embodichain.lab.sim.atomic_actions ArticulationOperationAffordance ArticulationOperationTarget AssembleAffordance + BASE_POSE_CHANNEL BUILTIN_ACTION_TYPES CoordinatedPickmentOptions CoordinatedPlacementOptions DynamicCollisionMode + EndpointTrackingChannelBinding + EndpointTrackingFeedbackAddress EntityState EffectVerificationRequirement EffectVerificationResult EffectVerifier ExecutionPlanAttempt + FeedbackTerminalAcceptance GRASP_COMMAND + InFlightTrackingPolicy + JOINT_POSITION_CHANNEL + JointPositionTrackingEvaluator + JointPositionTrackingMetric + JointPositionTrackingProjector + JointPositionTrackingState HandOverOptions InteractionPoints MoveEndEffectorOptions @@ -514,6 +524,10 @@ embodichain.lab.sim.atomic_actions OperateArticulationOptions PickUpOptions PlaceOptions + PlanningContextTrackingFeedbackProvider + PoseTrackingEvaluator + PoseTrackingMetric + PoseTrackingState PoseGoalValue RigidObjectSceneProvider RigidObjectSceneProviderCfg @@ -521,6 +535,33 @@ embodichain.lab.sim.atomic_actions SceneProvider SceneArticulationOperationGeometry SceneSnapshotSupplier + TimedTerminalAcceptance + TimedTrackingSequence + TerminalAcceptance + TrackingCommandProjector + TrackingEvaluation + TrackingEvaluatorRegistry + TrackingFeedbackAddress + TrackingFeedbackBatch + TrackingFeedbackProvider + TrackingFeedbackProviderRegistry + TrackingFeedbackSourceRef + TrackingFrame + TrackingMetricCfg + TrackingMetricEvaluator + TrackingPolicy + TrackingProjectorRef + TrackingProjectorRegistry + TrackingRuntime + TrackingSetpoint + TrackingState + WHOLE_BODY_POSE_CHANNEL + WholeBodyPoseTrackingEvaluator + WholeBodyPoseTrackingMetric + WholeBodyPoseTrackingState + get_registered_actions + register_action + unregister_action embodichain.lab.sim.atomic_actions.affordance --------------------------------------------- @@ -792,6 +833,53 @@ embodichain.lab.sim.atomic_actions.state SceneSnapshot TaskState +embodichain.lab.sim.atomic_actions.tracking +------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.tracking + +.. autosummary:: + + BASE_POSE_CHANNEL + FeedbackTerminalAcceptance + InFlightTrackingPolicy + JOINT_POSITION_CHANNEL + JointPositionTrackingEvaluator + JointPositionTrackingMetric + JointPositionTrackingProjector + JointPositionTrackingState + EndpointTrackingChannelBinding + EndpointTrackingFeedbackAddress + PlanningContextTrackingFeedbackProvider + PoseTrackingEvaluator + PoseTrackingMetric + PoseTrackingState + TerminalAcceptance + TimedTerminalAcceptance + TimedTrackingSequence + TrackingChannelId + TrackingCommandProjector + TrackingEvaluation + TrackingEvaluatorRegistry + TrackingFeedbackAddress + TrackingFeedbackBatch + TrackingFeedbackProvider + TrackingFeedbackProviderRegistry + TrackingFeedbackSourceRef + TrackingFrame + TrackingMetricCfg + TrackingMetricEvaluator + TrackingPolicy + TrackingProjectorRef + TrackingProjectorRegistry + TrackingRuntime + TrackingSetpoint + TrackingState + WHOLE_BODY_POSE_CHANNEL + WholeBodyPoseTrackingEvaluator + WholeBodyPoseTrackingMetric + WholeBodyPoseTrackingState + embodichain.lab.sim.atomic_actions.trajectory_ops ------------------------------------------------- @@ -1310,6 +1398,7 @@ embodichain.lab.sim.skills.runtime ResolvedCorePolicyTrace SkillCallTrace SkillEndpointBindingTrace + SkillEndpointTrackingChannelTrace SkillEffectTrace SkillFailure SkillPlanAttemptTrace diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index c2d38d545..790f44972 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -78,7 +78,6 @@ ActionPlan, CompiledTrajectory, EffectVerificationRequirement, - ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, @@ -106,6 +105,46 @@ TimedCommandSequence, ) from .transports import EndpointCommandRouter, EndpointCommandTransport +from .tracking import ( + BASE_POSE_CHANNEL, + JOINT_POSITION_CHANNEL, + WHOLE_BODY_POSE_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + FeedbackTerminalAcceptance, + InFlightTrackingPolicy, + JointPositionTrackingEvaluator, + JointPositionTrackingMetric, + JointPositionTrackingProjector, + JointPositionTrackingState, + PlanningContextTrackingFeedbackProvider, + PoseTrackingEvaluator, + PoseTrackingMetric, + PoseTrackingState, + TerminalAcceptance, + TimedTerminalAcceptance, + TimedTrackingSequence, + TrackingCommandProjector, + TrackingEvaluation, + TrackingEvaluatorRegistry, + TrackingFeedbackAddress, + TrackingFeedbackBatch, + TrackingFeedbackProvider, + TrackingFeedbackProviderRegistry, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingMetricCfg, + TrackingMetricEvaluator, + TrackingPolicy, + TrackingProjectorRef, + TrackingProjectorRegistry, + TrackingRuntime, + TrackingSetpoint, + TrackingState, + WholeBodyPoseTrackingEvaluator, + WholeBodyPoseTrackingMetric, + WholeBodyPoseTrackingState, +) from .primitives import ( AssembleGoal, BUILTIN_ACTION_TYPES, @@ -227,7 +266,6 @@ "EffectVerificationResult", "EffectVerifier", "ExecutionClock", - "ExecutionFeedbackMode", "ExecutionEvent", "ExecutionEventKind", "ExecutionPlanAttempt", @@ -236,6 +274,9 @@ "ExecutionSession", "ExecutionStatus", "ExecutionTick", + "EndpointTrackingChannelBinding", + "EndpointTrackingFeedbackAddress", + "FeedbackTerminalAcceptance", "GRASP_COMMAND", "GRASP_CAPABILITY", "GraspGoal", @@ -245,12 +286,16 @@ "HeldObjectState", "FORWARD_KINEMATICS_CAPABILITY", "INVERSE_KINEMATICS_CAPABILITY", + "InFlightTrackingPolicy", "InteractionPoints", "JointPositionGoal", "JointPositionCommand", "JointPositionPayload", "JointPositionTarget", "JOINT_POSITION_CAPABILITY", + "JOINT_POSITION_CHANNEL", + "JointPositionTrackingMetric", + "JointPositionTrackingState", "MotionPolicy", "MonotonicExecutionClock", "MoveEndEffector", @@ -275,6 +320,8 @@ "PlannerDiagnostics", "PlanningContext", "PoseGoalValue", + "PoseTrackingMetric", + "PoseTrackingState", "Press", "PressAffordance", "PressGoal", @@ -310,7 +357,38 @@ "SimulationExecutionAdapter", "TaskState", "TimedCommandSequence", + "TimedTerminalAcceptance", + "TimedTrackingSequence", "TimedTrajectory", - "TwistAffordance", + "TerminalAcceptance", "TrajectorySegment", + "TrackingCommandProjector", + "TrackingEvaluation", + "TrackingEvaluatorRegistry", + "TrackingFeedbackAddress", + "TrackingFeedbackBatch", + "TrackingFeedbackProvider", + "TrackingFeedbackProviderRegistry", + "TrackingFeedbackSourceRef", + "TrackingFrame", + "TrackingMetricCfg", + "TrackingMetricEvaluator", + "TrackingPolicy", + "TrackingProjectorRef", + "TrackingProjectorRegistry", + "TrackingRuntime", + "TrackingSetpoint", + "TrackingState", + "BASE_POSE_CHANNEL", + "JointPositionTrackingEvaluator", + "JointPositionTrackingProjector", + "PlanningContextTrackingFeedbackProvider", + "PoseTrackingEvaluator", + "WHOLE_BODY_POSE_CHANNEL", + "WholeBodyPoseTrackingEvaluator", + "WholeBodyPoseTrackingMetric", + "WholeBodyPoseTrackingState", + "get_registered_actions", + "register_action", + "unregister_action", ] diff --git a/embodichain/lab/sim/atomic_actions/bindings.py b/embodichain/lab/sim/atomic_actions/bindings.py index a43bbaf71..9717304d4 100644 --- a/embodichain/lab/sim/atomic_actions/bindings.py +++ b/embodichain/lab/sim/atomic_actions/bindings.py @@ -28,6 +28,10 @@ import torch from .control import ControlCommand +from .tracking import ( + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, +) def _validate_identifier(value: str, *, field_name: str) -> str: @@ -79,6 +83,49 @@ def _snapshot_commands( return MappingProxyType(commands) +def _snapshot_tracking_channels( + values: Mapping[str, EndpointTrackingChannelBinding], + *, + target: RuntimeEndpointTarget, +) -> Mapping[str, EndpointTrackingChannelBinding]: + """Validate and own endpoint-local tracking-channel bindings.""" + if not isinstance(values, Mapping): + raise TypeError("EndpointBinding.tracking_channels must be a mapping.") + channels: dict[str, EndpointTrackingChannelBinding] = {} + for channel_id, binding in values.items(): + _validate_identifier( + channel_id, + field_name="EndpointBinding tracking channel IDs", + ) + if not isinstance(binding, EndpointTrackingChannelBinding): + raise TypeError( + "EndpointBinding.tracking_channels values must be " + "EndpointTrackingChannelBinding instances." + ) + if binding.channel_id != channel_id: + raise ValueError( + f"Tracking channel key {channel_id!r} disagrees with its binding " + f"channel {binding.channel_id!r}." + ) + snapshot = binding.snapshot() + if snapshot is binding: + raise TypeError( + "EndpointTrackingChannelBinding.snapshot() must return an " + "independently owned value." + ) + address = snapshot.source.address + if ( + isinstance(address, EndpointTrackingFeedbackAddress) + and address.target.address_fingerprint != target.address_fingerprint + ): + raise ValueError( + f"Tracking channel {channel_id!r} addresses a different runtime " + "endpoint target." + ) + channels[channel_id] = snapshot + return MappingProxyType(channels) + + def _validate_target_fingerprint( target: RuntimeEndpointTarget, *, @@ -192,6 +239,9 @@ class EndpointBinding: task_state_key: str | None = None """Symbolic task-state key; direct-core defaults to ``target.target_id``.""" + tracking_channels: Mapping[str, EndpointTrackingChannelBinding] = field( + default_factory=dict + ) capabilities: frozenset[str] = frozenset() commands: Mapping[str, ControlCommand] = field(default_factory=dict) claim_tokens: frozenset[str] = frozenset() @@ -246,6 +296,11 @@ def __post_init__(self) -> None: field_name="EndpointBinding.task_state_key", ) object.__setattr__(self, "task_state_key", task_state_key) + object.__setattr__( + self, + "tracking_channels", + _snapshot_tracking_channels(self.tracking_channels, target=target), + ) object.__setattr__( self, "capabilities", @@ -317,6 +372,18 @@ def command(self, name: str) -> ControlCommand: ) from exc return command.snapshot() + def tracking_channel(self, channel_id: str) -> EndpointTrackingChannelBinding: + """Return one independently owned typed tracking-channel binding.""" + try: + binding = self.tracking_channels[channel_id] + except KeyError as exc: + raise KeyError( + f"Endpoint {self.slot_id}.{self.endpoint_id} has no tracking " + f"channel {channel_id!r}; available channels are " + f"{sorted(self.tracking_channels)}." + ) from exc + return binding.snapshot() + def joint_positions( self, name: str, @@ -356,6 +423,7 @@ def with_commands( adapter_id=self.adapter_id, target=self.target, task_state_key=self.task_state_key, + tracking_channels=self.tracking_channels, capabilities=self.capabilities, commands=merged, claim_tokens=self.claim_tokens, @@ -371,6 +439,7 @@ def snapshot(self) -> EndpointBinding: adapter_id=self.adapter_id, target=self.target, task_state_key=self.task_state_key, + tracking_channels=self.tracking_channels, capabilities=self.capabilities, commands=self.commands, claim_tokens=self.claim_tokens, diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index fe099461c..1c39615f1 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -43,7 +43,6 @@ from .plans import ( ActionPlan, EffectVerificationRequirement, - ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, @@ -57,6 +56,12 @@ RuntimeCommandFrame, TimedCommandSequence, ) +from .tracking import ( + FeedbackTerminalAcceptance, + TimedTrackingSequence, + TrackingFrame, + TrackingSetpoint, +) if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -374,6 +379,7 @@ def resolve_request( invocation.control_overrides, ), motion_policy=invocation.motion_policy, + tracking_policy=invocation.tracking_policy, recovery_policy=invocation.recovery_policy, skill_options=options, invocation_id=invocation.invocation_id, @@ -573,7 +579,6 @@ def build_plan( diagnostics=diagnostics, segment_lengths=segment_lengths, scene_dependency_monitor_until=scene_dependency_monitor_until, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, joint_trajectory=timed, ) @@ -590,14 +595,13 @@ def build_command_plan( diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, scene_dependency_monitor_until: Mapping[str, int] | None = None, - feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, joint_trajectory: TimedTrajectory | None = None, ) -> ActionPlan: """Build a plan from transport-neutral runtime command frames. - Non-joint command sequences use timed completion unless a future - endpoint-specific feedback evaluator is installed. Semantic effects - remain externally verified through the execution session. + Tracking targets are projected from the command payloads through the + typed channels declared by each bound endpoint. Semantic effects remain + externally verified through the execution session. Args: request: Resolved invocation snapshot being planned. @@ -616,9 +620,8 @@ def build_command_plan( its bound. ``0`` disables monitoring immediately; omitted dependencies remain monitored for the full action. Once the bound is reached, all pose changes for that entity are ignored. - feedback_mode: Feedback contract used to determine target completion. - joint_trajectory: Optional joint trajectory retained for joint-position - feedback and inspection. + joint_trajectory: Optional joint trajectory retained for offline + compilation and inspection. Returns: Side-effect-free action plan. @@ -642,6 +645,7 @@ def build_command_plan( commands, active_mask=success_mask, ) + tracking = self._tracking_sequence(request, commands) segments = self._build_segments( segment_lengths, frame_count=commands.frame_count, @@ -655,12 +659,13 @@ def build_command_plan( plan_success=success_mask, commands=commands, recovery_policy=request.recovery_policy, + tracking_policy=request.tracking_policy, planned_scene_version=context.scene.version, planned_collision_world_revision=( context.scene.collision_world_revisions(context.batch_size) ), diagnostics=diagnostics, - feedback_mode=feedback_mode, + tracking=tracking, joint_trajectory=joint_trajectory, segments=segments, scene_dependencies=self._scene_dependencies(request), @@ -677,6 +682,67 @@ def build_command_plan( invocation_revision=request.revision, ) + def _tracking_sequence( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + commands: TimedCommandSequence, + ) -> TimedTrackingSequence | None: + """Project command payloads through binding-owned tracking channels.""" + policy = request.tracking_policy + metrics = list(() if policy.in_flight is None else policy.in_flight.metrics) + if isinstance(policy.terminal, FeedbackTerminalAcceptance): + metrics.extend(policy.terminal.metrics) + if not metrics: + return None + + runtime = self.planning_services.tracking_runtime + for metric in metrics: + runtime.evaluators.resolve(metric) + metrics_by_channel = {metric.channel_id: metric for metric in metrics} + + endpoints_by_destination: dict[ + tuple[str, str], + tuple[EndpointBinding, ...], + ] = {} + for endpoint in request.binding.endpoints: + endpoints_by_destination.setdefault(endpoint.destination_key, ()) + endpoints_by_destination[endpoint.destination_key] += (endpoint,) + + tracking_frames: list[TrackingFrame] = [] + for frame_index, frame in enumerate(commands.frames): + setpoints: list[TrackingSetpoint] = [] + for command in frame.commands: + endpoints = endpoints_by_destination[command.destination_key] + for endpoint in endpoints: + for channel_id in metrics_by_channel: + channel = endpoint.tracking_channels.get(channel_id) + if channel is None: + continue + runtime.providers.resolve(channel.source) + runtime.projectors.resolve(channel.projector) + setpoints.append( + TrackingSetpoint( + endpoint_key=endpoint.key, + binding=channel, + desired=runtime.project(command, channel), + ) + ) + covered_channels = {setpoint.binding.channel_id for setpoint in setpoints} + missing_channels = sorted( + set(metrics_by_channel).difference(covered_channels) + ) + if missing_channels: + raise ValueError( + f"Command frame {frame_index} cannot project configured " + f"tracking channels {missing_channels}; bound endpoints must " + "declare a typed feedback source and projector." + ) + tracking_frames.append(TrackingFrame(tuple(setpoints))) + return TimedTrackingSequence( + env_ids=commands.env_ids, + frames=tuple(tracking_frames), + ) + @staticmethod def _authorize_command_targets( request: ResolvedActionRequest[GoalT, OptionsT], diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index c4214f382..3e3ccc728 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -31,6 +31,7 @@ from .policies import MotionPolicy, RecoveryPolicy from .runtime import ActionPlanningServices from .state import PlanningContext, RobotObservation, SceneSnapshot, TaskState +from .tracking import TrackingRuntime if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -58,6 +59,7 @@ def __init__( endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None ) = None, + tracking_runtime: TrackingRuntime | None = None, ) -> None: """Initialize one engine and bind its built-in action implementations. @@ -72,6 +74,9 @@ def __init__( ``skill_profile`` are mutually exclusive. endpoint_adapters: Optional exact-type endpoint adapters used when binding ``skill_profile``. Invalid without a profile. + tracking_runtime: Optional exact-version feedback, projector, and + metric registries. Built-in joint tracking is installed when + omitted. """ if endpoint_adapters is not None and skill_profile is None: raise ValueError("endpoint_adapters requires skill_profile.") @@ -89,6 +94,7 @@ def __init__( self._planning_services = ActionPlanningServices( motion_generator, control_profiles=control_profiles, + tracking_runtime=tracking_runtime, ) self._actions: dict[str, AtomicAction] = {} self._skill_catalog_revision = 0 @@ -121,6 +127,11 @@ def planning_services(self) -> ActionPlanningServices: """Engine-owned resources shared by every bound atomic action.""" return self._planning_services + @property + def tracking_runtime(self) -> TrackingRuntime: + """Typed endpoint-feedback runtime used by plans and sessions.""" + return self._planning_services.tracking_runtime + @property def binding_owner_id(self) -> str: """Return the opaque owner identity required by action bindings.""" @@ -666,6 +677,11 @@ def _validate_plan( raise ValueError( "ActionPlan.invocation_revision must preserve the request revision." ) + if plan.tracking_policy != request.tracking_policy: + raise ValueError( + "ActionPlan.tracking_policy must preserve the resolved request " + "tracking policy." + ) commands = plan.commands if commands.batch_size != context.batch_size: raise ValueError("Action plan batch size does not match the context.") diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 61a9e9e2a..34baa2d5e 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -27,20 +27,22 @@ from .effects import StateDelta from .invocation import ActionInvocation, ResolvedActionRequest -from .bindings import JointPositionTarget, RuntimeEndpointTarget +from .bindings import RuntimeEndpointTarget from .plans import ( ActionPlan, EffectVerificationRequirement, - ExecutionFeedbackMode, TrajectorySegment, ) from .policies import RecoveryPolicy -from .runtime_commands import ( - JointPositionPayload, - RuntimeCommandFrame, - TimedCommandSequence, -) +from .runtime_commands import RuntimeCommandFrame, TimedCommandSequence from .state import EntityState, PlanningContext, SceneSnapshot, TaskState +from .tracking import ( + FeedbackTerminalAcceptance, + TimedTerminalAcceptance, + TrackingEvaluation, + TrackingFrame, + TrackingMetricCfg, +) if TYPE_CHECKING: from .engine import AtomicActionEngine @@ -60,7 +62,10 @@ class ExecutionEventKind(str, Enum): ACTION_PLANNED = "action_planned" INVOCATION_REVISED = "invocation_revised" REPLANNED = "replanned" - TRACKING_ERROR = "tracking_error" + TRACKING_DIVERGED = "tracking_diverged" + TRACKING_FEEDBACK_FAILED = "tracking_feedback_failed" + TERMINAL_ACCEPTANCE_PENDING = "terminal_acceptance_pending" + TERMINAL_ACCEPTANCE_FAILED = "terminal_acceptance_failed" DYNAMIC_GOAL_CHANGED = "dynamic_goal_changed" COLLISION_WORLD_CHANGED = "collision_world_changed" ACTION_PLANNING_FAILED = "action_planning_failed" @@ -441,14 +446,27 @@ def __init__( tuple[str, str], RuntimeEndpointTarget, ] = {} + self._active_tracking_routes: dict[ + tuple[str, str, str], + tuple[object, str, str], + ] = {} self._planned_scene = context.scene self._action_started_at = context.robot.timestamp self._attempt_generation = -1 - self._last_joint_command: torch.Tensor | None = None - self._last_joint_ids: tuple[int, ...] = () + self._last_tracking_frame: TrackingFrame | None = None self._last_command_mask = torch.zeros( context.batch_size, dtype=torch.bool, device=context.robot.qpos.device ) + self._tracking_violation_counts = torch.zeros( + context.batch_size, + dtype=torch.long, + device=context.robot.qpos.device, + ) + self._terminal_acceptance_counts = torch.zeros_like( + self._tracking_violation_counts + ) + self._terminal_started_at: float | None = None + self._terminal_pending_reported = False self._eligible = ( torch.ones_like(self._last_command_mask) if eligible_mask is None @@ -655,6 +673,11 @@ def _install_prepared_revision( replacement_plan, ExecutionEventKind.INVOCATION_REVISED, ) + self._validate_tracking_continuity( + replacement_plan, + ExecutionEventKind.INVOCATION_REVISED, + ) + requests = list(self._requests) requests[self._invocation_index] = replacement self._requests = tuple(requests) @@ -888,14 +911,7 @@ def tick( events.extend(recovery_events) if self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) - if recovery_events and any( - event.kind - in { - ExecutionEventKind.REPLANNED, - ExecutionEventKind.RECOVERY_EXHAUSTED, - } - for event in recovery_events - ): + if recovery_events: assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success @@ -929,59 +945,97 @@ def tick( self._waypoint_index += 1 return self._tick_result(command=command, events=events) - terminal_error = self._terminal_error(plan) - not_reached = execution_mask & ( - terminal_error > plan.recovery_policy.tracking_error_threshold - ) - if not_reached.any(): - max_terminal_error = float(terminal_error[not_reached].amax().item()) - events.extend( - self._attempt_replan( - not_reached, - ExecutionEventKind.TRACKING_ERROR, - "Terminal command has not been reached " - f"(max_error={max_terminal_error:.6f}, " - "threshold=" - f"{plan.recovery_policy.tracking_error_threshold:.6f}).", + terminal = plan.tracking_policy.terminal + if self._terminal_started_at is None: + self._terminal_started_at = self._context.robot.timestamp + elapsed_terminal = self._context.robot.timestamp - self._terminal_started_at + terminal_pending = torch.zeros_like(execution_mask) + if isinstance(terminal, TimedTerminalAcceptance): + if elapsed_terminal < terminal.settle_duration: + terminal_pending = execution_mask.clone() + elif isinstance(terminal, FeedbackTerminalAcceptance): + if plan.tracking is None or not plan.tracking.frames: + raise RuntimeError( + "Feedback terminal acceptance requires a terminal tracking " + "frame." ) - ) - if self._status is not ExecutionStatus.RUNNING: - return self._tick_result(command=None, events=events) - assert self._plan is not None - plan = self._plan - execution_mask = self._pending & self._plan.plan_success - if not self._pending.any(): - command, hold_targets, completion_events = self._finish_action( - self._pending, - None, + try: + accepted, valid, normalized_error = self._evaluate_tracking_frame( + plan.tracking.frames[-1], + terminal.metrics, ) - events.extend(completion_events) - return self._tick_result( - command=command, - hold_targets=hold_targets, - events=events, + except Exception as exc: # noqa: BLE001 - fail required feedback closed + events.extend( + self._fail_tracking_feedback( + execution_mask, + "Terminal tracking feedback evaluation failed: " + f"{type(exc).__name__}: {exc}", + ) ) - if plan.commands.frame_count > 0: - command = self._command_at(plan, 0, execution_mask) - self._waypoint_index = 1 - return self._tick_result(command=command, events=events) - events.append( - self._event( - ExecutionEventKind.TRAJECTORY_COMPLETED, - execution_mask, - "Replanned action has no executable command frame.", + return self._tick_result(command=None, events=events) + invalid = execution_mask & ~valid + if invalid.any(): + events.extend( + self._fail_tracking_feedback( + invalid, + "Required terminal tracking feedback was invalid.", + ) ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + execution_mask = self._pending & plan.plan_success + accepted_now = execution_mask & valid & accepted + self._terminal_acceptance_counts[accepted_now] += 1 + self._terminal_acceptance_counts[execution_mask & ~accepted_now] = 0 + terminal_pending = execution_mask & ( + self._terminal_acceptance_counts < terminal.consecutive_acceptances ) - command, hold_targets, completion_events = self._finish_action( - execution_mask, - effect_result, + if terminal_pending.any() and elapsed_terminal >= terminal.settle_timeout: + max_error = float(normalized_error[terminal_pending].amax().item()) + events.extend( + self._attempt_action_retry( + terminal_pending, + ExecutionEventKind.TERMINAL_ACCEPTANCE_FAILED, + "Terminal feedback did not satisfy the acceptance " + "contract before its settle timeout " + f"(max_normalized_error={max_error:.6f}).", + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + assert self._plan is not None + plan = self._plan + execution_mask = self._pending & plan.plan_success + if plan.commands.frame_count > 0 and execution_mask.any(): + command = self._command_at(plan, 0, execution_mask) + self._waypoint_index = 1 + return self._tick_result(command=command, events=events) + terminal_pending.zero_() + else: # pragma: no cover - TrackingPolicy validates exact alternatives + raise AssertionError( + f"Unsupported terminal acceptance {type(terminal).__name__}." ) - events.extend(completion_events) - return self._tick_result( - command=command, - hold_targets=hold_targets, - events=events, + + if terminal_pending.any(): + if not self._terminal_pending_reported: + events.append( + self._event( + ExecutionEventKind.TERMINAL_ACCEPTANCE_PENDING, + terminal_pending, + "Maintaining the terminal command while acceptance is " + "pending.", + ) + ) + self._terminal_pending_reported = True + if plan.commands.frame_count == 0: + raise RuntimeError( + "Terminal settling requires an executable terminal command " + "frame." + ) + terminal_command = plan.commands.frames[-1].with_active_mask( + plan.commands.frames[-1].active_mask & terminal_pending ) + return self._tick_result(command=terminal_command, events=events) events.append( self._event( @@ -1057,8 +1111,9 @@ def _install_plan( for target in plan.commands.targets } replacement_destinations = frozenset(replacement_targets) - if not destination_continuity_validated: - self._validate_destination_continuity(plan, event_kind) + replacement_tracking_routes = self._tracking_routes(plan) + self._validate_destination_continuity(plan, event_kind) + self._validate_tracking_continuity(plan, event_kind) if ( event_kind not in ( @@ -1068,14 +1123,26 @@ def _install_plan( or replacement_destinations ): self._active_targets = replacement_targets + if ( + event_kind + not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ) + or replacement_tracking_routes + ): + self._active_tracking_routes = replacement_tracking_routes self._plan = plan self._attempt_generation += 1 self._waypoint_index = 0 self._planned_scene = context.scene self._action_started_at = context.robot.timestamp - self._last_joint_command = None - self._last_joint_ids = () + self._last_tracking_frame = None self._last_command_mask.zero_() + self._tracking_violation_counts.zero_() + self._terminal_acceptance_counts.zero_() + self._terminal_started_at = None + self._terminal_pending_reported = False self._pending_effect = None self._effect_failures.zero_() self._effect_requested_at = None @@ -1162,6 +1229,56 @@ def _validate_destination_continuity( f"replacement={sorted(replacement_destinations)}.{guidance}" ) + def _validate_tracking_continuity( + self, + plan: ActionPlan, + event_kind: ExecutionEventKind, + ) -> None: + """Reject in-place replacement of feedback ownership or projection.""" + if event_kind not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ): + return + if self._plan is None: + return + previous_routes = self._active_tracking_routes + replacement_routes = self._tracking_routes(plan) + if ( + event_kind is ExecutionEventKind.REPLANNED + and not plan.commands.targets + and not replacement_routes + ): + return + if previous_routes == replacement_routes: + return + prefix = ( + "Recovery replans" + if event_kind is ExecutionEventKind.REPLANNED + else "Invocation revisions" + ) + raise ValueError( + f"{prefix} must preserve endpoint tracking source fingerprints and " + "projector routes; start a new invocation to change feedback " + "ownership." + ) + + @staticmethod + def _tracking_routes( + plan: ActionPlan, + ) -> dict[tuple[str, str, str], tuple[object, str, str]]: + """Return the complete feedback/projector route owned by one plan.""" + if plan.tracking is None or not plan.tracking.frames: + return {} + return { + setpoint.key: ( + setpoint.binding.source.source_fingerprint, + setpoint.binding.projector.projector_id, + setpoint.binding.projector.revision, + ) + for setpoint in plan.tracking.frames[0].setpoints + } + def _recover_if_needed( self, plan: ActionPlan, @@ -1184,34 +1301,49 @@ def _recover_if_needed( ExecutionEventKind.COLLISION_WORLD_CHANGED, "The collision world changed after this trajectory was planned.", ) + in_flight = plan.tracking_policy.in_flight if ( - plan.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION - and self._last_joint_command is not None - and self._last_joint_ids + in_flight is not None + and self._last_tracking_frame is not None + and self._waypoint_index < plan.commands.frame_count ): - joint_ids = list(self._last_joint_ids) - tracking_error = torch.amax( - torch.abs( - self._context.robot.qpos[:, joint_ids] - - self._last_joint_command[:, joint_ids] - ), - dim=1, - ) - tracking_mask = ( - execution_mask - & self._last_command_mask - & (tracking_error > plan.recovery_policy.tracking_error_threshold) - ) - if tracking_mask.any(): - max_tracking_error = float(tracking_error[tracking_mask].amax().item()) - return self._attempt_replan( - tracking_mask, - ExecutionEventKind.TRACKING_ERROR, - "Observed joint tracking error exceeded the policy threshold " - f"(max_error={max_tracking_error:.6f}, " - "threshold=" - f"{plan.recovery_policy.tracking_error_threshold:.6f}).", + tracking_mask = execution_mask & self._last_command_mask + if ( + tracking_mask.any() + and self._context.robot.timestamp - self._action_started_at + >= in_flight.grace_period + ): + try: + accepted, valid, normalized_error = self._evaluate_tracking_frame( + self._last_tracking_frame, + in_flight.metrics, + ) + except Exception as exc: # noqa: BLE001 - fail required feedback closed + return self._fail_tracking_feedback( + tracking_mask, + "In-flight tracking feedback evaluation failed: " + f"{type(exc).__name__}: {exc}", + ) + invalid = tracking_mask & ~valid + if invalid.any(): + return self._fail_tracking_feedback( + invalid, + "Required in-flight tracking feedback was invalid.", + ) + violated = tracking_mask & valid & ~accepted + self._tracking_violation_counts[violated] += 1 + self._tracking_violation_counts[tracking_mask & ~violated] = 0 + diverged = tracking_mask & ( + self._tracking_violation_counts >= in_flight.consecutive_violations ) + if diverged.any(): + max_error = float(normalized_error[diverged].amax().item()) + return self._attempt_replan( + diverged, + ExecutionEventKind.TRACKING_DIVERGED, + "Observed in-flight tracking diverged from the commanded " + f"setpoint (max_normalized_error={max_error:.6f}).", + ) scene_mask, scene_message = self._dynamic_scene_change( plan, execution_mask, @@ -1517,76 +1649,72 @@ def _command_at( waypoint_index: int, active_mask: torch.Tensor, ) -> RuntimeCommandFrame: - """Return one frame and retain joint targets when feedback requires it.""" + """Return one frame and retain its generic typed tracking targets.""" frame = plan.commands.frames[waypoint_index] frame = frame.with_active_mask(frame.active_mask & active_mask) - if plan.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION: - positions = self._context.robot.qpos.clone() - commanded_joint_ids: list[int] = [] - for command in frame.commands: - if not isinstance( - command.target, JointPositionTarget - ) or not isinstance( - command.payload, - JointPositionPayload, - ): - raise TypeError( - "joint_position feedback requires only joint-position " - "targets and payloads." - ) - joint_ids = list(command.target.joint_ids) - commanded_joint_ids.extend(joint_ids) - positions[:, joint_ids] = torch.where( - frame.active_mask[:, None], - command.payload.positions, - positions[:, joint_ids], - ) - self._last_joint_command = positions - self._last_joint_ids = tuple(commanded_joint_ids) - self._last_command_mask = frame.active_mask.clone() - else: - self._last_joint_command = None - self._last_joint_ids = () - self._last_command_mask.zero_() + self._last_tracking_frame = ( + None + if plan.tracking is None + else plan.tracking.frames[waypoint_index].snapshot() + ) + self._last_command_mask = frame.active_mask.clone() + if waypoint_index == plan.commands.frame_count - 1: + self._terminal_started_at = self._context.robot.timestamp + self._terminal_acceptance_counts.zero_() + self._terminal_pending_reported = False return frame - def _terminal_error(self, plan: ActionPlan) -> torch.Tensor: - """Return terminal error for the plan's explicit feedback contract.""" - if plan.feedback_mode is ExecutionFeedbackMode.TIMED: - return torch.zeros( - self._context.batch_size, - dtype=self._context.robot.qpos.dtype, - device=self._context.robot.qpos.device, - ) - if plan.commands.frame_count == 0: - return torch.full_like( - self._eligible, - float("inf"), - dtype=self._context.robot.qpos.dtype, - ) - errors: list[torch.Tensor] = [] - for command in plan.commands.frames[-1].commands: - if not isinstance(command.target, JointPositionTarget) or not isinstance( - command.payload, - JointPositionPayload, - ): + def _evaluate_tracking_frame( + self, + frame: TrackingFrame, + metrics: tuple[TrackingMetricCfg, ...], + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Aggregate typed endpoint predicates without mixing physical units.""" + evaluations = self._engine.tracking_runtime.evaluate_frame( + frame, + metrics, + self._context, + ) + accepted = torch.ones_like(self._eligible) + valid = torch.ones_like(self._eligible) + normalized_error = torch.zeros( + self._context.batch_size, + dtype=self._context.robot.qpos.dtype, + device=self._context.robot.qpos.device, + ) + for evaluation in evaluations.values(): + if not isinstance(evaluation, TrackingEvaluation): raise TypeError( - "joint_position feedback requires only joint-position targets " - "and payloads." - ) - joint_ids = list(command.target.joint_ids) - errors.append( - torch.abs( - self._context.robot.qpos[:, joint_ids] - command.payload.positions + "TrackingRuntime.evaluate_frame() must return " + "TrackingEvaluation values." ) + accepted &= evaluation.accepted_mask + valid &= evaluation.valid_mask + normalized_error = torch.maximum( + normalized_error, + evaluation.normalized_error.to(normalized_error.dtype), ) - if not errors: - return torch.full_like( - self._eligible, - float("inf"), - dtype=self._context.robot.qpos.dtype, + return accepted, valid, normalized_error + + def _fail_tracking_feedback( + self, + failed_mask: torch.Tensor, + message: str, + ) -> list[ExecutionEvent]: + """Fail affected rows closed when required feedback is unavailable.""" + self._eligible &= ~failed_mask + self._pending &= ~failed_mask + events = [ + self._event( + ExecutionEventKind.TRACKING_FEEDBACK_FAILED, + failed_mask, + message, ) - return torch.amax(torch.cat(errors, dim=1), dim=1) + ] + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) + return events def _dynamic_scene_change( self, diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index 00bcfa72a..26acdc5b8 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -28,6 +28,7 @@ from .bindings import ActionBinding from .control import ActionControlOverrides from .policies import MotionPolicy, RecoveryPolicy +from .tracking import TrackingPolicy GoalT = TypeVar("GoalT") @@ -100,6 +101,11 @@ class ActionInvocation(Generic[GoalT, OptionsT]): motion_policy: MotionPolicy = field(default_factory=MotionPolicy) """Reusable motion-generation settings.""" + tracking_policy: TrackingPolicy = field( + default_factory=TrackingPolicy.joint_position + ) + """Typed in-flight tracking and terminal-acceptance settings.""" + recovery_policy: RecoveryPolicy = field(default_factory=RecoveryPolicy) """Bounded local execution recovery settings.""" @@ -124,6 +130,8 @@ def __post_init__(self) -> None: raise TypeError("binding must be an ActionBinding.") if not isinstance(self.motion_policy, MotionPolicy): raise TypeError("motion_policy must be a MotionPolicy.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") if self.skill_options is not None and not isinstance( @@ -154,6 +162,7 @@ class ResolvedActionRequest(Generic[GoalT, OptionsT]): goal: GoalT binding: ActionBinding motion_policy: MotionPolicy + tracking_policy: TrackingPolicy recovery_policy: RecoveryPolicy skill_options: OptionsT invocation_id: str | None = None @@ -166,6 +175,8 @@ def __post_init__(self) -> None: raise TypeError("binding must be an ActionBinding.") if not isinstance(self.motion_policy, MotionPolicy): raise TypeError("motion_policy must be a MotionPolicy.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") if not isinstance(self.skill_options, ActionOptions): @@ -190,6 +201,7 @@ 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, "skill_options", deepcopy(self.skill_options)) @@ -200,6 +212,7 @@ def snapshot(self) -> ResolvedActionRequest[GoalT, OptionsT]: goal=self.goal, binding=self.binding, motion_policy=self.motion_policy, + tracking_policy=self.tracking_policy, recovery_policy=self.recovery_policy, skill_options=self.skill_options, invocation_id=self.invocation_id, diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index 753b016cf..4bcdd6815 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -20,7 +20,6 @@ from copy import deepcopy from dataclasses import dataclass, field -from enum import Enum import math from types import MappingProxyType from typing import Any, Mapping, Sequence @@ -29,11 +28,15 @@ from embodichain.lab.sim.planners.utils import normalize_success_mask -from .bindings import JointPositionTarget from .effects import StateDelta from .policies import RecoveryPolicy -from .runtime_commands import JointPositionPayload, TimedCommandSequence +from .runtime_commands import TimedCommandSequence from .state import PlanningContext +from .tracking import ( + FeedbackTerminalAcceptance, + TimedTrackingSequence, + TrackingPolicy, +) def _validate_optional_trajectory_field( @@ -378,13 +381,6 @@ def __post_init__(self) -> None: ) -class ExecutionFeedbackMode(str, Enum): - """Feedback contract used to decide whether an action reached its target.""" - - JOINT_POSITION = "joint_position" - TIMED = "timed" - - @dataclass(frozen=True, slots=True) class EffectVerificationRequirement: """Explicit physical-effect verification independent of symbolic state. @@ -472,10 +468,11 @@ class ActionPlan: plan_success: torch.Tensor commands: TimedCommandSequence recovery_policy: RecoveryPolicy + tracking_policy: TrackingPolicy planned_scene_version: int planned_collision_world_revision: tuple[int, ...] diagnostics: PlannerDiagnostics - feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED + tracking: TimedTrackingSequence | None = None joint_trajectory: TimedTrajectory | None = None segments: tuple[TrajectorySegment, ...] = () scene_dependencies: tuple[str, ...] = () @@ -505,8 +502,8 @@ def __post_init__(self) -> None: raise ValueError("plan_success batch must match the command sequence.") if self.commands.device != self.plan_success.device: raise ValueError("plan_success and commands must share a device.") - if not isinstance(self.feedback_mode, ExecutionFeedbackMode): - raise TypeError("feedback_mode must be an ExecutionFeedbackMode.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") expected_target_types: dict[tuple[str, str], type[object]] | None = None expected_target_fingerprints: dict[tuple[str, str], object] | None = None for frame_index, frame in enumerate(self.commands.frames): @@ -567,101 +564,88 @@ def __post_init__(self) -> None: ) if self.joint_trajectory.positions.device != self.commands.device: raise ValueError("joint_trajectory and commands must share a device.") - if ( - self.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION - and self.joint_trajectory is None + required_channels = { + metric.channel_id + for metric in ( + () + if self.tracking_policy.in_flight is None + else self.tracking_policy.in_flight.metrics + ) + } + if isinstance( + self.tracking_policy.terminal, + FeedbackTerminalAcceptance, ): - raise ValueError( - "joint_position feedback requires an owned joint_trajectory." + required_channels.update( + metric.channel_id for metric in self.tracking_policy.terminal.metrics ) - if self.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION: - if bool(self.plan_success.any().item()) and self.commands.frame_count == 0: + if self.tracking is None: + if required_channels: + raise ValueError( + "Feedback tracking policies require an owned tracking sequence." + ) + else: + if not isinstance(self.tracking, TimedTrackingSequence): + raise TypeError("tracking must be a TimedTrackingSequence or None.") + if self.tracking.batch_size != self.commands.batch_size: + raise ValueError("tracking batch must match the command sequence.") + if self.tracking.frame_count != self.commands.frame_count: + raise ValueError("tracking frames must match command sequence frames.") + if not torch.equal(self.tracking.env_ids, self.commands.env_ids): + raise ValueError("tracking env_ids must match the command sequence.") + if self.tracking.device != self.commands.device: + raise ValueError("tracking and commands must share a device.") + if not required_channels: + raise ValueError( + "A tracking sequence requires an in-flight or terminal " + "feedback metric." + ) + if bool(self.plan_success.any().item()) and not self.tracking.frames: raise ValueError( - "joint_position feedback requires command frames when any " + "Feedback tracking requires command frames when any " "environment planned successfully." ) - assert self.joint_trajectory is not None - expected_destinations: dict[tuple[str, str], tuple[int, ...]] | None = None - for frame_index, frame in enumerate(self.commands.frames): - if not frame.commands: + expected_setpoint_keys: set[tuple[str, str, str]] | None = None + expected_setpoint_routes: ( + dict[ + tuple[str, str, str], + tuple[object, str, str], + ] + | None + ) = None + for frame_index, frame in enumerate(self.tracking.frames): + frame_keys = {setpoint.key for setpoint in frame.setpoints} + frame_routes = { + setpoint.key: ( + setpoint.binding.source.source_fingerprint, + setpoint.binding.projector.projector_id, + setpoint.binding.projector.revision, + ) + for setpoint in frame.setpoints + } + frame_channels = { + setpoint.binding.channel_id for setpoint in frame.setpoints + } + if frame_channels != required_channels: raise ValueError( - "joint_position feedback requires at least one endpoint " - f"command in frame {frame_index}." + "Every tracking frame must cover exactly the configured " + f"feedback channels; frame {frame_index} has " + f"{sorted(frame_channels)}, expected " + f"{sorted(required_channels)}." ) - if any( - not isinstance(command.target, JointPositionTarget) - or not isinstance(command.payload, JointPositionPayload) - for command in frame.commands - ): + if expected_setpoint_keys is None: + expected_setpoint_keys = frame_keys + expected_setpoint_routes = frame_routes + elif frame_keys != expected_setpoint_keys: raise ValueError( - "joint_position feedback accepts only JointPositionTarget " - "and JointPositionPayload commands." + "Tracking frames must preserve the same endpoint/channel " + f"set; frame {frame_index} differs from frame 0." ) - for command in frame.commands: - target = command.target - payload = command.payload - assert isinstance(target, JointPositionTarget) - assert isinstance(payload, JointPositionPayload) - if any( - joint_id >= self.joint_trajectory.robot_dof - for joint_id in target.joint_ids - ): - raise ValueError( - f"Joint target {command.destination_key} contains joint " - "IDs outside joint_trajectory robot_dof " - f"{self.joint_trajectory.robot_dof}." - ) - joint_ids = list(target.joint_ids) - expected_positions = self.joint_trajectory.positions[ - :, frame_index, joint_ids - ] - if ( - payload.positions.dtype != expected_positions.dtype - or not torch.equal(payload.positions, expected_positions) - ): - raise ValueError( - f"Joint payload positions for {command.destination_key} " - "must exactly match the corresponding joint_trajectory " - f"slice at frame {frame_index}." - ) - trajectory_velocities = self.joint_trajectory.velocities - if (payload.velocities is None) != (trajectory_velocities is None): - raise ValueError( - f"Joint payload velocities for {command.destination_key} " - "must have the same presence as joint_trajectory " - "velocities." - ) - if ( - payload.velocities is not None - and trajectory_velocities is not None - ): - expected_velocities = trajectory_velocities[ - :, frame_index, joint_ids - ] - if ( - payload.velocities.dtype != expected_velocities.dtype - or not torch.equal( - payload.velocities, - expected_velocities, - ) - ): - raise ValueError( - "Joint payload velocities for " - f"{command.destination_key} must exactly match the " - "corresponding joint_trajectory slice at frame " - f"{frame_index}." - ) - destinations = { - command.destination_key: command.target.joint_ids - for command in frame.commands - if isinstance(command.target, JointPositionTarget) - } - if expected_destinations is None: - expected_destinations = destinations - elif destinations != expected_destinations: + elif frame_routes != expected_setpoint_routes: raise ValueError( - "joint_position feedback requires a stable joint endpoint " - "set across every command frame." + "Tracking frames must preserve each endpoint/channel " + "source fingerprint and projector route; " + f"frame {frame_index} differs from frame 0." ) if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") @@ -747,6 +731,16 @@ def __post_init__(self) -> None: ) object.__setattr__(self, "plan_success", self.plan_success.clone()) object.__setattr__(self, "commands", self.commands.snapshot()) + object.__setattr__( + self, + "tracking_policy", + self.tracking_policy.snapshot(), + ) + object.__setattr__( + self, + "tracking", + None if self.tracking is None else self.tracking.snapshot(), + ) object.__setattr__( self, "joint_trajectory", @@ -805,10 +799,11 @@ def snapshot(self) -> ActionPlan: plan_success=self.plan_success, commands=self.commands, recovery_policy=self.recovery_policy, + tracking_policy=self.tracking_policy, planned_scene_version=self.planned_scene_version, planned_collision_world_revision=self.planned_collision_world_revision, diagnostics=self.diagnostics, - feedback_mode=self.feedback_mode, + tracking=self.tracking, joint_trajectory=self.joint_trajectory, segments=self.segments, scene_dependencies=self.scene_dependencies, @@ -909,7 +904,6 @@ def segment(self, action_index: int, name: str) -> TrajectorySegment: "ActionPlan", "CompiledTrajectory", "EffectVerificationRequirement", - "ExecutionFeedbackMode", "PlannerDiagnostics", "TimedTrajectory", "TrajectorySegment", diff --git a/embodichain/lab/sim/atomic_actions/policies.py b/embodichain/lab/sim/atomic_actions/policies.py index 66757ce50..668f1669e 100644 --- a/embodichain/lab/sim/atomic_actions/policies.py +++ b/embodichain/lab/sim/atomic_actions/policies.py @@ -141,9 +141,6 @@ class RecoveryPolicy: max_action_retries: int = 2 """Maximum whole-action retries after planning, execution, or effect failure.""" - tracking_error_threshold: float = 0.05 - """Joint tracking-error threshold in radians.""" - goal_translation_threshold: float = 0.02 """Dynamic-goal translation threshold in metres.""" @@ -159,7 +156,6 @@ def __post_init__(self) -> None: if self.max_action_retries < 0: raise ValueError("max_action_retries must be non-negative.") threshold_fields = ( - "tracking_error_threshold", "goal_translation_threshold", "goal_rotation_threshold", "action_timeout", diff --git a/embodichain/lab/sim/atomic_actions/runtime.py b/embodichain/lab/sim/atomic_actions/runtime.py index 13228ff37..868d31bc6 100644 --- a/embodichain/lab/sim/atomic_actions/runtime.py +++ b/embodichain/lab/sim/atomic_actions/runtime.py @@ -33,6 +33,14 @@ DisjointSlotEndpoints, SkillBindingContract, ) +from .tracking import ( + JOINT_POSITION_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + TrackingFeedbackSourceRef, + TrackingProjectorRef, + TrackingRuntime, +) if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -46,11 +54,18 @@ def __init__( self, motion_generator: MotionGenerator, control_profiles: Mapping[str, ControlPartCommandProfile] | None = None, + tracking_runtime: TrackingRuntime | None = None, ) -> None: self._motion_generator = motion_generator self._robot: Robot = motion_generator.robot self._device = resolve_runtime_device(motion_generator.device) self._binding_owner_id = uuid4().hex + if tracking_runtime is not None and not isinstance( + tracking_runtime, + TrackingRuntime, + ): + raise TypeError("tracking_runtime must be a TrackingRuntime or None.") + self._tracking_runtime = tracking_runtime or TrackingRuntime.with_builtins() self._control_profiles = self._snapshot_control_profiles( {} if control_profiles is None else control_profiles ) @@ -75,6 +90,11 @@ def binding_owner_id(self) -> str: """Return the opaque identity required by this engine's bindings.""" return self._binding_owner_id + @property + def tracking_runtime(self) -> TrackingRuntime: + """Return the engine-owned typed tracking runtime.""" + return self._tracking_runtime + @property def control_profiles(self) -> Mapping[str, ControlPartCommandProfile]: """Return owned direct-core command profiles by control-part name.""" @@ -232,14 +252,32 @@ def bind_control_parts( f"Endpoint {slot_id}.{endpoint_id} requires command {name!r} " f"of type {command_type.__name__}." ) + target = JointPositionTarget(control_part, joint_ids) resolved.append( EndpointBinding( slot_id=slot_id, endpoint_id=endpoint_id, resource_id=f"direct.{slot_id}", adapter_id="control_part", - target=JointPositionTarget(control_part, joint_ids), + target=target, task_state_key=resolved_task_state_keys[slot_id], + tracking_channels={ + JOINT_POSITION_CHANNEL: EndpointTrackingChannelBinding( + channel_id=JOINT_POSITION_CHANNEL, + source=TrackingFeedbackSourceRef( + provider_id="planning_context.robot", + revision="1", + address=EndpointTrackingFeedbackAddress( + target=target, + channel_id=JOINT_POSITION_CHANNEL, + ), + ), + projector=TrackingProjectorRef( + projector_id="joint_position_payload", + revision="1", + ), + ) + }, capabilities=requirement.capabilities, commands=commands, claim_tokens=frozenset({f"robot.control_part:{control_part}"}), diff --git a/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index 6b10d3a7a..47ad90cec 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -569,6 +569,38 @@ def __post_init__(self) -> None: raise ValueError("RobotObservation.qeffort must match qpos shape.") if self.qeffort.device != self.qpos.device: raise ValueError("RobotObservation.qeffort must share the qpos device.") + if self.root_pose is not None: + if not isinstance(self.root_pose, torch.Tensor): + raise TypeError("RobotObservation.root_pose must be a tensor or None.") + if self.root_pose.shape != (self.qpos.shape[0], 4, 4): + raise ValueError( + "RobotObservation.root_pose must have shape " + f"({self.qpos.shape[0]}, 4, 4)." + ) + if not self.root_pose.is_floating_point(): + raise TypeError("RobotObservation.root_pose must be floating point.") + if self.root_pose.device != self.qpos.device: + raise ValueError( + "RobotObservation.root_pose must share the qpos device." + ) + if not torch.isfinite(self.root_pose).all(): + raise ValueError("RobotObservation.root_pose must be finite.") + if self.root_twist is not None: + if not isinstance(self.root_twist, torch.Tensor): + raise TypeError("RobotObservation.root_twist must be a tensor or None.") + if self.root_twist.shape != (self.qpos.shape[0], 6): + raise ValueError( + "RobotObservation.root_twist must have shape " + f"({self.qpos.shape[0]}, 6)." + ) + if not self.root_twist.is_floating_point(): + raise TypeError("RobotObservation.root_twist must be floating point.") + if self.root_twist.device != self.qpos.device: + raise ValueError( + "RobotObservation.root_twist must share the qpos device." + ) + if not torch.isfinite(self.root_twist).all(): + raise ValueError("RobotObservation.root_twist must be finite.") object.__setattr__(self, "qpos", self.qpos.clone()) object.__setattr__(self, "qvel", self.qvel.clone()) if self.qeffort is not None: diff --git a/embodichain/lab/sim/atomic_actions/tracking.py b/embodichain/lab/sim/atomic_actions/tracking.py new file mode 100644 index 000000000..2a9e12acc --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/tracking.py @@ -0,0 +1,1210 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Typed, transport-neutral tracking contracts for atomic-action execution.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from copy import deepcopy +from dataclasses import dataclass, field +from types import MappingProxyType +from typing import TYPE_CHECKING, ClassVar, Hashable, Iterable, Mapping, Protocol + +import torch + +if TYPE_CHECKING: + from .bindings import RuntimeEndpointTarget + from .runtime_commands import EndpointCommand + from .state import PlanningContext + + +TrackingChannelId = str +"""Open string identifier for one typed endpoint-feedback channel.""" + +JOINT_POSITION_CHANNEL: TrackingChannelId = "joint.position" +BASE_POSE_CHANNEL: TrackingChannelId = "base.pose" +WHOLE_BODY_POSE_CHANNEL: TrackingChannelId = "whole_body.pose" + + +def _identifier(value: str, *, field_name: str) -> str: + if not isinstance(value, str) or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty trimmed string.") + return value + + +def _positive_float(value: float, *, field_name: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a number.") + normalized = float(value) + if not torch.isfinite(torch.tensor(normalized)).item() or normalized <= 0.0: + raise ValueError(f"{field_name} must be finite and positive.") + return normalized + + +def _non_negative_float(value: float, *, field_name: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a number.") + normalized = float(value) + if not torch.isfinite(torch.tensor(normalized)).item() or normalized < 0.0: + raise ValueError(f"{field_name} must be finite and non-negative.") + return normalized + + +def _tensor(value: torch.Tensor, *, field_name: str, dimensions: int) -> torch.Tensor: + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.dim() != dimensions or any(size < 1 for size in value.shape): + raise ValueError(f"{field_name} must be a non-empty {dimensions}-D tensor.") + if not torch.is_floating_point(value) or not torch.isfinite(value).all().item(): + raise ValueError(f"{field_name} must contain finite floating-point values.") + return value.clone() + + +class TrackingFeedbackAddress(ABC): + """Immutable address understood by one tracking-feedback provider.""" + + @property + @abstractmethod + def address_fingerprint(self) -> Hashable: + """Return a stable, hashable address identity.""" + + def snapshot(self) -> TrackingFeedbackAddress: + """Return an independently owned address snapshot.""" + return deepcopy(self) + + +@dataclass(frozen=True, slots=True) +class EndpointTrackingFeedbackAddress(TrackingFeedbackAddress): + """Feedback address for one runtime endpoint and open tracking channel.""" + + target: RuntimeEndpointTarget + channel_id: TrackingChannelId + + def __post_init__(self) -> None: + from .bindings import RuntimeEndpointTarget + + if not isinstance(self.target, RuntimeEndpointTarget): + raise TypeError("target must be a RuntimeEndpointTarget.") + snapshot = self.target.snapshot() + if type(snapshot) is not type(self.target) or snapshot is self.target: + raise TypeError("RuntimeEndpointTarget.snapshot() must own a new value.") + if snapshot.address_fingerprint != self.target.address_fingerprint: + raise ValueError("Target snapshot must preserve its address fingerprint.") + _identifier(self.channel_id, field_name="channel_id") + object.__setattr__(self, "target", snapshot) + + @property + def address_fingerprint(self) -> Hashable: + """Return the endpoint- and channel-scoped address identity.""" + return self.target.address_fingerprint, self.channel_id + + +@dataclass(frozen=True, slots=True) +class TrackingFeedbackSourceRef: + """Versioned provider route plus one immutable feedback address.""" + + provider_id: str + revision: str + address: TrackingFeedbackAddress + + def __post_init__(self) -> None: + _identifier(self.provider_id, field_name="provider_id") + _identifier(self.revision, field_name="revision") + if not isinstance(self.address, TrackingFeedbackAddress): + raise TypeError("address must be a TrackingFeedbackAddress.") + snapshot = self.address.snapshot() + if type(snapshot) is not type(self.address) or snapshot is self.address: + raise TypeError("TrackingFeedbackAddress.snapshot() must own a new value.") + if snapshot.address_fingerprint != self.address.address_fingerprint: + raise ValueError("Address snapshot must preserve its fingerprint.") + hash(snapshot.address_fingerprint) + object.__setattr__(self, "address", snapshot) + + @property + def source_fingerprint(self) -> Hashable: + """Return the exact versioned source identity.""" + return self.provider_id, self.revision, self.address.address_fingerprint + + def snapshot(self) -> TrackingFeedbackSourceRef: + """Return an independently owned source reference.""" + return TrackingFeedbackSourceRef(self.provider_id, self.revision, self.address) + + +@dataclass(frozen=True, slots=True) +class TrackingProjectorRef: + """Exact version of a command-to-tracking-state projector.""" + + projector_id: str + revision: str + + def __post_init__(self) -> None: + _identifier(self.projector_id, field_name="projector_id") + _identifier(self.revision, field_name="revision") + + def snapshot(self) -> TrackingProjectorRef: + """Return an independently owned projector route.""" + return TrackingProjectorRef(self.projector_id, self.revision) + + +@dataclass(frozen=True, slots=True) +class EndpointTrackingChannelBinding: + """Resolved source and projector for one endpoint tracking channel.""" + + channel_id: TrackingChannelId + source: TrackingFeedbackSourceRef + projector: TrackingProjectorRef + + def __post_init__(self) -> None: + _identifier(self.channel_id, field_name="channel_id") + if not isinstance(self.source, TrackingFeedbackSourceRef): + raise TypeError("source must be a TrackingFeedbackSourceRef.") + if not isinstance(self.projector, TrackingProjectorRef): + raise TypeError("projector must be a TrackingProjectorRef.") + address = self.source.address + if isinstance(address, EndpointTrackingFeedbackAddress): + if address.channel_id != self.channel_id: + raise ValueError("Binding and feedback-address channels must match.") + object.__setattr__(self, "source", self.source.snapshot()) + object.__setattr__(self, "projector", self.projector.snapshot()) + + def snapshot(self) -> EndpointTrackingChannelBinding: + """Return an independently owned channel binding.""" + return EndpointTrackingChannelBinding( + self.channel_id, self.source, self.projector + ) + + @property + def route_fingerprint(self) -> tuple[str, Hashable, str, str]: + """Return the exact channel, source, and projector route identity.""" + return ( + self.channel_id, + self.source.source_fingerprint, + self.projector.projector_id, + self.projector.revision, + ) + + +class TrackingState(ABC): + """Immutable-by-ownership typed desired or observed tracking state.""" + + channel_id: ClassVar[TrackingChannelId] + + @property + @abstractmethod + def batch_size(self) -> int: + """Return the represented environment count.""" + + @property + @abstractmethod + def device(self) -> torch.device: + """Return the tensor device.""" + + @abstractmethod + def snapshot(self) -> TrackingState: + """Return an independently owned state snapshot.""" + + +@dataclass(frozen=True, slots=True, eq=False) +class JointPositionTrackingState(TrackingState): + """Batched joint positions with shape ``(B, D)``.""" + + channel_id: ClassVar[str] = JOINT_POSITION_CHANNEL + positions: torch.Tensor + + def __post_init__(self) -> None: + object.__setattr__( + self, + "positions", + _tensor(self.positions, field_name="positions", dimensions=2), + ) + + @property + def batch_size(self) -> int: + return int(self.positions.shape[0]) + + @property + def device(self) -> torch.device: + return self.positions.device + + def snapshot(self) -> JointPositionTrackingState: + return JointPositionTrackingState(self.positions) + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseTrackingState(TrackingState): + """Batched homogeneous poses with shape ``(B, 4, 4)``.""" + + channel_id: ClassVar[str] = BASE_POSE_CHANNEL + poses: torch.Tensor + + def __post_init__(self) -> None: + poses = _tensor(self.poses, field_name="poses", dimensions=3) + if poses.shape[1:] != (4, 4): + raise ValueError("poses must have shape (batch_size, 4, 4).") + object.__setattr__(self, "poses", poses) + + @property + def batch_size(self) -> int: + return int(self.poses.shape[0]) + + @property + def device(self) -> torch.device: + return self.poses.device + + def snapshot(self) -> PoseTrackingState: + return PoseTrackingState(self.poses) + + +@dataclass(frozen=True, slots=True, eq=False) +class WholeBodyPoseTrackingState(TrackingState): + """Batched base poses and joint positions for whole-body tracking.""" + + channel_id: ClassVar[str] = WHOLE_BODY_POSE_CHANNEL + root_poses: torch.Tensor + joint_positions: torch.Tensor + + def __post_init__(self) -> None: + root_poses = _tensor(self.root_poses, field_name="root_poses", dimensions=3) + joints = _tensor( + self.joint_positions, + field_name="joint_positions", + dimensions=2, + ) + if root_poses.shape[1:] != (4, 4): + raise ValueError("root_poses must have shape (batch_size, 4, 4).") + if root_poses.shape[0] != joints.shape[0]: + raise ValueError("root_poses and joint_positions batches must match.") + if root_poses.device != joints.device: + raise ValueError("root_poses and joint_positions must share a device.") + object.__setattr__(self, "root_poses", root_poses) + object.__setattr__(self, "joint_positions", joints) + + @property + def batch_size(self) -> int: + return int(self.root_poses.shape[0]) + + @property + def device(self) -> torch.device: + return self.root_poses.device + + def snapshot(self) -> WholeBodyPoseTrackingState: + return WholeBodyPoseTrackingState(self.root_poses, self.joint_positions) + + +class TrackingMetricCfg(ABC): + """Immutable tolerance configuration dispatched by exact metric ID/revision.""" + + metric_id: ClassVar[str] + revision: ClassVar[str] = "1" + channel_id: ClassVar[TrackingChannelId] + + def snapshot(self) -> TrackingMetricCfg: + """Return an independently owned metric configuration.""" + return deepcopy(self) + + +@dataclass(frozen=True, slots=True) +class JointPositionTrackingMetric(TrackingMetricCfg): + """Maximum absolute joint-error tolerance.""" + + metric_id: ClassVar[str] = "joint.max_abs" + channel_id: ClassVar[str] = JOINT_POSITION_CHANNEL + tolerance: float = 0.05 + + def __post_init__(self) -> None: + object.__setattr__( + self, "tolerance", _positive_float(self.tolerance, field_name="tolerance") + ) + + +@dataclass(frozen=True, slots=True) +class PoseTrackingMetric(TrackingMetricCfg): + """Independent translation and rotation tolerances for base pose.""" + + metric_id: ClassVar[str] = "pose.se3" + channel_id: ClassVar[str] = BASE_POSE_CHANNEL + translation_tolerance: float = 0.02 + rotation_tolerance: float = 0.05 + + def __post_init__(self) -> None: + object.__setattr__( + self, + "translation_tolerance", + _positive_float( + self.translation_tolerance, field_name="translation_tolerance" + ), + ) + object.__setattr__( + self, + "rotation_tolerance", + _positive_float(self.rotation_tolerance, field_name="rotation_tolerance"), + ) + + +@dataclass(frozen=True, slots=True) +class WholeBodyPoseTrackingMetric(TrackingMetricCfg): + """Independent base-pose and joint-position tolerances.""" + + metric_id: ClassVar[str] = "whole_body.pose" + channel_id: ClassVar[str] = WHOLE_BODY_POSE_CHANNEL + translation_tolerance: float = 0.02 + rotation_tolerance: float = 0.05 + joint_position_tolerance: float = 0.05 + + def __post_init__(self) -> None: + for field_name in ( + "translation_tolerance", + "rotation_tolerance", + "joint_position_tolerance", + ): + object.__setattr__( + self, + field_name, + _positive_float(getattr(self, field_name), field_name=field_name), + ) + + +def _own_metrics( + metrics: Iterable[TrackingMetricCfg], *, field_name: str +) -> tuple[TrackingMetricCfg, ...]: + snapshots: list[TrackingMetricCfg] = [] + channels: set[str] = set() + for metric in metrics: + if not isinstance(metric, TrackingMetricCfg): + raise TypeError(f"{field_name} must contain TrackingMetricCfg values.") + _identifier(metric.metric_id, field_name=f"{field_name}.metric_id") + _identifier(metric.revision, field_name=f"{field_name}.revision") + _identifier(metric.channel_id, field_name=f"{field_name}.channel_id") + if metric.channel_id in channels: + raise ValueError( + f"{field_name} contains duplicate channel {metric.channel_id!r}." + ) + snapshot = metric.snapshot() + if type(snapshot) is not type(metric) or snapshot is metric: + raise TypeError("TrackingMetricCfg.snapshot() must own a same-type value.") + channels.add(metric.channel_id) + snapshots.append(snapshot) + if not snapshots: + raise ValueError(f"{field_name} must contain at least one metric.") + return tuple(snapshots) + + +@dataclass(frozen=True, slots=True) +class InFlightTrackingPolicy: + """Feedback checks used while a command sequence is still in flight.""" + + metrics: tuple[TrackingMetricCfg, ...] + consecutive_violations: int = 1 + grace_period: float = 0.0 + + def __post_init__(self) -> None: + object.__setattr__( + self, "metrics", _own_metrics(self.metrics, field_name="metrics") + ) + if ( + not isinstance(self.consecutive_violations, int) + or isinstance(self.consecutive_violations, bool) + or self.consecutive_violations < 1 + ): + raise ValueError("consecutive_violations must be a positive integer.") + object.__setattr__( + self, + "grace_period", + _non_negative_float(self.grace_period, field_name="grace_period"), + ) + + def snapshot(self) -> InFlightTrackingPolicy: + return InFlightTrackingPolicy( + self.metrics, self.consecutive_violations, self.grace_period + ) + + +@dataclass(frozen=True, slots=True) +class FeedbackTerminalAcceptance: + """Terminal acceptance proven by typed endpoint feedback.""" + + metrics: tuple[TrackingMetricCfg, ...] + settle_timeout: float = 0.0 + consecutive_acceptances: int = 1 + + def __post_init__(self) -> None: + object.__setattr__( + self, "metrics", _own_metrics(self.metrics, field_name="metrics") + ) + object.__setattr__( + self, + "settle_timeout", + _non_negative_float(self.settle_timeout, field_name="settle_timeout"), + ) + if ( + not isinstance(self.consecutive_acceptances, int) + or isinstance(self.consecutive_acceptances, bool) + or self.consecutive_acceptances < 1 + ): + raise ValueError("consecutive_acceptances must be a positive integer.") + + def snapshot(self) -> FeedbackTerminalAcceptance: + return FeedbackTerminalAcceptance( + self.metrics, self.settle_timeout, self.consecutive_acceptances + ) + + +@dataclass(frozen=True, slots=True) +class TimedTerminalAcceptance: + """Explicit terminal acceptance without endpoint feedback.""" + + settle_duration: float = 0.0 + + def __post_init__(self) -> None: + object.__setattr__( + self, + "settle_duration", + _non_negative_float(self.settle_duration, field_name="settle_duration"), + ) + + def snapshot(self) -> TimedTerminalAcceptance: + return TimedTerminalAcceptance(self.settle_duration) + + +TerminalAcceptance = FeedbackTerminalAcceptance | TimedTerminalAcceptance + + +@dataclass(frozen=True, slots=True) +class TrackingPolicy: + """Independent in-flight recovery signal and terminal acceptance contract.""" + + in_flight: InFlightTrackingPolicy | None + terminal: TerminalAcceptance + + def __post_init__(self) -> None: + if self.in_flight is not None and not isinstance( + self.in_flight, InFlightTrackingPolicy + ): + raise TypeError("in_flight must be InFlightTrackingPolicy or None.") + if not isinstance( + self.terminal, (FeedbackTerminalAcceptance, TimedTerminalAcceptance) + ): + raise TypeError("terminal must be a terminal-acceptance contract.") + if self.in_flight is not None: + object.__setattr__(self, "in_flight", self.in_flight.snapshot()) + object.__setattr__(self, "terminal", self.terminal.snapshot()) + in_flight = self.in_flight + terminal = self.terminal + if in_flight is not None and isinstance(terminal, FeedbackTerminalAcceptance): + in_flight_by_channel = { + metric.channel_id: metric for metric in in_flight.metrics + } + for terminal_metric in terminal.metrics: + in_flight_metric = in_flight_by_channel.get(terminal_metric.channel_id) + if in_flight_metric is None: + continue + if ( + in_flight_metric.metric_id != terminal_metric.metric_id + or in_flight_metric.revision != terminal_metric.revision + or type(in_flight_metric) is not type(terminal_metric) + ): + raise ValueError( + "In-flight and terminal metrics sharing a channel must " + "use the same exact metric ID, revision, and type." + ) + + def snapshot(self) -> TrackingPolicy: + return TrackingPolicy(self.in_flight, self.terminal) + + @classmethod + def timed(cls, *, settle_duration: float = 0.0) -> TrackingPolicy: + """Create an explicit time-only terminal contract with no tracking.""" + return cls( + in_flight=None, + terminal=TimedTerminalAcceptance(settle_duration=settle_duration), + ) + + @classmethod + def joint_position( + cls, + *, + in_flight_max_abs_error: float = 0.05, + terminal_max_abs_error: float = 0.05, + terminal_settle_timeout: float = 0.5, + consecutive_violations: int = 1, + consecutive_acceptances: int = 1, + grace_period: float = 0.0, + ) -> TrackingPolicy: + """Create the built-in joint-position tracking and acceptance contract.""" + return cls( + in_flight=InFlightTrackingPolicy( + metrics=(JointPositionTrackingMetric(in_flight_max_abs_error),), + consecutive_violations=consecutive_violations, + grace_period=grace_period, + ), + terminal=FeedbackTerminalAcceptance( + metrics=(JointPositionTrackingMetric(terminal_max_abs_error),), + settle_timeout=terminal_settle_timeout, + consecutive_acceptances=consecutive_acceptances, + ), + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class TrackingSetpoint: + """One endpoint-local desired state and its typed feedback route.""" + + endpoint_key: tuple[str, str] + binding: EndpointTrackingChannelBinding + desired: TrackingState + + def __post_init__(self) -> None: + if not isinstance(self.endpoint_key, tuple) or len(self.endpoint_key) != 2: + raise TypeError("endpoint_key must be a (slot_id, endpoint_id) tuple.") + _identifier(self.endpoint_key[0], field_name="endpoint_key.slot_id") + _identifier(self.endpoint_key[1], field_name="endpoint_key.endpoint_id") + if not isinstance(self.binding, EndpointTrackingChannelBinding): + raise TypeError("binding must be an EndpointTrackingChannelBinding.") + if not isinstance(self.desired, TrackingState): + raise TypeError("desired must be a TrackingState.") + if self.binding.channel_id != self.desired.channel_id: + raise ValueError("Binding and desired-state channels must match.") + desired = self.desired.snapshot() + if type(desired) is not type(self.desired) or desired is self.desired: + raise TypeError("TrackingState.snapshot() must own a same-type value.") + object.__setattr__(self, "binding", self.binding.snapshot()) + object.__setattr__(self, "desired", desired) + + @property + def key(self) -> tuple[str, str, str]: + return self.endpoint_key[0], self.endpoint_key[1], self.binding.channel_id + + def snapshot(self) -> TrackingSetpoint: + return TrackingSetpoint(self.endpoint_key, self.binding, self.desired) + + +@dataclass(frozen=True, slots=True) +class TrackingFrame: + """Desired endpoint states associated with one command frame.""" + + setpoints: tuple[TrackingSetpoint, ...] = () + + def __post_init__(self) -> None: + snapshots: list[TrackingSetpoint] = [] + keys: set[tuple[str, str, str]] = set() + for setpoint in self.setpoints: + if not isinstance(setpoint, TrackingSetpoint): + raise TypeError("setpoints must contain TrackingSetpoint values.") + if setpoint.key in keys: + raise ValueError(f"Duplicate tracking setpoint {setpoint.key!r}.") + keys.add(setpoint.key) + snapshots.append(setpoint.snapshot()) + object.__setattr__(self, "setpoints", tuple(snapshots)) + + def snapshot(self) -> TrackingFrame: + return TrackingFrame(self.setpoints) + + +@dataclass(frozen=True, slots=True) +class TimedTrackingSequence: + """Tracking frames aligned by index with an authoritative command sequence.""" + + env_ids: torch.Tensor + frames: tuple[TrackingFrame, ...] + + def __post_init__(self) -> None: + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if ( + self.env_ids.dtype != torch.long + or self.env_ids.dim() != 1 + or self.env_ids.numel() < 1 + ): + raise ValueError("env_ids must be a non-empty one-dimensional long tensor.") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + frames: list[TrackingFrame] = [] + for frame in self.frames: + if not isinstance(frame, TrackingFrame): + raise TypeError("frames must contain TrackingFrame values.") + snapshot = frame.snapshot() + for setpoint in snapshot.setpoints: + if setpoint.desired.batch_size != self.env_ids.numel(): + raise ValueError("Every setpoint batch must match env_ids.") + if setpoint.desired.device != self.env_ids.device: + raise ValueError("Every setpoint and env_ids must share a device.") + frames.append(snapshot) + object.__setattr__(self, "env_ids", self.env_ids.clone()) + object.__setattr__(self, "frames", tuple(frames)) + + @property + def batch_size(self) -> int: + """Return the represented environment count.""" + return int(self.env_ids.numel()) + + @property + def device(self) -> torch.device: + """Return the sequence tensor device.""" + return self.env_ids.device + + @property + def frame_count(self) -> int: + """Return the number of command-aligned tracking frames.""" + return len(self.frames) + + def snapshot(self) -> TimedTrackingSequence: + return TimedTrackingSequence(self.env_ids, self.frames) + + +@dataclass(frozen=True, slots=True, eq=False) +class TrackingFeedbackBatch: + """One synchronized typed observation from an exact feedback source.""" + + source: TrackingFeedbackSourceRef + state: TrackingState + valid_mask: torch.Tensor + timestamp: float + + def __post_init__(self) -> None: + if not isinstance(self.source, TrackingFeedbackSourceRef): + raise TypeError("source must be a TrackingFeedbackSourceRef.") + if not isinstance(self.state, TrackingState): + raise TypeError("state must be a TrackingState.") + if self.valid_mask.dtype != torch.bool or self.valid_mask.shape != ( + self.state.batch_size, + ): + raise ValueError("valid_mask must have shape (batch_size,) and bool dtype.") + if self.valid_mask.device != self.state.device: + raise ValueError("valid_mask and state must share a device.") + object.__setattr__(self, "source", self.source.snapshot()) + object.__setattr__(self, "state", self.state.snapshot()) + object.__setattr__(self, "valid_mask", self.valid_mask.clone()) + object.__setattr__( + self, + "timestamp", + _non_negative_float(self.timestamp, field_name="timestamp"), + ) + + def snapshot(self) -> TrackingFeedbackBatch: + return TrackingFeedbackBatch( + self.source, self.state, self.valid_mask, self.timestamp + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class TrackingEvaluation: + """Per-row metric result with unit-preserving component errors.""" + + channel_id: TrackingChannelId + accepted_mask: torch.Tensor + valid_mask: torch.Tensor + normalized_error: torch.Tensor + component_errors: Mapping[str, torch.Tensor] = field(default_factory=dict) + + def __post_init__(self) -> None: + _identifier(self.channel_id, field_name="channel_id") + expected = self.accepted_mask.shape + if self.accepted_mask.dtype != torch.bool or self.accepted_mask.dim() != 1: + raise ValueError("accepted_mask must be a one-dimensional bool tensor.") + if self.valid_mask.dtype != torch.bool or self.valid_mask.shape != expected: + raise ValueError("valid_mask must match accepted_mask with bool dtype.") + if self.normalized_error.shape != expected or not torch.is_floating_point( + self.normalized_error + ): + raise ValueError("normalized_error must be a floating tensor per row.") + if not ( + self.accepted_mask.device + == self.valid_mask.device + == self.normalized_error.device + ): + raise ValueError("Evaluation tensors must share a device.") + components: dict[str, torch.Tensor] = {} + for name, value in self.component_errors.items(): + _identifier(name, field_name="component_errors key") + if value.shape != expected or value.device != self.normalized_error.device: + raise ValueError("Every component error must be a per-row tensor.") + components[name] = value.clone() + object.__setattr__(self, "accepted_mask", self.accepted_mask.clone()) + object.__setattr__(self, "valid_mask", self.valid_mask.clone()) + object.__setattr__(self, "normalized_error", self.normalized_error.clone()) + object.__setattr__(self, "component_errors", MappingProxyType(components)) + + def snapshot(self) -> TrackingEvaluation: + return TrackingEvaluation( + self.channel_id, + self.accepted_mask, + self.valid_mask, + self.normalized_error, + self.component_errors, + ) + + +class TrackingFeedbackProvider(Protocol): + """Versioned live port that reads one exact tracking source.""" + + provider_id: str + revision: str + + def observe( + self, source: TrackingFeedbackSourceRef, context: PlanningContext + ) -> TrackingFeedbackBatch: + """Read one synchronized typed feedback batch.""" + + +class TrackingCommandProjector(Protocol): + """Versioned pure projector from an endpoint command to desired state.""" + + projector_id: str + revision: str + + def project( + self, command: EndpointCommand, binding: EndpointTrackingChannelBinding + ) -> TrackingState: + """Project one command into the binding's desired tracking channel.""" + + +class TrackingMetricEvaluator(Protocol): + """Versioned evaluator for one exact metric configuration type.""" + + metric_id: str + revision: str + metric_type: type[TrackingMetricCfg] + + def evaluate( + self, + desired: TrackingState, + observed: TrackingState, + valid_mask: torch.Tensor, + metric: TrackingMetricCfg, + ) -> TrackingEvaluation: + """Evaluate a desired and observed batch row by row.""" + + +class _ExactRegistry: + __slots__ = ("_values", "_kind") + + def __init__(self, values: Iterable[object], *, kind: str) -> None: + normalized: dict[tuple[str, str], object] = {} + for value in values: + identifier = _identifier( + getattr(value, f"{kind}_id"), field_name=f"{kind}_id" + ) + revision = _identifier(getattr(value, "revision"), field_name="revision") + key = identifier, revision + if key in normalized: + raise ValueError(f"Duplicate {kind} registration {key!r}.") + normalized[key] = value + self._values = MappingProxyType(normalized) + self._kind = kind + + @property + def values(self) -> Mapping[tuple[str, str], object]: + return self._values + + def _resolve(self, identifier: str, revision: str) -> object: + key = identifier, revision + try: + return self._values[key] + except KeyError as exc: + raise KeyError(f"Unknown {self._kind} registration {key!r}.") from exc + + +class TrackingFeedbackProviderRegistry(_ExactRegistry): + """Immutable exact-version feedback-provider registry.""" + + def __init__(self, providers: Iterable[TrackingFeedbackProvider] = ()) -> None: + super().__init__(providers, kind="provider") + + def resolve(self, source: TrackingFeedbackSourceRef) -> TrackingFeedbackProvider: + return self._resolve(source.provider_id, source.revision) # type: ignore[return-value] + + +class TrackingProjectorRegistry(_ExactRegistry): + """Immutable exact-version command-projector registry.""" + + def __init__(self, projectors: Iterable[TrackingCommandProjector] = ()) -> None: + super().__init__(projectors, kind="projector") + + def resolve(self, route: TrackingProjectorRef) -> TrackingCommandProjector: + return self._resolve(route.projector_id, route.revision) # type: ignore[return-value] + + +class TrackingEvaluatorRegistry(_ExactRegistry): + """Immutable exact-version metric-evaluator registry.""" + + def __init__(self, evaluators: Iterable[TrackingMetricEvaluator] = ()) -> None: + super().__init__(evaluators, kind="metric") + + def resolve(self, metric: TrackingMetricCfg) -> TrackingMetricEvaluator: + evaluator = self._resolve(metric.metric_id, metric.revision) + if type(metric) is not evaluator.metric_type: # type: ignore[attr-defined] + raise TypeError( + f"Metric {metric.metric_id!r} requires " + f"{evaluator.metric_type.__name__}." # type: ignore[attr-defined] + ) + return evaluator # type: ignore[return-value] + + +class PlanningContextTrackingFeedbackProvider: + """Built-in provider backed by :class:`PlanningContext.robot`.""" + + provider_id = "planning_context.robot" + revision = "1" + + def observe( + self, source: TrackingFeedbackSourceRef, context: PlanningContext + ) -> TrackingFeedbackBatch: + from .bindings import JointPositionTarget + from .state import PlanningContext + + if not isinstance(context, PlanningContext): + raise TypeError("context must be a PlanningContext.") + address = source.address + if not isinstance(address, EndpointTrackingFeedbackAddress): + raise TypeError( + "Built-in provider requires EndpointTrackingFeedbackAddress." + ) + target = address.target + if address.channel_id == JOINT_POSITION_CHANNEL: + if not isinstance(target, JointPositionTarget): + raise TypeError("joint.position requires a JointPositionTarget.") + state: TrackingState = JointPositionTrackingState( + context.robot.qpos[:, target.joint_ids] + ) + elif address.channel_id == BASE_POSE_CHANNEL: + if context.robot.root_pose is None: + raise RuntimeError("RobotObservation.root_pose is unavailable.") + state = PoseTrackingState(context.robot.root_pose) + elif address.channel_id == WHOLE_BODY_POSE_CHANNEL: + if context.robot.root_pose is None: + raise RuntimeError("RobotObservation.root_pose is unavailable.") + joints = ( + context.robot.qpos[:, target.joint_ids] + if isinstance(target, JointPositionTarget) + else context.robot.qpos + ) + state = WholeBodyPoseTrackingState(context.robot.root_pose, joints) + else: + raise KeyError( + f"Unsupported built-in tracking channel {address.channel_id!r}." + ) + return TrackingFeedbackBatch( + source=source, + state=state, + valid_mask=torch.ones( + context.batch_size, dtype=torch.bool, device=state.device + ), + timestamp=context.robot.timestamp, + ) + + +class JointPositionTrackingProjector: + """Built-in projector for joint-position endpoint commands.""" + + projector_id = "joint_position_payload" + revision = "1" + + def project( + self, command: EndpointCommand, binding: EndpointTrackingChannelBinding + ) -> JointPositionTrackingState: + from .runtime_commands import EndpointCommand, JointPositionPayload + + if not isinstance(command, EndpointCommand): + raise TypeError("command must be an EndpointCommand.") + if binding.channel_id != JOINT_POSITION_CHANNEL: + raise ValueError("Joint projector requires the joint.position channel.") + if not isinstance(command.payload, JointPositionPayload): + raise TypeError("Joint projector requires JointPositionPayload.") + address = binding.source.address + if isinstance(address, EndpointTrackingFeedbackAddress): + if address.target.address_fingerprint != command.target.address_fingerprint: + raise ValueError( + "Command and feedback binding target different endpoints." + ) + return JointPositionTrackingState(command.payload.positions) + + +def _compatible( + desired: TrackingState, + observed: TrackingState, + valid_mask: torch.Tensor, + expected_type: type[TrackingState], +) -> None: + if type(desired) is not expected_type or type(observed) is not expected_type: + raise TypeError(f"Metric requires {expected_type.__name__} values.") + if desired.batch_size != observed.batch_size or desired.device != observed.device: + raise ValueError("Desired and observed batches must match.") + if valid_mask.dtype != torch.bool or valid_mask.shape != (desired.batch_size,): + raise ValueError("valid_mask must be a bool tensor with one value per row.") + if valid_mask.device != desired.device: + raise ValueError("valid_mask and states must share a device.") + + +def _pose_errors( + desired: torch.Tensor, observed: torch.Tensor +) -> tuple[torch.Tensor, torch.Tensor]: + translation = torch.linalg.vector_norm( + desired[:, :3, 3] - observed[:, :3, 3], dim=1 + ) + relative = desired[:, :3, :3].transpose(1, 2) @ observed[:, :3, :3] + cosine = ((relative.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) * 0.5).clamp( + -1.0, 1.0 + ) + return translation, torch.acos(cosine) + + +class JointPositionTrackingEvaluator: + """Evaluator for :class:`JointPositionTrackingMetric`.""" + + metric_id = JointPositionTrackingMetric.metric_id + revision = JointPositionTrackingMetric.revision + metric_type = JointPositionTrackingMetric + + def evaluate(self, desired, observed, valid_mask, metric) -> TrackingEvaluation: + _compatible(desired, observed, valid_mask, JointPositionTrackingState) + if type(metric) is not JointPositionTrackingMetric: + raise TypeError("metric must be JointPositionTrackingMetric.") + if desired.positions.shape != observed.positions.shape: + raise ValueError("Joint-position state shapes must match.") + error = (desired.positions - observed.positions).abs().amax(dim=1) + normalized = error / metric.tolerance + return TrackingEvaluation( + JOINT_POSITION_CHANNEL, + valid_mask & (error <= metric.tolerance), + valid_mask, + normalized, + {"joint_max_abs": error}, + ) + + +class PoseTrackingEvaluator: + """Evaluator for :class:`PoseTrackingMetric`.""" + + metric_id = PoseTrackingMetric.metric_id + revision = PoseTrackingMetric.revision + metric_type = PoseTrackingMetric + + def evaluate(self, desired, observed, valid_mask, metric) -> TrackingEvaluation: + _compatible(desired, observed, valid_mask, PoseTrackingState) + if type(metric) is not PoseTrackingMetric: + raise TypeError("metric must be PoseTrackingMetric.") + translation, rotation = _pose_errors(desired.poses, observed.poses) + normalized = torch.maximum( + translation / metric.translation_tolerance, + rotation / metric.rotation_tolerance, + ) + return TrackingEvaluation( + BASE_POSE_CHANNEL, + valid_mask & (normalized <= 1.0), + valid_mask, + normalized, + {"translation": translation, "rotation": rotation}, + ) + + +class WholeBodyPoseTrackingEvaluator: + """Evaluator for :class:`WholeBodyPoseTrackingMetric`.""" + + metric_id = WholeBodyPoseTrackingMetric.metric_id + revision = WholeBodyPoseTrackingMetric.revision + metric_type = WholeBodyPoseTrackingMetric + + def evaluate(self, desired, observed, valid_mask, metric) -> TrackingEvaluation: + _compatible(desired, observed, valid_mask, WholeBodyPoseTrackingState) + if type(metric) is not WholeBodyPoseTrackingMetric: + raise TypeError("metric must be WholeBodyPoseTrackingMetric.") + if desired.joint_positions.shape != observed.joint_positions.shape: + raise ValueError("Whole-body joint-position shapes must match.") + translation, rotation = _pose_errors(desired.root_poses, observed.root_poses) + joint = (desired.joint_positions - observed.joint_positions).abs().amax(dim=1) + normalized = torch.maximum( + torch.maximum( + translation / metric.translation_tolerance, + rotation / metric.rotation_tolerance, + ), + joint / metric.joint_position_tolerance, + ) + return TrackingEvaluation( + WHOLE_BODY_POSE_CHANNEL, + valid_mask & (normalized <= 1.0), + valid_mask, + normalized, + {"translation": translation, "rotation": rotation, "joint_max_abs": joint}, + ) + + +class TrackingRuntime: + """Runtime facade for projecting commands and evaluating typed feedback.""" + + __slots__ = ("_providers", "_projectors", "_evaluators") + + def __init__( + self, + providers: TrackingFeedbackProviderRegistry, + projectors: TrackingProjectorRegistry, + evaluators: TrackingEvaluatorRegistry, + ) -> None: + if type(providers) is not TrackingFeedbackProviderRegistry: + raise TypeError( + "providers must be exactly TrackingFeedbackProviderRegistry." + ) + if type(projectors) is not TrackingProjectorRegistry: + raise TypeError("projectors must be exactly TrackingProjectorRegistry.") + if type(evaluators) is not TrackingEvaluatorRegistry: + raise TypeError("evaluators must be exactly TrackingEvaluatorRegistry.") + self._providers = providers + self._projectors = projectors + self._evaluators = evaluators + + @property + def providers(self) -> TrackingFeedbackProviderRegistry: + """Return the immutable exact-version provider registry.""" + return self._providers + + @property + def projectors(self) -> TrackingProjectorRegistry: + """Return the immutable exact-version projector registry.""" + return self._projectors + + @property + def evaluators(self) -> TrackingEvaluatorRegistry: + """Return the immutable exact-version evaluator registry.""" + return self._evaluators + + @classmethod + def with_builtins(cls) -> TrackingRuntime: + """Create a runtime with context feedback and built-in typed metrics.""" + return cls( + TrackingFeedbackProviderRegistry( + [PlanningContextTrackingFeedbackProvider()] + ), + TrackingProjectorRegistry([JointPositionTrackingProjector()]), + TrackingEvaluatorRegistry( + [ + JointPositionTrackingEvaluator(), + PoseTrackingEvaluator(), + WholeBodyPoseTrackingEvaluator(), + ] + ), + ) + + def project( + self, command: EndpointCommand, binding: EndpointTrackingChannelBinding + ) -> TrackingState: + """Project one command through the exact binding-owned projector.""" + return self.projectors.resolve(binding.projector).project(command, binding) + + def observe( + self, setpoint: TrackingSetpoint, context: PlanningContext + ) -> TrackingFeedbackBatch: + """Read the exact feedback source for one setpoint.""" + feedback = self.providers.resolve(setpoint.binding.source).observe( + setpoint.binding.source, context + ) + if ( + feedback.source.source_fingerprint + != setpoint.binding.source.source_fingerprint + ): + raise ValueError("Feedback provider returned a different source.") + if feedback.state.channel_id != setpoint.binding.channel_id: + raise TypeError("Feedback state does not match the bound channel.") + if feedback.timestamp != context.robot.timestamp: + raise ValueError( + "Tracking feedback must use the current planning-context timestamp." + ) + if feedback.state.batch_size != context.batch_size: + raise ValueError("Tracking feedback batch must match the context batch.") + if feedback.state.device != context.robot.qpos.device: + raise ValueError("Tracking feedback and context must share a device.") + return feedback + + def evaluate( + self, + setpoint: TrackingSetpoint, + feedback: TrackingFeedbackBatch, + metric: TrackingMetricCfg, + ) -> TrackingEvaluation: + """Evaluate one observed setpoint with an exact metric implementation.""" + if metric.channel_id != setpoint.binding.channel_id: + raise ValueError("Metric and setpoint channels must match.") + if ( + feedback.source.source_fingerprint + != setpoint.binding.source.source_fingerprint + ): + raise ValueError("Feedback source does not match the setpoint binding.") + return self.evaluators.resolve(metric).evaluate( + setpoint.desired, feedback.state, feedback.valid_mask, metric + ) + + def evaluate_frame( + self, + frame: TrackingFrame, + metrics: Iterable[TrackingMetricCfg], + context: PlanningContext, + ) -> Mapping[tuple[str, str, str], TrackingEvaluation]: + """Observe and evaluate every setpoint required by one frame.""" + by_channel = {metric.channel_id: metric for metric in metrics} + results: dict[tuple[str, str, str], TrackingEvaluation] = {} + for setpoint in frame.setpoints: + try: + metric = by_channel[setpoint.binding.channel_id] + except KeyError as exc: + raise KeyError( + f"No metric configured for channel {setpoint.binding.channel_id!r}." + ) from exc + results[setpoint.key] = self.evaluate( + setpoint, self.observe(setpoint, context), metric + ) + return MappingProxyType(results) + + +__all__ = [ + "BASE_POSE_CHANNEL", + "FeedbackTerminalAcceptance", + "InFlightTrackingPolicy", + "JOINT_POSITION_CHANNEL", + "JointPositionTrackingEvaluator", + "JointPositionTrackingMetric", + "JointPositionTrackingProjector", + "JointPositionTrackingState", + "EndpointTrackingChannelBinding", + "EndpointTrackingFeedbackAddress", + "PlanningContextTrackingFeedbackProvider", + "PoseTrackingEvaluator", + "PoseTrackingMetric", + "PoseTrackingState", + "TerminalAcceptance", + "TimedTerminalAcceptance", + "TimedTrackingSequence", + "TrackingChannelId", + "TrackingCommandProjector", + "TrackingEvaluation", + "TrackingEvaluatorRegistry", + "TrackingFeedbackAddress", + "TrackingFeedbackBatch", + "TrackingFeedbackProvider", + "TrackingFeedbackProviderRegistry", + "TrackingFeedbackSourceRef", + "TrackingFrame", + "TrackingMetricCfg", + "TrackingMetricEvaluator", + "TrackingPolicy", + "TrackingProjectorRef", + "TrackingProjectorRegistry", + "TrackingRuntime", + "TrackingSetpoint", + "TrackingState", + "WHOLE_BODY_POSE_CHANNEL", + "WholeBodyPoseTrackingEvaluator", + "WholeBodyPoseTrackingMetric", + "WholeBodyPoseTrackingState", +] diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 576d2f24a..9bd87c54a 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -200,6 +200,7 @@ ResolvedCorePolicyTrace, SkillCallTrace, SkillEndpointBindingTrace, + SkillEndpointTrackingChannelTrace, SkillEffectTrace, SkillFailure, SkillPlanAttemptTrace, @@ -367,6 +368,7 @@ "SkillPolicyPreset", "SkillCallTrace", "SkillEndpointBindingTrace", + "SkillEndpointTrackingChannelTrace", "SkillEffectTrace", "SkillFailure", "SkillPlanAttemptTrace", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 1c463ba6f..d9c1b814d 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -1052,6 +1052,7 @@ def ground( goal=lowering.goal, binding=bound.binding.action_binding, motion_policy=bound.preset.motion_policy, + tracking_policy=bound.preset.tracking_policy, recovery_policy=bound.preset.recovery_policy, skill_options=lowering.skill_options, control_overrides=lowering.control_overrides, diff --git a/embodichain/lab/sim/skills/integration.py b/embodichain/lab/sim/skills/integration.py index 08c1bdb6e..3040d9469 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -1216,6 +1216,7 @@ def link_call( preset.motion_policy, dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), + tracking_policy=preset.tracking_policy, recovery_policy=preset.recovery_policy, runner_cfg=preset.runner_cfg, effect_monitors=preset.effect_monitors, diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 290799dc1..893b54bb6 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -38,6 +38,14 @@ ) from embodichain.lab.sim.atomic_actions.core import SkillDescriptor from embodichain.lab.sim.atomic_actions.policies import MotionPolicy, RecoveryPolicy +from embodichain.lab.sim.atomic_actions.tracking import ( + JOINT_POSITION_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + TrackingFeedbackSourceRef, + TrackingPolicy, + TrackingProjectorRef, +) from embodichain.lab.sim.atomic_actions.requirements import ( BATCH_INVERSE_KINEMATICS_CAPABILITY, DisjointResourceSlots, @@ -171,6 +179,37 @@ def _snapshot_effect_sources( return MappingProxyType(snapshots) +def _snapshot_tracking_channels( + values: Mapping[str, EndpointTrackingChannelBinding], + *, + field_name: str, +) -> Mapping[str, EndpointTrackingChannelBinding]: + """Validate, own, and freeze endpoint tracking bindings by channel.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + snapshots: dict[str, EndpointTrackingChannelBinding] = {} + for channel_id, binding in values.items(): + _validate_identifier(channel_id, field_name=f"{field_name} channel IDs") + if not isinstance(binding, EndpointTrackingChannelBinding): + raise TypeError( + f"{field_name} values must be EndpointTrackingChannelBinding " + "instances." + ) + if binding.channel_id != channel_id: + raise ValueError( + f"{field_name}[{channel_id!r}] disagrees with binding channel " + f"{binding.channel_id!r}." + ) + snapshot = binding.snapshot() + if snapshot is binding: + raise TypeError( + f"{field_name}[{channel_id!r}].snapshot() must return an " + "independent channel binding." + ) + snapshots[channel_id] = snapshot + return MappingProxyType(snapshots) + + @dataclass(frozen=True, slots=True, kw_only=True) class ResourceEndpoint(ABC): """Extensible execution endpoint in a robot resource graph. @@ -242,6 +281,11 @@ class EndpointResolution: effect_sources: Mapping[str, EffectEvidenceSourceRef] = field(default_factory=dict) """Provider-routed raw observation sources keyed by open channel ID.""" + tracking_channels: Mapping[str, EndpointTrackingChannelBinding] = field( + default_factory=dict + ) + """Typed feedback source and desired-state projector by channel ID.""" + command_profile_key: str | None = None """Profile key that owns semantic commands for this endpoint, when any.""" @@ -293,6 +337,14 @@ def __post_init__(self) -> None: field_name="EndpointResolution.effect_sources", ), ) + object.__setattr__( + self, + "tracking_channels", + _snapshot_tracking_channels( + self.tracking_channels, + field_name="EndpointResolution.tracking_channels", + ), + ) if self.command_profile_key is not None: _validate_identifier( self.command_profile_key, @@ -435,11 +487,24 @@ def resolve( FORCE_EFFECT_CHANNEL, } ) - return EndpointResolution( - runtime_target=JointPositionTarget( - control_part=endpoint.control_part, - joint_ids=joint_ids, + runtime_target = JointPositionTarget( + control_part=endpoint.control_part, + joint_ids=joint_ids, + ) + tracking_channel = EndpointTrackingChannelBinding( + JOINT_POSITION_CHANNEL, + TrackingFeedbackSourceRef( + "planning_context.robot", + "1", + EndpointTrackingFeedbackAddress( + runtime_target, + JOINT_POSITION_CHANNEL, + ), ), + TrackingProjectorRef("joint_position_payload", "1"), + ) + return EndpointResolution( + runtime_target=runtime_target, command_profile_key=( endpoint.control_part if endpoint.command_profile is None @@ -454,6 +519,7 @@ def resolve( ) for channel in sorted(effect_channels) }, + tracking_channels={JOINT_POSITION_CHANNEL: tracking_channel}, claim_tokens=frozenset({f"robot.control_part:{endpoint.control_part}"}), joint_ids=joint_ids, ) @@ -468,6 +534,9 @@ class ResolvedResourceEndpoint: runtime_target: RuntimeEndpointTarget task_state_key: str | None = None effect_sources: Mapping[str, EffectEvidenceSourceRef] = field(default_factory=dict) + tracking_channels: Mapping[str, EndpointTrackingChannelBinding] = field( + default_factory=dict + ) command_profile_key: str | None = None requires_command_profile: bool = False commands: Mapping[str, ControlCommand] = field(default_factory=dict) @@ -496,6 +565,7 @@ def __post_init__(self) -> None: runtime_target=self.runtime_target, task_state_key=self.task_state_key, effect_sources=self.effect_sources, + tracking_channels=self.tracking_channels, command_profile_key=self.command_profile_key, requires_command_profile=self.requires_command_profile, claim_tokens=self.claim_tokens, @@ -510,6 +580,7 @@ def __post_init__(self) -> None: ) object.__setattr__(self, "task_state_key", resolved_state_key) object.__setattr__(self, "effect_sources", resolution.effect_sources) + object.__setattr__(self, "tracking_channels", resolution.tracking_channels) object.__setattr__( self, "command_profile_key", @@ -646,13 +717,14 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True, init=False) class SkillPolicyPreset: - """Versioned planning, recovery, runner, and effect-monitor bundle.""" + """Versioned planning, tracking, recovery, runner, and monitor bundle.""" preset_id: str schema_version: int required_planner: str | None """Optional planner backend required by this preset.""" _motion_policy: MotionPolicy + _tracking_policy: TrackingPolicy _recovery_policy: RecoveryPolicy _runner_cfg: ExecutionRunnerCfg _effect_monitors: Mapping[str, EffectMonitorRef] @@ -662,6 +734,7 @@ def __init__( preset_id: str, schema_version: int = 1, motion_policy: MotionPolicy | None = None, + tracking_policy: TrackingPolicy | None = None, recovery_policy: RecoveryPolicy | None = None, runner_cfg: ExecutionRunnerCfg | None = None, effect_monitors: Mapping[str, EffectMonitorRef] | None = None, @@ -682,12 +755,19 @@ def __init__( field_name="SkillPolicyPreset.required_planner", ) selected_motion = MotionPolicy() if motion_policy is None else motion_policy + selected_tracking = ( + TrackingPolicy.joint_position() + if tracking_policy is None + else tracking_policy + ) selected_recovery = ( RecoveryPolicy() if recovery_policy is None else 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.") + if not isinstance(selected_tracking, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") if not isinstance(selected_recovery, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") if not isinstance(selected_runner, ExecutionRunnerCfg): @@ -725,6 +805,7 @@ def __init__( object.__setattr__(self, "schema_version", schema_version) object.__setattr__(self, "required_planner", required_planner) 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, "_runner_cfg", deepcopy(selected_runner)) object.__setattr__( @@ -743,6 +824,11 @@ def recovery_policy(self) -> RecoveryPolicy: """Return an independently owned recovery policy.""" return deepcopy(self._recovery_policy) + @property + def tracking_policy(self) -> TrackingPolicy: + """Return independently owned endpoint-tracking settings.""" + return deepcopy(self._tracking_policy) + @property def runner_cfg(self) -> ExecutionRunnerCfg: """Return an independently owned runner configuration.""" @@ -764,6 +850,7 @@ def snapshot(self) -> SkillPolicyPreset: preset_id=self.preset_id, schema_version=self.schema_version, motion_policy=self.motion_policy, + tracking_policy=self.tracking_policy, recovery_policy=self.recovery_policy, runner_cfg=self.runner_cfg, effect_monitors=self.effect_monitors, @@ -1615,6 +1702,7 @@ def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: else resolution.task_state_key ), effect_sources=resolution.effect_sources, + tracking_channels=resolution.tracking_channels, command_profile_key=resolution.command_profile_key, requires_command_profile=resolution.requires_command_profile, commands=( @@ -1984,6 +2072,7 @@ def _lower_binding( adapter_id=endpoint.adapter_id, target=endpoint.runtime_target, task_state_key=endpoint.task_state_key, + tracking_channels=endpoint.tracking_channels, capabilities=endpoint.capabilities, commands=endpoint.commands, claim_tokens=endpoint.claim_tokens, diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index 0c9d90547..0fe00c182 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -19,7 +19,7 @@ from __future__ import annotations from collections.abc import Iterable, Mapping -from dataclasses import dataclass, replace +from dataclasses import dataclass, fields, is_dataclass, replace from enum import Enum import math from types import MappingProxyType @@ -35,7 +35,7 @@ ExecutionEvent, ExecutionPlanAttempt, ) -from ..atomic_actions.plans import ExecutionFeedbackMode, TrajectorySegment +from ..atomic_actions.plans import TrajectorySegment from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy from ..atomic_actions.runner import ( CommandSink, @@ -48,6 +48,12 @@ RunnerStep, ) from ..atomic_actions.state import PlanningContext, TaskState +from ..atomic_actions.tracking import ( + FeedbackTerminalAcceptance, + TimedTrackingSequence, + TrackingMetricCfg, + TrackingPolicy, +) from .calls import SemanticCallSpec from .compiler import SemanticSkillCompiler from .effects import ( @@ -109,6 +115,8 @@ def _metadata_value(value: object, *, depth: int = 0) -> object: return _metadata_value(value.detach().cpu().tolist(), depth=depth + 1) if isinstance(value, torch.device): return str(value) + if isinstance(value, type): + return {"__type__": f"{value.__module__}.{value.__qualname__}"} if isinstance(value, Mapping): items = sorted(value.items(), key=lambda item: str(item[0])) if all(type(key) is str and key and key == key.strip() for key, _ in items): @@ -134,6 +142,17 @@ def _metadata_value(value: object, *, depth: int = 0) -> object: return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} +def _freeze_metadata_value(value: object) -> object: + """Recursively freeze already JSON-safe metadata for immutable traces.""" + if isinstance(value, dict): + return MappingProxyType( + {key: _freeze_metadata_value(nested) for key, nested in value.items()} + ) + if isinstance(value, list): + return tuple(_freeze_metadata_value(nested) for nested in value) + return value + + def _snapshot_metadata_mapping(value: Mapping[str, object]) -> Mapping[str, object]: """Own one JSON-safe string-keyed metadata mapping.""" if not isinstance(value, Mapping): @@ -217,6 +236,59 @@ class SkillStatus(str, Enum): CANCELLED = "cancelled" +@dataclass(frozen=True, slots=True) +class SkillEndpointTrackingChannelTrace: + """Stable provider and projector route for one endpoint feedback channel.""" + + channel_id: str + provider_id: str + provider_revision: str + projector_id: str + projector_revision: str + feedback_address_type: str + address_fingerprint: object + route_fingerprint: object + + def __post_init__(self) -> None: + for name in ( + "channel_id", + "provider_id", + "provider_revision", + "projector_id", + "projector_revision", + "feedback_address_type", + ): + if type(getattr(self, name)) is not str or not getattr(self, name): + raise ValueError(f"{name} must be a non-empty string.") + object.__setattr__( + self, + "address_fingerprint", + _freeze_metadata_value(_metadata_value(self.address_fingerprint)), + ) + object.__setattr__( + self, + "route_fingerprint", + _freeze_metadata_value(_metadata_value(self.route_fingerprint)), + ) + + def to_metadata(self) -> dict[str, object]: + """Return the exact immutable tracking route without live objects.""" + return { + "channel_id": self.channel_id, + "feedback_source": { + "provider_id": self.provider_id, + "revision": self.provider_revision, + "address_type": self.feedback_address_type, + "address_fingerprint": _metadata_value(self.address_fingerprint), + }, + "projector": { + "projector_id": self.projector_id, + "revision": self.projector_revision, + }, + "route_fingerprint": _metadata_value(self.route_fingerprint), + } + + @dataclass(frozen=True, slots=True) class SkillEndpointBindingTrace: """JSON-safe typed projection of one resolved execution endpoint.""" @@ -231,6 +303,7 @@ class SkillEndpointBindingTrace: task_state_key: str capabilities: tuple[str, ...] command_ids: tuple[str, ...] + tracking_channels: tuple[SkillEndpointTrackingChannelTrace, ...] claim_tokens: tuple[str, ...] joint_ids: tuple[int, ...] @@ -255,6 +328,21 @@ def __post_init__(self) -> None: ): raise ValueError(f"{name} must contain sorted unique identifiers.") object.__setattr__(self, name, values) + tracking_channels = tuple(self.tracking_channels) + if not all( + type(value) is SkillEndpointTrackingChannelTrace + for value in tracking_channels + ): + raise TypeError( + "tracking_channels must contain exact " + "SkillEndpointTrackingChannelTrace values." + ) + channel_ids = tuple(value.channel_id for value in tracking_channels) + if tuple(sorted(set(channel_ids))) != channel_ids: + raise ValueError( + "tracking_channels must use sorted unique channel identifiers." + ) + object.__setattr__(self, "tracking_channels", tracking_channels) joint_ids = tuple(self.joint_ids) if len(set(joint_ids)) != len(joint_ids) or not all( type(value) is int and value >= 0 for value in joint_ids @@ -279,6 +367,22 @@ def from_binding(cls, binding: EndpointBinding) -> SkillEndpointBindingTrace: task_state_key=binding.task_state_key, capabilities=tuple(sorted(binding.capabilities)), command_ids=tuple(sorted(binding.commands)), + tracking_channels=tuple( + SkillEndpointTrackingChannelTrace( + channel_id=channel_id, + provider_id=channel.source.provider_id, + provider_revision=channel.source.revision, + projector_id=channel.projector.projector_id, + projector_revision=channel.projector.revision, + feedback_address_type=( + f"{type(channel.source.address).__module__}." + f"{type(channel.source.address).__qualname__}" + ), + address_fingerprint=(channel.source.address.address_fingerprint), + route_fingerprint=channel.route_fingerprint, + ) + for channel_id, channel in sorted(binding.tracking_channels.items()) + ), claim_tokens=tuple(sorted(binding.claim_tokens)), joint_ids=binding.joint_ids, ) @@ -296,6 +400,9 @@ def to_metadata(self) -> dict[str, object]: "task_state_key": self.task_state_key, "capabilities": list(self.capabilities), "command_ids": list(self.command_ids), + "tracking_channels": [ + channel.to_metadata() for channel in self.tracking_channels + ], "claim_tokens": list(self.claim_tokens), "joint_ids": list(self.joint_ids), } @@ -328,7 +435,6 @@ def _recovery_policy_to_metadata(policy: RecoveryPolicy) -> dict[str, object]: return { "max_replans": policy.max_replans, "max_action_retries": policy.max_action_retries, - "tracking_error_threshold": _metadata_value(policy.tracking_error_threshold), "goal_translation_threshold": _metadata_value( policy.goal_translation_threshold ), @@ -337,6 +443,97 @@ def _recovery_policy_to_metadata(policy: RecoveryPolicy) -> dict[str, object]: } +def _tracking_metric_to_metadata(metric: TrackingMetricCfg) -> dict[str, object]: + """Serialize one exact typed metric and its unit-preserving tolerances.""" + parameters = ( + { + value.name: _metadata_value(getattr(metric, value.name)) + for value in fields(metric) + } + if is_dataclass(metric) + else {} + ) + return { + "metric_id": metric.metric_id, + "revision": metric.revision, + "channel_id": metric.channel_id, + "type": f"{type(metric).__module__}.{type(metric).__qualname__}", + "parameters": parameters, + } + + +def _tracking_policy_to_metadata(policy: TrackingPolicy) -> dict[str, object]: + """Serialize independent in-flight and terminal tracking contracts.""" + in_flight = policy.in_flight + terminal = policy.terminal + return { + "in_flight": ( + None + if in_flight is None + else { + "metrics": [ + _tracking_metric_to_metadata(metric) for metric in in_flight.metrics + ], + "consecutive_violations": in_flight.consecutive_violations, + "grace_period": _metadata_value(in_flight.grace_period), + } + ), + "terminal": ( + { + "mode": "feedback", + "metrics": [ + _tracking_metric_to_metadata(metric) for metric in terminal.metrics + ], + "settle_timeout": _metadata_value(terminal.settle_timeout), + "consecutive_acceptances": terminal.consecutive_acceptances, + } + if isinstance(terminal, FeedbackTerminalAcceptance) + else { + "mode": "timed", + "settle_duration": _metadata_value(terminal.settle_duration), + } + ), + } + + +def _tracking_sequence_to_metadata( + sequence: TimedTrackingSequence | None, +) -> dict[str, object] | None: + """Serialize the provider/projector shape of one plan-owned contract.""" + if sequence is None: + return None + first_frame = None if not sequence.frames else sequence.frames[0] + return { + "env_ids": _metadata_value(sequence.env_ids), + "frame_count": sequence.frame_count, + "setpoints": [ + { + "endpoint": list(setpoint.endpoint_key), + "channel_id": setpoint.binding.channel_id, + "state_type": ( + f"{type(setpoint.desired).__module__}." + f"{type(setpoint.desired).__qualname__}" + ), + "feedback_source": { + "provider_id": setpoint.binding.source.provider_id, + "revision": setpoint.binding.source.revision, + "address_fingerprint": _metadata_value( + setpoint.binding.source.address.address_fingerprint + ), + }, + "projector": { + "projector_id": setpoint.binding.projector.projector_id, + "revision": setpoint.binding.projector.revision, + }, + "route_fingerprint": _metadata_value( + setpoint.binding.route_fingerprint + ), + } + for setpoint in (() if first_frame is None else first_frame.setpoints) + ], + } + + @dataclass(frozen=True, slots=True) class ResolvedCorePolicyTrace: """Resolved preset, core policies, and execution binding for one plan.""" @@ -345,6 +542,7 @@ class ResolvedCorePolicyTrace: preset_id: str preset_schema_version: int motion_policy: MotionPolicy + tracking_policy: TrackingPolicy recovery_policy: RecoveryPolicy endpoints: tuple[SkillEndpointBindingTrace, ...] @@ -360,6 +558,8 @@ def __post_init__(self) -> None: raise ValueError("preset_schema_version must be a positive integer.") if not isinstance(self.motion_policy, MotionPolicy): raise TypeError("motion_policy must be a MotionPolicy.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") endpoints = tuple(self.endpoints) @@ -371,6 +571,7 @@ def __post_init__(self) -> None: if len(set(keys)) != len(keys): raise ValueError("endpoints must use unique slot/endpoint keys.") object.__setattr__(self, "motion_policy", replace(self.motion_policy)) + object.__setattr__(self, "tracking_policy", self.tracking_policy.snapshot()) object.__setattr__(self, "recovery_policy", replace(self.recovery_policy)) object.__setattr__(self, "endpoints", endpoints) @@ -382,6 +583,7 @@ def from_resolved_binding( preset_id: str, preset_schema_version: int, motion_policy: MotionPolicy, + tracking_policy: TrackingPolicy, recovery_policy: RecoveryPolicy, endpoints: Iterable[EndpointBinding], ) -> ResolvedCorePolicyTrace: @@ -391,6 +593,7 @@ def from_resolved_binding( preset_id=preset_id, preset_schema_version=preset_schema_version, motion_policy=motion_policy, + tracking_policy=tracking_policy, recovery_policy=recovery_policy, endpoints=tuple( SkillEndpointBindingTrace.from_binding(endpoint) @@ -405,6 +608,7 @@ def snapshot(self) -> ResolvedCorePolicyTrace: preset_id=self.preset_id, preset_schema_version=self.preset_schema_version, motion_policy=self.motion_policy, + tracking_policy=self.tracking_policy, recovery_policy=self.recovery_policy, endpoints=self.endpoints, ) @@ -418,6 +622,7 @@ def to_metadata(self) -> dict[str, object]: "schema_version": self.preset_schema_version, }, "motion_policy": _motion_policy_to_metadata(self.motion_policy), + "tracking_policy": _tracking_policy_to_metadata(self.tracking_policy), "recovery_policy": _recovery_policy_to_metadata(self.recovery_policy), "endpoints": [endpoint.to_metadata() for endpoint in self.endpoints], } @@ -451,7 +656,8 @@ class SkillPlanAttemptTrace: scene_dependency_monitor_until: Mapping[str, int] collision_world_sensitive: bool replannable: bool - feedback_mode: ExecutionFeedbackMode + tracking_policy: TrackingPolicy + tracking: TimedTrackingSequence | None effect_verification_kind: str | None resolved_core_policy: ResolvedCorePolicyTrace planner_backend: str @@ -541,8 +747,12 @@ def __post_init__(self) -> None: raise TypeError("collision_world_sensitive must be a bool.") if type(self.replannable) is not bool: raise TypeError("replannable must be a bool.") - if not isinstance(self.feedback_mode, ExecutionFeedbackMode): - raise TypeError("feedback_mode must be an ExecutionFeedbackMode.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") + if self.tracking is not None and not isinstance( + self.tracking, TimedTrackingSequence + ): + raise TypeError("tracking must be a TimedTrackingSequence or None.") if self.effect_verification_kind is not None and ( type(self.effect_verification_kind) is not str or not self.effect_verification_kind @@ -573,6 +783,9 @@ def __post_init__(self) -> None: "scene_dependency_monitor_until", MappingProxyType(monitor_until), ) + object.__setattr__(self, "tracking_policy", self.tracking_policy.snapshot()) + if self.tracking is not None: + object.__setattr__(self, "tracking", self.tracking.snapshot()) object.__setattr__( self, "resolved_core_policy", @@ -619,7 +832,8 @@ def from_execution_attempt( scene_dependency_monitor_until=plan.scene_dependency_monitor_until, collision_world_sensitive=plan.collision_world_sensitive, replannable=plan.replannable, - feedback_mode=plan.feedback_mode, + tracking_policy=plan.tracking_policy, + tracking=plan.tracking, effect_verification_kind=( None if plan.effect_verification is None @@ -630,6 +844,7 @@ def from_execution_attempt( preset_id=preset_id, preset_schema_version=preset_schema_version, motion_policy=request.motion_policy, + tracking_policy=request.tracking_policy, recovery_policy=request.recovery_policy, endpoints=request.binding.endpoints, ), @@ -660,7 +875,8 @@ def snapshot(self) -> SkillPlanAttemptTrace: scene_dependency_monitor_until=self.scene_dependency_monitor_until, collision_world_sensitive=self.collision_world_sensitive, replannable=self.replannable, - feedback_mode=self.feedback_mode, + tracking_policy=self.tracking_policy, + tracking=self.tracking, effect_verification_kind=self.effect_verification_kind, resolved_core_policy=self.resolved_core_policy, planner_backend=self.planner_backend, @@ -705,7 +921,8 @@ def to_metadata(self) -> dict[str, object]: }, "collision_world_sensitive": self.collision_world_sensitive, "replannable": self.replannable, - "feedback_mode": self.feedback_mode.value, + "tracking_policy": _tracking_policy_to_metadata(self.tracking_policy), + "tracking_contract": _tracking_sequence_to_metadata(self.tracking), "effect_verification_kind": self.effect_verification_kind, "resolved_core_policy": self.resolved_core_policy.to_metadata(), "planner_diagnostics": { @@ -2040,6 +2257,11 @@ def _append_preparation_failure_trace( if invocation is None else invocation.motion_policy ), + tracking_policy=( + preset.tracking_policy + if invocation is None + else invocation.tracking_policy + ), recovery_policy=( preset.recovery_policy if invocation is None @@ -2280,6 +2502,7 @@ def cancel( "ResolvedCorePolicyTrace", "SkillCallTrace", "SkillEndpointBindingTrace", + "SkillEndpointTrackingChannelTrace", "SkillEffectTrace", "SkillFailure", "SkillPlanAttemptTrace", diff --git a/tests/gym/envs/expert_program/test_completion_metadata.py b/tests/gym/envs/expert_program/test_completion_metadata.py index eb8eda4cd..3bfc46fac 100644 --- a/tests/gym/envs/expert_program/test_completion_metadata.py +++ b/tests/gym/envs/expert_program/test_completion_metadata.py @@ -165,7 +165,7 @@ def _plan( class _TraceObservationProvider: - """Move one scene dependency after the first installed command frame.""" + """Move the scene once and report accepted commands as observed state.""" def __init__(self, clock: EnvironmentStepClock) -> None: self.clock = clock @@ -177,7 +177,10 @@ def observe(self, task_state: TaskState) -> PlanningContext: pose = torch.eye(4).repeat(BATCH_SIZE, 1, 1) if replanned_scene: pose[:, 0, 3] = 0.25 - qpos = torch.zeros(BATCH_SIZE, ROBOT_DOF) + qpos = torch.full( + (BATCH_SIZE, ROBOT_DOF), + float(min(max(self.calls - 1, 0), 3)), + ) timestamp = self.clock.now() return PlanningContext( robot=RobotObservation( diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 72d7ed079..be26157dc 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -35,10 +35,11 @@ DynamicCollisionMode, EndpointBinding, EndpointCommand, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, EndEffectorPoseGoal, EntityState, EffectVerificationRequirement, - ExecutionFeedbackMode, HeldObjectState, JointPositionPayload, JointPositionTarget, @@ -57,7 +58,15 @@ StateDelta, TaskState, TimedCommandSequence, + TimedTerminalAcceptance, + TimedTrackingSequence, TimedTrajectory, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingPolicy, + TrackingProjectorRef, + TrackingSetpoint, + JointPositionTrackingState, ) from embodichain.lab.sim.atomic_actions.goals import ( _resolve_object_pose, @@ -191,7 +200,8 @@ def _action_plan( *, plan_success: torch.Tensor | None = None, joint_trajectory: TimedTrajectory | None = None, - feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, + tracking_policy: TrackingPolicy | None = None, + tracking: TimedTrackingSequence | None = None, expected_effects: StateDelta | None = None, effect_verification: EffectVerificationRequirement | None = None, diagnostics: PlannerDiagnostics | None = None, @@ -209,12 +219,15 @@ def _action_plan( plan_success=plan_success, commands=commands, recovery_policy=RecoveryPolicy(), + tracking_policy=( + TrackingPolicy.timed() if tracking_policy is None else tracking_policy + ), planned_scene_version=0, planned_collision_world_revision=(0,) * commands.batch_size, diagnostics=( PlannerDiagnostics(backend="test") if diagnostics is None else diagnostics ), - feedback_mode=feedback_mode, + tracking=tracking, joint_trajectory=joint_trajectory, scene_dependencies=scene_dependencies, scene_dependency_monitor_until=( @@ -227,6 +240,41 @@ def _action_plan( ) +def _joint_tracking_sequence( + commands: TimedCommandSequence, +) -> TimedTrackingSequence: + frames: list[TrackingFrame] = [] + for command_frame in commands.frames: + setpoints: list[TrackingSetpoint] = [] + for command in command_frame.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + channel = EndpointTrackingChannelBinding( + channel_id="joint.position", + source=TrackingFeedbackSourceRef( + provider_id="planning_context.robot", + revision="1", + address=EndpointTrackingFeedbackAddress( + target=command.target, + channel_id="joint.position", + ), + ), + projector=TrackingProjectorRef( + projector_id="joint_position_payload", + revision="1", + ), + ) + setpoints.append( + TrackingSetpoint( + endpoint_key=("primary", "motion"), + binding=channel, + desired=JointPositionTrackingState(command.payload.positions), + ) + ) + frames.append(TrackingFrame(tuple(setpoints))) + return TimedTrackingSequence(commands.env_ids, tuple(frames)) + + @pytest.mark.parametrize("kind", ("", " physical", "physical ", 1, True)) def test_effect_verification_requirement_rejects_invalid_kind(kind: object) -> None: with pytest.raises(ValueError, match="kind"): @@ -348,6 +396,7 @@ def _plan( frame_count=1, ), recovery_policy=RecoveryPolicy(), + tracking_policy=TrackingPolicy.timed(), planned_scene_version=context.scene.version, planned_collision_world_revision=(0,) * context.batch_size, diagnostics=PlannerDiagnostics(backend="test"), @@ -778,6 +827,7 @@ def test_build_plan_uses_action_scene_dependency_hook() -> None: goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), binding=ActionBinding(owner_id=engine.binding_owner_id), motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) @@ -837,6 +887,7 @@ def test_build_command_plan_rejects_unbound_runtime_destination() -> None: goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), binding=ActionBinding(owner_id=engine.binding_owner_id), motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) @@ -865,6 +916,7 @@ def test_public_plan_authorizes_raw_action_plan_destinations() -> None: goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), binding=ActionBinding(owner_id=engine.binding_owner_id), motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) @@ -891,6 +943,7 @@ def test_command_target_authorization_rejects_altered_joint_claims() -> None: ), ), motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) @@ -931,6 +984,7 @@ def test_command_target_authorization_rejects_custom_claim_conflicts() -> None: goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), binding=ActionBinding(owner_id="test-engine", endpoints=endpoints), motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) @@ -975,7 +1029,6 @@ def test_action_plan_owns_commands_and_optional_joint_trajectory() -> None: commands, plan_success=plan_success, joint_trajectory=trajectory, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, ) payload = commands.frames[0].commands[0].payload assert isinstance(payload, JointPositionPayload) @@ -1091,7 +1144,8 @@ def test_action_plan_allows_timed_commands_without_joint_trajectory() -> None: assert plan.commands.frame_count == 1 assert plan.joint_trajectory is None - assert plan.feedback_mode is ExecutionFeedbackMode.TIMED + assert isinstance(plan.tracking_policy.terminal, TimedTerminalAcceptance) + assert plan.tracking is None def test_action_plan_rejects_command_device_mismatch() -> None: @@ -1133,7 +1187,6 @@ def test_action_plan_validates_joint_trajectory_against_commands( _action_plan( commands, joint_trajectory=trajectory, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, ) @@ -1152,7 +1205,54 @@ def test_joint_position_plan_rejects_empty_commands_for_successful_rows() -> Non commands, plan_success=torch.tensor([True]), joint_trajectory=trajectory, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + tracking_policy=TrackingPolicy.joint_position(), + tracking=_joint_tracking_sequence(commands), + ) + + +@pytest.mark.parametrize("changed_route", ["source", "projector"]) +def test_tracking_plan_rejects_route_changes_between_frames( + changed_route: str, +) -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ) + tracking = _joint_tracking_sequence(commands) + first_frame, second_frame = tracking.frames + original = second_frame.setpoints[0] + source = original.binding.source + projector = original.binding.projector + if changed_route == "source": + source = TrackingFeedbackSourceRef( + provider_id=source.provider_id, + revision="alternate", + address=source.address, + ) + else: + projector = TrackingProjectorRef( + projector_id=projector.projector_id, + revision="alternate", + ) + changed = TrackingSetpoint( + endpoint_key=original.endpoint_key, + binding=EndpointTrackingChannelBinding( + channel_id=original.binding.channel_id, + source=source, + projector=projector, + ), + desired=original.desired, + ) + changed_tracking = TimedTrackingSequence( + commands.env_ids, + (first_frame, TrackingFrame((changed,))), + ) + + with pytest.raises(ValueError, match="source fingerprint and projector route"): + _action_plan( + commands, + tracking_policy=TrackingPolicy.joint_position(), + tracking=changed_tracking, ) @@ -1170,19 +1270,14 @@ def test_joint_position_plan_allows_empty_commands_when_all_rows_fail() -> None: commands, plan_success=torch.tensor([False]), joint_trajectory=trajectory, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + tracking_policy=TrackingPolicy.joint_position(), + tracking=_joint_tracking_sequence(commands), ) assert plan.commands.frame_count == 0 -@pytest.mark.parametrize( - "feedback_mode", - [ExecutionFeedbackMode.TIMED, ExecutionFeedbackMode.JOINT_POSITION], -) -def test_action_plan_requires_stable_destination_set( - feedback_mode: ExecutionFeedbackMode, -) -> None: +def test_action_plan_requires_stable_destination_set() -> None: env_ids = torch.tensor([4], dtype=torch.long) commands = _command_sequence( env_ids=env_ids, @@ -1192,22 +1287,8 @@ def test_action_plan_requires_stable_destination_set( JointPositionTarget("other_arm", (0, 1)), ), ) - trajectory = ( - TimedTrajectory.from_uniform_step( - torch.tensor([[[1.0, 1.0], [2.0, 2.0]]]), - env_ids=env_ids, - step_dt=0.1, - ) - if feedback_mode is ExecutionFeedbackMode.JOINT_POSITION - else None - ) - with pytest.raises(ValueError, match="same destination set"): - _action_plan( - commands, - joint_trajectory=trajectory, - feedback_mode=feedback_mode, - ) + _action_plan(commands) def test_action_plan_requires_stable_exact_target_type() -> None: @@ -1240,87 +1321,6 @@ def test_action_plan_requires_stable_target_address_fingerprint() -> None: _action_plan(commands) -def test_joint_position_plan_rejects_joint_ids_outside_trajectory() -> None: - env_ids = torch.tensor([4], dtype=torch.long) - commands = _command_sequence( - env_ids=env_ids, - frame_count=1, - targets=(JointPositionTarget("arm", (0, 2)),), - ) - trajectory = TimedTrajectory.from_uniform_step( - torch.ones(1, 1, 2), - env_ids=env_ids, - step_dt=0.1, - ) - - with pytest.raises(ValueError, match="outside joint_trajectory robot_dof"): - _action_plan( - commands, - joint_trajectory=trajectory, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, - ) - - -def test_joint_position_plan_rejects_payload_position_mismatch() -> None: - env_ids = torch.tensor([4], dtype=torch.long) - commands = _command_sequence(env_ids=env_ids, frame_count=1) - trajectory = TimedTrajectory.from_uniform_step( - torch.zeros(1, 1, 2), - env_ids=env_ids, - step_dt=0.1, - ) - - with pytest.raises(ValueError, match="positions.*exactly match"): - _action_plan( - commands, - joint_trajectory=trajectory, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, - ) - - -def test_joint_position_plan_rejects_payload_velocity_presence_mismatch() -> None: - env_ids = torch.tensor([4], dtype=torch.long) - commands = _command_sequence( - env_ids=env_ids, - frame_count=1, - velocities=(torch.zeros(1, 2),), - ) - trajectory = TimedTrajectory.from_uniform_step( - torch.ones(1, 1, 2), - env_ids=env_ids, - step_dt=0.1, - ) - - with pytest.raises(ValueError, match="same presence"): - _action_plan( - commands, - joint_trajectory=trajectory, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, - ) - - -def test_joint_position_plan_rejects_payload_velocity_value_mismatch() -> None: - env_ids = torch.tensor([4], dtype=torch.long) - commands = _command_sequence( - env_ids=env_ids, - frame_count=1, - velocities=(torch.zeros(1, 2),), - ) - trajectory = TimedTrajectory.from_uniform_step( - torch.ones(1, 1, 2), - velocities=torch.ones(1, 1, 2), - env_ids=env_ids, - step_dt=0.1, - ) - - with pytest.raises(ValueError, match="velocities.*exactly match"): - _action_plan( - commands, - joint_trajectory=trajectory, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, - ) - - def test_scene_snapshot_expands_global_collision_world_revision() -> None: pose = torch.eye(4).repeat(2, 1, 1) snapshot = SceneSnapshot( diff --git a/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py index 6200b3596..44d8217bb 100644 --- a/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py +++ b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py @@ -52,6 +52,7 @@ SkillResourceSlot, TaskState, TimedCommandSequence, + TrackingPolicy, ) from embodichain.lab.sim.atomic_actions.invocation import ResolvedActionRequest from embodichain.lab.sim.planners import PlanResult @@ -484,6 +485,7 @@ def test_custom_planar_velocity_endpoint_runs_from_profile_through_router() -> N skill_id="drive_velocity", goal=_DriveGoal(goal_twist), binding=binding, + tracking_policy=TrackingPolicy.timed(), ) clock = _Clock() provider = _Provider(robot, clock) diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 7d6c9d9ea..53efa60ec 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -39,6 +39,8 @@ EndEffectorPoseGoal, EndpointBinding, EndpointCommand, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, EntityState, ExecutionEventKind, ExecutionSession, @@ -66,8 +68,13 @@ StateDelta, TaskState, TimedCommandSequence, + TimedTrackingSequence, TimedTrajectory, - TrajectorySegment, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingPolicy, + TrackingProjectorRef, + TrackingSetpoint, ) from embodichain.lab.sim.common import BatchEntity from embodichain.lab.sim.atomic_actions.goals import resolve_pose_goal @@ -323,9 +330,14 @@ class DestinationSequenceAction(AtomicAction[EndEffectorPoseGoal, ActionOptions] ) ) - def __init__(self, destinations: tuple[str | None, ...]) -> None: + def __init__( + self, + destinations: tuple[str | None, ...], + tracking_provider_revisions: tuple[str | None, ...] | None = None, + ) -> None: super().__init__() self.destinations = destinations + self.tracking_provider_revisions = tracking_provider_revisions self.plan_count = 0 def _plan( @@ -371,7 +383,7 @@ def _plan( device=context.robot.qpos.device, ), ) - return self.build_command_plan( + plan = self.build_command_plan( request, context, success=True, @@ -380,6 +392,35 @@ def _plan( env_ids=context.env_ids, ), ) + if self.tracking_provider_revisions is None: + return plan + provider_revision = self.tracking_provider_revisions[index] + if provider_revision is None or plan.tracking is None: + return plan + original = plan.tracking.frames[0].setpoints[0] + changed = TrackingSetpoint( + endpoint_key=original.endpoint_key, + binding=EndpointTrackingChannelBinding( + channel_id=original.binding.channel_id, + source=TrackingFeedbackSourceRef( + provider_id=original.binding.source.provider_id, + revision=provider_revision, + address=original.binding.source.address, + ), + projector=TrackingProjectorRef( + projector_id=original.binding.projector.projector_id, + revision=original.binding.projector.revision, + ), + ), + desired=original.desired, + ) + return replace( + plan, + tracking=TimedTrackingSequence( + plan.tracking.env_ids, + (TrackingFrame((changed,)),), + ), + ) class UncopyableEntity(BatchEntity): @@ -426,6 +467,7 @@ def _engine(batch_size: int = 1) -> tuple[AtomicActionEngine, DynamicAction]: def _destination_engine( destinations: tuple[str | None, ...], + tracking_provider_revisions: tuple[str | None, ...] | None = None, ) -> tuple[AtomicActionEngine, DestinationSequenceAction]: robot = Mock() robot.device = torch.device("cpu") @@ -443,7 +485,7 @@ def _destination_engine( generator.planner.cfg.planner_type = "stub" generator.supports_dynamic_collision_world = False engine = AtomicActionEngine(generator, load_builtins=False) - action = DestinationSequenceAction(destinations) + action = DestinationSequenceAction(destinations, tracking_provider_revisions) engine.register(action) return engine, action @@ -571,7 +613,6 @@ def _invocation( recovery_policy=RecoveryPolicy( max_replans=max_replans, max_action_retries=max_action_retries, - tracking_error_threshold=0.05, goal_translation_threshold=0.02, action_timeout=action_timeout, ), @@ -919,6 +960,7 @@ def test_request_snapshot_preserves_live_entity_identity() -> None: goal=goal, binding=ActionBinding(owner_id="snapshot-test"), motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) @@ -1087,6 +1129,23 @@ def test_empty_failed_replan_preserves_destination_for_same_target_retry() -> No assert resumed.command.commands[0].target.target_id == "arm_a" +def test_empty_failed_replan_does_not_erase_active_tracking_route() -> None: + engine, action = _destination_engine( + ("first", None, "first"), + tracking_provider_revisions=("1", None, "alternate"), + ) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + session.tick(initial) + + with pytest.raises(ValueError, match="tracking source fingerprints"): + session.tick(_context(0.1, 0.0, 0.3, 1)) + + assert action.plan_count == 3 + + def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: engine, action = _engine() generator = engine.motion_generator @@ -1546,6 +1605,7 @@ def test_session_revision_rejects_changed_target_address_fingerprint() -> None: owner_id=invocation.binding.owner_id, endpoints=(changed_endpoint,), ), + tracking_policy=TrackingPolicy.timed(), revision=1, ) @@ -1560,6 +1620,36 @@ def test_session_revision_rejects_changed_target_address_fingerprint() -> None: assert target.joint_ids == (0, 1) +def test_tracking_continuity_rejection_leaves_revision_state_transactional( + monkeypatch: pytest.MonkeyPatch, +) -> None: + engine, _ = _engine() + invocation = _invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + replacement_context = _context(0.5, 0.2, 0.1, 0) + session = engine.start((invocation,), initial) + revised = replace(invocation, revision=1) + attempt_count = len(session.plan_attempts) + + with monkeypatch.context() as scoped: + scoped.setattr( + session, + "_validate_tracking_continuity", + Mock(side_effect=ValueError("tracking route changed")), + ) + with pytest.raises(ValueError, match="tracking route changed"): + session.revise_current(revised, context=replacement_context) + + assert len(session.plan_attempts) == attempt_count + assert session.active_plan.invocation_revision == 0 + assert session.latest_context.robot.timestamp == pytest.approx(0.0) + + session.revise_current(revised, context=replacement_context) + + assert session.active_plan.invocation_revision == 1 + assert session.latest_context.robot.timestamp == pytest.approx(0.5) + + def test_tracking_error_fails_when_replan_budget_is_zero() -> None: engine, _ = _engine() session = engine.start( @@ -1571,7 +1661,7 @@ def test_tracking_error_fails_when_replan_budget_is_zero() -> None: tick = session.tick(_context(0.1, 1.0, 0.2, 0)) kinds = {event.kind for event in tick.events} - assert ExecutionEventKind.TRACKING_ERROR in kinds + assert ExecutionEventKind.TRACKING_DIVERGED in kinds assert ExecutionEventKind.RECOVERY_EXHAUSTED in kinds assert tick.status is ExecutionStatus.FAILED assert tick.eligible_mask.tolist() == [False] diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index 9b3bd3477..0c217fdb6 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -45,9 +45,11 @@ HeldObjectState, JOINT_POSITION_CAPABILITY, JointPositionPayload, + JointPositionTrackingMetric, JointPositionTarget, MotionPolicy, ObjectSemantics, + PlanningContextTrackingFeedbackProvider, PlanningContext, RecoveryPolicy, ResolvedActionRequest, @@ -62,6 +64,14 @@ StateDelta, TaskState, TimedTrajectory, + TrackingEvaluation, + TrackingEvaluatorRegistry, + TrackingFeedbackBatch, + TrackingFeedbackProviderRegistry, + TrackingFeedbackSourceRef, + TrackingMetricCfg, + TrackingRuntime, + TrackingState, ) BATCH_SIZE = 1 @@ -185,6 +195,66 @@ def cancel( return CommandAcknowledgement.accepted_ack() +class RaisingFeedbackProvider: + """Built-in-source replacement that simulates a provider failure.""" + + provider_id = "planning_context.robot" + revision = "1" + + def observe( + self, + source: TrackingFeedbackSourceRef, + context: PlanningContext, + ) -> TrackingFeedbackBatch: + """Raise instead of returning required feedback.""" + del source, context + raise RuntimeError("provider unavailable") + + +class RaisingJointTrackingEvaluator: + """Joint evaluator replacement that simulates an evaluation failure.""" + + metric_id = JointPositionTrackingMetric.metric_id + revision = JointPositionTrackingMetric.revision + metric_type = JointPositionTrackingMetric + + def evaluate( + self, + desired: TrackingState, + observed: TrackingState, + valid_mask: torch.Tensor, + metric: TrackingMetricCfg, + ) -> TrackingEvaluation: + """Raise instead of evaluating required feedback.""" + del desired, observed, valid_mask, metric + raise RuntimeError("evaluator unavailable") + + +class MaskedFeedbackProvider(PlanningContextTrackingFeedbackProvider): + """Context provider exposing a deterministic per-row validity mask.""" + + def __init__(self, valid_mask: tuple[bool, ...]) -> None: + self.valid_mask = valid_mask + + def observe( + self, + source: TrackingFeedbackSourceRef, + context: PlanningContext, + ) -> TrackingFeedbackBatch: + """Return built-in feedback with selected rows marked invalid.""" + feedback = super().observe(source, context) + return TrackingFeedbackBatch( + source=feedback.source, + state=feedback.state, + valid_mask=torch.tensor( + self.valid_mask, + dtype=torch.bool, + device=feedback.state.device, + ), + timestamp=feedback.timestamp, + ) + + class TimedAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): """Test action with explicit non-uniform command intervals.""" @@ -268,6 +338,7 @@ def _make_runner( control_joint_ids: tuple[int, ...] | None = None, max_action_retries: int = 2, action_timeout: float = 10.0, + tracking_runtime: TrackingRuntime | None = None, ) -> tuple[ ExecutionRunner, FakeClock, @@ -291,7 +362,7 @@ def _make_runner( generator.device = torch.device("cpu") generator.planner.cfg.planner_type = "stub" action = TimedAction(with_effect=with_effect) - engine = AtomicActionEngine(generator) + engine = AtomicActionEngine(generator, tracking_runtime=tracking_runtime) engine.register(action) initial_task = TaskState.empty(batch_size, "cpu") initial_context = provider.observe(initial_task) @@ -305,7 +376,6 @@ def _make_runner( recovery_policy=RecoveryPolicy( max_replans=2, max_action_retries=max_action_retries, - tracking_error_threshold=0.05, action_timeout=action_timeout, ), ) @@ -355,7 +425,7 @@ def test_joint_feedback_ignores_motion_outside_bound_endpoint() -> None: assert action.plan_count == 1 assert len(sink.sent) == 3 assert not any( - event.kind is ExecutionEventKind.TRACKING_ERROR + event.kind is ExecutionEventKind.TRACKING_DIVERGED for step in (second, completed) if step.tick is not None for event in step.tick.events @@ -506,11 +576,131 @@ def test_runner_replans_from_observation_after_tracking_error() -> None: assert action.plan_count == 2 assert recovered.tick is not None event_kinds = {event.kind for event in recovered.tick.events} - assert ExecutionEventKind.TRACKING_ERROR in event_kinds + assert ExecutionEventKind.TRACKING_DIVERGED in event_kinds assert ExecutionEventKind.REPLANNED in event_kinds assert recovered.status is RunnerStatus.RUNNING +@pytest.mark.parametrize("failure_kind", ["provider", "evaluator"]) +def test_runner_fails_closed_when_required_tracking_runtime_raises( + failure_kind: str, +) -> None: + builtins = TrackingRuntime.with_builtins() + if failure_kind == "provider": + tracking_runtime = TrackingRuntime( + TrackingFeedbackProviderRegistry((RaisingFeedbackProvider(),)), + builtins.projectors, + builtins.evaluators, + ) + else: + tracking_runtime = TrackingRuntime( + builtins.providers, + builtins.projectors, + TrackingEvaluatorRegistry((RaisingJointTrackingEvaluator(),)), + ) + runner, clock, _, sink, action = _make_runner(tracking_runtime=tracking_runtime) + + runner.step() + clock.advance(FIRST_INTERVAL) + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert failed.tick is not None + event_kinds = {event.kind for event in failed.tick.events} + assert ExecutionEventKind.TRACKING_FEEDBACK_FAILED in event_kinds + assert ExecutionEventKind.REPLANNED not in event_kinds + assert action.plan_count == 1 + assert sink.cancel_count == 1 + + +def test_runner_deactivates_only_rows_with_invalid_required_feedback() -> None: + builtins = TrackingRuntime.with_builtins() + tracking_runtime = TrackingRuntime( + TrackingFeedbackProviderRegistry((MaskedFeedbackProvider((True, False)),)), + builtins.projectors, + builtins.evaluators, + ) + runner, clock, _, _, _ = _make_runner( + batch_size=2, + tracking_runtime=tracking_runtime, + ) + + runner.step() + clock.advance(2.0 * FIRST_INTERVAL) + partial = runner.step() + + assert partial.status is RunnerStatus.RUNNING + assert partial.tick is not None + assert partial.tick.command is not None + assert partial.tick.command.active_mask.tolist() == [True, False] + feedback_failure = next( + event + for event in partial.tick.events + if event.kind is ExecutionEventKind.TRACKING_FEEDBACK_FAILED + ) + assert feedback_failure.env_mask.tolist() == [False, True] + + +def test_runner_maintains_final_target_while_terminal_acceptance_is_pending() -> None: + runner, clock, _, sink, action = _make_runner() + sink.follow_commands.extend([True, True, False]) + + runner.step() + clock.advance(FIRST_INTERVAL) + runner.step() + clock.advance(SECOND_INTERVAL) + runner.step() + final_command = sink.sent[-1] + + clock.advance(SECOND_INTERVAL) + settling = runner.step() + + assert action.plan_count == 1 + assert settling.status is RunnerStatus.RUNNING + assert settling.tick is not None + assert settling.tick.command is not None + assert len(sink.sent) == 4 + assert sink.sent[-1] is settling.tick.command + assert torch.equal(sink.sent[-1].active_mask, final_command.active_mask) + final_payload = final_command.commands[0].payload + settling_payload = sink.sent[-1].commands[0].payload + assert isinstance(final_payload, JointPositionPayload) + assert isinstance(settling_payload, JointPositionPayload) + assert torch.equal(settling_payload.positions, final_payload.positions) + event_kinds = {event.kind for event in settling.tick.events} + assert ExecutionEventKind.TERMINAL_ACCEPTANCE_PENDING in event_kinds + assert ExecutionEventKind.REPLANNED not in event_kinds + + +def test_terminal_settle_reemits_final_target_only_for_pending_rows() -> None: + runner, clock, provider, sink, action = _make_runner(batch_size=2) + + runner.step() + clock.advance(2.0 * FIRST_INTERVAL) + runner.step() + clock.advance(2.0 * SECOND_INTERVAL) + runner.step() + provider.qpos[1].zero_() + + clock.advance(2.0 * SECOND_INTERVAL) + settling = runner.step() + + assert action.plan_count == 1 + assert settling.status is RunnerStatus.RUNNING + assert settling.tick is not None + assert settling.tick.command is not None + assert settling.tick.command.active_mask.tolist() == [False, True] + pending = next( + event + for event in settling.tick.events + if event.kind is ExecutionEventKind.TERMINAL_ACCEPTANCE_PENDING + ) + assert pending.env_mask.tolist() == [False, True] + assert not any( + event.kind is ExecutionEventKind.REPLANNED for event in settling.tick.events + ) + + def test_runner_revision_waits_for_deadline_and_plans_from_fresh_observation() -> None: runner, clock, provider, sink, action = _make_runner() first = runner.step() @@ -525,7 +715,6 @@ def test_runner_revision_waits_for_deadline_and_plans_from_fresh_observation() - motion_policy=MotionPolicy(sample_count=3), recovery_policy=RecoveryPolicy( max_replans=2, - tracking_error_threshold=0.05, action_timeout=10.0, ), revision=1, @@ -572,7 +761,6 @@ def test_runner_revision_rejects_pending_effect_verification() -> None: motion_policy=MotionPolicy(sample_count=3), recovery_policy=RecoveryPolicy( max_replans=2, - tracking_error_threshold=0.05, action_timeout=10.0, ), revision=1, diff --git a/tests/sim/atomic_actions/test_tracking.py b/tests/sim/atomic_actions/test_tracking.py new file mode 100644 index 000000000..3bff07f3e --- /dev/null +++ b/tests/sim/atomic_actions/test_tracking.py @@ -0,0 +1,263 @@ +# ---------------------------------------------------------------------------- +# 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 + +from dataclasses import dataclass +from typing import ClassVar + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, +) +from embodichain.lab.sim.atomic_actions.state import ( + PlanningContext, + RobotObservation, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.atomic_actions.tracking import ( + BASE_POSE_CHANNEL, + JOINT_POSITION_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + FeedbackTerminalAcceptance, + InFlightTrackingPolicy, + JointPositionTrackingMetric, + JointPositionTrackingState, + PoseTrackingEvaluator, + PoseTrackingMetric, + PoseTrackingState, + TimedTerminalAcceptance, + TimedTrackingSequence, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingMetricCfg, + TrackingPolicy, + TrackingProjectorRef, + TrackingRuntime, + TrackingSetpoint, + WholeBodyPoseTrackingEvaluator, + WholeBodyPoseTrackingMetric, + WholeBodyPoseTrackingState, +) + + +@dataclass(frozen=True, slots=True) +class _AlternateJointMetric(TrackingMetricCfg): + """Different metric identity deliberately sharing the joint channel.""" + + metric_id: ClassVar[str] = "joint.alternate" + channel_id: ClassVar[str] = JOINT_POSITION_CHANNEL + + +def _joint_binding(target: JointPositionTarget) -> EndpointTrackingChannelBinding: + return EndpointTrackingChannelBinding( + channel_id=JOINT_POSITION_CHANNEL, + source=TrackingFeedbackSourceRef( + provider_id="planning_context.robot", + revision="1", + address=EndpointTrackingFeedbackAddress( + target=target, + channel_id=JOINT_POSITION_CHANNEL, + ), + ), + projector=TrackingProjectorRef( + projector_id="joint_position_payload", + revision="1", + ), + ) + + +def _context(qpos: torch.Tensor) -> PlanningContext: + batch_size = qpos.shape[0] + device = qpos.device + return PlanningContext( + robot=RobotObservation( + timestamp=1.0, + qpos=qpos, + qvel=torch.zeros_like(qpos), + root_pose=torch.eye(4, device=device).repeat(batch_size, 1, 1), + ), + task=TaskState.empty(batch_size=batch_size, device=device), + scene=SceneSnapshot.empty(), + env_ids=torch.arange(batch_size, dtype=torch.long, device=device), + ) + + +def test_joint_policy_factory_separates_in_flight_and_terminal_contracts() -> None: + policy = TrackingPolicy.joint_position( + in_flight_max_abs_error=0.1, + terminal_max_abs_error=0.08, + terminal_settle_timeout=0.25, + ) + + assert policy.in_flight is not None + assert policy.in_flight.metrics == (JointPositionTrackingMetric(0.1),) + assert isinstance(policy.terminal, FeedbackTerminalAcceptance) + assert policy.terminal.metrics == (JointPositionTrackingMetric(0.08),) + assert policy.terminal.settle_timeout == pytest.approx(0.25) + + +def test_policy_rejects_ambiguous_metric_id_for_a_shared_channel() -> None: + with pytest.raises(ValueError, match="same exact metric ID"): + TrackingPolicy( + in_flight=InFlightTrackingPolicy( + metrics=(JointPositionTrackingMetric(0.1),) + ), + terminal=FeedbackTerminalAcceptance(metrics=(_AlternateJointMetric(),)), + ) + + +def test_timed_policy_is_an_explicit_no_feedback_contract() -> None: + policy = TrackingPolicy.timed(settle_duration=0.2) + + assert policy.in_flight is None + assert isinstance(policy.terminal, TimedTerminalAcceptance) + assert policy.terminal.settle_duration == pytest.approx(0.2) + + +def test_tracking_values_and_routes_own_tensor_and_target_snapshots() -> None: + positions = torch.tensor([[0.1, 0.2]]) + target = JointPositionTarget(control_part="arm", joint_ids=(0, 1)) + setpoint = TrackingSetpoint( + endpoint_key=("arm", "controller"), + binding=_joint_binding(target), + desired=JointPositionTrackingState(positions), + ) + + positions.add_(1.0) + + assert torch.equal(setpoint.desired.positions, torch.tensor([[0.1, 0.2]])) + assert setpoint.binding.source.address.target is not target + assert setpoint.key == ("arm", "controller", JOINT_POSITION_CHANNEL) + + +def test_timed_tracking_sequence_owns_env_ids_and_validates_batches() -> None: + env_ids = torch.tensor([2, 5], dtype=torch.long) + target = JointPositionTarget(control_part="arm", joint_ids=(0, 1)) + frame = TrackingFrame( + ( + TrackingSetpoint( + endpoint_key=("arm", "controller"), + binding=_joint_binding(target), + desired=JointPositionTrackingState(torch.zeros(2, 2)), + ), + ) + ) + sequence = TimedTrackingSequence(env_ids=env_ids, frames=(frame,)) + + env_ids[0] = 99 + + assert sequence.env_ids.tolist() == [2, 5] + assert sequence.batch_size == 2 + assert sequence.frame_count == 1 + + +def test_timed_tracking_sequence_rejects_mismatched_setpoint_batch() -> None: + target = JointPositionTarget(control_part="arm", joint_ids=(0, 1)) + frame = TrackingFrame( + ( + TrackingSetpoint( + endpoint_key=("arm", "controller"), + binding=_joint_binding(target), + desired=JointPositionTrackingState(torch.zeros(1, 2)), + ), + ) + ) + + with pytest.raises(ValueError, match="setpoint batch"): + TimedTrackingSequence( + env_ids=torch.tensor([0, 1], dtype=torch.long), + frames=(frame,), + ) + + +def test_builtin_runtime_projects_observes_and_evaluates_joint_positions() -> None: + target = JointPositionTarget(control_part="arm", joint_ids=(1, 3)) + binding = _joint_binding(target) + command = EndpointCommand( + target=target, + payload=JointPositionPayload(positions=torch.tensor([[0.3, 0.5], [0.1, 0.2]])), + ) + runtime = TrackingRuntime.with_builtins() + desired = runtime.project(command, binding) + setpoint = TrackingSetpoint(("arm", "controller"), binding, desired) + context = _context( + torch.tensor( + [ + [0.0, 0.32, 0.0, 0.49], + [0.0, 0.25, 0.0, 0.2], + ] + ) + ) + + feedback = runtime.observe(setpoint, context) + evaluation = runtime.evaluate( + setpoint, + feedback, + JointPositionTrackingMetric(tolerance=0.05), + ) + + assert torch.equal(evaluation.accepted_mask, torch.tensor([True, False])) + assert torch.allclose( + evaluation.component_errors["joint_max_abs"], + torch.tensor([0.02, 0.15]), + ) + + +def test_pose_metric_preserves_translation_and_rotation_components() -> None: + desired = torch.eye(4).repeat(2, 1, 1) + observed = desired.clone() + observed[0, 0, 3] = 0.01 + observed[1, :2, :2] = torch.tensor([[0.0, -1.0], [1.0, 0.0]]) + evaluator = PoseTrackingEvaluator() + + evaluation = evaluator.evaluate( + PoseTrackingState(desired), + PoseTrackingState(observed), + torch.ones(2, dtype=torch.bool), + PoseTrackingMetric(translation_tolerance=0.02, rotation_tolerance=0.1), + ) + + assert evaluation.channel_id == BASE_POSE_CHANNEL + assert evaluation.accepted_mask.tolist() == [True, False] + assert set(evaluation.component_errors) == {"translation", "rotation"} + + +def test_whole_body_metric_requires_pose_and_joint_acceptance() -> None: + root = torch.eye(4).repeat(2, 1, 1) + desired = WholeBodyPoseTrackingState(root, torch.zeros(2, 2)) + observed = WholeBodyPoseTrackingState( + root, + torch.tensor([[0.01, 0.0], [0.0, 0.2]]), + ) + + evaluation = WholeBodyPoseTrackingEvaluator().evaluate( + desired, + observed, + torch.ones(2, dtype=torch.bool), + WholeBodyPoseTrackingMetric(joint_position_tolerance=0.05), + ) + + assert evaluation.accepted_mask.tolist() == [True, False] + assert torch.allclose( + evaluation.component_errors["joint_max_abs"], torch.tensor([0.01, 0.2]) + ) diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index 8d4ff1408..06f79f924 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -50,6 +50,10 @@ SceneEntityPose, TaskState, ) +from embodichain.lab.sim.atomic_actions.tracking import ( + JointPositionTrackingMetric, + TrackingPolicy, +) from embodichain.lab.sim.skills.calls import ( HandOver, Pick, @@ -953,6 +957,10 @@ def test_grounded_safe_invocation_requires_registered_dynamic_collision() -> Non preset=SkillPolicyPreset( "safe", motion_policy=MotionPolicy(strategy="motion_gen"), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.125, + terminal_max_abs_error=0.125, + ), ) ) compiler, engine = _compiler( @@ -972,6 +980,14 @@ def test_grounded_safe_invocation_requires_registered_dynamic_collision() -> Non engine.resolve(grounded.invocation).motion_policy.dynamic_collision_mode is DynamicCollisionMode.REQUIRED ) + invocation_tracking = grounded.invocation.tracking_policy.in_flight + resolved_tracking = engine.resolve(grounded.invocation).tracking_policy.in_flight + assert invocation_tracking is not None + assert resolved_tracking is not None + assert isinstance(invocation_tracking.metrics[0], JointPositionTrackingMetric) + assert isinstance(resolved_tracking.metrics[0], JointPositionTrackingMetric) + assert invocation_tracking.metrics[0].tolerance == 0.125 + assert resolved_tracking.metrics[0].tolerance == 0.125 def test_place_inherits_known_pick_resource_when_primary_is_omitted() -> None: diff --git a/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py index cfdb3ca3a..0aa252c9f 100644 --- a/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py +++ b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py @@ -46,6 +46,7 @@ SimulationExecutionAdapter, SkillDescriptor, ) +from embodichain.lab.sim.atomic_actions.tracking import TrackingPolicy # noqa: E402 from embodichain.lab.sim.cfg import RigidBodyAttributesCfg # noqa: E402 from embodichain.lab.sim.objects import RigidObjectCfg # noqa: E402 from embodichain.lab.sim.planners import MotionGenCfg, MotionGenerator # noqa: E402 @@ -185,9 +186,12 @@ def _profile() -> RobotSkillProfile: sample_count=SAMPLE_COUNT, control_dt=COMMAND_CYCLE_TIME, ), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.1, + terminal_max_abs_error=0.1, + ), recovery_policy=RecoveryPolicy( max_replans=2, - tracking_error_threshold=0.1, action_timeout=30.0, ), runner_cfg=ExecutionRunnerCfg(minimum_cycle_time=COMMAND_CYCLE_TIME), diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index f7d86c1e8..1bb576292 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -55,6 +55,12 @@ RuntimeEndpointTarget, ) from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.tracking import ( + JOINT_POSITION_CHANNEL, + EndpointTrackingFeedbackAddress, + JointPositionTrackingMetric, + TrackingPolicy, +) from embodichain.lab.sim.skills import ( AmbiguousSkillBindingError, COMPOSITE_EFFECT_MONITOR_ID, @@ -1150,6 +1156,19 @@ def test_unique_capability_binding_lowers_to_exact_action_binding() -> None: assert grasp.require_target(JointPositionTarget).control_part == "left_hand" assert motion.task_state_key == "left_actor" assert grasp.task_state_key == "left_actor" + motion_tracking = motion.tracking_channel(JOINT_POSITION_CHANNEL) + assert motion_tracking.source.provider_id == "planning_context.robot" + assert motion_tracking.source.revision == "1" + assert motion_tracking.projector.projector_id == "joint_position_payload" + assert motion_tracking.projector.revision == "1" + assert isinstance( + motion_tracking.source.address, + EndpointTrackingFeedbackAddress, + ) + assert ( + motion_tracking.source.address.target.address_fingerprint + == motion.target.address_fingerprint + ) resource = resolved.resources["primary"] motion_sources = resource.endpoints["motion"].effect_sources grasp_sources = resource.endpoints["grasp"].effect_sources @@ -1363,6 +1382,10 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: "safe", motion_policy=MotionPolicy(sample_count=80), required_planner="stub_planner", + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.125, + terminal_max_abs_error=0.125, + ), ) profile = RobotSkillProfile( "presets", @@ -1381,6 +1404,11 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: assert first.schema_version == 1 assert first.required_planner == "stub_planner" assert first.motion_policy.sample_count == 80 + assert first.tracking_policy is not second.tracking_policy + first_tracking = first.tracking_policy.in_flight + assert first_tracking is not None + assert isinstance(first_tracking.metrics[0], JointPositionTrackingMetric) + assert first_tracking.metrics[0].tolerance == 0.125 mutable_runner = first.runner_cfg mutable_runner.command_timeout = 99.0 assert bound.preset().runner_cfg.command_timeout == 1.0 diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py index 49356f49c..53ccaed74 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -39,6 +39,8 @@ EffectVerificationRequirement, EffectVerificationRequest, EndpointBinding, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, JointPositionTarget, MotionPolicy, PlanningContext, @@ -50,7 +52,10 @@ StateDelta, TaskState, TimedCommandSequence, + TrackingFeedbackSourceRef, + 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.effects import ( @@ -342,6 +347,7 @@ def ground( strategy="ik_interp", sample_count=7, ), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy( max_replans=0, max_action_retries=0, @@ -623,9 +629,16 @@ def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None } assert resolved["motion_policy"]["strategy"] == "ik_interp" assert resolved["motion_policy"]["sample_count"] == 7 + assert resolved["tracking_policy"] == { + "in_flight": None, + "terminal": {"mode": "timed", "settle_duration": 0.0}, + } assert resolved["recovery_policy"]["max_replans"] == 0 assert resolved["endpoints"] == [] assert attempt["resolved_core_policy"] == resolved + assert attempt["tracking_policy"] == resolved["tracking_policy"] + assert attempt["tracking_contract"] is 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["effect_spec"]["semantic_id"] == "test.metadata" @@ -666,16 +679,34 @@ def test_plan_attempt_trace_rejects_monitor_cutoff_for_non_dependency() -> None: def test_endpoint_binding_trace_records_only_stable_binding_choices() -> None: + target = JointPositionTarget("left_arm_control", (3, 1)) binding = EndpointBinding( slot_id="primary", endpoint_id="motion", resource_id="left_arm", adapter_id="control_part", - target=JointPositionTarget("left_arm_control", (3, 1)), + target=target, task_state_key="left_arm_state", capabilities=frozenset({"cartesian_pose", "joint_position"}), claim_tokens=frozenset({"arm_workspace", "left_side"}), joint_ids=(3, 1), + tracking_channels={ + "joint.position": EndpointTrackingChannelBinding( + channel_id="joint.position", + source=TrackingFeedbackSourceRef( + provider_id="planning_context.robot", + revision="1", + address=EndpointTrackingFeedbackAddress( + target=target, + channel_id="joint.position", + ), + ), + projector=TrackingProjectorRef( + projector_id="joint_position_payload", + revision="1", + ), + ) + }, ) trace = SkillEndpointBindingTrace.from_binding(binding) @@ -690,6 +721,32 @@ def test_endpoint_binding_trace_records_only_stable_binding_choices() -> None: assert metadata["claim_tokens"] == ["arm_workspace", "left_side"] assert metadata["joint_ids"] == [3, 1] assert "target" not in metadata + tracking = metadata["tracking_channels"][0] + target_fingerprint = [ + { + "__type__": ( + "embodichain.lab.sim.atomic_actions.bindings." "JointPositionTarget" + ) + }, + "robot.joint_position", + "left_arm_control", + [3, 1], + ] + address_fingerprint = [target_fingerprint, "joint.position"] + assert tracking["feedback_source"]["address_fingerprint"] == address_fingerprint + assert tracking["route_fingerprint"] == [ + "joint.position", + ["planning_context.robot", "1", address_fingerprint], + "joint_position_payload", + "1", + ] + tracking["feedback_source"]["address_fingerprint"][0][1] = "mutated" + assert ( + trace.to_metadata()["tracking_channels"][0]["feedback_source"][ + "address_fingerprint" + ][0][1] + == "robot.joint_position" + ) def test_preparation_failure_keeps_resolved_policy_without_plan_attempt( From de750cc95bc014324f4b98b4e1cd21cb72cb43e3 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 17:02:14 +0800 Subject: [PATCH 14/29] feat(expert-program): add task-owned pre-sim catalogs --- .../lab/gym/envs/expert_program/__init__.py | 14 +- .../lab/gym/envs/expert_program/catalog.py | 1047 +++++++++++++++++ .../gym/envs/expert_program/environment.py | 34 +- .../lab/gym/envs/expert_program/simulation.py | 240 ++++ .../expert_program/simulation_environment.py | 123 +- .../expert_program/simulation_policies.py | 8 +- embodichain/lab/gym/utils/gym_utils.py | 48 +- embodichain/lab/gym/utils/registration.py | 69 +- embodichain/lab/scripts/run_env.py | 6 - .../gym/multi_segments/cube_pick_place.json | 5 +- .../multi_segments/cube_pick_place.py | 39 +- .../tableware/open_drawer.py | 17 +- tests/gym/envs/expert_program/test_catalog.py | 596 ++++++++++ .../test_simulation_environment.py | 92 +- .../test_multi_segments_cube_pick_place.py | 28 +- tests/gym/envs/tasks/test_open_drawer.py | 9 +- tests/gym/utils/test_gym_utils.py | 91 +- tests/lab/scripts/test_run_env.py | 23 +- 18 files changed, 2307 insertions(+), 182 deletions(-) create mode 100644 embodichain/lab/gym/envs/expert_program/catalog.py create mode 100644 tests/gym/envs/expert_program/test_catalog.py diff --git a/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py index 12b371bec..746cf3d45 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -133,6 +133,11 @@ SimulationRobotSkillProfileBinding, SimulationSceneBinding, ) +from .catalog import ( + ExpertProgramIntegrationCatalog, + IntegrationFingerprintMismatch, + SimulationExpertProgramRegistration, +) from .simulation_environment import ( ControlCommandStateEvidenceTracker, MotionGeneratorFactory, @@ -142,7 +147,10 @@ SimulationPlanningObservationProvider, create_simulation_expert_program_adapter, ) -from .simulation_policies import SimulationSegmentPolicyPort +from .simulation_policies import ( + SimulationSegmentPolicyPort, + default_simulation_settle_presets, +) __all__ = [ "AcceptedRuntimeCommandObserver", @@ -186,6 +194,7 @@ "ExpertProgramEnvironmentFactory", "ExpertProgramEnvironmentMixin", "ExpertProgramIntegrationCfg", + "ExpertProgramIntegrationCatalog", "ExpertProgramRuntimeAssembly", "ExpertProgramSceneResolver", "ExpertProgramValidationContext", @@ -193,6 +202,7 @@ "HandOverCfg", "GymPlanningObservationProvider", "InvokeCfg", + "IntegrationFingerprintMismatch", "MAX_DECLARATIVE_DEPTH", "MAX_DECLARATIVE_NODES", "MAX_EXPANDED_CALLS", @@ -232,6 +242,7 @@ "SimulationArticulationLinkBinding", "SimulationExpertProgramEnvironment", "SimulationExpertProgramFactory", + "SimulationExpertProgramRegistration", "SimulationPlanningObservationProvider", "SimulationRigidObjectBinding", "SimulationResourceEndpointBinding", @@ -246,6 +257,7 @@ "ValidatorCfg", "WaitStablePostCfg", "create_simulation_expert_program_adapter", + "default_simulation_settle_presets", "decode_expert_program", "decode_semantic_call", "encode_semantic_call", diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py new file mode 100644 index 000000000..a683caa1a --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -0,0 +1,1047 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Immutable task-registration catalog for declarative Expert Programs.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, field, fields, is_dataclass, replace +from enum import Enum +import hashlib +import json +import math +from types import MappingProxyType +import torch + +from embodichain.lab.gym.envs.settling import DynamicSettleMonitorCfg +from embodichain.lab.sim.atomic_actions import ( + Affordance, + ArticulationOperationAffordance, + AtomicActionEngine, + SkillDescriptor, +) +from embodichain.lab.sim.atomic_actions.primitives import BUILTIN_ACTION_TYPES +from embodichain.lab.sim.skills import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + HandOverPoseProvider, + OperateArticulation, + Place, + RelationTargetGrounder, + RobotSkillProfile, + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneManifest, + SceneObjectRef, + SceneRegistry, + SemanticCallCatalog, + SemanticIntegrationManifest, + SemanticValidationError, + SkillPolicyPreset, + builtin_semantic_call_catalog, +) + +from .cfg import ( + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + OperateArticulationCfg, + PostPolicyCfg, + RegisteredSemanticCallCfg, + SemanticCallCfg, + ValidatorCfg, +) +from .compiler import ( + CompiledProgram, + ExpertProgramCompileError, + ExpertProgramCompiler, + ExpertProgramSceneResolver, +) +from .decoder import ( + ConfigPath, + ExpertProgramValidationError, + SceneReferenceRole, +) +from .simulation import SimulationRobotSkillProfileBinding, SimulationSceneBinding +from .simulation_policies import default_simulation_settle_presets + +_CATALOG_FINGERPRINT_SCHEMA_VERSION = 1 +_POST_POLICY_KINDS = frozenset({"wait_stable"}) +_VALIDATOR_KINDS = frozenset({"object_near_target"}) + + +class IntegrationFingerprintMismatch(RuntimeError): + """Raised when a live integration no longer matches its registration.""" + + +def _qualified_name(value: type[object] | object) -> str: + """Return a stable fully-qualified type name.""" + value_type = value if isinstance(value, type) else type(value) + return f"{value_type.__module__}.{value_type.__qualname__}" + + +def _canonical_value(value: object) -> object: + """Convert provider-free declarations to deterministic JSON values.""" + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError("Fingerprint metadata cannot contain non-finite floats.") + return value + if isinstance(value, Enum): + return { + "type": _qualified_name(value), + "value": _canonical_value(value.value), + } + if isinstance(value, type): + return {"type": _qualified_name(value)} + if isinstance(value, torch.Tensor): + tensor = value.detach().cpu() + return { + "tensor_dtype": str(tensor.dtype), + "tensor_shape": list(tensor.shape), + "tensor_value": tensor.tolist(), + } + if isinstance(value, Mapping): + normalized: dict[str, object] = {} + for key, nested in value.items(): + if type(key) is not str: + raise TypeError("Fingerprint mapping keys must be exact strings.") + normalized[key] = _canonical_value(nested) + return {key: normalized[key] for key in sorted(normalized)} + if isinstance(value, (tuple, list)): + return [_canonical_value(nested) for nested in value] + if isinstance(value, (set, frozenset)): + normalized = [_canonical_value(nested) for nested in value] + return sorted( + normalized, + key=lambda item: json.dumps( + item, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=False, + ), + ) + if is_dataclass(value): + metadata = { + data_field.name: _canonical_value(getattr(value, data_field.name)) + for data_field in fields(value) + } + return {"type": _qualified_name(value), "fields": metadata} + # Provider objects are not executable catalog data. Their declared type is + # still part of the integration surface, while live identity is excluded. + return {"provider_type": _qualified_name(value)} + + +def _canonical_json(value: object) -> str: + """Encode one declaration using the versioned canonical JSON form.""" + return json.dumps( + _canonical_value(value), + sort_keys=True, + separators=(",", ":"), + ensure_ascii=False, + allow_nan=False, + ) + + +def _digest(payload: object) -> str: + """Return the SHA-256 digest for one canonical declaration payload.""" + return hashlib.sha256(_canonical_json(payload).encode("utf-8")).hexdigest() + + +def _snapshot_settle_presets( + values: Mapping[str, DynamicSettleMonitorCfg], +) -> Mapping[str, DynamicSettleMonitorCfg]: + """Own one strict named settle-preset table.""" + if not isinstance(values, Mapping) or not values: + raise ValueError("settle_presets must be a non-empty mapping.") + normalized: dict[str, DynamicSettleMonitorCfg] = {} + for preset_id, preset in values.items(): + if ( + type(preset_id) is not str + or not preset_id + or preset_id != preset_id.strip() + ): + raise ValueError( + "Settle preset IDs must be non-empty strings without outer " + "whitespace." + ) + if not isinstance(preset, DynamicSettleMonitorCfg): + raise TypeError( + "settle_presets values must be DynamicSettleMonitorCfg values." + ) + normalized[preset_id] = preset.snapshot() + return MappingProxyType(normalized) + + +def _exact_identifier(value: object, *, field_name: str) -> str: + """Validate one exact catalog identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _relation_grounder_key( + grounder: RelationTargetGrounder, +) -> tuple[str, type[Affordance], str]: + """Return the compiler-compatible exact key for one relation grounder.""" + grounder_type = type(grounder) + capability = _exact_identifier( + getattr(grounder_type, "capability", None), + field_name="RelationTargetGrounder.capability", + ) + affordance_type = getattr(grounder_type, "affordance_type", None) + if not isinstance(affordance_type, type) or not issubclass( + affordance_type, + Affordance, + ): + raise TypeError( + "RelationTargetGrounder.affordance_type must be an Affordance subclass." + ) + revision = _exact_identifier( + getattr(grounder_type, "affordance_revision", None), + field_name="RelationTargetGrounder.affordance_revision", + ) + return capability, affordance_type, revision + + +def _relation_grounder_order_key( + grounder: RelationTargetGrounder, +) -> tuple[str, str, str]: + """Return one totally ordered rendering of a relation-grounder key.""" + capability, affordance_type, revision = _relation_grounder_key(grounder) + return capability, _qualified_name(affordance_type), revision + + +def _validate_provider_declaration(provider: object, *, field_name: str) -> None: + """Accept only frozen dataclass declarations or stateless providers.""" + dataclass_declaration = is_dataclass(provider) + dataclass_field_names: set[str] = set() + if dataclass_declaration: + params = getattr(type(provider), "__dataclass_params__", None) + if params is None or not params.frozen: + raise TypeError( + f"{field_name} stateful declarations must be frozen dataclasses " + "so every configuration field enters the registration fingerprint." + ) + dataclass_field_names.update( + declaration_field.name for declaration_field in fields(provider) + ) + + state_names: set[str] = set() + instance_state = getattr(provider, "__dict__", None) + if isinstance(instance_state, Mapping): + state_names.update(instance_state) + for owner in type(provider).__mro__: + declared_slots = getattr(owner, "__slots__", ()) + slots = (declared_slots,) if isinstance(declared_slots, str) else declared_slots + for slot_name in slots: + if slot_name in {"__dict__", "__weakref__"}: + continue + storage_name = ( + f"_{owner.__name__.lstrip('_')}{slot_name}" + if slot_name.startswith("__") and not slot_name.endswith("__") + else slot_name + ) + if hasattr(provider, storage_name): + state_names.add(storage_name) + undeclared_state = ( + state_names.difference(dataclass_field_names) + if dataclass_declaration + else state_names + ) + if undeclared_state: + raise TypeError( + f"{field_name} providers contain unfingerprinted state " + f"{sorted(undeclared_state)}. Use a frozen dataclass declaration with " + "every state field declared; non-dataclass providers must be stateless." + ) + + +def _snapshot_relation_grounders( + values: tuple[RelationTargetGrounder, ...], +) -> tuple[RelationTargetGrounder, ...]: + """Validate and own one immutable relation-grounder tuple.""" + if type(values) is not tuple: + raise TypeError("relation_grounders must be an exact tuple.") + seen: set[tuple[str, type[Affordance], str]] = set() + for grounder in values: + if not isinstance(grounder, RelationTargetGrounder): + raise TypeError( + "relation_grounders must contain RelationTargetGrounder instances." + ) + _validate_provider_declaration( + grounder, + field_name="relation_grounders", + ) + key = _relation_grounder_key(grounder) + if key in seen: + raise ValueError(f"Duplicate relation grounder key {key!r}.") + seen.add(key) + return tuple(values) + + +def _snapshot_relation_grounder_keys( + values: frozenset[tuple[str, type[Affordance], str]], +) -> frozenset[tuple[str, type[Affordance], str]]: + """Validate immutable provider-free relation-grounder lookup keys.""" + if type(values) is not frozenset: + raise TypeError("relation_grounder_keys must be an exact frozenset.") + normalized: set[tuple[str, type[Affordance], str]] = set() + for key in values: + if type(key) is not tuple or len(key) != 3: + raise TypeError("relation_grounder_keys must contain exact 3-tuple values.") + capability, affordance_type, revision = key + _exact_identifier(capability, field_name="relation grounder capability") + if not isinstance(affordance_type, type) or not issubclass( + affordance_type, + Affordance, + ): + raise TypeError( + "relation grounder affordance types must be Affordance subclasses." + ) + _exact_identifier(revision, field_name="relation grounder revision") + normalized.add((capability, affordance_type, revision)) + return frozenset(normalized) + + +def _handover_pose_provider_id(provider: HandOverPoseProvider) -> str: + """Return the compiler-compatible class ID for one hand-over provider.""" + return _exact_identifier( + getattr(type(provider), "provider_id", None), + field_name="HandOverPoseProvider.provider_id", + ) + + +def _snapshot_handover_pose_providers( + values: tuple[HandOverPoseProvider, ...], +) -> tuple[HandOverPoseProvider, ...]: + """Validate and own one immutable hand-over-provider tuple.""" + if type(values) is not tuple: + raise TypeError("handover_pose_providers must be an exact tuple.") + seen: set[str] = set() + for provider in values: + if not isinstance(provider, HandOverPoseProvider): + raise TypeError( + "handover_pose_providers must contain HandOverPoseProvider instances." + ) + _validate_provider_declaration( + provider, + field_name="handover_pose_providers", + ) + provider_id = _handover_pose_provider_id(provider) + if provider_id in seen: + raise ValueError(f"Duplicate handover pose provider {provider_id!r}.") + seen.add(provider_id) + return tuple(values) + + +def _declared_articulation_operation_targets( + scene_binding: SimulationSceneBinding, +) -> dict[str, frozenset[str]]: + """Derive named operation-target IDs from the task-owned scene binding.""" + return { + binding.entity_id: frozenset(binding.semantic_targets) + for binding in scene_binding.articulation_operations + } + + +def _snapshot_articulation_operation_targets( + values: Mapping[str, frozenset[str]], + *, + scene: SceneManifest, +) -> Mapping[str, frozenset[str]]: + """Own and cross-check provider-free named articulation targets.""" + if not isinstance(values, Mapping): + raise TypeError("articulation_operation_targets must be a mapping.") + normalized: dict[str, frozenset[str]] = {} + for affordance_id, target_ids in values.items(): + _exact_identifier( + affordance_id, + field_name="articulation operation affordance IDs", + ) + if type(target_ids) is not frozenset: + raise TypeError( + "articulation_operation_targets values must be exact frozensets." + ) + for target_id in target_ids: + _exact_identifier( + target_id, + field_name="articulation operation target IDs", + ) + entry = scene.lookup( + affordance_id, + expected_type=SceneAffordanceRef, + path=("articulation_operation_targets", affordance_id), + ) + if entry.ref.entity_id != affordance_id: + raise ValueError( + "articulation_operation_targets keys must use canonical " + "affordance IDs." + ) + if ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + not in entry.affordance_capabilities + or entry.affordance_payload_type is not ArticulationOperationAffordance + ): + raise TypeError( + f"Scene affordance {affordance_id!r} is not an articulation " + "operation affordance." + ) + normalized[affordance_id] = frozenset(target_ids) + + declared_affordance_ids = { + entry.ref.entity_id + for entry in scene.entries + if type(entry.ref) is SceneAffordanceRef + and ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + in entry.affordance_capabilities + } + if set(normalized) != declared_affordance_ids: + raise ValueError( + "articulation_operation_targets must cover every declared operation " + f"affordance exactly; expected {sorted(declared_affordance_ids)}, got " + f"{sorted(normalized)}." + ) + return MappingProxyType(normalized) + + +class _SceneManifestProgramResolver: + """Resolve compiler references from an immutable :class:`SceneManifest`.""" + + def __init__(self, scene: SceneManifest) -> None: + if type(scene) is not SceneManifest: + raise TypeError("scene must be exactly SceneManifest.") + self._scene = scene + + def resolve( + self, + reference: str, + *, + expected_types: tuple[type[SceneEntityRef], ...], + path: ConfigPath, + ) -> SceneEntityRef: + """Resolve one reference without retaining a live registry.""" + if ( + type(expected_types) is not tuple + or not expected_types + or not all( + isinstance(expected_type, type) + and issubclass(expected_type, SceneEntityRef) + for expected_type in expected_types + ) + ): + raise TypeError( + "expected_types must be a non-empty tuple of scene-ref types." + ) + try: + resolved = self._scene.resolve(reference, path=path) + except (KeyError, TypeError, ValueError) as exc: + raise ExpertProgramCompileError( + "unknown_scene_reference", + path, + str(exc), + ) from exc + if type(resolved) not in expected_types: + raise ExpertProgramCompileError( + "scene_reference_type_mismatch", + path, + f"Scene reference {reference!r} resolves to " + f"{type(resolved).__name__}, expected one of " + f"{tuple(value.__name__ for value in expected_types)}.", + ) + return type(resolved)(resolved.entity_id) + + +@dataclass(frozen=True, slots=True) +class ExpertProgramIntegrationCatalog: + """Provider-free integration directory owned by one task registration.""" + + scene_registry_id: str + robot_profile_id: str + scene: SceneManifest + robot_profile: RobotSkillProfile + call_catalog: SemanticCallCatalog + relation_grounder_keys: frozenset[tuple[str, type[Affordance], str]] + articulation_operation_targets: Mapping[str, frozenset[str]] + settle_preset_ids: frozenset[str] + fingerprint: str + _required_skills: Mapping[str, SkillDescriptor] = field( + repr=False, + compare=False, + ) + + def __post_init__(self) -> None: + for field_name in ("scene_registry_id", "robot_profile_id"): + value = getattr(self, field_name) + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be an exact identifier.") + if type(self.scene) is not SceneManifest: + raise TypeError("scene must be exactly SceneManifest.") + if type(self.robot_profile) is not RobotSkillProfile: + raise TypeError("robot_profile must be exactly RobotSkillProfile.") + if type(self.call_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + object.__setattr__( + self, + "relation_grounder_keys", + _snapshot_relation_grounder_keys(self.relation_grounder_keys), + ) + object.__setattr__( + self, + "articulation_operation_targets", + _snapshot_articulation_operation_targets( + self.articulation_operation_targets, + scene=self.scene, + ), + ) + if self.robot_profile.profile_id != self.robot_profile_id: + raise ValueError("robot_profile_id must match robot_profile.profile_id.") + preset_ids = frozenset(self.settle_preset_ids) + if not preset_ids: + raise ValueError("settle_preset_ids must not be empty.") + object.__setattr__(self, "settle_preset_ids", preset_ids) + if ( + type(self.fingerprint) is not str + or len(self.fingerprint) != 64 + or any( + character not in "0123456789abcdef" for character in self.fingerprint + ) + ): + raise ValueError("fingerprint must be a lowercase SHA-256 digest.") + object.__setattr__( + self, + "_required_skills", + MappingProxyType(dict(self._required_skills)), + ) + + def validate_integration( + self, + integration: ExpertProgramIntegrationCfg, + *, + path: ConfigPath, + ) -> None: + """Validate exact scene, profile, and runtime-preset selection.""" + del path + if integration.scene_registry != self.scene_registry_id: + raise ValueError( + f"Expected scene_registry {self.scene_registry_id!r}, got " + f"{integration.scene_registry!r}." + ) + if integration.robot_profile != self.robot_profile_id: + raise ValueError( + f"Expected robot_profile {self.robot_profile_id!r}, got " + f"{integration.robot_profile!r}." + ) + if integration.runtime_preset not in self.robot_profile.presets: + raise KeyError( + f"Unknown runtime preset {integration.runtime_preset!r}; available " + f"presets are {sorted(self.robot_profile.presets)}." + ) + + def validate_semantic_call( + self, + call: SemanticCallCfg, + *, + path: ConfigPath, + ) -> None: + """Validate semantic-call catalog and payload revision references.""" + call_id = call.call_id if type(call) is RegisteredSemanticCallCfg else call.kind + descriptor = self.call_catalog.discover(call_id) + if type(call) is RegisteredSemanticCallCfg and ( + call.schema_version != descriptor.schema_version + ): + raise ValueError( + f"Semantic call {call_id!r} requires schema_version " + f"{descriptor.schema_version}, got {call.schema_version}." + ) + if type(call) is OperateArticulationCfg and call.target is not None: + self._validate_articulation_operation_target( + articulation=call.articulation, + handle=call.handle, + target=call.target, + path=path, + ) + + def _validate_articulation_operation_target( + self, + *, + articulation: str | SceneArticulationRef, + handle: str | SceneAffordanceRef | None, + target: str, + path: ConfigPath, + ) -> None: + """Resolve one operation affordance and validate its named target.""" + try: + articulation_ref = self.scene.resolve( + articulation, + expected_type=SceneArticulationRef, + path=(*path, "articulation"), + ) + except SemanticValidationError as exc: + raise ExpertProgramValidationError( + exc.diagnostic.code, + exc.diagnostic.path, + exc.diagnostic.message, + ) from exc + try: + affordance = self.scene.resolve_affordance( + articulation_ref, + capability=ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + explicit=handle, + path=(*path, "handle"), + ) + except SemanticValidationError as exc: + raise ExpertProgramValidationError( + exc.diagnostic.code, + exc.diagnostic.path, + exc.diagnostic.message, + ) from exc + target_ids = self.articulation_operation_targets.get(affordance.entity_id) + if target_ids is None: + raise ExpertProgramValidationError( + "missing_articulation_operation_targets", + (*path, "handle"), + f"Operation affordance {affordance.entity_id!r} has no static " + "named-target declaration.", + ) + if target not in target_ids: + raise ExpertProgramValidationError( + "unknown_articulation_operation_target", + (*path, "target"), + f"Unknown target {target!r} for operation affordance " + f"{affordance.entity_id!r}; available targets are " + f"{sorted(target_ids)}.", + ) + + def _validate_place_relation_grounder( + self, + call: Place, + *, + affordance: SceneAffordanceRef, + path: ConfigPath, + ) -> None: + """Require the exact linked relation-affordance grounder pre-sim.""" + if call.on is not None: + capability = PLACE_ON_AFFORDANCE_CAPABILITY + relation_field = "on" + elif call.inside is not None: + capability = PLACE_IN_AFFORDANCE_CAPABILITY + relation_field = "inside" + else: + return + entry = self.scene.lookup( + affordance, + expected_type=SceneAffordanceRef, + path=(*path, relation_field), + ) + payload_type = entry.affordance_payload_type + revision = entry.affordance_revision + if payload_type is None or revision is None: + raise ExpertProgramValidationError( + "incomplete_relation_affordance_declaration", + (*path, relation_field), + f"Relation affordance {affordance.entity_id!r} must declare an " + "exact payload type and revision.", + ) + key = (capability, payload_type, revision) + if key not in self.relation_grounder_keys: + rendered_key = ( + capability, + _qualified_name(payload_type), + revision, + ) + raise ExpertProgramValidationError( + "relation_grounder_not_registered", + (*path, relation_field), + f"No task-registration relation grounder matches linked " + f"affordance {affordance.entity_id!r} with key {rendered_key!r}.", + ) + + def validate_scene_reference( + self, + reference: str, + *, + role: SceneReferenceRole, + path: ConfigPath, + ) -> None: + """Validate one typed scene reference against its declared role.""" + expected: dict[str, tuple[type[SceneEntityRef], ...]] = { + "entity": (SceneEntityRef,), + "object": (SceneObjectRef,), + "articulation": (SceneArticulationRef,), + "affordance": (SceneAffordanceRef,), + "object_or_affordance": (SceneObjectRef, SceneAffordanceRef), + } + expected_types = expected.get(role) + if expected_types is None: + raise ValueError(f"Unsupported scene reference role {role!r}.") + resolved = self.scene.resolve(reference, path=path) + if not isinstance(resolved, expected_types): + raise TypeError( + f"Scene reference {reference!r} is {type(resolved).__name__}, " + f"not one of {tuple(value.__name__ for value in expected_types)}." + ) + + def validate_post_policy( + self, + policy: PostPolicyCfg, + *, + path: ConfigPath, + ) -> None: + """Validate one registered post-policy kind and named preset.""" + del path + if policy.kind not in _POST_POLICY_KINDS: + raise KeyError( + f"Unknown post-policy kind {policy.kind!r}; available kinds are " + f"{sorted(_POST_POLICY_KINDS)}." + ) + if policy.preset not in self.settle_preset_ids: + raise KeyError( + f"Unknown settle preset {policy.preset!r}; available presets are " + f"{sorted(self.settle_preset_ids)}." + ) + + def validate_validator( + self, + validator: ValidatorCfg, + *, + path: ConfigPath, + ) -> None: + """Validate one registered segment-validator kind.""" + del path + if validator.kind not in _VALIDATOR_KINDS: + raise KeyError( + f"Unknown validator kind {validator.kind!r}; available kinds are " + f"{sorted(_VALIDATOR_KINDS)}." + ) + + def preflight(self, program: ExpertProgramCfg) -> CompiledProgram: + """Compile and statically link every expanded semantic call.""" + self.validate_integration(program.integration, path=("integration",)) + resolver: ExpertProgramSceneResolver = _SceneManifestProgramResolver(self.scene) + compiled = ExpertProgramCompiler(resolver).compile(program) + manifest = SemanticIntegrationManifest( + scene=self.scene, + robot_profile=self.robot_profile, + call_catalog=self.call_catalog, + runtime_preset=program.integration.runtime_preset, + ) + for segment in compiled.iter_segments(): + for call in segment.calls: + if ( + type(call.call) is OperateArticulation + and call.call.target is not None + ): + self._validate_articulation_operation_target( + articulation=call.call.articulation, + handle=call.call.handle, + target=call.call.target, + path=call.source_path, + ) + linked = manifest.link_call(call.call, path=call.source_path) + if type(linked.call) is Place and linked.call.at is None: + destination = linked.affordances.get("destination") + if destination is None: + raise AssertionError( + "Linked relation Place call lacks a destination " + "affordance." + ) + self._validate_place_relation_grounder( + linked.call, + affordance=destination, + path=call.source_path, + ) + return compiled + + def validate_engine(self, engine: AtomicActionEngine) -> None: + """Require the live engine to expose every statically selected skill.""" + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + for skill_id, expected in self._required_skills.items(): + actual = engine.skills.get(skill_id) + if actual != expected: + raise IntegrationFingerprintMismatch( + f"Live skill {skill_id!r} differs from the registered " + "semantic target descriptor." + ) + + +def _profile_with_control_dt( + profile: RobotSkillProfile, + *, + control_dt: float, +) -> RobotSkillProfile: + """Return the registration profile aligned to one Gym control 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), + tracking_policy=preset.tracking_policy, + recovery_policy=preset.recovery_policy, + runner_cfg=preset.runner_cfg, + effect_monitors=preset.effect_monitors, + ) + for preset_id, preset in profile.presets.items() + }, + ) + + +def _registration_payload( + *, + scene_binding: SimulationSceneBinding, + scene: SceneManifest, + articulation_operation_targets: Mapping[str, frozenset[str]], + robot_profile_binding: SimulationRobotSkillProfileBinding, + robot_profile: RobotSkillProfile, + call_catalog: SemanticCallCatalog, + settle_presets: Mapping[str, DynamicSettleMonitorCfg], + relation_grounder_keys: frozenset[tuple[str, type[Affordance], str]], + relation_grounders: tuple[RelationTargetGrounder, ...], + handover_pose_providers: tuple[HandOverPoseProvider, ...], +) -> dict[str, object]: + """Build the versioned canonical fingerprint payload.""" + return { + "schema_version": _CATALOG_FINGERPRINT_SCHEMA_VERSION, + "scene_binding": scene_binding, + "scene_manifest": scene.entries, + "articulation_operation_targets": articulation_operation_targets, + "robot_profile_binding": robot_profile_binding, + "robot_profile": robot_profile, + "call_descriptors": tuple( + sorted( + call_catalog.descriptors.values(), + key=lambda descriptor: descriptor.call_id, + ) + ), + "relation_grounder_keys": relation_grounder_keys, + "relation_grounders": tuple( + { + "key": _relation_grounder_key(grounder), + "provider": grounder, + } + for grounder in sorted( + relation_grounders, + key=_relation_grounder_order_key, + ) + ), + "handover_pose_providers": tuple( + { + "provider_id": _handover_pose_provider_id(provider), + "provider": provider, + } + for provider in sorted( + handover_pose_providers, + key=_handover_pose_provider_id, + ) + ), + "post_policy_kinds": _POST_POLICY_KINDS, + "settle_presets": settle_presets, + "validator_kinds": _VALIDATOR_KINDS, + } + + +@dataclass(frozen=True, slots=True) +class SimulationExpertProgramRegistration: + """Exact immutable task-owned simulation integration registration.""" + + scene_binding: SimulationSceneBinding + robot_profile_binding: SimulationRobotSkillProfileBinding + call_catalog: SemanticCallCatalog = field( + default_factory=builtin_semantic_call_catalog + ) + settle_presets: Mapping[str, DynamicSettleMonitorCfg] = field( + default_factory=default_simulation_settle_presets + ) + relation_grounders: tuple[RelationTargetGrounder, ...] = () + handover_pose_providers: tuple[HandOverPoseProvider, ...] = () + catalog: ExpertProgramIntegrationCatalog = field(init=False) + + def __post_init__(self) -> None: + if type(self.scene_binding) is not SimulationSceneBinding: + raise TypeError("scene_binding must be exactly SimulationSceneBinding.") + if type(self.robot_profile_binding) is not SimulationRobotSkillProfileBinding: + raise TypeError( + "robot_profile_binding must be exactly " + "SimulationRobotSkillProfileBinding." + ) + if type(self.call_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + settle_presets = _snapshot_settle_presets(self.settle_presets) + object.__setattr__(self, "settle_presets", settle_presets) + relation_grounders = _snapshot_relation_grounders(self.relation_grounders) + object.__setattr__(self, "relation_grounders", relation_grounders) + relation_grounder_keys = frozenset( + _relation_grounder_key(grounder) for grounder in relation_grounders + ) + handover_pose_providers = _snapshot_handover_pose_providers( + self.handover_pose_providers + ) + object.__setattr__( + self, + "handover_pose_providers", + handover_pose_providers, + ) + + scene = self.scene_binding.declare() + articulation_operation_targets = _declared_articulation_operation_targets( + self.scene_binding + ) + profile = self.robot_profile_binding.declare() + selected_handover_provider = profile.grounding_providers.get("hand_over") + registered_handover_provider_ids = { + _handover_pose_provider_id(provider) for provider in handover_pose_providers + } + if ( + selected_handover_provider is not None + and selected_handover_provider not in registered_handover_provider_ids + ): + raise ValueError( + "Robot profile selects handover pose provider " + f"{selected_handover_provider!r}, but the task registration did " + "not install it." + ) + builtin_skills = { + descriptor.skill_id: descriptor + for action_type in BUILTIN_ACTION_TYPES + if (descriptor := action_type.descriptor()).agent_visible + and descriptor.binding_contract is not None + } + required_skills: dict[str, SkillDescriptor] = {} + for descriptor in self.call_catalog.descriptors.values(): + target = descriptor.target_descriptor + installed = builtin_skills.get(descriptor.skill_id) + if target is None or installed != target: + raise ValueError( + f"Semantic call {descriptor.call_id!r} targets skill " + f"{descriptor.skill_id!r}, which is not installed by the " + "standard simulation factory." + ) + required_skills[descriptor.skill_id] = target + + fingerprint = _digest( + _registration_payload( + scene_binding=self.scene_binding, + scene=scene, + articulation_operation_targets=articulation_operation_targets, + robot_profile_binding=self.robot_profile_binding, + robot_profile=profile, + call_catalog=self.call_catalog, + settle_presets=settle_presets, + relation_grounder_keys=relation_grounder_keys, + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + ) + ) + object.__setattr__( + self, + "catalog", + ExpertProgramIntegrationCatalog( + scene_registry_id=self.scene_binding.registry_id, + robot_profile_id=self.robot_profile_binding.profile_id, + scene=scene, + robot_profile=profile, + call_catalog=self.call_catalog, + relation_grounder_keys=relation_grounder_keys, + articulation_operation_targets=articulation_operation_targets, + settle_preset_ids=frozenset(settle_presets), + fingerprint=fingerprint, + _required_skills=required_skills, + ), + ) + + @property + def fingerprint(self) -> str: + """Return the canonical registration fingerprint.""" + return self.catalog.fingerprint + + def assert_unchanged(self) -> None: + """Reject nested declaration drift before live component creation.""" + scene = self.scene_binding.declare() + articulation_operation_targets = _declared_articulation_operation_targets( + self.scene_binding + ) + profile = self.robot_profile_binding.declare() + try: + relation_grounders = _snapshot_relation_grounders(self.relation_grounders) + relation_grounder_keys = frozenset( + _relation_grounder_key(grounder) for grounder in relation_grounders + ) + handover_pose_providers = _snapshot_handover_pose_providers( + self.handover_pose_providers + ) + current = _digest( + _registration_payload( + scene_binding=self.scene_binding, + scene=scene, + articulation_operation_targets=(articulation_operation_targets), + robot_profile_binding=self.robot_profile_binding, + robot_profile=profile, + call_catalog=self.call_catalog, + settle_presets=self.settle_presets, + relation_grounder_keys=relation_grounder_keys, + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + ) + ) + except (TypeError, ValueError) as exc: + raise IntegrationFingerprintMismatch( + "Expert Program integration provider declaration changed after " + "task registration." + ) from exc + if current != self.fingerprint: + raise IntegrationFingerprintMismatch( + "Expert Program integration declaration changed after task " + "registration." + ) + + def validate_scene_registry(self, registry: SceneRegistry) -> None: + """Validate a live registry against the registered scene declaration.""" + self.assert_unchanged() + self.catalog.scene.validate_registry(registry) + + def validate_robot_profile( + self, + profile: RobotSkillProfile, + *, + step_dt: float, + ) -> None: + """Validate a cadence-aligned live profile against its declaration.""" + self.assert_unchanged() + if type(profile) is not RobotSkillProfile: + raise TypeError("profile must be exactly RobotSkillProfile.") + expected = _profile_with_control_dt( + self.catalog.robot_profile, + control_dt=step_dt, + ) + if _canonical_json(profile) != _canonical_json(expected): + raise IntegrationFingerprintMismatch( + "Live robot skill profile differs from the registered declaration." + ) + + +__all__ = [ + "ExpertProgramIntegrationCatalog", + "IntegrationFingerprintMismatch", + "SimulationExpertProgramRegistration", +] diff --git a/embodichain/lab/gym/envs/expert_program/environment.py b/embodichain/lab/gym/envs/expert_program/environment.py index d0167a935..593f9a953 100644 --- a/embodichain/lab/gym/envs/expert_program/environment.py +++ b/embodichain/lab/gym/envs/expert_program/environment.py @@ -80,6 +80,7 @@ SegmentPostPolicyPort, SegmentValidatorPort, ) +from .catalog import ExpertProgramIntegrationCatalog from .cfg import ExpertProgramCfg, ExpertProgramIntegrationCfg from .compiler import ( CompiledProgram, @@ -279,6 +280,8 @@ class ExpertProgramEnvironmentAdapter: Args: factory: Environment-owned live-provider and engine factory. step_dt: Authoritative Gym control cadence in seconds. + integration_catalog: Optional immutable task-registration catalog used + for provider-free compilation. call_catalog: Optional immutable semantic call catalog. The built-in catalog is used when omitted. endpoint_adapters: Optional custom robot endpoint adapters. @@ -302,6 +305,7 @@ def __init__( factory: ExpertProgramEnvironmentFactory, *, step_dt: float, + integration_catalog: ExpertProgramIntegrationCatalog | None = None, call_catalog: SemanticCallCatalog | None = None, endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None @@ -330,7 +334,32 @@ def __init__( factory.robot_profile_id, field_name="factory.robot_profile_id", ) - selected_catalog = call_catalog or builtin_semantic_call_catalog() + if ( + integration_catalog is not None + and type(integration_catalog) is not ExpertProgramIntegrationCatalog + ): + raise TypeError( + "integration_catalog must be exactly " + "ExpertProgramIntegrationCatalog or None." + ) + if integration_catalog is not None: + if integration_catalog.scene_registry_id != scene_registry_id: + raise ValueError( + "integration_catalog scene_registry_id does not match factory." + ) + if integration_catalog.robot_profile_id != robot_profile_id: + raise ValueError( + "integration_catalog robot_profile_id does not match factory." + ) + if call_catalog is not None and ( + call_catalog is not integration_catalog.call_catalog + ): + raise ValueError( + "call_catalog cannot override the task registration catalog." + ) + selected_catalog = integration_catalog.call_catalog + else: + selected_catalog = call_catalog or builtin_semantic_call_catalog() if type(selected_catalog) is not SemanticCallCatalog: raise TypeError("call_catalog must be exactly SemanticCallCatalog or None.") if endpoint_adapters is not None and not isinstance(endpoint_adapters, Mapping): @@ -364,6 +393,7 @@ def __init__( self._scene_registry_id = scene_registry_id self._robot_profile_id = robot_profile_id self._step_dt = float(step_dt) + self._integration_catalog = integration_catalog self._call_catalog = selected_catalog self._endpoint_adapters = ( None if endpoint_adapters is None else dict(endpoint_adapters) @@ -417,6 +447,8 @@ def compile(self, program: ExpertProgramCfg) -> CompiledProgram: if type(program) is not ExpertProgramCfg: raise TypeError("program must be exactly ExpertProgramCfg.") self._validate_selection(program.integration) + if self._integration_catalog is not None: + return self._integration_catalog.preflight(program) registry = self._create_scene_registry() return ExpertProgramCompiler.from_scene_registry(registry).compile(program) diff --git a/embodichain/lab/gym/envs/expert_program/simulation.py b/embodichain/lab/gym/envs/expert_program/simulation.py index 5317dc091..b08235c32 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation.py +++ b/embodichain/lab/gym/envs/expert_program/simulation.py @@ -50,6 +50,7 @@ RobotSkillProfile, SkillPolicyPreset, ) +from embodichain.lab.sim.skills.integration import SceneEntityManifest, SceneManifest from embodichain.lab.sim.skills.scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, GRASP_AFFORDANCE_CAPABILITY, @@ -596,6 +597,139 @@ def __post_init__(self) -> None: "collision_world_mode must be SceneCollisionWorldMode or None." ) + def declare(self) -> SceneManifest: + """Project the complete provider-free scene declaration. + + Canonical topology errors are rejected here, before a simulation is + constructed. Native simulation UIDs, mesh data, link names, and joint + names remain live validation owned by :meth:`build`. + """ + objects = {item.entity_id: item for item in self.rigid_objects} + articulations = {item.entity_id: item for item in self.articulations} + links = {item.entity_id: item for item in self.links} + entries: list[SceneEntityManifest] = [] + + for binding in self.rigid_objects: + native_aliases = ( + () + if binding.simulation_uid == binding.entity_id + else (binding.simulation_uid,) + ) + defaults = ( + {} + if binding.default_grasp_affordance is None + else { + GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( + binding.default_grasp_affordance + ) + } + ) + entries.append( + SceneEntityManifest( + ref=SceneObjectRef(binding.entity_id), + aliases=(*native_aliases, *binding.aliases), + dynamics=binding.dynamics, + collision_role=binding.collision_role, + semantic_type=binding.semantic_type, + default_affordances=defaults, + ) + ) + + for binding in self.articulations: + native_aliases = ( + () + if binding.simulation_uid == binding.entity_id + else (binding.simulation_uid,) + ) + defaults = ( + {} + if binding.default_operation_affordance is None + else { + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( + SceneAffordanceRef(binding.default_operation_affordance) + ) + } + ) + entries.append( + SceneEntityManifest( + ref=SceneArticulationRef(binding.entity_id), + aliases=(*native_aliases, *binding.aliases), + dynamics=binding.dynamics, + collision_role=binding.collision_role, + semantic_type=binding.semantic_type, + default_affordances=defaults, + ) + ) + + for binding in self.links: + if binding.articulation_id not in articulations: + raise KeyError( + f"Link {binding.entity_id!r} references unbound articulation " + f"{binding.articulation_id!r}." + ) + entries.append( + SceneEntityManifest( + ref=SceneLinkRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=binding.native_link_name, + dynamics=binding.dynamics, + semantic_type=binding.semantic_type, + ) + ) + + for binding in self.antipodal_grasps: + if binding.object_id not in objects: + raise KeyError( + f"Grasp affordance {binding.entity_id!r} references unbound " + f"object {binding.object_id!r}." + ) + entries.append( + SceneEntityManifest( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneObjectRef(binding.object_id), + native_name=binding.native_name, + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_payload_type=AntipodalAffordance, + affordance_revision=binding.revision, + relative_pose=binding.relative_pose, + ) + ) + + for binding in self.articulation_operations: + if binding.articulation_id not in articulations: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound articulation {binding.articulation_id!r}." + ) + link = links.get(binding.link_id) + if link is None: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound link {binding.link_id!r}." + ) + if link.articulation_id != binding.articulation_id: + raise ValueError( + f"Operation affordance {binding.entity_id!r} and link " + f"{binding.link_id!r} select different articulations." + ) + entries.append( + SceneEntityManifest( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=link.native_link_name, + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_payload_type=ArticulationOperationAffordance, + affordance_revision=binding.revision, + ) + ) + + return SceneManifest(entries) + def build(self, simulation: SimulationManager) -> SceneRegistry: """Build the existing authoritative scene registry. @@ -854,6 +988,21 @@ def build(self, *, control_dof: int) -> ControlPartCommandProfile: } ) + def declare(self) -> ControlPartCommandProfile: + """Build a provider-free command profile from declared tuple widths.""" + widths = {len(positions) for positions in self.commands.values()} + if len(widths) > 1: + raise ValueError( + f"Command preset {self.preset_id!r} declares inconsistent command " + f"widths {sorted(widths)}." + ) + return ControlPartCommandProfile.joint_positions( + **{ + command_id: torch.tensor(positions, dtype=torch.float32) + for command_id, positions in self.commands.items() + } + ) + def _require_control_part_dof(robot: Robot, control_part: str) -> int: """Validate one native joint-backed control part and return its width.""" @@ -902,6 +1051,9 @@ def endpoint_id(self) -> str: def build(self, robot: Robot) -> ResourceEndpoint: """Build and validate one endpoint declaration for ``robot``.""" + def declare(self) -> ResourceEndpoint: + """Return the provider-free endpoint declaration.""" + @runtime_checkable class SimulationRobotResourceBinding(Protocol): @@ -918,6 +1070,9 @@ def members(self) -> tuple[str, ...]: def build(self, robot: Robot) -> RobotResource: """Build and validate one owned robot resource declaration.""" + def declare(self) -> RobotResource: + """Return the provider-free resource declaration.""" + @dataclass(frozen=True, slots=True) class RobotResourceBinding: @@ -951,6 +1106,14 @@ def build(self, robot: Robot) -> RobotResource: members=self.members, ) + def declare(self) -> RobotResource: + """Return an independently owned provider-free resource.""" + return RobotResource( + resource_id=self.resource_id, + endpoints=self.endpoints, + members=self.members, + ) + @dataclass(frozen=True, slots=True) class ControlPartEndpointBinding: @@ -981,6 +1144,14 @@ def build(self, robot: Robot) -> ResourceEndpoint: capabilities=self.capabilities, ) + def declare(self) -> ResourceEndpoint: + """Return the endpoint contract without reading a robot.""" + return ControlPartEndpoint( + control_part=self.control_part, + command_profile=self.command_preset, + capabilities=self.capabilities, + ) + @dataclass(frozen=True, slots=True) class ControlPartResourceBinding: @@ -1026,6 +1197,16 @@ def build(self, robot: Robot) -> RobotResource: members=self.members, ) + def declare(self) -> RobotResource: + """Return the resource graph without reading native control parts.""" + return RobotResource( + resource_id=self.resource_id, + endpoints={ + binding.endpoint_id: binding.declare() for binding in self.endpoints + }, + members=self.members, + ) + def _owned_nested_identifier_mapping( values: Mapping[str, Mapping[str, str]], @@ -1221,6 +1402,65 @@ def require_control_part(control_part: str) -> int: grounding_providers=self.grounding_providers, ) + def declare(self) -> RobotSkillProfile: + """Project the complete provider-free robot skill profile.""" + resources: dict[str, RobotResource] = {} + for binding in self.resources: + resource = binding.declare() + if type(resource) is not RobotResource: + raise TypeError( + f"Resource binding {binding.resource_id!r} must declare " + "exactly RobotResource." + ) + if resource.resource_id != binding.resource_id: + raise ValueError( + f"Resource binding {binding.resource_id!r} declared " + f"resource ID {resource.resource_id!r}." + ) + if resource.members != tuple(binding.members): + raise ValueError( + f"Resource binding {binding.resource_id!r} changed its " + "declared resource members." + ) + resources[resource.resource_id] = resource + + command_presets = {preset.preset_id: preset for preset in self.command_presets} + for resource in resources.values(): + for endpoint_id, endpoint in resource.endpoints.items(): + if not isinstance(endpoint, ControlPartEndpoint): + continue + preset_id = endpoint.command_profile + if preset_id is None: + continue + preset = command_presets.get(preset_id) + if preset is None: + raise KeyError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} " + f"references unknown command preset {preset_id!r}." + ) + if preset.control_part != endpoint.control_part: + raise ValueError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} uses " + f"control part {endpoint.control_part!r}, but command " + f"preset {preset_id!r} targets {preset.control_part!r}." + ) + + return RobotSkillProfile( + profile_id=self.profile_id, + resources=resources, + command_profiles={ + preset.preset_id: preset.declare() for preset in self.command_presets + }, + defaults={ + skill_id: ResourceBinding(resources=bindings) + for skill_id, bindings in self.defaults.items() + }, + presets={preset.preset_id: preset for preset in self.presets}, + default_preset=self.default_preset, + skill_presets=self.skill_presets, + grounding_providers=self.grounding_providers, + ) + __all__ = [ "AntipodalGraspAffordanceBinding", diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py index b70104d45..16e08e92f 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -37,7 +37,6 @@ import torch -from embodichain.lab.gym.envs.settling import DynamicSettleMonitorCfg from embodichain.lab.sim.atomic_actions import ( AtomicActionEngine, EntityState, @@ -69,11 +68,8 @@ MotionGenerator, ToppraPlannerCfg, ) -from embodichain.lab.sim.skills.calls import SemanticCallCatalog from embodichain.lab.sim.skills.compiler import ( - HandOverPoseProvider, RegisteredSemanticLowerer, - RelationTargetGrounder, ) from embodichain.lab.sim.skills.effects import ( ControlPartEvidenceAddress, @@ -107,15 +103,12 @@ GymPlanningObservationProvider, RuntimeTransportActionEncoder, ) +from .catalog import SimulationExpertProgramRegistration from .environment import ( ExpertProgramEnvironmentAdapter, ExpertProgramEnvironmentFactory, PlanningObservationPort, ) -from .simulation import ( - SimulationRobotSkillProfileBinding, - SimulationSceneBinding, -) from .simulation_policies import SimulationSegmentPolicyPort if TYPE_CHECKING: @@ -725,8 +718,7 @@ class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): Args: simulation: Exact live simulation that owns ``robot`` and scene UIDs. robot: Exact robot selected for planning and evidence acquisition. - scene_binding: Canonical-to-native scene declaration. - robot_profile_binding: Typed robot resource and policy declaration. + registration: Exact task-owned static and live integration declaration. step_dt: Authoritative Gym control cadence. planner_cfg: Explicit planner configuration. ``None`` selects TOPPRA for ``robot.uid``. @@ -735,7 +727,6 @@ class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): planners and isolated tests. endpoint_adapters: Explicit adapters for non-built-in resource endpoint types. - settle_presets: Optional named segment settling policies. translation_threshold: Material scene translation threshold. rotation_threshold: Material scene rotation threshold. contact_observer: Optional raw contact evidence callback. @@ -753,8 +744,7 @@ def __init__( self, simulation: SimulationManager, robot: Robot, - scene_binding: SimulationSceneBinding, - robot_profile_binding: SimulationRobotSkillProfileBinding, + registration: SimulationExpertProgramRegistration, *, step_dt: float, planner_cfg: BasePlannerCfg | None = None, @@ -762,7 +752,6 @@ def __init__( endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None ) = None, - settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, translation_threshold: float = 1.0e-4, rotation_threshold: float = 1.0e-3, contact_observer: BinaryObservationCallback | None = None, @@ -770,13 +759,11 @@ def __init__( force_observer: ScalarObservationCallback | None = None, wrench_observer: ScalarObservationCallback | None = None, ) -> None: - if type(scene_binding) is not SimulationSceneBinding: - raise TypeError("scene_binding must be exactly SimulationSceneBinding.") - if type(robot_profile_binding) is not SimulationRobotSkillProfileBinding: + if type(registration) is not SimulationExpertProgramRegistration: raise TypeError( - "robot_profile_binding must be exactly " - "SimulationRobotSkillProfileBinding." + "registration must be exactly SimulationExpertProgramRegistration." ) + registration.assert_unchanged() if planner_cfg is not None and motion_generator_factory is not None: raise ValueError( "planner_cfg and motion_generator_factory are mutually exclusive." @@ -818,8 +805,9 @@ def __init__( self._simulation = simulation self._robot = robot - self._scene_binding = scene_binding - self._robot_profile_binding = robot_profile_binding + self._registration = registration + self._scene_binding = registration.scene_binding + self._robot_profile_binding = registration.robot_profile_binding self._step_dt = _positive_finite(step_dt, field_name="step_dt") self._planner_cfg = selected_planner_cfg self._motion_generator_factory = motion_generator_factory @@ -850,8 +838,8 @@ def __init__( self._segment_policy_port = SimulationSegmentPolicyPort( simulation, robot, - scene_binding, - settle_presets=settle_presets, + registration.scene_binding, + settle_presets=registration.settle_presets, env_ids=self._env_ids, ) @@ -860,14 +848,12 @@ def from_environment( cls, environment: SimulationExpertProgramEnvironment, *, - scene_binding: SimulationSceneBinding, - robot_profile_binding: SimulationRobotSkillProfileBinding, + registration: SimulationExpertProgramRegistration, planner_cfg: BasePlannerCfg | None = None, motion_generator_factory: MotionGeneratorFactory | None = None, endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None ) = None, - settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, translation_threshold: float = 1.0e-4, rotation_threshold: float = 1.0e-3, contact_observer: BinaryObservationCallback | None = None, @@ -887,13 +873,11 @@ def from_environment( return cls( simulation, robot, - scene_binding, - robot_profile_binding, + registration, step_dt=step_dt, planner_cfg=planner_cfg, motion_generator_factory=motion_generator_factory, endpoint_adapters=endpoint_adapters, - settle_presets=settle_presets, translation_threshold=translation_threshold, rotation_threshold=rotation_threshold, contact_observer=contact_observer, @@ -933,11 +917,39 @@ def endpoint_adapters( def create_scene_registry(self) -> SceneRegistry: """Build one fresh authoritative registry from explicit bindings.""" - return self._scene_binding.build(self._simulation) + registry = self._scene_binding.build(self._simulation) + self._registration.validate_scene_registry(registry) + return registry def create_robot_skill_profile(self) -> RobotSkillProfile: - """Build the declarative profile without embedding Gym cadence.""" - return self._robot_profile_binding.build(self._robot) + """Build a profile whose every motion policy uses the Gym cadence.""" + profile = self._robot_profile_binding.build(self._robot) + aligned_presets = { + preset_id: SkillPolicyPreset( + preset_id=preset.preset_id, + schema_version=preset.schema_version, + motion_policy=replace( + preset.motion_policy, + control_dt=self._step_dt, + ), + tracking_policy=preset.tracking_policy, + recovery_policy=preset.recovery_policy, + runner_cfg=preset.runner_cfg, + effect_monitors=preset.effect_monitors, + ) + for preset_id, preset in profile.presets.items() + } + aligned = replace(profile, presets=aligned_presets) + if any( + preset.motion_policy.control_dt != self._step_dt + for preset in aligned.presets.values() + ): + raise AssertionError("Profile motion policies were not cadence-aligned.") + self._registration.validate_robot_profile( + aligned, + step_dt=self._step_dt, + ) + return aligned def create_atomic_action_engine( self, @@ -956,11 +968,13 @@ def create_atomic_action_engine( raise ValueError( "Motion generator must own the exact robot selected by the factory." ) - return AtomicActionEngine( + engine = AtomicActionEngine( motion_generator, skill_profile=profile, endpoint_adapters=self._endpoint_adapters, ) + self._registration.catalog.validate_engine(engine) + return engine def create_planning_observation_provider( self, @@ -1068,24 +1082,22 @@ def create_accepted_runtime_command_observer( def create_adapter( self, *, - call_catalog: SemanticCallCatalog | None = None, registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), - relation_grounders: Iterable[RelationTargetGrounder] = (), - handover_pose_providers: Iterable[HandOverPoseProvider] = (), effect_monitor_registry: EffectMonitorRegistry | None = None, runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), runner_cfg: ExecutionRunnerCfg | None = None, parallel_safety_validator: ParallelCommandSafetyValidator | None = None, ) -> ExpertProgramEnvironmentAdapter: """Create the exact Gym adapter with shared simulation policy ports.""" + self._registration.assert_unchanged() return ExpertProgramEnvironmentAdapter( self, step_dt=self._step_dt, - call_catalog=call_catalog, + integration_catalog=self._registration.catalog, endpoint_adapters=self._endpoint_adapters, registered_lowerers=registered_lowerers, - relation_grounders=relation_grounders, - handover_pose_providers=handover_pose_providers, + relation_grounders=self._registration.relation_grounders, + handover_pose_providers=self._registration.handover_pose_providers, effect_monitor_registry=effect_monitor_registry, runtime_transports=runtime_transports, runner_cfg=runner_cfg, @@ -1115,17 +1127,13 @@ def _create_motion_generator(self) -> MotionGenerator: def create_simulation_expert_program_adapter( environment: SimulationExpertProgramEnvironment, *, - scene_binding: SimulationSceneBinding, - robot_profile_binding: SimulationRobotSkillProfileBinding, + registration: SimulationExpertProgramRegistration, planner_cfg: BasePlannerCfg | None = None, motion_generator_factory: MotionGeneratorFactory | None = None, endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None ) = None, - relation_grounders: Iterable[RelationTargetGrounder] = (), - handover_pose_providers: Iterable[HandOverPoseProvider] = (), runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), - settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, translation_threshold: float = 1.0e-4, rotation_threshold: float = 1.0e-3, contact_observer: BinaryObservationCallback | None = None, @@ -1137,26 +1145,23 @@ def create_simulation_expert_program_adapter( """Create a complete production adapter from one standard Gym environment. This is the intended task-side one-line integration. Relation-target - grounders and embodiment-owned handover pose providers are explicit and - default to empty collections, so calls that require an uninstalled provider - remain fail-closed during program preflight. Advanced callers can retain - :class:`SimulationExpertProgramFactory` and call ``create_adapter`` directly - to install registered semantic lowerers or custom monitors. Custom endpoint - adapters and their matching Gym runtime transports are accepted here so a - non-joint endpoint remains executable through the one-line path. + grounders and embodiment-owned handover pose providers come exclusively + from ``registration``, so the statically fingerprinted objects are the exact + objects consumed by the runtime compiler. Calls that require an unregistered + provider remain fail-closed during program preflight. Advanced callers can + retain :class:`SimulationExpertProgramFactory` and call ``create_adapter`` + directly to install registered semantic lowerers or custom monitors. Custom + endpoint adapters and their matching Gym runtime transports are accepted + here so a non-joint endpoint remains executable through the one-line path. Args: environment: Standard Gym simulation environment exposing ``sim``, ``robot``, and ``step_dt``. - scene_binding: Authoritative typed scene declaration. - robot_profile_binding: Typed robot resource and policy declaration. + registration: Exact task registration used during static config loading. planner_cfg: Optional planner configuration owned by the factory. motion_generator_factory: Optional factory for one fresh motion generator. endpoint_adapters: Optional exact-type custom endpoint adapters. - relation_grounders: Explicit typed relation-target grounders. - handover_pose_providers: Explicit embodiment-owned handover pose providers. runtime_transports: Additional runtime-command-to-Gym encoders. - settle_presets: Optional named dynamic-settling policies. translation_threshold: Scene translation revision threshold. rotation_threshold: Scene rotation revision threshold. contact_observer: Optional raw contact evidence callback. @@ -1170,12 +1175,10 @@ def create_simulation_expert_program_adapter( """ factory = SimulationExpertProgramFactory.from_environment( environment, - scene_binding=scene_binding, - robot_profile_binding=robot_profile_binding, + registration=registration, planner_cfg=planner_cfg, motion_generator_factory=motion_generator_factory, endpoint_adapters=endpoint_adapters, - settle_presets=settle_presets, translation_threshold=translation_threshold, rotation_threshold=rotation_threshold, contact_observer=contact_observer, @@ -1184,8 +1187,6 @@ def create_simulation_expert_program_adapter( wrench_observer=wrench_observer, ) return factory.create_adapter( - relation_grounders=relation_grounders, - handover_pose_providers=handover_pose_providers, runtime_transports=runtime_transports, parallel_safety_validator=parallel_safety_validator, ) diff --git a/embodichain/lab/gym/envs/expert_program/simulation_policies.py b/embodichain/lab/gym/envs/expert_program/simulation_policies.py index c18dace2c..408e7cac0 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_policies.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_policies.py @@ -62,7 +62,7 @@ class _SimulationSettleTarget: native_entity: Any -def _default_settle_presets() -> Mapping[str, DynamicSettleMonitorCfg]: +def default_simulation_settle_presets() -> Mapping[str, DynamicSettleMonitorCfg]: """Return independently owned built-in post-policy presets.""" return MappingProxyType( { @@ -133,7 +133,9 @@ def __init__( raise ValueError("env_ids must contain unique values.") selected_presets = ( - _default_settle_presets() if settle_presets is None else settle_presets + default_simulation_settle_presets() + if settle_presets is None + else settle_presets ) if not isinstance(selected_presets, Mapping) or not selected_presets: raise ValueError("settle_presets must be a non-empty mapping.") @@ -712,4 +714,4 @@ def _read_pose(self, entity: Any, *, entity_id: str) -> torch.Tensor: return pose.clone() -__all__ = ["SimulationSegmentPolicyPort"] +__all__ = ["SimulationSegmentPolicyPort", "default_simulation_settle_presets"] diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index e524b6765..c3df4bba8 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -399,6 +399,7 @@ def config_to_cfg( manager_modules: list | None = None, *, source_path: str | os.PathLike[str] | None = None, + expert_program_path_override: str | os.PathLike[str] | None = None, ) -> "EmbodiedEnvCfg": """Parser configuration file into cfgs for env initialization. @@ -410,6 +411,9 @@ def config_to_cfg( relative top-level ``expert_program_path`` is resolved from this file's directory. Without it, relative paths use the current working directory. + expert_program_path_override: Optional explicit program path. This is + selected instead of the Gym-config path and resolves from the + process working directory. Returns: EmbodiedEnvCfg: A configuration object for initializing the environment. @@ -456,13 +460,23 @@ class ComponentCfg: if key not in config: log_error(f"Missing required config key: {key}") - if "expert_program_path" in config: - expert_program_path = config["expert_program_path"] - if type(expert_program_path) is not str: - raise TypeError("expert_program_path must be an exact string.") - if ( - not expert_program_path - or expert_program_path != expert_program_path.strip() + configured_expert_program_path = config.get("expert_program_path") + if expert_program_path_override is not None or "expert_program_path" in config: + if expert_program_path_override is not None: + expert_program_path = expert_program_path_override + expert_program_base_dir = None + if not isinstance(expert_program_path, (str, os.PathLike)): + raise TypeError("expert_program_path must be a string or path.") + else: + expert_program_path = configured_expert_program_path + expert_program_base_dir = ( + None if source_path is None else Path(source_path).expanduser().parent + ) + if type(expert_program_path) is not str: + raise TypeError("expert_program_path must be an exact string.") + expert_program_path_text = os.fspath(expert_program_path) + if not expert_program_path_text or ( + expert_program_path_text != expert_program_path_text.strip() ): raise ValueError( "expert_program_path must be a non-empty string without outer " @@ -471,14 +485,23 @@ class ComponentCfg: from embodichain.lab.gym.envs.expert_program.loader import ( load_expert_program, ) + from embodichain.lab.gym.utils.registration import get_env_spec - expert_program_base_dir = ( - None if source_path is None else Path(source_path).expanduser().parent - ) - env_cfg.expert_program = load_expert_program( - expert_program_path, + env_spec = get_env_spec(config["id"]) + registration = env_spec.expert_program_registration + if registration is None: + raise ValueError( + f"Environment {config['id']!r} does not register an Expert " + "Program integration catalog." + ) + registration.assert_unchanged() + expert_program = load_expert_program( + expert_program_path_text, base_dir=expert_program_base_dir, + validation_context=registration.catalog, ) + registration.catalog.preflight(expert_program) + env_cfg.expert_program = expert_program env_cfg.max_episode_steps = config.get("max_episode_steps", 300) env_cfg.num_envs = config.get("num_envs", 1) @@ -1069,6 +1092,7 @@ def build_env_cfg_from_args( gym_config, manager_modules=get_manager_modules(), source_path=gym_config_source_path, + expert_program_path_override=getattr(args, "expert_program", None), ) cfg.filter_visual_rand = args.filter_visual_rand cfg.filter_dataset_saving = args.filter_dataset_saving diff --git a/embodichain/lab/gym/utils/registration.py b/embodichain/lab/gym/utils/registration.py index 2fce236aa..d571317a0 100644 --- a/embodichain/lab/gym/utils/registration.py +++ b/embodichain/lab/gym/utils/registration.py @@ -37,6 +37,9 @@ if TYPE_CHECKING: from embodichain.lab.gym.envs import BaseEnv, EmbodiedEnvCfg + from embodichain.lab.gym.envs.expert_program import ( + SimulationExpertProgramRegistration, + ) _logger = logging.getLogger(__name__) @@ -48,12 +51,27 @@ def __init__( cls: Type[BaseEnv], max_episode_steps=None, default_kwargs: dict = None, + expert_program_registration: SimulationExpertProgramRegistration | None = None, ): """A specification for a Embodied environment.""" + if expert_program_registration is not None: + from embodichain.lab.gym.envs.expert_program import ( + SimulationExpertProgramRegistration, + ) + + if ( + type(expert_program_registration) + is not SimulationExpertProgramRegistration + ): + raise TypeError( + "expert_program_registration must be exactly " + "SimulationExpertProgramRegistration or None." + ) self.uid = uid self.cls = cls self.max_episode_steps = max_episode_steps self.default_kwargs = {} if default_kwargs is None else default_kwargs + self.expert_program_registration = expert_program_registration def make(self, **kwargs): _kwargs = self.default_kwargs.copy() @@ -76,7 +94,11 @@ def gym_spec(self): def register( - name: str, cls: Type[BaseEnv], max_episode_steps=None, default_kwargs: dict = None + name: str, + cls: Type[BaseEnv], + max_episode_steps=None, + default_kwargs: dict = None, + expert_program_registration: SimulationExpertProgramRegistration | None = None, ): """Register a Embodied environment.""" @@ -88,7 +110,11 @@ def register( if not (issubclass(cls, BaseEnv) or issubclass(cls, BaseEnv)): raise TypeError(f"Env {name} must inherit from BaseEnv or BaseEnv") REGISTERED_ENVS[name] = EnvSpec( - name, cls, max_episode_steps=max_episode_steps, default_kwargs=default_kwargs + name, + cls, + max_episode_steps=max_episode_steps, + default_kwargs=default_kwargs, + expert_program_registration=expert_program_registration, ) @@ -146,6 +172,16 @@ def make(env_id, **kwargs): return env +def get_env_spec(env_id: str) -> EnvSpec: + """Return one registered environment specification or fail closed.""" + if type(env_id) is not str or not env_id or env_id != env_id.strip(): + raise ValueError("env_id must be a non-empty string without outer whitespace.") + try: + return REGISTERED_ENVS[env_id] + except KeyError as exc: + raise KeyError(f"Env {env_id!r} not found in registry.") from exc + + def build_env(env_id: str, base_env_cfg: EmbodiedEnvCfg): """Create an environment from a registered env id. @@ -172,7 +208,14 @@ def make_vec(env_id, **kwargs): return env -def register_env(uid: str, max_episode_steps=None, override=False, **kwargs): +def register_env( + uid: str, + max_episode_steps=None, + override=False, + *, + expert_program_registration: SimulationExpertProgramRegistration | None = None, + **kwargs, +): """A decorator to register Embodied environments. Args: @@ -193,13 +236,28 @@ def register_env(uid: str, max_episode_steps=None, override=False, **kwargs): ) def _register_env(cls): - cls = register_env_function(cls, uid, override, max_episode_steps, **kwargs) + cls = register_env_function( + cls, + uid, + override, + max_episode_steps, + expert_program_registration=expert_program_registration, + **kwargs, + ) return cls return _register_env -def register_env_function(cls, uid, override=False, max_episode_steps=None, **kwargs): +def register_env_function( + cls, + uid, + override=False, + max_episode_steps=None, + *, + expert_program_registration: SimulationExpertProgramRegistration | None = None, + **kwargs, +): if uid in REGISTERED_ENVS: if override: from gymnasium.envs.registration import registry @@ -216,6 +274,7 @@ def register_env_function(cls, uid, override=False, max_episode_steps=None, **kw cls, max_episode_steps=max_episode_steps, default_kwargs=deepcopy(kwargs), + expert_program_registration=expert_program_registration, ) # Register for gym diff --git a/embodichain/lab/scripts/run_env.py b/embodichain/lab/scripts/run_env.py index 8c99f098f..78cce4723 100644 --- a/embodichain/lab/scripts/run_env.py +++ b/embodichain/lab/scripts/run_env.py @@ -32,9 +32,6 @@ import tqdm from embodichain.lab.gym.envs.demo import DemoEpisodeResult, execute_demo_episode -from embodichain.lab.gym.envs.expert_program.loader import ( - load_expert_program as _load_expert_program, -) from embodichain.lab.gym.envs.wrapper import ReplayWrapper from embodichain.lab.gym.utils.gym_utils import ( add_env_launcher_args_to_parser, @@ -858,9 +855,6 @@ def cli(argv: Sequence[str] | None = None) -> None: execute_init_hooks() env_cfg, gym_config, action_config = build_env_cfg_from_args(args) - expert_program_path = getattr(args, "expert_program", None) - if expert_program_path is not None: - env_cfg.expert_program = _load_expert_program(expert_program_path) if args.replay and args.replay_mode == "control": log_info("Dataset saving disabled for control replay mode.", color="green") diff --git a/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json index 32cf15513..cbb4ba140 100644 --- a/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json +++ b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json @@ -50,10 +50,7 @@ } } }, - "extensions": { - "grasp_samples": 10000, - "force_reannotate": false - } + "extensions": {} }, "robot": { "class_type": "URRobot", 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 6965c6f95..1a048fd41 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -41,6 +41,7 @@ ExpertProgramEnvironmentAdapter, ExpertProgramEnvironmentMixin, SimulationRigidObjectBinding, + SimulationExpertProgramRegistration, SimulationRobotSkillProfileBinding, SimulationSceneBinding, create_simulation_expert_program_adapter, @@ -53,6 +54,7 @@ FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, RecoveryPolicy, + TrackingPolicy, ) from embodichain.lab.sim.cfg import ( LightCfg, @@ -72,6 +74,7 @@ __all__ = [ "MultiSegmentsCubePickPlaceEnv", + "CUBE_EXPERT_PROGRAM_REGISTRATION", "create_cube_robot_profile_binding", "create_cube_scene_binding", ] @@ -133,7 +136,12 @@ def _create_default_robot_cfg() -> URRobotCfg: def _load_default_expert_program() -> ExpertProgramCfg: """Decode the packaged semantic program for direct instantiation.""" - return load_expert_program(get_config_path(CUBE_EXPERT_PROGRAM_PATH)) + program = load_expert_program( + get_config_path(CUBE_EXPERT_PROGRAM_PATH), + validation_context=CUBE_EXPERT_PROGRAM_REGISTRATION.catalog, + ) + CUBE_EXPERT_PROGRAM_REGISTRATION.catalog.preflight(program) + return program def _create_default_env_cfg() -> EmbodiedEnvCfg: @@ -167,10 +175,7 @@ def _create_default_env_cfg() -> EmbodiedEnvCfg: init_pos=(-0.42, -0.08, 0.5 * CUBE_SIZE), ) ] - cfg.extensions = { - "grasp_samples": 10000, - "force_reannotate": False, - } + cfg.extensions = {} cfg.events = { "settle_cube_on_reset": EventCfg( func=wait_for_dynamic_objects_to_settle, @@ -289,14 +294,28 @@ def create_cube_robot_profile_binding() -> SimulationRobotSkillProfileBinding: presets=( SkillPolicyPreset( "safe", - recovery_policy=RecoveryPolicy(tracking_error_threshold=0.08), + recovery_policy=RecoveryPolicy(), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.08, + terminal_max_abs_error=0.08, + ), ), ), default_preset="safe", ) -@register_env("MultiSegmentsCubePickPlace-v1", max_episode_steps=1200) +CUBE_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(), + robot_profile_binding=create_cube_robot_profile_binding(), +) + + +@register_env( + "MultiSegmentsCubePickPlace-v1", + max_episode_steps=1200, + expert_program_registration=CUBE_EXPERT_PROGRAM_REGISTRATION, +) class MultiSegmentsCubePickPlaceEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): """Repeatedly pick and place a cube from a semantic config program.""" @@ -307,11 +326,7 @@ def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs: Any) -> None: super().__init__(cfg, **kwargs) self._expert_program_adapter = create_simulation_expert_program_adapter( self, - scene_binding=create_cube_scene_binding( - grasp_samples=getattr(self, "grasp_samples", 10000), - force_reannotate=getattr(self, "force_reannotate", False), - ), - robot_profile_binding=create_cube_robot_profile_binding(), + registration=CUBE_EXPERT_PROGRAM_REGISTRATION, ) @property diff --git a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py index ff1166c67..2661ac884 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py +++ b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py @@ -36,6 +36,7 @@ ExpertProgramEnvironmentMixin, SimulationArticulationBinding, SimulationArticulationLinkBinding, + SimulationExpertProgramRegistration, SimulationRobotSkillProfileBinding, SimulationSceneBinding, create_simulation_expert_program_adapter, @@ -51,6 +52,7 @@ __all__ = [ "OpenDrawerEnv", + "OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION", "create_open_drawer_robot_profile_binding", "create_open_drawer_scene_binding", ] @@ -204,7 +206,17 @@ def create_open_drawer_robot_profile_binding() -> SimulationRobotSkillProfileBin ) -@register_env("OpenDrawer-v1", max_episode_steps=300) +OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_open_drawer_scene_binding(), + robot_profile_binding=create_open_drawer_robot_profile_binding(), +) + + +@register_env( + "OpenDrawer-v1", + max_episode_steps=300, + expert_program_registration=OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, +) class OpenDrawerEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): """Open a drawer through a configured semantic Expert Program.""" @@ -213,8 +225,7 @@ def __init__(self, cfg: EmbodiedEnvCfg, **kwargs: Any) -> None: super().__init__(cfg, **kwargs) self._expert_program_adapter = create_simulation_expert_program_adapter( self, - scene_binding=create_open_drawer_scene_binding(), - robot_profile_binding=create_open_drawer_robot_profile_binding(), + registration=OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, ) @property diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py new file mode 100644 index 000000000..a63cc15c8 --- /dev/null +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -0,0 +1,596 @@ +# ---------------------------------------------------------------------------- +# 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 task-registration-owned Expert Program integration catalogs.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar + +import pytest + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramIntegrationCatalog, + ExpertProgramValidationError, + IntegrationFingerprintMismatch, + SimulationArticulationLinkBinding, + SimulationExpertProgramRegistration, + SimulationSceneBinding, + decode_expert_program, +) +from embodichain.lab.gym.utils.registration import EnvSpec +from embodichain.lab.sim.atomic_actions import Affordance, PlanningContext +from embodichain.lab.sim.skills import ( + PLACE_ON_AFFORDANCE_CAPABILITY, + BoundSemanticCall, + HandOver, + HandOverPoseProvider, + HandOverPoseTargets, + OperateArticulation, + RelationTargetGrounder, + SemanticCallCatalog, + SceneAffordanceRef, + SceneEntityManifest, + SceneManifest, + SceneObjectRef, + SemanticRelationTarget, + builtin_semantic_call_catalog, +) +from embodichain_tasks.multi_segments.cube_pick_place import ( + CUBE_ROBOT_PROFILE_ID, + CUBE_SCENE_REGISTRY_ID, + create_cube_robot_profile_binding, + create_cube_scene_binding, +) +from embodichain_tasks.tableware.open_drawer import ( + DRAWER_HANDLE_AFFORDANCE_ID, + DRAWER_ROBOT_PROFILE_ID, + DRAWER_SCENE_REGISTRY_ID, + DRAWER_UID, + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, +) + + +class _CatalogRelationGrounder(RelationTargetGrounder): + """Typed relation-grounder sentinel for registration validation.""" + + capability: ClassVar[str] = "test.catalog_relation" + affordance_type: ClassVar[type[Affordance]] = Affordance + affordance_revision: ClassVar[str] = "test-v1" + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> object: + """Remain unreachable in provider-free catalog tests.""" + del relation, affordance, context + raise AssertionError("Catalog tests must not execute live providers.") + + +class _CatalogPlaceAffordance(Affordance): + """Typed provider-free payload marker for relation-linking tests.""" + + +@dataclass(frozen=True, slots=True) +class _CatalogHandOverPoseProvider(HandOverPoseProvider): + """Frozen declaration used to prove malicious drift detection.""" + + provider_id: ClassVar[str] = "test.catalog_handover" + transfer_height: float + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable in provider-free catalog tests.""" + del call, context, bound + raise AssertionError("Catalog tests must not execute live providers.") + + +class _SecondCatalogRelationGrounder(_CatalogRelationGrounder): + """Second stateless grounder used for ordering regressions.""" + + capability: ClassVar[str] = "test.catalog_relation.second" + affordance_revision: ClassVar[str] = "test-v2" + + +class _SecondCatalogHandOverPoseProvider(HandOverPoseProvider): + """Second stateless hand-over provider used for ordering regressions.""" + + provider_id: ClassVar[str] = "test.catalog_handover.second" + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable in provider-free catalog tests.""" + del call, context, bound + raise AssertionError("Catalog tests must not execute live providers.") + + +class _StatefulCatalogRelationGrounder(_CatalogRelationGrounder): + """Invalid non-dataclass provider with public instance state.""" + + capability: ClassVar[str] = "test.catalog_relation.stateful" + + def __init__(self) -> None: + self.height = 0.5 + + +class _PrivateSlotHandOverPoseProvider(HandOverPoseProvider): + """Invalid provider whose state is hidden behind a mangled slot name.""" + + __slots__ = ("__height",) + + provider_id: ClassVar[str] = "test.catalog_handover.private_slot" + + def __init__(self) -> None: + self.__height = 0.5 + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable because registration rejects this provider.""" + del call, context, bound + raise AssertionError("Rejected providers must never execute.") + + +class _InheritedCachedHandOverPoseProvider(_CatalogHandOverPoseProvider): + """Invalid non-dataclass subclass adding state to a frozen declaration.""" + + __slots__ = ("cache",) + + provider_id: ClassVar[str] = "test.catalog_handover.inherited_cache" + + def __init__(self) -> None: + super().__init__(transfer_height=0.5) + object.__setattr__(self, "cache", {}) + + +def _program_payload( + *, + scene_registry: str = CUBE_SCENE_REGISTRY_ID, + runtime_preset: str = "safe", + object_id: str = "cube", +) -> dict[str, object]: + """Return one minimal catalog-linked program payload.""" + return { + "schema_version": 1, + "program_id": "catalog_pick", + "integration": { + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": scene_registry, + "runtime_preset": runtime_preset, + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": {"kind": "pick", "object": object_id}, + }, + } + + +def _registration() -> SimulationExpertProgramRegistration: + """Build one isolated provider-free task registration.""" + return SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + ) + + +def _operate_articulation_payload( + *, + target: str, + handle: str | None = None, +) -> dict[str, object]: + """Return one named drawer-operation program with an optional handle.""" + call: dict[str, object] = { + "kind": "operate_articulation", + "articulation": DRAWER_UID, + "target": target, + } + if handle is not None: + call["handle"] = handle + return { + "schema_version": 1, + "program_id": "catalog_open_drawer", + "integration": { + "robot_profile": DRAWER_ROBOT_PROFILE_ID, + "scene_registry": DRAWER_SCENE_REGISTRY_ID, + "runtime_preset": "safe", + }, + "targets": {}, + "program": {"kind": "invoke", "call": call}, + } + + +def _place_relation_catalog( + *, + install_grounder_key: bool, +) -> ExpertProgramIntegrationCatalog: + """Build one provider-free placement catalog with an optional grounder key.""" + base = _registration().catalog + support_ref = SceneObjectRef("support") + affordance_ref = SceneAffordanceRef("support_top") + scene = SceneManifest( + ( + SceneEntityManifest(ref=SceneObjectRef("cube")), + SceneEntityManifest( + ref=support_ref, + default_affordances={ + PLACE_ON_AFFORDANCE_CAPABILITY: affordance_ref, + }, + ), + SceneEntityManifest( + ref=affordance_ref, + parent=support_ref, + native_name="support_top_surface", + affordance_capabilities=frozenset({PLACE_ON_AFFORDANCE_CAPABILITY}), + affordance_payload_type=_CatalogPlaceAffordance, + affordance_revision="test-v1", + ), + ) + ) + grounder_keys = ( + frozenset( + { + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + _CatalogPlaceAffordance, + "test-v1", + ) + } + ) + if install_grounder_key + else frozenset() + ) + return ExpertProgramIntegrationCatalog( + scene_registry_id="relation_scene", + robot_profile_id=base.robot_profile_id, + scene=scene, + robot_profile=base.robot_profile, + call_catalog=base.call_catalog, + relation_grounder_keys=grounder_keys, + articulation_operation_targets={}, + settle_preset_ids=base.settle_preset_ids, + fingerprint="0" * 64, + _required_skills={}, + ) + + +def _place_relation_payload() -> dict[str, object]: + """Return one Place(on=object) program requiring relation grounding.""" + return { + "schema_version": 1, + "program_id": "catalog_place_relation", + "integration": { + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": "relation_scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "on": "support", + }, + }, + } + + +def test_catalog_decodes_compiles_and_links_without_simulation() -> None: + """All external references are linked before a simulation is available.""" + registration = _registration() + + program = decode_expert_program( + _program_payload(), + validation_context=registration.catalog, + ) + compiled = registration.catalog.preflight(program) + + assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "pick" + + +@pytest.mark.parametrize("validation_stage", ("decode", "preflight")) +def test_catalog_rejects_unknown_named_articulation_target_at_exact_path( + validation_stage: str, +) -> None: + """Unknown provider-owned target IDs fail before simulation startup.""" + catalog = OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION.catalog + payload = _operate_articulation_payload(target="does_not_exist") + + with pytest.raises(ExpertProgramValidationError) as error: + if validation_stage == "decode": + decode_expert_program(payload, validation_context=catalog) + else: + catalog.preflight(decode_expert_program(payload)) + + assert error.value.code == "unknown_articulation_operation_target" + assert error.value.path == ("program", "call", "target") + + +@pytest.mark.parametrize("handle", (None, DRAWER_HANDLE_AFFORDANCE_ID)) +def test_catalog_accepts_named_target_through_default_or_explicit_affordance( + handle: str | None, +) -> None: + """Both handle-selection forms resolve the same registered target table.""" + catalog = OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION.catalog + program = decode_expert_program( + _operate_articulation_payload(target="open", handle=handle), + validation_context=catalog, + ) + + compiled = catalog.preflight(program) + + call = tuple(compiled.iter_segments())[0].calls[0].call + assert type(call) is OperateArticulation + assert call.target == "open" + + +def test_catalog_owns_immutable_articulation_operation_target_metadata() -> None: + """Named target IDs are a read-only task-registration catalog surface.""" + targets = ( + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION.catalog.articulation_operation_targets + ) + + assert targets == {DRAWER_HANDLE_AFFORDANCE_ID: frozenset({"open"})} + with pytest.raises(TypeError): + targets[DRAWER_HANDLE_AFFORDANCE_ID] = frozenset() # type: ignore[index] + + +def test_catalog_rejects_linked_place_relation_without_exact_grounder() -> None: + """A linked affordance cannot defer a missing typed grounder to runtime.""" + catalog = _place_relation_catalog(install_grounder_key=False) + program = decode_expert_program( + _place_relation_payload(), + validation_context=catalog, + ) + + with pytest.raises(ExpertProgramValidationError) as error: + catalog.preflight(program) + + assert error.value.code == "relation_grounder_not_registered" + assert error.value.path == ("program", "call", "on") + + +def test_catalog_accepts_linked_place_relation_with_exact_grounder_key() -> None: + """The capability, payload type, and revision must all match exactly.""" + catalog = _place_relation_catalog(install_grounder_key=True) + program = decode_expert_program( + _place_relation_payload(), + validation_context=catalog, + ) + + compiled = catalog.preflight(program) + + assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "place" + + +@pytest.mark.parametrize( + ("overrides", "path"), + ( + ({"scene_registry": "other_scene"}, ("integration",)), + ({"runtime_preset": "unknown"}, ("integration",)), + ({"object_id": "unknown_object"}, ("program", "call", "object")), + ), +) +def test_catalog_rejects_unknown_references_at_decode_time( + overrides: dict[str, str], + path: tuple[str, ...], +) -> None: + """Invalid task integration references retain exact config paths.""" + registration = _registration() + + with pytest.raises(ExpertProgramValidationError) as error: + decode_expert_program( + _program_payload(**overrides), + validation_context=registration.catalog, + ) + + assert error.value.path == path + + +def test_scene_declare_rejects_orphan_link_without_simulation() -> None: + """Canonical topology failures do not reach native entity lookup.""" + binding = SimulationSceneBinding( + registry_id="orphan_scene", + links=( + SimulationArticulationLinkBinding( + entity_id="handle", + articulation_id="missing_drawer", + native_link_name="handle_link", + ), + ), + ) + + with pytest.raises(KeyError, match="missing_drawer"): + binding.declare() + + +def test_fingerprint_is_stable_for_equivalent_declarations() -> None: + """Fresh equivalent registrations produce the same canonical digest.""" + left = _registration() + right = _registration() + + assert left.fingerprint == right.fingerprint + assert len(left.fingerprint) == 64 + + +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()) + first_relation = _CatalogRelationGrounder() + second_relation = _SecondCatalogRelationGrounder() + first_handover = _CatalogHandOverPoseProvider(transfer_height=0.6) + second_handover = _SecondCatalogHandOverPoseProvider() + common = { + "scene_binding": create_cube_scene_binding(grasp_samples=32), + "robot_profile_binding": create_cube_robot_profile_binding(), + } + forward = SimulationExpertProgramRegistration( + **common, + call_catalog=SemanticCallCatalog(descriptors), + relation_grounders=(first_relation, second_relation), + handover_pose_providers=(first_handover, second_handover), + ) + reversed_registration = SimulationExpertProgramRegistration( + **common, + call_catalog=SemanticCallCatalog(tuple(reversed(descriptors))), + relation_grounders=(second_relation, first_relation), + handover_pose_providers=(second_handover, first_handover), + ) + + assert forward.fingerprint == reversed_registration.fingerprint + + +def test_fingerprint_owns_provider_ids_and_declarative_fields() -> None: + """Provider identity and dataclass configuration are registration data.""" + provider = _CatalogHandOverPoseProvider(transfer_height=0.6) + registration = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=(_CatalogRelationGrounder(),), + handover_pose_providers=(provider,), + ) + changed_value = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=(_CatalogRelationGrounder(),), + handover_pose_providers=(_CatalogHandOverPoseProvider(transfer_height=0.7),), + ) + + assert registration.handover_pose_providers == (provider,) + assert registration.fingerprint != changed_value.fingerprint + object.__setattr__(provider, "transfer_height", 0.8) + with pytest.raises(IntegrationFingerprintMismatch, match="changed"): + registration.assert_unchanged() + + +def test_registration_rejects_duplicate_provider_keys_and_ids() -> None: + """Provider lookup tables remain unambiguous before simulation startup.""" + common = { + "scene_binding": create_cube_scene_binding(grasp_samples=32), + "robot_profile_binding": create_cube_robot_profile_binding(), + } + + with pytest.raises(ValueError, match="Duplicate relation grounder key"): + SimulationExpertProgramRegistration( + **common, + relation_grounders=( + _CatalogRelationGrounder(), + _CatalogRelationGrounder(), + ), + ) + with pytest.raises(ValueError, match="Duplicate handover pose provider"): + SimulationExpertProgramRegistration( + **common, + handover_pose_providers=( + _CatalogHandOverPoseProvider(transfer_height=0.6), + _CatalogHandOverPoseProvider(transfer_height=0.7), + ), + ) + + +def test_registration_requires_immutable_provider_tuples() -> None: + """Mutable provider containers cannot enter task registration metadata.""" + with pytest.raises(TypeError, match="relation_grounders must be an exact tuple"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=[_CatalogRelationGrounder()], # type: ignore[arg-type] + ) + with pytest.raises( + TypeError, + match="handover_pose_providers must be an exact tuple", + ): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + handover_pose_providers=[ # type: ignore[arg-type] + _CatalogHandOverPoseProvider(transfer_height=0.6) + ], + ) + + +@pytest.mark.parametrize( + ("field_name", "provider"), + ( + ("relation_grounders", _StatefulCatalogRelationGrounder()), + ("handover_pose_providers", _PrivateSlotHandOverPoseProvider()), + ( + "handover_pose_providers", + _InheritedCachedHandOverPoseProvider(), + ), + ), +) +def test_registration_rejects_stateful_non_dataclass_providers( + field_name: str, + provider: object, +) -> None: + """Public and name-mangled provider state cannot evade fingerprinting.""" + kwargs = {field_name: (provider,)} + + with pytest.raises(TypeError, match="Use a frozen dataclass"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + **kwargs, + ) + + +def test_nested_declaration_drift_is_detected_before_live_build() -> None: + """Mutable nested config cannot silently change a registered binding.""" + registration = _registration() + generator_cfg = registration.scene_binding.antipodal_grasps[0].generator_cfg + assert generator_cfg is not None + generator_cfg.antipodal_sampler_cfg.n_sample = 64 + + with pytest.raises(IntegrationFingerprintMismatch, match="changed"): + registration.assert_unchanged() + + +def test_env_spec_keeps_typed_registration_out_of_gym_kwargs() -> None: + """The integration catalog is metadata, not a duplicated Gym config source.""" + + class _Environment: + pass + + registration = _registration() + spec = EnvSpec( + "CatalogTest-v1", + _Environment, + default_kwargs={"physical_option": 3}, + expert_program_registration=registration, + ) + + assert spec.expert_program_registration is registration + assert spec.gym_spec.kwargs == {"physical_option": 3} diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 3bd98e348..9d23c9adb 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -48,6 +48,7 @@ InvokeCfg, RobotResourceBinding, SharedTickSceneProvider, + SimulationExpertProgramRegistration, SimulationExpertProgramFactory, SimulationPlanningObservationProvider, SimulationRigidObjectBinding, @@ -72,7 +73,7 @@ PlanningContext, StateDelta, TaskState, - TimedTrajectory, + TrackingPolicy, ) from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget @@ -103,7 +104,6 @@ SemanticObjectTarget, SemanticPose, SemanticRelationTarget, - SemanticValidationError, SkillPolicyPreset, ) from embodichain.lab.sim.skills.effects import ( @@ -686,9 +686,6 @@ class _ForwardedHandOverPoseProvider(HandOverPoseProvider): provider_id: ClassVar[str] = "test.handover_pose" - def __init__(self) -> None: - self.calls = 0 - def resolve( self, call: HandOver, @@ -698,7 +695,6 @@ def resolve( ) -> HandOverPoseTargets: """Return owned direct targets without embedding task-side motion code.""" del call, context, bound - self.calls += 1 pose = SemanticPose( position=(0.0, 0.0, 0.5), quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), @@ -848,7 +844,11 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: presets=( SkillPolicyPreset( "safe", - motion_policy=MotionPolicy(sample_count=17), + motion_policy=MotionPolicy(control_dt=0.01), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.037, + terminal_max_abs_error=0.019, + ), ), ), default_preset="safe", @@ -975,8 +975,10 @@ def _factory() -> tuple[SimulationExpertProgramFactory, _Robot]: SimulationExpertProgramFactory( simulation, # type: ignore[arg-type] robot, # type: ignore[arg-type] - SimulationSceneBinding(registry_id="scene"), - _profile_binding(), + SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + ), step_dt=_STEP_DT, motion_generator_factory=lambda: _motion_generator(robot), ), @@ -1120,8 +1122,10 @@ def _evidence_adapter_runtime() -> tuple[ factory = SimulationExpertProgramFactory( simulation, # type: ignore[arg-type] robot, # type: ignore[arg-type] - scene_binding, - _evidence_profile_binding(), + SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=_evidence_profile_binding(), + ), step_dt=_STEP_DT, motion_generator_factory=lambda: _motion_generator(robot), ) @@ -1530,13 +1534,17 @@ def _assert_invocation_equivalent( ) -def test_simulation_factory_preserves_declarative_motion_policy() -> None: - """Gym cadence stays on observations rather than mutating motion policy.""" +def test_simulation_factory_aligns_every_motion_policy_to_gym_step() -> None: + """Cadence alignment preserves the exact registered tracking contract.""" factory, _ = _factory() profile = factory.create_robot_skill_profile() - assert profile.presets["safe"].motion_policy.sample_count == 17 + assert profile.presets["safe"].motion_policy.control_dt == pytest.approx(_STEP_DT) + assert profile.presets["safe"].tracking_policy == TrackingPolicy.joint_position( + in_flight_max_abs_error=0.037, + terminal_max_abs_error=0.019, + ) def test_mllm_config_and_atomic_skills_share_invocations_and_verified_results( @@ -1695,8 +1703,8 @@ def test_simulation_factory_returns_exact_environment_adapter() -> None: assert factory.segment_policy_port is not None -def test_simulation_helper_forwards_semantic_grounding_extensions() -> None: - """Both explicit grounding seams reach the runtime compiler unchanged.""" +def test_simulation_helper_consumes_registered_semantic_grounding_extensions() -> None: + """Both registration-owned grounding seams reach the compiler unchanged.""" robot = _Robot() environment = SimpleNamespace( sim=_Simulation(robot), @@ -1707,11 +1715,13 @@ def test_simulation_helper_forwards_semantic_grounding_extensions() -> None: handover_provider = _ForwardedHandOverPoseProvider() adapter = create_simulation_expert_program_adapter( environment, # type: ignore[arg-type] - scene_binding=SimulationSceneBinding(registry_id="scene"), - robot_profile_binding=_profile_binding(), + registration=SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + relation_grounders=(relation_grounder,), + handover_pose_providers=(handover_provider,), + ), motion_generator_factory=lambda: _motion_generator(robot), - relation_grounders=(relation_grounder,), - handover_pose_providers=(handover_provider,), ) assembly = adapter.assemble_runtime( @@ -1728,41 +1738,35 @@ def test_simulation_helper_forwards_semantic_grounding_extensions() -> None: ) -def test_simulation_helper_handover_preflight_is_fail_closed_by_default() -> None: - """Selecting a provider ID does not infer or auto-install an implementation.""" - environment, scene_binding, profile_binding = _handover_helper_inputs() - robot = environment.robot - adapter = create_simulation_expert_program_adapter( - environment, # type: ignore[arg-type] - scene_binding=scene_binding, - robot_profile_binding=profile_binding, - motion_generator_factory=lambda: _motion_generator(robot), - ) - compiled = adapter.compile(_handover_program()) +def test_handover_registration_is_fail_closed_without_selected_provider() -> None: + """A profile-selected provider must be installed before simulation startup.""" + _, scene_binding, profile_binding = _handover_helper_inputs() - with pytest.raises(SemanticValidationError) as error: - adapter.create_bridge(compiled) - - assert error.value.diagnostic.code == "handover_grounding_provider_not_installed" + with pytest.raises(ValueError, match="selects handover pose provider"): + SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=profile_binding, + ) -def test_simulation_helper_forwards_handover_provider_to_preflight() -> None: - """An explicitly supplied embodiment provider satisfies standard preflight.""" +def test_simulation_helper_uses_registered_handover_provider_for_preflight() -> None: + """A registration-owned embodiment provider satisfies standard preflight.""" environment, scene_binding, profile_binding = _handover_helper_inputs() robot = environment.robot provider = _ForwardedHandOverPoseProvider() adapter = create_simulation_expert_program_adapter( environment, # type: ignore[arg-type] - scene_binding=scene_binding, - robot_profile_binding=profile_binding, + registration=SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=profile_binding, + handover_pose_providers=(provider,), + ), motion_generator_factory=lambda: _motion_generator(robot), - handover_pose_providers=(provider,), ) bridge = adapter.create_bridge(adapter.compile(_handover_program())) assert bridge is not None - assert provider.calls == 0 def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joints() -> ( @@ -1795,8 +1799,10 @@ def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joint adapter = create_simulation_expert_program_adapter( environment, # type: ignore[arg-type] - scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), - robot_profile_binding=profile_binding, + registration=SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=profile_binding, + ), motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] endpoint_adapters={_MobileEndpoint: _MobileEndpointAdapter()}, runtime_transports=(_MobileTransportEncoder(),), 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 a54203df9..6e6fe9495 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 @@ -40,6 +40,7 @@ from embodichain_tasks.multi_segments.cube_pick_place import ( # noqa: E402 CUBE_ROBOT_PROFILE_ID, CUBE_SCENE_REGISTRY_ID, + CUBE_EXPERT_PROGRAM_REGISTRATION, MultiSegmentsCubePickPlaceEnv, _create_default_env_cfg, create_cube_robot_profile_binding, @@ -70,6 +71,8 @@ def test_registered_task_uses_shared_expert_program_mixin() -> None: spec = REGISTERED_ENVS["MultiSegmentsCubePickPlace-v1"] assert spec.cls is MultiSegmentsCubePickPlaceEnv assert spec.max_episode_steps == 1200 + assert spec.expert_program_registration is CUBE_EXPERT_PROGRAM_REGISTRATION + assert "expert_program_registration" not in spec.default_kwargs assert issubclass(MultiSegmentsCubePickPlaceEnv, ExpertProgramEnvironmentMixin) assert issubclass(MultiSegmentsCubePickPlaceEnv, EmbodiedEnv) @@ -82,11 +85,7 @@ def test_gym_config_selects_packaged_expert_program() -> None: assert payload["expert_program_path"] == ( "../../expert_program/multi_segments/repeated_cube_pick_place.yaml" ) - extensions = payload["env"]["extensions"] - assert extensions == { - "grasp_samples": 10000, - "force_reannotate": False, - } + assert payload["env"]["extensions"] == {} settle = payload["env"]["events"]["settle_cube_on_reset"] assert settle["func"] == "wait_for_dynamic_objects_to_settle" assert settle["mode"] == "reset" @@ -130,7 +129,10 @@ def test_robot_profile_calibrates_physical_tracking_tolerance() -> None: binding = create_cube_robot_profile_binding() assert binding.presets[0].preset_id == "safe" - assert binding.presets[0].recovery_policy.tracking_error_threshold == 0.08 + tracking = binding.presets[0].tracking_policy + assert tracking.in_flight is not None + assert tracking.in_flight.metrics[0].tolerance == 0.08 + assert tracking.terminal.metrics[0].tolerance == 0.08 def test_task_initialization_delegates_to_shared_simulation_factory( @@ -162,14 +164,16 @@ def fake_create_adapter(environment, **kwargs): assert env.expert_program_adapter is adapter assert captured["environment"] is env + registration = captured["registration"] + assert registration is CUBE_EXPERT_PROGRAM_REGISTRATION assert ( - captured["scene_binding"] - .antipodal_grasps[0] - .generator_cfg.antipodal_sampler_cfg.n_sample - == 48 + registration.scene_binding.antipodal_grasps[ + 0 + ].generator_cfg.antipodal_sampler_cfg.n_sample + == 10000 ) - assert captured["scene_binding"].antipodal_grasps[0].force_reannotate is True - assert captured["robot_profile_binding"].profile_id == CUBE_ROBOT_PROFILE_ID + assert registration.scene_binding.antipodal_grasps[0].force_reannotate is False + assert registration.robot_profile_binding.profile_id == CUBE_ROBOT_PROFILE_ID def test_task_config_compiles_through_real_simulation_factory( diff --git a/tests/gym/envs/tasks/test_open_drawer.py b/tests/gym/envs/tasks/test_open_drawer.py index 81893c5a0..725d3c77d 100644 --- a/tests/gym/envs/tasks/test_open_drawer.py +++ b/tests/gym/envs/tasks/test_open_drawer.py @@ -43,6 +43,7 @@ DRAWER_OPEN_POSITION, DRAWER_ROBOT_PROFILE_ID, DRAWER_UID, + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, OpenDrawerEnv, create_open_drawer_scene_binding, ) @@ -68,6 +69,8 @@ def test_registered_drawer_task_uses_shared_expert_program_mixin() -> None: spec = REGISTERED_ENVS["OpenDrawer-v1"] assert spec.cls is OpenDrawerEnv + assert spec.expert_program_registration is OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION + assert "expert_program_registration" not in spec.default_kwargs assert issubclass(OpenDrawerEnv, ExpertProgramEnvironmentMixin) assert issubclass(OpenDrawerEnv, EmbodiedEnv) assert "create_demo_action_list" not in OpenDrawerEnv.__dict__ @@ -144,8 +147,10 @@ def fake_create_adapter(environment, **kwargs): assert env.expert_program_adapter is adapter assert captured["environment"] is env - assert captured["scene_binding"].links[0].native_link_name == "handle_xpos" - assert captured["robot_profile_binding"].profile_id == DRAWER_ROBOT_PROFILE_ID + registration = captured["registration"] + assert registration is OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION + assert registration.scene_binding.links[0].native_link_name == "handle_xpos" + assert registration.robot_profile_binding.profile_id == DRAWER_ROBOT_PROFILE_ID def test_task_config_compiles_through_real_simulation_factory( diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index db3119281..c0cd8ee2d 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -27,6 +27,7 @@ from tensordict import TensorDict +from embodichain.lab.gym.envs.expert_program import IntegrationFingerprintMismatch from embodichain.lab.gym.utils.gym_utils import ( add_env_launcher_args_to_parser, build_env_cfg_from_args, @@ -39,6 +40,11 @@ ) from embodichain.lab.sim.robots import URRobotCfg from embodichain.utils.utility import load_config, save_config +from embodichain_tasks.multi_segments.cube_pick_place import ( + CUBE_EXPERT_PROGRAM_REGISTRATION, + CUBE_ROBOT_PROFILE_ID, + CUBE_SCENE_REGISTRY_ID, +) class TestInitRolloutBufferFromConfig: @@ -513,7 +519,7 @@ class TestConfigToCfgFromFile: def _minimal_gym_config() -> dict[str, object]: """Return a minimal config that reaches the generic parser.""" return { - "id": "EmbodiedEnv-v1", + "id": "MultiSegmentsCubePickPlace-v1", "env": {}, "robot": { "class_type": "URRobot", @@ -529,9 +535,9 @@ def _expert_program_payload() -> dict[str, object]: "schema_version": 1, "program_id": "configured_pick", "integration": { - "robot_profile": "default_robot", - "scene_registry": "default_scene", - "runtime_preset": "default_runtime", + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": CUBE_SCENE_REGISTRY_ID, + "runtime_preset": "safe", }, "targets": {}, "program": { @@ -585,7 +591,7 @@ def test_expert_program_path_is_resolved_from_gym_config_source( ) assert cfg.expert_program.program_id == "configured_pick" - assert cfg.expert_program.integration.scene_registry == "default_scene" + assert cfg.expert_program.integration.scene_registry == CUBE_SCENE_REGISTRY_ID def test_build_env_cfg_loads_source_relative_expert_program( self, @@ -621,6 +627,81 @@ def test_build_env_cfg_loads_source_relative_expert_program( assert cfg.expert_program.program_id == "configured_pick" + def test_cli_program_override_is_selected_and_loaded_once( + self, + tmp_path, + monkeypatch, + ) -> None: + """The CLI override replaces the Gym path at the single loader boundary.""" + from embodichain.lab.gym.envs.expert_program import loader + + gym_path = tmp_path / "gym_config.json" + override_path = tmp_path / "override.yaml" + save_config(override_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = "must_not_be_loaded.yaml" + save_config(gym_path, config) + args = argparse.Namespace( + gym_config=str(gym_path), + expert_program=str(override_path), + num_envs=1, + device="cpu", + headless=True, + renderer=None, + gpu_id=0, + arena_space=2.0, + max_episodes=None, + filter_visual_rand=False, + filter_dataset_saving=False, + preview=False, + action_config=None, + ) + calls: list[str] = [] + original = loader.load_expert_program + + def load_once(path, **kwargs): + calls.append(str(path)) + return original(path, **kwargs) + + monkeypatch.setattr(loader, "load_expert_program", load_once) + + cfg, _, _ = build_env_cfg_from_args(args) + + assert cfg.expert_program.program_id == "configured_pick" + assert calls == [str(override_path)] + + def test_registration_drift_fails_before_program_loader( + self, + tmp_path, + monkeypatch, + ) -> None: + """The config boundary checks registration integrity before file loading.""" + from embodichain.lab.gym.envs.expert_program import loader + + program_path = tmp_path / "program.yaml" + save_config(program_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = str(program_path) + generator_cfg = CUBE_EXPERT_PROGRAM_REGISTRATION.scene_binding.antipodal_grasps[ + 0 + ].generator_cfg + assert generator_cfg is not None + sampler_cfg = generator_cfg.antipodal_sampler_cfg + monkeypatch.setattr(sampler_cfg, "n_sample", sampler_cfg.n_sample + 1) + loader_calls: list[str] = [] + + def unexpected_load(path, **kwargs): + del kwargs + loader_calls.append(str(path)) + raise AssertionError("Drift must fail before program loading.") + + monkeypatch.setattr(loader, "load_expert_program", unexpected_load) + + with pytest.raises(IntegrationFingerprintMismatch, match="changed"): + config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + assert loader_calls == [] + def test_config_to_cfg_uses_cwd_without_source_path( self, tmp_path, diff --git a/tests/lab/scripts/test_run_env.py b/tests/lab/scripts/test_run_env.py index 788a89f9b..1b0b1d1d8 100644 --- a/tests/lab/scripts/test_run_env.py +++ b/tests/lab/scripts/test_run_env.py @@ -24,11 +24,13 @@ import torch from embodichain.lab.gym.envs.demo import DemoEpisodeResult +from embodichain.lab.gym.envs.expert_program.loader import ( + load_expert_program as _load_expert_program, +) from embodichain.lab.gym.utils.gym_utils import merge_args_with_gym_config from embodichain.lab.scripts import run_env from embodichain.lab.scripts.run_env import ( _create_parser, - _load_expert_program, _run_replay_control_loop, generate_function, ) @@ -549,7 +551,7 @@ def test_cli_aborts_before_closing_environment_once(monkeypatch) -> None: assert env.events == [abort_event, abort_event, ("close", None)] -def test_cli_injects_decoded_expert_program_before_environment_creation( +def test_cli_uses_program_already_loaded_by_config_builder( monkeypatch, ) -> None: """The CLI attaches the strict program config to the environment config.""" @@ -569,16 +571,13 @@ def test_cli_injects_decoded_expert_program_before_environment_creation( monkeypatch.setattr(run_env, "_create_parser", lambda: parser) monkeypatch.setattr(run_env, "discover_task_packages", lambda: None) monkeypatch.setattr(run_env, "execute_init_hooks", lambda: None) - monkeypatch.setattr( - run_env, - "build_env_cfg_from_args", - lambda parsed_args: (env_cfg, {"id": GYM_ID}, {}), - ) - monkeypatch.setattr( - run_env, - "_load_expert_program", - MagicMock(return_value=decoded_program), - ) + + def build(parsed_args): + assert parsed_args is args + env_cfg.expert_program = decoded_program + return env_cfg, {"id": GYM_ID}, {} + + monkeypatch.setattr(run_env, "build_env_cfg_from_args", build) monkeypatch.setattr(run_env.gymnasium, "make", make) monkeypatch.setattr(run_env, "main", lambda *args, **kwargs: None) monkeypatch.setattr( From ca0ef14cde7dcdd3586f10544aceb238926c9212 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 17:06:38 +0800 Subject: [PATCH 15/29] fix(expert-program): reject opaque catalog values --- .../lab/gym/envs/expert_program/catalog.py | 19 ++++++++---- tests/gym/envs/expert_program/test_catalog.py | 29 +++++++++++++++++++ 2 files changed, 43 insertions(+), 5 deletions(-) diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index a683caa1a..fe5e5d9ca 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -143,9 +143,18 @@ def _canonical_value(value: object) -> object: for data_field in fields(value) } return {"type": _qualified_name(value), "fields": metadata} - # Provider objects are not executable catalog data. Their declared type is - # still part of the integration surface, while live identity is excluded. - return {"provider_type": _qualified_name(value)} + raise TypeError( + "Registration fingerprint metadata contains unsupported value type " + f"{_qualified_name(value)!r}. Values must be complete declarative data; " + "live or opaque objects cannot be fingerprinted by type alone." + ) + + +def _provider_fingerprint_declaration(provider: object) -> object: + """Return the complete canonical declaration for one validated provider.""" + if is_dataclass(provider): + return provider + return {"provider_type": _qualified_name(provider)} def _canonical_json(value: object) -> str: @@ -838,7 +847,7 @@ def _registration_payload( "relation_grounders": tuple( { "key": _relation_grounder_key(grounder), - "provider": grounder, + "provider": _provider_fingerprint_declaration(grounder), } for grounder in sorted( relation_grounders, @@ -848,7 +857,7 @@ def _registration_payload( "handover_pose_providers": tuple( { "provider_id": _handover_pose_provider_id(provider), - "provider": provider, + "provider": _provider_fingerprint_declaration(provider), } for provider in sorted( handover_pose_providers, diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py index a63cc15c8..1d6c07fa7 100644 --- a/tests/gym/envs/expert_program/test_catalog.py +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -174,6 +174,25 @@ def __init__(self) -> None: object.__setattr__(self, "cache", {}) +@dataclass(frozen=True, slots=True) +class _OpaqueHandOverPoseProvider(HandOverPoseProvider): + """Provider declaration containing an unsupported opaque nested value.""" + + provider_id: ClassVar[str] = "test.catalog_handover.opaque" + opaque: object + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable because fingerprinting rejects this provider.""" + del call, context, bound + raise AssertionError("Opaque providers must never reach runtime.") + + def _program_payload( *, scene_registry: str = CUBE_SCENE_REGISTRY_ID, @@ -495,6 +514,16 @@ def test_fingerprint_owns_provider_ids_and_declarative_fields() -> None: registration.assert_unchanged() +def test_fingerprint_rejects_opaque_nested_declaration_values() -> None: + """Unknown nested values cannot silently collapse to their Python type.""" + with pytest.raises(TypeError, match="unsupported value type"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + handover_pose_providers=(_OpaqueHandOverPoseProvider(opaque=object()),), + ) + + def test_registration_rejects_duplicate_provider_keys_and_ids() -> None: """Provider lookup tables remain unambiguous before simulation startup.""" common = { From 8b8ad58b2cd8ee69ab7f9f025b1177b6ab02e8db Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 17:31:56 +0800 Subject: [PATCH 16/29] feat(expert-program): configure semantic action options --- .../atomic_actions/robot_skill_profiles.md | 30 ++ .../lab/gym/envs/expert_program/catalog.py | 3 +- .../expert_program/simulation_environment.py | 1 + embodichain/lab/sim/skills/compiler.py | 80 +++++- embodichain/lab/sim/skills/integration.py | 98 +++++++ embodichain/lab/sim/skills/profiles.py | 260 +++++++++++++++++- .../multi_segments/cube_pick_place.py | 6 + .../tableware/open_drawer.py | 10 +- .../envs/expert_program/test_environment.py | 16 +- .../envs/expert_program/test_simulation.py | 8 +- .../test_simulation_environment.py | 36 ++- .../sim/skills/test_articulation_semantics.py | 10 +- tests/sim/skills/test_compiler.py | 153 +++++++++-- ...o_semantic_runtime_dynamic_recovery_gpu.py | 7 +- tests/sim/skills/test_integration.py | 142 +++++++++- tests/sim/skills/test_profiles.py | 167 ++++++++++- 16 files changed, 944 insertions(+), 83 deletions(-) diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md index cbddb478b..fc84034ba 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -77,7 +77,10 @@ from embodichain.lab.sim.atomic_actions import ( FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, ControlPartCommandProfile, + HandOverOptions, MotionPolicy, + PickUpOptions, + PlaceOptions, ) from embodichain.lab.sim.skills import ( COMPOSITE_EFFECT_MONITOR_ID, @@ -140,6 +143,11 @@ profile = RobotSkillProfile( presets={ "default": SkillPolicyPreset( preset_id="default", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + "hand_over": HandOverOptions(), + }, motion_policy=MotionPolicy(strategy="ik_interp"), effect_monitors={ semantic_id: EffectMonitorRef( @@ -195,6 +203,28 @@ A linked call receives an effective immutable preset snapshot with Other presets, and scenes without dynamic collision entities, retain their configured collision mode. +## Configure semantic action behavior with the preset + +`SkillPolicyPreset.action_option_templates` is the required, typed action- +behavior table for semantic calls that can select the preset. Each key is the +exact semantic call ID (`pick`, `place`, `hand_over`, or +`operate_articulation`), and each value must be the target action's exact frozen +`ActionOptions` dataclass. Static linking rejects a missing entry, an unknown +call ID, or an options value of the wrong exact type before simulation starts. + +The preset owns independent snapshots of each template. Pick and HandOver +grounding only replace their compiler-owned dynamic target fields; distances, +directions, waypoint counts, and other reusable behavior remain configuration. +A registered semantic lowerer may build a goal but cannot return replacement +options. This keeps task extensions from silently moving action parameters back +into Python code. + +Pick's `downstream_object_target_poses` and HandOver's +`middle_object_pose`/`final_object_pose` are reserved for the semantic compiler +and must remain empty in a template. Planner choice, sample count, tracking, +recovery, runner timing, and effect monitors stay in their dedicated preset +fields rather than `ActionOptions`. + ## Select semantic effect monitors with the preset A {class}`SkillPolicyPreset` owns one coherent runtime choice: planning and diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index fe5e5d9ca..06963c265 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -80,7 +80,7 @@ from .simulation import SimulationRobotSkillProfileBinding, SimulationSceneBinding from .simulation_policies import default_simulation_settle_presets -_CATALOG_FINGERPRINT_SCHEMA_VERSION = 1 +_CATALOG_FINGERPRINT_SCHEMA_VERSION = 2 _POST_POLICY_KINDS = frozenset({"wait_stable"}) _VALIDATOR_KINDS = frozenset({"object_near_target"}) @@ -810,6 +810,7 @@ def _profile_with_control_dt( recovery_policy=preset.recovery_policy, runner_cfg=preset.runner_cfg, effect_monitors=preset.effect_monitors, + action_option_templates=preset.action_option_templates, ) for preset_id, preset in profile.presets.items() }, diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py index 16e08e92f..f2a5f2782 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -936,6 +936,7 @@ def create_robot_skill_profile(self) -> RobotSkillProfile: recovery_policy=preset.recovery_policy, runner_cfg=preset.runner_cfg, effect_monitors=preset.effect_monitors, + action_option_templates=preset.action_option_templates, ) for preset_id, preset in profile.presets.items() } diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index d9c1b814d..dd2db8254 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -20,9 +20,10 @@ from abc import ABC, abstractmethod from collections.abc import Iterable, Mapping +from copy import deepcopy from dataclasses import dataclass, field, replace from types import MappingProxyType -from typing import ClassVar +from typing import ClassVar, TypeVar from uuid import uuid4 import torch @@ -40,6 +41,7 @@ PlaceGoal, PlaceOptions, OperateArticulationGoal, + OperateArticulationOptions, PlanningContext, PoseGoalValue, SceneArticulationOperationGeometry, @@ -96,6 +98,8 @@ SceneObjectRef, ) +OptionT = TypeVar("OptionT", bound=ActionOptions) + def _validate_identifier(value: str, *, field_name: str) -> str: """Return one exact non-empty identifier.""" @@ -377,8 +381,15 @@ def lower( *, context: PlanningContext, bound: BoundSemanticCall, + option_template: ActionOptions, ) -> SemanticLowering: - """Lower one registered value to goal/options without changing policy.""" + """Lower a registered value with one owned typed option template. + + The lowerer must return :class:`SemanticLowering` with + ``skill_options=None``. The supplied template is an owned read-only + input for goal grounding; the selected policy preset remains the sole + owner of action options. + """ @dataclass(frozen=True, slots=True) @@ -1047,6 +1058,10 @@ def ground( raise AssertionError(f"Unsupported analyzed call {type(call).__name__}.") bound = analyzed.bound + if lowering.skill_options is None: + raise AssertionError( + "Semantic lowering must resolve a non-None action-options value." + ) invocation = ActionInvocation( skill_id=bound.linked.descriptor.skill_id, goal=lowering.goal, @@ -1278,13 +1293,15 @@ def _lower_pick( call.object, affordance=grasp_ref, ) + option_template = self._action_option_template(analyzed, PickUpOptions) return SemanticLowering( goal=GraspGoal(semantics=semantics), - skill_options=PickUpOptions( + skill_options=replace( + option_template, downstream_object_target_poses=tuple( self._ground_object_target(target, context) for target in analyzed.downstream_object_targets - ) + ), ), ) @@ -1322,7 +1339,10 @@ def _lower_place( xpos = self._compose_object_to_eef( object_target, held.object_to_eef, context ) - return SemanticLowering(goal=PlaceGoal(xpos=xpos), skill_options=PlaceOptions()) + return SemanticLowering( + goal=PlaceGoal(xpos=xpos), + skill_options=self._action_option_template(analyzed, PlaceOptions), + ) def _lower_handover( self, @@ -1366,9 +1386,11 @@ def _lower_handover( else targets.final ) final = self._ground_object_target(final_target, context) + option_template = self._action_option_template(analyzed, HandOverOptions) return SemanticLowering( goal=GraspGoal(semantics=semantics), - skill_options=HandOverOptions( + skill_options=replace( + option_template, middle_object_pose=middle, final_object_pose=final, ), @@ -1501,7 +1523,11 @@ def _lower_operate_articulation( source_position=source_position, target_position=target, target_displacement=displacement, - ) + ), + skill_options=self._action_option_template( + analyzed, + OperateArticulationOptions, + ), ) def _lower_registered( @@ -1522,19 +1548,24 @@ def _lower_registered( f"No lowerer is installed for {call.call_id!r}.", tuple(self._registered_lowerers), ) + descriptor = analyzed.bound.linked.descriptor + target = descriptor.target_descriptor + assert target is not None + option_template = self._action_option_template( + analyzed, + target.options_type, + ) lowering = lowerer.lower( call, context=context, bound=analyzed.bound, + option_template=deepcopy(option_template), ) if type(lowering) is not SemanticLowering: raise TypeError( "RegisteredSemanticLowerer.lower() must return exactly " "SemanticLowering." ) - descriptor = analyzed.bound.linked.descriptor - target = descriptor.target_descriptor - assert target is not None expected_goal_types = ( target.goal_type if isinstance(target.goal_type, tuple) @@ -1545,13 +1576,32 @@ def _lower_registered( f"Lowerer {call.call_id!r} produced {type(lowering.goal).__name__}; " f"target skill {target.skill_id!r} expects {target.goal_type!r}." ) - if lowering.skill_options is not None and ( - type(lowering.skill_options) is not target.options_type - ): + if lowering.skill_options is not None: raise TypeError( - f"Lowerer {call.call_id!r} produced incompatible skill options." + f"Lowerer {call.call_id!r} must not return skill_options; " + "the selected policy preset owns action options." + ) + return replace(lowering, skill_options=deepcopy(option_template)) + + @staticmethod + def _action_option_template( + analyzed: AnalyzedSemanticCall, + expected_type: type[OptionT], + ) -> OptionT: + """Return one owned exact template selected by semantic call ID.""" + semantic_id = analyzed.call.semantic_id + try: + template = analyzed.bound.preset.action_option_template(semantic_id) + except KeyError as exc: # pragma: no cover - static linking owns this check + raise AssertionError( + f"Linked call {semantic_id!r} has no action-option template." + ) from exc + if type(template) is not expected_type: + raise AssertionError( + f"Linked call {semantic_id!r} has {type(template).__name__}; " + f"expected exact {expected_type.__name__}." ) - return lowering + return template def _ground_effect_spec( self, diff --git a/embodichain/lab/sim/skills/integration.py b/embodichain/lab/sim/skills/integration.py index 3040d9469..823a21969 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -28,6 +28,8 @@ DynamicCollisionMode, DisjointResourceSlots, DisjointSlotEndpoints, + HandOverOptions, + PickUpOptions, SkillResourceSlot, ) @@ -547,6 +549,81 @@ def __post_init__(self) -> None: tuple(self.call_catalog.descriptors), ) ) + unknown_option_ids = sorted( + set(preset.action_option_templates).difference(known_semantic_ids) + ) + if unknown_option_ids: + semantic_id = unknown_option_ids[0] + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_action_option_call", + ( + "integration", + "robot_profile", + "presets", + preset_id, + "action_option_templates", + semantic_id, + ), + f"Action-option configuration references unknown semantic " + f"call {semantic_id!r}.", + tuple(self.call_catalog.descriptors), + ) + ) + for semantic_id, options in preset.action_option_templates.items(): + descriptor = self.call_catalog.descriptors[semantic_id] + target = descriptor.target_descriptor + assert target is not None + option_path = ( + "integration", + "robot_profile", + "presets", + preset_id, + "action_option_templates", + semantic_id, + ) + if type(options) is not target.options_type: + raise SemanticValidationError( + SemanticDiagnostic( + "incompatible_action_option_template", + option_path, + f"Semantic call {semantic_id!r} targets options type " + f"{target.options_type.__name__}, not " + f"{type(options).__name__}.", + (target.options_type.__name__,), + ) + ) + if semantic_id == Pick.call_kind: + assert type(options) is PickUpOptions + if options.downstream_object_target_poses: + raise SemanticValidationError( + SemanticDiagnostic( + "reserved_action_option_field", + (*option_path, "downstream_object_target_poses"), + "Pick downstream targets are compiler-owned and " + "the template field must be empty.", + ) + ) + if semantic_id == HandOver.call_kind: + assert type(options) is HandOverOptions + if options.middle_object_pose is not None: + raise SemanticValidationError( + SemanticDiagnostic( + "reserved_action_option_field", + (*option_path, "middle_object_pose"), + "HandOver middle_object_pose is compiler-owned and " + "the template field must be None.", + ) + ) + if options.final_object_pose is not None: + raise SemanticValidationError( + SemanticDiagnostic( + "reserved_action_option_field", + (*option_path, "final_object_pose"), + "HandOver final_object_pose is compiler-owned and " + "the template field must be None.", + ) + ) if self.runtime_preset is not None: _validate_identifier( self.runtime_preset, @@ -686,6 +763,26 @@ def link_call( descriptor, path=(*path, "preset"), ) + preset = self.robot_profile.presets[preset_id] + if descriptor.call_id not in preset.action_option_templates: + option_path = ( + "integration", + "robot_profile", + "presets", + preset_id, + "action_option_templates", + descriptor.call_id, + ) + raise SemanticValidationError( + SemanticDiagnostic( + "missing_action_option_template", + option_path, + f"Policy preset {preset_id!r} has no action-option template " + f"for semantic call {descriptor.call_id!r} selected at " + f"{_render_path(path)}.", + tuple(preset.action_option_templates), + ) + ) return LinkedSemanticCall( call=normalized_call, descriptor=descriptor, @@ -1220,6 +1317,7 @@ def link_call( recovery_policy=preset.recovery_policy, runner_cfg=preset.runner_cfg, effect_monitors=preset.effect_monitors, + action_option_templates=preset.action_option_templates, ) return BoundSemanticCall._create( linked=linked, diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 893b54bb6..5025af7b2 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -20,11 +20,14 @@ from abc import ABC, abstractmethod from copy import deepcopy -from dataclasses import dataclass, field +from dataclasses import dataclass, field, fields, is_dataclass +from enum import Enum from itertools import product from types import MappingProxyType from typing import ClassVar, Mapping, TYPE_CHECKING +import torch + from embodichain.lab.sim.atomic_actions.bindings import ( ActionBinding, EndpointBinding, @@ -37,6 +40,7 @@ JointPositionCommand, ) from embodichain.lab.sim.atomic_actions.core import SkillDescriptor +from embodichain.lab.sim.atomic_actions.invocation import ActionOptions from embodichain.lab.sim.atomic_actions.policies import MotionPolicy, RecoveryPolicy from embodichain.lab.sim.atomic_actions.tracking import ( JOINT_POSITION_CHANNEL, @@ -106,6 +110,167 @@ def _validate_identifier(value: str, *, field_name: str) -> str: return value +def _snapshot_graph_tokens( + value: object, + *, + path: str, + visited: set[int], +) -> set[tuple[object, ...]]: + """Collect identities for every mutable value and tensor storage. + + Immutable containers are traversed because they may retain mutable leaves. + Unknown opaque values fail closed: an action-options declaration must expose + its complete snapshot graph through dataclass fields and built-in containers. + """ + if value is None or type(value) in { + bool, + int, + float, + complex, + str, + bytes, + range, + slice, + torch.device, + torch.dtype, + }: + return set() + if isinstance(value, (Enum, type)): + return set() + + value_id = id(value) + if value_id in visited: + return set() + visited.add(value_id) + + if isinstance(value, torch.Tensor): + tokens: set[tuple[object, ...]] = {("object", value_id)} + storage = value.untyped_storage() + if storage.nbytes() > 0: + tokens.add( + ( + "tensor_storage", + value.device.type, + value.device.index, + storage.data_ptr(), + ) + ) + return tokens + if is_dataclass(value) and not isinstance(value, type): + tokens = {("object", value_id)} + for data_field in fields(value): + tokens.update( + _snapshot_graph_tokens( + getattr(value, data_field.name), + path=f"{path}.{data_field.name}", + visited=visited, + ) + ) + return tokens + if type(value) is dict: + tokens = {("object", value_id)} + for key, nested in value.items(): + tokens.update( + _snapshot_graph_tokens( + key, + path=f"{path}.", + visited=visited, + ) + ) + tokens.update( + _snapshot_graph_tokens( + nested, + path=f"{path}[{key!r}]", + visited=visited, + ) + ) + return tokens + if type(value) in {list, set, bytearray}: + tokens = {("object", value_id)} + for index, nested in enumerate(value): + tokens.update( + _snapshot_graph_tokens( + nested, + path=f"{path}[{index}]", + visited=visited, + ) + ) + return tokens + if type(value) in {tuple, frozenset}: + tokens = set() + for index, nested in enumerate(value): + tokens.update( + _snapshot_graph_tokens( + nested, + path=f"{path}[{index}]", + visited=visited, + ) + ) + return tokens + raise TypeError( + f"Action-options snapshot graph contains unsupported opaque value " + f"{type(value).__module__}.{type(value).__qualname__} at {path}." + ) + + +def _snapshot_action_options(options: ActionOptions) -> ActionOptions: + """Return one exact action-options snapshot with no mutable aliasing.""" + if not isinstance(options, ActionOptions): + raise TypeError( + "action_option_templates values must be ActionOptions instances." + ) + option_type = type(options) + dataclass_params = option_type.__dict__.get("__dataclass_params__") + dataclass_fields = option_type.__dict__.get("__dataclass_fields__") + if ( + dataclass_params is None + or dataclass_fields is None + or dataclass_params.frozen is not True + ): + raise TypeError( + "action_option_templates values must be exact frozen @dataclass " + "declarations, not inherited undecorated ActionOptions subclasses." + ) + if hasattr(options, "__dict__"): + raise TypeError("action_option_templates values must not carry __dict__ state.") + field_names = {data_field.name for data_field in fields(options)} + declared_slots: set[str] = set() + for base in option_type.__mro__: + slots = base.__dict__.get("__slots__", ()) + if isinstance(slots, str): + declared_slots.add(slots) + else: + declared_slots.update(slots) + opaque_slots = declared_slots.difference(field_names, {"__weakref__"}) + if opaque_slots: + raise TypeError( + "action_option_templates values must not carry non-dataclass " + f"slot state: {sorted(opaque_slots)}." + ) + snapshot = deepcopy(options) + if type(snapshot) is not option_type or snapshot is options: + raise TypeError( + "action_option_templates values must support independent deep-copy " + "snapshots of their exact type." + ) + source_tokens = _snapshot_graph_tokens( + options, + path=option_type.__name__, + visited=set(), + ) + snapshot_tokens = _snapshot_graph_tokens( + snapshot, + path=option_type.__name__, + visited=set(), + ) + if source_tokens.intersection(snapshot_tokens): + raise TypeError( + "action_option_templates values must support independently owned " + "snapshots without shared mutable objects or tensor storage." + ) + return snapshot + + def _normalize_identifier_set( values: frozenset[str], *, @@ -400,6 +565,21 @@ class ResourceEndpointAdapter(ABC): endpoint_type: ClassVar[type[ResourceEndpoint]] """Exact endpoint declaration type accepted by this adapter.""" + runtime_transport_ids: ClassVar[frozenset[str]] + """Exact endpoint-command transport IDs this adapter may resolve.""" + + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] + """Exact immutable runtime-target value types this adapter may resolve.""" + + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] + """Exact ``(provider_id, revision)`` tracking-feedback routes emitted.""" + + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] + """Exact ``(projector_id, revision)`` desired-state routes emitted.""" + + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] + """Exact ``(provider_id, revision)`` effect-evidence routes emitted.""" + @abstractmethod def resolve( self, @@ -423,6 +603,26 @@ class ControlPartEndpointAdapter(ResourceEndpointAdapter): adapter_id: ClassVar[str] = "control_part" endpoint_type: ClassVar[type[ResourceEndpoint]] = ControlPartEndpoint + runtime_transport_ids: ClassVar[frozenset[str]] = frozenset( + {JointPositionTarget.TRANSPORT_ID} + ) + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + JointPositionTarget, + ) + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + {("planning_context.robot", "1")} + ) + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + {("joint_position_payload", "1")} + ) + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + { + ( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ) + } + ) def resolve( self, @@ -717,7 +917,7 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True, init=False) class SkillPolicyPreset: - """Versioned planning, tracking, recovery, runner, and monitor bundle.""" + """Versioned policies and typed semantic-call option templates.""" preset_id: str schema_version: int @@ -728,11 +928,14 @@ class SkillPolicyPreset: _recovery_policy: RecoveryPolicy _runner_cfg: ExecutionRunnerCfg _effect_monitors: Mapping[str, EffectMonitorRef] + _action_option_templates: Mapping[str, ActionOptions] def __init__( self, preset_id: str, - schema_version: int = 1, + *, + action_option_templates: Mapping[str, ActionOptions], + schema_version: int = 2, motion_policy: MotionPolicy | None = None, tracking_policy: TrackingPolicy | None = None, recovery_policy: RecoveryPolicy | None = None, @@ -744,10 +947,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 != 1: + if schema_version != 2: raise ValueError( "Unsupported SkillPolicyPreset.schema_version " - f"{schema_version}; supported versions are [1]." + f"{schema_version}; supported versions are [2]." ) if required_planner is not None: _validate_identifier( @@ -801,6 +1004,17 @@ def __init__( "effect_monitors values must be EffectMonitorRef instances." ) normalized_effect_monitors[semantic_id] = monitor_ref.snapshot() + if not isinstance(action_option_templates, Mapping): + raise TypeError("action_option_templates must be a mapping.") + normalized_action_option_templates: dict[str, ActionOptions] = {} + for semantic_id, options in action_option_templates.items(): + _validate_identifier( + semantic_id, + field_name="SkillPolicyPreset action-option semantic IDs", + ) + normalized_action_option_templates[semantic_id] = _snapshot_action_options( + options + ) object.__setattr__(self, "preset_id", preset_id) object.__setattr__(self, "schema_version", schema_version) object.__setattr__(self, "required_planner", required_planner) @@ -813,6 +1027,11 @@ def __init__( "_effect_monitors", MappingProxyType(normalized_effect_monitors), ) + object.__setattr__( + self, + "_action_option_templates", + MappingProxyType(normalized_action_option_templates), + ) @property def motion_policy(self) -> MotionPolicy: @@ -844,6 +1063,35 @@ def effect_monitors(self) -> Mapping[str, EffectMonitorRef]: } ) + @property + def action_option_templates(self) -> Mapping[str, ActionOptions]: + """Return owned option templates keyed by exact semantic call ID.""" + return MappingProxyType( + { + semantic_id: _snapshot_action_options(options) + for semantic_id, options in self._action_option_templates.items() + } + ) + + def action_option_template(self, semantic_id: str) -> ActionOptions: + """Return one owned template for an exact semantic call ID. + + Raises: + KeyError: If this preset does not declare the semantic call. + """ + _validate_identifier( + semantic_id, + field_name="SkillPolicyPreset action-option semantic ID", + ) + try: + template = self._action_option_templates[semantic_id] + except KeyError as exc: + raise KeyError( + f"Preset {self.preset_id!r} has no action-option template for " + f"semantic call {semantic_id!r}." + ) from exc + return _snapshot_action_options(template) + def snapshot(self) -> SkillPolicyPreset: """Return an independently owned preset value.""" return SkillPolicyPreset( @@ -854,7 +1102,7 @@ def snapshot(self) -> SkillPolicyPreset: recovery_policy=self.recovery_policy, runner_cfg=self.runner_cfg, effect_monitors=self.effect_monitors, - required_planner=self.required_planner, + action_option_templates=self.action_option_templates, ) 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 1a048fd41..eef86d858 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -53,6 +53,8 @@ CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, + PickUpOptions, + PlaceOptions, RecoveryPolicy, TrackingPolicy, ) @@ -294,6 +296,10 @@ def create_cube_robot_profile_binding() -> SimulationRobotSkillProfileBinding: presets=( SkillPolicyPreset( "safe", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + }, recovery_policy=RecoveryPolicy(), tracking_policy=TrackingPolicy.joint_position( in_flight_max_abs_error=0.08, diff --git a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py index 2661ac884..e645e0ba4 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py +++ b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py @@ -46,6 +46,7 @@ CARTESIAN_POSE_CAPABILITY, GRASP_CAPABILITY, JOINT_POSITION_CAPABILITY, + OperateArticulationOptions, ) from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics from embodichain.lab.sim.skills.profiles import SkillPolicyPreset @@ -201,7 +202,14 @@ def create_open_drawer_robot_profile_binding() -> SimulationRobotSkillProfileBin defaults={ "operate_articulation": {"primary": "right_manipulator"}, }, - presets=(SkillPolicyPreset("safe"),), + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={ + "operate_articulation": OperateArticulationOptions(), + }, + ), + ), default_preset="safe", ) diff --git a/tests/gym/envs/expert_program/test_environment.py b/tests/gym/envs/expert_program/test_environment.py index b1be4a6a0..a7bd5edf9 100644 --- a/tests/gym/envs/expert_program/test_environment.py +++ b/tests/gym/envs/expert_program/test_environment.py @@ -69,7 +69,10 @@ GRASP_CAPABILITY, JOINT_POSITION_CAPABILITY, MotionPolicy, + OperateArticulationOptions, + PickUpOptions, PlanningContext, + PlaceOptions, RobotObservation, TaskState, ) @@ -201,6 +204,10 @@ def _robot_profile( presets={ "safe": SkillPolicyPreset( "safe", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + }, motion_policy=safe_motion_policy, ) }, @@ -279,7 +286,14 @@ def resource(resource_id: str) -> RobotResource: ) for hand in ("left_hand", "right_hand") }, - presets={"safe": SkillPolicyPreset("safe")}, + presets={ + "safe": SkillPolicyPreset( + "safe", + action_option_templates={ + "operate_articulation": OperateArticulationOptions(), + }, + ) + }, default_preset="safe", ) diff --git a/tests/gym/envs/expert_program/test_simulation.py b/tests/gym/envs/expert_program/test_simulation.py index 652df4702..b3a6fc664 100644 --- a/tests/gym/envs/expert_program/test_simulation.py +++ b/tests/gym/envs/expert_program/test_simulation.py @@ -44,6 +44,7 @@ ArticulationOperationAffordance, CARTESIAN_POSE_CAPABILITY, GRASP_CAPABILITY, + PickUpOptions, ) from embodichain.lab.sim.skills import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, @@ -244,7 +245,12 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: ), ), defaults={"pick_up": {"primary": "manipulator"}}, - presets=(SkillPolicyPreset("safe"),), + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={"pick": PickUpOptions()}, + ), + ), default_preset="safe", ) diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 9d23c9adb..54188235b 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -68,9 +68,12 @@ FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, HeldObjectState, + HandOverOptions, MotionPolicy, ObservedArticulationJointState, PlanningContext, + PickUpOptions, + PlaceOptions, StateDelta, TaskState, TrackingPolicy, @@ -844,11 +847,21 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: presets=( SkillPolicyPreset( "safe", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + }, motion_policy=MotionPolicy(control_dt=0.01), tracking_policy=TrackingPolicy.joint_position( in_flight_max_abs_error=0.037, terminal_max_abs_error=0.019, ), + runner_cfg=ExecutionRunnerCfg( + command_timeout=0.37, + safe_stop_timeout=0.61, + minimum_cycle_time=0.04, + hold_on_completion=False, + ), ), ), default_preset="safe", @@ -900,7 +913,12 @@ def _handover_profile_binding() -> SimulationRobotSkillProfileBinding: defaults={ "hand_over": {"source": "left", "destination": "right"}, }, - presets=(SkillPolicyPreset("safe"),), + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={"hand_over": HandOverOptions()}, + ), + ), default_preset="safe", grounding_providers={ "hand_over": _ForwardedHandOverPoseProvider.provider_id, @@ -1029,7 +1047,19 @@ def _evidence_profile_binding() -> SimulationRobotSkillProfileBinding: "pick_up": {"primary": "manipulator"}, "place": {"primary": "manipulator"}, }, - presets=(SkillPolicyPreset("evidence"),), + presets=( + SkillPolicyPreset( + "evidence", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + }, + runner_cfg=ExecutionRunnerCfg( + minimum_cycle_time=0.0, + hold_on_completion=False, + ), + ), + ), default_preset="evidence", ) @@ -1788,7 +1818,7 @@ def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joint }, ), ), - presets=(SkillPolicyPreset("runtime"),), + presets=(SkillPolicyPreset("runtime", action_option_templates={}),), default_preset="runtime", ) environment = SimpleNamespace( diff --git a/tests/sim/skills/test_articulation_semantics.py b/tests/sim/skills/test_articulation_semantics.py index 4cdcd1227..ec8910bdc 100644 --- a/tests/sim/skills/test_articulation_semantics.py +++ b/tests/sim/skills/test_articulation_semantics.py @@ -35,6 +35,7 @@ JOINT_POSITION_CAPABILITY, ObservedArticulationJointState, OperateArticulationGoal, + OperateArticulationOptions, PlanningContext, RobotObservation, SceneSnapshot, @@ -210,7 +211,14 @@ def _profile() -> RobotSkillProfile: grasp=torch.tensor((1.0,)), ) }, - presets={"safe": SkillPolicyPreset("safe")}, + presets={ + "safe": SkillPolicyPreset( + "safe", + action_option_templates={ + "operate_articulation": OperateArticulationOptions(), + }, + ) + }, default_preset="safe", ) diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index 06f79f924..eafaaff78 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -26,6 +26,7 @@ import torch from embodichain.lab.sim.atomic_actions import ( + ActionOptions, Affordance, AntipodalAffordance, AtomicActionEngine, @@ -42,9 +43,11 @@ HeldObjectState, MotionPolicy, ObjectSemantics, + OperateArticulationOptions, PickUp, PickUpOptions, PlaceGoal, + PlaceOptions, PlanningContext, RobotObservation, SceneEntityPose, @@ -125,6 +128,33 @@ _PICK_TARGET = PickUp.descriptor() +def _action_option_templates(*, registered: bool = False) -> dict[str, object]: + """Return complete exact option declarations for the selected catalog.""" + templates: dict[str, object] = { + "pick": PickUpOptions(), + "place": PlaceOptions(), + "hand_over": HandOverOptions(), + "operate_articulation": OperateArticulationOptions(), + } + if registered: + templates["vendor.inspect"] = PickUpOptions() + return templates + + +def _preset( + preset_id: str, + *, + registered: bool = False, + **kwargs: object, +) -> SkillPolicyPreset: + """Build one complete schema-v2 test preset.""" + kwargs.setdefault( + "action_option_templates", + _action_option_templates(registered=registered), + ) + return SkillPolicyPreset(preset_id, **kwargs) + + class _PoseProvider: """Return a fixed pose while exposing observation call count.""" @@ -174,14 +204,19 @@ class _InspectLowerer(RegisteredSemanticLowerer): call_id: ClassVar[str] = "vendor.inspect" schema_version: ClassVar[int] = 1 + def __init__(self) -> None: + self.option_templates: list[ActionOptions] = [] + def lower( self, call: RegisteredSemanticCall, *, context: PlanningContext, bound: object, + option_template: ActionOptions, ) -> SemanticLowering: del call, context, bound + self.option_templates.append(option_template) return SemanticLowering( goal=GraspGoal( semantics=ObjectSemantics( @@ -190,7 +225,6 @@ def lower( entity_id="cube", ) ), - skill_options=PickUpOptions(), ) @@ -198,12 +232,8 @@ class _DerivedGraspGoal(GraspGoal): """Executable subclass that an extension must not smuggle into the core.""" -class _DerivedPickUpOptions(PickUpOptions): - """Options subclass that must fail the registered target contract.""" - - class _SubclassOutputLowerer(RegisteredSemanticLowerer): - """Try to bypass exact target contracts with executable subclasses.""" + """Try to bypass exact goal or preset-owned options contracts.""" call_id: ClassVar[str] = "vendor.inspect" schema_version: ClassVar[int] = 1 @@ -217,8 +247,9 @@ def lower( *, context: PlanningContext, bound: BoundSemanticCall, + option_template: ActionOptions, ) -> SemanticLowering: - del call, context, bound + del call, context, bound, option_template semantics = ObjectSemantics( affordance=AntipodalAffordance(), geometry={}, @@ -227,11 +258,10 @@ def lower( if self.output == "goal": return SemanticLowering( goal=_DerivedGraspGoal(semantics=semantics), - skill_options=PickUpOptions(), ) return SemanticLowering( goal=GraspGoal(semantics=semantics), - skill_options=_DerivedPickUpOptions(), + skill_options=PickUpOptions(pre_grasp_distance=0.99), ) @@ -348,7 +378,11 @@ def _scene_registry( return registry, (cube_provider, table_provider) -def _profile(*, preset: SkillPolicyPreset | None = None) -> RobotSkillProfile: +def _profile( + *, + preset: SkillPolicyPreset | None = None, + registered: bool = False, +) -> RobotSkillProfile: return RobotSkillProfile( profile_id="test_robot", resources={ @@ -372,12 +406,20 @@ def _profile(*, preset: SkillPolicyPreset | None = None) -> RobotSkillProfile: grasp=torch.tensor([1.0]), ) }, - presets={"safe": SkillPolicyPreset("safe") if preset is None else preset}, + presets={ + "safe": ( + _preset("safe", registered=registered) if preset is None else preset + ) + }, default_preset="safe", ) -def _dual_profile(*, provider_id: str | None = "dual_center") -> RobotSkillProfile: +def _dual_profile( + *, + provider_id: str | None = "dual_center", + preset: SkillPolicyPreset | None = None, +) -> RobotSkillProfile: resources = { side: RobotResource( resource_id=side, @@ -408,7 +450,7 @@ def _dual_profile(*, provider_id: str | None = "dual_center") -> RobotSkillProfi "pick_up": ResourceBinding({"primary": "left"}), "hand_over": ResourceBinding({"source": "left", "destination": "right"}), }, - presets={"safe": SkillPolicyPreset("safe")}, + presets={"safe": _preset("safe") if preset is None else preset}, default_preset="safe", grounding_providers=({} if provider_id is None else {"hand_over": provider_id}), ) @@ -453,7 +495,7 @@ def _integration( profile: RobotSkillProfile | None = None, supports_dynamic_collision_world: bool = False, ) -> tuple[SemanticIntegrationManifest, AtomicActionEngine]: - selected_profile = _profile() if profile is None else profile + selected_profile = _profile(registered=registered) if profile is None else profile catalog = builtin_semantic_call_catalog() if registered: assert _PICK_TARGET.binding_contract is not None @@ -608,7 +650,7 @@ def test_curated_analysis_selects_exact_preset_monitor_without_creating_it() -> def test_curated_analysis_rejects_explicitly_missing_monitor() -> None: registry, _ = _scene_registry() profile = _profile( - preset=SkillPolicyPreset("safe", effect_monitors={}), + preset=_preset("safe", effect_monitors={}), ) compiler, _ = _compiler(registry, profile=profile) @@ -622,7 +664,7 @@ def test_uninstalled_effect_monitor_fails_analysis_without_factory_creation() -> registry, providers = _scene_registry() factory = _CountingRelationMonitorFactory() profile = _profile( - preset=SkillPolicyPreset( + preset=_preset( "safe", effect_monitors={ "pick": EffectMonitorRef("test.not_installed", "1"), @@ -647,7 +689,7 @@ def test_invalid_effect_monitor_config_fails_analysis_without_side_effects() -> registry, providers = _scene_registry() factory = _CountingRelationMonitorFactory() profile = _profile( - preset=SkillPolicyPreset( + preset=_preset( "safe", effect_monitors={ "pick": EffectMonitorRef( @@ -859,10 +901,21 @@ def test_handover_effect_spec_binds_source_and_destination_relations() -> None: def test_registered_call_without_monitor_has_no_effect_contract() -> None: registry, _ = _scene_registry() factory = _CountingRelationMonitorFactory() + templates = _action_option_templates(registered=True) + templates["vendor.inspect"] = PickUpOptions(pre_grasp_distance=0.07) + profile = _profile( + preset=_preset( + "safe", + registered=True, + action_option_templates=templates, + ) + ) + lowerer = _InspectLowerer() compiler, _ = _compiler( registry, registered=True, - registered_lowerers=(_InspectLowerer(),), + registered_lowerers=(lowerer,), + profile=profile, effect_monitor_registry=EffectMonitorRegistry((factory,)), ) workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) @@ -874,14 +927,21 @@ 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 + options = grounded.invocation.skill_options + assert type(options) is PickUpOptions + assert options.pre_grasp_distance == 0.07 + assert len(lowerer.option_templates) == 1 + assert lowerer.option_templates[0] is not options + assert type(lowerer.option_templates[0]) is PickUpOptions assert factory.calls == 0 def test_registered_monitor_without_effect_grounder_fails_during_analysis() -> None: registry, _ = _scene_registry() profile = _profile( - preset=SkillPolicyPreset( + preset=_preset( "safe", + registered=True, effect_monitors={ "vendor.inspect": EffectMonitorRef( COMPOSITE_EFFECT_MONITOR_ID, @@ -923,7 +983,17 @@ def test_ground_wraps_effect_monitor_factory_contract_failure_with_path() -> Non def test_analysis_is_provider_free_and_propagates_object_target() -> None: registry, providers = _scene_registry() - compiler, engine = _compiler(registry) + templates = _action_option_templates() + templates["pick"] = PickUpOptions( + pick_object_part="top", + pre_grasp_distance=0.08, + ) + compiler, engine = _compiler( + registry, + profile=_profile( + preset=_preset("safe", action_option_templates=templates), + ), + ) drop = SemanticPose((0.4, 0.2, 0.3), (1.0, 0.0, 0.0, 0.0)) workflow = compiler.analyze( @@ -944,6 +1014,8 @@ def test_analysis_is_provider_free_and_propagates_object_target() -> None: assert grounded.invocation.goal.semantics.entity_id == "cube" options = grounded.invocation.skill_options assert type(options) is PickUpOptions + assert options.pick_object_part == "top" + assert options.pre_grasp_distance == 0.08 torch.testing.assert_close( options.downstream_object_target_poses[0], drop.to_matrix(), @@ -954,7 +1026,7 @@ def test_analysis_is_provider_free_and_propagates_object_target() -> None: def test_grounded_safe_invocation_requires_registered_dynamic_collision() -> None: registry, _ = _scene_registry(dynamic_collision=True) profile = _profile( - preset=SkillPolicyPreset( + preset=_preset( "safe", motion_policy=MotionPolicy(strategy="motion_gen"), tracking_policy=TrackingPolicy.joint_position( @@ -1150,7 +1222,14 @@ def fail_after_capture( def test_handover_uses_profile_selected_named_provider_and_stops_lookahead() -> None: registry, providers = _scene_registry() - profile = _dual_profile() + templates = _action_option_templates() + templates["hand_over"] = HandOverOptions( + receive_pick_object_part="top", + pre_grasp_distance=0.06, + ) + profile = _dual_profile( + preset=_preset("safe", action_option_templates=templates), + ) manifest = SemanticIntegrationManifest( scene=SceneManifest.from_registry(registry), robot_profile=profile, @@ -1203,6 +1282,8 @@ def test_handover_uses_profile_selected_named_provider_and_stops_lookahead() -> assert provider.calls == 2 options = handover.invocation.skill_options assert type(options) is HandOverOptions + assert options.receive_pick_object_part == "top" + assert options.pre_grasp_distance == 0.06 assert type(options.middle_object_pose) is SceneEntityPose assert options.middle_object_pose.entity_id == "table_top" assert options.final_object_pose[0, 3].item() == pytest.approx(0.8) @@ -1294,7 +1375,17 @@ def test_relation_call_requires_exact_typed_versioned_grounder() -> None: def test_place_uses_verified_object_to_eef_transform() -> None: registry, _ = _scene_registry() - compiler, engine = _compiler(registry) + templates = _action_option_templates() + templates["place"] = PlaceOptions( + lift_height=0.22, + cartesian_waypoint_count=3, + ) + compiler, engine = _compiler( + registry, + profile=_profile( + preset=_preset("safe", action_option_templates=templates), + ), + ) drop = SemanticPose((0.5, -0.2, 0.4), (1.0, 0.0, 0.0, 0.0)) workflow = compiler.analyze((Place(object=SceneObjectRef("cube"), at=drop),)) pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) @@ -1310,6 +1401,10 @@ def test_place_uses_verified_object_to_eef_transform() -> None: grounded = compiler.ground(workflow, 0, context) assert type(grounded.invocation.goal) is PlaceGoal + options = grounded.invocation.skill_options + assert type(options) is PlaceOptions + assert options.lift_height == 0.22 + assert options.cartesian_waypoint_count == 3 expected = torch.bmm(drop.to_matrix().repeat(2, 1, 1), object_to_eef) torch.testing.assert_close(grounded.invocation.goal.xpos, expected) engine.resolve(grounded.invocation) @@ -1430,8 +1525,14 @@ def test_registered_lowerer_is_explicit_and_opaque_to_lookahead() -> None: engine.resolve(grounded.invocation) -@pytest.mark.parametrize("output", ["goal", "options"]) -def test_registered_lowerer_cannot_return_target_subclasses(output: str) -> None: +@pytest.mark.parametrize( + ("output", "message"), + (("goal", "produced"), ("options", "must not return skill_options")), +) +def test_registered_lowerer_cannot_replace_owned_contracts( + output: str, + message: str, +) -> None: registry, _ = _scene_registry() compiler, _ = _compiler( registry, @@ -1440,7 +1541,7 @@ def test_registered_lowerer_cannot_return_target_subclasses(output: str) -> None ) workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) - with pytest.raises(TypeError, match="produced|incompatible"): + with pytest.raises(TypeError, match=message): compiler.ground(workflow, 0, _context(registry)) diff --git a/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py index 0aa252c9f..90b4a0924 100644 --- a/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py +++ b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py @@ -39,6 +39,7 @@ ExecutionRunnerCfg, MotionPolicy, MoveEndEffector, + MoveEndEffectorOptions, PlanningContext, RecoveryPolicy, RuntimeCommandFrame, @@ -114,8 +115,9 @@ def lower( *, context: PlanningContext, bound: BoundSemanticCall, + option_template: MoveEndEffectorOptions, ) -> SemanticLowering: - del bound + del bound, option_template values = call.arguments.get("xpos") if type(values) is not tuple or len(values) != 16: raise ValueError("xpos must contain one flattened 4x4 pose matrix.") @@ -181,6 +183,9 @@ def _profile() -> RobotSkillProfile: presets={ "safe": SkillPolicyPreset( "safe", + action_option_templates={ + CALL_ID: MoveEndEffectorOptions(), + }, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_COUNT, diff --git a/tests/sim/skills/test_integration.py b/tests/sim/skills/test_integration.py index c28c99fb6..af6b76b23 100644 --- a/tests/sim/skills/test_integration.py +++ b/tests/sim/skills/test_integration.py @@ -34,7 +34,11 @@ EntityState, FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, + HandOverOptions, MotionPolicy, + OperateArticulationOptions, + PickUpOptions, + PlaceOptions, ) from embodichain.lab.sim.skills.calls import ( Pick, @@ -80,6 +84,22 @@ ) +def _action_option_templates() -> dict[str, object]: + """Return exact built-in semantic-call option declarations.""" + return { + "pick": PickUpOptions(), + "place": PlaceOptions(), + "hand_over": HandOverOptions(), + "operate_articulation": OperateArticulationOptions(), + } + + +def _preset(preset_id: str, **kwargs: object) -> SkillPolicyPreset: + """Build one complete schema-v2 test preset.""" + kwargs.setdefault("action_option_templates", _action_option_templates()) + return SkillPolicyPreset(preset_id, **kwargs) + + class _NeverObservedStateProvider: """Fail if provider-backed state leaks into static validation.""" @@ -179,7 +199,7 @@ def _semantic_integration( skill_presets: dict[str, str] | None = None, runtime_preset: str | None = None, ) -> SemanticIntegrationManifest: - selected_preset = SkillPolicyPreset("safe") if preset is None else preset + selected_preset = _preset("safe") if preset is None else preset presets = {selected_preset.preset_id: selected_preset} presets.update( { @@ -443,7 +463,7 @@ def test_semantic_integration_rejects_monitor_for_unknown_call_with_path() -> No with pytest.raises(SemanticValidationError) as error: _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "safe", effect_monitors={ unknown_semantic_id: EffectMonitorRef("test.monitor", "1") @@ -466,6 +486,98 @@ def test_semantic_integration_rejects_monitor_for_unknown_call_with_path() -> No ) +def test_semantic_integration_rejects_unknown_action_option_call() -> None: + registry, _ = _scene_registry(with_default=True) + + with pytest.raises(SemanticValidationError) as error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + action_option_templates={"vendor.unknown": PickUpOptions()}, + ), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_action_option_call" + assert diagnostic.path[-2:] == ( + "action_option_templates", + "vendor.unknown", + ) + + +def test_semantic_integration_validates_exact_action_option_type() -> None: + registry, _ = _scene_registry(with_default=True) + + with pytest.raises(SemanticValidationError) as error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + action_option_templates={"pick": PlaceOptions()}, + ), + ) + + assert error.value.diagnostic.code == "incompatible_action_option_template" + assert error.value.diagnostic.path[-1] == "pick" + + +def test_semantic_integration_rejects_compiler_owned_option_fields() -> None: + registry, _ = _scene_registry(with_default=True) + + with pytest.raises(SemanticValidationError) as pick_error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + action_option_templates={ + "pick": PickUpOptions( + downstream_object_target_poses=(torch.eye(4),) + ) + }, + ), + ) + assert pick_error.value.diagnostic.code == "reserved_action_option_field" + assert pick_error.value.diagnostic.path[-1] == ("downstream_object_target_poses") + + with pytest.raises(SemanticValidationError) as handover_error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + action_option_templates={ + "hand_over": HandOverOptions( + middle_object_pose=torch.eye(4), + ) + }, + ), + ) + assert handover_error.value.diagnostic.code == "reserved_action_option_field" + assert handover_error.value.diagnostic.path[-1] == "middle_object_pose" + + +def test_static_link_requires_selected_preset_action_option_template() -> None: + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset("safe", action_option_templates={}), + ) + + with pytest.raises(SemanticValidationError) as error: + integration.link_call(Pick(object=SceneObjectRef("cube"))) + + assert error.value.diagnostic.code == "missing_action_option_template" + assert error.value.diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "action_option_templates", + "pick", + ) + assert "selected at call" in error.value.diagnostic.message + + def test_scene_manifest_reports_structured_pathful_diagnostic() -> None: manifest = SceneManifest((SceneEntityManifest(ref=SceneObjectRef("cube")),)) @@ -607,7 +719,7 @@ def test_safe_preset_requires_dynamic_collision_for_dynamic_scene( ) integration = _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "safe", motion_policy=MotionPolicy( strategy="motion_gen", @@ -642,7 +754,7 @@ def test_safe_preset_rejects_unsupported_dynamic_planner_before_observation() -> ) integration = _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "safe", motion_policy=MotionPolicy(strategy="motion_gen"), ), @@ -678,9 +790,9 @@ def test_per_skill_safe_preset_is_conservatively_preflighted() -> None: pick_skill_id = builtin_semantic_call_catalog().descriptors["pick"].skill_id integration = _semantic_integration( registry, - preset=SkillPolicyPreset("fast"), + preset=_preset("fast"), additional_presets=( - SkillPolicyPreset( + _preset( "safe", motion_policy=MotionPolicy(strategy="motion_gen"), ), @@ -704,11 +816,11 @@ def test_fully_overridden_safe_default_is_not_reachable() -> None: catalog = builtin_semantic_call_catalog() integration = _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "safe", motion_policy=MotionPolicy(strategy="motion_gen"), ), - additional_presets=(SkillPolicyPreset("fast"),), + additional_presets=(_preset("fast"),), skill_presets={ descriptor.skill_id: "fast" for descriptor in catalog.descriptors.values() }, @@ -730,11 +842,11 @@ def test_runtime_non_safe_override_makes_safe_default_unreachable() -> None: ) integration = _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "safe", motion_policy=MotionPolicy(strategy="motion_gen"), ), - additional_presets=(SkillPolicyPreset("fast"),), + additional_presets=(_preset("fast"),), runtime_preset="fast", ) engine = _engine_for_integration(integration) @@ -754,7 +866,7 @@ def test_bound_integration_cannot_bypass_safe_dynamic_planner_preflight() -> Non ) integration = _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "safe", motion_policy=MotionPolicy(strategy="motion_gen"), ), @@ -782,7 +894,7 @@ def test_bind_rejects_invalid_engine_before_safe_capability_lookup() -> None: ) integration = _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "safe", motion_policy=MotionPolicy(strategy="motion_gen"), ), @@ -801,7 +913,7 @@ def test_safe_preset_rejects_non_motion_generator_strategy_for_dynamic_scene() - ) integration = _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "safe", motion_policy=MotionPolicy(strategy="ik_interp"), ), @@ -832,7 +944,7 @@ def test_non_safe_preset_preserves_dynamic_collision_policy( ) integration = _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "fast", motion_policy=MotionPolicy(dynamic_collision_mode=source_mode), ), @@ -858,7 +970,7 @@ def test_safe_preset_preserves_policy_without_dynamic_collision( registry, provider = _scene_registry(with_default=True) integration = _semantic_integration( registry, - preset=SkillPolicyPreset( + preset=_preset( "safe", motion_policy=MotionPolicy(dynamic_collision_mode=source_mode), ), diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index 1bb576292..1156786b3 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -45,6 +45,7 @@ JointPositionGoal, MotionPolicy, OPEN_COMMAND, + PickUpOptions, ResolvedActionRequest, SkillBindingContract, SkillEndpointRequirement, @@ -70,6 +71,8 @@ ControlPartEndpoint, ControlPartEndpointAdapter, ControlPartEvidenceAddress, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, EffectEvidenceSourceRef, EffectMonitorRef, EndpointResolution, @@ -519,6 +522,29 @@ def test_endpoint_resolution_owns_and_freezes_effect_sources() -> None: resolution.effect_sources["new"] = source # type: ignore[index] +def test_control_part_adapter_declares_every_builtin_integration_route() -> None: + adapter = ControlPartEndpointAdapter + + assert adapter.runtime_transport_ids == frozenset( + {JointPositionTarget.TRANSPORT_ID} + ) + assert adapter.runtime_target_types == (JointPositionTarget,) + assert adapter.tracking_feedback_source_keys == frozenset( + {("planning_context.robot", "1")} + ) + assert adapter.tracking_projector_keys == frozenset( + {("joint_position_payload", "1")} + ) + assert adapter.effect_evidence_source_keys == frozenset( + { + ( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ) + } + ) + + @pytest.mark.parametrize("returns_self", [False, True]) def test_endpoint_resolution_rejects_invalid_target_snapshot( returns_self: bool, @@ -1380,8 +1406,10 @@ def test_generic_profile_supports_base_and_whole_body_without_arm_tool_fields() def test_presets_are_versioned_snapshots_and_validate_planner() -> None: preset = SkillPolicyPreset( "safe", - motion_policy=MotionPolicy(sample_count=80), - required_planner="stub_planner", + action_option_templates={ + "pick": PickUpOptions(pre_grasp_distance=0.08), + }, + motion_policy=MotionPolicy(planner="stub_planner", sample_count=80), tracking_policy=TrackingPolicy.joint_position( in_flight_max_abs_error=0.125, terminal_max_abs_error=0.125, @@ -1401,10 +1429,16 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: second = bound.preset() assert first is not second - assert first.schema_version == 1 - assert first.required_planner == "stub_planner" + assert first.schema_version == 2 assert first.motion_policy.sample_count == 80 assert first.tracking_policy is not second.tracking_policy + assert first.action_option_templates["pick"] is not ( + second.action_option_templates["pick"] + ) + assert ( + first.action_option_templates["pick"].pre_grasp_distance # type: ignore[attr-defined] + == 0.08 + ) first_tracking = first.tracking_policy.in_flight assert first_tracking is not None assert isinstance(first_tracking.metrics[0], JointPositionTrackingMetric) @@ -1416,10 +1450,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 \[1\]"): - SkillPolicyPreset("future", schema_version=2) - with pytest.raises(ValueError, match="required_planner"): - SkillPolicyPreset("invalid", required_planner="") + with pytest.raises(ValueError, match=r"supported versions are \[2\]"): + SkillPolicyPreset("legacy", action_option_templates={}, schema_version=1) incompatible = RobotSkillProfile( "bad_preset", @@ -1428,7 +1460,8 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: presets={ "other": SkillPolicyPreset( "other", - required_planner="other_planner", + action_option_templates={}, + motion_policy=MotionPolicy(planner="other_planner"), ) }, ) @@ -1437,7 +1470,7 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: def test_policy_preset_defaults_exact_builtin_effect_monitor_refs() -> None: - preset = SkillPolicyPreset("safe") + preset = SkillPolicyPreset("safe", action_option_templates={}) assert set(preset.effect_monitors) == { "pick", @@ -1452,7 +1485,11 @@ def test_policy_preset_defaults_exact_builtin_effect_monitor_refs() -> None: def test_policy_preset_distinguishes_explicit_empty_effect_monitor_mapping() -> None: - preset = SkillPolicyPreset("unmonitored", effect_monitors={}) + preset = SkillPolicyPreset( + "unmonitored", + action_option_templates={}, + effect_monitors={}, + ) assert dict(preset.effect_monitors) == {} assert dict(preset.snapshot().effect_monitors) == {} @@ -1465,7 +1502,11 @@ def test_policy_preset_owns_and_snapshots_effect_monitor_refs() -> None: } source_ref = EffectMonitorRef("test.monitor", "2", source_params) source_mapping = {"pick": source_ref} - preset = SkillPolicyPreset("custom", effect_monitors=source_mapping) + preset = SkillPolicyPreset( + "custom", + action_option_templates={}, + effect_monitors=source_mapping, + ) source_params["consecutive_samples"] = 99 source_params["metadata"][1]["source"] = "mutated" # type: ignore[index] @@ -1489,6 +1530,108 @@ def test_policy_preset_owns_and_snapshots_effect_monitor_refs() -> None: first["pick"].params["consecutive_samples"] = 4 # type: ignore[index] +def test_policy_preset_owns_and_freezes_action_option_templates() -> None: + direction = torch.tensor([0.0, 1.0, 0.0]) + source = PickUpOptions( + pick_object_part="top", + approach_direction=direction, + ) + source_mapping = {"pick": source} + preset = SkillPolicyPreset( + "custom", + action_option_templates=source_mapping, + ) + + direction.fill_(9.0) + source.approach_direction.fill_(8.0) + source_mapping.clear() + first = preset.action_option_templates + second = preset.snapshot().action_option_templates + selected = preset.action_option_template("pick") + + assert type(first["pick"]) is PickUpOptions + assert first["pick"] is not source + assert second["pick"] is not first["pick"] + assert selected is not first["pick"] + assert first["pick"].pick_object_part == "top" # type: ignore[attr-defined] + torch.testing.assert_close( + first["pick"].approach_direction, # type: ignore[attr-defined] + torch.tensor([0.0, 1.0, 0.0]), + ) + with pytest.raises(TypeError): + first["place"] = PickUpOptions() # type: ignore[index] + with pytest.raises(KeyError, match="no action-option template"): + preset.action_option_template("place") + + +def test_policy_preset_allows_empty_templates_but_rejects_invalid_values() -> None: + with pytest.raises(TypeError, match="action_option_templates"): + SkillPolicyPreset("missing") # type: ignore[call-arg] + + assert ( + dict( + SkillPolicyPreset( + "empty", action_option_templates={} + ).action_option_templates + ) + == {} + ) + + with pytest.raises(TypeError, match="ActionOptions"): + SkillPolicyPreset( + "invalid", + action_option_templates={"pick": object()}, # type: ignore[dict-item] + ) + + +def test_policy_preset_rejects_inherited_action_options_with_extra_slot_state() -> None: + class InheritedOptions(PickUpOptions): + __slots__ = ("runtime_cache",) + + options = InheritedOptions() + object.__setattr__(options, "runtime_cache", ["live"]) + + with pytest.raises(TypeError, match="exact frozen @dataclass"): + SkillPolicyPreset( + "invalid", + action_option_templates={"pick": options}, + ) + + +def test_policy_preset_rejects_deepcopy_with_nested_mutable_aliases() -> None: + @dataclass(frozen=True, slots=True) + class AliasingOptions(ActionOptions): + values: list[int] + + def __deepcopy__(self, memo: dict[int, object]) -> AliasingOptions: + del memo + return type(self)(self.values) + + with pytest.raises(TypeError, match="without shared mutable objects"): + SkillPolicyPreset( + "invalid", + action_option_templates={"vendor.alias": AliasingOptions([1])}, + ) + + +def test_policy_preset_rejects_deepcopy_with_shared_tensor_storage() -> None: + @dataclass(frozen=True, slots=True) + class TensorViewOptions(ActionOptions): + values: torch.Tensor + + def __deepcopy__(self, memo: dict[int, object]) -> TensorViewOptions: + del memo + return type(self)(self.values.view_as(self.values)) + + with pytest.raises(TypeError, match="tensor storage"): + SkillPolicyPreset( + "invalid", + action_option_templates={ + "vendor.tensor_alias": TensorViewOptions(torch.ones(2)) + }, + ) + + def test_profile_owns_named_grounding_provider_selections() -> None: selections = {"hand_over": "dual_center"} profile = RobotSkillProfile( From 33c56c046e1e5c60db75e092c66c0d0d3a8dea45 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 18:11:00 +0800 Subject: [PATCH 17/29] feat(expert-program): own standard runtime extensions --- .../design/declarative_expert_program_plan.md | 27 + ...mbodichain.lab.gym.envs.expert_program.rst | 16 + docs/source/api_reference/public_api.rst | 33 + .../sim/atomic_actions/expert_programs.md | 39 +- .../atomic_actions/robot_skill_profiles.md | 28 +- .../lab/gym/envs/expert_program/__init__.py | 14 + .../lab/gym/envs/expert_program/bridge.py | 149 ++- .../lab/gym/envs/expert_program/catalog.py | 734 ++++++++++++-- .../gym/envs/expert_program/environment.py | 268 +++++- .../lab/gym/envs/expert_program/extensions.py | 908 ++++++++++++++++++ .../expert_program/simulation_environment.py | 205 ++-- .../lab/sim/skills/parallel_runtime.py | 58 +- embodichain/lab/sim/skills/profiles.py | 1 + tests/gym/envs/expert_program/test_bridge.py | 150 ++- tests/gym/envs/expert_program/test_catalog.py | 363 ++++++- .../envs/expert_program/test_extensions.py | 542 +++++++++++ .../test_simulation_environment.py | 855 ++++++++++++++++- tests/sim/skills/test_parallel_runtime.py | 148 ++- tests/sim/skills/test_profiles.py | 5 +- 19 files changed, 4228 insertions(+), 315 deletions(-) create mode 100644 embodichain/lab/gym/envs/expert_program/extensions.py create mode 100644 tests/gym/envs/expert_program/test_extensions.py diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index cab412e62..10ff7647a 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1275,6 +1275,33 @@ validator and parallel physical integration remain pending. The PourWater task 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 +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 +`SimulationExpertProgramRegistration`, enter its provider-free fingerprint, +and are cross-checked again against live endpoint resolution. The standard +factory consumes the same registration objects, freezes the assembled command +encoder, takes runner timing from the selected preset, and creates a fresh live +safety validator for every runtime assembly. No helper argument can replace +those registered components after preflight. Stateful extension declarations +must be frozen dataclasses with recursively immutable configuration, preventing +nested mutable values from becoming a post-registration runtime side channel. + +This registration slice deliberately covers command transport, not arbitrary +closed-loop backend injection. In C1, every custom endpoint adapter must declare +empty tracking and effect-evidence route sets and therefore supports only +timed/open-loop completion. The built-in `ControlPartEndpoint` retains its exact +built-in routes. A non-joint feedback provider, desired-state projector, metric +evaluator, or effect-evidence backend needs a separate registration-owned +live-provider factory contract before it can be advertised as standard +mobile/whole-body closed-loop support. Such providers must become fingerprinted +capabilities; they must not return as task-side runtime callbacks. Transport +`hold()` remains a trusted safe primitive owned and tested by each transport, +while the parallel safety validator authorizes active merged command frames +before dispatch. + Deliverables: - `Parallel` and explicit `Barrier` nodes in a new schema version; diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst index a3fb7e654..53e87e615 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst @@ -114,6 +114,16 @@ embodichain.lab.gym.envs.expert_program encode_semantic_call render_config_path validate_expert_program + EndpointAdapterDeclaration + ExpertProgramIntegrationCatalog + IntegrationFingerprintMismatch + ParallelCommandSafetyValidatorFactory + ParallelSafetyDeclaration + RuntimeTransportDeclaration + SimulationExpertProgramRegistration + StandardExtensionDeclarations + VersionedKey + default_simulation_settle_presets .. currentmodule:: embodichain.lab.gym.envs.expert_program @@ -166,6 +176,12 @@ Compilation and environment integration .. autoclass:: SkillRuntimeAssemblyPort :members: +.. autoclass:: ExpertProgramIntegrationCatalog + :members: + +.. autoclass:: SimulationExpertProgramRegistration + :members: + Simulation integration ---------------------- diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 4dc60ae8e..e5fc66098 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -127,6 +127,17 @@ embodichain.lab.gym.envs.expert_program.bridge SequentialSkillRuntimePort UnsupportedRuntimeTransportError +embodichain.lab.gym.envs.expert_program.catalog +------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.catalog + +.. autosummary:: + + ExpertProgramIntegrationCatalog + IntegrationFingerprintMismatch + SimulationExpertProgramRegistration + embodichain.lab.gym.envs.expert_program.cfg --------------------------------------------- @@ -227,6 +238,25 @@ embodichain.lab.gym.envs.expert_program.environment PlanningObservationPort SkillRuntimeAssemblyPort +embodichain.lab.gym.envs.expert_program.extensions +---------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.extensions + +.. autosummary:: + + EndpointAdapterDeclaration + ParallelCommandSafetyValidatorFactory + ParallelSafetyDeclaration + RuntimeTransportDeclaration + StandardExtensionDeclarations + VersionedKey + build_standard_extension_declarations + declare_endpoint_adapter + declare_parallel_safety_factory + declare_runtime_transport + validate_immutable_extension_declaration + embodichain.lab.gym.envs.expert_program.loader ------------------------------------------------ @@ -284,6 +314,7 @@ embodichain.lab.gym.envs.expert_program.simulation_policies .. autosummary:: SimulationSegmentPolicyPort + default_simulation_settle_presets embodichain.lab.gym.envs.settling --------------------------------- @@ -2108,6 +2139,7 @@ embodichain_tasks.multi_segments.cube_pick_place .. autosummary:: MultiSegmentsCubePickPlaceEnv + CUBE_EXPERT_PROGRAM_REGISTRATION create_cube_robot_profile_binding create_cube_scene_binding @@ -2201,6 +2233,7 @@ embodichain_tasks.tableware.open_drawer .. autosummary:: OpenDrawerEnv + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION create_open_drawer_robot_profile_binding create_open_drawer_scene_binding diff --git a/docs/source/overview/sim/atomic_actions/expert_programs.md b/docs/source/overview/sim/atomic_actions/expert_programs.md index 532138d90..3ba563d83 100644 --- a/docs/source/overview/sim/atomic_actions/expert_programs.md +++ b/docs/source/overview/sim/atomic_actions/expert_programs.md @@ -127,13 +127,18 @@ task then delegates runtime assembly to the shared factory; it does not construct approach, grasp, pull, or placement trajectories: ```python +MY_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_my_scene_binding(), + robot_profile_binding=create_my_robot_profile_binding(), +) + + class MyTaskEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): def __init__(self, cfg, **kwargs): super().__init__(cfg, **kwargs) self._expert_program_adapter = create_simulation_expert_program_adapter( self, - scene_binding=create_my_scene_binding(), - robot_profile_binding=create_my_robot_profile_binding(), + registration=MY_EXPERT_PROGRAM_REGISTRATION, ) @property @@ -149,8 +154,20 @@ monitor selection. `SimulationRobotSkillProfileBinding` accepts generic `ResourceEndpoint` values; `ControlPartResourceBinding` is its stricter joint-backed convenience. Endpoint adapters and runtime transports are the extension boundary for mobile-base, whole-body, or non-joint controllers and -are accepted by the standard simulation helper. Task programs keep the same -semantic calls and do not gain controller-shaped fields. +are owned by `SimulationExpertProgramRegistration`, not passed as live helper +overrides. Their exact static target, payload, route, and transport declarations +enter the catalog fingerprint, while transport tuple order defines deterministic +Gym-action composition order. Task programs keep the same semantic calls and do +not gain controller-shaped fields. + +The current standard registration installs built-in joint tracking and effect +evidence providers only for `ControlPartEndpoint`. Every custom endpoint adapter +must declare empty tracking/evidence route sets and therefore uses +timed/open-loop completion. A non-joint closed-loop projector, feedback source, +or effect-evidence backend still requires the planned registration-owned +provider-factory extension; it must not be injected from a task after preflight. +Whole-body controllers expressed through existing joint control parts continue +to use the built-in joint route. Relation and rendezvous semantics are also explicit integration capabilities. `Place(on=...)` and `Place(inside=...)` require an exact typed/versioned @@ -185,9 +202,12 @@ of evidence: - the live object-to-endpoint pose relation from the shared scene snapshot. The command-state update is transactional: encoder, buffer, cancellation, or -safe-stop failures invalidate it. An integration with contact, constraint, -force, or wrench sensing can install typed evidence callbacks without changing -the semantic call or program. +safe-stop failures invalidate it. The current C1 standard path does not accept +task-side evidence callbacks: custom endpoint adapters must expose empty +tracking and effect-evidence route sets. Contact, constraint, force, wrench, or +other custom closed-loop sensing requires a future registration-owned provider +factory whose declaration enters the integration fingerprint; this will not +change the semantic call or program. Program/demo-segment metadata records runtime call results, named trajectory segments, effect decisions, recovery events, scene and collision revisions, @@ -199,7 +219,10 @@ Schema-version-2 parallel blocks additionally require an authoritative `ParallelCommandSafetyValidator`. Resource-claim disjointness is necessary but is not treated as proof of physical safety. If no validator is installed, the parallel block refuses to start; the standard simulation adapter intentionally -does not invent one from resource names. Every parallel frame must occupy +does not invent one from resource names. Its task registration must instead +declare a safety factory covering the exact registered transport set; each +runtime assembly receives a fresh validator instance from that factory. Every +parallel frame must occupy exactly one `BaseEnv.step_dt`; shorter lanes repeat their last safe target as hold padding, while fractional frames are rejected rather than resampled. Version 2 also uses strict symbolic key-level conflict detection at the barrier: diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md index fc84034ba..9e827d7ee 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -387,13 +387,33 @@ profile = SimulationRobotSkillProfileBinding( adapter = create_simulation_expert_program_adapter( env, - scene_binding=scene_binding, - robot_profile_binding=profile, - endpoint_adapters={MobileVelocityEndpoint: MobileVelocityEndpointAdapter()}, - runtime_transports=(MobileVelocityGymEncoder(),), + registration=SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=profile, + endpoint_adapters=(MobileVelocityEndpointAdapter(),), + runtime_transports=(MobileVelocityGymEncoder(),), + ), ) ``` +The adapter and encoder publish exact class-level declarations before a live +robot is created. The adapter declares its endpoint type, runtime target types, +transport IDs, and versioned tracking/evidence routes. The encoder declares its +transport ID plus exact target and payload types; each target and payload type +declares the same `TRANSPORT_ID`. Registration rejects missing, unused, +duplicate, or conflicting declarations, and runtime profile binding verifies +that `adapter.resolve()` returns only those declared routes. A stateful adapter, +transport, grounding provider, or safety factory must be a frozen dataclass whose +configuration is recursively immutable; mutable leaves such as lists, mappings, +sets, byte arrays, and tensors are rejected before registration. + +The standard factory currently accepts its built-in tracking feedback, +projector, evaluator, and effect-evidence routes only for +`ControlPartEndpoint`. In C1, every custom endpoint adapter must declare empty +route sets and therefore supports timed/open-loop execution only. Custom +closed-loop mobile or whole-body tracking/evidence needs a registration-owned +provider factory in C2; task code must not supply a live provider side channel. + `RobotResourceBinding` snapshots arbitrary typed `ResourceEndpoint` values. `ControlPartResourceBinding` remains the stricter joint-backed convenience and continues to validate native control parts, joint IDs, and command-preset diff --git a/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py index 746cf3d45..b3bd32cf1 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -138,6 +138,14 @@ IntegrationFingerprintMismatch, SimulationExpertProgramRegistration, ) +from .extensions import ( + EndpointAdapterDeclaration, + ParallelCommandSafetyValidatorFactory, + ParallelSafetyDeclaration, + RuntimeTransportDeclaration, + StandardExtensionDeclarations, + VersionedKey, +) from .simulation_environment import ( ControlCommandStateEvidenceTracker, MotionGeneratorFactory, @@ -185,6 +193,7 @@ "EXPERT_PROGRAM_SCHEMA_VERSION_V2", "EnvironmentStepClock", "EnvironmentStepTimingError", + "EndpointAdapterDeclaration", "ExpertProgramCfg", "ExpertProgramCompileError", "ExpertProgramCompiler", @@ -215,6 +224,8 @@ "ObjectNearTargetValidatorCfg", "OperateArticulationCfg", "ParallelCfg", + "ParallelCommandSafetyValidatorFactory", + "ParallelSafetyDeclaration", "PickCfg", "PlaceCfg", "PlanningObservationPort", @@ -225,6 +236,7 @@ "RepeatCfg", "RobotResourceBinding", "RuntimeCommandFrameEncoder", + "RuntimeTransportDeclaration", "RuntimeTransportActionEncoder", "SceneReferenceRole", "SceneRegistryProgramResolver", @@ -250,11 +262,13 @@ "SimulationRobotSkillProfileBinding", "SimulationSceneBinding", "SimulationSegmentPolicyPort", + "StandardExtensionDeclarations", "SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS", "TargetCfg", "TargetRefCfg", "UnsupportedRuntimeTransportError", "ValidatorCfg", + "VersionedKey", "WaitStablePostCfg", "create_simulation_expert_program_adapter", "default_simulation_settle_presets", diff --git a/embodichain/lab/gym/envs/expert_program/bridge.py b/embodichain/lab/gym/envs/expert_program/bridge.py index f72e616d3..aeb2506ab 100644 --- a/embodichain/lab/gym/envs/expert_program/bridge.py +++ b/embodichain/lab/gym/envs/expert_program/bridge.py @@ -27,9 +27,10 @@ from collections import deque from collections.abc import Callable, Iterable, Iterator, Mapping +from copy import deepcopy from dataclasses import dataclass, field import math -from typing import Any, Protocol, runtime_checkable +from typing import Any, ClassVar, Protocol, runtime_checkable import torch @@ -41,11 +42,13 @@ from embodichain.lab.sim.atomic_actions.runner import ( CommandAcknowledgement, ExecutionClock, + ExecutionRunnerCfg, ) from embodichain.lab.sim.atomic_actions.runtime_commands import ( EndpointCommand, JointPositionPayload, RuntimeCommandFrame, + RuntimeCommandPayload, ) from embodichain.lab.sim.atomic_actions.state import PlanningContext, TaskState from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy @@ -109,9 +112,14 @@ class RuntimeTransportActionEncoder(Protocol): action manager exposes a structured controller boundary. """ - @property - def transport_id(self) -> str: - """Return the exact runtime transport ID handled by this encoder.""" + transport_id: ClassVar[str] + """Exact runtime transport ID handled by this encoder.""" + + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] + """Exact runtime-target types accepted by this encoder.""" + + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] + """Exact runtime-payload types accepted by this encoder.""" def encode( self, @@ -129,7 +137,12 @@ def hold( base_action: EnvAction, context: PlanningContext, ) -> EnvAction: - """Merge this transport's safe state into ``base_action``.""" + """Merge this transport's self-proven safe hold into ``base_action``. + + The transport remains authoritative for neutralizing its own controller; + parallel command validation does not replace this transport-specific hold + contract. + """ @runtime_checkable @@ -423,10 +436,13 @@ def advance_after_env_step(self, steps: int = 1) -> None: class JointPositionGymTransportEncoder: """Built-in ``robot.joint_position`` to full-qpos action encoder.""" - @property - def transport_id(self) -> str: - """Return the built-in joint-position transport ID.""" - return JointPositionTarget.TRANSPORT_ID + transport_id: ClassVar[str] = JointPositionTarget.TRANSPORT_ID + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + JointPositionTarget, + ) + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] = ( + JointPositionPayload, + ) def encode( self, @@ -495,7 +511,10 @@ class RuntimeCommandFrameEncoder: Args: qpos_provider: Full-qpos source aligned to a frame's explicit ``env_ids``. transports: Optional additional transport encoders. The built-in - joint-position encoder is always installed first. + joint-position encoder precedes them when enabled. + include_joint_position: Whether to install the built-in joint-position + encoder. Standard assemblies disable it when their exact profile uses + only custom endpoint transports. """ def __init__( @@ -503,12 +522,17 @@ def __init__( qpos_provider: CurrentQposProvider, *, transports: Iterable[RuntimeTransportActionEncoder] = (), + include_joint_position: bool = True, ) -> None: if not isinstance(qpos_provider, CurrentQposProvider): raise TypeError("qpos_provider must implement CurrentQposProvider.") + if type(include_joint_position) is not bool: + raise TypeError("include_joint_position must be a bool.") self._qpos_provider = qpos_provider self._transports: dict[str, RuntimeTransportActionEncoder] = {} - self.register_transport(JointPositionGymTransportEncoder()) + self._frozen = False + if include_joint_position: + self.register_transport(JointPositionGymTransportEncoder()) for transport in transports: self.register_transport(transport) @@ -517,6 +541,15 @@ def transport_ids(self) -> tuple[str, ...]: """Return registered transport IDs in deterministic encoding order.""" return tuple(self._transports) + @property + def is_frozen(self) -> bool: + """Return whether runtime transport registration is permanently closed.""" + return self._frozen + + def freeze(self) -> None: + """Permanently close transport registration for a standard assembly.""" + self._frozen = True + def register_transport( self, transport: RuntimeTransportActionEncoder, @@ -524,18 +557,84 @@ def register_transport( replace: bool = False, ) -> None: """Register one shared transport-to-Gym action encoder.""" + if self._frozen: + raise RuntimeError( + "Runtime transport registration is frozen for this command encoder." + ) if not isinstance(transport, RuntimeTransportActionEncoder): raise TypeError("transport must implement RuntimeTransportActionEncoder.") + transport_type = type(transport) transport_id = _validate_identifier( - transport.transport_id, + getattr(transport_type, "transport_id", None), field_name="RuntimeTransportActionEncoder.transport_id", ) + self._validate_declared_types( + getattr(transport_type, "target_types", None), + base_type=RuntimeEndpointTarget, + field_name="RuntimeTransportActionEncoder.target_types", + ) + self._validate_declared_types( + getattr(transport_type, "payload_types", None), + base_type=RuntimeCommandPayload, + field_name="RuntimeTransportActionEncoder.payload_types", + ) if type(replace) is not bool: raise TypeError("replace must be a bool.") if transport_id in self._transports and not replace: raise ValueError(f"Transport {transport_id!r} is already registered.") self._transports[transport_id] = transport + @staticmethod + def _validate_declared_types( + values: object, + *, + base_type: type[object], + field_name: str, + ) -> None: + """Validate one non-empty exact tuple of supported runtime types.""" + if type(values) is not tuple or not values: + raise TypeError(f"{field_name} must be a non-empty exact tuple.") + if not all( + isinstance(value, type) and issubclass(value, base_type) for value in values + ): + raise TypeError( + f"{field_name} must contain {base_type.__name__} subclasses." + ) + if len(set(values)) != len(values): + raise ValueError(f"{field_name} must not contain duplicate types.") + + @staticmethod + def _validate_command_types( + transport: RuntimeTransportActionEncoder, + command: EndpointCommand, + ) -> None: + """Require exact target and payload coverage before transport routing.""" + transport_type = type(transport) + if type(command.target) not in transport_type.target_types: + raise TypeError( + f"Transport {transport_type.transport_id!r} does not declare exact " + f"target type {type(command.target).__name__}." + ) + if type(command.payload) not in transport_type.payload_types: + raise TypeError( + f"Transport {transport_type.transport_id!r} does not declare exact " + f"payload type {type(command.payload).__name__}." + ) + + @staticmethod + def _validate_hold_target_types( + transport: RuntimeTransportActionEncoder, + targets: Iterable[RuntimeEndpointTarget], + ) -> None: + """Require exact target coverage before safe-hold routing.""" + transport_type = type(transport) + for target in targets: + if type(target) not in transport_type.target_types: + raise TypeError( + f"Transport {transport_type.transport_id!r} does not declare " + f"exact hold target type {type(target).__name__}." + ) + def _base_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: """Capture and validate one owned full-qpos hold action.""" qpos = self._qpos_provider.current_qpos(env_ids) @@ -556,6 +655,7 @@ def encode(self, frame: RuntimeCommandFrame) -> EnvAction: if not isinstance(frame, RuntimeCommandFrame): raise TypeError("frame must be a RuntimeCommandFrame.") action: EnvAction = self._base_qpos(frame.env_ids) + by_transport: dict[str, list[EndpointCommand]] = {} for command in frame.commands: transport = self._transports.get(command.transport_id) if transport is None: @@ -563,11 +663,15 @@ def encode(self, frame: RuntimeCommandFrame) -> EnvAction: f"No Gym action encoder is registered for runtime transport " f"{command.transport_id!r}." ) - action = transport.encode( - command, - base_action=action, - active_mask=frame.active_mask, - ) + self._validate_command_types(transport, command) + by_transport.setdefault(command.transport_id, []).append(command) + for transport_id, transport in self._transports.items(): + for command in by_transport.get(transport_id, ()): + action = transport.encode( + command, + base_action=action, + active_mask=frame.active_mask, + ) return action def encode_hold( @@ -591,6 +695,11 @@ def encode_hold( f"No Gym action encoder is registered for runtime transport " f"{transport_id!r}." ) + self._validate_hold_target_types(transport, grouped) + for transport_id, transport in self._transports.items(): + grouped = by_transport.get(transport_id) + if grouped is None: + continue action = transport.hold( tuple(grouped), base_action=action, @@ -897,6 +1006,7 @@ class AtomicDemoBridge: clock: The same environment-step clock installed in ``runtime``. post_policy_port: Optional environment-aware post-policy executor. validator_port: Optional environment-aware validator executor. + runner_cfg: Runner transport policy selected by the runtime preset. parallel_safety_validator: Optional authoritative physical-safety gate required before any parallel branch can start. @@ -914,6 +1024,7 @@ def __init__( *, post_policy_port: SegmentPostPolicyPort | None = None, validator_port: SegmentValidatorPort | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, parallel_safety_validator: ParallelCommandSafetyValidator | None = None, ) -> None: if not isinstance(program, CompiledProgramPort): @@ -937,6 +1048,8 @@ def __init__( validator_port, SegmentValidatorPort ): raise TypeError("validator_port must implement SegmentValidatorPort.") + if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") if parallel_safety_validator is not None and not isinstance( parallel_safety_validator, ParallelCommandSafetyValidator ): @@ -950,6 +1063,7 @@ def __init__( self._clock = clock self._post_policy_port = post_policy_port self._validator_port = validator_port + self._runner_cfg = deepcopy(runner_cfg or ExecutionRunnerCfg()) self._parallel_safety_validator = parallel_safety_validator self._active_segment_id: str | None = None self._eligible_mask: torch.Tensor | None = None @@ -1351,6 +1465,7 @@ def _parallel_runtime(self, segment: Any) -> ParallelSkillRuntime: self._parallel_safety_validator, timeout_steps=barrier.timeout_steps, failure_policy=barrier.failure_policy, + runner_cfg=self._runner_cfg, workflow_id=( f"{self._program.program_id}/{segment.segment_id}:parallel_analysis" ), diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index 06963c265..be266f8bb 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -24,6 +24,8 @@ import hashlib import json import math +from _thread import LockType +from threading import Lock from types import MappingProxyType import torch @@ -32,30 +34,60 @@ Affordance, ArticulationOperationAffordance, AtomicActionEngine, + EndpointTrackingFeedbackAddress, + GRASP_CAPABILITY, + JOINT_POSITION_CHANNEL, SkillDescriptor, ) from embodichain.lab.sim.atomic_actions.primitives import BUILTIN_ACTION_TYPES +from embodichain.lab.sim.atomic_actions.tracking import ( + FeedbackTerminalAcceptance, + TrackingRuntime, +) from embodichain.lab.sim.skills import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + ControlPartEndpoint, + ControlPartEvidenceAddress, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, + POSE_RELATION_EFFECT_CHANNEL, + BoundRobotSkillProfile, HandOverPoseProvider, OperateArticulation, Place, RelationTargetGrounder, + RobotResource, RobotSkillProfile, + RegisteredSemanticCall, + ResourceEndpoint, + ResourceEndpointAdapter, SceneAffordanceRef, SceneArticulationRef, SceneEntityRef, + SceneEntityManifest, SceneManifest, SceneObjectRef, SceneRegistry, SemanticCallCatalog, + SemanticCallDescriptor, SemanticIntegrationManifest, SemanticValidationError, SkillPolicyPreset, builtin_semantic_call_catalog, ) +from embodichain.lab.sim.skills.effects import ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CompositeEffectMonitorFactory, + EffectMonitorRegistry, +) +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, +) from .cfg import ( ExpertProgramCfg, @@ -77,6 +109,16 @@ ExpertProgramValidationError, SceneReferenceRole, ) +from .bridge import RuntimeTransportActionEncoder +from .extensions import ( + EndpointAdapterDeclaration, + ParallelCommandSafetyValidatorFactory, + ParallelSafetyDeclaration, + RuntimeTransportDeclaration, + StandardExtensionDeclarations, + build_standard_extension_declarations, + validate_immutable_extension_declaration, +) from .simulation import SimulationRobotSkillProfileBinding, SimulationSceneBinding from .simulation_policies import default_simulation_settle_presets @@ -239,51 +281,6 @@ def _relation_grounder_order_key( return capability, _qualified_name(affordance_type), revision -def _validate_provider_declaration(provider: object, *, field_name: str) -> None: - """Accept only frozen dataclass declarations or stateless providers.""" - dataclass_declaration = is_dataclass(provider) - dataclass_field_names: set[str] = set() - if dataclass_declaration: - params = getattr(type(provider), "__dataclass_params__", None) - if params is None or not params.frozen: - raise TypeError( - f"{field_name} stateful declarations must be frozen dataclasses " - "so every configuration field enters the registration fingerprint." - ) - dataclass_field_names.update( - declaration_field.name for declaration_field in fields(provider) - ) - - state_names: set[str] = set() - instance_state = getattr(provider, "__dict__", None) - if isinstance(instance_state, Mapping): - state_names.update(instance_state) - for owner in type(provider).__mro__: - declared_slots = getattr(owner, "__slots__", ()) - slots = (declared_slots,) if isinstance(declared_slots, str) else declared_slots - for slot_name in slots: - if slot_name in {"__dict__", "__weakref__"}: - continue - storage_name = ( - f"_{owner.__name__.lstrip('_')}{slot_name}" - if slot_name.startswith("__") and not slot_name.endswith("__") - else slot_name - ) - if hasattr(provider, storage_name): - state_names.add(storage_name) - undeclared_state = ( - state_names.difference(dataclass_field_names) - if dataclass_declaration - else state_names - ) - if undeclared_state: - raise TypeError( - f"{field_name} providers contain unfingerprinted state " - f"{sorted(undeclared_state)}. Use a frozen dataclass declaration with " - "every state field declared; non-dataclass providers must be stateless." - ) - - def _snapshot_relation_grounders( values: tuple[RelationTargetGrounder, ...], ) -> tuple[RelationTargetGrounder, ...]: @@ -296,7 +293,7 @@ def _snapshot_relation_grounders( raise TypeError( "relation_grounders must contain RelationTargetGrounder instances." ) - _validate_provider_declaration( + validate_immutable_extension_declaration( grounder, field_name="relation_grounders", ) @@ -351,7 +348,7 @@ def _snapshot_handover_pose_providers( raise TypeError( "handover_pose_providers must contain HandOverPoseProvider instances." ) - _validate_provider_declaration( + validate_immutable_extension_declaration( provider, field_name="handover_pose_providers", ) @@ -362,6 +359,72 @@ def _snapshot_handover_pose_providers( return tuple(values) +def _validate_standard_call_catalog(call_catalog: SemanticCallCatalog) -> None: + """Reject semantic lowerer extensions from the standard registration path.""" + builtins = builtin_semantic_call_catalog().descriptors + for descriptor in call_catalog.descriptors.values(): + if descriptor.spec_type is RegisteredSemanticCall: + raise ValueError( + f"Registered semantic call {descriptor.call_id!r} is not " + "supported by the standard simulation registration; only " + "curated semantic calls may be registered." + ) + expected = builtins.get(descriptor.call_id) + if expected != descriptor: + raise ValueError( + f"Semantic call {descriptor.call_id!r} does not match its exact " + "curated descriptor." + ) + + +def _validate_standard_effect_monitors(profile: RobotSkillProfile) -> None: + """Require every preset to use the exact built-in effect-monitor factory.""" + registry = EffectMonitorRegistry((CompositeEffectMonitorFactory(),)) + builtin_key = ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + for preset_id, preset in profile.presets.items(): + for semantic_id, monitor_ref in preset.effect_monitors.items(): + key = monitor_ref.monitor_id, monitor_ref.revision + if key != builtin_key: + raise ValueError( + f"Preset {preset_id!r} semantic call {semantic_id!r} selects " + f"non-built-in effect monitor {key!r}; the standard " + "simulation registration supports only {builtin_key!r}." + ) + try: + registry.validate_ref(monitor_ref) + except (KeyError, TypeError, ValueError) as exc: + raise ValueError( + f"Preset {preset_id!r} semantic call {semantic_id!r} has an " + "invalid built-in effect-monitor declaration." + ) from exc + + +def _validate_standard_tracking_metrics(profile: RobotSkillProfile) -> None: + """Resolve every reachable metric through the exact built-in evaluator table.""" + evaluators = TrackingRuntime.with_builtins().evaluators + for preset_id, preset in profile.presets.items(): + policy = preset.tracking_policy + metric_groups = [] + if policy.in_flight is not None: + metric_groups.append(("in_flight", policy.in_flight.metrics)) + if isinstance(policy.terminal, FeedbackTerminalAcceptance): + metric_groups.append(("terminal", policy.terminal.metrics)) + for phase, metrics in metric_groups: + for metric in metrics: + try: + evaluators.resolve(metric) + except (KeyError, TypeError, ValueError) as exc: + key = metric.metric_id, metric.revision, _qualified_name(metric) + raise ValueError( + f"Preset {preset_id!r} {phase} tracking metric {key!r} " + "has no exact built-in evaluator in the standard " + "simulation registration." + ) from exc + + def _declared_articulation_operation_targets( scene_binding: SimulationSceneBinding, ) -> dict[str, frozenset[str]]: @@ -479,6 +542,89 @@ def resolve( return type(resolved)(resolved.entity_id) +def _planner_scene_entry(entry: SceneEntityManifest) -> dict[str, object]: + """Project one provider-free scene entry for task planning.""" + return { + "id": entry.ref.entity_id, + "ref_type": type(entry.ref).__name__, + "aliases": list(entry.aliases), + "parent": None if entry.parent is None else entry.parent.entity_id, + "native_name": entry.native_name, + "dynamics": entry.dynamics.value, + "collision_role": entry.collision_role.value, + "semantic_type": entry.semantic_type, + "affordance_capabilities": sorted(entry.affordance_capabilities), + "default_affordances": { + capability: reference.entity_id + for capability, reference in sorted(entry.default_affordances.items()) + }, + "affordance_payload_type": ( + None + if entry.affordance_payload_type is None + else _qualified_name(entry.affordance_payload_type) + ), + "affordance_revision": entry.affordance_revision, + "relative_pose": ( + None if entry.relative_pose is None else list(entry.relative_pose) + ), + } + + +def _planner_call_descriptor( + descriptor: SemanticCallDescriptor, +) -> dict[str, object]: + """Project one semantic call and its robot-independent resource contract.""" + contract = descriptor.binding_contract + return { + "call_id": descriptor.call_id, + "schema_version": descriptor.schema_version, + "skill_id": descriptor.skill_id, + "config_type": _qualified_name(descriptor.spec_type), + "slots": [ + { + "slot_id": slot.slot_id, + "endpoints": [ + { + "endpoint_id": endpoint.endpoint_id, + "capabilities": sorted(endpoint.capabilities), + "required_commands": { + command_id: _qualified_name(command_type) + for command_id, command_type in sorted( + endpoint.required_commands.items() + ) + }, + } + for endpoint in slot.endpoints + ], + "constraints": [ + _canonical_value(constraint) for constraint in slot.constraints + ], + } + for slot in contract.slots + ], + "constraints": [ + _canonical_value(constraint) for constraint in contract.constraints + ], + } + + +def _planner_robot_resource(resource: RobotResource) -> dict[str, object]: + """Project one robot resource without live binding or controller state.""" + return { + "resource_id": resource.resource_id, + "members": list(resource.members), + "endpoints": [ + { + "endpoint_id": endpoint_id, + "endpoint_type": _qualified_name(endpoint), + "capabilities": sorted(endpoint.capabilities), + "declaration": _canonical_value(endpoint), + } + for endpoint_id, endpoint in sorted(resource.endpoints.items()) + ], + } + + @dataclass(frozen=True, slots=True) class ExpertProgramIntegrationCatalog: """Provider-free integration directory owned by one task registration.""" @@ -491,6 +637,11 @@ class ExpertProgramIntegrationCatalog: relation_grounder_keys: frozenset[tuple[str, type[Affordance], str]] articulation_operation_targets: Mapping[str, frozenset[str]] settle_preset_ids: frozenset[str] + endpoint_adapter_declarations: Mapping[ + type[ResourceEndpoint], EndpointAdapterDeclaration + ] + runtime_transport_declarations: tuple[RuntimeTransportDeclaration, ...] + parallel_safety_declaration: ParallelSafetyDeclaration | None fingerprint: str _required_skills: Mapping[str, SkillDescriptor] = field( repr=False, @@ -521,6 +672,36 @@ def __post_init__(self) -> None: scene=self.scene, ), ) + extensions = StandardExtensionDeclarations( + endpoint_adapters=self.endpoint_adapter_declarations, + runtime_transports=self.runtime_transport_declarations, + parallel_safety=self.parallel_safety_declaration, + ) + profile_endpoint_types = frozenset( + type(endpoint) + for resource in self.robot_profile.resources.values() + for endpoint in resource.endpoints.values() + ) + if profile_endpoint_types != frozenset(extensions.endpoint_adapters): + raise ValueError( + "endpoint_adapter_declarations must cover every exact robot " + "profile endpoint type and no others." + ) + object.__setattr__( + self, + "endpoint_adapter_declarations", + extensions.endpoint_adapters, + ) + object.__setattr__( + self, + "runtime_transport_declarations", + extensions.runtime_transports, + ) + object.__setattr__( + self, + "parallel_safety_declaration", + extensions.parallel_safety, + ) if self.robot_profile.profile_id != self.robot_profile_id: raise ValueError("robot_profile_id must match robot_profile.profile_id.") preset_ids = frozenset(self.settle_preset_ids) @@ -541,6 +722,97 @@ def __post_init__(self) -> None: MappingProxyType(dict(self._required_skills)), ) + def planner_projection(self) -> dict[str, object]: + """Return a deterministic JSON-safe planner view of this integration. + + The projection is derived exclusively from the canonical catalog and + carries its exact fingerprint. It contains no providers, live handles, + bound resource claims, motion policy, or executable implementation. + """ + profile = self.robot_profile + return { + "schema_version": "semantic_integration_planner_projection/v1", + "integration_fingerprint": self.fingerprint, + "scene_registry_id": self.scene_registry_id, + "robot_profile_id": self.robot_profile_id, + "scene": { + "collision_world_mode": ( + None + if self.scene.collision_world_mode is None + else self.scene.collision_world_mode.value + ), + "entities": [ + _planner_scene_entry(entry) for entry in self.scene.entries + ], + }, + "semantic_calls": [ + _planner_call_descriptor(descriptor) + for descriptor in sorted( + self.call_catalog.descriptors.values(), + key=lambda value: value.call_id, + ) + ], + "robot": { + "resources": [ + _planner_robot_resource(resource) + for resource in sorted( + profile.resources.values(), + key=lambda value: value.resource_id, + ) + ], + "defaults": { + skill_id: dict(binding.resources) + for skill_id, binding in sorted(profile.defaults.items()) + }, + "preset_ids": sorted(profile.presets), + "default_preset": profile.default_preset, + "skill_presets": dict(sorted(profile.skill_presets.items())), + "grounding_providers": dict( + sorted(profile.grounding_providers.items()) + ), + }, + "providers": { + "relation_grounders": [ + { + "capability": capability, + "affordance_type": _qualified_name(affordance_type), + "revision": revision, + } + for capability, affordance_type, revision in sorted( + self.relation_grounder_keys, + key=lambda value: ( + value[0], + _qualified_name(value[1]), + value[2], + ), + ) + ], + "articulation_operation_targets": { + entity_id: sorted(targets) + for entity_id, targets in sorted( + self.articulation_operation_targets.items() + ) + }, + "settle_preset_ids": sorted(self.settle_preset_ids), + "endpoint_adapters": [ + _canonical_value(declaration) + for declaration in sorted( + self.endpoint_adapter_declarations.values(), + key=lambda value: value.adapter_id, + ) + ], + "runtime_transports": [ + _canonical_value(declaration) + for declaration in self.runtime_transport_declarations + ], + "parallel_safety": ( + None + if self.parallel_safety_declaration is None + else _canonical_value(self.parallel_safety_declaration) + ), + }, + } + def validate_integration( self, integration: ExpertProgramIntegrationCfg, @@ -754,6 +1026,16 @@ def preflight(self, program: ExpertProgramCfg) -> CompiledProgram: runtime_preset=program.integration.runtime_preset, ) for segment in compiled.iter_segments(): + if ( + segment.parallel_block is not None + and self.parallel_safety_declaration is None + ): + raise ExpertProgramValidationError( + "parallel_safety_factory_not_registered", + segment.parallel_block.source_path, + "Parallel execution requires a task-registration-owned " + "physical safety-validator factory.", + ) for call in segment.calls: if ( type(call.call) is OperateArticulation @@ -791,30 +1073,200 @@ def validate_engine(self, engine: AtomicActionEngine) -> None: f"Live skill {skill_id!r} differs from the registered " "semantic target descriptor." ) + bound_profile = engine.skill_profile + if type(bound_profile) is not BoundRobotSkillProfile: + raise IntegrationFingerprintMismatch( + "The standard live engine must own one exact bound robot profile." + ) + self.validate_bound_endpoint_extensions(bound_profile) + def validate_bound_endpoint_extensions( + self, + bound_profile: BoundRobotSkillProfile, + ) -> None: + """Match every live resolved endpoint to its fingerprinted declaration.""" + if type(bound_profile) is not BoundRobotSkillProfile: + raise TypeError("bound_profile must be exactly BoundRobotSkillProfile.") + if bound_profile.profile_id != self.robot_profile_id: + raise IntegrationFingerprintMismatch( + "The bound robot profile ID differs from the registered profile." + ) -def _profile_with_control_dt( - profile: RobotSkillProfile, - *, - control_dt: float, -) -> RobotSkillProfile: - """Return the registration profile aligned to one Gym control 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), - tracking_policy=preset.tracking_policy, - recovery_policy=preset.recovery_policy, - runner_cfg=preset.runner_cfg, - effect_monitors=preset.effect_monitors, - action_option_templates=preset.action_option_templates, + transport_owner_by_target_type = { + target_type: transport + for transport in self.runtime_transport_declarations + for target_type in transport.target_types + } + expected_resource_ids = frozenset(self.robot_profile.resources) + live_resource_ids = frozenset(bound_profile.resources) + if live_resource_ids != expected_resource_ids: + raise IntegrationFingerprintMismatch( + "Bound robot resource IDs differ from the registered profile; " + f"expected {sorted(expected_resource_ids)}, " + f"got {sorted(live_resource_ids)}." ) - for preset_id, preset in profile.presets.items() - }, - ) + for resource_id, resource in bound_profile.resources.items(): + expected_resource = self.robot_profile.resources[resource_id] + if resource.resource_id != expected_resource.resource_id: + raise IntegrationFingerprintMismatch( + f"Bound resource {resource_id!r} declaration ID differs from " + "the registered profile." + ) + if resource.members != expected_resource.members: + raise IntegrationFingerprintMismatch( + f"Bound resource {resource_id!r} members differ from the " + "registered profile." + ) + expected_endpoint_ids = frozenset(expected_resource.endpoints) + live_endpoint_ids = frozenset(resource.endpoints) + if live_endpoint_ids != expected_endpoint_ids: + raise IntegrationFingerprintMismatch( + f"Bound resource {resource_id!r} endpoint IDs differ from the " + f"registered profile; expected {sorted(expected_endpoint_ids)}, " + f"got {sorted(live_endpoint_ids)}." + ) + for endpoint_id, endpoint in resource.endpoints.items(): + location = f"{resource_id}.{endpoint_id}" + expected_endpoint = expected_resource.endpoints[endpoint_id] + if type(endpoint.endpoint) is not type(expected_endpoint) or ( + _canonical_json(endpoint.endpoint) + != _canonical_json(expected_endpoint) + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} declaration differs from the " + "registered robot profile." + ) + endpoint_type = type(endpoint.endpoint) + declaration = self.endpoint_adapter_declarations.get(endpoint_type) + if declaration is None: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} has undeclared exact type " + f"{_qualified_name(endpoint_type)!r}." + ) + if endpoint.adapter_id != declaration.adapter_id: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} adapter ID " + f"{endpoint.adapter_id!r} differs from registered " + f"{declaration.adapter_id!r}." + ) + + target = endpoint.runtime_target + target_type = type(target) + if target_type not in declaration.runtime_target_types: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} resolved undeclared exact " + f"runtime target type {_qualified_name(target_type)!r}." + ) + owner = transport_owner_by_target_type.get(target_type) + if owner is None or owner.transport_id not in ( + declaration.runtime_transport_ids + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} target type has no registered " + "adapter transport owner." + ) + if target.transport_id != owner.transport_id: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} live transport " + f"{target.transport_id!r} differs from target type owner " + f"{owner.transport_id!r}." + ) + + feedback_keys = frozenset( + (binding.source.provider_id, binding.source.revision) + for binding in endpoint.tracking_channels.values() + ) + if feedback_keys != declaration.tracking_feedback_source_keys: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} tracking-feedback routes " + "differ from its registered adapter declaration." + ) + projector_keys = frozenset( + (binding.projector.projector_id, binding.projector.revision) + for binding in endpoint.tracking_channels.values() + ) + if projector_keys != declaration.tracking_projector_keys: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} tracking-projector routes " + "differ from its registered adapter declaration." + ) + evidence_keys = frozenset( + (source.provider_id, source.revision) + for source in endpoint.effect_sources.values() + ) + if evidence_keys != declaration.effect_evidence_source_keys: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} effect-evidence routes " + "differ from its registered adapter declaration." + ) + if endpoint_type is ControlPartEndpoint: + control_part = endpoint.endpoint.control_part + if getattr(target, "control_part", None) != control_part: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} runtime target addresses " + "a different control part." + ) + if frozenset(endpoint.tracking_channels) != frozenset( + {JOINT_POSITION_CHANNEL} + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} must expose exactly the " + "built-in joint-position tracking channel." + ) + tracking = endpoint.tracking_channels[JOINT_POSITION_CHANNEL] + feedback_address = tracking.source.address + if type(feedback_address) is not EndpointTrackingFeedbackAddress: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} must use the exact " + "built-in endpoint tracking address." + ) + if ( + feedback_address.channel_id != JOINT_POSITION_CHANNEL + or type(feedback_address.target) is not target_type + or _canonical_json(feedback_address.target) + != _canonical_json(target) + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} tracking address differs " + "from its runtime target or channel." + ) + + expected_effect_channels = { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } + if GRASP_CAPABILITY in endpoint.endpoint.capabilities: + expected_effect_channels.update( + { + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + } + ) + if frozenset(endpoint.effect_sources) != frozenset( + expected_effect_channels + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} effect-evidence channels " + "differ from the exact built-in control-part routes." + ) + for channel, source in endpoint.effect_sources.items(): + address = source.address + if ( + type(address) is not ControlPartEvidenceAddress + or address.control_part != control_part + or address.channel != channel + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} effect-evidence " + f"address for channel {channel!r} differs from its " + "control part or channel." + ) + elif endpoint.tracking_channels or endpoint.effect_sources: + raise IntegrationFingerprintMismatch( + f"Bound custom endpoint {location!r} exposes closed-loop " + "routes forbidden by the C1 standard runtime." + ) def _registration_payload( @@ -829,6 +1281,10 @@ def _registration_payload( relation_grounder_keys: frozenset[tuple[str, type[Affordance], str]], relation_grounders: tuple[RelationTargetGrounder, ...], handover_pose_providers: tuple[HandOverPoseProvider, ...], + extensions: StandardExtensionDeclarations, + endpoint_adapters: tuple[ResourceEndpointAdapter, ...], + runtime_transports: tuple[RuntimeTransportActionEncoder, ...], + parallel_safety_factory: ParallelCommandSafetyValidatorFactory | None, ) -> dict[str, object]: """Build the versioned canonical fingerprint payload.""" return { @@ -865,6 +1321,48 @@ def _registration_payload( key=_handover_pose_provider_id, ) ), + "standard_extensions": { + "endpoint_adapters": tuple( + sorted( + extensions.endpoint_adapters.values(), + key=lambda declaration: declaration.adapter_id, + ) + ), + "runtime_transports": extensions.runtime_transports, + "parallel_safety": extensions.parallel_safety, + }, + "endpoint_adapters": tuple( + { + "declaration": extensions.endpoint_adapters[ + getattr(type(adapter), "endpoint_type") + ], + "provider": _provider_fingerprint_declaration(adapter), + } + for adapter in sorted( + endpoint_adapters, + key=lambda value: getattr(type(value), "adapter_id"), + ) + ), + "runtime_transports": tuple( + { + "declaration": next( + declaration + for declaration in extensions.runtime_transports + if declaration.transport_id + == getattr(type(transport), "transport_id") + ), + "provider": _provider_fingerprint_declaration(transport), + } + for transport in runtime_transports + ), + "parallel_safety_factory": ( + None + if parallel_safety_factory is None + else { + "declaration": extensions.parallel_safety, + "provider": _provider_fingerprint_declaration(parallel_safety_factory), + } + ), "post_policy_kinds": _POST_POLICY_KINDS, "settle_presets": settle_presets, "validator_kinds": _VALIDATOR_KINDS, @@ -885,7 +1383,20 @@ class SimulationExpertProgramRegistration: ) relation_grounders: tuple[RelationTargetGrounder, ...] = () handover_pose_providers: tuple[HandOverPoseProvider, ...] = () + endpoint_adapters: tuple[ResourceEndpointAdapter, ...] = () + runtime_transports: tuple[RuntimeTransportActionEncoder, ...] = () + parallel_safety_factory: ParallelCommandSafetyValidatorFactory | None = None catalog: ExpertProgramIntegrationCatalog = field(init=False) + _parallel_safety_validator_history: list[ParallelCommandSafetyValidator] = field( + init=False, + repr=False, + compare=False, + ) + _parallel_safety_validator_lock: LockType = field( + init=False, + repr=False, + compare=False, + ) def __post_init__(self) -> None: if type(self.scene_binding) is not SimulationSceneBinding: @@ -897,6 +1408,7 @@ def __post_init__(self) -> None: ) if type(self.call_catalog) is not SemanticCallCatalog: raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + _validate_standard_call_catalog(self.call_catalog) settle_presets = _snapshot_settle_presets(self.settle_presets) object.__setattr__(self, "settle_presets", settle_presets) relation_grounders = _snapshot_relation_grounders(self.relation_grounders) @@ -918,6 +1430,14 @@ def __post_init__(self) -> None: self.scene_binding ) profile = self.robot_profile_binding.declare() + _validate_standard_effect_monitors(profile) + _validate_standard_tracking_metrics(profile) + extensions = build_standard_extension_declarations( + profile=profile, + endpoint_adapters=self.endpoint_adapters, + runtime_transports=self.runtime_transports, + parallel_safety_factory=self.parallel_safety_factory, + ) selected_handover_provider = profile.grounding_providers.get("hand_over") registered_handover_provider_ids = { _handover_pose_provider_id(provider) for provider in handover_pose_providers @@ -961,6 +1481,10 @@ def __post_init__(self) -> None: relation_grounder_keys=relation_grounder_keys, relation_grounders=relation_grounders, handover_pose_providers=handover_pose_providers, + extensions=extensions, + endpoint_adapters=self.endpoint_adapters, + runtime_transports=self.runtime_transports, + parallel_safety_factory=self.parallel_safety_factory, ) ) object.__setattr__( @@ -975,10 +1499,15 @@ def __post_init__(self) -> None: relation_grounder_keys=relation_grounder_keys, articulation_operation_targets=articulation_operation_targets, settle_preset_ids=frozenset(settle_presets), + endpoint_adapter_declarations=extensions.endpoint_adapters, + runtime_transport_declarations=extensions.runtime_transports, + parallel_safety_declaration=extensions.parallel_safety, fingerprint=fingerprint, _required_skills=required_skills, ), ) + object.__setattr__(self, "_parallel_safety_validator_history", []) + object.__setattr__(self, "_parallel_safety_validator_lock", Lock()) @property def fingerprint(self) -> str: @@ -993,6 +1522,15 @@ def assert_unchanged(self) -> None: ) profile = self.robot_profile_binding.declare() try: + _validate_standard_call_catalog(self.call_catalog) + _validate_standard_effect_monitors(profile) + _validate_standard_tracking_metrics(profile) + extensions = build_standard_extension_declarations( + profile=profile, + endpoint_adapters=self.endpoint_adapters, + runtime_transports=self.runtime_transports, + parallel_safety_factory=self.parallel_safety_factory, + ) relation_grounders = _snapshot_relation_grounders(self.relation_grounders) relation_grounder_keys = frozenset( _relation_grounder_key(grounder) for grounder in relation_grounders @@ -1012,6 +1550,10 @@ def assert_unchanged(self) -> None: relation_grounder_keys=relation_grounder_keys, relation_grounders=relation_grounders, handover_pose_providers=handover_pose_providers, + extensions=extensions, + endpoint_adapters=self.endpoint_adapters, + runtime_transports=self.runtime_transports, + parallel_safety_factory=self.parallel_safety_factory, ) ) except (TypeError, ValueError) as exc: @@ -1025,25 +1567,71 @@ def assert_unchanged(self) -> None: "registration." ) + @property + def endpoint_adapter_map( + self, + ) -> Mapping[type[ResourceEndpoint], ResourceEndpointAdapter]: + """Return custom live adapters keyed by their exact endpoint type.""" + return MappingProxyType( + { + getattr(type(adapter), "endpoint_type"): adapter + for adapter in self.endpoint_adapters + } + ) + + def create_parallel_safety_validator( + self, + *, + simulation: object, + robot: object, + ) -> ParallelCommandSafetyValidator | None: + """Create and strictly validate the registration-owned live safety gate.""" + self.assert_unchanged() + factory = self.parallel_safety_factory + if factory is None: + return None + with self._parallel_safety_validator_lock: + validator = factory.create(simulation=simulation, robot=robot) + if not isinstance(validator, ParallelCommandSafetyValidator): + raise TypeError( + "parallel_safety_factory.create() must return a " + "ParallelCommandSafetyValidator." + ) + if any( + validator is previous + for previous in self._parallel_safety_validator_history + ): + raise ValueError( + "ParallelCommandSafetyValidatorFactory.create() must return a " + "fresh validator for every runtime assembly owned by this " + "registration." + ) + self._parallel_safety_validator_history.append(validator) + return validator + def validate_scene_registry(self, registry: SceneRegistry) -> None: """Validate a live registry against the registered scene declaration.""" self.assert_unchanged() self.catalog.scene.validate_registry(registry) + def validate_engine(self, engine: AtomicActionEngine) -> None: + """Validate live skills and resolved endpoints against this registration.""" + self.assert_unchanged() + self.catalog.validate_engine(engine) + def validate_robot_profile( self, profile: RobotSkillProfile, *, step_dt: float, ) -> None: - """Validate a cadence-aligned live profile against its declaration.""" + """Validate a live profile against its provider-free declaration.""" self.assert_unchanged() if type(profile) is not RobotSkillProfile: raise TypeError("profile must be exactly RobotSkillProfile.") - expected = _profile_with_control_dt( - self.catalog.robot_profile, - control_dt=step_dt, - ) + if not isinstance(step_dt, (int, float)) or isinstance(step_dt, bool): + raise TypeError("step_dt must be a real number.") + expected = self.catalog.robot_profile if _canonical_json(profile) != _canonical_json(expected): raise IntegrationFingerprintMismatch( "Live robot skill profile differs from the registered declaration." diff --git a/embodichain/lab/gym/envs/expert_program/environment.py b/embodichain/lab/gym/envs/expert_program/environment.py index 593f9a953..2f690a5ca 100644 --- a/embodichain/lab/gym/envs/expert_program/environment.py +++ b/embodichain/lab/gym/envs/expert_program/environment.py @@ -26,6 +26,7 @@ from __future__ import annotations from collections.abc import Iterable, Mapping +from copy import deepcopy from dataclasses import dataclass import math from typing import Protocol, runtime_checkable @@ -61,6 +62,7 @@ analyze_parallel_branches, ) from embodichain.lab.sim.skills.profiles import ( + BoundRobotSkillProfile, ResourceEndpoint, ResourceEndpointAdapter, RobotSkillProfile, @@ -75,12 +77,17 @@ CurrentQposProvider, DemoBridgeError, EnvironmentStepClock, + JointPositionGymTransportEncoder, RuntimeCommandFrameEncoder, RuntimeTransportActionEncoder, SegmentPostPolicyPort, SegmentValidatorPort, ) -from .catalog import ExpertProgramIntegrationCatalog +from .catalog import ( + ExpertProgramIntegrationCatalog, + IntegrationFingerprintMismatch, + SimulationExpertProgramRegistration, +) from .cfg import ExpertProgramCfg, ExpertProgramIntegrationCfg from .compiler import ( CompiledProgram, @@ -216,6 +223,34 @@ def create_accepted_runtime_command_observer( """Return the observer shared by the command sink and evidence ports.""" +@runtime_checkable +class ParallelCommandSafetyValidatorProvider(Protocol): + """Runtime-factory capability for a fresh registration-owned safety gate.""" + + def create_parallel_command_safety_validator( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> ParallelCommandSafetyValidator: + """Create the live gate for the exact assembled runtime components.""" + + +@runtime_checkable +class _RegistrationOwningExpertProgramFactory(Protocol): + """Internal capability exposing one exact standard registration owner.""" + + @property + def expert_program_registration(self) -> SimulationExpertProgramRegistration: + """Return the exact registration owned by this live factory.""" + + def registration_owned_segment_policy_ports( + self, + ) -> tuple[SegmentPostPolicyPort | None, SegmentValidatorPort | None]: + """Return factory-owned post-policy and validator ports.""" + + @dataclass(frozen=True, slots=True) class ExpertProgramRuntimeAssembly: """Auditable result of one fresh environment runtime assembly. @@ -233,6 +268,8 @@ class ExpertProgramRuntimeAssembly: command_encoder: Runtime-frame to Gym-action encoder. command_sink: Buffered Gym command sink. accepted_command_observer: Optional transactional command-state owner. + runner_cfg: Runner policy selected by the integration runtime preset. + parallel_safety_validator: Optional fresh registration-owned safety gate. runtime: Nonblocking semantic skill runtime. """ @@ -248,6 +285,8 @@ class ExpertProgramRuntimeAssembly: command_encoder: RuntimeCommandFrameEncoder command_sink: BufferedGymCommandSink accepted_command_observer: AcceptedRuntimeCommandObserver | None + runner_cfg: ExecutionRunnerCfg + parallel_safety_validator: ParallelCommandSafetyValidator | None runtime: SkillRuntime @@ -282,6 +321,8 @@ class ExpertProgramEnvironmentAdapter: step_dt: Authoritative Gym control cadence in seconds. integration_catalog: Optional immutable task-registration catalog used for provider-free compilation. + registration: Optional exact standard task registration. When present, + every compiler/runtime extension comes exclusively from it. call_catalog: Optional immutable semantic call catalog. The built-in catalog is used when omitted. endpoint_adapters: Optional custom robot endpoint adapters. @@ -306,6 +347,7 @@ def __init__( *, step_dt: float, integration_catalog: ExpertProgramIntegrationCatalog | None = None, + registration: SimulationExpertProgramRegistration | None = None, call_catalog: SemanticCallCatalog | None = None, endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None @@ -342,6 +384,86 @@ def __init__( "integration_catalog must be exactly " "ExpertProgramIntegrationCatalog or None." ) + if ( + registration is not None + and type(registration) is not SimulationExpertProgramRegistration + ): + raise TypeError( + "registration must be exactly " + "SimulationExpertProgramRegistration or None." + ) + registration_owner = ( + factory + if isinstance(factory, _RegistrationOwningExpertProgramFactory) + else None + ) + if registration_owner is not None: + owned_registration = registration_owner.expert_program_registration + if type(owned_registration) is not SimulationExpertProgramRegistration: + raise TypeError( + "A registration-owning factory must expose exactly " + "SimulationExpertProgramRegistration." + ) + if registration is None: + raise ValueError( + "A registration-owning factory requires its exact registration; " + "catalog-only or unregistered adapter construction is forbidden." + ) + if registration is not owned_registration: + raise ValueError( + "registration must be the exact object owned by the factory." + ) + elif registration is not None: + raise TypeError( + "registration requires a factory that exposes exact registration " + "ownership and factory-owned segment policy ports." + ) + registered_lowerer_values = tuple(registered_lowerers) + relation_grounder_values = tuple(relation_grounders) + handover_pose_provider_values = tuple(handover_pose_providers) + runtime_transport_values = tuple(runtime_transports) + if registration is not None: + if integration_catalog is not None: + raise ValueError( + "integration_catalog cannot override an exact task registration." + ) + forbidden = { + "call_catalog": call_catalog is not None, + "endpoint_adapters": endpoint_adapters is not None, + "registered_lowerers": bool(registered_lowerer_values), + "relation_grounders": bool(relation_grounder_values), + "handover_pose_providers": bool(handover_pose_provider_values), + "effect_monitor_registry": effect_monitor_registry is not None, + "runtime_transports": bool(runtime_transport_values), + "runner_cfg": runner_cfg is not None, + "post_policy_port": post_policy_port is not None, + "validator_port": validator_port is not None, + "parallel_safety_validator": parallel_safety_validator is not None, + } + supplied = tuple(name for name, present in forbidden.items() if present) + if supplied: + raise ValueError( + "Standard task registration owns all semantic and runtime " + f"extensions; external overrides are forbidden: {supplied}." + ) + registration.assert_unchanged() + integration_catalog = registration.catalog + endpoint_adapters = dict(registration.endpoint_adapter_map) + registered_lowerer_values = () + relation_grounder_values = registration.relation_grounders + handover_pose_provider_values = registration.handover_pose_providers + effect_monitor_registry = None + runtime_transport_values = registration.runtime_transports + runner_cfg = None + parallel_safety_validator = None + assert registration_owner is not None + owned_ports = registration_owner.registration_owned_segment_policy_ports() + if type(owned_ports) is not tuple or len(owned_ports) != 2: + raise TypeError( + "registration_owned_segment_policy_ports() must return an " + "exact 2-tuple." + ) + post_policy_port, validator_port = owned_ports if integration_catalog is not None: if integration_catalog.scene_registry_id != scene_registry_id: raise ValueError( @@ -393,16 +515,17 @@ def __init__( self._scene_registry_id = scene_registry_id self._robot_profile_id = robot_profile_id self._step_dt = float(step_dt) + self._registration = registration self._integration_catalog = integration_catalog self._call_catalog = selected_catalog self._endpoint_adapters = ( None if endpoint_adapters is None else dict(endpoint_adapters) ) - self._registered_lowerers = tuple(registered_lowerers) - self._relation_grounders = tuple(relation_grounders) - self._handover_pose_providers = tuple(handover_pose_providers) + self._registered_lowerers = registered_lowerer_values + self._relation_grounders = relation_grounder_values + self._handover_pose_providers = handover_pose_provider_values self._effect_monitor_registry = effect_monitor_registry - self._runtime_transports = tuple(runtime_transports) + self._runtime_transports = runtime_transport_values self._runner_cfg = runner_cfg self._post_policy_port = post_policy_port self._validator_port = validator_port @@ -484,6 +607,7 @@ def _assemble_semantic_components( f"{self._robot_profile_id!r}, got {current_profile_id!r}." ) profile = self._factory.create_robot_skill_profile() + self._validate_registration_ownership() if type(profile) is not RobotSkillProfile: raise TypeError( "create_robot_skill_profile() must return exactly RobotSkillProfile." @@ -493,12 +617,31 @@ def _assemble_semantic_components( "Factory robot profile declaration drifted: expected " f"{self._robot_profile_id!r}, got {profile.profile_id!r}." ) + if self._registration is not None: + self._registration.validate_robot_profile( + profile, + step_dt=self._step_dt, + ) engine = self._factory.create_atomic_action_engine(profile) + self._validate_registration_ownership() if not isinstance(engine, AtomicActionEngine): raise TypeError( "create_atomic_action_engine() must return an AtomicActionEngine." ) + if self._registration is not None: + bound_profile = engine.skill_profile + if type(bound_profile) is not BoundRobotSkillProfile: + raise IntegrationFingerprintMismatch( + "The standard factory engine must own one exact bound robot " + "profile." + ) + if bound_profile.source_profile is not profile: + raise IntegrationFingerprintMismatch( + "The standard factory engine is bound to a different robot " + "profile object than the adapter validated." + ) + self._registration.validate_engine(engine) manifest = self._create_manifest( registry, @@ -510,6 +653,12 @@ def _assemble_semantic_components( engine, endpoint_adapters=self._endpoint_adapters, ) + if self._registration is not None: + self._validate_registration_ownership() + # ``manifest.bind`` resolves endpoints again and replaces the + # engine-owned bound profile. Revalidate that second live result so a + # provider cannot pass factory construction and drift before compile. + self._registration.validate_engine(engine) compiler = SemanticSkillCompiler( bound, registered_lowerers=self._registered_lowerers, @@ -539,6 +688,7 @@ def _assemble_execution_runtime( """Attach live observation, evidence, command, and runtime boundaries.""" if type(semantic) is not _ExpertProgramSemanticAssembly: raise TypeError("semantic must be exactly _ExpertProgramSemanticAssembly.") + self._validate_registration_ownership() clock = EnvironmentStepClock(self._step_dt) observation_provider = self._factory.create_planning_observation_provider( @@ -546,6 +696,7 @@ def _assemble_execution_runtime( engine=semantic.engine, clock=clock, ) + self._validate_registration_ownership() if not isinstance(observation_provider, PlanningObservationPort): raise TypeError( "create_planning_observation_provider() must return a port " @@ -556,6 +707,7 @@ def _assemble_execution_runtime( engine=semantic.engine, observation_provider=observation_provider, ) + self._validate_registration_ownership() if isinstance(providers, (str, bytes)): raise TypeError( "create_effect_evidence_providers() must return an iterable of " @@ -571,10 +723,30 @@ def _assemble_execution_runtime( evidence_collector = EffectEvidenceCollector( EffectEvidenceProviderRegistry(provider_values) ) + expected_transport_ids: tuple[str, ...] | None = None + include_joint_position = True + if self._registration is not None: + expected_transport_ids = tuple( + declaration.transport_id + for declaration in ( + self._registration.catalog.runtime_transport_declarations + ) + ) + include_joint_position = ( + JointPositionGymTransportEncoder.transport_id in expected_transport_ids + ) command_encoder = RuntimeCommandFrameEncoder( observation_provider, transports=self._runtime_transports, + include_joint_position=include_joint_position, ) + if expected_transport_ids is not None: + if command_encoder.transport_ids != expected_transport_ids: + raise IntegrationFingerprintMismatch( + "Live command encoder transport order differs from the exact " + "registration catalog." + ) + command_encoder.freeze() accepted_command_observer: AcceptedRuntimeCommandObserver | None = None if isinstance(self._factory, AcceptedRuntimeCommandObserverFactory): accepted_command_observer = ( @@ -584,6 +756,7 @@ def _assemble_execution_runtime( observation_provider=observation_provider, ) ) + self._validate_registration_ownership() if not isinstance( accepted_command_observer, AcceptedRuntimeCommandObserver, @@ -597,14 +770,56 @@ def _assemble_execution_runtime( clock, accepted_command_observer=accepted_command_observer, ) + try: + selected_preset = semantic.robot_profile.presets[ + semantic.integration.runtime_preset + ] + except KeyError as exc: + raise ValueError( + "The selected runtime preset is absent from the assembled robot " + "profile." + ) from exc + selected_runner_cfg = selected_preset.runner_cfg + if self._registration is None and self._runner_cfg is not None: + selected_runner_cfg = deepcopy(self._runner_cfg) runtime = SkillRuntime.from_components( semantic.compiler, observation_provider, command_sink, evidence_collector, clock=clock, - runner_cfg=self._runner_cfg, + runner_cfg=deepcopy(selected_runner_cfg), ) + parallel_safety_validator = self._parallel_safety_validator + if ( + self._registration is not None + and self._registration.parallel_safety_factory is not None + ): + if not isinstance( + self._factory, + ParallelCommandSafetyValidatorProvider, + ): + raise TypeError( + "A registration-owned parallel_safety_factory requires the " + "environment factory to implement " + "ParallelCommandSafetyValidatorProvider." + ) + parallel_safety_validator = ( + self._factory.create_parallel_command_safety_validator( + scene_registry=semantic.scene_registry, + engine=semantic.engine, + observation_provider=observation_provider, + ) + ) + self._validate_registration_ownership() + if not isinstance( + parallel_safety_validator, + ParallelCommandSafetyValidator, + ): + raise TypeError( + "create_parallel_command_safety_validator() must return a " + "ParallelCommandSafetyValidator." + ) return ExpertProgramRuntimeAssembly( integration=semantic.integration, scene_registry=semantic.scene_registry, @@ -618,6 +833,8 @@ def _assemble_execution_runtime( command_encoder=command_encoder, command_sink=command_sink, accepted_command_observer=accepted_command_observer, + runner_cfg=selected_runner_cfg, + parallel_safety_validator=parallel_safety_validator, runtime=runtime, ) @@ -645,7 +862,8 @@ def create_bridge(self, program: CompiledProgram) -> AtomicDemoBridge: assembly.clock, post_policy_port=self._post_policy_port, validator_port=self._validator_port, - parallel_safety_validator=self._parallel_safety_validator, + runner_cfg=assembly.runner_cfg, + parallel_safety_validator=assembly.parallel_safety_validator, ) def _preflight_program_surfaces( @@ -695,12 +913,13 @@ def _preflight_program( raise TypeError("compiler must be a SemanticSkillCompiler.") analyses = program.preflight_analyses() if any(analysis.kind == "parallel_branch" for analysis in analyses) and ( - self._parallel_safety_validator is None + not self._parallel_safety_is_registered ): raise ValueError( "Expert Programs containing parallel blocks require an explicit " "ParallelCommandSafetyValidator before bridge creation." ) + index = 0 while index < len(analyses): analysis = analyses[index] @@ -734,6 +953,13 @@ def _preflight_program( branch_paths=branch_paths, ) + @property + def _parallel_safety_is_registered(self) -> bool: + """Whether static assembly owns an authoritative parallel safety gate.""" + if self._registration is not None: + return self._registration.parallel_safety_factory is not None + return self._parallel_safety_validator is not None + def _validate_selection( self, integration: ExpertProgramIntegrationCfg, @@ -741,6 +967,7 @@ def _validate_selection( """Reject an integration selection owned by another adapter.""" if type(integration) is not ExpertProgramIntegrationCfg: raise TypeError("integration must be exactly ExpertProgramIntegrationCfg.") + self._validate_registration_ownership() current_scene_id = _validate_identifier( self._factory.scene_registry_id, field_name="factory.scene_registry_id", @@ -772,6 +999,28 @@ def _validate_selection( f"only {self._robot_profile_id!r}." ) + def _validate_registration_ownership(self) -> None: + """Reject a standard factory whose exact registration owner drifted.""" + registration = self._registration + if registration is None: + return + if not isinstance(self._factory, _RegistrationOwningExpertProgramFactory): + raise IntegrationFingerprintMismatch( + "The standard environment factory no longer exposes registration " + "ownership." + ) + current = self._factory.expert_program_registration + if type(current) is not SimulationExpertProgramRegistration: + raise IntegrationFingerprintMismatch( + "The standard environment factory no longer exposes an exact " + "SimulationExpertProgramRegistration." + ) + if current is not registration: + raise IntegrationFingerprintMismatch( + "The standard environment factory registration ownership changed " + "after adapter construction." + ) + def _create_scene_registry(self) -> SceneRegistry: """Create and validate one exact live scene registry.""" current_id = _validate_identifier( @@ -784,10 +1033,13 @@ def _create_scene_registry(self) -> SceneRegistry: f"{self._scene_registry_id!r}, got {current_id!r}." ) registry = self._factory.create_scene_registry() + self._validate_registration_ownership() if type(registry) is not SceneRegistry: raise TypeError( "create_scene_registry() must return exactly SceneRegistry." ) + if self._registration is not None: + self._registration.validate_scene_registry(registry) return registry def _create_manifest( diff --git a/embodichain/lab/gym/envs/expert_program/extensions.py b/embodichain/lab/gym/envs/expert_program/extensions.py new file mode 100644 index 000000000..a0d549f02 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/extensions.py @@ -0,0 +1,908 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Typed standard-runtime extension declarations for Expert Programs. + +The values in this module deliberately describe extension wiring without +creating a simulator or resolving one live robot endpoint. A task +registration owns the corresponding adapter, transport, and safety-factory +instances, while its provider-free catalog owns the exact declarations below. +""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, fields, is_dataclass +from enum import Enum +from types import MappingProxyType +from typing import ClassVar, Protocol, runtime_checkable + +import torch + +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import RuntimeCommandPayload +from embodichain.lab.sim.skills.effects import ( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, +) +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + ControlPartEndpointAdapter, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotSkillProfile, +) + +from .bridge import ( + JointPositionGymTransportEncoder, + RuntimeTransportActionEncoder, +) + +VersionedKey = tuple[str, str] +"""Exact ``(provider_or_projector_id, revision)`` registry key.""" + +_BUILTIN_TRACKING_FEEDBACK_SOURCE_KEYS = frozenset({("planning_context.robot", "1")}) +_BUILTIN_TRACKING_PROJECTOR_KEYS = frozenset({("joint_position_payload", "1")}) +_BUILTIN_EFFECT_EVIDENCE_SOURCE_KEYS = frozenset( + { + ( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ) + } +) + + +def _identifier(value: object, *, field_name: str) -> str: + """Validate one exact, non-empty registration identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _qualified_name(value: type[object] | object) -> str: + """Return one deterministic diagnostic name.""" + value_type = value if isinstance(value, type) else type(value) + return f"{value_type.__module__}.{value_type.__qualname__}" + + +def _class_attribute(value: object, name: str, *, field_name: str) -> object: + """Read registration metadata from the provider type, never instance state.""" + owner = type(value) + if not hasattr(owner, name): + raise TypeError(f"{field_name} must be declared on {owner.__name__}.") + return getattr(owner, name) + + +def _versioned_keys(value: object, *, field_name: str) -> frozenset[VersionedKey]: + """Validate one exact immutable set of versioned registry keys.""" + if type(value) is not frozenset: + raise TypeError(f"{field_name} must be an exact frozenset.") + normalized: set[VersionedKey] = set() + for key in value: + if type(key) is not tuple or len(key) != 2: + raise TypeError(f"{field_name} must contain exact 2-tuples.") + identifier, revision = key + normalized.add( + ( + _identifier(identifier, field_name=f"{field_name} IDs"), + _identifier(revision, field_name=f"{field_name} revisions"), + ) + ) + return frozenset(normalized) + + +def _identifier_set(value: object, *, field_name: str) -> frozenset[str]: + """Validate one exact immutable set of identifiers.""" + if type(value) is not frozenset: + raise TypeError(f"{field_name} must be an exact frozenset.") + return frozenset(_identifier(item, field_name=field_name) for item in value) + + +def _type_tuple( + value: object, + *, + base_type: type[object], + field_name: str, +) -> tuple[type[object], ...]: + """Validate one non-empty exact tuple of unique exact value types.""" + if type(value) is not tuple or not value: + raise TypeError(f"{field_name} must be a non-empty exact tuple.") + normalized: list[type[object]] = [] + for item in value: + if not isinstance(item, type) or not issubclass(item, base_type): + raise TypeError( + f"{field_name} values must be {base_type.__name__} subclasses." + ) + normalized.append(item) + if len(set(normalized)) != len(normalized): + raise ValueError(f"{field_name} must not contain duplicate exact types.") + return tuple(normalized) + + +def validate_immutable_extension_declaration( + value: object, + *, + field_name: str, +) -> None: + """Accept only a deeply immutable frozen dataclass or stateless instance. + + Frozen dataclass fields may contain only immutable scalar values, types, + enums with immutable values, exact tuples, exact frozensets, and recursively + frozen dataclasses. + Mutable leaves such as mappings, lists, sets, bytearrays, and tensors are + rejected because registration-owned live extensions are shared with an + assembled runtime. A non-dataclass extension must not have instance or + slot state at all. + """ + if isinstance(value, type): + raise TypeError(f"{field_name} must contain instances, not types.") + + def validate_state( + declaration: object, + *, + path: str, + ) -> tuple[bool, tuple[str, ...]]: + """Validate declared state and return dataclass field names.""" + dataclass_declaration = is_dataclass(declaration) + dataclass_field_names: set[str] = set() + if dataclass_declaration: + params = getattr(type(declaration), "__dataclass_params__", None) + if params is None or not params.frozen: + raise TypeError( + f"{path} stateful declarations must be frozen dataclasses." + ) + dataclass_field_names.update(item.name for item in fields(declaration)) + + state_names: set[str] = set() + instance_state = getattr(declaration, "__dict__", None) + if isinstance(instance_state, Mapping): + state_names.update(instance_state) + for owner in type(declaration).__mro__: + declared_slots = getattr(owner, "__slots__", ()) + slots = ( + (declared_slots,) if isinstance(declared_slots, str) else declared_slots + ) + for slot_name in slots: + if slot_name in {"__dict__", "__weakref__"}: + continue + storage_name = ( + f"_{owner.__name__.lstrip('_')}{slot_name}" + if slot_name.startswith("__") and not slot_name.endswith("__") + else slot_name + ) + if hasattr(declaration, storage_name): + state_names.add(storage_name) + undeclared_state = ( + state_names.difference(dataclass_field_names) + if dataclass_declaration + else state_names + ) + if undeclared_state: + raise TypeError( + f"{path} contains unfingerprinted state " + f"{sorted(undeclared_state)}; Use a frozen dataclass with every " + "configuration field declared, or a stateless instance." + ) + return dataclass_declaration, tuple(sorted(dataclass_field_names)) + + def validate_nested( + nested: object, + *, + path: str, + active: set[int], + ) -> None: + """Reject every mutable or opaque leaf in one declaration graph.""" + if nested is None or type(nested) in {bool, int, float, str}: + return + if isinstance(nested, type): + return + if isinstance(nested, Enum): + validate_nested( + nested.value, + path=f"{path}.value", + active=active, + ) + return + if isinstance(nested, torch.Tensor) or type(nested) in { + list, + dict, + set, + bytearray, + }: + raise TypeError( + f"{path} must be deeply immutable; mutable value type " + f"{_qualified_name(nested)!r} is forbidden." + ) + if isinstance(nested, Mapping): + raise TypeError( + f"{path} must be deeply immutable; mapping values are forbidden." + ) + + nested_id = id(nested) + if nested_id in active: + raise TypeError(f"{path} must not contain a cyclic declaration graph.") + if type(nested) in {tuple, frozenset}: + active.add(nested_id) + try: + for index, item in enumerate(nested): + validate_nested( + item, + path=f"{path}[{index}]", + active=active, + ) + finally: + active.remove(nested_id) + return + if is_dataclass(nested) and not isinstance(nested, type): + active.add(nested_id) + try: + _, nested_field_names = validate_state(nested, path=path) + for nested_field_name in nested_field_names: + validate_nested( + getattr(nested, nested_field_name), + path=f"{path}.{nested_field_name}", + active=active, + ) + finally: + active.remove(nested_id) + return + raise TypeError( + f"{path} contains unsupported value type " + f"{_qualified_name(nested)!r}; extension declarations must be " + "complete deeply immutable data." + ) + + dataclass_declaration, dataclass_field_names = validate_state( + value, + path=field_name, + ) + if dataclass_declaration: + for dataclass_field_name in dataclass_field_names: + validate_nested( + getattr(value, dataclass_field_name), + path=f"{field_name}.{dataclass_field_name}", + active={id(value)}, + ) + + +@dataclass(frozen=True, slots=True) +class EndpointAdapterDeclaration: + """Provider-free declaration of one exact endpoint adapter.""" + + endpoint_type: type[ResourceEndpoint] + adapter_type: type[ResourceEndpointAdapter] + adapter_id: str + runtime_transport_ids: frozenset[str] + runtime_target_types: tuple[type[RuntimeEndpointTarget], ...] + tracking_feedback_source_keys: frozenset[VersionedKey] + tracking_projector_keys: frozenset[VersionedKey] + effect_evidence_source_keys: frozenset[VersionedKey] + + def __post_init__(self) -> None: + if not isinstance(self.endpoint_type, type) or not issubclass( + self.endpoint_type, ResourceEndpoint + ): + raise TypeError("endpoint_type must be a ResourceEndpoint subclass.") + if not isinstance(self.adapter_type, type) or not issubclass( + self.adapter_type, ResourceEndpointAdapter + ): + raise TypeError("adapter_type must be a ResourceEndpointAdapter subclass.") + _identifier(self.adapter_id, field_name="adapter_id") + object.__setattr__( + self, + "runtime_transport_ids", + _identifier_set( + self.runtime_transport_ids, + field_name="runtime_transport_ids", + ), + ) + if not self.runtime_transport_ids: + raise ValueError("runtime_transport_ids must not be empty.") + object.__setattr__( + self, + "runtime_target_types", + _type_tuple( + self.runtime_target_types, + base_type=RuntimeEndpointTarget, + field_name="runtime_target_types", + ), + ) + object.__setattr__( + self, + "tracking_feedback_source_keys", + _versioned_keys( + self.tracking_feedback_source_keys, + field_name="tracking_feedback_source_keys", + ), + ) + object.__setattr__( + self, + "tracking_projector_keys", + _versioned_keys( + self.tracking_projector_keys, + field_name="tracking_projector_keys", + ), + ) + object.__setattr__( + self, + "effect_evidence_source_keys", + _versioned_keys( + self.effect_evidence_source_keys, + field_name="effect_evidence_source_keys", + ), + ) + + +@dataclass(frozen=True, slots=True) +class RuntimeTransportDeclaration: + """Provider-free declaration of one ordered runtime transport encoder.""" + + transport_type: type[RuntimeTransportActionEncoder] + transport_id: str + target_types: tuple[type[RuntimeEndpointTarget], ...] + payload_types: tuple[type[RuntimeCommandPayload], ...] + + def __post_init__(self) -> None: + if not isinstance(self.transport_type, type): + raise TypeError("transport_type must be a type.") + _identifier(self.transport_id, field_name="transport_id") + object.__setattr__( + self, + "target_types", + _type_tuple( + self.target_types, + base_type=RuntimeEndpointTarget, + field_name="target_types", + ), + ) + object.__setattr__( + self, + "payload_types", + _type_tuple( + self.payload_types, + base_type=RuntimeCommandPayload, + field_name="payload_types", + ), + ) + for field_name, declared_types in ( + ("target_types", self.target_types), + ("payload_types", self.payload_types), + ): + for declared_type in declared_types: + try: + type_transport_id = declared_type.__dict__["TRANSPORT_ID"] + except KeyError as exc: + raise TypeError( + f"{field_name} value {declared_type.__name__} must declare " + "an exact ClassVar TRANSPORT_ID on that type; inherited or " + "instance-only transport IDs are forbidden." + ) from exc + _identifier( + type_transport_id, + field_name=f"{declared_type.__name__}.TRANSPORT_ID", + ) + if type_transport_id != self.transport_id: + raise ValueError( + f"{field_name} value {declared_type.__name__} declares " + f"transport {type_transport_id!r}, not " + f"{self.transport_id!r}." + ) + + +@dataclass(frozen=True, slots=True) +class ParallelSafetyDeclaration: + """Provider-free identity and transport coverage of one safety factory.""" + + factory_type: type[object] + validator_id: str + revision: str + supported_transport_ids: frozenset[str] + + def __post_init__(self) -> None: + if not isinstance(self.factory_type, type): + raise TypeError("factory_type must be a type.") + _identifier(self.validator_id, field_name="validator_id") + _identifier(self.revision, field_name="revision") + object.__setattr__( + self, + "supported_transport_ids", + _identifier_set( + self.supported_transport_ids, + field_name="supported_transport_ids", + ), + ) + if not self.supported_transport_ids: + raise ValueError("supported_transport_ids must not be empty.") + + +@runtime_checkable +class ParallelCommandSafetyValidatorFactory(Protocol): + """Registration-owned factory for one authoritative live safety gate.""" + + validator_id: ClassVar[str] + revision: ClassVar[str] + supported_transport_ids: ClassVar[frozenset[str]] + + def create( + self, + *, + simulation: object, + robot: object, + ) -> ParallelCommandSafetyValidator: + """Create one live validator bound to the exact simulation and robot.""" + + +@dataclass(frozen=True, slots=True) +class StandardExtensionDeclarations: + """Cross-checked provider-free declarations for the standard factory.""" + + endpoint_adapters: Mapping[type[ResourceEndpoint], EndpointAdapterDeclaration] + runtime_transports: tuple[RuntimeTransportDeclaration, ...] + parallel_safety: ParallelSafetyDeclaration | None + + def __post_init__(self) -> None: + if not isinstance(self.endpoint_adapters, Mapping): + raise TypeError("endpoint_adapters must be a mapping.") + normalized: dict[type[ResourceEndpoint], EndpointAdapterDeclaration] = {} + for endpoint_type, declaration in self.endpoint_adapters.items(): + if type(declaration) is not EndpointAdapterDeclaration: + raise TypeError( + "endpoint_adapters values must be EndpointAdapterDeclaration " + "values." + ) + if endpoint_type is not declaration.endpoint_type: + raise ValueError( + "endpoint_adapters keys must exactly match declaration " + "endpoint_type values." + ) + normalized[endpoint_type] = declaration + object.__setattr__(self, "endpoint_adapters", MappingProxyType(normalized)) + transports = tuple(self.runtime_transports) + if not transports or not all( + type(value) is RuntimeTransportDeclaration for value in transports + ): + raise TypeError( + "runtime_transports must contain RuntimeTransportDeclaration values." + ) + object.__setattr__(self, "runtime_transports", transports) + if ( + self.parallel_safety is not None + and type(self.parallel_safety) is not ParallelSafetyDeclaration + ): + raise TypeError( + "parallel_safety must be ParallelSafetyDeclaration or None." + ) + adapter_ids = [value.adapter_id for value in normalized.values()] + if len(set(adapter_ids)) != len(adapter_ids): + raise ValueError("Endpoint adapter IDs must be unique.") + transport_ids = [value.transport_id for value in transports] + if len(set(transport_ids)) != len(transport_ids): + raise ValueError("Runtime transport IDs must be unique.") + transport_by_id = {value.transport_id: value for value in transports} + required_transport_ids = frozenset( + transport_id + for declaration in normalized.values() + for transport_id in declaration.runtime_transport_ids + ) + if required_transport_ids != frozenset(transport_by_id): + raise ValueError( + "Provider-free runtime transports must exactly cover endpoint " + f"adapter transport IDs; expected {sorted(required_transport_ids)}, " + f"got {sorted(transport_by_id)}." + ) + target_owners: dict[type[RuntimeEndpointTarget], str] = {} + for transport in transports: + for target_type in transport.target_types: + if target_type in target_owners: + raise ValueError( + f"Runtime target type {_qualified_name(target_type)!r} has " + "multiple transport owners." + ) + target_owners[target_type] = transport.transport_id + declared_target_types: set[type[RuntimeEndpointTarget]] = set() + for adapter in normalized.values(): + counts = {transport_id: 0 for transport_id in adapter.runtime_transport_ids} + for target_type in adapter.runtime_target_types: + owner = target_owners.get(target_type) + if owner is None or owner not in counts: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} target type " + f"{_qualified_name(target_type)!r} has no matching " + "declared transport." + ) + counts[owner] += 1 + declared_target_types.add(target_type) + unused = sorted(key for key, count in counts.items() if count == 0) + if unused: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} declares unused " + f"transport IDs {unused}." + ) + if declared_target_types != set(target_owners): + raise ValueError( + "Provider-free runtime target types must be covered exactly by " + "endpoint adapter declarations." + ) + _validate_builtin_routes(normalized) + if self.parallel_safety is not None and ( + self.parallel_safety.supported_transport_ids != frozenset(transport_by_id) + ): + raise ValueError( + "Parallel safety transport coverage must exactly match the " + "provider-free runtime transports." + ) + + +def declare_endpoint_adapter( + adapter: ResourceEndpointAdapter, +) -> EndpointAdapterDeclaration: + """Read one endpoint adapter's exact static extension contract.""" + if not isinstance(adapter, ResourceEndpointAdapter): + raise TypeError("endpoint adapters must be ResourceEndpointAdapter instances.") + validate_immutable_extension_declaration( + adapter, + field_name="endpoint_adapters", + ) + endpoint_type = _class_attribute( + adapter, + "endpoint_type", + field_name="ResourceEndpointAdapter.endpoint_type", + ) + if not isinstance(endpoint_type, type) or not issubclass( + endpoint_type, ResourceEndpoint + ): + raise TypeError( + "ResourceEndpointAdapter.endpoint_type must be a ResourceEndpoint " + "subclass." + ) + return EndpointAdapterDeclaration( + endpoint_type=endpoint_type, + adapter_type=type(adapter), + adapter_id=_identifier( + _class_attribute( + adapter, + "adapter_id", + field_name="ResourceEndpointAdapter.adapter_id", + ), + field_name="ResourceEndpointAdapter.adapter_id", + ), + runtime_transport_ids=_class_attribute( + adapter, + "runtime_transport_ids", + field_name="ResourceEndpointAdapter.runtime_transport_ids", + ), + runtime_target_types=_class_attribute( + adapter, + "runtime_target_types", + field_name="ResourceEndpointAdapter.runtime_target_types", + ), + tracking_feedback_source_keys=_class_attribute( + adapter, + "tracking_feedback_source_keys", + field_name="ResourceEndpointAdapter.tracking_feedback_source_keys", + ), + tracking_projector_keys=_class_attribute( + adapter, + "tracking_projector_keys", + field_name="ResourceEndpointAdapter.tracking_projector_keys", + ), + effect_evidence_source_keys=_class_attribute( + adapter, + "effect_evidence_source_keys", + field_name="ResourceEndpointAdapter.effect_evidence_source_keys", + ), + ) + + +def declare_runtime_transport( + transport: RuntimeTransportActionEncoder, +) -> RuntimeTransportDeclaration: + """Read one runtime encoder's exact static target/payload contract.""" + if not isinstance(transport, RuntimeTransportActionEncoder): + raise TypeError( + "runtime_transports must implement RuntimeTransportActionEncoder." + ) + validate_immutable_extension_declaration( + transport, + field_name="runtime_transports", + ) + return RuntimeTransportDeclaration( + transport_type=type(transport), + transport_id=_identifier( + _class_attribute( + transport, + "transport_id", + field_name="RuntimeTransportActionEncoder.transport_id", + ), + field_name="RuntimeTransportActionEncoder.transport_id", + ), + target_types=_class_attribute( + transport, + "target_types", + field_name="RuntimeTransportActionEncoder.target_types", + ), + payload_types=_class_attribute( + transport, + "payload_types", + field_name="RuntimeTransportActionEncoder.payload_types", + ), + ) + + +def declare_parallel_safety_factory( + factory: ParallelCommandSafetyValidatorFactory, +) -> ParallelSafetyDeclaration: + """Read one safety factory's exact static identity and coverage.""" + create = getattr(factory, "create", None) + if not callable(create): + raise TypeError("parallel_safety_factory must define create().") + validate_immutable_extension_declaration( + factory, + field_name="parallel_safety_factory", + ) + return ParallelSafetyDeclaration( + factory_type=type(factory), + validator_id=_identifier( + _class_attribute( + factory, + "validator_id", + field_name="ParallelCommandSafetyValidatorFactory.validator_id", + ), + field_name="ParallelCommandSafetyValidatorFactory.validator_id", + ), + revision=_identifier( + _class_attribute( + factory, + "revision", + field_name="ParallelCommandSafetyValidatorFactory.revision", + ), + field_name="ParallelCommandSafetyValidatorFactory.revision", + ), + supported_transport_ids=_class_attribute( + factory, + "supported_transport_ids", + field_name=( + "ParallelCommandSafetyValidatorFactory.supported_transport_ids" + ), + ), + ) + + +def _profile_endpoint_types( + profile: RobotSkillProfile, +) -> frozenset[type[ResourceEndpoint]]: + """Return every exact endpoint declaration type used by one profile.""" + if type(profile) is not RobotSkillProfile: + raise TypeError("profile must be exactly RobotSkillProfile.") + return frozenset( + type(endpoint) + for resource in profile.resources.values() + for endpoint in resource.endpoints.values() + ) + + +def _validate_builtin_routes( + declarations: Mapping[type[ResourceEndpoint], EndpointAdapterDeclaration], +) -> None: + """Keep C1 custom endpoints open-loop and preserve exact built-in routes.""" + for endpoint_type, declaration in declarations.items(): + if endpoint_type is ControlPartEndpoint: + if ( + declaration.tracking_feedback_source_keys + != _BUILTIN_TRACKING_FEEDBACK_SOURCE_KEYS + or declaration.tracking_projector_keys + != _BUILTIN_TRACKING_PROJECTOR_KEYS + or declaration.effect_evidence_source_keys + != _BUILTIN_EFFECT_EVIDENCE_SOURCE_KEYS + ): + raise ValueError( + "The built-in ControlPartEndpoint adapter must retain its " + "exact tracking and effect-evidence routes." + ) + continue + if ( + declaration.tracking_feedback_source_keys + or declaration.tracking_projector_keys + or declaration.effect_evidence_source_keys + ): + raise ValueError( + f"Custom endpoint adapter {declaration.adapter_id!r} must declare " + "empty tracking and effect-evidence routes; the C1 standard " + "simulation factory does not install custom closed-loop providers." + ) + + +def build_standard_extension_declarations( + *, + profile: RobotSkillProfile, + endpoint_adapters: tuple[ResourceEndpointAdapter, ...], + runtime_transports: tuple[RuntimeTransportActionEncoder, ...], + parallel_safety_factory: ParallelCommandSafetyValidatorFactory | None, +) -> StandardExtensionDeclarations: + """Cross-check standard-runtime extensions against one exact profile. + + The built-in control-part adapter and joint-position transport cannot be + overridden. They are installed first only when the profile uses a + :class:`ControlPartEndpoint`; a pure-custom profile contains only its custom + declarations. Custom adapters and transports must cover exactly the endpoint + types and transport IDs used by the registered profile; unused declarations + fail closed. + """ + if type(endpoint_adapters) is not tuple: + raise TypeError("endpoint_adapters must be an exact tuple.") + if type(runtime_transports) is not tuple: + raise TypeError("runtime_transports must be an exact tuple.") + + builtin_adapter = declare_endpoint_adapter(ControlPartEndpointAdapter()) + custom_adapters = tuple( + declare_endpoint_adapter(adapter) for adapter in endpoint_adapters + ) + adapter_declarations = (builtin_adapter, *custom_adapters) + endpoint_types = [value.endpoint_type for value in adapter_declarations] + adapter_ids = [value.adapter_id for value in adapter_declarations] + if len(set(endpoint_types)) != len(endpoint_types): + raise ValueError( + "Endpoint adapter declarations contain a duplicate exact endpoint " + "type or attempt to override the built-in ControlPartEndpoint." + ) + if len(set(adapter_ids)) != len(adapter_ids): + raise ValueError( + "Endpoint adapter declarations contain a duplicate adapter ID or " + "attempt to override a built-in adapter." + ) + installed_by_type = { + declaration.endpoint_type: declaration for declaration in adapter_declarations + } + used_endpoint_types = _profile_endpoint_types(profile) + missing_adapters = used_endpoint_types - set(installed_by_type) + unused_adapters = set(installed_by_type) - used_endpoint_types + unused_adapters.discard(ControlPartEndpoint) + if missing_adapters or unused_adapters: + raise ValueError( + "Endpoint adapter coverage must exactly match profile endpoint types; " + f"missing={sorted(_qualified_name(value) for value in missing_adapters)}, " + f"unused={sorted(_qualified_name(value) for value in unused_adapters)}." + ) + if ControlPartEndpoint not in used_endpoint_types: + installed_by_type.pop(ControlPartEndpoint) + + _validate_builtin_routes(installed_by_type) + + builtin_transport = declare_runtime_transport(JointPositionGymTransportEncoder()) + custom_transports = tuple( + declare_runtime_transport(transport) for transport in runtime_transports + ) + transport_declarations = (builtin_transport, *custom_transports) + transport_ids = [value.transport_id for value in transport_declarations] + if len(set(transport_ids)) != len(transport_ids): + raise ValueError( + "Runtime transport declarations contain a duplicate transport ID or " + "attempt to override the built-in joint-position transport." + ) + transport_by_id = { + declaration.transport_id: declaration for declaration in transport_declarations + } + required_transport_ids = frozenset( + transport_id + for declaration in installed_by_type.values() + for transport_id in declaration.runtime_transport_ids + ) + missing_transports = required_transport_ids - set(transport_by_id) + unused_transports = set(transport_by_id) - required_transport_ids + unused_transports.discard(JointPositionTarget.TRANSPORT_ID) + if missing_transports or unused_transports: + raise ValueError( + "Runtime transport coverage must exactly match endpoint adapters; " + f"missing={sorted(missing_transports)}, " + f"unused={sorted(unused_transports)}." + ) + if JointPositionTarget.TRANSPORT_ID not in required_transport_ids: + transport_declarations = custom_transports + transport_by_id.pop(JointPositionTarget.TRANSPORT_ID) + + target_owners: dict[type[RuntimeEndpointTarget], str] = {} + for transport in transport_declarations: + for target_type in transport.target_types: + previous = target_owners.get(target_type) + if previous is not None: + raise ValueError( + f"Runtime target type {_qualified_name(target_type)!r} is " + f"declared by both transports {previous!r} and " + f"{transport.transport_id!r}." + ) + target_owners[target_type] = transport.transport_id + + adapter_target_types: set[type[RuntimeEndpointTarget]] = set() + for adapter in installed_by_type.values(): + for transport_id in adapter.runtime_transport_ids: + if transport_id not in transport_by_id: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} requires missing " + f"transport {transport_id!r}." + ) + per_transport_counts = { + transport_id: 0 for transport_id in adapter.runtime_transport_ids + } + for target_type in adapter.runtime_target_types: + owner = target_owners.get(target_type) + if owner is None or owner not in adapter.runtime_transport_ids: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} target type " + f"{_qualified_name(target_type)!r} is not covered by one of " + f"its transports {sorted(adapter.runtime_transport_ids)}." + ) + per_transport_counts[owner] += 1 + adapter_target_types.add(target_type) + unused_adapter_transport_ids = sorted( + transport_id + for transport_id, count in per_transport_counts.items() + if count == 0 + ) + if unused_adapter_transport_ids: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} declares unused " + f"transport IDs {unused_adapter_transport_ids}." + ) + extra_transport_target_types = set(target_owners) - adapter_target_types + if extra_transport_target_types: + raise ValueError( + "Runtime transports declare target types unused by endpoint adapters: " + f"{sorted(_qualified_name(value) for value in extra_transport_target_types)}." + ) + + parallel_safety = ( + None + if parallel_safety_factory is None + else declare_parallel_safety_factory(parallel_safety_factory) + ) + if parallel_safety is not None: + installed_transport_ids = frozenset(transport_by_id) + if parallel_safety.supported_transport_ids != installed_transport_ids: + raise ValueError( + "parallel_safety_factory must support exactly the registered " + f"runtime transports; expected {sorted(installed_transport_ids)}, " + f"got {sorted(parallel_safety.supported_transport_ids)}." + ) + + return StandardExtensionDeclarations( + endpoint_adapters=installed_by_type, + runtime_transports=transport_declarations, + parallel_safety=parallel_safety, + ) + + +__all__ = [ + "EndpointAdapterDeclaration", + "ParallelCommandSafetyValidatorFactory", + "ParallelSafetyDeclaration", + "RuntimeTransportDeclaration", + "StandardExtensionDeclarations", + "VersionedKey", + "build_standard_extension_declarations", + "declare_endpoint_adapter", + "declare_parallel_safety_factory", + "declare_runtime_transport", + "validate_immutable_extension_declaration", +] diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py index f2a5f2782..b5f5a118d 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -57,7 +57,6 @@ ControlPartCommandProfile, JointPositionCommand, ) -from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg from embodichain.lab.sim.atomic_actions.runtime_commands import ( JointPositionPayload, RuntimeCommandFrame, @@ -68,30 +67,22 @@ MotionGenerator, ToppraPlannerCfg, ) -from embodichain.lab.sim.skills.compiler import ( - RegisteredSemanticLowerer, -) from embodichain.lab.sim.skills.effects import ( ControlPartEvidenceAddress, - EffectMonitorRegistry, ) from embodichain.lab.sim.skills.evidence import ( BinaryEffectEvidenceQuery, - BinaryObservationCallback, BinaryEffectObservation, ControlPartRobotEvidenceSource, ControlPartSimulationEvidenceProvider, EffectEvidenceCollectionContext, EffectEvidenceProvider, - ScalarObservationCallback, SceneArticulationEvidenceProvider, ) from embodichain.lab.sim.skills.parallel_runtime import ( ParallelCommandSafetyValidator, ) from embodichain.lab.sim.skills.profiles import ( - ResourceEndpoint, - ResourceEndpointAdapter, RobotSkillProfile, SkillPolicyPreset, ) @@ -101,7 +92,6 @@ AcceptedRuntimeCommandObserver, EnvironmentStepClock, GymPlanningObservationProvider, - RuntimeTransportActionEncoder, ) from .catalog import SimulationExpertProgramRegistration from .environment import ( @@ -725,19 +715,12 @@ class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): motion_generator_factory: Optional fresh-generator factory. It is mutually exclusive with ``planner_cfg`` and intended for custom planners and isolated tests. - endpoint_adapters: Explicit adapters for non-built-in resource endpoint - types. translation_threshold: Material scene translation threshold. rotation_threshold: Material scene rotation threshold. - contact_observer: Optional raw contact evidence callback. - constraint_observer: Optional raw constraint evidence callback. - force_observer: Optional raw force evidence callback. - wrench_observer: Optional raw wrench evidence callback. - - 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. + + The Gym cadence is carried by each fresh ``PlanningContext``. Motion policy + remains a provider-free planner declaration and is never rewritten with + environment timing. """ def __init__( @@ -749,15 +732,8 @@ def __init__( step_dt: float, planner_cfg: BasePlannerCfg | None = None, motion_generator_factory: MotionGeneratorFactory | None = None, - endpoint_adapters: ( - Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None - ) = None, translation_threshold: float = 1.0e-4, rotation_threshold: float = 1.0e-3, - contact_observer: BinaryObservationCallback | None = None, - constraint_observer: BinaryObservationCallback | None = None, - force_observer: ScalarObservationCallback | None = None, - wrench_observer: ScalarObservationCallback | None = None, ) -> None: if type(registration) is not SimulationExpertProgramRegistration: raise TypeError( @@ -774,17 +750,6 @@ def __init__( motion_generator_factory ): raise TypeError("motion_generator_factory must be callable or None.") - if endpoint_adapters is not None and not isinstance(endpoint_adapters, Mapping): - raise TypeError("endpoint_adapters must be a mapping or None.") - for name, callback in ( - ("contact_observer", contact_observer), - ("constraint_observer", constraint_observer), - ("force_observer", force_observer), - ("wrench_observer", wrench_observer), - ): - if callback is not None and not callable(callback): - raise TypeError(f"{name} must be callable or None.") - robot_uid = _robot_uid(robot) get_robot = getattr(simulation, "get_robot", None) if not callable(get_robot): @@ -811,9 +776,7 @@ def __init__( self._step_dt = _positive_finite(step_dt, field_name="step_dt") self._planner_cfg = selected_planner_cfg self._motion_generator_factory = motion_generator_factory - self._endpoint_adapters = ( - None if endpoint_adapters is None else dict(endpoint_adapters) - ) + self._endpoint_adapters = dict(registration.endpoint_adapter_map) self._translation_threshold = _non_negative_finite( translation_threshold, field_name="translation_threshold", @@ -822,10 +785,6 @@ def __init__( rotation_threshold, field_name="rotation_threshold", ) - self._contact_observer = contact_observer - self._constraint_observer = constraint_observer - self._force_observer = force_observer - self._wrench_observer = wrench_observer self._owner_token = object() qpos = _full_robot_tensor(robot, "get_qpos", required=True) @@ -851,15 +810,8 @@ def from_environment( registration: SimulationExpertProgramRegistration, planner_cfg: BasePlannerCfg | None = None, motion_generator_factory: MotionGeneratorFactory | None = None, - endpoint_adapters: ( - Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None - ) = None, translation_threshold: float = 1.0e-4, rotation_threshold: float = 1.0e-3, - contact_observer: BinaryObservationCallback | None = None, - constraint_observer: BinaryObservationCallback | None = None, - force_observer: ScalarObservationCallback | None = None, - wrench_observer: ScalarObservationCallback | None = None, ) -> SimulationExpertProgramFactory: """Create a factory from the explicit standard Gym environment surface.""" simulation = getattr(environment, "sim", None) @@ -877,13 +829,8 @@ def from_environment( step_dt=step_dt, planner_cfg=planner_cfg, motion_generator_factory=motion_generator_factory, - endpoint_adapters=endpoint_adapters, translation_threshold=translation_threshold, rotation_threshold=rotation_threshold, - contact_observer=contact_observer, - constraint_observer=constraint_observer, - force_observer=force_observer, - wrench_observer=wrench_observer, ) @property @@ -901,19 +848,21 @@ def step_dt(self) -> float: """Return the authoritative Gym control cadence.""" return self._step_dt + @property + def expert_program_registration(self) -> SimulationExpertProgramRegistration: + """Return the exact standard registration owned by this factory.""" + return self._registration + @property def segment_policy_port(self) -> SimulationSegmentPolicyPort: """Return the shared simulation post-policy and validator port.""" return self._segment_policy_port - @property - def endpoint_adapters( + def registration_owned_segment_policy_ports( self, - ) -> Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None: - """Return an owned copy of installed custom endpoint adapters.""" - return ( - None if self._endpoint_adapters is None else dict(self._endpoint_adapters) - ) + ) -> tuple[SimulationSegmentPolicyPort, SimulationSegmentPolicyPort]: + """Return the exact factory-owned segment policy ports.""" + return self._segment_policy_port, self._segment_policy_port def create_scene_registry(self) -> SceneRegistry: """Build one fresh authoritative registry from explicit bindings.""" @@ -922,35 +871,13 @@ 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 and validate the registered declarative robot profile.""" profile = self._robot_profile_binding.build(self._robot) - aligned_presets = { - preset_id: SkillPolicyPreset( - preset_id=preset.preset_id, - schema_version=preset.schema_version, - motion_policy=replace( - preset.motion_policy, - control_dt=self._step_dt, - ), - tracking_policy=preset.tracking_policy, - recovery_policy=preset.recovery_policy, - runner_cfg=preset.runner_cfg, - effect_monitors=preset.effect_monitors, - action_option_templates=preset.action_option_templates, - ) - for preset_id, preset in profile.presets.items() - } - aligned = replace(profile, presets=aligned_presets) - if any( - preset.motion_policy.control_dt != self._step_dt - for preset in aligned.presets.values() - ): - raise AssertionError("Profile motion policies were not cadence-aligned.") self._registration.validate_robot_profile( - aligned, + profile, step_dt=self._step_dt, ) - return aligned + return profile def create_atomic_action_engine( self, @@ -974,7 +901,7 @@ def create_atomic_action_engine( skill_profile=profile, endpoint_adapters=self._endpoint_adapters, ) - self._registration.catalog.validate_engine(engine) + self._registration.validate_engine(engine) return engine def create_planning_observation_provider( @@ -1038,18 +965,14 @@ def create_effect_evidence_providers( raise ValueError("observation_provider belongs to another factory.") scene_provider = observation_provider.scene_provider command_state_tracker = observation_provider.command_state_tracker - contact_observer = self._contact_observer or command_state_tracker - constraint_observer = self._constraint_observer or command_state_tracker providers: list[EffectEvidenceProvider] = [] if isinstance(self._robot, ControlPartRobotEvidenceSource): providers.append( ControlPartSimulationEvidenceProvider( self._robot, scene_provider=scene_provider, - contact_observer=contact_observer, - constraint_observer=constraint_observer, - force_observer=self._force_observer, - wrench_observer=self._wrench_observer, + contact_observer=command_state_tracker, + constraint_observer=command_state_tracker, ) ) providers.append( @@ -1080,31 +1003,49 @@ def create_accepted_runtime_command_observer( raise ValueError("observation_provider belongs to another factory.") return observation_provider.command_state_tracker - def create_adapter( + def create_parallel_command_safety_validator( self, *, - registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), - effect_monitor_registry: EffectMonitorRegistry | None = None, - runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), - runner_cfg: ExecutionRunnerCfg | None = None, - parallel_safety_validator: ParallelCommandSafetyValidator | None = None, - ) -> ExpertProgramEnvironmentAdapter: + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> ParallelCommandSafetyValidator: + """Create one fresh live gate from the registration-owned factory.""" + self._registration.assert_unchanged() + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if ( + not isinstance(engine, AtomicActionEngine) + or engine.robot is not self._robot + ): + raise ValueError("engine must own the exact factory robot.") + if type(observation_provider) is not SimulationPlanningObservationProvider: + raise TypeError( + "observation_provider must be exactly " + "SimulationPlanningObservationProvider." + ) + if not observation_provider.is_owned_by(self._owner_token): + raise ValueError("observation_provider belongs to another factory.") + if self._registration.parallel_safety_factory is None: + raise RuntimeError("No parallel_safety_factory is registered.") + validator = self._registration.create_parallel_safety_validator( + simulation=self._simulation, + robot=self._robot, + ) + if not isinstance(validator, ParallelCommandSafetyValidator): + raise TypeError( + "ParallelCommandSafetyValidatorFactory.create() must return a " + "ParallelCommandSafetyValidator." + ) + return validator + + def create_adapter(self) -> ExpertProgramEnvironmentAdapter: """Create the exact Gym adapter with shared simulation policy ports.""" self._registration.assert_unchanged() return ExpertProgramEnvironmentAdapter( self, step_dt=self._step_dt, - integration_catalog=self._registration.catalog, - endpoint_adapters=self._endpoint_adapters, - registered_lowerers=registered_lowerers, - relation_grounders=self._registration.relation_grounders, - handover_pose_providers=self._registration.handover_pose_providers, - effect_monitor_registry=effect_monitor_registry, - runtime_transports=runtime_transports, - runner_cfg=runner_cfg, - post_policy_port=self._segment_policy_port, - validator_port=self._segment_policy_port, - parallel_safety_validator=parallel_safety_validator, + registration=self._registration, ) def _create_motion_generator(self) -> MotionGenerator: @@ -1131,17 +1072,8 @@ def create_simulation_expert_program_adapter( registration: SimulationExpertProgramRegistration, planner_cfg: BasePlannerCfg | None = None, motion_generator_factory: MotionGeneratorFactory | None = None, - endpoint_adapters: ( - Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None - ) = None, - runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), translation_threshold: float = 1.0e-4, rotation_threshold: float = 1.0e-3, - contact_observer: BinaryObservationCallback | None = None, - constraint_observer: BinaryObservationCallback | None = None, - force_observer: ScalarObservationCallback | None = None, - wrench_observer: ScalarObservationCallback | None = None, - parallel_safety_validator: ParallelCommandSafetyValidator | None = None, ) -> ExpertProgramEnvironmentAdapter: """Create a complete production adapter from one standard Gym environment. @@ -1149,11 +1081,9 @@ def create_simulation_expert_program_adapter( grounders and embodiment-owned handover pose providers come exclusively from ``registration``, so the statically fingerprinted objects are the exact objects consumed by the runtime compiler. Calls that require an unregistered - provider remain fail-closed during program preflight. Advanced callers can - retain :class:`SimulationExpertProgramFactory` and call ``create_adapter`` - directly to install registered semantic lowerers or custom monitors. Custom - endpoint adapters and their matching Gym runtime transports are accepted - here so a non-joint endpoint remains executable through the one-line path. + provider remain fail-closed during program preflight. Endpoint adapters, + runtime transports, and parallel safety are also registration-owned; the + standard helper exposes no live extension override surface. Args: environment: Standard Gym simulation environment exposing ``sim``, @@ -1161,15 +1091,8 @@ def create_simulation_expert_program_adapter( registration: Exact task registration used during static config loading. planner_cfg: Optional planner configuration owned by the factory. motion_generator_factory: Optional factory for one fresh motion generator. - endpoint_adapters: Optional exact-type custom endpoint adapters. - runtime_transports: Additional runtime-command-to-Gym encoders. translation_threshold: Scene translation revision threshold. rotation_threshold: Scene rotation revision threshold. - contact_observer: Optional raw contact evidence callback. - constraint_observer: Optional raw constraint evidence callback. - force_observer: Optional raw force evidence callback. - wrench_observer: Optional raw wrench evidence callback. - parallel_safety_validator: Optional authoritative parallel-command gate. Returns: Complete production Expert Program environment adapter. @@ -1179,18 +1102,10 @@ def create_simulation_expert_program_adapter( registration=registration, planner_cfg=planner_cfg, motion_generator_factory=motion_generator_factory, - endpoint_adapters=endpoint_adapters, translation_threshold=translation_threshold, rotation_threshold=rotation_threshold, - contact_observer=contact_observer, - constraint_observer=constraint_observer, - force_observer=force_observer, - wrench_observer=wrench_observer, - ) - return factory.create_adapter( - runtime_transports=runtime_transports, - parallel_safety_validator=parallel_safety_validator, ) + return factory.create_adapter() __all__ = [ diff --git a/embodichain/lab/sim/skills/parallel_runtime.py b/embodichain/lab/sim/skills/parallel_runtime.py index 235bb7e18..bbaf43e9c 100644 --- a/embodichain/lab/sim/skills/parallel_runtime.py +++ b/embodichain/lab/sim/skills/parallel_runtime.py @@ -19,6 +19,7 @@ from __future__ import annotations from collections.abc import Hashable, Mapping +from copy import deepcopy from dataclasses import dataclass, field import math from types import MappingProxyType @@ -30,6 +31,7 @@ CommandAcknowledgement, CommandSink, ExecutionClock, + ExecutionRunnerCfg, PlanningContext, RuntimeCommandFrame, RuntimeEndpointTarget, @@ -644,6 +646,7 @@ def __init__( *, timeout_steps: int, failure_policy: str = "fail_fast", + runner_cfg: ExecutionRunnerCfg | None = None, ) -> None: if not isinstance(branches, tuple) or len(branches) < 2: raise ValueError("ParallelSkillRuntime requires at least two branches.") @@ -674,6 +677,8 @@ def __init__( raise ValueError("timeout_steps must be positive.") if failure_policy != "fail_fast": raise ValueError("failure_policy must be exactly 'fail_fast'.") + if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") initial = branches[0].runtime.result for branch in branches[1:]: result = branch.runtime.result @@ -696,6 +701,7 @@ def __init__( self._clock = clock self._timing_policy = timing_policy self._safety_validator = safety_validator + self._runner_cfg = deepcopy(runner_cfg or ExecutionRunnerCfg()) self._timeout_steps = timeout_steps self._initial_state = initial.task_state self._task_state = initial.task_state @@ -731,6 +737,7 @@ def from_template( *, timeout_steps: int, failure_policy: str = "fail_fast", + runner_cfg: ExecutionRunnerCfg | None = None, workflow_id: str = "parallel_static_analysis", branch_paths: Mapping[str, tuple[PathPart, ...]] | None = None, ) -> ParallelSkillRuntime: @@ -750,6 +757,8 @@ def from_template( synchronized outbound command. timeout_steps: Maximum environment steps at the barrier. failure_policy: Row-local barrier failure policy. + runner_cfg: Shared command timeout, safe-stop, completion-hold, and + minimum-cycle policy selected by the runtime preset. workflow_id: Stable prefix for provider-free claim analysis. branch_paths: Optional exact source path for every branch. @@ -790,6 +799,7 @@ def from_template( safety_validator, timeout_steps=timeout_steps, failure_policy=failure_policy, + runner_cfg=runner_cfg, ) @property @@ -824,6 +834,11 @@ def branch_claims(self) -> Mapping[str, ResourceClaim]: {branch.branch_id: branch.claim for branch in self._branches} ) + @property + def runner_cfg(self) -> ExecutionRunnerCfg: + """Return an owned copy of the coordinator transport policy.""" + return deepcopy(self._runner_cfg) + def start( self, *, @@ -914,6 +929,10 @@ def step(self) -> ParallelSkillResult: accepted and not self._pending.any() and self._status is SkillStatus.RUNNING + and ( + self._runner_cfg.hold_on_completion + or bool((self._failure | self._cancelled).any().item()) + ) ): self._terminal_hold_pending = True self._finish_if_complete() @@ -1068,8 +1087,12 @@ def _remaining_transport_wait(self) -> float: def _record_transport_action(self) -> None: """Arm the next physical grid boundary after one accepted action.""" - self._next_transport_at = self._read_clock() + self._timing_policy.step_dt - self._wait_duration = self._timing_policy.step_dt + interval = max( + self._timing_policy.step_dt, + self._runner_cfg.minimum_cycle_time, + ) + self._next_transport_at = self._read_clock() + interval + self._wait_duration = interval def _update_barrier(self) -> None: results = {branch.branch_id: branch.runtime.result for branch in self._branches} @@ -1209,7 +1232,15 @@ def _dispatch_grid_frame(self) -> None: # without producing another action, then send exactly one grid frame. self._dispatch_requested_hold() accepted = self._send_merged_frame(frame, lane_frames) - if accepted and not self._pending.any() and self._status is SkillStatus.RUNNING: + if ( + accepted + and not self._pending.any() + and self._status is SkillStatus.RUNNING + and ( + self._runner_cfg.hold_on_completion + or bool((self._failure | self._cancelled).any().item()) + ) + ): self._terminal_hold_pending = True def _dispatch_deferred_frame(self) -> bool: @@ -1247,7 +1278,10 @@ def _send_merged_frame( raise ParallelSafetyError( "ParallelCommandSafetyValidator.validate() must return None." ) - acknowledgement = self._command_sink.send(frame, timeout=1.0) + acknowledgement = self._command_sink.send( + frame, + timeout=self._runner_cfg.command_timeout, + ) if not isinstance(acknowledgement, CommandAcknowledgement): raise TypeError("CommandSink.send() returned an invalid value.") if not acknowledgement.accepted: @@ -1314,7 +1348,7 @@ def _dispatch_requested_hold( acknowledgement = self._command_sink.hold( tuple(targets.values()), context, - timeout=1.0, + timeout=self._runner_cfg.safe_stop_timeout, ) if not isinstance(acknowledgement, CommandAcknowledgement): raise TypeError("CommandSink.hold() returned an invalid value.") @@ -1366,7 +1400,10 @@ def _forward_safe_stop(self) -> tuple[bool, str | None]: snapshots = tuple(targets.values()) errors: list[str] = [] try: - cancel_ack = self._command_sink.cancel(snapshots, timeout=1.0) + cancel_ack = self._command_sink.cancel( + snapshots, + timeout=self._runner_cfg.safe_stop_timeout, + ) if not isinstance(cancel_ack, CommandAcknowledgement): raise TypeError("CommandSink.cancel() returned an invalid value.") if not cancel_ack.accepted: @@ -1380,7 +1417,7 @@ def _forward_safe_stop(self) -> tuple[bool, str | None]: hold_ack = self._command_sink.hold( snapshots, context, - timeout=1.0, + timeout=self._runner_cfg.safe_stop_timeout, ) if not isinstance(hold_ack, CommandAcknowledgement): raise TypeError("CommandSink.hold() returned an invalid value.") @@ -1464,7 +1501,12 @@ def _finish_if_complete(self) -> None: return self._merge_verified_state() if self._status is SkillStatus.RUNNING and not self._terminal_stop_forwarded: - self._dispatch_requested_hold(required=True, include_last_targets=True) + terminal_failure = bool((self._failure | self._cancelled).any().item()) + require_hold = self._runner_cfg.hold_on_completion or terminal_failure + self._dispatch_requested_hold( + required=require_hold, + include_last_targets=require_hold, + ) self._wait_duration = 0.0 if self._failure.any(): self._status = SkillStatus.FAILED diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 5025af7b2..9238fb9d8 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -1103,6 +1103,7 @@ def snapshot(self) -> SkillPolicyPreset: runner_cfg=self.runner_cfg, effect_monitors=self.effect_monitors, action_option_templates=self.action_option_templates, + required_planner=self.required_planner, ) diff --git a/tests/gym/envs/expert_program/test_bridge.py b/tests/gym/envs/expert_program/test_bridge.py index 9389fd5a9..7859d0063 100644 --- a/tests/gym/envs/expert_program/test_bridge.py +++ b/tests/gym/envs/expert_program/test_bridge.py @@ -43,6 +43,7 @@ ExecutionEvent, ExecutionEventKind, ) +from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg from embodichain.lab.sim.atomic_actions.runtime_commands import ( EndpointCommand, JointPositionPayload, @@ -173,9 +174,9 @@ def snapshot(self) -> _DummyPayload: class _DummyTransportEncoder: """Test registration proving the frame encoder is transport-extensible.""" - @property - def transport_id(self) -> str: - return "test.transport" + transport_id = "test.transport" + target_types = (_DummyTarget,) + payload_types = (_DummyPayload,) def encode( self, @@ -863,6 +864,7 @@ def _bridge( post_policy_port: object | None = None, validator_port: object | None = None, parallel_safety_validator: object | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, ) -> tuple[AtomicDemoBridge, _FakeRuntime, EnvironmentStepClock]: clock = EnvironmentStepClock(STEP_DT) encoder = RuntimeCommandFrameEncoder( @@ -877,11 +879,23 @@ def _bridge( clock, post_policy_port=post_policy_port, validator_port=validator_port, + runner_cfg=runner_cfg, parallel_safety_validator=parallel_safety_validator, ) return bridge, runtime, clock +def test_bridge_snapshots_runner_cfg_before_lazy_parallel_creation() -> None: + """Later advanced-path config mutation cannot change lazy bridge policy.""" + runner_cfg = ExecutionRunnerCfg(command_timeout=0.25) + bridge, _, _ = _bridge(duration=STEP_DT, runner_cfg=runner_cfg) + + runner_cfg.command_timeout = 9.0 + + assert bridge._runner_cfg is not runner_cfg + assert bridge._runner_cfg.command_timeout == pytest.approx(0.25) + + def test_environment_step_clock_advances_only_explicitly() -> None: clock = EnvironmentStepClock(STEP_DT) @@ -947,6 +961,133 @@ def test_frame_encoder_supports_registered_future_transport() -> None: assert action[1, 0].item() == 0.0 +def test_frame_encoder_composes_in_registration_not_frame_order( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Transport registration is the stable controller composition order.""" + calls: list[str] = [] + original_joint_encode = bridge_module.JointPositionGymTransportEncoder.encode + original_dummy_encode = _DummyTransportEncoder.encode + + def record_joint(self: object, *args: object, **kwargs: object) -> object: + calls.append("joint") + return original_joint_encode(self, *args, **kwargs) + + def record_dummy(self: object, *args: object, **kwargs: object) -> object: + calls.append("dummy") + return original_dummy_encode(self, *args, **kwargs) + + monkeypatch.setattr( + bridge_module.JointPositionGymTransportEncoder, + "encode", + record_joint, + ) + monkeypatch.setattr(_DummyTransportEncoder, "encode", record_dummy) + joint = _joint_frame(duration=STEP_DT).commands[0] + dummy = _dummy_frame().commands[0] + frame = RuntimeCommandFrame( + commands=(dummy, joint), + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([7, 3], dtype=torch.long), + hold_duration=torch.full((BATCH_SIZE,), STEP_DT), + ) + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)), + transports=(_DummyTransportEncoder(),), + ) + + encoder.encode(frame) + + assert calls == ["joint", "dummy"] + + +def test_hold_encoder_composes_in_registration_not_target_order( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Safe-hold transport composition uses the same registered ordering.""" + calls: list[str] = [] + original_joint_hold = bridge_module.JointPositionGymTransportEncoder.hold + original_dummy_hold = _DummyTransportEncoder.hold + + def record_joint(self: object, *args: object, **kwargs: object) -> object: + calls.append("joint") + return original_joint_hold(self, *args, **kwargs) + + def record_dummy(self: object, *args: object, **kwargs: object) -> object: + calls.append("dummy") + return original_dummy_hold(self, *args, **kwargs) + + monkeypatch.setattr( + bridge_module.JointPositionGymTransportEncoder, + "hold", + record_joint, + ) + monkeypatch.setattr(_DummyTransportEncoder, "hold", record_dummy) + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)), + transports=(_DummyTransportEncoder(),), + ) + dummy_target = _dummy_frame().targets[0] + joint_target = _joint_frame(duration=STEP_DT).targets[0] + + encoder.encode_hold((dummy_target, joint_target), _context()) + + assert calls == ["joint", "dummy"] + + +def test_frame_encoder_rejects_transport_without_static_type_declarations() -> None: + """Every runtime transport declares its exact pre-sim routing surface.""" + + class MissingDeclarations: + transport_id = "test.missing" + + def encode(self, *args: object, **kwargs: object) -> object: + raise AssertionError + + def hold(self, *args: object, **kwargs: object) -> object: + raise AssertionError + + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)) + ) + + with pytest.raises(TypeError, match="RuntimeTransportActionEncoder"): + encoder.register_transport(MissingDeclarations()) # type: ignore[arg-type] + + +def test_frame_encoder_requires_exact_declared_target_coverage() -> None: + """Transport routing never widens a declaration through subclass checks.""" + + class WrongTargetCoverage(_DummyTransportEncoder): + target_types = (JointPositionTarget,) + + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)), + transports=(WrongTargetCoverage(),), + ) + + with pytest.raises(TypeError, match="does not declare exact target type"): + encoder.encode(_dummy_frame()) + + with pytest.raises(TypeError, match="does not declare exact hold target type"): + encoder.encode_hold(_dummy_frame().targets, _context()) + + +def test_frame_encoder_requires_exact_declared_payload_coverage() -> None: + """Payload declarations are enforced independently of target coverage.""" + + class WrongPayloadCoverage(_DummyTransportEncoder): + payload_types = (JointPositionPayload,) + + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)), + transports=(WrongPayloadCoverage(),), + ) + + with pytest.raises(TypeError, match="does not declare exact payload type"): + encoder.encode(_dummy_frame()) + + def test_buffered_sink_rejects_off_grid_frame_before_buffering() -> None: clock = EnvironmentStepClock(STEP_DT) sink = BufferedGymCommandSink( @@ -1891,6 +2032,7 @@ def from_template( *, timeout_steps: int, failure_policy: str, + runner_cfg: object, workflow_id: str, branch_paths: dict[str, tuple[object, ...]], ) -> _FakeParallelRuntime: @@ -1904,6 +2046,7 @@ def from_template( "safety_validator": supplied_safety_validator, "timeout_steps": timeout_steps, "failure_policy": failure_policy, + "runner_cfg": runner_cfg, "workflow_id": workflow_id, "branch_paths": branch_paths, } @@ -1931,6 +2074,7 @@ def from_template( assert captured["safety_validator"] is safety_validator assert captured["timeout_steps"] == 17 assert captured["failure_policy"] == "fail_fast" + assert captured["runner_cfg"] is not None assert captured["workflow_id"].endswith(":parallel_analysis") assert captured["branch_paths"] == { "branch_0": segment.source_path, diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py index 1d6c07fa7..b4a9badab 100644 --- a/tests/gym/envs/expert_program/test_catalog.py +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -18,7 +18,10 @@ from __future__ import annotations -from dataclasses import dataclass +from concurrent.futures import ThreadPoolExecutor +from dataclasses import dataclass, replace +import json +from threading import Event, Lock from typing import ClassVar import pytest @@ -37,6 +40,7 @@ from embodichain.lab.sim.skills import ( PLACE_ON_AFFORDANCE_CAPABILITY, BoundSemanticCall, + ControlPartEndpoint, HandOver, HandOverPoseProvider, HandOverPoseTargets, @@ -48,8 +52,21 @@ SceneManifest, SceneObjectRef, SemanticRelationTarget, + RegisteredSemanticCall, + SemanticCallDescriptor, + SkillPolicyPreset, builtin_semantic_call_catalog, ) +from embodichain.lab.sim.atomic_actions.tracking import ( + InFlightTrackingPolicy, + TimedTerminalAcceptance, + TrackingMetricCfg, + TrackingPolicy, +) +from embodichain.lab.sim.skills.effects import EffectMonitorRef +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, +) from embodichain_tasks.multi_segments.cube_pick_place import ( CUBE_ROBOT_PROFILE_ID, CUBE_SCENE_REGISTRY_ID, @@ -140,6 +157,14 @@ def __init__(self) -> None: self.height = 0.5 +@dataclass(frozen=True, slots=True) +class _NestedMutableCatalogRelationGrounder(_CatalogRelationGrounder): + """Invalid frozen provider retaining one mutable nested configuration.""" + + capability: ClassVar[str] = "test.catalog_relation.mutable_nested" + offsets: list[float] + + class _PrivateSlotHandOverPoseProvider(HandOverPoseProvider): """Invalid provider whose state is hidden behind a mangled slot name.""" @@ -193,6 +218,95 @@ def resolve( raise AssertionError("Opaque providers must never reach runtime.") +class _AcceptParallelSafety: + """Stateless safety sentinel returned by the registration-owned factory.""" + + def validate(self, *, branch_frames: object, merged_frame: object) -> None: + """Accept the provider-free test command without observing simulation.""" + del branch_frames, merged_frame + + +@dataclass(frozen=True, slots=True) +class _CatalogParallelSafetyFactory: + """Frozen declaration covering the built-in transport exactly.""" + + validator_id: ClassVar[str] = "test.catalog_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {"robot.joint_position"} + ) + margin: float = 0.02 + + def create( + self, + *, + simulation: object, + robot: object, + ) -> ParallelCommandSafetyValidator: + """Return one independent protocol-compatible safety gate.""" + del simulation, robot + return _AcceptParallelSafety() + + +class _SerializedParallelSafetyFactory: + """Instrument concurrent create calls without carrying instance state.""" + + validator_id: ClassVar[str] = "test.serialized_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {"robot.joint_position"} + ) + _state_lock: ClassVar[Lock] = Lock() + _first_entered: ClassVar[Event] = Event() + _second_entered: ClassVar[Event] = Event() + _release_first: ClassVar[Event] = Event() + _calls: ClassVar[int] = 0 + _active: ClassVar[int] = 0 + _max_active: ClassVar[int] = 0 + + @classmethod + def reset(cls) -> None: + """Reset class-owned concurrency instrumentation for one test.""" + cls._first_entered = Event() + cls._second_entered = Event() + cls._release_first = Event() + cls._calls = 0 + cls._active = 0 + cls._max_active = 0 + + def create( + self, + *, + simulation: object, + robot: object, + ) -> ParallelCommandSafetyValidator: + """Block the first call so a second call can attempt registration entry.""" + del simulation, robot + with self._state_lock: + call_index = self._calls + type(self)._calls += 1 + type(self)._active += 1 + type(self)._max_active = max(self._max_active, self._active) + if call_index == 0: + self._first_entered.set() + if not self._release_first.wait(timeout=2.0): + raise TimeoutError("Timed out waiting to release first safety create.") + else: + self._second_entered.set() + with self._state_lock: + type(self)._active -= 1 + return _AcceptParallelSafety() + + +@dataclass(frozen=True, slots=True) +class _CatalogCustomTrackingMetric(TrackingMetricCfg): + """Metric with no built-in exact evaluator registration.""" + + metric_id: ClassVar[str] = "test.catalog_metric" + revision: ClassVar[str] = "1" + channel_id: ClassVar[str] = "joint.position" + + def _program_payload( *, scene_registry: str = CUBE_SCENE_REGISTRY_ID, @@ -299,6 +413,9 @@ def _place_relation_catalog( relation_grounder_keys=grounder_keys, articulation_operation_targets={}, settle_preset_ids=base.settle_preset_ids, + endpoint_adapter_declarations=base.endpoint_adapter_declarations, + runtime_transport_declarations=base.runtime_transport_declarations, + parallel_safety_declaration=base.parallel_safety_declaration, fingerprint="0" * 64, _required_skills={}, ) @@ -326,6 +443,50 @@ def _place_relation_payload() -> dict[str, object]: } +def _parallel_pick_payload() -> dict[str, object]: + """Return one schema-v2 parallel program rooted at an exact config path.""" + return { + "schema_version": 2, + "program_id": "catalog_parallel_pick", + "integration": { + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": CUBE_SCENE_REGISTRY_ID, + "runtime_preset": "safe", + }, + "targets": {}, + "program": { + "kind": "parallel", + "branches": [ + { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + ], + "barrier": { + "kind": "barrier", + "name": "catalog_join", + "timeout_steps": 40, + "failure_policy": "fail_fast", + }, + }, + } + + +def _registration_with_preset( + preset: SkillPolicyPreset, +) -> SimulationExpertProgramRegistration: + """Replace the Cube task's sole preset for registration validation tests.""" + binding = create_cube_robot_profile_binding() + return SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=replace(binding, presets=(preset,)), + ) + + def test_catalog_decodes_compiles_and_links_without_simulation() -> None: """All external references are linked before a simulation is available.""" registration = _registration() @@ -339,6 +500,172 @@ def test_catalog_decodes_compiles_and_links_without_simulation() -> None: assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "pick" +def test_catalog_declares_builtin_endpoint_and_ordered_transport_contracts() -> None: + """The standard provider-free catalog contains its exact built-in wiring.""" + catalog = _registration().catalog + + adapter = catalog.endpoint_adapter_declarations[ControlPartEndpoint] + + assert adapter.adapter_id == "control_part" + assert adapter.runtime_transport_ids == frozenset({"robot.joint_position"}) + assert tuple( + value.transport_id for value in catalog.runtime_transport_declarations + ) == ("robot.joint_position",) + + +def test_parallel_preflight_requires_registered_safety_factory_at_exact_path() -> None: + """Parallel programs cannot defer physical-safety wiring to live startup.""" + registration = _registration() + program = decode_expert_program( + _parallel_pick_payload(), + validation_context=registration.catalog, + ) + + with pytest.raises(ExpertProgramValidationError) as error: + registration.catalog.preflight(program) + + assert error.value.code == "parallel_safety_factory_not_registered" + assert error.value.path == ("program",) + + +def test_parallel_preflight_accepts_exact_registration_owned_safety_factory() -> None: + """A factory declaration covers preflight and creates a fresh live gate.""" + registration = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + parallel_safety_factory=_CatalogParallelSafetyFactory(), + ) + program = decode_expert_program( + _parallel_pick_payload(), + validation_context=registration.catalog, + ) + + compiled = registration.catalog.preflight(program) + validator = registration.create_parallel_safety_validator( + simulation=object(), + robot=object(), + ) + + assert tuple(compiled.iter_segments())[0].parallel_block is not None + assert isinstance(validator, ParallelCommandSafetyValidator) + + +def test_parallel_safety_factory_must_return_a_validator() -> None: + """A malformed registration-owned factory fails before runtime dispatch.""" + + class InvalidParallelSafetyFactory: + validator_id: ClassVar[str] = "test.invalid_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {"robot.joint_position"} + ) + + def create(self, *, simulation: object, robot: object) -> object: + del simulation, robot + return object() + + registration = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + parallel_safety_factory=InvalidParallelSafetyFactory(), + ) + + with pytest.raises(TypeError, match="must return a ParallelCommandSafetyValidator"): + registration.create_parallel_safety_validator( + simulation=object(), + robot=object(), + ) + + +def test_parallel_safety_creation_and_history_are_one_registration_lock_scope() -> None: + """Concurrent assemblies cannot enter one registration factory together.""" + factory_type = _SerializedParallelSafetyFactory + factory_type.reset() + registration = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + parallel_safety_factory=factory_type(), + ) + + def create_validator() -> ParallelCommandSafetyValidator | None: + return registration.create_parallel_safety_validator( + simulation=object(), + robot=object(), + ) + + with ThreadPoolExecutor(max_workers=2) as executor: + first = executor.submit(create_validator) + assert factory_type._first_entered.wait(timeout=1.0) + second = executor.submit(create_validator) + assert not factory_type._second_entered.wait(timeout=0.05) + factory_type._release_first.set() + assert isinstance(first.result(timeout=1.0), ParallelCommandSafetyValidator) + assert isinstance(second.result(timeout=1.0), ParallelCommandSafetyValidator) + + assert factory_type._calls == 2 + assert factory_type._max_active == 1 + + +def test_standard_registration_rejects_registered_semantic_descriptors() -> None: + """Executable lowerer extensions are outside the standard factory contract.""" + catalog = builtin_semantic_call_catalog() + target = catalog.descriptors["pick"].target_descriptor + assert target is not None and target.binding_contract is not None + custom = SemanticCallDescriptor( + call_id="test.catalog_call", + spec_type=RegisteredSemanticCall, + target_descriptor=target, + ) + + with pytest.raises(ValueError, match="Registered semantic call"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + call_catalog=catalog.with_descriptor(custom), + ) + + +def test_standard_registration_rejects_nonbuiltin_effect_monitor() -> None: + """Custom effect-monitor factories cannot be injected after registration.""" + base = create_cube_robot_profile_binding().presets[0] + preset = SkillPolicyPreset( + "safe", + action_option_templates=base.action_option_templates, + motion_policy=base.motion_policy, + tracking_policy=base.tracking_policy, + recovery_policy=base.recovery_policy, + runner_cfg=base.runner_cfg, + effect_monitors={"pick": EffectMonitorRef("test.monitor", "1")}, + ) + + with pytest.raises(ValueError, match="non-built-in effect monitor"): + _registration_with_preset(preset) + + +def test_standard_registration_rejects_tracking_metric_without_builtin_evaluator() -> ( + None +): + """Metric evaluator availability is proven before simulation startup.""" + base = create_cube_robot_profile_binding().presets[0] + preset = SkillPolicyPreset( + "safe", + action_option_templates=base.action_option_templates, + motion_policy=base.motion_policy, + tracking_policy=TrackingPolicy( + in_flight=InFlightTrackingPolicy( + metrics=(_CatalogCustomTrackingMetric(),), + ), + terminal=TimedTerminalAcceptance(), + ), + recovery_policy=base.recovery_policy, + runner_cfg=base.runner_cfg, + effect_monitors=base.effect_monitors, + ) + + with pytest.raises(ValueError, match="no exact built-in evaluator"): + _registration_with_preset(preset) + + @pytest.mark.parametrize("validation_stage", ("decode", "preflight")) def test_catalog_rejects_unknown_named_articulation_target_at_exact_path( validation_stage: str, @@ -464,6 +791,30 @@ def test_fingerprint_is_stable_for_equivalent_declarations() -> None: assert len(left.fingerprint) == 64 +def test_planner_projection_is_json_safe_deterministic_and_owned() -> None: + """Planner discovery is one catalog-derived view with the exact digest.""" + left = _registration() + right = _registration() + + projection = left.catalog.planner_projection() + encoded = json.dumps(projection, allow_nan=False, sort_keys=True) + + assert projection == right.catalog.planner_projection() + assert projection["schema_version"] == ( + "semantic_integration_planner_projection/v1" + ) + assert projection["integration_fingerprint"] == left.fingerprint + assert {call["call_id"] for call in projection["semantic_calls"]} >= { + "pick", + "place", + } + assert "ActionInvocation" not in encoded + assert "qpos" not in encoded + + projection["scene"]["entities"].clear() + assert left.catalog.planner_projection()["scene"]["entities"] + + 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()) @@ -596,6 +947,16 @@ def test_registration_rejects_stateful_non_dataclass_providers( ) +def test_registration_rejects_nested_mutable_relation_grounder_state() -> None: + """Catalog providers reuse the standard recursive immutability boundary.""" + with pytest.raises(TypeError, match="deeply immutable"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=(_NestedMutableCatalogRelationGrounder(offsets=[0.1]),), + ) + + def test_nested_declaration_drift_is_detected_before_live_build() -> None: """Mutable nested config cannot silently change a registered binding.""" registration = _registration() diff --git a/tests/gym/envs/expert_program/test_extensions.py b/tests/gym/envs/expert_program/test_extensions.py new file mode 100644 index 000000000..e51f8ef8d --- /dev/null +++ b/tests/gym/envs/expert_program/test_extensions.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 exact standard-runtime Expert Program extension declarations.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + RobotResourceBinding, + SimulationExpertProgramRegistration, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, +) +from embodichain.lab.gym.envs.expert_program.bridge import ( + JointPositionGymTransportEncoder, +) +from embodichain.lab.gym.envs.expert_program.extensions import ( + RuntimeTransportDeclaration, + build_standard_extension_declarations, + validate_immutable_extension_declaration, +) +from embodichain.lab.sim.atomic_actions import PlanningContext +from embodichain.lab.sim.atomic_actions.bindings import RuntimeEndpointTarget +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + RuntimeCommandPayload, +) +from embodichain.lab.sim.skills import ( + ControlPartEndpoint, + ControlPartEndpointAdapter, + EndpointResolution, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, +) +from embodichain.lab.sim.types import EnvAction + + +@dataclass(frozen=True, slots=True, kw_only=True) +class _MobileEndpoint(ResourceEndpoint): + """Custom endpoint declaration used by the catalog-only tests.""" + + controller: str = "base" + + +@dataclass(frozen=True, slots=True, kw_only=True) +class _ToolEndpoint(ResourceEndpoint): + """Second exact endpoint type used to prove transport ordering.""" + + controller: str = "tool" + + +@dataclass(frozen=True, slots=True) +class _MobileTarget(RuntimeEndpointTarget): + """Immutable custom runtime destination.""" + + TRANSPORT_ID: ClassVar[str] = "test.mobile" + controller: str + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + @property + def target_id(self) -> str: + return self.controller + + +@dataclass(frozen=True, slots=True) +class _ToolTarget(RuntimeEndpointTarget): + """Immutable destination owned by the second transport.""" + + TRANSPORT_ID: ClassVar[str] = "test.tool" + controller: str + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + @property + def target_id(self) -> str: + return self.controller + + +@dataclass(frozen=True, slots=True, eq=False) +class _MobilePayload(RuntimeCommandPayload): + """Minimal typed payload declaration for the mobile transport.""" + + TRANSPORT_ID: ClassVar[str] = _MobileTarget.TRANSPORT_ID + values: torch.Tensor + + @property + def batch_size(self) -> int: + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + return self.values.device + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + def snapshot(self) -> _MobilePayload: + return _MobilePayload(self.values.clone()) + + +@dataclass(frozen=True, slots=True, eq=False) +class _ToolPayload(RuntimeCommandPayload): + """Minimal typed payload declaration for the tool transport.""" + + TRANSPORT_ID: ClassVar[str] = _ToolTarget.TRANSPORT_ID + values: torch.Tensor + + @property + def batch_size(self) -> int: + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + return self.values.device + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + def snapshot(self) -> _ToolPayload: + return _ToolPayload(self.values.clone()) + + +class _MobileAdapter(ResourceEndpointAdapter): + """Stateless custom adapter with only standard-factory provider routes.""" + + adapter_id: ClassVar[str] = "test.mobile" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _MobileEndpoint + runtime_transport_ids: ClassVar[frozenset[str]] = frozenset( + {_MobileTarget.TRANSPORT_ID} + ) + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _MobileTarget, + ) + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + + def resolve( + self, endpoint: ResourceEndpoint, *, engine: object + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution( + runtime_target=_MobileTarget("base"), + claim_tokens=frozenset({"test.mobile:base"}), + ) + + +class _ToolAdapter(ResourceEndpointAdapter): + """Second stateless adapter used by ordering tests.""" + + adapter_id: ClassVar[str] = "test.tool" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _ToolEndpoint + runtime_transport_ids: ClassVar[frozenset[str]] = frozenset( + {_ToolTarget.TRANSPORT_ID} + ) + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _ToolTarget, + ) + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + + def resolve( + self, endpoint: ResourceEndpoint, *, engine: object + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution( + runtime_target=_ToolTarget("tool"), + claim_tokens=frozenset({"test.tool:tool"}), + ) + + +class _MobileTransport: + """Stateless action composition transport for the mobile target.""" + + transport_id: ClassVar[str] = _MobileTarget.TRANSPORT_ID + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = (_MobileTarget,) + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] = (_MobilePayload,) + + def encode( + self, + command: EndpointCommand, + *, + base_action: EnvAction, + active_mask: torch.Tensor, + ) -> EnvAction: + del command, active_mask + return base_action + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: EnvAction, + context: PlanningContext, + ) -> EnvAction: + del targets, context + return base_action + + +class _ToolTransport(_MobileTransport): + """Second stateless action composition transport.""" + + transport_id: ClassVar[str] = _ToolTarget.TRANSPORT_ID + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = (_ToolTarget,) + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] = (_ToolPayload,) + + +class _SafetyValidator: + """Protocol-compatible no-op validator used only for factory typing.""" + + def validate(self, *, branch_frames: object, merged_frame: object) -> None: + del branch_frames, merged_frame + + +class _MobileSafetyFactory: + """Stateless exact safety-factory declaration.""" + + validator_id: ClassVar[str] = "test.mobile_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {_MobileTarget.TRANSPORT_ID} + ) + + def create(self, *, simulation: object, robot: object) -> _SafetyValidator: + del simulation, robot + return _SafetyValidator() + + +def _custom_profile(*, include_tool: bool = False) -> RobotSkillProfile: + """Return a pure provider-free profile with exact custom endpoint types.""" + endpoints: dict[str, ResourceEndpoint] = { + "motion": _MobileEndpoint(capabilities=frozenset()) + } + if include_tool: + endpoints["tool"] = _ToolEndpoint(capabilities=frozenset()) + resource = RobotResource(resource_id="custom", endpoints=endpoints) + return RobotSkillProfile(profile_id="custom", resources={"custom": resource}) + + +def test_custom_endpoint_transport_and_safety_declarations_are_exact() -> None: + """A complete custom extension set produces an immutable provider-free catalog.""" + declarations = build_standard_extension_declarations( + profile=_custom_profile(), + endpoint_adapters=(_MobileAdapter(),), + runtime_transports=(_MobileTransport(),), + parallel_safety_factory=_MobileSafetyFactory(), + ) + + assert declarations.endpoint_adapters[_MobileEndpoint].adapter_id == "test.mobile" + assert tuple(value.transport_id for value in declarations.runtime_transports) == ( + "test.mobile", + ) + assert declarations.parallel_safety is not None + assert declarations.parallel_safety.supported_transport_ids == frozenset( + {"test.mobile"} + ) + + +def test_parallel_safety_transport_coverage_must_match_registration() -> None: + """A safety factory must cover the exact installed transport set.""" + + class MismatchedSafetyFactory(_MobileSafetyFactory): + supported_transport_ids: ClassVar[frozenset[str]] = frozenset({"test.other"}) + + with pytest.raises(ValueError, match="must support exactly"): + build_standard_extension_declarations( + profile=_custom_profile(), + endpoint_adapters=(_MobileAdapter(),), + runtime_transports=(_MobileTransport(),), + parallel_safety_factory=MismatchedSafetyFactory(), + ) + + +@pytest.mark.parametrize("declaration_kind", ("target", "payload")) +def test_runtime_transport_types_require_direct_transport_id( + declaration_kind: str, +) -> None: + """Every registered runtime value type owns its transport ID directly.""" + + class MissingTarget(RuntimeEndpointTarget): + @property + def transport_id(self) -> str: + return _MobileTarget.TRANSPORT_ID + + @property + def target_id(self) -> str: + return "missing" + + class MissingPayload(RuntimeCommandPayload): + @property + def batch_size(self) -> int: + return 1 + + @property + def device(self) -> torch.device: + return torch.device("cpu") + + @property + def transport_id(self) -> str: + return _MobileTarget.TRANSPORT_ID + + def snapshot(self) -> MissingPayload: + return MissingPayload() + + target_types = ( + (MissingTarget,) if declaration_kind == "target" else (_MobileTarget,) + ) + payload_types = ( + (MissingPayload,) if declaration_kind == "payload" else (_MobilePayload,) + ) + + with pytest.raises(TypeError, match="must declare an exact ClassVar TRANSPORT_ID"): + RuntimeTransportDeclaration( + transport_type=_MobileTransport, + transport_id=_MobileTarget.TRANSPORT_ID, + target_types=target_types, + payload_types=payload_types, + ) + + +@pytest.mark.parametrize("declaration_kind", ("target", "payload")) +def test_runtime_transport_types_cannot_inherit_transport_id( + declaration_kind: str, +) -> None: + """A subtype cannot silently inherit another runtime type's transport owner.""" + + class InheritedTarget(_MobileTarget): + pass + + class InheritedPayload(_MobilePayload): + pass + + target_types = ( + (InheritedTarget,) if declaration_kind == "target" else (_MobileTarget,) + ) + payload_types = ( + (InheritedPayload,) if declaration_kind == "payload" else (_MobilePayload,) + ) + + with pytest.raises(TypeError, match="inherited or instance-only"): + RuntimeTransportDeclaration( + transport_type=_MobileTransport, + transport_id=_MobileTarget.TRANSPORT_ID, + target_types=target_types, + payload_types=payload_types, + ) + + +@pytest.mark.parametrize("declaration_kind", ("target", "payload")) +def test_runtime_transport_type_transport_id_must_match_encoder( + declaration_kind: str, +) -> None: + """Static runtime value ownership must match the encoder transport exactly.""" + + class MismatchedTarget(_MobileTarget): + TRANSPORT_ID: ClassVar[str] = "test.mismatched" + + class MismatchedPayload(_MobilePayload): + TRANSPORT_ID: ClassVar[str] = "test.mismatched" + + target_types = ( + (MismatchedTarget,) if declaration_kind == "target" else (_MobileTarget,) + ) + payload_types = ( + (MismatchedPayload,) if declaration_kind == "payload" else (_MobilePayload,) + ) + + with pytest.raises(ValueError, match="not 'test.mobile'"): + RuntimeTransportDeclaration( + transport_type=_MobileTransport, + transport_id=_MobileTarget.TRANSPORT_ID, + target_types=target_types, + payload_types=payload_types, + ) + + +@pytest.mark.parametrize( + ("adapters", "transports", "message"), + ( + ((), (_MobileTransport(),), "missing"), + ((_MobileAdapter(),), (), "missing"), + ( + (_MobileAdapter(), _ToolAdapter()), + (_MobileTransport(), _ToolTransport()), + "unused", + ), + ), +) +def test_extension_coverage_rejects_missing_and_unused_declarations( + adapters: tuple[ResourceEndpointAdapter, ...], + transports: tuple[object, ...], + message: str, +) -> None: + """Every custom adapter and transport must be necessary and complete.""" + with pytest.raises(ValueError, match=message): + build_standard_extension_declarations( + profile=_custom_profile(), + endpoint_adapters=adapters, + runtime_transports=transports, # type: ignore[arg-type] + parallel_safety_factory=None, + ) + + +def test_builtin_adapter_and_transport_cannot_be_overridden() -> None: + """Standard built-ins retain exact ownership of their endpoint and transport.""" + profile = RobotSkillProfile( + profile_id="joint", + resources={ + "arm": RobotResource( + resource_id="arm", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=frozenset(), + ) + }, + ) + }, + ) + + with pytest.raises(ValueError, match="override the built-in ControlPartEndpoint"): + build_standard_extension_declarations( + profile=profile, + endpoint_adapters=(ControlPartEndpointAdapter(),), + runtime_transports=(), + parallel_safety_factory=None, + ) + with pytest.raises(ValueError, match="override the built-in joint-position"): + build_standard_extension_declarations( + profile=profile, + endpoint_adapters=(), + runtime_transports=(JointPositionGymTransportEncoder(),), + parallel_safety_factory=None, + ) + + +def test_nonbuiltin_provider_route_is_rejected_by_standard_registration() -> None: + """Provider declarations cannot name a live registry absent from the factory.""" + + class UnsupportedProviderAdapter(_MobileAdapter): + adapter_id: ClassVar[str] = "test.unsupported_provider" + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + {("test.feedback", "1")} + ) + + with pytest.raises(ValueError, match="does not install"): + build_standard_extension_declarations( + profile=_custom_profile(), + endpoint_adapters=(UnsupportedProviderAdapter(),), + runtime_transports=(_MobileTransport(),), + parallel_safety_factory=None, + ) + + +@pytest.mark.parametrize( + "mutable_leaf", + ( + [0.1], + {"gain": 0.1}, + {0.1}, + bytearray(b"gain"), + torch.tensor((0.1,)), + ), + ids=("list", "dict", "set", "bytearray", "tensor"), +) +def test_extension_declarations_reject_nested_mutable_state( + mutable_leaf: object, +) -> None: + """Frozen wrappers cannot retain mutable state used by a live extension.""" + + @dataclass(frozen=True, slots=True) + class NestedDeclaration: + config: tuple[object, ...] + + with pytest.raises(TypeError, match="deeply immutable"): + validate_immutable_extension_declaration( + NestedDeclaration((mutable_leaf,)), + field_name="runtime_transports", + ) + + +def test_runtime_transport_tuple_order_changes_registration_fingerprint() -> None: + """Transport composition order is semantic registration data.""" + profile_binding = SimulationRobotSkillProfileBinding( + profile_id="custom", + resources=( + RobotResourceBinding( + resource_id="custom", + endpoints={ + "motion": _MobileEndpoint(capabilities=frozenset()), + "tool": _ToolEndpoint(capabilities=frozenset()), + }, + ), + ), + ) + common = { + "scene_binding": SimulationSceneBinding(registry_id="custom_scene"), + "robot_profile_binding": profile_binding, + "endpoint_adapters": (_MobileAdapter(), _ToolAdapter()), + } + forward = SimulationExpertProgramRegistration( + **common, + runtime_transports=(_MobileTransport(), _ToolTransport()), + ) + reversed_registration = SimulationExpertProgramRegistration( + **common, + runtime_transports=(_ToolTransport(), _MobileTransport()), + ) + + assert forward.fingerprint != reversed_registration.fingerprint + + +__all__: list[str] = [] diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 54188235b..2f304c21b 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -20,11 +20,11 @@ import ast from collections.abc import Mapping, Sequence -from dataclasses import dataclass, fields, is_dataclass +from dataclasses import dataclass, fields, is_dataclass, replace import inspect import json import textwrap -from types import MethodType, SimpleNamespace +from types import MappingProxyType, MethodType, SimpleNamespace from typing import Any, ClassVar from unittest.mock import MagicMock @@ -46,6 +46,7 @@ ExpertProgramRuntimeAssembly, HandOverCfg, InvokeCfg, + IntegrationFingerprintMismatch, RobotResourceBinding, SharedTickSceneProvider, SimulationExpertProgramRegistration, @@ -76,9 +77,17 @@ PlaceOptions, StateDelta, TaskState, + TimedTrajectory, TrackingPolicy, ) from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg +from embodichain.lab.sim.atomic_actions.tracking import ( + JOINT_POSITION_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + TrackingFeedbackSourceRef, + TrackingProjectorRef, +) from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.bindings import RuntimeEndpointTarget from embodichain.lab.sim.atomic_actions.control import ControlPartCommandProfile @@ -86,6 +95,7 @@ EndpointCommand, JointPositionPayload, RuntimeCommandFrame, + RuntimeCommandPayload, ) from embodichain.lab.sim.planners import MotionGenerator from embodichain.lab.sim.skills import ( @@ -119,6 +129,7 @@ EffectEvidenceSourceRef, HeldObjectRelation, HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, ) from embodichain.lab.sim.skills.evidence import ( BinaryEffectEvidenceQuery, @@ -719,12 +730,13 @@ class _MobileEndpoint(ResourceEndpoint): class _MobileTarget(RuntimeEndpointTarget): """Runtime destination for the test mobile controller.""" + TRANSPORT_ID: ClassVar[str] = "test.mobile_velocity" controller_id: str @property def transport_id(self) -> str: """Return the matching test Gym transport ID.""" - return "test.mobile_velocity" + return self.TRANSPORT_ID @property def target_id(self) -> str: @@ -732,11 +744,52 @@ def target_id(self) -> str: return self.controller_id +@dataclass(frozen=True, slots=True) +class _UndeclaredMobileTarget(RuntimeEndpointTarget): + """Live target intentionally absent from the adapter declaration.""" + + TRANSPORT_ID: ClassVar[str] = _MobileTarget.TRANSPORT_ID + controller_id: str + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + @property + def target_id(self) -> str: + return self.controller_id + + +@dataclass(frozen=True, slots=True) +class _LyingTransportMobileTarget(RuntimeEndpointTarget): + """Declare one transport statically but expose another on the live value.""" + + TRANSPORT_ID: ClassVar[str] = _MobileTarget.TRANSPORT_ID + controller_id: str + + @property + def transport_id(self) -> str: + return "test.unregistered_live_transport" + + @property + def target_id(self) -> str: + return self.controller_id + + class _MobileEndpointAdapter(ResourceEndpointAdapter): """Resolve a mobile endpoint without consulting robot control parts.""" adapter_id: ClassVar[str] = "test.mobile_velocity" endpoint_type: ClassVar[type[ResourceEndpoint]] = _MobileEndpoint + runtime_transport_ids: ClassVar[frozenset[str]] = frozenset( + {_MobileTarget.TRANSPORT_ID} + ) + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _MobileTarget, + ) + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() def resolve( self, @@ -754,13 +807,162 @@ def resolve( ) -class _MobileTransportEncoder: - """Minimal Gym encoder registered for the custom mobile target.""" +class _LyingMobileEndpointAdapter(_MobileEndpointAdapter): + """Declare one target type but resolve a different live target type.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_velocity" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_LyingMobileEndpointAdapter requires _MobileEndpoint.") + return EndpointResolution( + runtime_target=_UndeclaredMobileTarget(endpoint.controller_id), + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +class _LyingTransportMobileEndpointAdapter(_MobileEndpointAdapter): + """Resolve a target whose live transport contradicts its static owner.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_transport" + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _LyingTransportMobileTarget, + ) + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_LyingTransportMobileEndpointAdapter requires mobile.") + return EndpointResolution( + runtime_target=_LyingTransportMobileTarget(endpoint.controller_id), + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +class _LyingAdapterIdMobileEndpointAdapter(_MobileEndpointAdapter): + """Expose a different live adapter ID than the class declaration.""" + + adapter_id: ClassVar[str] = "test.declared_mobile_adapter" + + def __getattribute__(self, name: str) -> Any: + if name == "adapter_id": + return "test.live_mobile_adapter" + return super().__getattribute__(name) + + +class _LyingFeedbackMobileEndpointAdapter(_MobileEndpointAdapter): + """Emit fingerprinted tracking routes absent from the declaration.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_feedback" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_LyingFeedbackMobileEndpointAdapter requires mobile.") + target = _MobileTarget(endpoint.controller_id) + tracking = EndpointTrackingChannelBinding( + JOINT_POSITION_CHANNEL, + TrackingFeedbackSourceRef( + "planning_context.robot", + "1", + EndpointTrackingFeedbackAddress(target, JOINT_POSITION_CHANNEL), + ), + TrackingProjectorRef("joint_position_payload", "1"), + ) + return EndpointResolution( + runtime_target=target, + tracking_channels={JOINT_POSITION_CHANNEL: tracking}, + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +class _LyingProjectorMobileEndpointAdapter(_LyingFeedbackMobileEndpointAdapter): + """Declare only the live feedback route while hiding its projector route.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_projector" + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + {("planning_context.robot", "1")} + ) + + +class _LyingEvidenceMobileEndpointAdapter(_MobileEndpointAdapter): + """Emit effect evidence absent from the adapter declaration.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_evidence" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_LyingEvidenceMobileEndpointAdapter requires mobile.") + return EndpointResolution( + runtime_target=_MobileTarget(endpoint.controller_id), + effect_sources={ + JOINT_STATE_EFFECT_CHANNEL: EffectEvidenceSourceRef( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress( + endpoint.controller_id, + JOINT_STATE_EFFECT_CHANNEL, + ), + ) + }, + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _MobilePayload(RuntimeCommandPayload): + """Minimal payload declaration for the custom transport contract.""" + + TRANSPORT_ID: ClassVar[str] = _MobileTarget.TRANSPORT_ID + values: torch.Tensor + + def __post_init__(self) -> None: + object.__setattr__(self, "values", self.values.clone()) + + @property + def batch_size(self) -> int: + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + return self.values.device @property def transport_id(self) -> str: - """Return the custom mobile transport ID.""" - return "test.mobile_velocity" + return self.TRANSPORT_ID + + def snapshot(self) -> _MobilePayload: + return _MobilePayload(self.values) + + +class _MobileTransportEncoder: + """Minimal Gym encoder registered for the custom mobile target.""" + + transport_id: ClassVar[str] = _MobileTarget.TRANSPORT_ID + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = (_MobileTarget,) + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] = (_MobilePayload,) def encode( self, @@ -769,9 +971,12 @@ def encode( base_action: Any, active_mask: torch.Tensor, ) -> Any: - """Preserve the base action in this assembly-only test transport.""" - del command, active_mask - return base_action.clone() + """Write the custom payload into one test controller channel.""" + if type(command.payload) is not _MobilePayload: + raise TypeError("_MobileTransportEncoder requires _MobilePayload.") + action = base_action.clone() + action[active_mask, 0] = command.payload.values[active_mask] + return action def hold( self, @@ -785,6 +990,14 @@ def hold( return base_action.clone() +class _LyingTargetMobileTransportEncoder(_MobileTransportEncoder): + """Declare the statically owned type whose live transport property lies.""" + + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _LyingTransportMobileTarget, + ) + + class _MobileRobot: """Full-state robot fixture with no control-parts or joint-ID surface.""" @@ -828,8 +1041,62 @@ def get_rigid_object(self, uid: str) -> _RigidObject | None: return self.rigid_objects.get(uid) +class _RegisteredParallelSafety: + """Fresh test gate produced only by its registration-owned factory.""" + + def validate( + self, + *, + branch_frames: Mapping[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + del branch_frames, merged_frame + + +class _RegisteredParallelSafetyFactory: + """Stateless declarative factory for a live joint-transport gate.""" + + validator_id: ClassVar[str] = "test.registered_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {JointPositionTarget.TRANSPORT_ID} + ) + + def create(self, *, simulation: object, robot: object) -> _RegisteredParallelSafety: + assert getattr(simulation, "get_robot")(getattr(robot, "uid")) is robot + return _RegisteredParallelSafety() + + +class _ReusedParallelSafetyFactory(_RegisteredParallelSafetyFactory): + """Invalid stateless factory that reuses one live validator singleton.""" + + validator_id: ClassVar[str] = "test.reused_parallel_safety" + _validator: ClassVar[_RegisteredParallelSafety] = _RegisteredParallelSafety() + + def create(self, *, simulation: object, robot: object) -> _RegisteredParallelSafety: + del simulation, robot + return self._validator + + +class _AlternatingReusedParallelSafetyFactory(_RegisteredParallelSafetyFactory): + """Invalid factory that hides A/B/A reuse behind alternating instances.""" + + validator_id: ClassVar[str] = "test.alternating_parallel_safety" + _validators: ClassVar[tuple[_RegisteredParallelSafety, ...]] = ( + _RegisteredParallelSafety(), + _RegisteredParallelSafety(), + ) + _next_index: ClassVar[int] = 0 + + def create(self, *, simulation: object, robot: object) -> _RegisteredParallelSafety: + del simulation, robot + validator = self._validators[self._next_index % len(self._validators)] + type(self)._next_index += 1 + return validator + + def _profile_binding() -> SimulationRobotSkillProfileBinding: - """Build one motion-only profile with an intentionally wrong cadence.""" + """Build one motion-only profile with typed tracking policy.""" return SimulationRobotSkillProfileBinding( profile_id="robot_profile", resources=( @@ -851,7 +1118,7 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: "pick": PickUpOptions(), "place": PlaceOptions(), }, - motion_policy=MotionPolicy(control_dt=0.01), + motion_policy=MotionPolicy(sample_count=17), tracking_policy=TrackingPolicy.joint_position( in_flight_max_abs_error=0.037, terminal_max_abs_error=0.019, @@ -868,6 +1135,26 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: ) +def _mobile_profile_binding() -> SimulationRobotSkillProfileBinding: + """Build one pure custom-endpoint profile without a joint transport.""" + return SimulationRobotSkillProfileBinding( + profile_id="mobile_profile", + resources=( + RobotResourceBinding( + resource_id="mobile_base", + endpoints={ + "motion": _MobileEndpoint( + controller_id="base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ), + ), + presets=(SkillPolicyPreset("runtime", action_option_templates={}),), + default_preset="runtime", + ) + + def _handover_profile_binding() -> SimulationRobotSkillProfileBinding: """Declare two disjoint manipulators and one selected pose provider ID.""" motion_capabilities = frozenset( @@ -1004,6 +1291,31 @@ def _factory() -> tuple[SimulationExpertProgramFactory, _Robot]: ) +def _mobile_factory() -> tuple[ + SimulationExpertProgramFactory, + SimulationExpertProgramRegistration, +]: + """Create one pure-custom standard factory and its exact registration.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(_MobileEndpointAdapter(),), + runtime_transports=(_MobileTransportEncoder(),), + ) + return ( + SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + ), + registration, + ) + + def _evidence_profile_binding() -> SimulationRobotSkillProfileBinding: """Declare one manipulation resource with exact open/grasp semantics.""" motion_capabilities = frozenset( @@ -1159,12 +1471,7 @@ def _evidence_adapter_runtime() -> tuple[ step_dt=_STEP_DT, motion_generator_factory=lambda: _motion_generator(robot), ) - adapter = factory.create_adapter( - runner_cfg=ExecutionRunnerCfg( - minimum_cycle_time=0.0, - hold_on_completion=False, - ) - ) + adapter = factory.create_adapter() assembly = adapter.assemble_runtime(_evidence_integration()) pick_action = assembly.engine.actions["pick_up"] place_action = assembly.engine.actions["place"] @@ -1564,13 +1871,13 @@ def _assert_invocation_equivalent( ) -def test_simulation_factory_aligns_every_motion_policy_to_gym_step() -> None: - """Cadence alignment preserves the exact registered tracking contract.""" +def test_simulation_factory_preserves_policy_and_tracking_contract() -> None: + """Gym cadence does not mutate the registered planner or tracking policy.""" factory, _ = _factory() profile = factory.create_robot_skill_profile() - assert profile.presets["safe"].motion_policy.control_dt == pytest.approx(_STEP_DT) + assert profile.presets["safe"].motion_policy.sample_count == 17 assert profile.presets["safe"].tracking_policy == TrackingPolicy.joint_position( in_flight_max_abs_error=0.037, terminal_max_abs_error=0.019, @@ -1733,6 +2040,244 @@ def test_simulation_factory_returns_exact_environment_adapter() -> None: assert factory.segment_policy_port is not None +@pytest.mark.parametrize( + "override", + ( + {"call_catalog": object()}, + {"endpoint_adapters": {}}, + {"registered_lowerers": (object(),)}, + {"relation_grounders": (object(),)}, + {"handover_pose_providers": (object(),)}, + {"effect_monitor_registry": object()}, + {"runtime_transports": (object(),)}, + {"runner_cfg": ExecutionRunnerCfg()}, + {"post_policy_port": object()}, + {"validator_port": object()}, + {"parallel_safety_validator": object()}, + ), +) +def test_standard_registration_rejects_runtime_side_channel_overrides( + override: dict[str, object], +) -> None: + """The exact registration is the standard path's only extension owner.""" + factory, _ = _factory() + + with pytest.raises(ValueError, match="external overrides are forbidden"): + ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + registration=factory.expert_program_registration, + **override, + ) + + +def test_registration_owning_factory_rejects_catalog_only_adapter() -> None: + """A standard factory cannot be rewrapped through the advanced catalog seam.""" + factory, _ = _factory() + + with pytest.raises(ValueError, match="catalog-only"): + ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + integration_catalog=factory.expert_program_registration.catalog, + ) + + +def test_standard_registration_rejects_integration_catalog_override() -> None: + """Even the owner's catalog cannot be resupplied beside exact registration.""" + factory, _ = _factory() + registration = factory.expert_program_registration + + with pytest.raises(ValueError, match="cannot override"): + ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + registration=registration, + integration_catalog=registration.catalog, + ) + + +def test_registration_owning_factory_rejects_equivalent_registration_object() -> None: + """Equal IDs and fingerprint cannot substitute for the factory-owned object.""" + factory, _ = _factory() + owned = factory.expert_program_registration + equivalent = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + ) + assert equivalent is not owned + assert equivalent.fingerprint == owned.fingerprint + + with pytest.raises(ValueError, match="exact object owned by the factory"): + ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + registration=equivalent, + ) + + +def test_adapter_rejects_factory_registration_ownership_drift() -> None: + """A factory cannot replace its registration after adapter construction.""" + factory, _ = _factory() + adapter = factory.create_adapter() + equivalent = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + ) + factory._registration = equivalent + + with pytest.raises(IntegrationFingerprintMismatch, match="ownership changed"): + adapter.assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + ) + + +def test_adapter_rejects_engine_bound_to_equivalent_profile_object() -> None: + """The engine must bind the exact profile object validated by the adapter.""" + factory, _ = _factory() + original_create_engine = factory.create_atomic_action_engine + + def create_with_different_profile( + owner: SimulationExpertProgramFactory, + profile: Any, + ) -> Any: + replacement = owner.create_robot_skill_profile() + assert replacement is not profile + return original_create_engine(replacement) + + factory.create_atomic_action_engine = MethodType( + create_with_different_profile, + factory, + ) + + with pytest.raises( + IntegrationFingerprintMismatch, + match="different robot profile object", + ): + factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + ) + + +def test_standard_factory_uses_preset_runner_and_fresh_registered_safety() -> None: + """Live assembly consumes preset policy and creates no shared safety gate.""" + robot = _Robot() + simulation = _Simulation(robot) + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + parallel_safety_factory=_RegisteredParallelSafetyFactory(), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ) + adapter = factory.create_adapter() + integration = ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + + first = adapter.assemble_runtime(integration) + second = adapter.assemble_runtime(integration) + + assert first.runner_cfg.command_timeout == pytest.approx(0.37) + assert first.runner_cfg.safe_stop_timeout == pytest.approx(0.61) + assert first.runner_cfg.minimum_cycle_time == pytest.approx(0.04) + assert first.runner_cfg.hold_on_completion is False + assert type(first.parallel_safety_validator) is _RegisteredParallelSafety + assert type(second.parallel_safety_validator) is _RegisteredParallelSafety + assert first.parallel_safety_validator is not second.parallel_safety_validator + + +def test_standard_factory_rejects_reused_live_safety_validator() -> None: + """A declarative factory cannot leak one validator across runtime assemblies.""" + robot = _Robot() + simulation = _Simulation(robot) + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + parallel_safety_factory=_ReusedParallelSafetyFactory(), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ) + adapter = factory.create_adapter() + integration = ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + adapter.assemble_runtime(integration) + + with pytest.raises(ValueError, match="fresh validator"): + adapter.assemble_runtime(integration) + + +def test_registration_rejects_a_b_a_safety_reuse_across_factories() -> None: + """Freshness history belongs to the registration rather than one factory.""" + robot = _Robot() + simulation = _Simulation(robot) + _AlternatingReusedParallelSafetyFactory._next_index = 0 + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + parallel_safety_factory=_AlternatingReusedParallelSafetyFactory(), + ) + factories = tuple( + SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ) + for _ in range(3) + ) + integration = ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + + factories[0].create_adapter().assemble_runtime(integration) + factories[1].create_adapter().assemble_runtime(integration) + + with pytest.raises(ValueError, match="fresh validator"): + factories[2].create_adapter().assemble_runtime(integration) + + +def test_standard_simulation_helper_has_no_live_extension_override_parameters() -> None: + """Task code can select only its immutable registration on the standard path.""" + parameters = inspect.signature(create_simulation_expert_program_adapter).parameters + + assert { + "endpoint_adapters", + "runtime_transports", + "contact_observer", + "constraint_observer", + "force_observer", + "wrench_observer", + "parallel_safety_validator", + }.isdisjoint(parameters) + + def test_simulation_helper_consumes_registered_semantic_grounding_extensions() -> None: """Both registration-owned grounding seams reach the compiler unchanged.""" robot = _Robot() @@ -1805,21 +2350,11 @@ def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joint """The one-line factory path supports a custom non-joint controller.""" robot = _MobileRobot() simulation = _Simulation(robot) # type: ignore[arg-type] - profile_binding = SimulationRobotSkillProfileBinding( - profile_id="mobile_profile", - resources=( - RobotResourceBinding( - resource_id="mobile_base", - endpoints={ - "motion": _MobileEndpoint( - controller_id="base_velocity", - capabilities=frozenset({"motion.base.velocity"}), - ) - }, - ), - ), - presets=(SkillPolicyPreset("runtime", action_option_templates={}),), - default_preset="runtime", + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(_MobileEndpointAdapter(),), + runtime_transports=(_MobileTransportEncoder(),), ) environment = SimpleNamespace( sim=simulation, @@ -1829,13 +2364,8 @@ def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joint adapter = create_simulation_expert_program_adapter( environment, # type: ignore[arg-type] - registration=SimulationExpertProgramRegistration( - scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), - robot_profile_binding=profile_binding, - ), + registration=registration, motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] - endpoint_adapters={_MobileEndpoint: _MobileEndpointAdapter()}, - runtime_transports=(_MobileTransportEncoder(),), ) assembly = adapter.assemble_runtime( ExpertProgramIntegrationCfg( @@ -1847,11 +2377,250 @@ def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joint endpoint = assembly.robot_profile.resources["mobile_base"].endpoints["motion"] assert isinstance(endpoint, _MobileEndpoint) - assert "test.mobile_velocity" in assembly.command_encoder.transport_ids + assert assembly.command_encoder.transport_ids == (_MobileTarget.TRANSPORT_ID,) + assert assembly.command_encoder.is_frozen + with pytest.raises(RuntimeError, match="registration is frozen"): + assembly.command_encoder.register_transport( + _MobileTransportEncoder(), + replace=True, + ) assert assembly.engine.skill_profile is not None resolved = assembly.engine.skill_profile.resources["mobile_base"] assert isinstance(resolved.endpoints["motion"].runtime_target, _MobileTarget) assert resolved.claim.claim_tokens == frozenset({"controller:base_velocity"}) + context = assembly.observation_provider.observe( + TaskState.empty(_BATCH_SIZE, robot.device) + ) + values = torch.linspace(0.1, 0.2, _BATCH_SIZE) + action = assembly.command_encoder.encode( + RuntimeCommandFrame( + commands=( + EndpointCommand( + _MobileTarget("base_velocity"), + _MobilePayload(values), + ), + ), + active_mask=torch.ones(_BATCH_SIZE, dtype=torch.bool), + env_ids=context.env_ids, + hold_duration=torch.full((_BATCH_SIZE,), _STEP_DT), + ) + ) + torch.testing.assert_close(action[:, 0], values) + + +@pytest.mark.parametrize( + "drift", + ("missing_resource", "extra_resource", "missing_endpoint", "extra_endpoint"), +) +def test_catalog_rejects_live_resource_and_endpoint_coverage_drift( + drift: str, +) -> None: + """Live bound topology must cover the registered profile exactly.""" + factory, registration = _mobile_factory() + assembly = factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) + bound_profile = assembly.engine.skill_profile + assert bound_profile is not None + resources = dict(bound_profile.resources) + resource = resources["mobile_base"] + if drift == "missing_resource": + resources.pop("mobile_base") + elif drift == "extra_resource": + resources["extra"] = replace( + resource, + resource_id="extra", + claim=replace( + resource.claim, + leaf_resource_ids=frozenset({"extra"}), + ), + ) + elif drift == "missing_endpoint": + resources["mobile_base"] = replace( + resource, + endpoints={}, + claim=replace( + resource.claim, + joint_ids=(), + claim_tokens=frozenset(), + ), + ) + else: + endpoints = dict(resource.endpoints) + endpoints["extra"] = endpoints["motion"] + resources["mobile_base"] = replace(resource, endpoints=endpoints) + bound_profile._resources = MappingProxyType(resources) + + with pytest.raises(IntegrationFingerprintMismatch, match="IDs differ"): + registration.catalog.validate_bound_endpoint_extensions(bound_profile) + + +@pytest.mark.parametrize( + ("route", "message"), + (("tracking", "tracking address"), ("evidence", "effect-evidence address")), +) +def test_catalog_rejects_control_part_live_route_address_drift( + route: str, + message: str, +) -> None: + """Built-in route IDs cannot hide a different target or evidence address.""" + factory, _ = _factory() + registration = factory.expert_program_registration + assembly = factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + ) + bound_profile = assembly.engine.skill_profile + assert bound_profile is not None + resources = dict(bound_profile.resources) + resource = resources["manipulator"] + endpoints = dict(resource.endpoints) + endpoint = endpoints["motion"] + if route == "tracking": + tracking = endpoint.tracking_channels[JOINT_POSITION_CHANNEL] + wrong_target = JointPositionTarget( + "different_arm", + endpoint.runtime_target.joint_ids, + ) + wrong_tracking = EndpointTrackingChannelBinding( + JOINT_POSITION_CHANNEL, + TrackingFeedbackSourceRef( + tracking.source.provider_id, + tracking.source.revision, + EndpointTrackingFeedbackAddress( + wrong_target, + JOINT_POSITION_CHANNEL, + ), + ), + tracking.projector, + ) + endpoints["motion"] = replace( + endpoint, + tracking_channels={JOINT_POSITION_CHANNEL: wrong_tracking}, + ) + else: + effect_sources = dict(endpoint.effect_sources) + channel = next(iter(effect_sources)) + source = effect_sources[channel] + effect_sources[channel] = EffectEvidenceSourceRef( + source.provider_id, + source.revision, + ControlPartEvidenceAddress("different_arm", channel), + ) + endpoints["motion"] = replace( + endpoint, + effect_sources=effect_sources, + ) + resources["manipulator"] = replace(resource, endpoints=endpoints) + bound_profile._resources = MappingProxyType(resources) + + with pytest.raises(IntegrationFingerprintMismatch, match=message): + registration.catalog.validate_bound_endpoint_extensions(bound_profile) + + +def test_standard_factory_rejects_adapter_live_target_declaration_drift() -> None: + """A lying adapter cannot emit a target absent from its catalog declaration.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(_LyingMobileEndpointAdapter(),), + runtime_transports=(_MobileTransportEncoder(),), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + ) + + with pytest.raises( + IntegrationFingerprintMismatch, + match="undeclared exact runtime target type", + ): + factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) + + +def test_standard_factory_rejects_target_live_transport_declaration_drift() -> None: + """A target instance cannot contradict its statically registered transport.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(_LyingTransportMobileEndpointAdapter(),), + runtime_transports=(_LyingTargetMobileTransportEncoder(),), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + ) + + with pytest.raises(IntegrationFingerprintMismatch, match="live transport"): + factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) + + +@pytest.mark.parametrize( + ("endpoint_adapter", "message"), + ( + (_LyingAdapterIdMobileEndpointAdapter(), "adapter ID"), + (_LyingFeedbackMobileEndpointAdapter(), "tracking-feedback routes"), + (_LyingEvidenceMobileEndpointAdapter(), "effect-evidence routes"), + ), +) +def test_standard_factory_rejects_adapter_live_route_declaration_drift( + endpoint_adapter: ResourceEndpointAdapter, + message: str, +) -> None: + """Every live adapter identity and provider route must match its fingerprint.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(endpoint_adapter,), + runtime_transports=(_MobileTransportEncoder(),), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + ) + + with pytest.raises(IntegrationFingerprintMismatch, match=message): + factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) def test_pick_place_effects_require_accepted_hand_state_and_live_pose() -> None: diff --git a/tests/sim/skills/test_parallel_runtime.py b/tests/sim/skills/test_parallel_runtime.py index 6d53cb1bf..ec21d8913 100644 --- a/tests/sim/skills/test_parallel_runtime.py +++ b/tests/sim/skills/test_parallel_runtime.py @@ -28,6 +28,7 @@ ArticulationJointState, CommandAcknowledgement, EndpointCommand, + ExecutionRunnerCfg, JointPositionPayload, JointPositionTarget, PlanningContext, @@ -82,6 +83,7 @@ def __init__( self.hold_targets: list[tuple[str, ...]] = [] self.hold_fingerprints: list[tuple[object, ...]] = [] self.operations: list[str] = [] + self.timeouts: list[tuple[str, float]] = [] self.holds = 0 self.cancels = 0 @@ -91,7 +93,7 @@ def send( *, timeout: float, ) -> CommandAcknowledgement: - del timeout + self.timeouts.append(("send", timeout)) if self.raise_send: raise RuntimeError("send exploded") self.operations.append("send") @@ -107,7 +109,8 @@ def hold( *, timeout: float, ) -> CommandAcknowledgement: - del context, timeout + del context + self.timeouts.append(("hold", timeout)) self.operations.append("hold") self.holds += 1 self.hold_targets.append( @@ -126,7 +129,8 @@ def cancel( *, timeout: float, ) -> CommandAcknowledgement: - del targets, timeout + del targets + self.timeouts.append(("cancel", timeout)) self.operations.append("cancel") self.cancels += 1 if self.reject_cancel: @@ -606,6 +610,144 @@ def test_completion_hold_waits_for_clock_after_accepted_command() -> None: assert outbound.operations == ["send", "hold"] +def test_parallel_runtime_uses_runner_transport_timeouts() -> None: + """Merged sends and safe stops share the selected preset runner policy.""" + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + runner_cfg=ExecutionRunnerCfg( + command_timeout=0.25, + safe_stop_timeout=0.75, + hold_on_completion=False, + ), + ) + + runtime.start() + runtime.step() + runtime.cancel("operator stop") + + assert outbound.timeouts == [ + ("send", pytest.approx(0.25)), + ("cancel", pytest.approx(0.75)), + ("hold", pytest.approx(0.75)), + ] + + +def test_parallel_failure_safe_holds_when_completion_hold_is_disabled() -> None: + """Failure policy always cancels and holds independently of success policy.""" + outbound = _OutboundSink() + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _RejectSafety(), + timeout_steps=5, + runner_cfg=ExecutionRunnerCfg(hold_on_completion=False), + ) + + runtime.start() + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert outbound.operations == ["cancel", "hold"] + + +def test_parallel_completion_respects_disabled_completion_hold() -> None: + """Successful completion does not synthesize a hold when policy disables it.""" + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + runner_cfg=ExecutionRunnerCfg(hold_on_completion=False), + ) + + runtime.start() + runtime.step() + clock.time = 0.1 + result = runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert outbound.operations == ["send"] + + +def test_parallel_minimum_cycle_time_limits_coordinator_cadence() -> None: + """Coordinator dispatches no faster than the preset's minimum cycle time.""" + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _running_step(frame=_frame(0, (3.0, 3.0))), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _running_step(frame=_frame(1, (4.0, 4.0))), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + runner_cfg=ExecutionRunnerCfg(minimum_cycle_time=0.25), + ) + + runtime.start() + first = runtime.step() + clock.time = 0.1 + waiting = runtime.step() + clock.time = 0.25 + runtime.step() + + assert first.wait_duration == pytest.approx(0.25) + assert waiting.wait_duration == pytest.approx(0.15) + assert len(outbound.frames) == 2 + + def test_parallel_runtime_fail_fast_is_row_local() -> None: left = _branch( "left", diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index 1156786b3..01b229788 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -1409,7 +1409,8 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: action_option_templates={ "pick": PickUpOptions(pre_grasp_distance=0.08), }, - motion_policy=MotionPolicy(planner="stub_planner", sample_count=80), + motion_policy=MotionPolicy(sample_count=80), + required_planner="stub_planner", tracking_policy=TrackingPolicy.joint_position( in_flight_max_abs_error=0.125, terminal_max_abs_error=0.125, @@ -1461,7 +1462,7 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: "other": SkillPolicyPreset( "other", action_option_templates={}, - motion_policy=MotionPolicy(planner="other_planner"), + required_planner="other_planner", ) }, ) From e7214d7bf496704b174fabbce3fbe3c56abbac8f Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 18:38:26 +0800 Subject: [PATCH 18/29] fix(solvers): construct configs with final parameters --- embodichain/lab/sim/solvers/base_solver.py | 42 ++++-- tests/sim/solvers/test_base_solver.py | 155 +++++++++++++++++++++ 2 files changed, 186 insertions(+), 11 deletions(-) create mode 100644 tests/sim/solvers/test_base_solver.py 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/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."] From 895f063ef91501019fb5349f6040a1b70b59cf30 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 20:12:10 +0800 Subject: [PATCH 19/29] feat(tasks): add declarative physical hand-over --- .../topics/atomic-actions/atomic-actions.md | 51 +- .../design/declarative_expert_program_plan.md | 66 ++- .../lab/gym/envs/expert_program/__init__.py | 2 + .../lab/gym/envs/expert_program/catalog.py | 34 +- .../expert_program/simulation_environment.py | 36 +- .../expert_program/simulation_handover.py | 146 +++++ .../expert_program/simulation_policies.py | 67 ++- embodichain/lab/sim/atomic_actions/runner.py | 16 +- .../expert_program/tableware/hand_over.yaml | 41 ++ .../configs/gym/hand_over/dual_ur5.json | 213 +++++++ .../embodichain_tasks/tableware/__init__.py | 3 +- .../embodichain_tasks/tableware/hand_over.py | 503 ++++++++++++++++ .../test_simulation_environment.py | 46 +- .../test_simulation_handover.py | 85 +++ .../test_simulation_policies.py | 127 +++- tests/gym/envs/tasks/test_hand_over.py | 541 ++++++++++++++++++ tests/sim/atomic_actions/test_runner.py | 113 +++- 17 files changed, 2030 insertions(+), 60 deletions(-) create mode 100644 embodichain/lab/gym/envs/expert_program/simulation_handover.py create mode 100644 embodichain_tasks/configs/expert_program/tableware/hand_over.yaml create mode 100644 embodichain_tasks/configs/gym/hand_over/dual_ur5.json create mode 100644 embodichain_tasks/embodichain_tasks/tableware/hand_over.py create mode 100644 tests/gym/envs/expert_program/test_simulation_handover.py create mode 100644 tests/gym/envs/tasks/test_hand_over.py diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 04e2ed834..4d2c336ed 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -569,6 +569,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 @@ -588,6 +595,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, @@ -789,7 +807,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 @@ -803,9 +827,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 10ff7647a..03647090b 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -6,8 +6,10 @@ 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. +- 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) @@ -583,6 +585,50 @@ 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. Terminal effect failure already enters bounded recovery; +phase-aware detection and failure-outcome state invalidation are not yet +implemented. For handover, success transfers the verified relation from source +to destination while the destination remains physically closed. Releasing the +destination is a separate ``Place`` or ``Release`` semantic call. + +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. + +This B stage is not the final continuation abstraction for mobile-base or +whole-body transports. Such endpoints need a typed, transport-owned +continuation command that can preserve the last successful setpoint per +endpoint; they must not inherit a joint-qpos-specific latch. The phase-aware +in-flight held-object guard and failure-state reconciliation described below +also remain follow-up work. + ## 8. Expert Program configuration ### 8.1 Version 1 schema @@ -1172,8 +1218,10 @@ 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. +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; the full repeated-cube run +remains validation work. Deliverables: @@ -1421,7 +1469,15 @@ 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. - [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/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py index b3bd32cf1..79422cc06 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -155,6 +155,7 @@ SimulationPlanningObservationProvider, create_simulation_expert_program_adapter, ) +from .simulation_handover import ConfiguredHandOverPoseProvider from .simulation_policies import ( SimulationSegmentPolicyPort, default_simulation_settle_presets, @@ -186,6 +187,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 be266f8bb..b6f582711 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -1269,6 +1269,33 @@ def validate_bound_endpoint_extensions( ) +def _profile_with_step_dt( + profile: RobotSkillProfile, + *, + step_dt: float, +) -> RobotSkillProfile: + """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=step_dt), + tracking_policy=preset.tracking_policy, + recovery_policy=preset.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, + ) + for preset_id, preset in profile.presets.items() + }, + ) + + def _registration_payload( *, scene_binding: SimulationSceneBinding, @@ -1629,9 +1656,10 @@ def validate_robot_profile( self.assert_unchanged() if type(profile) is not RobotSkillProfile: raise TypeError("profile must be exactly RobotSkillProfile.") - if not isinstance(step_dt, (int, float)) or isinstance(step_dt, bool): - raise TypeError("step_dt must be a real number.") - expected = self.catalog.robot_profile + expected = _profile_with_step_dt( + self.catalog.robot_profile, + step_dt=step_dt, + ) if _canonical_json(profile) != _canonical_json(expected): raise IntegrationFingerprintMismatch( "Live robot skill profile differs from the registered declaration." diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py index b5f5a118d..b20801142 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -718,9 +718,11 @@ class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): translation_threshold: Material scene translation threshold. rotation_threshold: Material scene rotation threshold. - The Gym cadence is carried by each fresh ``PlanningContext``. Motion policy - remains a provider-free planner declaration and is never rewritten with - environment timing. + 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__( @@ -871,8 +873,34 @@ def create_scene_registry(self) -> SceneRegistry: return registry def create_robot_skill_profile(self) -> RobotSkillProfile: - """Build and validate the registered declarative robot profile.""" + """Build a profile whose motion and runner policies use Gym cadence.""" profile = self._robot_profile_binding.build(self._robot) + aligned_presets = { + preset_id: SkillPolicyPreset( + preset_id=preset.preset_id, + schema_version=preset.schema_version, + motion_policy=replace( + preset.motion_policy, + control_dt=self._step_dt, + ), + tracking_policy=preset.tracking_policy, + recovery_policy=preset.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, + ) + for preset_id, preset in profile.presets.items() + } + 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 runtime policies were not cadence-aligned.") self._registration.validate_robot_profile( profile, 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/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 8dac661a6..2db094a81 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -236,6 +236,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 +254,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): @@ -581,7 +592,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 +618,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() 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/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..a1337f25e --- /dev/null +++ b/embodichain_tasks/embodichain_tasks/tableware/hand_over.py @@ -0,0 +1,503 @@ +# ---------------------------------------------------------------------------- +# 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 +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), + 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/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 2f304c21b..be4a32cc9 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -149,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 @@ -1008,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: @@ -1118,7 +1120,7 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: "pick": PickUpOptions(), "place": PlaceOptions(), }, - motion_policy=MotionPolicy(sample_count=17), + 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, @@ -1126,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, ), ), @@ -1272,17 +1274,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), @@ -1871,17 +1878,36 @@ def _assert_invocation_equivalent( ) -def test_simulation_factory_preserves_policy_and_tracking_contract() -> None: - """Gym cadence does not mutate the registered planner or tracking policy.""" - 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() + source_preset = binding.presets[0] + source_runner_cfg = source_preset.runner_cfg + factory, _ = _factory(binding) profile = factory.create_robot_skill_profile() - assert profile.presets["safe"].motion_policy.sample_count == 17 - 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 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( 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..7f7293e42 --- /dev/null +++ b/tests/gym/envs/tasks/test_hand_over.py @@ -0,0 +1,541 @@ +# ---------------------------------------------------------------------------- +# 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].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/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index 0c217fdb6..6875291d5 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -57,6 +57,7 @@ RuntimeCommandFrame, RuntimeEndpointTarget, RunnerStatus, + RunnerStep, SceneSnapshot, SkillBindingContract, SkillEndpointRequirement, @@ -339,6 +340,8 @@ def _make_runner( 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, @@ -385,7 +388,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 @@ -410,6 +417,26 @@ def _successful_effect_result( ) +def _unresolved_effect_result( + context: PlanningContext, + request: EffectVerificationRequest, +) -> EffectVerificationResult: + """Keep every row pending at the current effect boundary.""" + return EffectVerificationResult( + 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,)) @@ -857,6 +884,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() @@ -1125,6 +1208,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() From 1ca417069a06014b74ad53eb3afae4a4665d9569 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 21:16:42 +0800 Subject: [PATCH 20/29] feat(atomic-actions): guard in-flight held objects --- .../design/declarative_expert_program_plan.md | 52 +- .../overview/sim/atomic_actions/index.md | 17 + .../lab/sim/atomic_actions/__init__.py | 6 + .../lab/sim/atomic_actions/execution.py | 447 ++++++++++++++++++ embodichain/lab/sim/atomic_actions/runner.py | 77 ++- embodichain/lab/sim/skills/__init__.py | 4 + embodichain/lab/sim/skills/compiler.py | 308 ++++++++++++ embodichain/lab/sim/skills/runtime.py | 230 ++++++++- .../sim/atomic_actions/test_engine_per_env.py | 247 ++++++++++ tests/sim/atomic_actions/test_runner.py | 51 ++ tests/sim/skills/test_compiler.py | 46 ++ tests/sim/skills/test_runtime.py | 118 ++++- 12 files changed, 1568 insertions(+), 35 deletions(-) diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 03647090b..cf5294d4a 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -597,11 +597,19 @@ controller intent, not physical proof. passed the selected monitor. The target contract must treat contradictory evidence, including object-to-endpoint slip, as a real effect failure, invalidate the affected row's assumed relation, and enter bounded recovery instead of -repairing the scene. Terminal effect failure already enters bounded recovery; -phase-aware detection and failure-outcome state invalidation are not yet -implemented. For handover, success transfers the verified relation from source -to destination while the destination remains physically closed. Releasing the -destination is a separate ``Place`` or ``Release`` semantic call. +repairing the scene. The runtime now exposes the active named motion phase, +observes phase-scoped held-object invariants from fresh physical evidence, and +applies removal-only ``StateDelta`` reconciliation to failed rows before any +retry or recovery hand-off. ``Pick`` can use the existing bounded action retry; +``Place`` and ``HandOver`` currently emit a typed ``RECOVERY_REQUIRED`` boundary +because replaying the same invocation after its required relation was removed +would be invalid. A workflow-level re-acquisition policy, blocking acquisition +gates, per-expectation terminal failure reconciliation, and fail-closed +reconciliation for evidence that remains unresolved at the action deadline +remain explicit design decisions rather than implicit scene repair. For +handover, success transfers the verified relation from source to destination +while the destination remains physically closed. Releasing the destination is +a separate ``Place`` or ``Release`` semantic call. The first pure-dynamics rollout uses the staged **B** continuation policy. The standard simulation factory lowers both trajectory ``control_dt`` and runner @@ -622,12 +630,12 @@ embodiment calibration, not effect-monitor success shortcuts: the normal ``0.05 rad`` tracking gate, bounded replanning, physical effect evidence, and settling thresholds remain enabled. -This B stage is not the final continuation abstraction for mobile-base or -whole-body transports. Such endpoints need a typed, transport-owned -continuation command that can preserve the last successful setpoint per -endpoint; they must not inherit a joint-qpos-specific latch. The phase-aware -in-flight held-object guard and failure-state reconciliation described below -also remain follow-up work. +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 @@ -1216,12 +1224,16 @@ 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. The embodiment-owned dual-UR5/PGI HandOver slice now -completes Pick, transfer, terminal physical-effect verification, settling, and -target validation through real contact dynamics; the full repeated-cube run -remains validation work. +production simulation evidence ports are wired end to end. Phase-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; blocking acquisition gates, workflow-level re-acquisition, +per-expectation terminal reconciliation, fault-injection coverage, and the full +repeated-cube run remain validation or design work. Deliverables: @@ -1477,7 +1489,11 @@ The design is complete when all of the following hold: synthetic attachment, freezes the object, or overrides its pose. - [ ] Physical held-object loss is observed as effect failure, invalidates the affected symbolic relation, and exercises bounded recovery rather than - being hidden by a simulator-side attachment. + being hidden by a simulator-side attachment. The phase-aware observation, + row-local invalidation, bounded Pick retry, and typed recovery boundary + are implemented; blocking acquisition, per-expectation terminal + reconciliation, workflow-level re-acquisition, and real-simulation fault + injection remain open. - [x] Repeated sub-threshold motion eventually publishes the correct scene revision. - [x] Custom actions have a documented and tested intentional hard-break diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index f63527335..90a27d09a 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -854,6 +854,23 @@ and resets the history. Evidence exactly at the deadline is valid, while a due observation after the deadline is handled by session timeout without invoking the verifier. +The curated semantic runtime also installs phase-scoped, negative +held-object guards for named trajectory segments. Before a due command is +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 current guard is observational and negative; a blocking positive +acquisition gate, outcome-aware terminal reconciliation, and workflow-level +re-acquisition remain separate policies. Neither the monitor nor runtime +creates a simulator attachment, freezes an object, or overrides its pose. + ## Action Agent integration An MLLM should not construct `ActionInvocation` by copying arbitrary JSON into diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 790f44972..438b64e19 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -65,6 +65,8 @@ ExecutionSession, ExecutionStatus, ExecutionTick, + HeldObjectGuardRequest, + HeldObjectGuardResult, ) from .goals import ( ActionGoal, @@ -194,6 +196,7 @@ ExecutionClock, ExecutionRunner, ExecutionRunnerCfg, + HeldObjectGuardVerifier, MonotonicExecutionClock, ObservationProvider, RunnerStatus, @@ -274,6 +277,9 @@ "ExecutionSession", "ExecutionStatus", "ExecutionTick", + "HeldObjectGuardRequest", + "HeldObjectGuardResult", + "HeldObjectGuardVerifier", "EndpointTrackingChannelBinding", "EndpointTrackingFeedbackAddress", "FeedbackTerminalAcceptance", diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 34baa2d5e..f43937ee4 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -74,8 +74,10 @@ class ExecutionEventKind(str, Enum): EFFECT_VERIFICATION_REQUIRED = "effect_verification_required" EFFECT_VERIFICATION_FAILED = "effect_verification_failed" EFFECT_VERIFICATION_TIMEOUT = "effect_verification_timeout" + 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" @@ -350,6 +352,191 @@ def __post_init__(self) -> None: object.__setattr__(self, "failure_mask", self.failure_mask.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) class ExecutionTick: """Result returned after one closed-loop execution update.""" @@ -481,6 +668,7 @@ def __init__( self._effect_failures = torch.zeros_like(self._eligible) self._effect_requested_at: float | None = None self._next_effect_verification_id = 0 + self._next_held_object_guard_verification_id = 0 self._plan_attempt_records: list[_ExecutionPlanAttemptRecord] = [] self._status = ( ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED @@ -527,6 +715,22 @@ 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 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, @@ -765,6 +969,7 @@ def tick( context: PlanningContext, *, effect_result: EffectVerificationResult | None = None, + held_object_guard_result: HeldObjectGuardResult | None = None, ) -> ExecutionTick: """Advance execution by one observation/command cycle. @@ -773,6 +978,10 @@ 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. + 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. @@ -791,10 +1000,52 @@ def tick( "effect_result verification_id does not match the pending " "effect boundary." ) + 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 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, @@ -1849,6 +2100,200 @@ def _batched_entity_pose(self, state: EntityState) -> torch.Tensor: raise ValueError("Scene entity pose batch does not match the session.") return pose + 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._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): @@ -1970,4 +2415,6 @@ def _tick_result( "ExecutionSession", "ExecutionStatus", "ExecutionTick", + "HeldObjectGuardRequest", + "HeldObjectGuardResult", ] diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 2db094a81..0c7601fad 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -36,6 +36,8 @@ ExecutionSession, ExecutionStatus, ExecutionTick, + HeldObjectGuardRequest, + HeldObjectGuardResult, ) from .invocation import ActionInvocation, ResolvedActionRequest from .runtime_commands import RuntimeCommandFrame @@ -310,6 +312,12 @@ 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.""" + RunnerStepCallback = Callable[[RunnerStep], None] """Optional observer called after every blocking runner-loop iteration.""" @@ -477,6 +485,7 @@ def step( *, effect_result: EffectVerificationResult | None = None, effect_verifier: EffectVerifier | None = None, + held_object_guard_verifier: HeldObjectGuardVerifier | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. @@ -489,6 +498,11 @@ def step( and before the session consumes the result. It is not called after the request deadline. Mutually exclusive with ``effect_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, @@ -500,6 +514,10 @@ def step( ) 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.") now = self._clock_now() if self._status is not RunnerStatus.RUNNING: return self._result(timestamp=now) @@ -523,6 +541,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 @@ -542,14 +573,39 @@ def step( context=context, ) - try: - if self._pending_revision is not None: - self._session._install_prepared_revision( - self._pending_revision, + 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, ) - self._pending_revision = None - tick = self._session.tick(context, effect_result=effect_result) + 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, + held_object_guard_result=held_object_guard_result, + ) context = self._session.latest_context self._last_context = context except Exception as exc: @@ -699,6 +755,7 @@ def run_until_blocked( self, *, effect_verifier: EffectVerifier | None = None, + held_object_guard_verifier: HeldObjectGuardVerifier | None = None, on_step: RunnerStepCallback | None = None, max_steps: int = 100_000, ) -> RunnerStep: @@ -709,6 +766,8 @@ def run_until_blocked( due-cycle observations while effect verification is pending. Without one, the method returns the running boundary so the caller can verify externally. + 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. @@ -727,7 +786,10 @@ 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, + held_object_guard_verifier=held_object_guard_verifier, + ) if on_step is not None: try: on_step(result) @@ -939,6 +1001,7 @@ def _result( "ExecutionClock", "ExecutionRunner", "ExecutionRunnerCfg", + "HeldObjectGuardVerifier", "MonotonicExecutionClock", "ObservationProvider", "RunnerStatus", diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 9bd87c54a..95381c478 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -34,9 +34,11 @@ ) from .compiler import ( AnalyzedSemanticCall, + GroundedHeldObjectGuard, GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, + HeldObjectGuardBaseline, RegisteredSemanticLowerer, RelationTargetGrounder, SemanticEffectDependency, @@ -267,12 +269,14 @@ "EffectStateExpectation", "FORCE_EFFECT_CHANNEL", "GRASP_AFFORDANCE_CAPABILITY", + "GroundedHeldObjectGuard", "GroundedSemanticCall", "HeldObjectRelation", "HeldObjectStateExpectation", "HandOver", "HandOverPoseProvider", "HandOverPoseTargets", + "HeldObjectGuardBaseline", "LinkedSemanticCall", "JOINT_STATE_EFFECT_CHANNEL", "JointStateEffectClause", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index dd2db8254..f0c094867 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 @@ -76,6 +77,8 @@ JointStateEffectClause, PoseRelationClause, PoseRelationExpectation, + ScalarEffectClause, + ScalarExpectation, SemanticEffectKind, SemanticEffectSpec, SymbolicStateKey, @@ -431,7 +434,86 @@ 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, init=False) class GroundedSemanticCall: """Call lowered from the latest observed context.""" @@ -439,6 +521,7 @@ class GroundedSemanticCall: invocation: ActionInvocation effect_spec: SemanticEffectSpec | None effect_monitor: EffectMonitor | None = field(repr=False, compare=False) + effect_guards: tuple[GroundedHeldObjectGuard, ...] _eligible_mask: torch.Tensor = field(repr=False, compare=False) def __init__(self, *args: object, **kwargs: object) -> None: @@ -457,6 +540,7 @@ def _create( invocation: ActionInvocation, effect_spec: SemanticEffectSpec | None, effect_monitor: EffectMonitor | None, + effect_guards: tuple[GroundedHeldObjectGuard, ...], eligible_mask: torch.Tensor, ) -> GroundedSemanticCall: """Create one compiler-owned grounded result.""" @@ -465,6 +549,7 @@ 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, "_eligible_mask", eligible_mask.clone()) instance.__post_init__() return instance @@ -490,6 +575,17 @@ 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) 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: @@ -1093,11 +1189,18 @@ 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"), + ) return GroundedSemanticCall._create( analyzed=analyzed, invocation=invocation, effect_spec=effect_spec, effect_monitor=effect_monitor, + effect_guards=effect_guards, eligible_mask=eligible, ) @@ -1716,6 +1819,209 @@ def _ground_effect_spec( clauses=tuple(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, @@ -2138,9 +2444,11 @@ def _broadcast_joint_position( __all__ = [ "AnalyzedSemanticCall", + "GroundedHeldObjectGuard", "GroundedSemanticCall", "HandOverPoseProvider", "HandOverPoseTargets", + "HeldObjectGuardBaseline", "RelationTargetGrounder", "RegisteredSemanticLowerer", "SemanticEffectDependency", diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index 0fe00c182..db352b387 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -29,11 +29,14 @@ from ..atomic_actions.bindings import EndpointBinding from ..atomic_actions.engine import AtomicActionEngine +from ..atomic_actions.effects import StateDelta from ..atomic_actions.execution import ( EffectVerificationRequest, EffectVerificationResult, ExecutionEvent, ExecutionPlanAttempt, + HeldObjectGuardRequest, + HeldObjectGuardResult, ) from ..atomic_actions.plans import TrajectorySegment from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy @@ -47,7 +50,7 @@ RunnerStatus, RunnerStep, ) -from ..atomic_actions.state import PlanningContext, TaskState +from ..atomic_actions.state import HeldObjectState, PlanningContext, TaskState from ..atomic_actions.tracking import ( FeedbackTerminalAcceptance, TimedTrackingSequence, @@ -55,11 +58,16 @@ TrackingPolicy, ) from .calls import SemanticCallSpec -from .compiler import SemanticSkillCompiler +from .compiler import ( + GroundedHeldObjectGuard, + HeldObjectGuardBaseline, + SemanticSkillCompiler, +) from .effects import ( BinaryEffectEvidenceBatch, EffectEvidenceBatch, EffectMonitor, + EffectMonitorDecision, EffectMonitorRef, JointStateEvidenceBatch, PoseRelationEvidenceBatch, @@ -949,6 +957,9 @@ class SkillEffectTrace: 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 + segment_name: str | None = None def __post_init__(self) -> None: if type(self.call_index) is not int or self.call_index < 0: @@ -957,6 +968,21 @@ def __post_init__(self) -> None: raise ValueError("verification_id must be a non-negative integer.") if type(self.observation_revision) is not int or self.observation_revision < 0: raise ValueError("observation_revision must be non-negative.") + if self.boundary_kind not in {"terminal", "in_flight_guard"}: + raise ValueError("boundary_kind must be 'terminal' or 'in_flight_guard'.") + for name in ("guard_id", "segment_name"): + value = getattr(self, name) + if value is not None and (type(value) is not str or not value): + raise ValueError(f"{name} must be a non-empty string or None.") + if self.boundary_kind == "terminal": + if self.guard_id is not None or self.segment_name is not None: + raise ValueError( + "Terminal effect traces cannot declare guard phase metadata." + ) + elif self.guard_id is None or self.segment_name is None: + raise ValueError( + "In-flight guard traces require guard_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"): @@ -1024,11 +1050,14 @@ def snapshot(self) -> SkillEffectTrace: 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, + 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, @@ -1049,6 +1078,15 @@ def to_metadata(self) -> dict[str, object]: "failure_mask": _metadata_value(self.failure_mask), }, } + 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, + } + ) + return metadata @dataclass(frozen=True, slots=True, eq=False) @@ -1536,6 +1574,7 @@ def __init__( self._call_event_offset = 0 self._call_effect_offset = 0 self._observation_revision = 0 + self._next_guard_verification_id = 0 self._wait_duration = 0.0 self._message: str | None = None @@ -1701,7 +1740,12 @@ 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 + runner_step = runner.step( + effect_verifier=verifier, + held_object_guard_verifier=guard_verifier, + ) self._consume_runner_step(runner_step) if ( runner_step.status is RunnerStatus.RUNNING @@ -1967,6 +2011,7 @@ def _reset_workflow( self._call_event_offset = 0 self._call_effect_offset = 0 self._observation_revision = 0 + self._next_guard_verification_id = 0 self._wait_duration = 0.0 self._message = None self._status = SkillStatus.RUNNING @@ -2018,6 +2063,7 @@ def _prepare_call(self, call_index: int) -> 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", ())) if invocation is None: raise TypeError("Semantic compiler ground() must return an invocation.") if not isinstance(grounded_eligible, torch.Tensor) or not torch.equal( @@ -2039,6 +2085,13 @@ 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.") self._grounded = grounded session = self._engine.start( @@ -2086,6 +2139,166 @@ 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, + ) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + ) + + 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, + 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)] @@ -2124,13 +2337,12 @@ def _effect_verifier( configured_monitor_params=configured_monitor_params, resolved_monitor_params=resolved_monitor_params, evidence=evidence, + boundary_kind=boundary_kind, + guard_id=guard_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.""" diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 53efa60ec..b37bb7d03 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -49,6 +49,8 @@ EffectVerificationRequirement, EffectVerificationResult, GraspGoal, + HeldObjectGuardRequest, + HeldObjectGuardResult, HeldObjectState, JointPositionPayload, JointPositionTarget, @@ -520,6 +522,54 @@ 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 _multi_dependency_context( timestamp: float, *, @@ -707,6 +757,203 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +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() diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index 6875291d5..ff9156174 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, @@ -484,6 +485,56 @@ 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_dispatches_transport_neutral_endpoint_frames() -> None: runner, _, _, sink, _ = _make_runner() diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index eafaaff78..92f684325 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -69,6 +69,7 @@ from embodichain.lab.sim.skills.compiler import ( HandOverPoseProvider, HandOverPoseTargets, + HeldObjectGuardBaseline, RegisteredSemanticLowerer, RelationTargetGrounder, SemanticLowering, @@ -771,6 +772,17 @@ 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 def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: @@ -822,6 +834,20 @@ 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 def test_handover_effect_spec_binds_source_and_destination_relations() -> None: @@ -896,6 +922,26 @@ 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 def test_registered_call_without_monitor_has_no_effect_contract() -> None: diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py index 53ccaed74..f07c45971 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -32,6 +32,7 @@ ActionInvocation, ActionOptions, ActionPlan, + Affordance, ArticulationJointState, AtomicAction, AtomicActionEngine, @@ -41,8 +42,11 @@ EndpointBinding, EndpointTrackingChannelBinding, EndpointTrackingFeedbackAddress, + HeldObjectGuardRequest, + HeldObjectState, JointPositionTarget, MotionPolicy, + ObjectSemantics, PlanningContext, RecoveryPolicy, ResolvedActionRequest, @@ -57,14 +61,22 @@ ) 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.compiler import ( + GroundedHeldObjectGuard, + HeldObjectGuardBaseline, + SemanticSkillCompiler, +) from embodichain.lab.sim.skills.effects import ( ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEvidenceKind, ControlPartEvidenceAddress, EffectEvidenceBatch, EffectEvidenceSourceRef, EffectMonitor, EffectMonitorDecision, + HeldObjectRelation, + HeldObjectStateExpectation, JOINT_STATE_EFFECT_CHANNEL, JointStateEffectClause, SemanticEffectKind, @@ -599,6 +611,109 @@ def test_nonblocking_step_routes_effect_feedback_through_collector() -> None: assert system.compiler.monitors[0].requests[0].verification_id == 0 +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_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None: system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) @@ -641,6 +756,7 @@ 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"] == {} From c13a5d44331fc1ebaa322894a6444ee5fda0ff51 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 21:43:08 +0800 Subject: [PATCH 21/29] feat(skills): expose per-expectation effect outcomes --- embodichain/lab/sim/skills/__init__.py | 2 + embodichain/lab/sim/skills/effects.py | 244 ++++++++++++-- .../test_multi_segments_cube_pick_place.py | 3 +- tests/gym/envs/tasks/test_open_drawer.py | 3 +- tests/sim/skills/test_effects.py | 300 ++++++++++++++++++ 5 files changed, 516 insertions(+), 36 deletions(-) diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 95381c478..2b192950e 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -69,6 +69,7 @@ EffectEvidenceAddress, EffectEvidenceBatch, EffectEvidenceSourceRef, + EffectExpectationDecision, EffectMonitor, EffectMonitorDecision, EffectMonitorFactory, @@ -260,6 +261,7 @@ "EffectEvidenceQuery", "EffectEvidenceQueryValue", "EffectEvidenceSourceRef", + "EffectExpectationDecision", "EffectMonitor", "EffectMonitorDecision", "EffectMonitorFactory", 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/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..5f9dd99f8 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 @@ -185,7 +185,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/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: From d85725029d22ddbce47b5501082b3a8c20d5fc2a Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 21:58:59 +0800 Subject: [PATCH 22/29] feat(atomic-actions): reconcile terminal effect failures --- .../topics/atomic-actions/atomic-actions.md | 12 + .../design/declarative_expert_program_plan.md | 43 ++- .../overview/sim/atomic_actions/index.md | 12 + docs/source/tutorial/atomic_actions.rst | 14 +- .../lab/sim/atomic_actions/__init__.py | 2 + .../lab/sim/atomic_actions/execution.py | 335 +++++++++++++++++- embodichain/lab/sim/skills/runtime.py | 164 ++++++++- .../atomic_action/moving_target_recovery.py | 2 + .../sim/atomic_actions/test_engine_per_env.py | 200 +++++++++-- tests/sim/atomic_actions/test_runner.py | 38 +- tests/sim/skills/test_runtime.py | 88 ++++- 11 files changed, 836 insertions(+), 74 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 4d2c336ed..0ab5b6994 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -539,15 +539,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 diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index cf5294d4a..0f5cc4984 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -600,16 +600,22 @@ the affected row's assumed relation, and enter bounded recovery instead of repairing the scene. The runtime now exposes the active named motion phase, observes phase-scoped held-object invariants from fresh physical evidence, and applies removal-only ``StateDelta`` reconciliation to failed rows before any -retry or recovery hand-off. ``Pick`` can use the existing bounded action retry; -``Place`` and ``HandOver`` currently emit a typed ``RECOVERY_REQUIRED`` boundary -because replaying the same invocation after its required relation was removed -would be invalid. A workflow-level re-acquisition policy, blocking acquisition -gates, per-expectation terminal failure reconciliation, and fail-closed -reconciliation for evidence that remains unresolved at the action deadline -remain explicit design decisions rather than implicit scene repair. For -handover, success transfers the verified relation from source to destination -while the destination remains physically closed. Releasing the destination is -a separate ``Place`` or ``Release`` semantic call. +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 and blocking acquisition gates remain open; neither may repair +the scene implicitly. For handover, success transfers the verified relation +from source to destination while the destination remains physically closed. +Releasing the destination is a separate ``Place`` or ``Release`` semantic call. The first pure-dynamics rollout uses the staged **B** continuation policy. The standard simulation factory lowers both trajectory ``control_dt`` and runner @@ -1231,9 +1237,11 @@ 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; blocking acquisition gates, workflow-level re-acquisition, -per-expectation terminal reconciliation, fault-injection coverage, and the full -repeated-cube run remain validation or design work. +contact dynamics. Per-expectation terminal outcomes, core-owned failure +invalidation, row-local retry/recovery decisions, and fail-closed deadline +reconciliation are implemented. Blocking acquisition gates, workflow-level +re-acquisition, fault-injection coverage, and the full repeated-cube run remain +validation or design work. Deliverables: @@ -1490,10 +1498,11 @@ The design is complete when all of the following hold: - [ ] Physical held-object loss is observed as effect failure, invalidates the affected symbolic relation, and exercises bounded recovery rather than being hidden by a simulator-side attachment. The phase-aware observation, - row-local invalidation, bounded Pick retry, and typed recovery boundary - are implemented; blocking acquisition, per-expectation terminal - reconciliation, workflow-level re-acquisition, and real-simulation fault - injection remain open. + row-local core-owned invalidation, per-expectation terminal + reconciliation, fail-closed deadline handling, bounded Pick/retained-Place + retry, and typed recovery boundary are implemented; blocking acquisition, + workflow-level re-acquisition, and real-simulation fault injection remain + open. - [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/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 90a27d09a..3637f2c71 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -804,6 +804,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) @@ -814,6 +816,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) ``` @@ -826,6 +830,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 diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 88b2584c1..86530563d 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -510,6 +510,8 @@ correlated per-environment verification result: .. code-block:: python + import torch + from embodichain.lab.sim.atomic_actions import EffectVerificationResult def verify_effect(context, request): @@ -518,6 +520,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) @@ -539,6 +543,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: @@ -557,7 +563,13 @@ 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. Adding an action ---------------- diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 438b64e19..a5ea5c0c2 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -57,6 +57,7 @@ from .effects import StateDelta from .engine import AtomicActionEngine from .execution import ( + EffectExpectationResult, EffectVerificationRequest, EffectVerificationResult, ExecutionEvent, @@ -264,6 +265,7 @@ "EndpointCommandRouter", "EndpointCommandTransport", "EntityState", + "EffectExpectationResult", "EffectVerificationRequest", "EffectVerificationRequirement", "EffectVerificationResult", diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index f43937ee4..43011ffd3 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 @@ -229,6 +229,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 @@ -243,6 +245,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: @@ -290,8 +293,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", @@ -317,6 +344,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, ) @@ -324,32 +419,96 @@ 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()) - object.__setattr__(self, "failure_mask", self.failure_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) @@ -1063,12 +1222,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( @@ -1762,6 +1945,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", @@ -1770,17 +1955,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 @@ -1800,8 +2035,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 = ( @@ -1810,7 +2049,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( @@ -1822,7 +2061,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, @@ -2332,8 +2571,69 @@ 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, kind: ExecutionEventKind, @@ -2407,6 +2707,7 @@ def _tick_result( __all__ = [ + "EffectExpectationResult", "EffectVerificationRequest", "EffectVerificationResult", "ExecutionEvent", diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index db352b387..358b925f3 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -31,6 +31,7 @@ from ..atomic_actions.engine import AtomicActionEngine from ..atomic_actions.effects import StateDelta from ..atomic_actions.execution import ( + EffectExpectationResult, EffectVerificationRequest, EffectVerificationResult, ExecutionEvent, @@ -57,7 +58,7 @@ TrackingMetricCfg, TrackingPolicy, ) -from .calls import SemanticCallSpec +from .calls import HandOver, Pick, Place, SemanticCallSpec from .compiler import ( GroundedHeldObjectGuard, HeldObjectGuardBaseline, @@ -66,6 +67,7 @@ from .effects import ( BinaryEffectEvidenceBatch, EffectEvidenceBatch, + EffectExpectationDecision, EffectMonitor, EffectMonitorDecision, EffectMonitorRef, @@ -951,6 +953,7 @@ 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 @@ -999,6 +1002,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 ( @@ -1022,6 +1073,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, @@ -1044,6 +1100,7 @@ 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, @@ -1076,6 +1133,17 @@ 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} @@ -2145,12 +2213,94 @@ def _effect_verifier( 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 _held_object_guard_verifier( self, context: PlanningContext, @@ -2308,7 +2458,16 @@ def _observe_effect_monitor( 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): @@ -2331,6 +2490,7 @@ def _observe_effect_monitor( 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, diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index 3738d3a2c..a8f23a774 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -428,6 +428,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/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index b37bb7d03..ebb2782ea 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, @@ -83,6 +84,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.""" @@ -702,6 +727,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) @@ -721,9 +747,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)) @@ -1998,7 +2027,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]), @@ -2023,7 +2052,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]), @@ -2053,7 +2082,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]), @@ -2078,7 +2107,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]), @@ -2097,7 +2126,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]), @@ -2153,7 +2182,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]), @@ -2196,7 +2225,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]), @@ -2217,7 +2246,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]), @@ -2228,7 +2257,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]), @@ -2251,7 +2280,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]), @@ -2261,7 +2290,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]), @@ -2277,17 +2306,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) @@ -2310,7 +2367,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]), @@ -2325,7 +2382,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]), @@ -2376,7 +2433,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]), @@ -2392,7 +2449,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]), @@ -2420,6 +2477,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 ): @@ -2429,7 +2575,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]), @@ -2459,7 +2605,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]), @@ -2518,7 +2664,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]), @@ -2550,7 +2696,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]), @@ -2579,7 +2725,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]), @@ -2616,7 +2762,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]), @@ -2641,7 +2787,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]), @@ -2712,7 +2858,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]), @@ -2780,7 +2926,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 ff9156174..b590c3dbd 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -84,6 +84,28 @@ 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, + ) + + class FakeClock: """Deterministic clock used by non-blocking runner tests.""" @@ -403,7 +425,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, @@ -423,7 +445,7 @@ def _unresolved_effect_result( request: EffectVerificationRequest, ) -> EffectVerificationResult: """Keep every row pending at the current effect boundary.""" - return EffectVerificationResult( + return _effect_result( verification_id=request.verification_id, success_mask=torch.zeros( context.batch_size, @@ -1092,7 +1114,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]), @@ -1152,7 +1174,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), @@ -1188,7 +1210,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]), @@ -1197,7 +1219,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]), @@ -1310,7 +1332,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]), @@ -1330,7 +1352,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_runtime.py b/tests/sim/skills/test_runtime.py index f07c45971..8f247cb32 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -60,7 +60,7 @@ TrackingProjectorRef, ) from embodichain.lab.sim.atomic_actions.tracking import TrackingPolicy -from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.calls import HandOver, Place, RegisteredSemanticCall from embodichain.lab.sim.skills.compiler import ( GroundedHeldObjectGuard, HeldObjectGuardBaseline, @@ -73,6 +73,7 @@ ControlPartEvidenceAddress, EffectEvidenceBatch, EffectEvidenceSourceRef, + EffectExpectationDecision, EffectMonitor, EffectMonitorDecision, HeldObjectRelation, @@ -91,7 +92,7 @@ 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.scene import SceneObjectRef, SceneRegistry BATCH_SIZE = 2 @@ -213,6 +214,7 @@ def observe( return EffectMonitorDecision( self._decision.success_mask, self._decision.failure_mask, + self._decision.expectation_decisions, ) @@ -611,6 +613,88 @@ 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 ): From 31104ae1d77576ab50274d640c59f25c5ce68a0c Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 22:23:58 +0800 Subject: [PATCH 23/29] feat(skills): gate motion on physical effects --- .../design/declarative_expert_program_plan.md | 48 +- .../embodichain.lab.sim.atomic_actions.rst | 6 + .../overview/sim/atomic_actions/index.md | 27 +- docs/source/tutorial/atomic_actions.rst | 28 ++ .../lab/sim/atomic_actions/__init__.py | 14 +- embodichain/lab/sim/atomic_actions/core.py | 1 + .../lab/sim/atomic_actions/execution.py | 474 +++++++++++++++++- .../lab/sim/atomic_actions/invocation.py | 82 ++- embodichain/lab/sim/atomic_actions/runner.py | 62 +++ embodichain/lab/sim/skills/__init__.py | 2 + embodichain/lab/sim/skills/compiler.py | 171 +++++++ embodichain/lab/sim/skills/runtime.py | 170 ++++++- .../test_simulation_environment.py | 66 ++- .../sim/atomic_actions/test_engine_per_env.py | 240 +++++++++ tests/sim/atomic_actions/test_runner.py | 142 +++++- tests/sim/skills/test_compiler.py | 35 ++ tests/sim/skills/test_runtime.py | 116 +++++ 17 files changed, 1639 insertions(+), 45 deletions(-) diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 0f5cc4984..a41dbcab8 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -8,7 +8,9 @@ Pick/Place/settle/validator cycle, while the full three-cycle run remains in threshold calibration. Dual-UR5/PGI HandOver has completed three consecutive supported-simulation Pick/transfer/settle/validator runs using contact - dynamics only. + dynamics only. Named trajectory-segment effect gates now block Pick lift, + Place retract, and HandOver source release until fresh physical evidence + confirms the required acquisition or release. - Baseline: `main@bcccb787dcafdafd7b944ba210e5e85f9cd1d0cb` - Last updated: 2026-08-11 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), @@ -597,8 +599,9 @@ controller intent, not physical proof. passed the selected monitor. The target contract must treat contradictory evidence, including object-to-endpoint slip, as a real effect failure, invalidate the affected row's assumed relation, and enter bounded recovery instead of -repairing the scene. The runtime now exposes the active named motion phase, -observes phase-scoped held-object invariants from fresh physical evidence, and +repairing the scene. The runtime now exposes the active named trajectory +segment, observes segment-scoped held-object invariants from fresh physical +evidence, and applies removal-only ``StateDelta`` reconciliation to failed rows before any retry or recovery hand-off. The monitor publishes one current-observation outcome per physical expectation, including the stronger proof that every @@ -612,10 +615,23 @@ external recovery, but the core owns the removal-only invalidation delta and applies it before either path. Evidence that remains unresolved at the action deadline is reconciled fail-closed: any active verified state covered by the pending effect is removed before external recovery. Workflow-level -re-acquisition and blocking acquisition gates remain open; neither may repair -the scene implicitly. For handover, success transfers the verified relation -from source to destination while the destination remains physically closed. -Releasing the destination is a separate ``Place`` or ``Release`` semantic call. +re-acquisition remains open and may not repair the scene implicitly. + +Blocking physical-effect gates are enforced at named trajectory-segment +entries. ``Pick`` requires destination attachment before ``lift``; ``Place`` +requires source detachment before ``retract``; and ``HandOver`` requires +destination attachment before the source ``release`` segment. While a gate is +unresolved, the session does not advance its waypoint cursor and replays the +preceding command for the complete synchronized active cohort, so gripper +preload or open intent remains active under real dynamics. Gate success only +unlocks motion and never commits ``TaskState``; terminal effect verification +remains authoritative. Contradiction uses the enclosing action's bounded retry +policy, stale request IDs are rejected, and the action deadline covers gate +polling. Every gate owns a fresh monitor instance independent from the terminal +monitor and in-flight loss guard. For handover, terminal success transfers the +verified relation from source to destination while the destination remains +physically closed. Releasing the destination is a separate ``Place`` or +``Release`` semantic call. The first pure-dynamics rollout uses the staged **B** continuation policy. The standard simulation factory lowers both trajectory ``control_dt`` and runner @@ -1230,7 +1246,7 @@ The backend-neutral typed state expectations, evidence addresses and sources, pose/binary/scalar/joint evidence clauses, versioned monitor registry, profile-owned monitor selection, grounded Pick/Place/HandOver/articulation effects, row-local composite hysteresis kernel, canonical `SkillRuntime`, and -production simulation evidence ports are wired end to end. Phase-scoped +production simulation evidence ports are wired end to end. Segment-scoped held-object guard requests, live evidence collection, row-local symbolic invalidation, bounded Pick retry, and typed external-recovery hand-off are also implemented. Physical simulation acceptance is partial: Open Drawer and one @@ -1238,10 +1254,10 @@ cube Pick/Place/settle/validator cycle have completed. The embodiment-owned dual-UR5/PGI HandOver slice now completes Pick, transfer, terminal physical-effect verification, settling, and target validation through real contact dynamics. Per-expectation terminal outcomes, core-owned failure -invalidation, row-local retry/recovery decisions, and fail-closed deadline -reconciliation are implemented. Blocking acquisition gates, workflow-level -re-acquisition, fault-injection coverage, and the full repeated-cube run remain -validation or design work. +invalidation, row-local retry/recovery decisions, fail-closed deadline +reconciliation, and blocking named-segment effect gates are implemented. +Workflow-level re-acquisition, fault-injection coverage, and the full +repeated-cube run remain validation or design work. Deliverables: @@ -1497,12 +1513,12 @@ The design is complete when all of the following hold: synthetic attachment, freezes the object, or overrides its pose. - [ ] Physical held-object loss is observed as effect failure, invalidates the affected symbolic relation, and exercises bounded recovery rather than - being hidden by a simulator-side attachment. The phase-aware observation, + being hidden by a simulator-side attachment. The segment-aware observation, row-local core-owned invalidation, per-expectation terminal reconciliation, fail-closed deadline handling, bounded Pick/retained-Place - retry, and typed recovery boundary are implemented; blocking acquisition, - workflow-level re-acquisition, and real-simulation fault injection remain - open. + retry, typed recovery boundary, and blocking acquisition/release gates are + implemented; workflow-level re-acquisition and real-simulation fault + injection remain open. - [x] Repeated sub-threshold motion eventually publishes the correct scene revision. - [x] Custom actions have a documented and tested intentional hard-break diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst index ae595999d..7544f9d80 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 @@ -16,6 +16,7 @@ embodichain.lab.sim.atomic_actions ControlPartCommandProfile ActionControlOverrides ActionInvocation + PhaseEffectGateRequirement ResolvedActionRequest ActionOptions MotionPolicy @@ -72,6 +73,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 3637f2c71..dc8ea5192 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -866,8 +866,8 @@ and resets the history. Evidence exactly at the deadline is valid, while a due observation after the deadline is handled by session timeout without invoking the verifier. -The curated semantic runtime also installs phase-scoped, negative -held-object guards for named trajectory segments. Before a due command is +The curated semantic runtime also installs segment-scoped, negative held-object +guards for named trajectory segments. Before a due command is dispatched, `ExecutionRunner` passes a fresh observation and the current `HeldObjectGuardRequest` to its synchronous guard verifier. Each request has a single-use verification ID, the active waypoint/segment identity, and the @@ -878,10 +878,27 @@ before retrying or emitting `RECOVERY_REQUIRED`. Unavailable or unresolved evidence does not count as a physical contradiction, and the guard verifier is not invoked after the authoritative action deadline. -The current guard is observational and negative; a blocking positive -acquisition gate, outcome-aware terminal reconciliation, and workflow-level -re-acquisition remain separate policies. Neither the monitor nor runtime +The curated `Pick`, `Place`, and `HandOver` paths additionally install blocking +positive-effect gates at named trajectory-segment entries. Pick must verify the +destination attachment before `lift`, Place must verify source detachment before +`retract`, and HandOver must verify the destination attachment before source +`release`. The compiler creates a monitor instance for each gate independently +from both the terminal monitor and negative held-object guard. + +`ExecutionSession` exposes a correlated `PhaseEffectGateRequest` at the segment +boundary. While its result remains unresolved, the waypoint cursor does not +advance and the preceding command is replayed for the complete synchronized +active cohort. This preserves gripper preload or open intent instead of +replacing it with an observed-position hold. A successful +`PhaseEffectGateResult` only unlocks the segment; it does not commit +`TaskState`. Contradiction uses the enclosing action's bounded retry policy, +request IDs are single-use, and the action deadline covers all polling. Calling +`run_until_blocked()` without a gate verifier returns this boundary for an +external verifier. + +The guards and gates are observational. Neither a monitor nor the runtime creates a simulator attachment, freezes an object, or overrides its pose. +Workflow-level re-acquisition remains a separate recovery policy. ## Action Agent integration diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 86530563d..e2902608c 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -571,6 +571,34 @@ same invocation remains physically valid. Other failed rows enter external recovery after selected invalidation. Unresolved evidence at the action deadline is reconciled fail-closed when covered verified state is still active. +Trajectory-segment effect gates +------------------------------- + +An invocation may declare a +:class:`~embodichain.lab.sim.atomic_actions.PhaseEffectGateRequirement` for a +named, non-initial trajectory segment. The execution session then exposes a +:class:`~embodichain.lab.sim.atomic_actions.PhaseEffectGateRequest` immediately +before the first frame of that segment. Curated semantic calls install these +automatically: Pick gates ``lift`` on destination attachment, Place gates +``retract`` on source detachment, and HandOver gates source ``release`` on +destination attachment. + +Supply ``phase_effect_gate_verifier(context, request)`` to ``runner.step()`` or +``runner.run_until_blocked()``. It runs on a fresh due-cycle observation and +returns a correlated +:class:`~embodichain.lab.sim.atomic_actions.PhaseEffectGateResult`. If neither +the success nor failure mask selects every remaining active row, the session +keeps the whole cohort at the boundary and resends the command immediately +before the gated segment. This preserves a close/open command and its physical +preload; it is not an observed-position hold. + +Gate success only permits the next command and does not update ``TaskState``. +The terminal effect verifier still owns the semantic commit. A contradictory +row may consume the enclosing action's retry budget; a row outside the result's +``retry_mask`` requires external recovery. The gate shares the action timeout, +and each consumed observation replaces its request ID. Without a gate verifier, +``run_until_blocked()`` returns the pending boundary for asynchronous handling. + Adding an action ---------------- diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index a5ea5c0c2..82a247584 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -68,6 +68,8 @@ ExecutionTick, HeldObjectGuardRequest, HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) from .goals import ( ActionGoal, @@ -76,7 +78,12 @@ SceneArticulationOperationGeometry, SceneEntityPose, ) -from .invocation import ActionInvocation, ActionOptions, ResolvedActionRequest +from .invocation import ( + ActionInvocation, + ActionOptions, + PhaseEffectGateRequirement, + ResolvedActionRequest, +) from .plans import ( ActionPlan, CompiledTrajectory, @@ -200,6 +207,7 @@ HeldObjectGuardVerifier, MonotonicExecutionClock, ObservationProvider, + PhaseEffectGateVerifier, RunnerStatus, RunnerStep, RunnerStepCallback, @@ -281,6 +289,10 @@ "ExecutionTick", "HeldObjectGuardRequest", "HeldObjectGuardResult", + "PhaseEffectGateRequest", + "PhaseEffectGateRequirement", + "PhaseEffectGateResult", + "PhaseEffectGateVerifier", "HeldObjectGuardVerifier", "EndpointTrackingChannelBinding", "EndpointTrackingFeedbackAddress", diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index 1c39615f1..226182c27 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -381,6 +381,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 43011ffd3..0835ddb92 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -26,7 +26,11 @@ import torch from .effects import StateDelta -from .invocation import ActionInvocation, ResolvedActionRequest +from .invocation import ( + ActionInvocation, + PhaseEffectGateRequirement, + ResolvedActionRequest, +) from .bindings import RuntimeEndpointTarget from .plans import ( ActionPlan, @@ -74,6 +78,9 @@ class ExecutionEventKind(str, Enum): EFFECT_VERIFICATION_REQUIRED = "effect_verification_required" EFFECT_VERIFICATION_FAILED = "effect_verification_failed" EFFECT_VERIFICATION_TIMEOUT = "effect_verification_timeout" + PHASE_EFFECT_GATE_REQUIRED = "phase_effect_gate_required" + PHASE_EFFECT_GATE_SATISFIED = "phase_effect_gate_satisfied" + PHASE_EFFECT_GATE_FAILED = "phase_effect_gate_failed" HELD_OBJECT_LOST = "held_object_lost" ACTION_RETRY = "action_retry" ACTION_COMPLETED = "action_completed" @@ -511,6 +518,167 @@ def __post_init__(self) -> None: ) +@dataclass(frozen=True, slots=True, eq=False) +class PhaseEffectGateRequest: + """Correlate a blocking physical-effect check with a segment entry. + + The action's preceding command remains active while the gate is unresolved. + A gate is scoped to the enclosing action attempt and does not create a + separate planning, recovery, or timeout budget. + + Args: + verification_id: Session-local single-use request identity. + gate_id: Invocation-local stable gate identity. + skill_id: Registered action skill identity. + invocation_id: Optional logical invocation correlation identity. + invocation_revision: Active invocation revision. + invocation_index: Active invocation position in the session. + attempt_generation: Installed action-plan attempt generation. + next_waypoint_index: First command frame blocked by the gate. + segment_name: Named trajectory segment blocked by the gate. + requested_at: Request creation time in the observation timestamp domain. + deadline: Enclosing action deadline in that same timestamp domain. + env_mask: Active rows that must satisfy the gate together. + """ + + verification_id: int + gate_id: str + skill_id: str + invocation_id: str | None + invocation_revision: int + invocation_index: int + attempt_generation: int + next_waypoint_index: int + segment_name: str + requested_at: float + deadline: float + env_mask: torch.Tensor + + def __post_init__(self) -> None: + for name in ( + "verification_id", + "invocation_revision", + "invocation_index", + "attempt_generation", + "next_waypoint_index", + ): + value = getattr(self, name) + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a non-negative integer.") + for name in ("gate_id", "skill_id", "segment_name"): + value = getattr(self, name) + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{name} must be a non-empty string without outer whitespace." + ) + if self.invocation_id is not None and ( + type(self.invocation_id) is not str or not self.invocation_id + ): + raise ValueError("invocation_id must be a non-empty string or None.") + if not math.isfinite(self.requested_at) or self.requested_at < 0.0: + raise ValueError("requested_at must be finite and non-negative.") + if not math.isfinite(self.deadline) or self.deadline < self.requested_at: + raise ValueError( + "deadline must be finite and no earlier than requested_at." + ) + if not isinstance(self.env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") + if self.env_mask.dtype != torch.bool or self.env_mask.dim() != 1: + raise ValueError("env_mask must be a one-dimensional bool tensor.") + if not self.env_mask.any(): + raise ValueError("env_mask must contain at least one gated row.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + + def snapshot(self) -> PhaseEffectGateRequest: + """Return an independently owned gate request.""" + return PhaseEffectGateRequest( + verification_id=self.verification_id, + gate_id=self.gate_id, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + invocation_index=self.invocation_index, + attempt_generation=self.attempt_generation, + next_waypoint_index=self.next_waypoint_index, + segment_name=self.segment_name, + requested_at=self.requested_at, + deadline=self.deadline, + env_mask=self.env_mask, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class PhaseEffectGateResult: + """Current-observation decision for one blocking segment-entry gate. + + Rows absent from both decision masks remain unresolved. ``retry_mask`` is + a subset of failed rows for which replaying the enclosing action remains + valid; no gate outcome mutates verified task state. + + Args: + verification_id: Identity copied from the consumed gate request. + gate_id: Stable gate identity copied from the request. + attempt_generation: Action attempt copied from the request. + invocation_index: Session invocation index copied from the request. + next_waypoint_index: Blocked waypoint copied from the request. + success_mask: Rows whose current evidence satisfies the gate. + failure_mask: Rows whose current evidence contradicts the gate. + retry_mask: Failed rows allowed to retry the enclosing action. + message: Optional physical-failure diagnostic. + """ + + verification_id: int + gate_id: str + attempt_generation: int + invocation_index: int + next_waypoint_index: int + success_mask: torch.Tensor + failure_mask: torch.Tensor + retry_mask: torch.Tensor + message: str = "" + + def __post_init__(self) -> None: + for name in ( + "verification_id", + "attempt_generation", + "invocation_index", + "next_waypoint_index", + ): + value = getattr(self, name) + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a non-negative integer.") + if ( + type(self.gate_id) is not str + or not self.gate_id + or self.gate_id != self.gate_id.strip() + ): + raise ValueError( + "gate_id must be a non-empty string without outer whitespace." + ) + masks = (self.success_mask, self.failure_mask, self.retry_mask) + for name, value in zip( + ("success_mask", "failure_mask", "retry_mask"), + masks, + strict=True, + ): + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if any(value.shape != masks[0].shape for value in masks[1:]): + raise ValueError("Phase-effect gate masks must have equal shapes.") + if any(value.device != masks[0].device for value in masks[1:]): + raise ValueError("Phase-effect gate masks must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("Gate success and failure masks must not overlap.") + if (self.retry_mask & ~self.failure_mask).any(): + raise ValueError("retry_mask must be a subset of failure_mask.") + if type(self.message) is not str: + raise TypeError("message must be a string.") + for name in ("success_mask", "failure_mask", "retry_mask"): + object.__setattr__(self, name, getattr(self, name).clone()) + + @dataclass(frozen=True, slots=True, eq=False) class HeldObjectGuardRequest: """Describe the next in-flight command boundary for held-object checks. @@ -707,6 +875,7 @@ class ExecutionTick: events: tuple[ExecutionEvent, ...] task_state: TaskState pending_effect: EffectVerificationRequest | None = None + pending_phase_effect_gate: PhaseEffectGateRequest | None = None def __post_init__(self) -> None: if self.eligible_mask.dtype != torch.bool or self.eligible_mask.dim() != 1: @@ -717,6 +886,21 @@ def __post_init__(self) -> None: raise TypeError( "pending_effect must be an EffectVerificationRequest or None." ) + if self.pending_phase_effect_gate is not None and not isinstance( + self.pending_phase_effect_gate, + PhaseEffectGateRequest, + ): + raise TypeError( + "pending_phase_effect_gate must be a PhaseEffectGateRequest or None." + ) + if ( + self.pending_effect is not None + and self.pending_phase_effect_gate is not None + ): + raise ValueError( + "Terminal effect verification and a phase-effect gate cannot be " + "pending together." + ) if self.command is not None and not isinstance( self.command, RuntimeCommandFrame, @@ -738,6 +922,12 @@ def __post_init__(self) -> None: "pending_effect", self.pending_effect.snapshot(), ) + if self.pending_phase_effect_gate is not None: + object.__setattr__( + self, + "pending_phase_effect_gate", + self.pending_phase_effect_gate.snapshot(), + ) hold_targets: list[RuntimeEndpointTarget] = [] for target in self.hold_targets: snapshot = target.snapshot() @@ -828,6 +1018,10 @@ def __init__( self._effect_requested_at: float | None = None self._next_effect_verification_id = 0 self._next_held_object_guard_verification_id = 0 + self._pending_phase_effect_gate: PhaseEffectGateRequest | None = None + self._satisfied_phase_effect_gates: set[str] = set() + self._reported_phase_effect_gates: set[str] = set() + self._next_phase_effect_gate_verification_id = 0 self._plan_attempt_records: list[_ExecutionPlanAttemptRecord] = [] self._status = ( ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED @@ -874,6 +1068,17 @@ def pending_effect(self) -> EffectVerificationRequest | None: """Owned snapshot of the current effect boundary, when present.""" return None if self._pending_effect is None else self._pending_effect.snapshot() + @property + def phase_effect_gate_request(self) -> PhaseEffectGateRequest | None: + """Return the blocking gate at the next trajectory-segment entry. + + Returns: + Owned request snapshot, or ``None`` when the next command is not + blocked by a physical-effect gate. + """ + request = self._phase_effect_gate_request() + return None if request is None else request.snapshot() + @property def held_object_guard_request(self) -> HeldObjectGuardRequest | None: """Describe the phase that must be checked before the next command. @@ -942,6 +1147,9 @@ def deactivate_rows( ) else: self._pending_effect = None + if self._pending_phase_effect_gate is not None: + self._pending_phase_effect_gate = None + self._next_phase_effect_gate_verification_id += 1 terminal_event = self._update_terminal_status() if terminal_event is not None: self._queued_events.append(terminal_event) @@ -995,7 +1203,11 @@ def _prepare_revision( raise TypeError("invocation must be an ActionInvocation.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - if self._pending_effect is not None or self._effect_failures.any(): + if ( + self._pending_effect is not None + or self._pending_phase_effect_gate is not None + or self._effect_failures.any() + ): raise RuntimeError( "Cannot revise while physical-effect resolution is pending; " "resolve it or cancel and start a new invocation." @@ -1017,7 +1229,11 @@ def _install_prepared_revision( raise TypeError("replacement must be a ResolvedActionRequest.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - if self._pending_effect is not None or self._effect_failures.any(): + if ( + self._pending_effect is not None + or self._pending_phase_effect_gate is not None + or self._effect_failures.any() + ): raise RuntimeError( "Cannot revise while physical-effect resolution is pending; " "resolve it or cancel and start a new invocation." @@ -1128,6 +1344,7 @@ def tick( context: PlanningContext, *, effect_result: EffectVerificationResult | None = None, + phase_effect_gate_result: PhaseEffectGateResult | None = None, held_object_guard_result: HeldObjectGuardResult | None = None, ) -> ExecutionTick: """Advance execution by one observation/command cycle. @@ -1137,6 +1354,8 @@ def tick( state is replaced by the session's verified task state. effect_result: Optional correlated semantic-effect result for an action waiting at its terminal waypoint. + phase_effect_gate_result: Optional correlated physical-effect + decision for a blocked trajectory-segment entry. held_object_guard_result: Optional correlated in-flight held-object loss result for the current waypoint phase. ``None`` means the verifier found no applicable guard for this phase or no result @@ -1159,6 +1378,38 @@ def tick( "effect_result verification_id does not match the pending " "effect boundary." ) + phase_gate_request = self._phase_effect_gate_request() + if phase_effect_gate_result is not None: + if type(phase_effect_gate_result) is not PhaseEffectGateResult: + raise TypeError( + "phase_effect_gate_result must be exactly " + "PhaseEffectGateResult or None." + ) + if phase_gate_request is None: + raise ValueError("No phase-effect gate is awaiting verification.") + if phase_effect_gate_result.verification_id != ( + phase_gate_request.verification_id + ): + raise ValueError( + "phase_effect_gate_result verification_id does not match " + "the pending gate." + ) + for name in ( + "gate_id", + "attempt_generation", + "invocation_index", + "next_waypoint_index", + ): + if getattr(phase_effect_gate_result, name) != getattr( + phase_gate_request, + name, + ): + raise ValueError( + f"phase_effect_gate_result {name} does not match the " + "pending gate." + ) + self._next_phase_effect_gate_verification_id += 1 + self._pending_phase_effect_gate = None guard_request = self._held_object_guard_request() if held_object_guard_result is not None: if type(held_object_guard_result) is not HeldObjectGuardResult: @@ -1194,6 +1445,17 @@ def tick( return self._tick_result(command=None, events=events) assert self._plan is not None + if phase_effect_gate_result is not None: + assert phase_gate_request is not None + events.extend( + self._apply_phase_effect_gate_result( + phase_effect_gate_result, + phase_gate_request, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + assert self._plan is not None if held_object_guard_result is not None: assert guard_request is not None events.extend( @@ -1373,6 +1635,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) @@ -1548,6 +1817,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 ( @@ -1580,6 +1850,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( @@ -1602,6 +1875,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, @@ -2339,12 +2635,177 @@ def _batched_entity_pose(self, state: EntityState) -> torch.Tensor: raise ValueError("Scene entity pose batch does not match the session.") return pose + def _phase_effect_gate_requirement( + self, + ) -> PhaseEffectGateRequirement | None: + """Resolve a gate exactly at the next named segment's first frame.""" + if ( + self._status is not ExecutionStatus.RUNNING + or self._plan is None + or self._pending_effect is not None + or self._effect_failures.any() + or self._plan.commands.frame_count == 0 + or self._waypoint_index >= self._plan.commands.frame_count + ): + return None + segment = self._plan.segment_at(self._waypoint_index) + if self._waypoint_index != segment.start: + return None + request = self._requests[self._invocation_index] + return next( + ( + value + for value in request.phase_effect_gates + if value.segment_name == segment.name + and value.gate_id not in self._satisfied_phase_effect_gates + ), + None, + ) + + def _phase_effect_gate_request(self) -> PhaseEffectGateRequest | None: + """Build or retain the current blocking segment-entry gate request.""" + requirement = self._phase_effect_gate_requirement() + if requirement is None: + self._pending_phase_effect_gate = None + return None + assert self._plan is not None + env_mask = self._pending & self._plan.plan_success + if not env_mask.any(): + self._pending_phase_effect_gate = None + return None + current = self._pending_phase_effect_gate + if ( + current is not None + and current.gate_id == requirement.gate_id + and current.attempt_generation == self._attempt_generation + and current.next_waypoint_index == self._waypoint_index + and torch.equal(current.env_mask, env_mask) + ): + return current + invocation = self._requests[self._invocation_index] + deadline = self._action_started_at + self._plan.recovery_policy.action_timeout + current = PhaseEffectGateRequest( + verification_id=self._next_phase_effect_gate_verification_id, + gate_id=requirement.gate_id, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + invocation_index=self._invocation_index, + attempt_generation=self._attempt_generation, + next_waypoint_index=self._waypoint_index, + segment_name=requirement.segment_name, + requested_at=min(self._context.robot.timestamp, deadline), + deadline=deadline, + env_mask=env_mask, + ) + self._pending_phase_effect_gate = current + return current + + def _phase_effect_gate_required_events( + self, + request: PhaseEffectGateRequest, + ) -> list[ExecutionEvent]: + """Emit the gate boundary once per installed action attempt.""" + if request.gate_id in self._reported_phase_effect_gates: + return [] + self._reported_phase_effect_gates.add(request.gate_id) + return [ + self._event( + ExecutionEventKind.PHASE_EFFECT_GATE_REQUIRED, + request.env_mask, + f"Physical-effect gate {request.gate_id!r} blocks segment " + f"{request.segment_name!r} until current evidence succeeds.", + ) + ] + + def _apply_phase_effect_gate_result( + self, + result: PhaseEffectGateResult, + request: PhaseEffectGateRequest, + ) -> list[ExecutionEvent]: + """Resolve one gate observation without mutating verified task state.""" + success = self._normalize_mask( + result.success_mask, + "phase_effect_gate_result.success_mask", + ) + failure = self._normalize_mask( + result.failure_mask, + "phase_effect_gate_result.failure_mask", + ) + retry = self._normalize_mask( + result.retry_mask, + "phase_effect_gate_result.retry_mask", + ) + request_mask = request.env_mask.to(self._eligible.device) + if ((success | failure | retry) & ~request_mask).any(): + raise ValueError( + "Phase-effect gate result masks must be subsets of the pending " + "request env_mask." + ) + events = self._phase_effect_gate_required_events(request) + message = result.message or ( + f"Physical evidence contradicted gate {request.gate_id!r} before " + f"segment {request.segment_name!r}." + ) + non_retry = failure & ~retry + if non_retry.any(): + self._eligible &= ~non_retry + self._pending &= ~non_retry + self._last_command_mask &= ~non_retry + events.extend( + ( + self._event( + ExecutionEventKind.PHASE_EFFECT_GATE_FAILED, + non_retry, + message, + ), + self._event( + ExecutionEventKind.RECOVERY_REQUIRED, + non_retry, + "The failed segment-entry gate requires recovery outside " + "the current action retry policy.", + ), + ) + ) + previous_generation = self._attempt_generation + if retry.any(): + events.extend( + self._attempt_action_retry( + retry, + ExecutionEventKind.PHASE_EFFECT_GATE_FAILED, + message, + ) + ) + else: + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) + if ( + self._status is not ExecutionStatus.RUNNING + or self._attempt_generation != previous_generation + ): + return events + assert self._plan is not None + remaining = request_mask & self._pending & self._plan.plan_success + if remaining.any() and torch.equal(success & remaining, remaining): + self._satisfied_phase_effect_gates.add(request.gate_id) + events.append( + self._event( + ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED, + remaining, + f"Physical-effect gate {request.gate_id!r} released segment " + f"{request.segment_name!r}.", + ) + ) + return events + def _held_object_guard_request(self) -> HeldObjectGuardRequest | None: """Build the current command-phase held-object guard request.""" if ( self._status is not ExecutionStatus.RUNNING or self._plan is None or self._pending_effect is not None + or self._phase_effect_gate_request() is not None or self._effect_failures.any() or self._plan.commands.frame_count == 0 ): @@ -2678,6 +3139,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( @@ -2695,6 +3157,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, @@ -2703,6 +3168,7 @@ def _tick_result( events=tuple(events), task_state=self._task_state, pending_effect=self._pending_effect, + pending_phase_effect_gate=phase_gate, ) @@ -2718,4 +3184,6 @@ def _tick_result( "ExecutionTick", "HeldObjectGuardRequest", "HeldObjectGuardResult", + "PhaseEffectGateRequest", + "PhaseEffectGateResult", ] diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index 26acdc5b8..31a038d7c 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -46,7 +46,39 @@ class ActionOptions: OptionsT = TypeVar("OptionsT", bound=ActionOptions) -def _goal_snapshot_memo(goal: object) -> dict[int, object]: +@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] = {} visited: set[int] = set() @@ -109,6 +141,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.""" @@ -134,6 +169,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 ): @@ -146,6 +197,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) @@ -165,6 +221,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 @@ -179,6 +236,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 ( @@ -203,6 +276,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]: @@ -214,6 +292,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, @@ -225,5 +304,6 @@ def snapshot(self) -> ResolvedActionRequest[GoalT, OptionsT]: "ActionOptions", "GoalT", "OptionsT", + "PhaseEffectGateRequirement", "ResolvedActionRequest", ] diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 0c7601fad..bd231598b 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -38,6 +38,8 @@ ExecutionTick, HeldObjectGuardRequest, HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) from .invocation import ActionInvocation, ResolvedActionRequest from .runtime_commands import RuntimeCommandFrame @@ -318,6 +320,12 @@ def is_waiting(self) -> bool: ] """Synchronous phase-aware held-object verifier for one due command cycle.""" +PhaseEffectGateVerifier = Callable[ + [PlanningContext, PhaseEffectGateRequest], + PhaseEffectGateResult, +] +"""Synchronous verifier for one blocking trajectory-segment entry gate.""" + RunnerStepCallback = Callable[[RunnerStep], None] """Optional observer called after every blocking runner-loop iteration.""" @@ -485,6 +493,8 @@ def step( *, effect_result: EffectVerificationResult | None = None, effect_verifier: EffectVerifier | None = None, + phase_effect_gate_result: PhaseEffectGateResult | None = None, + phase_effect_gate_verifier: PhaseEffectGateVerifier | None = None, held_object_guard_verifier: HeldObjectGuardVerifier | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. @@ -498,6 +508,11 @@ def step( and before the session consumes the result. It is not called after the request deadline. Mutually exclusive with ``effect_result``. + phase_effect_gate_result: Optional externally produced result for + the current blocking trajectory-segment entry gate. + phase_effect_gate_verifier: Optional synchronous verifier for the + current gate. It runs on a fresh due-cycle observation and is + mutually exclusive with ``phase_effect_gate_result``. held_object_guard_verifier: Optional synchronous phase-aware verifier. It receives a fresh observation and the current command-phase request before :meth:`ExecutionSession.tick` and @@ -512,12 +527,24 @@ def step( raise ValueError( "effect_result and effect_verifier are mutually exclusive." ) + if ( + phase_effect_gate_result is not None + and phase_effect_gate_verifier is not None + ): + raise ValueError( + "phase_effect_gate_result and phase_effect_gate_verifier are " + "mutually exclusive." + ) if effect_verifier is not None and not callable(effect_verifier): raise TypeError("effect_verifier must be callable or None.") if held_object_guard_verifier is not None and not callable( held_object_guard_verifier ): raise TypeError("held_object_guard_verifier must be callable or None.") + if phase_effect_gate_verifier is not None and not callable( + phase_effect_gate_verifier + ): + raise TypeError("phase_effect_gate_verifier must be callable or None.") now = self._clock_now() if self._status is not RunnerStatus.RUNNING: return self._result(timestamp=now) @@ -573,6 +600,29 @@ def step( context=context, ) + phase_effect_gate_request = self._session.phase_effect_gate_request + if ( + phase_effect_gate_verifier is not None + and phase_effect_gate_request is not None + and context.robot.timestamp <= phase_effect_gate_request.deadline + ): + try: + phase_effect_gate_result = phase_effect_gate_verifier( + context, + phase_effect_gate_request, + ) + if type(phase_effect_gate_result) is not PhaseEffectGateResult: + raise TypeError( + "PhaseEffectGateVerifier must return exactly " + "PhaseEffectGateResult." + ) + except Exception as exc: + return self._fail( + "Phase-effect gate verifier failed: " + f"{type(exc).__name__}: {exc}", + context=context, + ) + held_object_guard_result: HeldObjectGuardResult | None = None held_object_guard_request = self._session.held_object_guard_request if ( @@ -604,6 +654,7 @@ def step( tick = self._session.tick( context, effect_result=effect_result, + phase_effect_gate_result=phase_effect_gate_result, held_object_guard_result=held_object_guard_result, ) context = self._session.latest_context @@ -755,6 +806,7 @@ def run_until_blocked( self, *, effect_verifier: EffectVerifier | None = None, + phase_effect_gate_verifier: PhaseEffectGateVerifier | None = None, held_object_guard_verifier: HeldObjectGuardVerifier | None = None, on_step: RunnerStepCallback | None = None, max_steps: int = 100_000, @@ -766,6 +818,8 @@ def run_until_blocked( due-cycle observations while effect verification is pending. Without one, the method returns the running boundary so the caller can verify externally. + phase_effect_gate_verifier: Optional synchronous callback used on + fresh observations while a trajectory-segment entry is gated. held_object_guard_verifier: Optional synchronous phase-aware held-object verifier used before every due command cycle. on_step: Optional callback for tracing or tutorial visualization. @@ -788,6 +842,7 @@ def run_until_blocked( for _ in range(max_steps): result = self.step( effect_verifier=effect_verifier, + phase_effect_gate_verifier=phase_effect_gate_verifier, held_object_guard_verifier=held_object_guard_verifier, ) if on_step is not None: @@ -808,6 +863,12 @@ def run_until_blocked( ) if verification_required and effect_verifier is None: return result + gate_required = ( + result.tick is not None + and result.tick.pending_phase_effect_gate is not None + ) + if gate_required and phase_effect_gate_verifier is None: + return result if result.wait_duration > 0.0: try: self._clock.sleep(result.wait_duration) @@ -1004,6 +1065,7 @@ def _result( "HeldObjectGuardVerifier", "MonotonicExecutionClock", "ObservationProvider", + "PhaseEffectGateVerifier", "RunnerStatus", "RunnerStep", "RunnerStepCallback", diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 2b192950e..04c168dc1 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -35,6 +35,7 @@ from .compiler import ( AnalyzedSemanticCall, GroundedHeldObjectGuard, + GroundedPhaseEffectGate, GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, @@ -272,6 +273,7 @@ "FORCE_EFFECT_CHANNEL", "GRASP_AFFORDANCE_CAPABILITY", "GroundedHeldObjectGuard", + "GroundedPhaseEffectGate", "GroundedSemanticCall", "HeldObjectRelation", "HeldObjectStateExpectation", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index f0c094867..5ad44e4bb 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -43,6 +43,7 @@ PlaceOptions, OperateArticulationGoal, OperateArticulationOptions, + PhaseEffectGateRequirement, PlanningContext, PoseGoalValue, SceneArticulationOperationGeometry, @@ -513,6 +514,55 @@ def task_state_key(self) -> str: return expectation.task_state_key +@dataclass(frozen=True, slots=True) +class GroundedPhaseEffectGate: + """One independently monitored physical-effect segment-entry gate. + + Args: + gate_id: Invocation-local stable gate identity. + segment_name: Named trajectory segment blocked by the gate. + effect_spec: Single-expectation physical observation contract. + effect_monitor: Fresh monitor instance owned only by this gate. + retry_action: Whether contradiction may retry the enclosing action. + """ + + gate_id: str + segment_name: str + effect_spec: SemanticEffectSpec + effect_monitor: EffectMonitor = field(repr=False, compare=False) + retry_action: bool = True + + def __post_init__(self) -> None: + _validate_identifier(self.gate_id, field_name="gate_id") + _validate_identifier(self.segment_name, field_name="segment_name") + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec.") + physical_ids = {clause.expectation_id for clause in self.effect_spec.clauses} + if ( + len(self.effect_spec.state_expectations) != 1 + or len(physical_ids) != 1 + or next(iter(physical_ids)) + != self.effect_spec.state_expectations[0].expectation_id + ): + raise ValueError( + "A phase-effect gate must own exactly one physically observed " + "state expectation." + ) + if not isinstance(self.effect_monitor, EffectMonitor): + raise TypeError("effect_monitor must be an EffectMonitor.") + if type(self.retry_action) is not bool: + raise TypeError("retry_action must be a bool.") + object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) + + @property + def requirement(self) -> PhaseEffectGateRequirement: + """Return the core-owned blocking requirement for this monitor.""" + return PhaseEffectGateRequirement( + gate_id=self.gate_id, + segment_name=self.segment_name, + ) + + @dataclass(frozen=True, slots=True, init=False) class GroundedSemanticCall: """Call lowered from the latest observed context.""" @@ -522,6 +572,7 @@ class GroundedSemanticCall: effect_spec: SemanticEffectSpec | None effect_monitor: EffectMonitor | None = field(repr=False, compare=False) effect_guards: tuple[GroundedHeldObjectGuard, ...] + effect_gates: tuple[GroundedPhaseEffectGate, ...] _eligible_mask: torch.Tensor = field(repr=False, compare=False) def __init__(self, *args: object, **kwargs: object) -> None: @@ -541,6 +592,7 @@ def _create( effect_spec: SemanticEffectSpec | None, effect_monitor: EffectMonitor | None, effect_guards: tuple[GroundedHeldObjectGuard, ...], + effect_gates: tuple[GroundedPhaseEffectGate, ...], eligible_mask: torch.Tensor, ) -> GroundedSemanticCall: """Create one compiler-owned grounded result.""" @@ -550,6 +602,7 @@ def _create( object.__setattr__(instance, "effect_spec", effect_spec) object.__setattr__(instance, "effect_monitor", effect_monitor) object.__setattr__(instance, "effect_guards", tuple(effect_guards)) + object.__setattr__(instance, "effect_gates", tuple(effect_gates)) object.__setattr__(instance, "_eligible_mask", eligible_mask.clone()) instance.__post_init__() return instance @@ -586,6 +639,28 @@ def __post_init__(self) -> None: if guards and self.effect_spec is None: raise ValueError("Held-object guards require a terminal effect spec.") object.__setattr__(self, "effect_guards", guards) + gates = tuple(self.effect_gates) + if not all(type(value) is GroundedPhaseEffectGate for value in gates): + raise TypeError( + "effect_gates must contain exact GroundedPhaseEffectGate values." + ) + gate_ids = [value.gate_id for value in gates] + gate_segments = [value.segment_name for value in gates] + if len(set(gate_ids)) != len(gate_ids): + raise ValueError("Grounded phase-effect gate IDs must be unique.") + if len(set(gate_segments)) != len(gate_segments): + raise ValueError( + "At most one grounded phase-effect gate may block each segment." + ) + if tuple(value.requirement for value in gates) != ( + self.invocation.phase_effect_gates + ): + raise ValueError( + "Grounded phase-effect gates must match invocation requirements." + ) + if gates and self.effect_spec is None: + raise ValueError("Phase-effect gates require a terminal effect spec.") + object.__setattr__(self, "effect_gates", gates) if not isinstance(self._eligible_mask, torch.Tensor): raise TypeError("eligible_mask must be a torch.Tensor.") if self._eligible_mask.dtype != torch.bool or self._eligible_mask.dim() != 1: @@ -1195,12 +1270,23 @@ def ground( context, path=(*path, call_index, "effect_guards"), ) + effect_gates = self._ground_phase_effect_gates( + analyzed, + effect_spec, + path=(*path, call_index, "effect_gates"), + ) + if effect_gates: + invocation = replace( + invocation, + phase_effect_gates=tuple(value.requirement for value in effect_gates), + ) return GroundedSemanticCall._create( analyzed=analyzed, invocation=invocation, effect_spec=effect_spec, effect_monitor=effect_monitor, effect_guards=effect_guards, + effect_gates=effect_gates, eligible_mask=eligible, ) @@ -1819,6 +1905,90 @@ def _ground_effect_spec( clauses=tuple(clauses), ) + def _ground_phase_effect_gates( + self, + analyzed: AnalyzedSemanticCall, + effect_spec: SemanticEffectSpec | None, + *, + path: tuple[PathPart, ...], + ) -> tuple[GroundedPhaseEffectGate, ...]: + """Create blocking acquisition/release gates for built-in semantics.""" + monitor_ref = analyzed.effect_monitor_ref + if effect_spec is None or monitor_ref is None: + return () + call = analyzed.call + if type(call) is Pick: + definitions = (("destination_acquired", "lift", "destination"),) + elif type(call) is Place: + definitions = (("source_released", "retract", "source"),) + elif type(call) is HandOver: + definitions = (("destination_acquired", "release", "destination"),) + else: + return () + + gates: list[GroundedPhaseEffectGate] = [] + for gate_id, segment_name, expectation_id in definitions: + gate_spec = self._single_held_expectation_effect_spec( + effect_spec, + expectation_id=expectation_id, + ) + try: + monitor = self._effect_monitor_registry.create( + gate_spec, + monitor_ref, + ) + except (KeyError, TypeError, ValueError) as exc: + raise _diagnostic( + "effect_gate_monitor_creation_failed", + (*path, gate_id), + f"Could not create phase-effect gate monitor: {exc}", + ) from exc + gates.append( + GroundedPhaseEffectGate( + gate_id=gate_id, + segment_name=segment_name, + effect_spec=gate_spec, + effect_monitor=monitor, + retry_action=True, + ) + ) + return tuple(gates) + + @staticmethod + def _single_held_expectation_effect_spec( + terminal_spec: SemanticEffectSpec, + *, + expectation_id: str, + ) -> SemanticEffectSpec: + """Project one terminal held relation into an independent gate spec.""" + expectation = terminal_spec.state_expectation(expectation_id) + if type(expectation) is not HeldObjectStateExpectation: + raise TypeError("Phase-effect gates require held-object expectations.") + clauses = tuple( + clause + for clause in terminal_spec.clauses + if clause.expectation_id == expectation_id + ) + if not clauses: + raise ValueError( + f"Held-object expectation {expectation_id!r} has no physical clauses." + ) + effect_kind = ( + SemanticEffectKind.ATTACH + if expectation.relation is HeldObjectRelation.ATTACHED + else SemanticEffectKind.RELEASE + ) + return SemanticEffectSpec( + semantic_id=terminal_spec.semantic_id, + effect_kind=effect_kind, + skill_id=terminal_spec.skill_id, + invocation_id=terminal_spec.invocation_id, + invocation_revision=terminal_spec.invocation_revision, + env_ids=terminal_spec.env_ids, + state_expectations=(expectation,), + clauses=clauses, + ) + def _ground_held_object_guards( self, analyzed: AnalyzedSemanticCall, @@ -2445,6 +2615,7 @@ def _broadcast_joint_position( __all__ = [ "AnalyzedSemanticCall", "GroundedHeldObjectGuard", + "GroundedPhaseEffectGate", "GroundedSemanticCall", "HandOverPoseProvider", "HandOverPoseTargets", diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index 358b925f3..9995b62cb 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -38,6 +38,8 @@ ExecutionPlanAttempt, HeldObjectGuardRequest, HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) from ..atomic_actions.plans import TrajectorySegment from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy @@ -61,6 +63,7 @@ from .calls import HandOver, Pick, Place, SemanticCallSpec from .compiler import ( GroundedHeldObjectGuard, + GroundedPhaseEffectGate, HeldObjectGuardBaseline, SemanticSkillCompiler, ) @@ -71,6 +74,8 @@ EffectMonitor, EffectMonitorDecision, EffectMonitorRef, + HeldObjectRelation, + HeldObjectStateExpectation, JointStateEvidenceBatch, PoseRelationEvidenceBatch, ScalarEffectEvidenceBatch, @@ -962,6 +967,7 @@ class SkillEffectTrace: evidence: Mapping[str, EffectEvidenceBatch] boundary_kind: str = "terminal" guard_id: str | None = None + gate_id: str | None = None segment_name: str | None = None def __post_init__(self) -> None: @@ -971,20 +977,43 @@ def __post_init__(self) -> None: raise ValueError("verification_id must be a non-negative integer.") if type(self.observation_revision) is not int or self.observation_revision < 0: raise ValueError("observation_revision must be non-negative.") - if self.boundary_kind not in {"terminal", "in_flight_guard"}: - raise ValueError("boundary_kind must be 'terminal' or 'in_flight_guard'.") - for name in ("guard_id", "segment_name"): + if self.boundary_kind not in { + "terminal", + "in_flight_guard", + "phase_effect_gate", + }: + raise ValueError( + "boundary_kind must be 'terminal', 'in_flight_guard', or " + "'phase_effect_gate'." + ) + for name in ("guard_id", "gate_id", "segment_name"): value = getattr(self, name) if value is not None and (type(value) is not str or not value): raise ValueError(f"{name} must be a non-empty string or None.") if self.boundary_kind == "terminal": - if self.guard_id is not None or self.segment_name is not None: + if ( + self.guard_id is not None + or self.gate_id is not None + or self.segment_name is not None + ): raise ValueError( - "Terminal effect traces cannot declare guard phase metadata." + "Terminal effect traces cannot declare segment-boundary metadata." ) - elif self.guard_id is None or self.segment_name is None: + elif self.boundary_kind == "in_flight_guard" and ( + self.guard_id is None + or self.gate_id is not None + or self.segment_name is None + ): + raise ValueError( + "In-flight guard traces require only guard_id and segment_name." + ) + elif self.boundary_kind == "phase_effect_gate" and ( + self.gate_id is None + or self.guard_id is not None + or self.segment_name is None + ): raise ValueError( - "In-flight guard traces require guard_id and segment_name." + "Phase-effect gate traces require only gate_id and segment_name." ) if not math.isfinite(self.timestamp) or self.timestamp < 0.0: raise ValueError("timestamp must be finite and non-negative.") @@ -1109,6 +1138,7 @@ def snapshot(self) -> SkillEffectTrace: evidence=self.evidence, boundary_kind=self.boundary_kind, guard_id=self.guard_id, + gate_id=self.gate_id, segment_name=self.segment_name, ) @@ -1154,6 +1184,13 @@ def to_metadata(self) -> dict[str, object]: "segment_name": self.segment_name, } ) + elif self.boundary_kind == "phase_effect_gate": + metadata["boundary"].update( + { + "gate_id": self.gate_id, + "segment_name": self.segment_name, + } + ) return metadata @@ -1643,6 +1680,7 @@ def __init__( self._call_effect_offset = 0 self._observation_revision = 0 self._next_guard_verification_id = 0 + self._next_gate_verification_id = 0 self._wait_duration = 0.0 self._message: str | None = None @@ -1810,8 +1848,11 @@ def step(self) -> SkillResult: verifier = self._effect_verifier if monitor is not None else None guards = tuple(getattr(grounded, "effect_guards", ())) guard_verifier = self._held_object_guard_verifier if guards else None + gates = tuple(getattr(grounded, "effect_gates", ())) + gate_verifier = self._phase_effect_gate_verifier if gates else None runner_step = runner.step( effect_verifier=verifier, + phase_effect_gate_verifier=gate_verifier, held_object_guard_verifier=guard_verifier, ) self._consume_runner_step(runner_step) @@ -1826,6 +1867,17 @@ def step(self) -> SkillResult: "semantic call did not install an effect monitor." ) return self.result + if ( + runner_step.status is RunnerStatus.RUNNING + and runner_step.tick is not None + and runner_step.tick.pending_phase_effect_gate is not None + and not gates + ): + self._abort( + "The atomic invocation requested a phase-effect gate, but the " + "grounded semantic call did not install its monitor." + ) + return self.result if runner_step.status is RunnerStatus.RUNNING: return self.result self._finish_current_call(runner_step) @@ -2080,6 +2132,7 @@ def _reset_workflow( self._call_effect_offset = 0 self._observation_revision = 0 self._next_guard_verification_id = 0 + self._next_gate_verification_id = 0 self._wait_duration = 0.0 self._message = None self._status = SkillStatus.RUNNING @@ -2132,6 +2185,7 @@ def _prepare_call(self, call_index: int) -> None: effect_spec = getattr(grounded, "effect_spec", None) effect_monitor = getattr(grounded, "effect_monitor", None) effect_guards = tuple(getattr(grounded, "effect_guards", ())) + effect_gates = tuple(getattr(grounded, "effect_gates", ())) if invocation is None: raise TypeError("Semantic compiler ground() must return an invocation.") if not isinstance(grounded_eligible, torch.Tensor) or not torch.equal( @@ -2160,6 +2214,13 @@ def _prepare_call(self, call_index: int) -> None: ) if effect_guards and effect_spec is None: raise ValueError("Grounded held-object guards require an effect spec.") + if not all(type(value) is GroundedPhaseEffectGate for value in effect_gates): + raise TypeError( + "Grounded effect_gates must contain exact " + "GroundedPhaseEffectGate values." + ) + if effect_gates and effect_spec is None: + raise ValueError("Grounded phase-effect gates require an effect spec.") self._grounded = grounded session = self._engine.start( @@ -2301,6 +2362,99 @@ def _terminal_failure_policy( return failure_mask & ~retained, torch.zeros_like(failure_mask) return invalidation, retry + def _phase_effect_gate_verifier( + self, + context: PlanningContext, + request: PhaseEffectGateRequest, + ) -> PhaseEffectGateResult: + """Observe one blocking segment-entry effect on a fresh due cycle.""" + grounded = self._require_grounded() + gates = tuple(getattr(grounded, "effect_gates", ())) + matches = tuple(value for value in gates if value.gate_id == request.gate_id) + if len(matches) != 1: + raise RuntimeError( + f"Grounded call must own exactly one phase-effect gate " + f"{request.gate_id!r}." + ) + gate = matches[0] + if gate.segment_name != request.segment_name: + raise ValueError( + "Phase-effect gate request segment does not match its grounded " + "monitor." + ) + session = self._require_runner().session + monitor_request = EffectVerificationRequest( + verification_id=self._next_gate_verification_id, + skill_id=request.skill_id, + invocation_id=request.invocation_id, + invocation_revision=request.invocation_revision, + invocation_index=request.invocation_index, + attempt_generation=request.attempt_generation, + terminal_segment=request.segment_name, + requested_at=request.requested_at, + deadline=request.deadline, + env_mask=request.env_mask, + expected_effects=self._phase_effect_gate_expected_effects( + gate, + session.active_plan.expected_effects, + ), + ) + self._next_gate_verification_id += 1 + decision = self._observe_effect_monitor( + context, + monitor_request, + spec=gate.effect_spec, + monitor=gate.effect_monitor, + boundary_kind="phase_effect_gate", + gate_id=gate.gate_id, + segment_name=gate.segment_name, + ) + return PhaseEffectGateResult( + verification_id=request.verification_id, + gate_id=request.gate_id, + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + retry_mask=( + decision.failure_mask + if gate.retry_action + else torch.zeros_like(decision.failure_mask) + ), + message=( + f"Physical evidence contradicted gate {gate.gate_id!r} before " + f"segment {gate.segment_name!r}." + if decision.failure_mask.any() + else "" + ), + ) + + @staticmethod + def _phase_effect_gate_expected_effects( + gate: GroundedPhaseEffectGate, + action_effects: StateDelta, + ) -> StateDelta: + """Project the action-owned held relation required by one gate.""" + expectation = gate.effect_spec.state_expectations[0] + if type(expectation) is not HeldObjectStateExpectation: + raise TypeError("Built-in phase-effect gates require held-object state.") + key = expectation.task_state_key + if key not in action_effects.held_object_updates: + raise ValueError(f"Active action does not declare gate state key {key!r}.") + candidate = action_effects.held_object_updates[key] + if expectation.relation is HeldObjectRelation.ATTACHED: + if not isinstance(candidate, HeldObjectState): + raise ValueError( + f"Attached gate {gate.gate_id!r} requires an action-owned " + "HeldObjectState candidate." + ) + elif candidate is not None: + raise ValueError( + f"Detached gate {gate.gate_id!r} requires an action-owned removal." + ) + return StateDelta(held_object_updates={key: candidate}) + def _held_object_guard_verifier( self, context: PlanningContext, @@ -2445,6 +2599,7 @@ def _observe_effect_monitor( monitor: EffectMonitor, boundary_kind: str = "terminal", guard_id: str | None = None, + gate_id: str | None = None, segment_name: str | None = None, ) -> EffectMonitorDecision: """Collect evidence, run one monitor, and append an auditable trace.""" @@ -2499,6 +2654,7 @@ def _observe_effect_monitor( evidence=evidence, boundary_kind=boundary_kind, guard_id=guard_id, + gate_id=gate_id, segment_name=segment_name, ) self._effect_traces.append(trace) diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index be4a32cc9..a01a84add 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -1386,8 +1386,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, @@ -1407,14 +1407,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, @@ -1428,6 +1429,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}, ) @@ -1535,11 +1537,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: @@ -1793,6 +1801,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 @@ -2650,8 +2666,38 @@ def test_standard_factory_rejects_adapter_live_route_declaration_drift( def test_pick_place_effects_require_accepted_hand_state_and_live_pose() -> None: - """Production Pick/Place evidence stays conjunctive through runtime traces.""" + """Terminal Pick/Place evidence stays conjunctive through runtime traces.""" assembly, robot, cube = _evidence_runtime() + + def without_phase_gates( + self: Any, + analyzed: Any, + effect_spec: Any, + *, + path: tuple[Any, ...], + ) -> tuple[()]: + del self, analyzed, effect_spec, path + return () + + def without_in_flight_guards( + self: Any, + analyzed: Any, + effect_spec: Any, + context: Any, + *, + path: tuple[Any, ...], + ) -> tuple[()]: + del self, analyzed, effect_spec, context, path + return () + + assembly.compiler._ground_phase_effect_gates = MethodType( + without_phase_gates, + assembly.compiler, + ) + assembly.compiler._ground_held_object_guards = MethodType( + without_in_flight_guards, + assembly.compiler, + ) assert type(assembly.accepted_command_observer) is ( ControlCommandStateEvidenceTracker ) diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index ebb2782ea..390cc2488 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -59,6 +59,9 @@ ObjectSemantics, PlannerDiagnostics, PlanningContext, + PhaseEffectGateRequest, + PhaseEffectGateRequirement, + PhaseEffectGateResult, RecoveryPolicy, ResolvedActionRequest, RobotObservation, @@ -150,6 +153,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.""" @@ -595,6 +624,26 @@ def _held_object_loss_result( ) +def _phase_gate_result( + request: PhaseEffectGateRequest, + *, + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + retry_mask: torch.Tensor | None = None, +) -> PhaseEffectGateResult: + """Build one result exactly correlated with a pending segment-entry gate.""" + return PhaseEffectGateResult( + verification_id=request.verification_id, + gate_id=request.gate_id, + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + success_mask=success_mask, + failure_mask=failure_mask, + retry_mask=(failure_mask.clone() if retry_mask is None else retry_mask), + ) + + def _multi_dependency_context( timestamp: float, *, @@ -720,6 +769,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, @@ -786,6 +858,174 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +@pytest.mark.parametrize( + ("segment_name", "message"), + (("missing", "missing segment"), ("prepare", "first trajectory segment")), +) +def test_phase_effect_gate_requires_a_noninitial_named_segment( + segment_name: str, + message: str, +) -> None: + engine, _ = _engine() + engine.register(PhaseGateAction()) + + with pytest.raises(ValueError, match=message): + engine.start( + (_phase_gate_invocation(engine, segment_name=segment_name),), + _context(0.0, 0.0, 0.2, 0), + ) + + +def test_unresolved_phase_effect_gate_replays_preceding_command_for_full_cohort() -> ( + None +): + engine, _ = _engine(batch_size=2) + action = PhaseGateAction() + engine.register(action) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + session = engine.start((_phase_gate_invocation(engine),), initial) + + first = session.tick(initial) + boundary = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.4), 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + assert request.gate_id == "physical_ready" + assert request.segment_name == "commit" + assert request.next_waypoint_index == 2 + assert request.env_mask.tolist() == [True, True] + assert torch.allclose(_joint_positions(first.command), torch.zeros(2, 2)) + predecessor = _joint_positions(boundary.command) + assert torch.allclose(predecessor, torch.tensor([[0.1, 0.1], [0.2, 0.2]])) + + unresolved = session.tick( + _context(0.2, (0.1, 0.2), (0.2, 0.4), 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert unresolved.status is ExecutionStatus.RUNNING + assert unresolved.pending_phase_effect_gate is not None + assert unresolved.pending_phase_effect_gate.verification_id == ( + request.verification_id + 1 + ) + assert unresolved.pending_phase_effect_gate.next_waypoint_index == 2 + assert torch.equal(_joint_positions(unresolved.command), predecessor) + assert unresolved.command is not None + assert unresolved.command.active_mask.tolist() == [True, True] + assert unresolved.task_state.held_objects == {} + kinds = [event.kind for event in (*boundary.events, *unresolved.events)] + assert kinds.count(ExecutionEventKind.PHASE_EFFECT_GATE_REQUIRED) == 1 + assert ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED not in kinds + assert action.plan_count == 1 + + +def test_phase_effect_gate_success_unlocks_segment_without_committing_task_state() -> ( + None +): + engine, _ = _engine(batch_size=2) + engine.register(PhaseGateAction()) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + session = engine.start((_phase_gate_invocation(engine),), initial) + session.tick(initial) + boundary = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.4), 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + + released = session.tick( + _context(0.2, (0.1, 0.2), (0.2, 0.4), 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([True, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert released.pending_phase_effect_gate is None + assert torch.allclose( + _joint_positions(released.command), + torch.tensor([[0.2, 0.2], [0.4, 0.4]]), + ) + assert released.task_state.held_objects == {} + satisfied = next( + event + for event in released.events + if event.kind is ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED + ) + assert satisfied.env_mask.tolist() == [True, True] + + +def test_phase_effect_gate_contradiction_retries_action_without_state_mutation() -> ( + None +): + engine, _ = _engine(batch_size=2) + action = PhaseGateAction() + engine.register(action) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + session = engine.start( + (_phase_gate_invocation(engine, max_action_retries=1),), + initial, + ) + session.tick(initial) + boundary = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.4), 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + + retried = session.tick( + _context(0.2, (0.1, 0.2), (0.2, 0.4), 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([True, False]), + retry_mask=torch.tensor([True, False]), + ), + ) + + assert retried.status is ExecutionStatus.RUNNING + assert retried.pending_phase_effect_gate is None + assert retried.command is not None + assert retried.command.active_mask.tolist() == [True, True] + assert action.plan_count == 2 + assert session.plan_attempts[-1].attempt_generation == 1 + assert session.plan_attempts[-1].action_retry_counts == (1, 0) + assert retried.task_state.held_objects == {} + kinds = [event.kind for event in retried.events] + assert ExecutionEventKind.PHASE_EFFECT_GATE_FAILED in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + + +def test_stale_phase_effect_gate_result_is_rejected_after_unresolved_poll() -> None: + engine, _ = _engine() + engine.register(PhaseGateAction()) + initial = _context(0.0, 0.0, 0.2, 0) + session = engine.start((_phase_gate_invocation(engine),), initial) + session.tick(initial) + boundary = session.tick(_context(0.1, 0.0, 0.2, 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + unresolved = session.tick( + _context(0.2, 0.1, 0.2, 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([False]), + failure_mask=torch.tensor([False]), + ), + ) + assert unresolved.pending_phase_effect_gate is not None + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.3, 0.1, 0.2, 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + + def test_held_object_loss_retries_only_failed_row_with_reconciled_state() -> None: engine, _ = _engine(batch_size=2) initial = _with_held_object(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index b590c3dbd..1b88f769d 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -50,6 +50,9 @@ JointPositionTarget, MotionPolicy, ObjectSemantics, + PhaseEffectGateRequest, + PhaseEffectGateRequirement, + PhaseEffectGateResult, PlanningContextTrackingFeedbackProvider, PlanningContext, RecoveryPolicy, @@ -106,6 +109,27 @@ def _effect_result( ) +def _phase_gate_result( + request: PhaseEffectGateRequest, + *, + success: bool, + batch_size: int, +) -> PhaseEffectGateResult: + """Build one all-row gate decision correlated with a runner request.""" + success_mask = torch.full((batch_size,), success, dtype=torch.bool) + failure_mask = torch.zeros(batch_size, dtype=torch.bool) + return PhaseEffectGateResult( + verification_id=request.verification_id, + gate_id=request.gate_id, + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + success_mask=success_mask, + failure_mask=failure_mask, + retry_mask=failure_mask, + ) + + class FakeClock: """Deterministic clock used by non-blocking runner tests.""" @@ -298,9 +322,15 @@ class TimedAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): ) ) - def __init__(self, *, with_effect: bool = False) -> None: + def __init__( + self, + *, + with_effect: bool = False, + with_phase_gate: bool = False, + ) -> None: super().__init__() self.with_effect = with_effect + self.with_phase_gate = with_phase_gate self.plan_count = 0 def _plan( @@ -344,6 +374,9 @@ def _plan( success=True, trajectory=trajectory, expected_effects=effects, + segment_lengths=( + {"prepare": 2, "commit": 1} if self.with_phase_gate else None + ), ) @@ -358,6 +391,7 @@ def _timed_action_binding(action: TimedAction) -> ActionBinding: def _make_runner( *, with_effect: bool = False, + with_phase_gate: bool = False, batch_size: int = BATCH_SIZE, control_joint_ids: tuple[int, ...] | None = None, max_action_retries: int = 2, @@ -387,7 +421,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") @@ -404,6 +441,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( @@ -557,6 +604,97 @@ def verifier( assert "guard evidence unavailable" in failed.message +def test_runner_phase_gate_polls_fresh_state_and_replays_preceding_command() -> None: + runner, clock, _, sink, _ = _make_runner(with_phase_gate=True) + requests: list[tuple[float, PhaseEffectGateRequest]] = [] + + first = runner.step() + clock.advance(first.wait_duration) + boundary = runner.step() + assert boundary.tick is not None + assert boundary.tick.pending_phase_effect_gate is not None + clock.advance(boundary.wait_duration) + + def verifier( + context: PlanningContext, + request: PhaseEffectGateRequest, + ) -> PhaseEffectGateResult: + requests.append((context.robot.timestamp, request)) + return _phase_gate_result( + request, + success=len(requests) == 2, + batch_size=context.batch_size, + ) + + unresolved = runner.step(phase_effect_gate_verifier=verifier) + assert unresolved.tick is not None + assert unresolved.tick.pending_phase_effect_gate is not None + clock.advance(unresolved.wait_duration) + released = runner.step(phase_effect_gate_verifier=verifier) + + assert released.status is RunnerStatus.RUNNING + assert released.tick is not None + assert released.tick.pending_phase_effect_gate is None + assert [value[1].verification_id for value in requests] == [0, 1] + assert [value[1].next_waypoint_index for value in requests] == [2, 2] + assert [value[1].segment_name for value in requests] == ["commit", "commit"] + assert requests[0][0] < requests[1][0] + assert len(sink.sent) == 4 + boundary_payload = sink.sent[1].commands[0].payload + replay_payload = sink.sent[2].commands[0].payload + released_payload = sink.sent[3].commands[0].payload + assert isinstance(boundary_payload, JointPositionPayload) + assert isinstance(replay_payload, JointPositionPayload) + assert isinstance(released_payload, JointPositionPayload) + assert torch.equal(replay_payload.positions, boundary_payload.positions) + assert torch.allclose( + released_payload.positions, + torch.full((BATCH_SIZE, ROBOT_DOF), TARGET_POSITION), + ) + assert any( + event.kind is ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED + for event in released.tick.events + ) + + +def test_blocking_runner_returns_unverified_phase_gate_boundary() -> None: + runner, _, _, sink, _ = _make_runner(with_phase_gate=True) + + blocked = runner.run_until_blocked() + + assert blocked.status is RunnerStatus.RUNNING + assert blocked.tick is not None + assert blocked.tick.pending_phase_effect_gate is not None + assert blocked.tick.pending_phase_effect_gate.segment_name == "commit" + assert len(sink.sent) == 2 + + +def test_runner_phase_gate_verifier_exception_performs_safe_stop() -> None: + runner, clock, _, sink, _ = _make_runner(with_phase_gate=True) + first = runner.step() + clock.advance(first.wait_duration) + boundary = runner.step() + clock.advance(boundary.wait_duration) + + def verifier( + context: PlanningContext, + request: PhaseEffectGateRequest, + ) -> PhaseEffectGateResult: + del context, request + raise RuntimeError("gate evidence unavailable") + + failed = runner.step(phase_effect_gate_verifier=verifier) + + assert failed.status is RunnerStatus.FAILED + assert [dispatch.operation for dispatch in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "gate evidence unavailable" in failed.message + + def test_runner_dispatches_transport_neutral_endpoint_frames() -> None: runner, _, _, sink, _ = _make_runner() diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index 92f684325..48265365e 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -783,6 +783,17 @@ def test_pick_effect_spec_binds_destination_and_fresh_monitor_per_grounding() -> assert guard.effect_monitor is not first.effect_monitor assert guard.effect_spec.effect_kind is SemanticEffectKind.ATTACH assert repeated.effect_guards[0].effect_monitor is not guard.effect_monitor + assert len(first.effect_gates) == 1 + gate = first.effect_gates[0] + assert gate.gate_id == "destination_acquired" + assert gate.segment_name == "lift" + assert gate.retry_action is True + assert gate.effect_monitor is not first.effect_monitor + assert gate.effect_monitor is not guard.effect_monitor + assert gate.effect_spec.state_expectations[0].expectation_id == "destination" + assert gate.effect_spec.effect_kind is SemanticEffectKind.ATTACH + assert first.invocation.phase_effect_gates == (gate.requirement,) + assert repeated.effect_gates[0].effect_monitor is not gate.effect_monitor def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: @@ -848,6 +859,17 @@ def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: assert guard_pose.baseline_object_to_endpoint is None assert isinstance(guard_constraint, BinaryEffectClause) assert guard_constraint.expected is True + assert len(grounded.effect_gates) == 1 + gate = grounded.effect_gates[0] + assert gate.gate_id == "source_released" + assert gate.segment_name == "retract" + assert gate.retry_action is True + assert gate.effect_spec.effect_kind is SemanticEffectKind.RELEASE + gate_relation = gate.effect_spec.state_expectations[0] + assert isinstance(gate_relation, HeldObjectStateExpectation) + assert gate_relation.expectation_id == "source" + assert gate_relation.relation is HeldObjectRelation.DETACHED + assert grounded.invocation.phase_effect_gates == (gate.requirement,) def test_handover_effect_spec_binds_source_and_destination_relations() -> None: @@ -942,6 +964,17 @@ def test_handover_effect_spec_binds_source_and_destination_relations() -> None: assert destination_guard.task_state_key == "right" assert destination_guard.invalidation_task_state_keys == ("left", "right") assert destination_guard.retry_action is False + assert len(grounded.effect_gates) == 1 + gate = grounded.effect_gates[0] + assert gate.gate_id == "destination_acquired" + assert gate.segment_name == "release" + assert gate.retry_action is True + assert gate.effect_monitor is not destination_guard.effect_monitor + gate_relation = gate.effect_spec.state_expectations[0] + assert isinstance(gate_relation, HeldObjectStateExpectation) + assert gate_relation.expectation_id == "destination" + assert gate_relation.relation is HeldObjectRelation.ATTACHED + assert grounded.invocation.phase_effect_gates == (gate.requirement,) def test_registered_call_without_monitor_has_no_effect_contract() -> None: @@ -973,6 +1006,8 @@ def test_registered_call_without_monitor_has_no_effect_contract() -> None: assert workflow.calls[0].effect_monitor_ref is None assert grounded.effect_spec is None assert grounded.effect_monitor is None + assert grounded.effect_gates == () + assert grounded.invocation.phase_effect_gates == () options = grounded.invocation.skill_options assert type(options) is PickUpOptions assert options.pre_grasp_distance == 0.07 diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py index 8f247cb32..dea670ed2 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -47,6 +47,7 @@ JointPositionTarget, MotionPolicy, ObjectSemantics, + PhaseEffectGateRequest, PlanningContext, RecoveryPolicy, ResolvedActionRequest, @@ -63,6 +64,7 @@ from embodichain.lab.sim.skills.calls import HandOver, Place, RegisteredSemanticCall from embodichain.lab.sim.skills.compiler import ( GroundedHeldObjectGuard, + GroundedPhaseEffectGate, HeldObjectGuardBaseline, SemanticSkillCompiler, ) @@ -798,6 +800,120 @@ def test_in_flight_guard_collects_live_evidence_and_builds_loss_reconciliation() assert torch.equal(system.collector.calls[0][2], torch.tensor([0, 1])) +def test_phase_effect_gate_uses_independent_monitor_and_records_boundary_trace() -> ( + None +): + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="object", + entity_id="cube", + ) + poses = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + held = HeldObjectState( + semantics=semantics, + object_to_eef=poses, + grasp_xpos=poses, + env_mask=_mask(True, True), + ) + expectation = HeldObjectStateExpectation( + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + object_id="cube", + slot_id="primary", + resource_id="arm", + task_state_key="arm", + ) + spec = SemanticEffectSpec( + semantic_id="pick", + effect_kind=SemanticEffectKind.ATTACH, + skill_id="pick_up", + invocation_id="workflow:0", + invocation_revision=0, + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + state_expectations=(expectation,), + clauses=( + BinaryEffectClause( + clause_id="destination.constraint", + expectation_id="destination", + source=EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("hand", "constraint"), + ), + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + expected=True, + ), + ), + ) + terminal_monitor = _DecisionMonitor( + spec, + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + gate_monitor = _DecisionMonitor( + spec, + EffectMonitorDecision(_mask(False, True), _mask(True, False)), + ) + gate = GroundedPhaseEffectGate( + gate_id="destination_acquired", + segment_name="lift", + effect_spec=spec, + effect_monitor=gate_monitor, + retry_action=True, + ) + system.runtime._grounded = SimpleNamespace( + analyzed=SimpleNamespace(effect_monitor_ref=None), + effect_monitor=terminal_monitor, + effect_gates=(gate,), + ) + system.runtime._runner = SimpleNamespace( + session=SimpleNamespace( + active_plan=SimpleNamespace( + expected_effects=StateDelta(held_object_updates={"arm": held}) + ) + ) + ) + system.runtime._current_call_index = 0 + context = system.observation.observe(TaskState.empty(BATCH_SIZE, "cpu")) + request = PhaseEffectGateRequest( + verification_id=7, + gate_id="destination_acquired", + skill_id="pick_up", + invocation_id="workflow:0", + invocation_revision=0, + invocation_index=0, + attempt_generation=3, + next_waypoint_index=4, + segment_name="lift", + requested_at=0.0, + deadline=10.0, + env_mask=_mask(True, True), + ) + + result = system.runtime._phase_effect_gate_verifier(context, request) + + assert result.verification_id == 7 + assert result.gate_id == "destination_acquired" + assert result.attempt_generation == 3 + assert result.next_waypoint_index == 4 + assert torch.equal(result.success_mask, _mask(False, True)) + assert torch.equal(result.failure_mask, _mask(True, False)) + assert torch.equal(result.retry_mask, _mask(True, False)) + assert terminal_monitor.calls == 0 + assert gate_monitor.calls == 1 + assert gate_monitor.requests[0].terminal_segment == "lift" + candidate = gate_monitor.requests[0].expected_effects.held_object_updates["arm"] + assert isinstance(candidate, HeldObjectState) + assert candidate.semantics.entity_id == "cube" + trace = system.runtime._effect_traces[0] + assert trace.boundary_kind == "phase_effect_gate" + assert trace.guard_id is None + assert trace.gate_id == "destination_acquired" + assert trace.segment_name == "lift" + assert torch.equal(system.collector.calls[0][2], torch.tensor([0, 1])) + + def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None: system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) From fc294f7bf5d52130baa59c758fca85e164cc5d47 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 23:08:53 +0800 Subject: [PATCH 24/29] feat(skills): add bounded workflow reacquisition --- .../design/declarative_expert_program_plan.md | 35 +- ...mbodichain.lab.gym.envs.expert_program.rst | 1 + .../embodichain.lab.sim.skills.rst | 7 + docs/source/api_reference/public_api.rst | 41 + .../lab/gym/envs/expert_program/catalog.py | 6 +- .../expert_program/simulation_environment.py | 27 +- .../expert_program/simulation_handover.py | 4 +- embodichain/lab/sim/skills/__init__.py | 6 + embodichain/lab/sim/skills/integration.py | 1 + embodichain/lab/sim/skills/profiles.py | 58 +- embodichain/lab/sim/skills/runtime.py | 892 ++++++++++++++++-- .../multi_segments/cube_pick_place.py | 8 +- .../embodichain_tasks/tableware/hand_over.py | 8 +- tests/gym/envs/expert_program/test_catalog.py | 37 +- .../test_simulation_environment.py | 34 +- tests/gym/envs/tasks/test_hand_over.py | 1 + .../test_multi_segments_cube_pick_place.py | 1 + .../sim/atomic_actions/test_engine_per_env.py | 6 +- tests/sim/skills/test_compiler.py | 2 +- tests/sim/skills/test_integration.py | 7 +- tests/sim/skills/test_profiles.py | 36 +- tests/sim/skills/test_runtime.py | 755 ++++++++++++++- 22 files changed, 1814 insertions(+), 159 deletions(-) diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index a41dbcab8..675ea9b4a 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -10,7 +10,10 @@ 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. + 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), @@ -420,8 +423,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, - plus an optional required-planner compatibility constraint; +- 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 @@ -615,7 +618,19 @@ 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 remains open and may not repair the scene implicitly. +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`` @@ -1256,8 +1271,9 @@ 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, fault-injection coverage, and the full -repeated-cube run remain validation or design work. +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: @@ -1360,7 +1376,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 @@ -1517,8 +1533,9 @@ The design is complete when all of the following hold: 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; workflow-level re-acquisition and real-simulation fault - injection remain open. + 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.gym.envs.expert_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst index 53e87e615..c915cf947 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst @@ -25,6 +25,7 @@ embodichain.lab.gym.envs.expert_program SimulationExpertProgramFactory SimulationSegmentPolicyPort ControlCommandStateEvidenceTracker + ConfiguredHandOverPoseProvider AcceptedRuntimeCommandObserver AcceptedRuntimeCommandObserverFactory AntipodalGraspAffordanceBinding diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst index dfd1cc2dc..8b49ec461 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -125,6 +125,9 @@ embodichain.lab.sim.skills FORCE_EFFECT_CHANNEL GRASP_AFFORDANCE_CAPABILITY GroundedSemanticCall + GroundedHeldObjectGuard + GroundedPhaseEffectGate + HeldObjectGuardBaseline HandOverPoseProvider HandOverPoseTargets HeldObjectRelation @@ -185,6 +188,10 @@ embodichain.lab.sim.skills SemanticWorkflow SkillEndpointBindingTrace SkillEndpointTrackingChannelTrace + SkillWorkflowRecoveryRole + SkillWorkflowRecoveryTrace + WorkflowRecoveryPolicy + EffectExpectationDecision SkillFailure SkillRuntimeProvider SkillScene diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index e5fc66098..6454d39f5 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -316,6 +316,15 @@ embodichain.lab.gym.envs.expert_program.simulation_policies SimulationSegmentPolicyPort default_simulation_settle_presets +embodichain.lab.gym.envs.expert_program.simulation_handover +------------------------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation_handover + +.. autosummary:: + + ConfiguredHandOverPoseProvider + embodichain.lab.gym.envs.settling --------------------------------- @@ -532,6 +541,7 @@ embodichain.lab.sim.atomic_actions EntityState EffectVerificationRequirement EffectVerificationResult + EffectExpectationResult EffectVerifier ExecutionPlanAttempt FeedbackTerminalAcceptance @@ -543,6 +553,7 @@ embodichain.lab.sim.atomic_actions JointPositionTrackingProjector JointPositionTrackingState HandOverOptions + HeldObjectGuardVerifier InteractionPoints MoveEndEffectorOptions MoveHeldObjectOptions @@ -555,6 +566,7 @@ embodichain.lab.sim.atomic_actions OperateArticulationOptions PickUpOptions PlaceOptions + PhaseEffectGateVerifier PlanningContextTrackingFeedbackProvider PoseTrackingEvaluator PoseTrackingMetric @@ -676,12 +688,17 @@ embodichain.lab.sim.atomic_actions.execution EffectVerificationRequest EffectVerificationResult + EffectExpectationResult ExecutionEvent ExecutionEventKind ExecutionPlanAttempt ExecutionSession ExecutionStatus ExecutionTick + HeldObjectGuardRequest + HeldObjectGuardResult + PhaseEffectGateRequest + PhaseEffectGateResult embodichain.lab.sim.atomic_actions.goals ---------------------------------------- @@ -711,6 +728,7 @@ embodichain.lab.sim.atomic_actions.invocation ActionOptions GoalT OptionsT + PhaseEffectGateRequirement ResolvedActionRequest embodichain.lab.sim.atomic_actions.plans @@ -798,8 +816,10 @@ embodichain.lab.sim.atomic_actions.runner ExecutionClock ExecutionRunner ExecutionRunnerCfg + HeldObjectGuardVerifier MonotonicExecutionClock ObservationProvider + PhaseEffectGateVerifier RunnerStatus RunnerStep RunnerStepCallback @@ -1248,8 +1268,11 @@ embodichain.lab.sim.skills.compiler AnalyzedSemanticCall GroundedSemanticCall + GroundedHeldObjectGuard + GroundedPhaseEffectGate HandOverPoseProvider HandOverPoseTargets + HeldObjectGuardBaseline RelationTargetGrounder RegisteredSemanticLowerer SemanticEffectDependency @@ -1287,6 +1310,7 @@ embodichain.lab.sim.skills.effects EffectEvidenceAddress EffectEvidenceBatch EffectEvidenceSourceRef + EffectExpectationDecision EffectMonitor EffectMonitorDecision EffectMonitorFactory @@ -1415,6 +1439,7 @@ embodichain.lab.sim.skills.profiles RobotResource RobotSkillProfile SkillPolicyPreset + WorkflowRecoveryPolicy UnsupportedSkillError embodichain.lab.sim.skills.runtime @@ -1438,6 +1463,8 @@ embodichain.lab.sim.skills.runtime SkillRuntimeProvider SkillScene SkillStatus + SkillWorkflowRecoveryRole + SkillWorkflowRecoveryTrace task_state_to_metadata embodichain.lab.sim.skills.scene @@ -2196,8 +2223,22 @@ embodichain_tasks.tableware .. autosummary:: + HandOverEnv OpenDrawerEnv +embodichain_tasks.tableware.hand_over +------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.hand_over + +.. autosummary:: + + HandOverEnv + HAND_OVER_EXPERT_PROGRAM_REGISTRATION + HAND_OVER_POSE_PROVIDER + create_hand_over_robot_profile_binding + create_hand_over_scene_binding + embodichain_tasks.tableware.blocks_ranking_rgb ---------------------------------------------- diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index b6f582711..a466a0bd7 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -1274,22 +1274,24 @@ def _profile_with_step_dt( *, step_dt: float, ) -> RobotSkillProfile: - """Return the registration profile aligned to one Gym runtime cadence.""" + """Return the registration profile with runner cadence aligned to Gym.""" 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=step_dt), + motion_policy=preset.motion_policy, tracking_policy=preset.tracking_policy, recovery_policy=preset.recovery_policy, + 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, + required_planner=preset.required_planner, ) for preset_id, preset in profile.presets.items() }, diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py index b20801142..d1d24ba67 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -32,6 +32,7 @@ from collections.abc import Callable, Iterable, Mapping from copy import deepcopy +from dataclasses import replace import math from typing import Any, Protocol, TYPE_CHECKING @@ -718,11 +719,9 @@ 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`` 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. + Every runner policy is rebuilt with ``minimum_cycle_time == step_dt``. The + Gym cadence is also carried by each ``PlanningContext``; motion policy stays + provider-free and does not own environment timing. """ def __init__( @@ -873,39 +872,37 @@ def create_scene_registry(self) -> SceneRegistry: return registry def create_robot_skill_profile(self) -> RobotSkillProfile: - """Build a profile whose motion and runner policies use Gym cadence.""" + """Build a profile whose runner policy uses Gym cadence.""" profile = self._robot_profile_binding.build(self._robot) aligned_presets = { preset_id: SkillPolicyPreset( preset_id=preset.preset_id, schema_version=preset.schema_version, - motion_policy=replace( - preset.motion_policy, - control_dt=self._step_dt, - ), + motion_policy=preset.motion_policy, tracking_policy=preset.tracking_policy, recovery_policy=preset.recovery_policy, + 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, + required_planner=preset.required_planner, ) for preset_id, preset in profile.presets.items() } 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 + preset.runner_cfg.minimum_cycle_time != self._step_dt for preset in aligned.presets.values() ): - raise AssertionError("Profile runtime policies were not cadence-aligned.") + raise AssertionError("Profile runner policies were not cadence-aligned.") self._registration.validate_robot_profile( - profile, + aligned, step_dt=self._step_dt, ) - return profile + return aligned def create_atomic_action_engine( self, diff --git a/embodichain/lab/gym/envs/expert_program/simulation_handover.py b/embodichain/lab/gym/envs/expert_program/simulation_handover.py index 9ac4d6f50..a346861d6 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_handover.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_handover.py @@ -129,13 +129,13 @@ def resolve( del call, context, bound return HandOverPoseTargets( middle=SemanticObjectTarget( - pose=SemanticPose( + SemanticPose( position=self.middle_position, quaternion_wxyz=self.middle_quaternion_wxyz, ) ), final=SemanticObjectTarget( - pose=SemanticPose( + SemanticPose( position=self.final_position, quaternion_wxyz=self.final_quaternion_wxyz, ) diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 04c168dc1..94a315f19 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -171,6 +171,7 @@ RobotSkillProfile, SkillPolicyPreset, UnsupportedSkillError, + WorkflowRecoveryPolicy, ) from .scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, @@ -213,6 +214,8 @@ SkillRuntimeProvider, SkillScene, SkillStatus, + SkillWorkflowRecoveryRole, + SkillWorkflowRecoveryTrace, task_state_to_metadata, ) @@ -385,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/integration.py b/embodichain/lab/sim/skills/integration.py index 823a21969..12f20d8be 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -1315,6 +1315,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 9238fb9d8..5aef8053a 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.""" @@ -926,6 +955,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] @@ -935,10 +965,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, required_planner: str | None = None, @@ -947,10 +978,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]." ) if required_planner is not None: _validate_identifier( @@ -966,6 +997,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.") @@ -973,6 +1009,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 = ( @@ -1021,6 +1061,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, @@ -1043,6 +1088,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.""" @@ -1100,6 +1150,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, @@ -2359,4 +2410,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 9995b62cb..aa529e874 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 @@ -35,6 +36,7 @@ EffectVerificationRequest, EffectVerificationResult, ExecutionEvent, + ExecutionEventKind, ExecutionPlanAttempt, HeldObjectGuardRequest, HeldObjectGuardResult, @@ -89,6 +91,7 @@ SceneObjectRef, SceneRegistry, ) +from .profiles import WorkflowRecoveryPolicy def _snapshot_task_state(state: TaskState) -> TaskState: @@ -251,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.""" @@ -1403,6 +1414,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.""" @@ -1419,6 +1635,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 @@ -1474,6 +1691,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", @@ -1498,6 +1722,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", @@ -1516,13 +1745,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, @@ -1538,6 +1769,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, @@ -1663,6 +1897,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, @@ -1675,12 +1912,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 @@ -1789,6 +2028,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, @@ -1880,36 +2120,18 @@ def step(self) -> SkillResult: 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( @@ -1946,21 +2168,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 @@ -1998,16 +2233,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 @@ -2016,6 +2279,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 @@ -2120,6 +2384,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) @@ -2127,12 +2394,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 @@ -2173,12 +2442,60 @@ 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) @@ -2190,7 +2507,7 @@ def _prepare_call(self, call_index: int) -> 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): @@ -2226,7 +2543,7 @@ def _prepare_call(self, call_index: int) -> None: session = self._engine.start( (invocation,), context, - eligible_mask=self._eligible, + eligible_mask=active_mask, ) primed = _PrimedObservationProvider(context, self._observation_provider) runner = ExecutionRunner( @@ -2236,9 +2553,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 @@ -2671,11 +2990,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") @@ -2688,20 +3010,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, @@ -2711,27 +3019,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.""" @@ -2750,6 +3450,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( @@ -2823,6 +3526,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, @@ -2830,7 +3534,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 @@ -2849,6 +3563,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 @@ -2862,6 +3577,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.") @@ -3039,5 +3759,7 @@ def cancel( "SkillRuntimeProvider", "SkillScene", "SkillStatus", + "SkillWorkflowRecoveryRole", + "SkillWorkflowRecoveryTrace", "task_state_to_metadata", ] 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/hand_over.py b/embodichain_tasks/embodichain_tasks/tableware/hand_over.py index a1337f25e..4f8d3a77a 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/hand_over.py +++ b/embodichain_tasks/embodichain_tasks/tableware/hand_over.py @@ -70,7 +70,10 @@ ) from embodichain.lab.sim.shapes import CubeCfg, MeshCfg 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, @@ -455,6 +458,9 @@ def create_hand_over_robot_profile_binding() -> SimulationRobotSkillProfileBindi ), }, 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, diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py index b4a9badab..8df5319ec 100644 --- a/tests/gym/envs/expert_program/test_catalog.py +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -55,6 +55,7 @@ RegisteredSemanticCall, SemanticCallDescriptor, SkillPolicyPreset, + WorkflowRecoveryPolicy, builtin_semantic_call_catalog, ) from embodichain.lab.sim.atomic_actions.tracking import ( @@ -791,28 +792,24 @@ def test_fingerprint_is_stable_for_equivalent_declarations() -> None: assert len(left.fingerprint) == 64 -def test_planner_projection_is_json_safe_deterministic_and_owned() -> None: - """Planner discovery is one catalog-derived view with the exact digest.""" - left = _registration() - right = _registration() - - projection = left.catalog.planner_projection() - encoded = json.dumps(projection, allow_nan=False, sort_keys=True) - - assert projection == right.catalog.planner_projection() - assert projection["schema_version"] == ( - "semantic_integration_planner_projection/v1" +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 projection["integration_fingerprint"] == left.fingerprint - assert {call["call_id"] for call in projection["semantic_calls"]} >= { - "pick", - "place", - } - assert "ActionInvocation" not in encoded - assert "qpos" not in encoded - projection["scene"]["entities"].clear() - assert left.catalog.planner_projection()["scene"]["entities"] + assert _registration().fingerprint != _registration_with_preset(changed).fingerprint def test_fingerprint_is_independent_of_catalog_and_provider_insertion_order() -> None: diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index a01a84add..1900512e3 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -118,6 +118,7 @@ SemanticPose, SemanticRelationTarget, SkillPolicyPreset, + WorkflowRecoveryPolicy, ) from embodichain.lab.sim.skills.effects import ( CONSTRAINT_EFFECT_CHANNEL, @@ -715,8 +716,8 @@ def resolve( quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), ) return HandOverPoseTargets( - middle=SemanticObjectTarget(pose=pose), - final=SemanticObjectTarget(pose=pose), + middle=SemanticObjectTarget(pose), + final=SemanticObjectTarget(pose), ) @@ -1120,7 +1121,7 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: "pick": PickUpOptions(), "place": PlaceOptions(), }, - motion_policy=MotionPolicy(control_dt=_UNALIGNED_PROFILE_DT), + motion_policy=MotionPolicy(sample_count=17), tracking_policy=TrackingPolicy.joint_position( in_flight_max_abs_error=0.037, terminal_max_abs_error=0.019, @@ -1894,10 +1895,24 @@ def _assert_invocation_equivalent( ) -def test_simulation_factory_aligns_every_runtime_policy_to_gym_step() -> None: - """Cadence lowering preserves source declarations and unrelated policy.""" +def test_simulation_factory_aligns_runner_policy_to_gym_step() -> None: + """Runner cadence lowering preserves source declarations and policy.""" binding = _profile_binding() - source_preset = binding.presets[0] + 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) @@ -1905,7 +1920,7 @@ def test_simulation_factory_aligns_every_runtime_policy_to_gym_step() -> None: 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_preset.motion_policy.sample_count == 17 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 @@ -1918,9 +1933,8 @@ def test_simulation_factory_aligns_every_runtime_policy_to_gym_step() -> None: in_flight_max_abs_error=0.037, terminal_max_abs_error=0.019, ) - assert binding.presets[0].motion_policy.control_dt == pytest.approx( - _UNALIGNED_PROFILE_DT - ) + assert aligned_preset.workflow_recovery_policy.max_recovery_attempts == 2 + assert binding.presets[0].motion_policy.sample_count == 17 assert binding.presets[0].runner_cfg.minimum_cycle_time == pytest.approx( _UNALIGNED_PROFILE_DT ) diff --git a/tests/gym/envs/tasks/test_hand_over.py b/tests/gym/envs/tasks/test_hand_over.py index 7f7293e42..96a1e11bb 100644 --- a/tests/gym/envs/tasks/test_hand_over.py +++ b/tests/gym/envs/tasks/test_hand_over.py @@ -239,6 +239,7 @@ def test_hand_over_profile_binds_left_pick_and_left_to_right_transfer() -> None: } 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 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 5f9dd99f8..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( diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 390cc2488..27ccb2468 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -174,7 +174,11 @@ def _plan( request, context, success=True, - trajectory=torch.stack([context.robot.qpos, midpoint, target], dim=1), + trajectory=TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, midpoint, target], dim=1), + env_ids=context.env_ids, + step_dt=0.1, + ), segment_lengths={"prepare": 2, "commit": 1}, ) diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index 48265365e..1bc9b0ba4 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -148,7 +148,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), diff --git a/tests/sim/skills/test_integration.py b/tests/sim/skills/test_integration.py index af6b76b23..b9d204dd3 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) @@ -725,6 +726,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( @@ -744,6 +748,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 01b229788..6aaef9844 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 = { @@ -1430,7 +1431,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 ( @@ -1451,8 +1452,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", @@ -1470,6 +1471,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 dea670ed2..b7d0e52cd 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -29,6 +29,7 @@ import embodichain.lab.sim.skills.runtime as runtime_module from embodichain.lab.sim.atomic_actions import ( + ActionBinding, ActionInvocation, ActionOptions, ActionPlan, @@ -42,6 +43,7 @@ EndpointBinding, EndpointTrackingChannelBinding, EndpointTrackingFeedbackAddress, + ExecutionEventKind, HeldObjectGuardRequest, HeldObjectState, JointPositionTarget, @@ -54,6 +56,8 @@ RobotObservation, SceneSnapshot, SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, TimedCommandSequence, @@ -61,7 +65,13 @@ TrackingProjectorRef, ) from embodichain.lab.sim.atomic_actions.tracking import TrackingPolicy -from embodichain.lab.sim.skills.calls import HandOver, Place, RegisteredSemanticCall +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, +) from embodichain.lab.sim.skills.compiler import ( GroundedHeldObjectGuard, GroundedPhaseEffectGate, @@ -72,6 +82,7 @@ ArticulationJointStateExpectation, BinaryEffectClause, BinaryEvidenceKind, + CONSTRAINT_EFFECT_CHANNEL, ControlPartEvidenceAddress, EffectEvidenceBatch, EffectEvidenceSourceRef, @@ -90,10 +101,14 @@ 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.profiles import ( + ResourceClaim, + WorkflowRecoveryPolicy, +) from embodichain.lab.sim.skills.scene import SceneObjectRef, SceneRegistry BATCH_SIZE = 2 @@ -277,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) @@ -323,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",), @@ -428,6 +564,198 @@ 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( + strategy="ik_interp", + sample_count=7, + ) + 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 @@ -440,10 +768,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}") @@ -492,6 +848,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: @@ -597,6 +1002,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")) @@ -921,7 +1665,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" @@ -960,6 +1704,7 @@ def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None 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] From 9c5d12f336d547b0c6591999aa5e68891e222f7c Mon Sep 17 00:00:00 2001 From: skywhite1024 <129768272+skywhite1024@users.noreply.github.com> Date: Fri, 21 Aug 2026 17:24:01 +0800 Subject: [PATCH 25/29] test(skills): use explicit handover resource slots --- tests/sim/skills/test_runtime.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py index b7d0e52cd..0a980f6fd 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -1103,8 +1103,7 @@ def test_runtime_retries_directly_when_verified_source_relation_remains() -> Non result = system.runtime.run( HandOver( object=SceneObjectRef("cube"), - receiver="right_actor", - resources={"source": "left_actor"}, + resources={"source": "left_actor", "destination": "right_actor"}, ) ) @@ -1162,8 +1161,7 @@ def test_runtime_partitions_retained_and_lost_source_rows_in_one_barrier() -> No result = system.runtime.run( HandOver( object=SceneObjectRef("cube"), - receiver="right_actor", - resources={"source": "left_actor"}, + resources={"source": "left_actor", "destination": "right_actor"}, ) ) From 420cad6764a03d82211c533c3fb2da3a7c595ad3 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 23:54:06 +0800 Subject: [PATCH 26/29] feat(skills): add declarative placement relations --- ...mbodichain.lab.gym.envs.expert_program.rst | 2 + .../embodichain.lab.sim.skills.rst | 5 + docs/source/api_reference/public_api.rst | 8 + .../lab/gym/envs/expert_program/__init__.py | 4 + .../lab/gym/envs/expert_program/catalog.py | 24 +- .../lab/gym/envs/expert_program/simulation.py | 290 +++++++++++++++--- embodichain/lab/sim/skills/__init__.py | 10 + embodichain/lab/sim/skills/compiler.py | 53 ++++ embodichain/lab/sim/skills/scene.py | 58 ++++ tests/gym/envs/expert_program/test_catalog.py | 58 ++++ .../envs/expert_program/test_simulation.py | 82 +++++ tests/sim/skills/test_compiler.py | 49 +++ 12 files changed, 606 insertions(+), 37 deletions(-) diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst index c915cf947..82b377833 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst @@ -47,6 +47,7 @@ embodichain.lab.gym.envs.expert_program ConfigPath ConfigPathPart ControlPartCommandPreset + ContainerAffordanceBinding CyclicPoseTargetCfg DeclarativeCfgValue DemoBridgeError @@ -104,6 +105,7 @@ embodichain.lab.gym.envs.expert_program SimulationExpertProgramEnvironment SimulationPlanningObservationProvider SimulationRigidObjectBinding + SupportSurfaceAffordanceBinding SkillRuntimeAssemblyPort TargetCfg TargetRefCfg diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst index 8b49ec461..cfcf25534 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -102,6 +102,8 @@ embodichain.lab.sim.skills CompositeEffectMonitor CompositeEffectMonitorCfg CompositeEffectMonitorFactory + ContainerAffordance + ContainerRelationTargetGrounder ControlPartEvidenceAddress ControlPartRobotEvidenceSource ControlPartSimulationEvidenceProvider @@ -140,6 +142,7 @@ embodichain.lab.sim.skills LinkedSemanticCall PLACE_IN_AFFORDANCE_CAPABILITY PLACE_ON_AFFORDANCE_CAPABILITY + PLACEMENT_TARGET_AFFORDANCE_REVISION POSE_RELATION_EFFECT_CHANNEL ParallelBarrierUpdate ParallelBranchPlan @@ -175,6 +178,8 @@ embodichain.lab.sim.skills SceneEntityManifest SceneEntityMetadata SceneManifest + SupportSurfaceAffordance + SupportSurfaceRelationTargetGrounder SemanticCallDescriptor SemanticDiagnostic SemanticEffectDependency diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 6454d39f5..0eb05da52 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -280,6 +280,7 @@ embodichain.lab.gym.envs.expert_program.simulation ArticulationOperationAffordanceBinding ArticulationOperationTargetBinding ControlPartCommandPreset + ContainerAffordanceBinding ControlPartEndpointBinding ControlPartResourceBinding RobotResourceBinding @@ -290,6 +291,7 @@ embodichain.lab.gym.envs.expert_program.simulation SimulationRobotResourceBinding SimulationRobotSkillProfileBinding SimulationSceneBinding + SupportSurfaceAffordanceBinding embodichain.lab.gym.envs.expert_program.simulation_environment ---------------------------------------------------------------- @@ -1270,6 +1272,7 @@ embodichain.lab.sim.skills.compiler GroundedSemanticCall GroundedHeldObjectGuard GroundedPhaseEffectGate + ContainerRelationTargetGrounder HandOverPoseProvider HandOverPoseTargets HeldObjectGuardBaseline @@ -1282,6 +1285,7 @@ embodichain.lab.sim.skills.compiler SemanticObjectTarget SemanticRelationTarget SemanticSkillCompiler + SupportSurfaceRelationTargetGrounder SemanticWorkflow embodichain.lab.sim.skills.effects @@ -1311,6 +1315,7 @@ embodichain.lab.sim.skills.effects EffectEvidenceBatch EffectEvidenceSourceRef EffectExpectationDecision + EffectExpectationDecision EffectMonitor EffectMonitorDecision EffectMonitorFactory @@ -1476,6 +1481,7 @@ embodichain.lab.sim.skills.scene ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY ArticulationJointEvidenceAddress + ContainerAffordance SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION SceneArticulationJointStateProvider @@ -1497,6 +1503,8 @@ embodichain.lab.sim.skills.scene GRASP_AFFORDANCE_CAPABILITY PLACE_IN_AFFORDANCE_CAPABILITY PLACE_ON_AFFORDANCE_CAPABILITY + PLACEMENT_TARGET_AFFORDANCE_REVISION + SupportSurfaceAffordance UnsupportedSceneAffordanceError embodichain.lab.sim.solvers.neural_ik_solver diff --git a/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py index 79422cc06..3a786378d 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -121,6 +121,7 @@ AntipodalGraspAffordanceBinding, ArticulationOperationAffordanceBinding, ArticulationOperationTargetBinding, + ContainerAffordanceBinding, ControlPartCommandPreset, ControlPartEndpointBinding, ControlPartResourceBinding, @@ -132,6 +133,7 @@ SimulationRobotResourceBinding, SimulationRobotSkillProfileBinding, SimulationSceneBinding, + SupportSurfaceAffordanceBinding, ) from .catalog import ( ExpertProgramIntegrationCatalog, @@ -183,6 +185,7 @@ "CompiledProgramValidator", "CompiledRepeatFrame", "CompiledTargetSelection", + "ContainerAffordanceBinding", "ControlCommandStateEvidenceTracker", "ControlPartCommandPreset", "ControlPartEndpointBinding", @@ -265,6 +268,7 @@ "SimulationSceneBinding", "SimulationSegmentPolicyPort", "StandardExtensionDeclarations", + "SupportSurfaceAffordanceBinding", "SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS", "TargetCfg", "TargetRefCfg", diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index a466a0bd7..64e2c553a 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -56,6 +56,7 @@ PLACE_ON_AFFORDANCE_CAPABILITY, POSE_RELATION_EFFECT_CHANNEL, BoundRobotSkillProfile, + ContainerRelationTargetGrounder, HandOverPoseProvider, OperateArticulation, Place, @@ -77,6 +78,7 @@ SemanticIntegrationManifest, SemanticValidationError, SkillPolicyPreset, + SupportSurfaceRelationTargetGrounder, builtin_semantic_call_catalog, ) from embodichain.lab.sim.skills.effects import ( @@ -304,6 +306,18 @@ def _snapshot_relation_grounders( return tuple(values) +def _builtin_relation_grounders( + scene_binding: SimulationSceneBinding, +) -> tuple[RelationTargetGrounder, ...]: + """Install standard grounders for declared production relation bindings.""" + values: list[RelationTargetGrounder] = [] + if scene_binding.support_surfaces: + values.append(SupportSurfaceRelationTargetGrounder()) + if scene_binding.containers: + values.append(ContainerRelationTargetGrounder()) + return tuple(values) + + def _snapshot_relation_grounder_keys( values: frozenset[tuple[str, type[Affordance], str]], ) -> frozenset[tuple[str, type[Affordance], str]]: @@ -1440,7 +1454,15 @@ def __post_init__(self) -> None: _validate_standard_call_catalog(self.call_catalog) settle_presets = _snapshot_settle_presets(self.settle_presets) object.__setattr__(self, "settle_presets", settle_presets) - relation_grounders = _snapshot_relation_grounders(self.relation_grounders) + configured_relation_grounders = _snapshot_relation_grounders( + self.relation_grounders + ) + relation_grounders = _snapshot_relation_grounders( + ( + *_builtin_relation_grounders(self.scene_binding), + *configured_relation_grounders, + ) + ) object.__setattr__(self, "relation_grounders", relation_grounders) relation_grounder_keys = frozenset( _relation_grounder_key(grounder) for grounder in relation_grounders diff --git a/embodichain/lab/gym/envs/expert_program/simulation.py b/embodichain/lab/gym/envs/expert_program/simulation.py index b08235c32..8f8fd5f1e 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation.py +++ b/embodichain/lab/gym/envs/expert_program/simulation.py @@ -53,17 +53,23 @@ from embodichain.lab.sim.skills.integration import SceneEntityManifest, SceneManifest from embodichain.lab.sim.skills.scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, GRASP_AFFORDANCE_CAPABILITY, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneArticulationRef, SceneCollisionRole, SceneCollisionWorldMode, SceneDynamics, + SceneEntityRef, SceneEntityRegistration, SceneGeometryProvider, SceneLinkRef, SceneObjectRef, SceneRegistry, + SupportSurfaceAffordance, ) from embodichain.toolkits.graspkit.pg_grasp import GraspGeneratorCfg from embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker import ( @@ -326,6 +332,86 @@ def __post_init__(self) -> None: ) +def _validate_placement_binding(value: object) -> None: + """Validate fields shared by built-in placement-frame declarations.""" + for field_name in ("entity_id", "parent_id", "native_name"): + _identifier(getattr(value, field_name), field_name=field_name) + object.__setattr__( + value, + "aliases", + _identifier_tuple(getattr(value, "aliases"), field_name="aliases"), + ) + object.__setattr__( + value, + "object_target_pose", + _pose_tuple( + getattr(value, "object_target_pose"), + field_name="object_target_pose", + ), + ) + minimum_confidence = _finite( + getattr(value, "minimum_confidence"), + field_name="minimum_confidence", + ) + if not 0.0 <= minimum_confidence <= 1.0: + raise ValueError("minimum_confidence must be in [0, 1].") + object.__setattr__(value, "minimum_confidence", minimum_confidence) + if type(getattr(value, "is_default")) is not bool: + raise TypeError("is_default must be a bool.") + + +@dataclass(frozen=True, slots=True) +class SupportSurfaceAffordanceBinding: + """Declare one exact object target frame on a support parent. + + Args: + entity_id: Canonical ID of the placement affordance. + parent_id: Canonical object, articulation, or link parent ID. + native_name: Stable native name of this target frame. + aliases: Optional non-authoritative lookup aliases. + object_target_pose: Desired object pose relative to the parent. + minimum_confidence: Minimum parent/affordance observation confidence. + is_default: Whether this is the parent's default ``Place(on=...)`` frame. + """ + + entity_id: str + parent_id: str + native_name: str + aliases: tuple[str, ...] = () + object_target_pose: tuple[float, ...] = _IDENTITY_POSE + minimum_confidence: float = 0.0 + is_default: bool = False + + def __post_init__(self) -> None: + _validate_placement_binding(self) + + +@dataclass(frozen=True, slots=True) +class ContainerAffordanceBinding: + """Declare one exact object target frame inside a container parent. + + Args: + entity_id: Canonical ID of the placement affordance. + parent_id: Canonical object, articulation, or link parent ID. + native_name: Stable native name of this target frame. + aliases: Optional non-authoritative lookup aliases. + object_target_pose: Desired object pose relative to the parent. + minimum_confidence: Minimum parent/affordance observation confidence. + is_default: Whether this is the parent's default ``Place(inside=...)`` frame. + """ + + entity_id: str + parent_id: str + native_name: str + aliases: tuple[str, ...] = () + object_target_pose: tuple[float, ...] = _IDENTITY_POSE + minimum_confidence: float = 0.0 + is_default: bool = False + + def __post_init__(self) -> None: + _validate_placement_binding(self) + + @dataclass(frozen=True, slots=True) class ArticulationOperationTargetBinding: """Declarative named target for one articulation operation.""" @@ -554,6 +640,48 @@ def _antipodal_affordance( ) +def _placement_parent_ref( + parent_id: str, + *, + objects: Mapping[str, SimulationRigidObjectBinding], + articulations: Mapping[str, SimulationArticulationBinding], + links: Mapping[str, SimulationArticulationLinkBinding], +) -> SceneEntityRef: + """Resolve an explicitly declared placement parent to its exact ref type.""" + if parent_id in objects: + return SceneObjectRef(parent_id) + if parent_id in articulations: + return SceneArticulationRef(parent_id) + if parent_id in links: + return SceneLinkRef(parent_id) + raise KeyError(f"Placement affordance references unbound parent {parent_id!r}.") + + +def _placement_defaults( + support_surfaces: tuple[SupportSurfaceAffordanceBinding, ...], + containers: tuple[ContainerAffordanceBinding, ...], +) -> Mapping[str, Mapping[str, SceneAffordanceRef]]: + """Collect explicitly selected capability-scoped placement defaults.""" + defaults: dict[str, dict[str, SceneAffordanceRef]] = {} + for capability, bindings in ( + (PLACE_ON_AFFORDANCE_CAPABILITY, support_surfaces), + (PLACE_IN_AFFORDANCE_CAPABILITY, containers), + ): + for binding in bindings: + if not binding.is_default: + continue + parent_defaults = defaults.setdefault(binding.parent_id, {}) + previous = parent_defaults.get(capability) + if previous is not None: + raise ValueError( + f"Placement parent {binding.parent_id!r} has multiple default " + f"affordances for capability {capability!r}: " + f"{previous.entity_id!r} and {binding.entity_id!r}." + ) + parent_defaults[capability] = SceneAffordanceRef(binding.entity_id) + return defaults + + @dataclass(frozen=True, slots=True) class SimulationSceneBinding: """Build one authoritative registry from explicit simulation bindings.""" @@ -564,6 +692,8 @@ class SimulationSceneBinding: links: tuple[SimulationArticulationLinkBinding, ...] = () antipodal_grasps: tuple[AntipodalGraspAffordanceBinding, ...] = () articulation_operations: tuple[ArticulationOperationAffordanceBinding, ...] = () + support_surfaces: tuple[SupportSurfaceAffordanceBinding, ...] = () + containers: tuple[ContainerAffordanceBinding, ...] = () collision_world_mode: SceneCollisionWorldMode | None = None def __post_init__(self) -> None: @@ -574,6 +704,8 @@ def __post_init__(self) -> None: "links": SimulationArticulationLinkBinding, "antipodal_grasps": AntipodalGraspAffordanceBinding, "articulation_operations": ArticulationOperationAffordanceBinding, + "support_surfaces": SupportSurfaceAffordanceBinding, + "containers": ContainerAffordanceBinding, } all_ids: list[str] = [] for field_name, expected_type in expected_types.items(): @@ -596,6 +728,7 @@ def __post_init__(self) -> None: raise TypeError( "collision_world_mode must be SceneCollisionWorldMode or None." ) + _placement_defaults(self.support_surfaces, self.containers) def declare(self) -> SceneManifest: """Project the complete provider-free scene declaration. @@ -607,6 +740,10 @@ def declare(self) -> SceneManifest: objects = {item.entity_id: item for item in self.rigid_objects} articulations = {item.entity_id: item for item in self.articulations} links = {item.entity_id: item for item in self.links} + placement_defaults = _placement_defaults( + self.support_surfaces, + self.containers, + ) entries: list[SceneEntityManifest] = [] for binding in self.rigid_objects: @@ -615,15 +752,15 @@ def declare(self) -> SceneManifest: if binding.simulation_uid == binding.entity_id else (binding.simulation_uid,) ) - defaults = ( - {} - if binding.default_grasp_affordance is None - else { - GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( - binding.default_grasp_affordance - ) - } - ) + defaults = dict(placement_defaults.get(binding.entity_id, {})) + if binding.default_grasp_affordance is not None: + defaults.update( + { + GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( + binding.default_grasp_affordance + ) + } + ) entries.append( SceneEntityManifest( ref=SceneObjectRef(binding.entity_id), @@ -641,15 +778,15 @@ def declare(self) -> SceneManifest: if binding.simulation_uid == binding.entity_id else (binding.simulation_uid,) ) - defaults = ( - {} - if binding.default_operation_affordance is None - else { - ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( - SceneAffordanceRef(binding.default_operation_affordance) - ) - } - ) + defaults = dict(placement_defaults.get(binding.entity_id, {})) + if binding.default_operation_affordance is not None: + defaults.update( + { + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( + SceneAffordanceRef(binding.default_operation_affordance) + ) + } + ) entries.append( SceneEntityManifest( ref=SceneArticulationRef(binding.entity_id), @@ -675,6 +812,10 @@ def declare(self) -> SceneManifest: native_name=binding.native_link_name, dynamics=binding.dynamics, semantic_type=binding.semantic_type, + default_affordances=placement_defaults.get( + binding.entity_id, + {}, + ), ) ) @@ -728,6 +869,38 @@ def declare(self) -> SceneManifest: ) ) + for capability, payload_type, bindings in ( + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + self.support_surfaces, + ), + ( + PLACE_IN_AFFORDANCE_CAPABILITY, + ContainerAffordance, + self.containers, + ), + ): + for binding in bindings: + parent = _placement_parent_ref( + binding.parent_id, + objects=objects, + articulations=articulations, + links=links, + ) + entries.append( + SceneEntityManifest( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=parent, + native_name=binding.native_name, + affordance_capabilities=frozenset({capability}), + affordance_payload_type=payload_type, + affordance_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + relative_pose=binding.object_target_pose, + ) + ) + return SceneManifest(entries) def build(self, simulation: SimulationManager) -> SceneRegistry: @@ -741,6 +914,10 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: """ objects = {item.entity_id: item for item in self.rigid_objects} articulations = {item.entity_id: item for item in self.articulations} + placement_defaults = _placement_defaults( + self.support_surfaces, + self.containers, + ) geometry = { item.entity_id: item.geometry_provider for item in (*self.rigid_objects, *self.articulations) @@ -768,26 +945,26 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: entity_id = registration.ref.entity_id if isinstance(registration.ref, SceneObjectRef): binding = objects[entity_id] - defaults = ( - {} - if binding.default_grasp_affordance is None - else { - GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( - binding.default_grasp_affordance - ) - } - ) + defaults = dict(placement_defaults.get(entity_id, {})) + if binding.default_grasp_affordance is not None: + defaults.update( + { + GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( + binding.default_grasp_affordance + ) + } + ) else: binding = articulations[entity_id] - defaults = ( - {} - if binding.default_operation_affordance is None - else { - ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( - SceneAffordanceRef(binding.default_operation_affordance) - ) - } - ) + defaults = dict(placement_defaults.get(entity_id, {})) + if binding.default_operation_affordance is not None: + defaults.update( + { + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( + SceneAffordanceRef(binding.default_operation_affordance) + ) + } + ) registrations.append( replace( registration, @@ -841,6 +1018,10 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: native_name=binding.native_link_name, dynamics=binding.dynamics, semantic_type=binding.semantic_type, + default_affordances=placement_defaults.get( + binding.entity_id, + {}, + ), ) ) @@ -939,6 +1120,41 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: ) ) + for capability, payload_type, bindings in ( + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + self.support_surfaces, + ), + ( + PLACE_IN_AFFORDANCE_CAPABILITY, + ContainerAffordance, + self.containers, + ), + ): + for binding in bindings: + parent = _placement_parent_ref( + binding.parent_id, + objects=objects, + articulations=articulations, + links=links, + ) + payload = payload_type( + minimum_confidence=binding.minimum_confidence, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=parent, + native_name=binding.native_name, + affordance=payload, + affordance_capabilities=frozenset({capability}), + affordance_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + relative_pose=_pose_tensor(binding.object_target_pose), + ) + ) + return SceneRegistry( registrations, collision_world_mode=self.collision_world_mode, @@ -1466,6 +1682,7 @@ def declare(self) -> RobotSkillProfile: "AntipodalGraspAffordanceBinding", "ArticulationOperationAffordanceBinding", "ArticulationOperationTargetBinding", + "ContainerAffordanceBinding", "ControlPartCommandPreset", "ControlPartEndpointBinding", "ControlPartResourceBinding", @@ -1477,4 +1694,5 @@ def declare(self) -> RobotSkillProfile: "SimulationRobotResourceBinding", "SimulationRobotSkillProfileBinding", "SimulationSceneBinding", + "SupportSurfaceAffordanceBinding", ] diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 94a315f19..2760ce102 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -34,6 +34,7 @@ ) from .compiler import ( AnalyzedSemanticCall, + ContainerRelationTargetGrounder, GroundedHeldObjectGuard, GroundedPhaseEffectGate, GroundedSemanticCall, @@ -49,6 +50,7 @@ SemanticRelationTarget, SemanticSkillCompiler, SemanticWorkflow, + SupportSurfaceRelationTargetGrounder, ) from .effects import ( ArticulationJointStateExpectation, @@ -175,6 +177,8 @@ ) from .scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, + PLACEMENT_TARGET_AFFORDANCE_REVISION, SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, AmbiguousSceneAffordanceError, @@ -197,6 +201,7 @@ SceneLinkRef, SceneObjectRef, SceneRegistry, + SupportSurfaceAffordance, UnsupportedSceneAffordanceError, ) from .runtime import ( @@ -242,6 +247,8 @@ "ControlPartEvidenceAddress", "ControlPartRobotEvidenceSource", "ControlPartSimulationEvidenceProvider", + "ContainerAffordance", + "ContainerRelationTargetGrounder", "CoordinatedHeldObjectCleanupExpectation", "COMPOSITE_EFFECT_MONITOR_ID", "COMPOSITE_EFFECT_MONITOR_REVISION", @@ -293,6 +300,7 @@ "POSE_RELATION_EFFECT_CHANNEL", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", + "PLACEMENT_TARGET_AFFORDANCE_REVISION", "PathPart", "OperateArticulation", "ParallelBarrierUpdate", @@ -367,6 +375,8 @@ "SemanticEffectSpec", "SymbolicStateDomain", "SymbolicStateKey", + "SupportSurfaceAffordance", + "SupportSurfaceRelationTargetGrounder", "SemanticHandOverTarget", "SemanticIntegrationManifest", "SemanticLowering", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 5ad44e4bb..34680d85c 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -93,6 +93,8 @@ ) from .scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, + PLACEMENT_TARGET_AFFORDANCE_REVISION, SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, ArticulationJointEvidenceAddress, @@ -100,6 +102,7 @@ PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneObjectRef, + SupportSurfaceAffordance, ) OptionT = TypeVar("OptionT", bound=ActionOptions) @@ -179,6 +182,54 @@ def ground( """ +class SupportSurfaceRelationTargetGrounder(RelationTargetGrounder): + """Ground a declared support target frame to a late-bound object pose.""" + + capability: ClassVar[str] = PLACE_ON_AFFORDANCE_CAPABILITY + affordance_type: ClassVar[type[Affordance]] = SupportSurfaceAffordance + affordance_revision: ClassVar[str] = PLACEMENT_TARGET_AFFORDANCE_REVISION + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> SceneEntityPose: + """Return the current support-relative target frame.""" + del context + if type(affordance) is not SupportSurfaceAffordance: + raise TypeError("affordance must be exactly SupportSurfaceAffordance.") + return SceneEntityPose( + relation.affordance.entity_id, + minimum_confidence=affordance.minimum_confidence, + ) + + +class ContainerRelationTargetGrounder(RelationTargetGrounder): + """Ground a declared container target frame to a late-bound object pose.""" + + capability: ClassVar[str] = PLACE_IN_AFFORDANCE_CAPABILITY + affordance_type: ClassVar[type[Affordance]] = ContainerAffordance + affordance_revision: ClassVar[str] = PLACEMENT_TARGET_AFFORDANCE_REVISION + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> SceneEntityPose: + """Return the current container-relative target frame.""" + del context + if type(affordance) is not ContainerAffordance: + raise TypeError("affordance must be exactly ContainerAffordance.") + return SceneEntityPose( + relation.affordance.entity_id, + minimum_confidence=affordance.minimum_confidence, + ) + + @dataclass(frozen=True, slots=True) class SemanticObjectTarget: """One object-space look-ahead target. @@ -2614,6 +2665,7 @@ def _broadcast_joint_position( __all__ = [ "AnalyzedSemanticCall", + "ContainerRelationTargetGrounder", "GroundedHeldObjectGuard", "GroundedPhaseEffectGate", "GroundedSemanticCall", @@ -2630,4 +2682,5 @@ def _broadcast_joint_position( "SemanticRelationTarget", "SemanticSkillCompiler", "SemanticWorkflow", + "SupportSurfaceRelationTargetGrounder", ] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py index 3cc15309d..16dc7ee4e 100644 --- a/embodichain/lab/sim/skills/scene.py +++ b/embodichain/lab/sim/skills/scene.py @@ -60,6 +60,9 @@ PLACE_IN_AFFORDANCE_CAPABILITY = "affordance.place.in" """Capability for an affordance that defines an ``inside`` placement relation.""" +PLACEMENT_TARGET_AFFORDANCE_REVISION = "1" +"""Schema revision for built-in support/container object-target frames.""" + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID = "builtin.scene_articulation" """Stable route for explicitly injected articulation-joint observations.""" @@ -92,6 +95,58 @@ class AmbiguousSceneAffordanceError(ValueError): """Raised when compatible affordances lack one explicitly scoped default.""" +@dataclass +class SupportSurfaceAffordance(Affordance): + """Typed target frame for placing an object's origin on a support surface. + + The registered affordance pose is the desired object pose, expressed + relative to its parent scene entity. The optional confidence threshold is + enforced whenever that late-bound target pose is resolved. + + Args: + minimum_confidence: Minimum confidence accepted while resolving the + late-bound target pose. + """ + + minimum_confidence: float = 0.0 + + def __post_init__(self) -> None: + if isinstance(self.minimum_confidence, bool) or not isinstance( + self.minimum_confidence, + (int, float), + ): + raise TypeError("minimum_confidence must be a number.") + self.minimum_confidence = float(self.minimum_confidence) + if not 0.0 <= self.minimum_confidence <= 1.0: + raise ValueError("minimum_confidence must be in [0, 1].") + + +@dataclass +class ContainerAffordance(Affordance): + """Typed target frame for placing an object's origin inside a container. + + The registered affordance pose is the desired object pose, expressed + relative to its parent scene entity. The optional confidence threshold is + enforced whenever that late-bound target pose is resolved. + + Args: + minimum_confidence: Minimum confidence accepted while resolving the + late-bound target pose. + """ + + minimum_confidence: float = 0.0 + + def __post_init__(self) -> None: + if isinstance(self.minimum_confidence, bool) or not isinstance( + self.minimum_confidence, + (int, float), + ): + raise TypeError("minimum_confidence must be a number.") + self.minimum_confidence = float(self.minimum_confidence) + if not 0.0 <= self.minimum_confidence <= 1.0: + raise ValueError("minimum_confidence must be in [0, 1].") + + def _validate_identifier(value: str, name: str) -> None: """Validate an exact, non-empty identifier without normalizing it.""" if type(value) is not str or not value or value != value.strip(): @@ -2166,7 +2221,9 @@ def _pose_change_mask( "ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY", "ArticulationJointEvidenceAddress", "AmbiguousSceneAffordanceError", + "ContainerAffordance", "GRASP_AFFORDANCE_CAPABILITY", + "PLACEMENT_TARGET_AFFORDANCE_REVISION", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", "RegistrySceneProvider", @@ -2186,5 +2243,6 @@ def _pose_change_mask( "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "SupportSurfaceAffordance", "UnsupportedSceneAffordanceError", ] diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py index 8df5319ec..5e0772afb 100644 --- a/tests/gym/envs/expert_program/test_catalog.py +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -32,12 +32,15 @@ IntegrationFingerprintMismatch, SimulationArticulationLinkBinding, SimulationExpertProgramRegistration, + SimulationRigidObjectBinding, SimulationSceneBinding, + SupportSurfaceAffordanceBinding, decode_expert_program, ) from embodichain.lab.gym.utils.registration import EnvSpec from embodichain.lab.sim.atomic_actions import Affordance, PlanningContext from embodichain.lab.sim.skills import ( + PLACEMENT_TARGET_AFFORDANCE_REVISION, PLACE_ON_AFFORDANCE_CAPABILITY, BoundSemanticCall, ControlPartEndpoint, @@ -48,6 +51,7 @@ RelationTargetGrounder, SemanticCallCatalog, SceneAffordanceRef, + SceneDynamics, SceneEntityManifest, SceneManifest, SceneObjectRef, @@ -55,6 +59,8 @@ RegisteredSemanticCall, SemanticCallDescriptor, SkillPolicyPreset, + SupportSurfaceAffordance, + SupportSurfaceRelationTargetGrounder, WorkflowRecoveryPolicy, builtin_semantic_call_catalog, ) @@ -742,6 +748,58 @@ def test_catalog_accepts_linked_place_relation_with_exact_grounder_key() -> None assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "place" +def test_standard_support_binding_installs_grounder_without_task_code() -> None: + """A task relation declaration supplies its production grounder implicitly.""" + base = create_cube_scene_binding(grasp_samples=32) + scene = replace( + base, + registry_id="relation_scene", + rigid_objects=( + *base.rigid_objects, + SimulationRigidObjectBinding( + entity_id="support", + simulation_uid="support", + dynamics=SceneDynamics.STATIC, + semantic_type="support_surface", + ), + ), + support_surfaces=( + SupportSurfaceAffordanceBinding( + entity_id="support_top", + parent_id="support", + native_name="top_object_target", + is_default=True, + ), + ), + ) + registration = SimulationExpertProgramRegistration( + scene_binding=scene, + robot_profile_binding=create_cube_robot_profile_binding(), + ) + + assert len(registration.relation_grounders) == 1 + assert type(registration.relation_grounders[0]) is ( + SupportSurfaceRelationTargetGrounder + ) + assert registration.catalog.relation_grounder_keys == frozenset( + { + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + ) + } + ) + + program = decode_expert_program( + _place_relation_payload(), + validation_context=registration.catalog, + ) + compiled = registration.catalog.preflight(program) + + assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "place" + + @pytest.mark.parametrize( ("overrides", "path"), ( diff --git a/tests/gym/envs/expert_program/test_simulation.py b/tests/gym/envs/expert_program/test_simulation.py index b3a6fc664..2cb1d27c4 100644 --- a/tests/gym/envs/expert_program/test_simulation.py +++ b/tests/gym/envs/expert_program/test_simulation.py @@ -27,6 +27,7 @@ AntipodalGraspAffordanceBinding, ArticulationOperationAffordanceBinding, ArticulationOperationTargetBinding, + ContainerAffordanceBinding, ControlPartCommandPreset, ControlPartEndpointBinding, ControlPartResourceBinding, @@ -38,6 +39,7 @@ SimulationRobotResourceBinding, SimulationRobotSkillProfileBinding, SimulationSceneBinding, + SupportSurfaceAffordanceBinding, ) from embodichain.lab.sim.atomic_actions import ( AntipodalAffordance, @@ -48,13 +50,17 @@ ) from embodichain.lab.sim.skills import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneArticulationRef, SceneDynamics, SceneLinkRef, SceneObjectRef, SkillPolicyPreset, + SupportSurfaceAffordance, ) from embodichain.lab.sim.skills.profiles import ResourceEndpoint @@ -212,6 +218,60 @@ def _scene_binding() -> SimulationSceneBinding: }, ), ), + support_surfaces=( + SupportSurfaceAffordanceBinding( + entity_id="cube_support_target", + parent_id="cube", + native_name="support_target", + object_target_pose=( + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.25, + 0.0, + 0.0, + 0.0, + 1.0, + ), + minimum_confidence=0.7, + is_default=True, + ), + ), + containers=( + ContainerAffordanceBinding( + entity_id="drawer_inside_target", + parent_id="drawer_handle_link", + native_name="inside_target", + object_target_pose=( + 1.0, + 0.0, + 0.0, + 0.1, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + ), + minimum_confidence=0.8, + is_default=True, + ), + ), ) @@ -293,6 +353,28 @@ def test_scene_binding_builds_existing_registry_contracts() -> None: snapshot.entities["drawer_handle_operation"].pose, simulation.articulation.link_pose, ) + support_ref = registry.resolve_affordance( + "cube", + capability=PLACE_ON_AFFORDANCE_CAPABILITY, + ) + support = registry.lookup(support_ref).affordance + assert type(support) is SupportSurfaceAffordance + assert support.minimum_confidence == pytest.approx(0.7) + assert torch.equal( + snapshot.entities[support_ref.entity_id].pose[:, 2, 3], + torch.full((_BATCH_SIZE,), 0.25), + ) + container_ref = registry.resolve_affordance( + "drawer_handle_link", + capability=PLACE_IN_AFFORDANCE_CAPABILITY, + ) + container = registry.lookup(container_ref).affordance + assert type(container) is ContainerAffordance + assert container.minimum_confidence == pytest.approx(0.8) + assert torch.allclose( + snapshot.entities[container_ref.entity_id].pose[:, 0, 3], + torch.tensor((0.4, 0.5)), + ) def test_scene_binding_fails_closed_on_missing_native_entity() -> None: diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index 1bc9b0ba4..b5de61a65 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -67,6 +67,8 @@ builtin_semantic_call_catalog, ) from embodichain.lab.sim.skills.compiler import ( + ContainerRelationTargetGrounder, + GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, HeldObjectGuardBaseline, @@ -76,6 +78,8 @@ SemanticObjectTarget, SemanticRelationTarget, SemanticSkillCompiler, + SemanticWorkflow, + SupportSurfaceRelationTargetGrounder, ) from embodichain.lab.sim.skills.effects import ( BinaryEffectClause, @@ -109,7 +113,10 @@ SkillPolicyPreset, ) from embodichain.lab.sim.skills.scene import ( + ContainerAffordance, GRASP_AFFORDANCE_CAPABILITY, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneCollisionRole, @@ -117,6 +124,7 @@ SceneEntityRegistration, SceneObjectRef, SceneRegistry, + SupportSurfaceAffordance, ) _MOTION_CAPABILITIES = frozenset( @@ -626,6 +634,47 @@ def _held_context( ) +@pytest.mark.parametrize( + ("grounder", "capability", "affordance_type"), + ( + ( + SupportSurfaceRelationTargetGrounder(), + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + ), + ( + ContainerRelationTargetGrounder(), + PLACE_IN_AFFORDANCE_CAPABILITY, + ContainerAffordance, + ), + ), +) +def test_builtin_relation_grounders_preserve_late_pose_and_confidence( + grounder: RelationTargetGrounder, + capability: str, + affordance_type: type[Affordance], +) -> None: + """Production relation grounders keep target frames live and typed.""" + registry, _ = _scene_registry() + relation = SemanticRelationTarget( + capability=capability, + affordance=SceneAffordanceRef("declared_target"), + payload_type=affordance_type, + payload_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + ) + + target = grounder.ground( + relation, + affordance=affordance_type(minimum_confidence=0.65), + context=_context(registry), + ) + + assert type(target) is SceneEntityPose + assert target.entity_id == "declared_target" + assert target.relative_pose is None + assert target.minimum_confidence == pytest.approx(0.65) + + def test_curated_analysis_selects_exact_preset_monitor_without_creating_it() -> None: registry, providers = _scene_registry() factory = _CountingRelationMonitorFactory() From ba13943df0c84a048f42cfb62d75aee41554c08b Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 23:54:59 +0800 Subject: [PATCH 27/29] feat(expert-program): validate parallel joint segments --- ...mbodichain.lab.gym.envs.expert_program.rst | 2 + docs/source/api_reference/public_api.rst | 10 + .../lab/gym/envs/expert_program/__init__.py | 6 + .../lab/gym/envs/expert_program/catalog.py | 15 +- .../lab/gym/envs/expert_program/extensions.py | 10 +- .../expert_program/simulation_environment.py | 2 + .../simulation_parallel_safety.py | 356 ++++++++++++++++++ embodichain/lab/sim/planners/base_planner.py | 35 ++ .../lab/sim/planners/curobo/curobo_planner.py | 105 ++++++ .../lab/sim/planners/motion_generator.py | 62 +++ tests/gym/envs/expert_program/test_catalog.py | 50 ++- .../envs/expert_program/test_extensions.py | 11 +- .../test_simulation_environment.py | 33 +- .../test_simulation_parallel_safety.py | 238 ++++++++++++ tests/sim/planners/test_curobo_planner.py | 68 ++++ 15 files changed, 985 insertions(+), 18 deletions(-) create mode 100644 embodichain/lab/gym/envs/expert_program/simulation_parallel_safety.py create mode 100644 tests/gym/envs/expert_program/test_simulation_parallel_safety.py diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst index 82b377833..970e5dadd 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst @@ -49,6 +49,8 @@ embodichain.lab.gym.envs.expert_program ControlPartCommandPreset ContainerAffordanceBinding CyclicPoseTargetCfg + CuroboParallelCommandSafetyValidator + CuroboParallelSafetyValidatorFactory DeclarativeCfgValue DemoBridgeError EXPERT_PROGRAM_SCHEMA_VERSION diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 0eb05da52..c11b81455 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -318,6 +318,16 @@ embodichain.lab.gym.envs.expert_program.simulation_policies SimulationSegmentPolicyPort default_simulation_settle_presets +embodichain.lab.gym.envs.expert_program.simulation_parallel_safety +------------------------------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation_parallel_safety + +.. autosummary:: + + CuroboParallelCommandSafetyValidator + CuroboParallelSafetyValidatorFactory + embodichain.lab.gym.envs.expert_program.simulation_handover ------------------------------------------------------------ diff --git a/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py index 3a786378d..e22b51b0a 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -158,6 +158,10 @@ create_simulation_expert_program_adapter, ) from .simulation_handover import ConfiguredHandOverPoseProvider +from .simulation_parallel_safety import ( + CuroboParallelCommandSafetyValidator, + CuroboParallelSafetyValidatorFactory, +) from .simulation_policies import ( SimulationSegmentPolicyPort, default_simulation_settle_presets, @@ -192,6 +196,8 @@ "ControlPartResourceBinding", "ConfiguredHandOverPoseProvider", "CyclicPoseTargetCfg", + "CuroboParallelCommandSafetyValidator", + "CuroboParallelSafetyValidatorFactory", "DeclarativeCfgValue", "DemoBridgeError", "EXPERT_PROGRAM_SCHEMA_VERSION", diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index 64e2c553a..29cc5276f 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -1635,14 +1635,27 @@ def create_parallel_safety_validator( *, simulation: object, robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, ) -> ParallelCommandSafetyValidator | None: """Create and strictly validate the registration-owned live safety gate.""" self.assert_unchanged() factory = self.parallel_safety_factory if factory is None: return None + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not robot: + raise ValueError("engine and factory must reference the exact same robot.") with self._parallel_safety_validator_lock: - validator = factory.create(simulation=simulation, robot=robot) + validator = factory.create( + simulation=simulation, + robot=robot, + scene_registry=scene_registry, + engine=engine, + ) if not isinstance(validator, ParallelCommandSafetyValidator): raise TypeError( "parallel_safety_factory.create() must return a " diff --git a/embodichain/lab/gym/envs/expert_program/extensions.py b/embodichain/lab/gym/envs/expert_program/extensions.py index a0d549f02..f5b019d8b 100644 --- a/embodichain/lab/gym/envs/expert_program/extensions.py +++ b/embodichain/lab/gym/envs/expert_program/extensions.py @@ -28,7 +28,7 @@ from dataclasses import dataclass, fields, is_dataclass from enum import Enum from types import MappingProxyType -from typing import ClassVar, Protocol, runtime_checkable +from typing import ClassVar, Protocol, runtime_checkable, TYPE_CHECKING import torch @@ -57,6 +57,10 @@ RuntimeTransportActionEncoder, ) +if TYPE_CHECKING: + from embodichain.lab.sim.atomic_actions import AtomicActionEngine + from embodichain.lab.sim.skills import SceneRegistry + VersionedKey = tuple[str, str] """Exact ``(provider_or_projector_id, revision)`` registry key.""" @@ -450,8 +454,10 @@ def create( *, simulation: object, robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, ) -> ParallelCommandSafetyValidator: - """Create one live validator bound to the exact simulation and robot.""" + """Create one live gate bound to the exact assembled runtime.""" @dataclass(frozen=True, slots=True) diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py index d1d24ba67..0c3c83c7e 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -1056,6 +1056,8 @@ def create_parallel_command_safety_validator( validator = self._registration.create_parallel_safety_validator( simulation=self._simulation, robot=self._robot, + scene_registry=scene_registry, + engine=engine, ) if not isinstance(validator, ParallelCommandSafetyValidator): raise TypeError( diff --git a/embodichain/lab/gym/envs/expert_program/simulation_parallel_safety.py b/embodichain/lab/gym/envs/expert_program/simulation_parallel_safety.py new file mode 100644 index 000000000..97317e3f6 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation_parallel_safety.py @@ -0,0 +1,356 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""cuRobo-backed physical safety gate for synchronized simulation commands.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +import math +from typing import ClassVar + +import torch + +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + JointPositionPayload, + JointPositionTarget, + RuntimeCommandFrame, +) +from embodichain.lab.sim.planners import CuroboPlanner, MotionGenerator +from embodichain.lab.sim.skills import ( + ParallelSafetyError, + RegistrySceneProvider, + SceneRegistry, +) + + +def _identifier(value: object, *, field_name: str) -> str: + """Validate one exact identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +@dataclass(frozen=True, slots=True) +class CuroboParallelSafetyValidatorFactory: + """Create exact-sample collision gates for one aggregate control part. + + ``validation_control_part`` must contain every joint that any parallel + branch can command. A common dual-arm example is ``"dual_arm"``. The + cuRobo model for that part remains the authoritative bounds, self-collision, + and world-collision model. + + Args: + validation_control_part: Aggregate robot control part containing every + joint that a parallel lane may command. + max_joint_step: Maximum absolute joint displacement between collision + samples in radians or the joint's native linear unit. + max_interpolation_samples: Fail-closed upper bound on samples per frame. + """ + + validator_id: ClassVar[str] = "builtin.simulation.curobo_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {JointPositionTarget.TRANSPORT_ID} + ) + + validation_control_part: str + max_joint_step: float = 0.025 + max_interpolation_samples: int = 256 + + def __post_init__(self) -> None: + _identifier( + self.validation_control_part, + field_name="validation_control_part", + ) + if isinstance(self.max_joint_step, bool) or not isinstance( + self.max_joint_step, + (int, float), + ): + raise TypeError("max_joint_step must be a real number.") + normalized_step = float(self.max_joint_step) + if not math.isfinite(normalized_step) or normalized_step <= 0.0: + raise ValueError("max_joint_step must be finite and positive.") + object.__setattr__(self, "max_joint_step", normalized_step) + if ( + type(self.max_interpolation_samples) is not int + or self.max_interpolation_samples < 2 + or self.max_interpolation_samples > 4096 + ): + raise ValueError( + "max_interpolation_samples must be an integer in [2, 4096]." + ) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> CuroboParallelCommandSafetyValidator: + """Create one fresh validator bound to the assembled live runtime.""" + del simulation + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not robot: + raise ValueError("engine and factory must reference the exact same robot.") + return CuroboParallelCommandSafetyValidator( + robot=robot, + motion_generator=engine.motion_generator, + scene_registry=scene_registry, + validation_control_part=self.validation_control_part, + max_joint_step=self.max_joint_step, + max_interpolation_samples=self.max_interpolation_samples, + ) + + +class CuroboParallelCommandSafetyValidator: + """Validate the exact synchronized joint segment before transport dispatch. + + Args: + robot: Live robot supplying measured joint state and control-part IDs. + motion_generator: Runtime motion generator backed by exact cuRobo. + scene_registry: Authoritative live collision-scene registry. + validation_control_part: Aggregate control part for merged commands. + max_joint_step: Maximum displacement between collision samples. + max_interpolation_samples: Fail-closed sample-count upper bound. + """ + + def __init__( + self, + *, + robot: object, + motion_generator: MotionGenerator, + scene_registry: SceneRegistry, + validation_control_part: str, + max_joint_step: float, + max_interpolation_samples: int, + ) -> None: + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(motion_generator, MotionGenerator): + raise TypeError("motion_generator must be a MotionGenerator.") + if type(motion_generator.planner) is not CuroboPlanner: + raise TypeError( + "CuroboParallelCommandSafetyValidator requires the active " + "CuroboPlanner backend." + ) + if not motion_generator.supports_joint_trajectory_validation: + raise ValueError( + "The active motion generator does not validate exact joint " + "trajectories." + ) + get_joint_ids = getattr(robot, "get_joint_ids", None) + if not callable(get_joint_ids): + raise TypeError("robot must provide get_joint_ids().") + joint_ids = tuple(get_joint_ids(name=validation_control_part)) + if not joint_ids or not all( + type(joint_id) is int and joint_id >= 0 for joint_id in joint_ids + ): + raise ValueError( + "The validation control part must resolve non-negative joint IDs." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ValueError("The validation control part joint IDs must be unique.") + self._robot = robot + self._motion_generator = motion_generator + self._scene_registry = scene_registry + self._validation_control_part = validation_control_part + self._validation_joint_ids = joint_ids + self._local_joint_columns = { + joint_id: index for index, joint_id in enumerate(joint_ids) + } + self._max_joint_step = max_joint_step + self._max_interpolation_samples = max_interpolation_samples + self._scene_provider: RegistrySceneProvider | None = None + self._scene_timestamp = 0.0 + + def validate( + self, + *, + branch_frames: Mapping[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + """Reject a merged command whose exact interpolated segment collides.""" + if not isinstance(branch_frames, Mapping) or len(branch_frames) < 2: + raise TypeError("branch_frames must contain at least two branch frames.") + if type(merged_frame) is not RuntimeCommandFrame: + raise TypeError("merged_frame must be exactly RuntimeCommandFrame.") + for branch_id, frame in branch_frames.items(): + _identifier(branch_id, field_name="parallel branch IDs") + if type(frame) is not RuntimeCommandFrame: + raise TypeError( + "branch_frames values must be exact RuntimeCommandFrame values." + ) + if not torch.equal(frame.env_ids, merged_frame.env_ids): + raise ValueError("Parallel branch and merged env_ids must match.") + + active = merged_frame.active_mask + if not bool(active.any().item()): + return + current = self._current_control_part_qpos(merged_frame.env_ids) + target = current.clone() + commanded_joint_ids: set[int] = set() + for command in merged_frame.commands: + if ( + type(command.target) is not JointPositionTarget + or type(command.payload) is not JointPositionPayload + ): + raise ParallelSafetyError( + "cuRobo parallel safety accepts only exact joint-position " + "targets and payloads." + ) + missing = sorted( + set(command.target.joint_ids).difference(self._local_joint_columns) + ) + if missing: + raise ParallelSafetyError( + f"Parallel target {command.target.target_id!r} commands joints " + f"{missing} outside validation control part " + f"{self._validation_control_part!r}." + ) + for payload_column, joint_id in enumerate(command.target.joint_ids): + if joint_id in commanded_joint_ids: + raise ParallelSafetyError( + f"Merged parallel commands overlap on joint {joint_id}." + ) + commanded_joint_ids.add(joint_id) + target[:, self._local_joint_columns[joint_id]] = ( + command.payload.positions[:, payload_column] + ) + target = torch.where(active[:, None], target, current) + trajectory = self._interpolate(current, target) + obstacle_poses = self._obstacle_poses( + env_ids=merged_frame.env_ids, + device=trajectory.device, + dtype=trajectory.dtype, + ) + validity = self._motion_generator.validate_joint_trajectory( + trajectory, + control_part=self._validation_control_part, + obstacle_poses=obstacle_poses, + ) + row_valid = validity.all(dim=1) + failed = active & ~row_valid + if not bool(failed.any().item()): + return + failed_rows = failed.nonzero(as_tuple=False).flatten() + failed_env_ids = merged_frame.env_ids.index_select(0, failed_rows) + first_invalid_samples = tuple( + int((~validity[row]).nonzero(as_tuple=False)[0, 0].item()) + for row in failed_rows.detach().cpu().tolist() + ) + raise ParallelSafetyError( + "Merged parallel joint segment is not collision-free for env IDs " + f"{tuple(failed_env_ids.detach().cpu().tolist())}; first invalid " + f"samples={first_invalid_samples}." + ) + + def _current_control_part_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + """Read current full robot state and select the validator joint order.""" + getter = getattr(self._robot, "get_qpos", None) + if not callable(getter): + raise TypeError("robot must provide get_qpos().") + full = getter(target=False) + if ( + not isinstance(full, torch.Tensor) + or not full.is_floating_point() + or full.dim() != 2 + or not bool(torch.isfinite(full).all().item()) + ): + raise ValueError("robot.get_qpos() must return finite floating (B, D).") + if env_ids.device != full.device: + raise ValueError("Parallel env_ids and robot qpos must share a device.") + if ( + bool((env_ids < 0).any().item()) + or int(env_ids.max().item()) >= full.shape[0] + ): + raise ValueError("Parallel env_ids do not address robot qpos rows.") + if max(self._validation_joint_ids) >= full.shape[1]: + raise ValueError( + "Validation control-part joint IDs exceed robot qpos width." + ) + rows = full.index_select(0, env_ids) + columns = torch.tensor( + self._validation_joint_ids, + dtype=torch.long, + device=full.device, + ) + return rows.index_select(1, columns).clone() + + def _interpolate( + self, + current: torch.Tensor, + target: torch.Tensor, + ) -> torch.Tensor: + """Densify the exact controller segment under a bounded joint step.""" + max_delta = float((target - current).abs().max().item()) + sample_count = max(2, math.ceil(max_delta / self._max_joint_step) + 1) + if sample_count > self._max_interpolation_samples: + raise ParallelSafetyError( + "Merged parallel joint segment needs " + f"{sample_count} collision samples at max_joint_step=" + f"{self._max_joint_step}, exceeding configured limit " + f"{self._max_interpolation_samples}." + ) + alpha = torch.linspace( + 0.0, + 1.0, + sample_count, + device=current.device, + dtype=current.dtype, + ) + return ( + current[:, None, :] + alpha[None, :, None] * (target - current)[:, None, :] + ) + + def _obstacle_poses( + self, + *, + env_ids: torch.Tensor, + device: torch.device, + dtype: torch.dtype, + ) -> Mapping[str, torch.Tensor] | None: + """Observe the exact dynamic collision world for this safety decision.""" + if not self._scene_registry.dynamic_collision_entity_ids: + return None + if self._scene_provider is None: + self._scene_provider = self._scene_registry.make_scene_provider( + batch_size=int(env_ids.numel()) + ) + snapshot = self._scene_provider.snapshot( + timestamp=self._scene_timestamp, + env_ids=env_ids, + ) + self._scene_timestamp += 1.0 + return snapshot.collision_obstacle_poses( + batch_size=int(env_ids.numel()), + device=device, + dtype=dtype, + ) + + +__all__ = [ + "CuroboParallelCommandSafetyValidator", + "CuroboParallelSafetyValidatorFactory", +] diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index 3533c6c3c..5dabfc930 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -233,6 +233,12 @@ def __init__(self, cfg: BasePlannerCfg): waypoint count. """ + supports_collision_world_updates: bool = False + """Whether per-plan dynamic obstacle poses can update the collision world.""" + + supports_joint_trajectory_validation: bool = False + """Whether exact joint samples can be checked against bounds/collisions.""" + @property def collision_world_info(self) -> CollisionWorldInfo | None: """Return the planner's collision-world contract, if it has one.""" @@ -297,6 +303,35 @@ def with_collision_world( """ return options + def validate_joint_trajectory( + self, + trajectory: torch.Tensor, + *, + control_part: str, + obstacle_poses: Mapping[str, torch.Tensor] | None = None, + ) -> torch.Tensor: + """Validate exact joint samples without replacing their path. + + Backends that implement this contract must evaluate every supplied + sample against joint bounds, self-collision, and their configured world + collision model. They return a boolean mask with shape ``(B, T)``. + + Args: + trajectory: Simulator-order joint samples with shape ``(B, T, D)``. + control_part: Robot control part whose ordered joints form ``D``. + obstacle_poses: Optional current dynamic-obstacle world poses. + + Returns: + Per-environment, per-sample validity mask. + + Raises: + NotImplementedError: Always for the base planner. + """ + del trajectory, control_part, obstacle_poses + raise NotImplementedError( + f"{type(self).__name__} does not validate exact joint trajectories." + ) + @validate_plan_options @abstractmethod def plan( diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index f350f5038..283146bec 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -716,6 +716,7 @@ def _require_curobo(log_level: str = "error") -> "Any": try: planner_mod = importlib.import_module("curobo.motion_planner") batch_mod = importlib.import_module("curobo.batch_motion_planner") + collision_mod = importlib.import_module("curobo.collision_checking") types_mod = importlib.import_module("curobo.types") except ModuleNotFoundError as exc: raise ImportError( @@ -730,6 +731,8 @@ def _require_curobo(log_level: str = "error") -> "Any": MotionPlanner=planner_mod.MotionPlanner, MotionPlannerCfg=planner_mod.MotionPlannerCfg, BatchMotionPlanner=batch_mod.BatchMotionPlanner, + RobotCollisionChecker=collision_mod.RobotCollisionChecker, + RobotCollisionCheckerCfg=collision_mod.RobotCollisionCheckerCfg, JointState=types_mod.JointState, Pose=types_mod.Pose, GoalToolPose=types_mod.GoalToolPose, @@ -816,6 +819,8 @@ class CuroboPlanner(BasePlanner): """ supported_move_types = frozenset({MoveType.EEF_MOVE, MoveType.JOINT_MOVE}) + supports_collision_world_updates = True + supports_joint_trajectory_validation = True @property def preserve_plan_samples(self) -> bool: @@ -966,6 +971,105 @@ def prepare_backend( "use_cuda_graph": backend.use_cuda_graph, } + def validate_joint_trajectory( + self, + trajectory: torch.Tensor, + *, + control_part: str, + obstacle_poses: Mapping[str, torch.Tensor] | None = None, + ) -> torch.Tensor: + """Validate every supplied joint sample with cuRobo collision models. + + The samples are not replanned or replaced. They are mapped from the + simulator's control-part order into the exact cuRobo model, then checked + against joint bounds, self-collision, and the live world collision + checker. Calling the configuration validator once per horizon sample + works around cuRobo 0.8's configuration-only ``validate`` contract while + retaining batched environments. + """ + if ( + not isinstance(trajectory, torch.Tensor) + or not trajectory.is_floating_point() + or trajectory.dim() != 3 + or 0 in trajectory.shape + or not bool(torch.isfinite(trajectory).all().item()) + ): + raise ValueError("trajectory must be finite floating shape (B, T, D).") + batch_size, horizon, dof = trajectory.shape + backend = self._get_backend( + control_part, + batch_size, + MoveType.JOINT_MOVE, + ) + if dof != len(backend.sim_joint_names): + raise ValueError( + f"Trajectory for {control_part!r} has {dof} joints, expected " + f"{len(backend.sim_joint_names)}." + ) + + dynamic_ids = tuple(self.cfg.world.dynamic_obstacle_names) + if dynamic_ids and obstacle_poses is None: + raise ValueError( + "Live dynamic-obstacle poses are required for exact trajectory " + "validation." + ) + poses = None if obstacle_poses is None else dict(obstacle_poses) + if poses is not None: + _validate_dynamic_obstacles(poses, list(dynamic_ids)) + missing = sorted(set(dynamic_ids).difference(poses)) + extra = sorted(set(poses).difference(dynamic_ids)) + if missing or extra: + raise ValueError( + "Dynamic collision obstacle IDs do not match the planner " + f"configuration; missing={missing}, extra={extra}." + ) + if poses: + base_pose_inv = pose_inv(self._get_sim_base_pose(backend, batch_size)) + self.update_dynamic_obstacles( + poses, + backend, + base_pose_inv, + ) + + if backend.collision_checker is None: + checker_cfg = self._bindings.RobotCollisionCheckerCfg.load_from_config( + robot_config=backend.profile.robot_config_path, + scene_collision_checker=(backend.planner.scene_collision_checker), + device_cfg=self._bindings.DeviceCfg(device=self._curobo_device), + num_envs=batch_size, + collision_activation_distance=self.cfg.collision_activation_distance, + ) + backend.collision_checker = self._bindings.RobotCollisionChecker( + checker_cfg + ) + + self._to_curobo_joint_state(trajectory[:, 0], backend) + assert backend.sim_to_curobo_col_idx is not None + curobo_trajectory = trajectory.to( + device=self._curobo_device, + dtype=torch.float32, + ).index_select(-1, backend.sim_to_curobo_col_idx) + env_query_idx = ( + torch.arange(batch_size, device=self._curobo_device, dtype=torch.int32) + if self.cfg.world.multi_env + else None + ) + samples: list[torch.Tensor] = [] + device_context = ( + torch.cuda.device(self._curobo_device) + if self._curobo_device.type == "cuda" + else nullcontext() + ) + with device_context: + for sample_index in range(horizon): + sample = curobo_trajectory[:, sample_index : sample_index + 1] + valid = backend.collision_checker.validate( + sample, + env_query_idx=env_query_idx, + ) + samples.append(valid[:, 0].to(torch.bool)) + return torch.stack(samples, dim=1).to(trajectory.device) + def with_collision_world( self, options: PlanOptions, @@ -2423,6 +2527,7 @@ class _CuroboBackend: batch_size: int use_cuda_graph: bool planning_mode: MoveType + collision_checker: "Any | None" = None # Lazily-built device-tensor caches for the shared post-processing. The # cuRobo joint order and the profile's fixed transforms are stable for a # planner's life, so these are built once on first use and reused across diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index a2ee41596..a15c224b8 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -200,6 +200,18 @@ def supports_dynamic_collision_world(self) -> bool: info = self.collision_world_info return info is not None and info.supports_updates + @property + def supports_joint_trajectory_validation(self) -> bool: + """Whether the backend checks exact joint samples for collisions.""" + return ( + getattr( + self.planner, + "supports_joint_trajectory_validation", + False, + ) + is True + ) + @property def dynamic_collision_entity_ids(self) -> tuple[str, ...]: """Return canonical dynamic-obstacle IDs declared by the planner.""" @@ -325,6 +337,56 @@ def bind_collision_world( ) return bound + def validate_joint_trajectory( + self, + trajectory: torch.Tensor, + *, + control_part: str, + obstacle_poses: Mapping[str, torch.Tensor] | None = None, + ) -> torch.Tensor: + """Check exact joint samples through the selected planner backend. + + Args: + trajectory: Simulator-order joint samples with shape ``(B, T, D)``. + control_part: Robot control part whose ordered joints form ``D``. + obstacle_poses: Optional live dynamic-obstacle poses. + + Returns: + Boolean validity mask with shape ``(B, T)`` on the trajectory device. + """ + if not self.supports_joint_trajectory_validation: + raise ValueError( + f"Planner {type(self.planner).__name__} does not support exact " + "joint-trajectory collision validation." + ) + if not isinstance(trajectory, torch.Tensor): + raise TypeError("trajectory must be a torch.Tensor.") + if ( + not trajectory.is_floating_point() + or trajectory.dim() != 3 + or 0 in trajectory.shape + or not bool(torch.isfinite(trajectory).all().item()) + ): + raise ValueError( + "trajectory must be finite floating shape (B, T, D) with " + "non-zero dimensions." + ) + if type(control_part) is not str or not control_part: + raise ValueError("control_part must be a non-empty string.") + validity = self.planner.validate_joint_trajectory( + trajectory, + control_part=control_part, + obstacle_poses=obstacle_poses, + ) + if not isinstance(validity, torch.Tensor): + raise TypeError("Planner.validate_joint_trajectory() must return a tensor.") + if validity.dtype != torch.bool or validity.shape != trajectory.shape[:2]: + raise ValueError( + "Planner.validate_joint_trajectory() must return bool shape " + f"{tuple(trajectory.shape[:2])}." + ) + return validity.to(trajectory.device).clone() + def resolve_plan_options( self, plan_opts: PlanOptions | None, diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py index 5e0772afb..c1e06825e 100644 --- a/tests/gym/envs/expert_program/test_catalog.py +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -22,6 +22,7 @@ from dataclasses import dataclass, replace import json from threading import Event, Lock +from types import SimpleNamespace from typing import ClassVar import pytest @@ -38,7 +39,11 @@ decode_expert_program, ) from embodichain.lab.gym.utils.registration import EnvSpec -from embodichain.lab.sim.atomic_actions import Affordance, PlanningContext +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AtomicActionEngine, + PlanningContext, +) from embodichain.lab.sim.skills import ( PLACEMENT_TARGET_AFFORDANCE_REVISION, PLACE_ON_AFFORDANCE_CAPABILITY, @@ -55,6 +60,7 @@ SceneEntityManifest, SceneManifest, SceneObjectRef, + SceneRegistry, SemanticRelationTarget, RegisteredSemanticCall, SemanticCallDescriptor, @@ -249,9 +255,11 @@ def create( *, simulation: object, robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, ) -> ParallelCommandSafetyValidator: """Return one independent protocol-compatible safety gate.""" - del simulation, robot + del simulation, robot, scene_registry, engine return _AcceptParallelSafety() @@ -286,9 +294,11 @@ def create( *, simulation: object, robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, ) -> ParallelCommandSafetyValidator: """Block the first call so a second call can attempt registration entry.""" - del simulation, robot + del simulation, robot, scene_registry, engine with self._state_lock: call_index = self._calls type(self)._calls += 1 @@ -345,6 +355,14 @@ def _registration() -> SimulationExpertProgramRegistration: ) +def _parallel_live_inputs() -> tuple[object, SceneRegistry, AtomicActionEngine]: + """Build minimal identity-consistent inputs for factory lifecycle tests.""" + robot = object() + engine = AtomicActionEngine.__new__(AtomicActionEngine) + engine._planning_services = SimpleNamespace(robot=robot) # type: ignore[attr-defined] + return robot, SceneRegistry(), engine + + def _operate_articulation_payload( *, target: str, @@ -548,9 +566,12 @@ def test_parallel_preflight_accepts_exact_registration_owned_safety_factory() -> ) compiled = registration.catalog.preflight(program) + robot, scene_registry, engine = _parallel_live_inputs() validator = registration.create_parallel_safety_validator( simulation=object(), - robot=object(), + robot=robot, + scene_registry=scene_registry, + engine=engine, ) assert tuple(compiled.iter_segments())[0].parallel_block is not None @@ -567,8 +588,15 @@ class InvalidParallelSafetyFactory: {"robot.joint_position"} ) - def create(self, *, simulation: object, robot: object) -> object: - del simulation, robot + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> object: + del simulation, robot, scene_registry, engine return object() registration = SimulationExpertProgramRegistration( @@ -577,10 +605,13 @@ def create(self, *, simulation: object, robot: object) -> object: parallel_safety_factory=InvalidParallelSafetyFactory(), ) + robot, scene_registry, engine = _parallel_live_inputs() with pytest.raises(TypeError, match="must return a ParallelCommandSafetyValidator"): registration.create_parallel_safety_validator( simulation=object(), - robot=object(), + robot=robot, + scene_registry=scene_registry, + engine=engine, ) @@ -593,11 +624,14 @@ def test_parallel_safety_creation_and_history_are_one_registration_lock_scope() robot_profile_binding=create_cube_robot_profile_binding(), parallel_safety_factory=factory_type(), ) + robot, scene_registry, engine = _parallel_live_inputs() def create_validator() -> ParallelCommandSafetyValidator | None: return registration.create_parallel_safety_validator( simulation=object(), - robot=object(), + robot=robot, + scene_registry=scene_registry, + engine=engine, ) with ThreadPoolExecutor(max_workers=2) as executor: diff --git a/tests/gym/envs/expert_program/test_extensions.py b/tests/gym/envs/expert_program/test_extensions.py index e51f8ef8d..602013fc5 100644 --- a/tests/gym/envs/expert_program/test_extensions.py +++ b/tests/gym/envs/expert_program/test_extensions.py @@ -250,8 +250,15 @@ class _MobileSafetyFactory: {_MobileTarget.TRANSPORT_ID} ) - def create(self, *, simulation: object, robot: object) -> _SafetyValidator: - del simulation, robot + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: object, + engine: object, + ) -> _SafetyValidator: + del simulation, robot, scene_registry, engine return _SafetyValidator() diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 1900512e3..11e07b7c0 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -1065,8 +1065,17 @@ class _RegisteredParallelSafetyFactory: {JointPositionTarget.TRANSPORT_ID} ) - def create(self, *, simulation: object, robot: object) -> _RegisteredParallelSafety: + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: object, + engine: object, + ) -> _RegisteredParallelSafety: assert getattr(simulation, "get_robot")(getattr(robot, "uid")) is robot + assert scene_registry is not None + assert getattr(engine, "robot") is robot return _RegisteredParallelSafety() @@ -1076,8 +1085,15 @@ class _ReusedParallelSafetyFactory(_RegisteredParallelSafetyFactory): validator_id: ClassVar[str] = "test.reused_parallel_safety" _validator: ClassVar[_RegisteredParallelSafety] = _RegisteredParallelSafety() - def create(self, *, simulation: object, robot: object) -> _RegisteredParallelSafety: - del simulation, robot + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: object, + engine: object, + ) -> _RegisteredParallelSafety: + del simulation, robot, scene_registry, engine return self._validator @@ -1091,8 +1107,15 @@ class _AlternatingReusedParallelSafetyFactory(_RegisteredParallelSafetyFactory): ) _next_index: ClassVar[int] = 0 - def create(self, *, simulation: object, robot: object) -> _RegisteredParallelSafety: - del simulation, robot + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: object, + engine: object, + ) -> _RegisteredParallelSafety: + del simulation, robot, scene_registry, engine validator = self._validators[self._next_index % len(self._validators)] type(self)._next_index += 1 return validator diff --git a/tests/gym/envs/expert_program/test_simulation_parallel_safety.py b/tests/gym/envs/expert_program/test_simulation_parallel_safety.py new file mode 100644 index 000000000..ce037af0c --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation_parallel_safety.py @@ -0,0 +1,238 @@ +# ---------------------------------------------------------------------------- +# 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 production cuRobo parallel-command safety gate.""" + +from __future__ import annotations + +from types import MethodType, SimpleNamespace + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + CuroboParallelCommandSafetyValidator, + CuroboParallelSafetyValidatorFactory, +) +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + EndpointCommand, + JointPositionPayload, + JointPositionTarget, + RuntimeCommandFrame, +) +from embodichain.lab.sim.planners import CuroboPlanner, MotionGenerator +from embodichain.lab.sim.skills import ParallelSafetyError, SceneRegistry + + +class _Robot: + """Expose current full joint state and one aggregate validation part.""" + + def __init__(self) -> None: + self.qpos = torch.zeros(2, 3) + + def get_qpos(self, *, target: bool = False) -> torch.Tensor: + assert target is False + return self.qpos.clone() + + def get_joint_ids(self, *, name: str) -> list[int]: + assert name == "dual_arm" + return [0, 1] + + +def _motion_generator( + *, + reject_env: int | None = None, +) -> tuple[MotionGenerator, list[torch.Tensor]]: + """Build a no-CUDA shell around the exact CuroboPlanner type.""" + observed: list[torch.Tensor] = [] + planner = CuroboPlanner.__new__(CuroboPlanner) + + def validate_joint_trajectory( + self: CuroboPlanner, + trajectory: torch.Tensor, + *, + control_part: str, + obstacle_poses: object, + ) -> torch.Tensor: + del self, obstacle_poses + assert control_part == "dual_arm" + observed.append(trajectory.clone()) + validity = torch.ones(trajectory.shape[:2], dtype=torch.bool) + if reject_env is not None: + validity[reject_env, trajectory.shape[1] // 2] = False + return validity + + planner.validate_joint_trajectory = MethodType( # type: ignore[method-assign] + validate_joint_trajectory, + planner, + ) + generator = MotionGenerator.__new__(MotionGenerator) + generator.planner = planner + return generator, observed + + +def _frame( + *commands: EndpointCommand, + active: tuple[bool, bool] = (True, True), +) -> RuntimeCommandFrame: + """Build one two-row runtime frame.""" + return RuntimeCommandFrame( + commands=commands, + active_mask=torch.tensor(active, dtype=torch.bool), + env_ids=torch.tensor((0, 1), dtype=torch.long), + hold_duration=torch.full((2,), 0.05), + ) + + +def _command( + target_id: str, + joint_id: int, + positions: tuple[float, float], +) -> EndpointCommand: + """Build one single-joint batched command.""" + return EndpointCommand( + target=JointPositionTarget(target_id, (joint_id,)), + payload=JointPositionPayload( + positions=torch.tensor(positions, dtype=torch.float32).unsqueeze(1) + ), + ) + + +def _validator( + *, + reject_env: int | None = None, + max_joint_step: float = 0.05, + max_interpolation_samples: int = 16, +) -> tuple[CuroboParallelCommandSafetyValidator, list[torch.Tensor]]: + """Create one validator with a deterministic collision backend shell.""" + generator, observed = _motion_generator(reject_env=reject_env) + return ( + CuroboParallelCommandSafetyValidator( + robot=_Robot(), + motion_generator=generator, + scene_registry=SceneRegistry(), + validation_control_part="dual_arm", + max_joint_step=max_joint_step, + max_interpolation_samples=max_interpolation_samples, + ), + observed, + ) + + +def test_curobo_parallel_safety_checks_exact_dense_merged_segment() -> None: + """Disjoint lane targets become one densely sampled aggregate trajectory.""" + validator, observed = _validator() + left = _command("left_arm", 0, (0.1, 0.0)) + right = _command("right_arm", 1, (0.2, -0.1)) + + validator.validate( + branch_frames={"left": _frame(left), "right": _frame(right)}, + merged_frame=_frame(left, right), + ) + + assert len(observed) == 1 + trajectory = observed[0] + # float32 represents 0.2 just above the mathematical value, so the strict + # 0.05 maximum step requires five intervals rather than rounding down. + assert trajectory.shape == (2, 6, 2) + torch.testing.assert_close(trajectory[:, 0], torch.zeros(2, 2)) + torch.testing.assert_close( + trajectory[:, -1], + torch.tensor(((0.1, 0.2), (0.0, -0.1))), + ) + + +def test_curobo_parallel_safety_reports_row_local_collision() -> None: + """One invalid environment rejects dispatch with its stable env ID.""" + validator, _ = _validator(reject_env=1) + left = _command("left_arm", 0, (0.1, 0.1)) + right = _command("right_arm", 1, (0.2, 0.2)) + + with pytest.raises(ParallelSafetyError, match=r"env IDs \(1,\)"): + validator.validate( + branch_frames={"left": _frame(left), "right": _frame(right)}, + merged_frame=_frame(left, right), + ) + + +def test_curobo_parallel_safety_rejects_uncovered_joint() -> None: + """Every outgoing joint must belong to the aggregate collision model.""" + validator, _ = _validator() + left = _command("left_arm", 0, (0.1, 0.1)) + hand = _command("left_hand", 2, (0.2, 0.2)) + + with pytest.raises(ParallelSafetyError, match="outside validation control part"): + validator.validate( + branch_frames={"left": _frame(left), "hand": _frame(hand)}, + merged_frame=_frame(left, hand), + ) + + +def test_curobo_parallel_safety_fails_instead_of_under_sampling() -> None: + """The configured memory bound cannot silently enlarge the joint step.""" + validator, _ = _validator( + max_joint_step=0.01, + max_interpolation_samples=4, + ) + left = _command("left_arm", 0, (0.1, 0.1)) + right = _command("right_arm", 1, (0.0, 0.0)) + + with pytest.raises(ParallelSafetyError, match="exceeding configured limit"): + validator.validate( + branch_frames={"left": _frame(left), "right": _frame(right)}, + merged_frame=_frame(left, right), + ) + + +@pytest.mark.parametrize( + "kwargs", + ( + {"validation_control_part": ""}, + {"validation_control_part": "dual_arm", "max_joint_step": 0.0}, + { + "validation_control_part": "dual_arm", + "max_interpolation_samples": 1, + }, + ), +) +def test_curobo_parallel_safety_factory_validates_configuration( + kwargs: dict[str, object], +) -> None: + """Invalid safety declarations fail during task registration.""" + with pytest.raises((TypeError, ValueError)): + CuroboParallelSafetyValidatorFactory(**kwargs) # type: ignore[arg-type] + + +def test_curobo_parallel_safety_factory_binds_exact_runtime_components() -> None: + """The production factory consumes the assembled engine and registry.""" + robot = _Robot() + motion_generator, _ = _motion_generator() + engine = AtomicActionEngine.__new__(AtomicActionEngine) + engine._planning_services = SimpleNamespace( # type: ignore[attr-defined] + robot=robot, + motion_generator=motion_generator, + ) + factory = CuroboParallelSafetyValidatorFactory(validation_control_part="dual_arm") + + validator = factory.create( + simulation=object(), + robot=robot, + scene_registry=SceneRegistry(), + engine=engine, + ) + + assert type(validator) is CuroboParallelCommandSafetyValidator diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index d4feb6d38..552ef6dfc 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -701,6 +701,74 @@ def test_dynamic_update_uses_registry_id_in_curobo_backend(): assert [(name, env_idx) for name, _, env_idx in updates] == [("registry_cube", 0)] +def test_validate_joint_trajectory_checks_every_exact_sample_in_curobo_order(): + """The collision gate preserves samples and maps simulator joint order.""" + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg(multi_env=True), + ) + planner._curobo_device = torch.device("cpu") + joint_states = [] + collision_queries = [] + + def from_position(position, *, joint_names): + joint_states.append((position.clone(), tuple(joint_names))) + return SimpleNamespace(position=position) + + def validate(sample, *, env_query_idx): + collision_queries.append((sample.clone(), env_query_idx.clone())) + validity = torch.ones(sample.shape[:2], dtype=torch.bool) + if len(collision_queries) == 2: + validity[1, 0] = False + return validity + + planner._bindings = SimpleNamespace( + JointState=SimpleNamespace(from_position=from_position), + ) + backend = SimpleNamespace( + sim_joint_names=["sim_left", "sim_right"], + sim_to_curobo_col_idx=None, + collision_checker=SimpleNamespace(validate=validate), + profile=SimpleNamespace( + sim_to_curobo_joint_names={ + "sim_left": "curobo_left", + "sim_right": "curobo_right", + }, + ), + planner=SimpleNamespace( + joint_names=["curobo_right", "curobo_left"], + ), + ) + planner._get_backend = lambda control_part, batch_size, move_type: backend + trajectory = torch.tensor( + ( + ((0.0, 1.0), (0.1, 1.1), (0.2, 1.2)), + ((2.0, 3.0), (2.1, 3.1), (2.2, 3.2)), + ), + dtype=torch.float32, + ) + + validity = planner.validate_joint_trajectory( + trajectory, + control_part="dual_arm", + ) + + assert torch.equal( + validity, + torch.tensor(((True, True, True), (True, False, True))), + ) + assert len(collision_queries) == trajectory.shape[1] + for sample_index, (sample, env_query_idx) in enumerate(collision_queries): + torch.testing.assert_close( + sample[:, 0], + trajectory[:, sample_index].flip(dims=(-1,)), + ) + assert torch.equal(env_query_idx, torch.tensor((0, 1), dtype=torch.int32)) + torch.testing.assert_close(joint_states[0][0], trajectory[:, 0].flip(dims=(-1,))) + assert joint_states[0][1] == ("curobo_right", "curobo_left") + + def test_generate_mesh_world_yaml_assembles_schema(tmp_path): rigid_object = _FakeRigidObject( "demo_block", From 30dd2d09147719fd544890e2cb2d5f1f6736d8e4 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 23:56:19 +0800 Subject: [PATCH 28/29] test(tasks): gate cube physical recovery --- .../design/declarative_expert_program_plan.md | 62 +++-- docs/design/expert_program_rollout_report.md | 23 +- .../sim/atomic_actions/expert_programs.md | 37 ++- .../multi_segments/cube_pick_place.py | 2 + .../tools/expert_program_rollout_report.py | 39 +-- .../test_cube_physical_recovery_sim.py | 241 ++++++++++++++++++ .../test_demo_success_cube_sim.py | 150 +++++++++++ .../test_multi_segments_cube_pick_place.py | 5 +- .../test_expert_program_rollout_report.py | 4 +- 9 files changed, 500 insertions(+), 63 deletions(-) create mode 100644 tests/benchmark/expert_program/test_cube_physical_recovery_sim.py create mode 100644 tests/benchmark/expert_program/test_demo_success_cube_sim.py diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 675ea9b4a..8c140b707 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -4,9 +4,11 @@ branches. A real CUDA/cuRobo dynamic-obstacle recovery gate is landed and runs conditionally when cuRobo is installed, CUDA is available, and GPU/slow 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. Dual-UR5/PGI HandOver has completed three consecutive + supported-simulation physical run; repeated cube pick/place now completes all + three independently observed Pick/Place/settle/validator cycles. A physical + gripper-command fault gate also proves held-object loss, symbolic invalidation, + real re-acquisition, and retry without simulator-side repair. 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 @@ -1236,9 +1238,11 @@ it only when GPU and slow tests are explicitly selected. Implementation status: the semantic facade, provider-free linking, canonical compiler, bounded program preflight, and cross-segment sequential look-ahead are -implemented. Relation placement remains an exact typed integration capability; -a reusable production support-surface/container affordance and grounder are -follow-up work rather than inferred behavior. +implemented. Relation placement remains an exact typed integration capability. +Production support-surface and container bindings now declare a desired object +target frame relative to an object, articulation, or link parent; registration +installs their exact versioned grounders automatically. No geometric target is +guessed from a name, mesh, or bounding box. Deliverables: @@ -1264,16 +1268,19 @@ effects, row-local composite hysteresis kernel, canonical `SkillRuntime`, and 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 +implemented. Physical simulation acceptance covers Open Drawer and all three +cube Pick/Place/settle/validator cycles. 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. +policy and canonical runtime. A supported-simulation fault gate now replaces a +bounded window of outgoing gripper commands with a real open command during +Place, observes pose-relation contradiction and symbolic invalidation, performs +a real Pick, retries Place, and completes the remaining program. The wrapper +never writes object pose, velocity, attachment, constraint, or task state. Deliverables: @@ -1318,9 +1325,9 @@ strict decoder/loader, lazy compiler, environment/CLI integration, shared simulation factory, and three-segment cube program are implemented. The task combines declarative program configuration with typed scene/profile integration declarations and installs the shared adapter without overriding task motion -generation. A supported-simulation run has completed the first physical -Pick/Place/settle/validator cycle; completing all three cycles remains an -acceptance item while thresholds are calibrated. +generation. The UR5 embodiment preset uses 100 motion samples while retaining +the 0.08-rad tracking gate and bounded replanning. A supported-simulation run +now completes all three physical Pick/Place/settle/validator cycles. Deliverables: @@ -1365,13 +1372,16 @@ trajectories in task code. ### Phase 7: parallel execution and PourWater -Implementation status: the schema/runtime contracts and fail-closed safety -boundary are implemented. Schema +Implementation status: the schema/runtime contracts, fail-closed safety +boundary, and production simulation validator are implemented. Schema version 2 provides explicit parallel branches and barriers; static resource conflict analysis, shared-clock lane coordination, deterministic hold padding, transport/safety validation, row-local failure and cancellation, timeouts, and -deterministic state merge are covered by tests. A production simulation safety -validator and parallel physical integration remain pending. The PourWater task +deterministic state merge are covered by tests. The cuRobo validator assembles +the exact aggregate joint segment, densifies it under a configured maximum joint +step, and checks every sample against joint bounds, self collision, and the +registry-backed live world without replanning or replacing the command. A +parallel physical integration remains pending. The PourWater task migration is outside the current scope because it would require modifying Action Bank code. @@ -1520,22 +1530,22 @@ The design is complete when all of the following hold: - [x] Automatic grasping tracks target revisions and receives downstream object 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. 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 +- [x] Built-in grasp, release, handover, and supported articulation effect + monitors work in simulation across the repeated cube, dual-UR5/PGI + HandOver, and Open Drawer vertical slices. +- [x] 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 +- [x] 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. + real `Pick` and semantic-call retries. The supported-simulation gate opens + the real gripper during Place, observes the loss, re-acquires the fallen + cube, and completes the retried call. - [x] Repeated sub-threshold motion eventually publishes the correct scene revision. - [x] Custom actions have a documented and tested intentional hard-break @@ -1547,7 +1557,7 @@ The design is complete when all of the following hold: through `env.step()`. - [x] No program post-policy, effect, or tracing integration depends on hard-coded waypoint indices. -- [ ] Repeated cube pick/place completes at least three lazy, independently +- [x] Repeated cube pick/place completes at least three lazy, independently observed program/demo segments with settle/effect/validation metadata. - [x] Version 1 uses one shared program/call barrier while per-environment task state, effects, recovery, eligibility, success, and failure remain diff --git a/docs/design/expert_program_rollout_report.md b/docs/design/expert_program_rollout_report.md index 6c8228748..175e74e53 100644 --- a/docs/design/expert_program_rollout_report.md +++ b/docs/design/expert_program_rollout_report.md @@ -13,25 +13,26 @@ This is a deterministic, static Phase 8 snapshot of checked-in framework and int | OperateArticulation | framework-tested | per-embodiment integration | Typed articulation goals and execution contracts are covered. | | Articulation effect | framework-tested | per-embodiment integration | Joint-state terminal effect validation is covered. | | V1 sequential | framework-tested | per-task integration | Ordered call execution and failure propagation are covered. | -| HandOver | framework-tested | integration-required | No landed task integration is claimed by this report. | -| Place relation (on/inside) | framework-tested | integration-required | Embodiment frames and relation validators must be supplied. | +| HandOver | framework-tested | per-embodiment integration | Coordinated effects and bounded recovery are covered. | +| Place relation (on/inside) | framework-tested | per-scene integration | Standard support/container target-frame bindings install exact grounders. | | Registered call | framework-tested | integration-required | Production registration must declare and validate its concrete contract. | -| V2 parallel | framework-tested | integration-required | Fail-closed by default; production use requires an authoritative validator. | +| V2 parallel | framework-tested | integration-required | Joint/cuRobo validation is available; physical parallel acceptance remains. | Parallel execution remains fail-closed by default. Resource declarations alone do not authorize production concurrency; the selected embodiment must provide an authoritative validator. ## Checked-in Integration Matrix -Only the two checked-in vertical slices below are classified as integration/production code. Physical acceptance is tracked separately. +Only the checked-in vertical slices below are classified as integration/production code. Physical acceptance is tracked separately. | Embodiment | Task | Skill contract | Terminal effect | Program schema | Code status | Physical acceptance | | --- | --- | --- | --- | --- | --- | --- | -| UR5 | Cube Pick + Place | Pick + Place(at) | attach/release | V1 sequential | checked in | pending: one cycle passed; full three-cycle gate remains | +| UR5 | Cube Pick + Place | Pick + Place(at) | attach/release | V1 sequential | checked in | fixed-seed three-cycle and physical-loss recovery slow gates | | CobotMagic | Open Drawer | OperateArticulation | articulation effect | V1 sequential | checked in | fixed-seed supported-simulation slow gate; not release-required | +| Dual UR5 + PGI | HandOver | Pick + HandOver | attach/transfer | V1 sequential | checked in | three consecutive supported-simulation contact-dynamics runs | -HandOver, Place relations (`on`/`inside`), Registered calls, and V2 parallel are framework-tested but integration-required. They are intentionally not listed as checked-in integrations. +Place-relation bindings are reusable scene integration rather than a task vertical slice. Registered calls and V2 parallel remain integration-required; physical parallel acceptance is still open. -Both checked-in environment classes have zero task-local motion or demo-generation overrides; `test_task_classes_do_not_override_motion_or_demo_generation` keeps that structural metric at zero. +The checked-in environment classes have zero task-local motion or demo-generation overrides; `test_task_classes_do_not_override_motion_or_demo_generation` keeps that structural metric at zero. ## Migration Size Snapshot @@ -43,15 +44,15 @@ Counting rule: `lines` is the number of raw LF (`0x0A`) bytes; `bytes` is the ra | Task | Baseline lines | Current lines | Line delta | Baseline bytes | Current bytes | Byte delta | Current source files | | --- | --- | --- | --- | --- | --- | --- | --- | -| Cube | 598 | 366 | -232 (-38.8%) | 23912 | 12448 | -11464 (-47.9%) | `embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py`
`embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml` | -| Drawer | 245 | 246 | +1 (+0.4%) | 8833 | 8391 | -442 (-5.0%) | `embodichain_tasks/embodichain_tasks/tableware/open_drawer.py`
`embodichain_tasks/configs/expert_program/tableware/open_drawer.json` | -| Total | 843 | 612 | -231 (-27.4%) | 32745 | 20839 | -11906 (-36.4%) | the four files above | +| Cube | 598 | 395 | -203 (-33.9%) | 23912 | 13272 | -10640 (-44.5%) | `embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py`
`embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml` | +| Drawer | 245 | 265 | +20 (+8.2%) | 8833 | 8916 | +83 (+0.9%) | `embodichain_tasks/embodichain_tasks/tableware/open_drawer.py`
`embodichain_tasks/configs/expert_program/tableware/open_drawer.json` | +| Total | 843 | 660 | -183 (-21.7%) | 32745 | 22188 | -10557 (-32.2%) | the four files above | ## Demo Success Measurement `scripts/benchmark/expert_program/demo_success.py` executes each fixed seed exactly once, always discards the episode buffer, and counts executor exceptions as failed rows. It writes raw JSON plus a three-table Markdown report. Its CLI supports offline raw-JSON re-aggregation and an explicit `--run-simulation` mode that constructs one standard Gym environment from Gym and Expert Program configurations. -No success-rate result or release gate is checked in yet. Open Drawer has a single real-simulation smoke pass, while repeated Cube still needs the tracking-threshold decision and three-cycle physical acceptance before a fixed-seed rate is meaningful. +No multi-seed success-rate or release gate is checked in yet. Open Drawer has a real-simulation smoke pass; repeated Cube has a fixed-seed three-cycle pass plus physical-loss/re-acquisition gate; and HandOver has three consecutive contact-dynamics runs. ## Drift Check diff --git a/docs/source/overview/sim/atomic_actions/expert_programs.md b/docs/source/overview/sim/atomic_actions/expert_programs.md index 3ba563d83..95275883a 100644 --- a/docs/source/overview/sim/atomic_actions/expert_programs.md +++ b/docs/source/overview/sim/atomic_actions/expert_programs.md @@ -176,6 +176,16 @@ requires the profile-selected `HandOverPoseProvider`. A direct `Place(at=...)` does not require a relation grounder. Missing, ambiguous, or stale providers fail during provider-aware program preflight, before the first physical action. +The standard simulation registration supplies the common relation path without +task-owned grounder code. Declare `SupportSurfaceAffordanceBinding` or +`ContainerAffordanceBinding` in `SimulationSceneBinding`; its +`object_target_pose` is the desired object pose relative to the explicitly named +object, articulation, or link parent. Mark one capability-scoped target per +parent as `is_default=True` when calls should reference only that parent. The +registration installs the matching exact grounder automatically and resolves +the target from a fresh scene snapshot. It never infers a placement frame from +an entity name, mesh, or bounding box. + The same preflight rejects a reachable `safe` preset for a dynamic scene before the first observation when the active motion generator cannot provide the required dynamic collision world. @@ -229,6 +239,16 @@ Version 2 also uses strict symbolic key-level conflict detection at the barrier: two branches may not commit the same task-state key, even when their physical changes occurred in disjoint environment rows. +Joint-position integrations using the active cuRobo planner can declare +`CuroboParallelSafetyValidatorFactory(validation_control_part="dual_arm")`. +The selected aggregate control part must contain every joint commanded by any +lane. Before dispatch, the validator combines the exact merged target with the +current measured joint state, interpolates the segment under `max_joint_step`, +and asks cuRobo to check every supplied sample against joint bounds, +self-collision, and the registry-backed static/dynamic world. It fails closed +when a joint is uncovered or the configured sample cap would under-sample the +segment. This gate does not replan or replace the trajectory. + ## Python semantic calls Standalone applications can use the same compiler and runtime through @@ -242,8 +262,9 @@ result = skills.run(Pick(object=cube), Place(object=cube, on=tray)) ``` In this example, `runtime_provider` owns the typed relation grounder for the -tray's placement affordance. Applications without such a provider can use a -direct `SemanticPose` through `Place(at=...)`. +tray's placement affordance. A standard simulation registration obtains it from +the support/container binding above. Applications without such a provider can +use a direct `SemanticPose` through `Place(at=...)`. `from_env` requires an explicit `SkillRuntimeProvider`; it never scans arbitrary environment attributes. Gym demonstration environments intentionally use the @@ -261,18 +282,20 @@ robot resource and endpoint declarations, see {doc}`robot_skill_profiles`. | --- | --- | --- | | `Pick` | Compiler, runtime, effect verification | Antipodal grasp binding plus motion/grasp resources | | `Place(at=...)` | Object-centric lowering with verified held state | Direct semantic pose target | -| `Place(on=...)` / `Place(inside=...)` | Exact typed relation dispatch | Integration must install the matching `RelationTargetGrounder` | +| `Place(on=...)` / `Place(inside=...)` | Exact typed relation dispatch | Support/container bindings install standard target-frame grounders; custom payloads install an exact `RelationTargetGrounder` | | `HandOver` | Coordinated call, state flow, and effect contract | Embodiment must install its named `HandOverPoseProvider` and evidence sources | | `OperateArticulation` | Named/absolute/displacement target and joint effect | Link, joint, operation-affordance, and interaction endpoint bindings | | Registered calls | Typed call catalog and explicit lowerer | Physical extensions must add an explicit effect contract | | Mobile/whole-body extensions | Generic resources, claims, endpoint targets, command frames, and routing | Requires a reusable semantic skill/lowerer plus matching adapter, payload, transport, and effect integration; no curated navigation or whole-body skill is installed today | -| Parallel blocks | Shared-clock coordinator and strict barrier merge | Requires an authoritative `ParallelCommandSafetyValidator`; none is inferred by default | +| Parallel blocks | Shared-clock coordinator and strict barrier merge | Joint-position/cuRobo tasks may declare the aggregate collision validator; other transports require an authoritative validator | The table separates implemented reusable contracts from embodiment-specific providers. It is not a claim that every row has completed task-level physical simulation acceptance. 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; its full three-cycle -run remains in threshold calibration. +run and reached the configured drawer joint target. Repeated cube pick/place +completes three independently observed physical Pick/Place/settle/validator +cycles. Its physical-fault gate opens the commanded gripper during Place, +observes held-object loss, performs a real re-acquisition Pick, retries Place, +and completes without simulator-side repair. 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 1a5b7d4cf..4ea67de33 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -53,6 +53,7 @@ CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, + MotionPolicy, PickUpOptions, PlaceOptions, RecoveryPolicy, @@ -303,6 +304,7 @@ def create_cube_robot_profile_binding() -> SimulationRobotSkillProfileBinding: "pick": PickUpOptions(), "place": PlaceOptions(), }, + motion_policy=MotionPolicy(sample_count=100), recovery_policy=RecoveryPolicy(), workflow_recovery_policy=WorkflowRecoveryPolicy( max_recovery_attempts=2, diff --git a/scripts/tools/expert_program_rollout_report.py b/scripts/tools/expert_program_rollout_report.py index eeff71a75..ee0bf2b42 100644 --- a/scripts/tools/expert_program_rollout_report.py +++ b/scripts/tools/expert_program_rollout_report.py @@ -150,14 +150,14 @@ class _TaskSizeSpec: ( "HandOver", "framework-tested", - "integration-required", - "No landed task integration is claimed by this report.", + "per-embodiment integration", + "Coordinated effects and bounded recovery are covered.", ), ( "Place relation (on/inside)", "framework-tested", - "integration-required", - "Embodiment frames and relation validators must be supplied.", + "per-scene integration", + "Standard support/container target-frame bindings install exact grounders.", ), ( "Registered call", @@ -169,7 +169,7 @@ class _TaskSizeSpec: "V2 parallel", "framework-tested", "integration-required", - "Fail-closed by default; production use requires an authoritative validator.", + "Joint/cuRobo validation is available; physical parallel acceptance remains.", ), ) @@ -182,7 +182,7 @@ class _TaskSizeSpec: "attach/release", "V1 sequential", "checked in", - "pending: one cycle passed; full three-cycle gate remains", + "fixed-seed three-cycle and physical-loss recovery slow gates", ), ( "CobotMagic", @@ -193,6 +193,15 @@ class _TaskSizeSpec: "checked in", "fixed-seed supported-simulation slow gate; not release-required", ), + ( + "Dual UR5 + PGI", + "HandOver", + "Pick + HandOver", + "attach/transfer", + "V1 sequential", + "checked in", + "three consecutive supported-simulation contact-dynamics runs", + ), ) @@ -329,7 +338,7 @@ def render_report(metrics: Sequence[TaskSizeMetric]) -> str: "## Checked-in Integration Matrix", "", ( - "Only the two checked-in vertical slices below are classified as " + "Only the checked-in vertical slices below are classified as " "integration/production code. Physical acceptance is tracked " "separately." ), @@ -354,13 +363,13 @@ def render_report(metrics: Sequence[TaskSizeMetric]) -> str: [ "", ( - "HandOver, Place relations (`on`/`inside`), Registered calls, and V2 " - "parallel are framework-tested but integration-required. They are " - "intentionally not listed as checked-in integrations." + "Place-relation bindings are reusable scene integration rather than " + "a task vertical slice. Registered calls and V2 parallel remain " + "integration-required; physical parallel acceptance is still open." ), "", ( - "Both checked-in environment classes have zero task-local motion or " + "The checked-in environment classes have zero task-local motion or " "demo-generation overrides; " "`test_task_classes_do_not_override_motion_or_demo_generation` " "keeps that structural metric at zero." @@ -422,10 +431,10 @@ def render_report(metrics: Sequence[TaskSizeMetric]) -> str: ), "", ( - "No success-rate result or release gate is checked in yet. Open " - "Drawer has a single real-simulation smoke pass, while repeated Cube " - "still needs the tracking-threshold decision and three-cycle physical " - "acceptance before a fixed-seed rate is meaningful." + "No multi-seed success-rate or release gate is checked in yet. Open " + "Drawer has a real-simulation smoke pass; repeated Cube has a " + "fixed-seed three-cycle pass plus physical-loss/re-acquisition gate; " + "and HandOver has three consecutive contact-dynamics runs." ), "", "## Drift Check", diff --git a/tests/benchmark/expert_program/test_cube_physical_recovery_sim.py b/tests/benchmark/expert_program/test_cube_physical_recovery_sim.py new file mode 100644 index 000000000..f1b64384a --- /dev/null +++ b/tests/benchmark/expert_program/test_cube_physical_recovery_sim.py @@ -0,0 +1,241 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Live physical held-object loss and workflow-recovery regression coverage.""" + +from __future__ import annotations + +import argparse +from pathlib import Path +import subprocess +import sys +from typing import Any + +import pytest +import torch + +from embodichain.lab.gym.envs.demo import ( + DemoEpisodeResult, + ProcessedEnvAction, + execute_demo_episode, +) +from embodichain_tasks.configs import get_config_path +from scripts.benchmark.expert_program.demo_success import ( + DemoSuccessCase, + _build_parser, + load_raw_trials, + run_gym_demo_success_benchmark, +) + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +_CUBE_GYM_CONFIG = get_config_path("gym/multi_segments/cube_pick_place.json") +_CUBE_EXPERT_PROGRAM = get_config_path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" +) +_FAULT_SEGMENT_INDEX = 1 +_FAULT_CALL_INDEX = 1 +_FAULT_OPEN_STEPS = 20 +_SUBPROCESS_TIMEOUT_SECONDS = 240 + + +class _GripperOpenFaultEnvironment: + """Replace a bounded command window with a real gripper-open command. + + The wrapper changes only the controller-ready action before the ordinary + ``env.step()`` call. It never writes an object pose, velocity, attachment, + constraint, or symbolic task state. + """ + + def __init__(self, environment: Any) -> None: + self._environment = environment + target = getattr(environment, "unwrapped", environment) + self._hand_joint_ids = tuple(target.robot.get_joint_ids(name="hand")) + if not self._hand_joint_ids: + raise ValueError("The cube recovery gate requires hand joint IDs.") + self.injected_open_steps = 0 + + @property + def unwrapped(self) -> Any: + """Return the original task environment for demo lifecycle hooks.""" + return getattr(self._environment, "unwrapped", self._environment) + + def __getattr__(self, name: str) -> Any: + return getattr(self._environment, name) + + def step(self, action: object) -> object: + """Open the physical gripper during the selected Place approach.""" + if isinstance(action, ProcessedEnvAction): + metadata = action.metadata + should_inject = ( + metadata.get("program_segment_index") == _FAULT_SEGMENT_INDEX + and metadata.get("runtime_call_index") == _FAULT_CALL_INDEX + and metadata.get("bridge_action_kind") == "runtime_command" + and self.injected_open_steps < _FAULT_OPEN_STEPS + ) + if should_inject: + if not isinstance(action.value, torch.Tensor): + raise TypeError("The cube task must emit a tensor action.") + value = action.value.clone() + value[:, self._hand_joint_ids] = 0.0 + action = ProcessedEnvAction( + value=value, + metadata={ + **metadata, + "test_fault": "open_gripper_before_place", + }, + ) + self.injected_open_steps += 1 + return self._environment.step(action) + + +def _execute_fault_episode( + environment: Any, + *, + episode_index: int, +) -> DemoEpisodeResult: + """Execute one episode through the controller-command fault wrapper.""" + fault_environment = _GripperOpenFaultEnvironment(environment) + result = execute_demo_episode( + fault_environment, + episode_index=episode_index, + ) + print(f"injected_open_steps={fault_environment.injected_open_steps}") + return result + + +def _run_fault_subprocess(raw_path: Path, report_path: Path) -> int: + """Create and close the native simulator inside the child process.""" + launcher_args = _build_parser().parse_args( + [ + "--run-simulation", + "--gym_config", + str(_CUBE_GYM_CONFIG), + "--expert-program", + str(_CUBE_EXPERT_PROGRAM), + "--case-id", + "cube_physical_loss_recovery", + "--seeds", + "0", + "--raw-json", + str(raw_path), + "--report", + str(report_path), + "--headless", + "--device", + "cuda", + "--num_envs", + "1", + "--filter_dataset_saving", + ] + ) + run_gym_demo_success_benchmark( + DemoSuccessCase("cube_physical_loss_recovery", (0,)), + launcher_args=launcher_args, + expert_program_path=_CUBE_EXPERT_PROGRAM, + raw_json_path=raw_path, + report_path=report_path, + episode_executor=_execute_fault_episode, + ) + return 0 + + +@pytest.mark.requires_sim +@pytest.mark.slow +@pytest.mark.gpu +def test_physical_cube_loss_triggers_real_reacquisition(tmp_path: Path) -> None: + """Observe physical loss, invalidate state, reacquire, and finish the task.""" + raw_path = tmp_path / "fault_raw.json" + report_path = tmp_path / "fault_report.md" + completed = subprocess.run( + [ + sys.executable, + str(Path(__file__).resolve()), + "--run-fault", + "--raw-json", + str(raw_path), + "--report", + str(report_path), + ], + cwd=_REPOSITORY_ROOT, + capture_output=True, + text=True, + timeout=_SUBPROCESS_TIMEOUT_SECONDS, + check=False, + ) + + assert completed.returncode == 0, completed.stdout + completed.stderr + assert f"injected_open_steps={_FAULT_OPEN_STEPS}" in completed.stdout + trial = load_raw_trials(raw_path)[0] + assert trial.rows[0].success + assert trial.rows[0].terminal_reason == "success" + + segments = trial.episode_result["segments"] + assert isinstance(segments, list) + assert len(segments) == 3 + recovery_segment = segments[_FAULT_SEGMENT_INDEX] + runtime = recovery_segment["metadata"]["runtime"] + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + assert runtime["task_state"]["held_objects"] == [] + + failed_place = runtime["calls"][_FAULT_CALL_INDEX] + assert failed_place["semantic_id"] == "place" + assert failed_place["status"] == "failed" + assert failed_place["masks"]["failed"] == [True] + assert {event["kind"] for event in failed_place["events"]} >= { + "held_object_lost", + "recovery_required", + } + physical_failures = [ + effect + for effect in failed_place["effects"] + if effect["boundary"]["kind"] == "in_flight_guard" + and effect["decision"]["failure_mask"] == [True] + ] + assert len(physical_failures) == 1 + physical_failure = physical_failures[0] + assert physical_failure["decision"]["expectations"][0]["contradicted_mask"] == [ + True + ] + assert physical_failure["evidence"]["source.constraint"]["values"] == [True] + assert physical_failure["evidence"]["source.pose"]["valid_mask"] == [True] + + recoveries = runtime["workflow_recoveries"] + assert [recovery["role"] for recovery in recoveries] == [ + "reacquire", + "retry_reacquired", + ] + assert [recovery["attempt_index"] for recovery in recoveries] == [1, 1] + assert [recovery["call"]["semantic_id"] for recovery in recoveries] == [ + "pick", + "place", + ] + assert all(recovery["call"]["status"] == "completed" for recovery in recoveries) + assert recovery_segment["metadata"]["validation"]["accepted_mask"] == [True] + + +def _main(argv: list[str] | None = None) -> int: + """Run only the isolated native-simulation helper mode.""" + parser = argparse.ArgumentParser() + parser.add_argument("--run-fault", action="store_true", required=True) + parser.add_argument("--raw-json", type=Path, required=True) + parser.add_argument("--report", type=Path, required=True) + args = parser.parse_args(argv) + return _run_fault_subprocess(args.raw_json, args.report) + + +if __name__ == "__main__": + raise SystemExit(_main()) diff --git a/tests/benchmark/expert_program/test_demo_success_cube_sim.py b/tests/benchmark/expert_program/test_demo_success_cube_sim.py new file mode 100644 index 000000000..d77d757e7 --- /dev/null +++ b/tests/benchmark/expert_program/test_demo_success_cube_sim.py @@ -0,0 +1,150 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Live repeated-cube regression coverage for the Expert Program benchmark.""" + +from __future__ import annotations + +from pathlib import Path +import subprocess +import sys + +import pytest + +from embodichain_tasks.configs import get_config_path +from scripts.benchmark.expert_program.demo_success import ( + aggregate_demo_success_trials, + load_raw_trials, +) + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +_CUBE_GYM_CONFIG = get_config_path("gym/multi_segments/cube_pick_place.json") +_CUBE_EXPERT_PROGRAM = get_config_path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" +) +_CASE_ID = "repeated_cube_three_cycle_live" +_SEED = 0 +_NUM_ENVS = 1 +_SUBPROCESS_TIMEOUT_SECONDS = 180 +_RUN_PUBLIC_MAIN = ( + "from scripts.benchmark.expert_program.demo_success import main; " + "raise SystemExit(main())" +) + + +def _successful_effect_decisions(call: dict[str, object]) -> list[dict[str, object]]: + """Return successful physical-effect observations from one call trace.""" + effects = call["effects"] + assert isinstance(effects, list) + return [ + effect + for effect in effects + if isinstance(effect, dict) + and isinstance(effect.get("decision"), dict) + and effect["decision"].get("success_mask") == [True] + ] + + +@pytest.mark.requires_sim +@pytest.mark.slow +@pytest.mark.gpu +def test_live_repeated_cube_completes_three_physical_cycles( + tmp_path: Path, +) -> None: + """Run all three no-retry segments through the public live entry point.""" + raw_path = tmp_path / "cube_raw.json" + report_path = tmp_path / "cube_report.md" + completed = subprocess.run( + [ + sys.executable, + "-c", + _RUN_PUBLIC_MAIN, + "--run-simulation", + "--gym_config", + str(_CUBE_GYM_CONFIG), + "--expert-program", + str(_CUBE_EXPERT_PROGRAM), + "--case-id", + _CASE_ID, + "--seeds", + str(_SEED), + "--raw-json", + str(raw_path), + "--report", + str(report_path), + "--headless", + "--device", + "cuda", + "--num_envs", + str(_NUM_ENVS), + "--filter_dataset_saving", + ], + cwd=_REPOSITORY_ROOT, + capture_output=True, + text=True, + timeout=_SUBPROCESS_TIMEOUT_SECONDS, + check=False, + ) + + assert completed.returncode == 0, completed.stdout + completed.stderr + decoded_trials = load_raw_trials(raw_path) + assert len(decoded_trials) == 1 + trial = decoded_trials[0] + assert trial.case_id == _CASE_ID + assert trial.seed == _SEED + assert trial.rows[0].success + assert trial.rows[0].terminal_reason == "success" + + episode = trial.episode_result + assert episode["completed"] is True + assert episode["success"] == [True] + assert episode["terminal_reason"] == "success" + segments = episode["segments"] + assert isinstance(segments, list) + assert len(segments) == 3 + + for segment_index, (segment, target_index) in enumerate( + zip(segments, (0, 1, 0), strict=True) + ): + assert isinstance(segment, dict) + assert segment["segment_id"] == segment_index + assert segment["name"] == "move_cube" + metadata = segment["metadata"] + assert isinstance(metadata, dict) + runtime = metadata["runtime"] + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + calls = runtime["calls"] + assert [call["semantic_id"] for call in calls] == ["pick", "place"] + assert all(call["status"] == "completed" for call in calls) + assert all(_successful_effect_decisions(call) for call in calls) + + post_policies = metadata["post_policies"] + assert len(post_policies) == 1 + assert post_policies[0]["kind"] == "wait_stable" + assert post_policies[0]["result"]["status"] == "settled" + validation = metadata["validation"] + assert validation["accepted_mask"] == [True] + validators = validation["validators"] + assert len(validators) == 1 + assert validators[0]["result"]["target_value_index"] == target_index + assert validators[0]["result"]["accepted_mask"] == [True] + + aggregates = aggregate_demo_success_trials(decoded_trials) + metrics = aggregates.success_and_metrics[0] + assert metrics["attempted"] == 1 + assert metrics["successes"] == 1 + assert metrics["success_rate"] == 1.0 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 fe816f85a..5649a005a 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 @@ -124,11 +124,12 @@ def test_direct_default_cfg_loads_the_same_typed_program() -> None: assert settle.params["entity_cfgs"][0].uid == "cube" -def test_robot_profile_calibrates_physical_tracking_tolerance() -> None: - """The UR5 preset tolerates its measured drive lag without disabling feedback.""" +def test_robot_profile_calibrates_physical_motion_and_tracking() -> None: + """The UR5 preset slows motion without weakening feedback or recovery.""" binding = create_cube_robot_profile_binding() assert binding.presets[0].preset_id == "safe" + assert binding.presets[0].motion_policy.sample_count == 100 tracking = binding.presets[0].tracking_policy assert tracking.in_flight is not None assert tracking.in_flight.metrics[0].tolerance == 0.08 diff --git a/tests/scripts/tools/test_expert_program_rollout_report.py b/tests/scripts/tools/test_expert_program_rollout_report.py index 260f15a81..0c295cce0 100644 --- a/tests/scripts/tools/test_expert_program_rollout_report.py +++ b/tests/scripts/tools/test_expert_program_rollout_report.py @@ -28,8 +28,8 @@ EXPECTED_CURRENT_COUNTS = { # Each tuple is (raw LF bytes, raw file bytes) for the explicit task pair. - "Cube": (366, 12_448), - "Drawer": (246, 8_391), + "Cube": (395, 13_272), + "Drawer": (265, 8_916), } EXPECTED_SOURCE_PATHS = { From ac03e82905dec4f04b296d36a61f9f81cb059703 Mon Sep 17 00:00:00 2001 From: skywhite1024 <129768272+skywhite1024@users.noreply.github.com> Date: Fri, 21 Aug 2026 17:39:36 +0800 Subject: [PATCH 29/29] feat(atomic-actions): improve upright grasp selection --- .../lab/sim/atomic_actions/affordance.py | 13 ++ .../sim/atomic_actions/primitives/pick_up.py | 211 ++++++++++++++++-- .../graspkit/pg_grasp/antipodal_generator.py | 21 ++ tests/sim/atomic_actions/test_affordance.py | 23 ++ .../atomic_actions/test_primitives_helpers.py | 20 ++ 5 files changed, 274 insertions(+), 14 deletions(-) diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index 3b0c7f458..7a08dccdc 100644 --- a/embodichain/lab/sim/atomic_actions/affordance.py +++ b/embodichain/lab/sim/atomic_actions/affordance.py @@ -16,6 +16,8 @@ from __future__ import annotations +from collections.abc import Callable + import torch from collections.abc import Mapping from copy import deepcopy @@ -122,16 +124,27 @@ def get_valid_grasp_poses( [0, 0, -1], dtype=torch.float32 ), object_part: str = "center", + grasp_cost_fn: ( + Callable[[torch.Tensor, torch.Tensor, torch.Tensor], torch.Tensor] | None + ) = None, ) -> list[tuple[torch.Tensor, torch.Tensor]]: if self._generator is None: self._init_generator() approach_direction = self._resolve_approach_direction(approach_direction) results = [] for i, obj_pose in enumerate(obj_poses): + pose_cost_fn = None + if grasp_cost_fn is not None: + pose_cost_fn = lambda grasp_poses, costs: grasp_cost_fn( + obj_pose, + grasp_poses, + costs, + ) is_success, grasp_poses, _, costs = self._generator.get_valid_grasp_poses( object_pose=obj_pose, approach_direction=approach_direction, object_part=object_part, + pose_cost_fn=pose_cost_fn, ) if grasp_poses.shape == (4, 4): grasp_poses = grasp_poses.unsqueeze(0) diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index 93f90c683..8916536c5 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -74,6 +74,33 @@ make_manipulation_slot, ) +_UPRIGHT_SIDE_GRASP_MAX_AXIS_ALIGNMENT = 0.65 +_UPRIGHT_SIDE_GRASP_MIN_AXIS_FRACTION = 0.35 +_UPRIGHT_SIDE_GRASP_MAX_AXIS_FRACTION = 0.75 +_UPRIGHT_SIDE_GRASP_HEIGHT_COST_WEIGHT = 2.0 + + +def _upright_yaw_pose_variants( + target_pose: torch.Tensor, + sample_count: int, +) -> torch.Tensor: + """Return object poses with evenly sampled world-Z yaw rotations.""" + signed_steps = [0] + for step in range(1, (sample_count + 1) // 2): + signed_steps.extend((step, -step)) + if sample_count % 2 == 0: + signed_steps.append(sample_count // 2) + angles = target_pose.new_tensor(signed_steps) * (2.0 * math.pi / sample_count) + yaw = target_pose.new_zeros((sample_count, 3, 3)) + yaw[:, 0, 0] = torch.cos(angles) + yaw[:, 0, 1] = -torch.sin(angles) + yaw[:, 1, 0] = torch.sin(angles) + yaw[:, 1, 1] = torch.cos(angles) + yaw[:, 2, 2] = 1.0 + variants = target_pose[:, None].repeat(1, sample_count, 1, 1) + variants[:, :, :3, :3] = torch.matmul(yaw[None], target_pose[:, None, :3, :3]) + return variants + @dataclass(frozen=True, slots=True, eq=False) class GraspGoal(ObjectActionGoal): @@ -120,6 +147,9 @@ class PickUpOptions(ActionOptions): downstream_object_target_poses: tuple[PoseGoalValue, ...] = () """Future object poses that must be reachable with the selected grasp.""" + upright_yaw_samples: int = 1 + """Equivalent world-yaw samples for semantically upright downstream targets.""" + obj_upright_direction: torch.Tensor | None = None """Optional object local direction used to choose the upright grasp rotation.""" @@ -135,6 +165,8 @@ def __post_init__(self) -> None: raise ValueError("lift_height must be non-negative.") if self.pre_grasp_distance < 0.0: raise ValueError("pre_grasp_distance must be non-negative.") + if self.upright_yaw_samples < 1: + raise ValueError("upright_yaw_samples must be positive.") if self.approach_direction.shape != (3,): raise ValueError("approach_direction must have shape (3,).") if not torch.isfinite(self.approach_direction).all(): @@ -470,10 +502,22 @@ def _resolve_grasp_pose( options: PickUpOptions, approach_direction: torch.Tensor, ) -> tuple[torch.Tensor, torch.Tensor]: + grasp_cost_fn = None + if options.rotate_upright is not None: + grasp_cost_fn = lambda object_pose, grasp_poses, costs: ( + self._upright_grasp_costs( + semantics, + object_pose, + grasp_poses, + costs, + options, + ) + ) grasp_poses_result = semantics.affordance.get_valid_grasp_poses( obj_poses=object_pose, approach_direction=approach_direction, object_part=options.pick_object_part, + grasp_cost_fn=grasp_cost_fn, ) num_envs = object_pose.shape[0] n_max_pose = max(r[0].shape[0] for r in grasp_poses_result) @@ -557,8 +601,17 @@ def _select_feasible_grasp_variants( alignment_success = self._approach_alignment_mask( grasp_variants, options, approach_direction ) + upright_compatible = self._upright_grasp_compatibility_mask( + grasp_variants, + object_poses, + options, + ) pickup_success = ( - alignment_success & pre_grasp_success & grasp_success & lift_success + upright_compatible + & alignment_success + & pre_grasp_success + & grasp_success + & lift_success ) downstream_success_counts: list[list[int]] = [] object_to_eef_variants = torch.matmul( @@ -581,17 +634,37 @@ def _select_feasible_grasp_variants( f"(4, 4) or ({num_envs}, 4, 4), but got " f"{object_target_pose.shape}." ) - downstream_eef_variants = torch.matmul( - object_target_pose[:, None, None], object_to_eef_variants - ) - downstream_success, downstream_seed = self._compute_batch_candidate_ik( - downstream_eef_variants, downstream_seed, manipulator + object_target_variants = _upright_yaw_pose_variants( + object_target_pose, + options.upright_yaw_samples, ) + downstream_success = torch.zeros_like(pickup_success) + selected_qpos = downstream_seed + for yaw_target in object_target_variants.unbind(dim=1): + downstream_eef_variants = torch.matmul( + yaw_target[:, None, None], object_to_eef_variants + ) + yaw_success, yaw_qpos = self._compute_batch_candidate_ik( + downstream_eef_variants, + downstream_seed, + manipulator, + ) + newly_solved = ~downstream_success & yaw_success + selected_qpos = torch.where( + newly_solved[..., None], + yaw_qpos, + selected_qpos, + ) + downstream_success |= yaw_success + if bool((pickup_success & downstream_success).any(dim=(1, 2)).all()): + break + downstream_seed = selected_qpos pickup_success &= downstream_success downstream_success_counts.append(pickup_success.sum(dim=(1, 2)).tolist()) if not pickup_success.any(dim=(1, 2)).all(): logger.log_warning( "PickUp found no candidate with a feasible vertical pickup path: " + f"upright_compatible={upright_compatible.sum(dim=(1, 2)).tolist()}, " f"aligned={alignment_success.sum(dim=(1, 2)).tolist()}, " f"pre_grasp={pre_grasp_success.sum(dim=(1, 2)).tolist()}, " f"grasp={(pre_grasp_success & grasp_success).sum(dim=(1, 2)).tolist()}, " @@ -665,6 +738,116 @@ def _compute_batch_candidate_ik( qpos.reshape(num_envs, n_pose, n_variant, manipulator_dof), ) + def _upright_grasp_compatibility_mask( + self, + grasp_xpos: torch.Tensor, + object_poses: torch.Tensor, + options: PickUpOptions, + ) -> torch.Tensor: + """Reject upright grasps that clamp the object's support and top faces.""" + shape = grasp_xpos.shape[:3] + if options.rotate_upright is None: + return torch.ones(shape, dtype=torch.bool, device=grasp_xpos.device) + local_upright = self._normalized_obj_upright_direction(options).to( + device=grasp_xpos.device, + dtype=grasp_xpos.dtype, + ) + object_poses = object_poses.to( + device=grasp_xpos.device, + dtype=grasp_xpos.dtype, + ) + world_upright = torch.matmul(object_poses[:, :3, :3], local_upright) + closing_axes = torch.nn.functional.normalize( + grasp_xpos[..., :3, 0], + dim=-1, + ) + axis_alignment = torch.abs( + torch.sum(closing_axes * world_upright[:, None, None, :], dim=-1) + ) + return axis_alignment <= _UPRIGHT_SIDE_GRASP_MAX_AXIS_ALIGNMENT + + def _upright_grasp_costs( + self, + semantics: ObjectSemantics, + object_pose: torch.Tensor, + grasp_poses: torch.Tensor, + costs: torch.Tensor, + options: PickUpOptions, + ) -> torch.Tensor: + """Rank side grasps before generator top-k truncation.""" + local_upright = self._normalized_obj_upright_direction(options).to( + device=grasp_poses.device, + dtype=grasp_poses.dtype, + ) + object_pose = object_pose.to( + device=grasp_poses.device, + dtype=grasp_poses.dtype, + ) + world_upright = torch.matmul(object_pose[:3, :3], local_upright) + closing_axes = torch.nn.functional.normalize( + grasp_poses[:, :3, 0], + dim=-1, + ) + axis_alignment = torch.abs( + torch.sum(closing_axes * world_upright[None, :], dim=-1) + ) + adjusted = torch.where( + axis_alignment <= _UPRIGHT_SIDE_GRASP_MAX_AXIS_ALIGNMENT, + costs, + torch.full_like(costs, torch.inf), + ) + + vertices = semantics.geometry.get("mesh_vertices") + if vertices is None: + return adjusted + vertices = torch.as_tensor( + vertices, + dtype=grasp_poses.dtype, + device=grasp_poses.device, + ) + if vertices.ndim != 2 or vertices.shape[-1] != 3 or vertices.numel() == 0: + return adjusted + vertex_axis_positions = torch.matmul(vertices, local_upright) + axis_min = vertex_axis_positions.min() + axis_extent = vertex_axis_positions.max() - axis_min + if float(axis_extent) <= 1.0e-6: + return adjusted + + relative_centers = grasp_poses[:, :3, 3] - object_pose[None, :3, 3] + center_axis_positions = torch.sum( + relative_centers * world_upright[None, :], + dim=-1, + ) + center_fractions = (center_axis_positions - axis_min) / axis_extent + interval = ( + _UPRIGHT_SIDE_GRASP_MAX_AXIS_FRACTION + - _UPRIGHT_SIDE_GRASP_MIN_AXIS_FRACTION + ) + height_penalty = ( + torch.clamp( + _UPRIGHT_SIDE_GRASP_MIN_AXIS_FRACTION - center_fractions, + min=0.0, + ) + + torch.clamp( + center_fractions - _UPRIGHT_SIDE_GRASP_MAX_AXIS_FRACTION, + min=0.0, + ) + ) / interval + return adjusted + _UPRIGHT_SIDE_GRASP_HEIGHT_COST_WEIGHT * height_penalty + + def _normalized_obj_upright_direction( + self, + options: PickUpOptions, + ) -> torch.Tensor: + direction = options.obj_upright_direction + if direction is None: + direction = torch.tensor([0, 0, 1], dtype=torch.float32) + direction = direction.to(device=self.device, dtype=torch.float32) + norm = torch.linalg.vector_norm(direction) + if norm <= 1.0e-6: + logger.log_error("obj_upright_direction must be non-zero.", ValueError) + return direction / norm + def _upright_adjusted_grasp_poses( self, grasp_xpos: torch.Tensor, @@ -675,14 +858,14 @@ def _upright_adjusted_grasp_poses( if options.rotate_upright is None: return grasp_xpos - if options.obj_upright_direction is None: - upright_direction = torch.tensor( - [0, 0, 1], dtype=torch.float32, device=self.device - ) - else: - upright_direction = options.obj_upright_direction.to( - device=self.device, dtype=torch.float32 - ) + upright_direction = self._normalized_obj_upright_direction(options).to( + device=grasp_xpos.device, + dtype=grasp_xpos.dtype, + ) + object_pose = object_pose.to( + device=grasp_xpos.device, + dtype=grasp_xpos.dtype, + ) obj_upright = torch.matmul(object_pose[:, :3, :3], upright_direction) adjusted_grasp_xpos = grasp_xpos.clone() grasp_ry = adjusted_grasp_xpos[..., :3, 1] diff --git a/embodichain/toolkits/graspkit/pg_grasp/antipodal_generator.py b/embodichain/toolkits/graspkit/pg_grasp/antipodal_generator.py index abc0466ef..c53d92613 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/antipodal_generator.py +++ b/embodichain/toolkits/graspkit/pg_grasp/antipodal_generator.py @@ -14,8 +14,11 @@ # limitations under the License. # ---------------------------------------------------------------------------- +from __future__ import annotations + import os import argparse +from collections.abc import Callable import open3d as o3d import time import torch @@ -613,6 +616,9 @@ def get_valid_grasp_poses( approach_direction: torch.Tensor, object_part: str = "center", visualize_collision: bool = False, + pose_cost_fn: ( + Callable[[torch.Tensor, torch.Tensor], torch.Tensor] | None + ) = None, ): if self._hit_point_pairs is None: logger.log_warning( @@ -659,6 +665,7 @@ def get_valid_grasp_poses( approach_direction=approach_direction, mesh_vert_transformed=mesh_vert_transformed, visualize_collision=visualize_collision, + pose_cost_fn=pose_cost_fn, ) def get_dual_arm_valid_grasp_poses( @@ -762,6 +769,9 @@ def _filter_valid_grasp_poses( mesh_vert_transformed: torch.Tensor, object_pose: torch.Tensor, visualize_collision: bool = False, + pose_cost_fn: ( + Callable[[torch.Tensor, torch.Tensor], torch.Tensor] | None + ) = None, ): grasp_x = F.normalize(hit_points_ - origin_points_, dim=-1) cos_angle = torch.clamp((grasp_x * approach_direction).sum(dim=-1), -1.0, 1.0) @@ -850,6 +860,17 @@ def _filter_valid_grasp_poses( center_cost = center_distance / center_distance.max() length_cost = 1 - valid_open_lengths / valid_open_lengths.max() total_cost = 0.2 * angle_cost + 0.2 * length_cost + 0.6 * center_cost + if pose_cost_fn is not None: + adjusted_cost = pose_cost_fn(valid_grasp_poses, total_cost) + if adjusted_cost.shape != total_cost.shape: + logger.log_error( + "pose_cost_fn must preserve the grasp cost shape.", + ValueError, + ) + total_cost = adjusted_cost.to( + device=total_cost.device, + dtype=total_cost.dtype, + ) n_valid = valid_grasp_poses.shape[0] if n_valid == 0: diff --git a/tests/sim/atomic_actions/test_affordance.py b/tests/sim/atomic_actions/test_affordance.py index 0e123eefe..3835c801b 100644 --- a/tests/sim/atomic_actions/test_affordance.py +++ b/tests/sim/atomic_actions/test_affordance.py @@ -104,6 +104,29 @@ def test_valid_grasp_poses_casts_approach_direction_to_generator_device(self): assert approach_direction.dtype == torch.float32 assert approach_direction.device == generator.device + def test_valid_grasp_poses_applies_object_aware_cost_callback(self): + aff = AntipodalAffordance() + generator = Mock() + generator.device = torch.device("cpu") + object_pose = torch.eye(4) + object_pose[0, 3] = 0.25 + grasp_poses = torch.eye(4).repeat(2, 1, 1) + costs = torch.tensor([0.2, 0.4]) + + def get_valid_grasp_poses(**kwargs): + adjusted = kwargs["pose_cost_fn"](grasp_poses, costs) + return True, grasp_poses, 0.0, adjusted + + generator.get_valid_grasp_poses.side_effect = get_valid_grasp_poses + aff._generator = generator + + results = aff.get_valid_grasp_poses( + object_pose.unsqueeze(0), + grasp_cost_fn=lambda obj, _grasps, current: current + obj[0, 3], + ) + + assert torch.allclose(results[0][1], torch.tensor([0.45, 0.65])) + def test_best_grasp_poses_casts_approach_direction_to_generator_device(self): aff = AntipodalAffordance() generator = Mock() diff --git a/tests/sim/atomic_actions/test_primitives_helpers.py b/tests/sim/atomic_actions/test_primitives_helpers.py index 2ff754fe0..f5f297738 100644 --- a/tests/sim/atomic_actions/test_primitives_helpers.py +++ b/tests/sim/atomic_actions/test_primitives_helpers.py @@ -27,6 +27,10 @@ resolve_batched_pose, resolve_object_target, ) +from embodichain.lab.sim.atomic_actions.primitives.pick_up import ( + PickUpOptions, + _upright_yaw_pose_variants, +) BATCH_SIZE = 2 ROBOT_DOF = 6 @@ -83,3 +87,19 @@ def test_resolve_object_target_uses_custom_name_in_shape_error() -> None: device=torch.device("cpu"), name="placing_object_target_pose", ) + + +def test_upright_yaw_pose_variants_preserve_translation() -> None: + pose = torch.eye(4).repeat(2, 1, 1) + pose[:, :3, 3] = torch.tensor([[0.2, -0.1, 0.8], [-0.3, 0.4, 0.7]]) + + variants = _upright_yaw_pose_variants(pose, 4) + + assert variants.shape == (2, 4, 4, 4) + assert torch.allclose(variants[:, :, :3, 3], pose[:, None, :3, 3].expand(-1, 4, -1)) + assert torch.allclose(variants[:, 0], pose) + + +def test_upright_yaw_samples_must_be_positive() -> None: + with pytest.raises(ValueError, match="upright_yaw_samples"): + PickUpOptions(upright_yaw_samples=0)