From a7b2be5dc15e821d89f2f37607bd9e24e9a3ef41 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 20:28:33 +0800 Subject: [PATCH 01/28] fix(atomic-actions): tighten phase-zero validation --- .../topics/atomic-actions/atomic-actions.md | 5 +++ .../topics/motion-planning/motion-planning.md | 6 +++ .../lab/sim/atomic_actions/trajectory_ops.py | 3 +- .../lab/sim/planners/curobo/curobo_planner.py | 31 ++++++++++++++- embodichain/lab/sim/planners/utils.py | 13 +++++-- .../sim/atomic_actions/test_trajectory_ops.py | 20 ++++++++++ tests/sim/planners/test_curobo_planner.py | 38 +++++++++++++++++++ 7 files changed, 110 insertions(+), 6 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 112d2596b..1c692b475 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -87,6 +87,11 @@ their last successful qpos. Use invocation `skill_options` for multiple variants with the same stable `skill_id`; do not create per-variant built-in instances. +Composite actions allocate their named trajectory segments from the total +sample budget with `split_three_segments()`. The first motion allocation rounds +`(sample_count - hand_interp_steps) * first_segment_ratio`; callers must not +reproduce that calculation or assume truncation. + ## Dynamic execution and recovery `SceneEntityPose(entity_id, relative_pose)` is resolved from the latest scene diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 4f5644cc2..6b10e5524 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -126,6 +126,9 @@ them into `CuroboPlanOptions.dynamic_obstacle_poses`. to the backend hook. Atomic actions use that facade from their framework-owned `plan()` template when a `SceneSnapshot` declares collision entities; individual skills must not construct backend obstacle options themselves. +`CuroboWorldCfg` rejects duplicate obstacle names and requires every +`dynamic_obstacle_name` to match an object registered in `rigid_objects`, so a +planner-local mismatch fails before backend construction. `MotionGenerator.resolve_plan_options()` is the corresponding option-ownership boundary. It copies caller-supplied typed options, otherwise obtains backend @@ -261,6 +264,9 @@ The decorator checks that every `PlanState` in `target_states` shares the same l accepts only `EEF_MOVE` and `JOINT_MOVE` and raises for other target types. - **Missing interpolation inputs** — `strategy="ik_interp"` requires explicit `start_qpos` and `sample_count`; it never reads live robot state implicitly. +- **CUDA requested on a CPU-only runtime** — planner success-mask normalization + raises a direct `ValueError` before querying the active CUDA device. It never + silently falls back to CPU. - **Constraint tolerance** — `is_satisfied_constraint` allows 10% velocity / 25% acceleration overshoot. Dense waypoint trajectories may appear to violate constraints but pass validation. - **Fork safety with GPU sim** — `ToppraPlannerCfg.mp_context=None` defaults to `spawn` on GPU to avoid fork-after-CUDA-init hazards. Force `fork` only when the sim device is CPU or you have verified it is safe. - **cuRobo shared-world mismatch** — World-frame poses may differ solely because replicated arenas are offset. Compare poses after robot-base rebasing: keep `multi_env=False` if they match, and enable it only when robot-relative layouts differ. diff --git a/embodichain/lab/sim/atomic_actions/trajectory_ops.py b/embodichain/lab/sim/atomic_actions/trajectory_ops.py index 0154e8418..ca4d4c61a 100644 --- a/embodichain/lab/sim/atomic_actions/trajectory_ops.py +++ b/embodichain/lab/sim/atomic_actions/trajectory_ops.py @@ -18,7 +18,6 @@ from __future__ import annotations -import numpy as np import torch from embodichain.lab.sim.planners import MoveType, PlanResult, PlanState @@ -182,7 +181,7 @@ def split_three_segments( third_segment_name: str = "third", ) -> tuple[int, int, int]: """Split a sample budget into motion, hand, and motion segments.""" - first = int(np.round(sample_count - hand_interp_steps) * first_segment_ratio) + first = int(round((sample_count - hand_interp_steps) * first_segment_ratio)) if first < 2: raise ValueError( f"Not enough waypoints for {first_segment_name} trajectory. " diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index f9cf5fce5..51766737f 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -178,7 +178,7 @@ class CuroboWorldCfg: """ dynamic_obstacle_names: list[str] = [] - """Obstacle names whose poses may be updated between plans.""" + """Registered rigid-object names whose poses may be updated between plans.""" multi_env: bool = False """Whether cuRobo allocates one collision-world instance per environment. @@ -211,6 +211,35 @@ class CuroboWorldCfg: """ def __post_init__(self) -> None: + dynamic_names = list(self.dynamic_obstacle_names) + if len(set(dynamic_names)) != len(dynamic_names) or not all( + isinstance(name, str) and name for name in dynamic_names + ): + raise ValueError( + "dynamic_obstacle_names must contain unique non-empty names." + ) + + rigid_objects = list(self.rigid_objects or ()) + rigid_names = [ + getattr(obj, "uid", None) or f"obstacle_{index}" + for index, obj in enumerate(rigid_objects) + ] + if not all(isinstance(name, str) and name for name in rigid_names): + raise ValueError( + "CuroboWorldCfg.rigid_objects must have non-empty string names." + ) + if len(set(rigid_names)) != len(rigid_names): + raise ValueError( + "CuroboWorldCfg.rigid_objects must have unique obstacle names." + ) + missing = set(dynamic_names).difference(rigid_names) + if missing: + raise ValueError( + "dynamic_obstacle_names reference objects not present in " + f"rigid_objects: {sorted(missing)}." + ) + self.dynamic_obstacle_names = dynamic_names + # Wrap live RigidObjects so the @configclass field-deepcopy (run right # after this by custom_post_init) shares references instead of trying to # pickle non-pickleable C++ dexsim handles held by each RigidObject. diff --git a/embodichain/lab/sim/planners/utils.py b/embodichain/lab/sim/planners/utils.py index 1913449eb..76a4e1beb 100644 --- a/embodichain/lab/sim/planners/utils.py +++ b/embodichain/lab/sim/planners/utils.py @@ -58,11 +58,18 @@ def normalize_success_mask( Raises: TypeError: If ``success`` is neither boolean nor binary integer data. - ValueError: If a tensor does not match the required batch shape. + ValueError: If a tensor does not match the required batch shape or a + CUDA device is requested while CUDA is unavailable. """ resolved_device = torch.device(device) - if resolved_device.type == "cuda" and resolved_device.index is None: - resolved_device = torch.device(f"cuda:{torch.cuda.current_device()}") + if resolved_device.type == "cuda": + if not torch.cuda.is_available(): + raise ValueError( + "CUDA device requested for success-mask normalization, but " + "torch.cuda.is_available() is False." + ) + if resolved_device.index is None: + resolved_device = torch.device(f"cuda:{torch.cuda.current_device()}") if isinstance(success, bool): return torch.full((n_envs,), success, dtype=torch.bool, device=resolved_device) if not isinstance(success, torch.Tensor): diff --git a/tests/sim/atomic_actions/test_trajectory_ops.py b/tests/sim/atomic_actions/test_trajectory_ops.py index e3dae34a6..41c9c4ed5 100644 --- a/tests/sim/atomic_actions/test_trajectory_ops.py +++ b/tests/sim/atomic_actions/test_trajectory_ops.py @@ -83,6 +83,21 @@ def test_non_binary_integer_success_is_rejected(self): name="IK success", ) + def test_cuda_device_requires_available_runtime(self, monkeypatch): + def unexpected_current_device(): + raise AssertionError("current_device must not be queried") + + monkeypatch.setattr(torch.cuda, "is_available", lambda: False) + monkeypatch.setattr(torch.cuda, "current_device", unexpected_current_device) + + with pytest.raises(ValueError, match="CUDA device requested"): + normalize_success_mask( + True, + n_envs=2, + device="cuda", + name="IK success", + ) + class TestResolvePoseTarget: def test_unbatched_pose_broadcasts(self): @@ -279,6 +294,11 @@ def test_raises_when_first_segment_too_small(self): with pytest.raises(ValueError): split_three_segments(6, 5) + def test_ratio_is_rounded_after_multiplication(self): + first, hand, third = split_three_segments(10, 2) + + assert (first, hand, third) == (5, 2, 3) + class TestTranslatePoseWorld: def test_offset_adds_to_translation(self): diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index f62563d2c..ba86c7d4a 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -218,6 +218,44 @@ def test_curobo_world_cfg_uses_v2_safe_default_collision_cache(): assert cfg.obstacle_representation == "sphere" +def test_curobo_world_cfg_accepts_registered_dynamic_obstacle(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + cfg = CuroboWorldCfg( + rigid_objects=[obstacle], + dynamic_obstacle_names=["known"], + ) + + assert cfg.dynamic_obstacle_names == ["known"] + + +def test_curobo_world_cfg_rejects_unregistered_dynamic_obstacle(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(ValueError, match="not present in rigid_objects"): + CuroboWorldCfg( + rigid_objects=[obstacle], + dynamic_obstacle_names=["unknown"], + ) + + +def test_curobo_world_cfg_rejects_duplicate_dynamic_obstacle_names(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(ValueError, match="unique non-empty"): + CuroboWorldCfg( + rigid_objects=[obstacle], + dynamic_obstacle_names=["known", "known"], + ) + + +def test_curobo_world_cfg_rejects_duplicate_rigid_object_names(): + obstacle_type = type("NamedObstacle", (), {"uid": "duplicate"}) + + with pytest.raises(ValueError, match="unique obstacle names"): + CuroboWorldCfg(rigid_objects=[obstacle_type(), obstacle_type()]) + + def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): planner = object.__new__(CuroboPlanner) configured_pose = torch.eye(4).unsqueeze(0) From 92a9d5f967a5f7c8a664201d9ab4fd30ce970b79 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 20:28:40 +0800 Subject: [PATCH 02/28] docs(atomic-actions): align expert program plan with main --- .../design/declarative_expert_program_plan.md | 144 +++++++++++------- 1 file changed, 92 insertions(+), 52 deletions(-) diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 66c78b4e4..1a2999d58 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,10 +1,12 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime - Status: design plan -- Baseline: `main@26b69c22d7efbf96cb35f5487f6922c8645f91d7` +- Baseline: `main@e445133c79c8b32019dab1c844b799b43a1658d6` - Last updated: 2026-08-10 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), [#474](https://github.com/DexForce/EmbodiChain/issues/474) +- Related implementation: + [#475](https://github.com/DexForce/EmbodiChain/pull/475) ## 1. Executive summary @@ -88,8 +90,8 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is based on commit `26b69c22` rather than uncommitted working-tree -changes. +This plan is updated against committed `main@e445133c` after PR #475 rather +than uncommitted working-tree changes. | Capability | Current main | Design consequence | |---|---|---| @@ -97,24 +99,27 @@ changes. | Lazy `DemoSegment` execution and legacy demo compatibility (#460) | Available | Use a thin demo adapter; do not create a second dataset executor. | | Closed-loop `ExecutionRunner` and simulator ports (#449) | Available | `SkillRuntime` wraps/reuses the runner rather than scheduling commands itself. | | Dynamic scene recovery and `DynamicCollisionMode` (#450) | Available | Profiles select precise collision semantics and fail early when required capabilities are unavailable. | +| Refined planning architecture (#475) | `MotionGenerator.generate()` is the single planning facade; each `ActionPlan` owns one trajectory and one recovery boundary; named `TrajectorySegment`s are metadata | Do not reintroduce `TrajectoryBuilder`, `MotionPlanningAdapter`, or trajectory-segment recovery. | | Environment cadence through `BaseEnv.step_dt` (#472) | Available | Expert configuration does not expose a separate control period. | | Adaptive dynamic-object settling (#470) | Reset/event implementation exists | Extract a reusable monitor; demo post-policies must advance through `env.step()`. | | Repeated cube pick/place demo | Manually constructs invocations and transform math | First configuration-only vertical slice. | | Open Drawer task (#473) | Manually builds approach, grasp, pull, and command trajectories | Evidence that the semantic layer needs articulation/link/affordance references and a reusable articulation skill. | | Action Bank | Configuration plus task-specific Python node/edge functions | Keep only as a compatibility path while semantic coverage is built. | -Several #474 findings remain prerequisites on this baseline: +PR #475 resolved cumulative translation/rotation publication, removed the dead +`MotionPolicy.interpolation` field, and unified strategy dispatch. The +remaining #474 prerequisites on this baseline are: -- `RigidObjectSceneProvider` still updates its pose baseline on every snapshot, - so repeated sub-threshold movement may never publish a revision. - `AtomicAction` rejects the formerly documented `plan()` extension override and requires `_plan()` without a compatibility window. - scene pose, semantics, affordance, and collision registration still have multiple sources of truth; - ordinary callers still see a large low-level public surface and must perform semantic transform and verifier plumbing; -- `MotionPolicy` still exposes implementation-level tuning, including an - unused/misleading interpolation option. +- provider collision entity IDs and planner-declared dynamic obstacle names are + not cross-validated at integration construction time; +- `MotionPolicy` still exposes implementation-level tuning that should be + hidden behind semantic presets for ordinary users. One #474 finding has changed since its review branch: the ambiguous `collision_check` switch has been replaced by `DynamicCollisionMode.OFF`, @@ -130,7 +135,8 @@ The following #471 decisions remain valid: - lazy re-observation when later goals depend on physical effects; - distinct action-effect verification, segment post-policy, and task-level validation responsibilities; -- named phases instead of trajectory indices; +- stable named trajectory segments for tracing instead of recomputed trajectory + indices; - sequential execution first, then resource-aware parallel execution; - continued legacy compatibility during migration. @@ -145,6 +151,22 @@ The following parts must be adjusted: | Callers may supply place EEF poses and pickup look-ahead options. | `Place` is object-centric; the compiler derives EEF targets from verified held state and propagates downstream targets automatically. | | Configuration and handwritten code are separate entry paths. | Both construct the same semantic call specification and converge before binding or grounding. | +### 5.1 Segment terminology after #475 + +The design uses three different segment layers. Bare "segment" should be +avoided wherever the layer would be ambiguous. + +| Term | Type | Meaning | +|---|---|---| +| Program segment | `SegmentCfg` | Expert Program logical transaction boundary; owns post-policies, validators, and re-observation semantics. | +| Demo segment | `DemoSegment` | Lazy Gym/demo executor carrier and dataset boundary produced from a program segment. | +| Trajectory segment | `TrajectorySegment` | Named half-open waypoint range within one `ActionPlan`; used for inspection, visualization, tracing, and terminal-effect correlation only. | + +A trajectory segment is not an independent planning, recovery, effect, or +timeout boundary. One atomic action remains the recovery/effect boundary. +"Phase 0" through "Phase 8" below refer only to implementation-plan stages; +atomic motion structure is called a trajectory segment, not a phase. + ## 6. Proposed architecture ### 6.1 API layers @@ -331,7 +353,7 @@ the compiler partitions safe static stages and inserts observed boundaries. - persistent, per-environment verified `TaskState`; - built-in effect-monitor selection and feedback to `ExecutionSession`; - uniform `SkillResult`, cancellation, timeout, and safe-stop behavior; -- semantic and named-phase events. +- semantic action events and optional trajectory-segment trace metadata. Catalog discovery and runtime installation should have distinct names. For example, a catalog can `discover` a descriptor while an engine explicitly @@ -462,13 +484,14 @@ examples. Stable names should be preferred over internal fields: ```yaml advanced: - phase_presets: - secure_grasp: precise + call_presets: + pick: precise recovery_preset: dynamic_scene ``` Raw planner instances, callables, arbitrary imports, and environment paths are -never serializable configuration values. +never serializable configuration values. Version 1 does not attach motion or +recovery policies to individual `TrajectorySegment`s. ## 9. Demonstration execution semantics @@ -482,8 +505,8 @@ Gym-aware runtime ports: - command sink: buffers the next full-robot command for the environment action manager; - clock: advances only when the demo executor calls `env.step()`; -- metadata sink: records compiler decisions, phases, effects, recovery, scene - revisions, and post-policy results. +- metadata sink: records compiler decisions, action trajectory segments, + effects, recovery, scene revisions, and post-policy results. The existing `SimulationExecutionAdapter` is not the demo execution loop because direct simulator updates can bypass environment managers and recorders. @@ -501,23 +524,27 @@ per yielded command. An incompatible command is rejected with a clear timing error; it is not silently resampled. Explicit timed-command resampling can be a later, separately tested feature. -Timeout for a named phase starts when its first command is dispatched, not when -an earlier phase or the whole segment is compiled. +Recovery timeout and retry budgets are scoped to the enclosing action attempt. +A `TrajectorySegment` does not start an independent timer or own a recovery +policy. Program-segment settling and validation use separate post-policy +deadlines. -### 9.3 Named phases +### 9.3 Named atomic trajectory segments -Plans and execution events need stable semantic phase names. Initial built-ins -should expose at least: +Plans need stable semantic trajectory-segment names. Current built-ins expose: -- pick: `approach`, `grasp_close`, `lift`; -- place: `lower`, `release`, `retract`; -- handover: role-specific approach, transfer, release, and retreat phases; -- articulation operation: `approach`, `grasp_close`, `operate`, `release`, - `retract`. +- pick: `approach`, `close`, `lift`; +- place: `approach`, `release`, `retract`; +- handover: `transfer`, `approach`, `close`, optional `hold`, `release`, and + `deliver`. -Post-policies and effect monitors subscribe to names, not trajectory sample -indices. The runtime validates requested phase names against the active skill -descriptor before execution. +Names are validated by `ActionPlan`; ranges may change after replanning when a +backend returns a different sample count. Effect monitors run at the action +effect boundary and may use `EffectVerificationRequest.terminal_segment` for +correlation. Program post-policies and validators subscribe to program/demo +segment boundaries, not trajectory segments. Articulation segment names should +be stabilized with the reusable articulation skill rather than predeclared in +the configuration schema. ### 9.4 Dynamic settling @@ -541,7 +568,7 @@ clear object dynamics. All runtime state is indexed by stable environment IDs: - scene revisions and active collision dependencies; -- current call/phase and command deadline; +- current program segment, semantic call, action waypoint, and command deadline; - recovery budgets and failure masks; - verified held-object/effect state; - post-policy progress and segment validation; @@ -558,7 +585,7 @@ capability parity. | Action Bank concept | Expert Program / semantic runtime | |---|---| -| scope | `Segment` or nested `Sequence` | +| scope | Program `SegmentCfg` or nested `SequenceCfg` | | custom node function | registered semantic call and shared compiler | | custom edge/target function | typed target provider or goal grounder | | graph edge | explicit sequence/effect dependency inferred by compiler | @@ -640,20 +667,29 @@ Semantic calls/compiler --> SkillRuntime/effect monitors ### Phase 0: correctness and compatibility prerequisites -Deliverables: +Landed on `main` through #475: + +- cumulative sub-threshold translation and rotation compare against the last + published pose; +- target/general-scene and per-environment collision revisions have regression + coverage; +- the dead `MotionPolicy.interpolation` field is removed and strategy dispatch + is unified; +- one action owns one trajectory and one recovery/effect boundary, while named + `TrajectorySegment`s remain metadata. + +Remaining gates: -- fix cumulative sub-threshold translation and rotation publication in - `RigidObjectSceneProvider` by comparing with the last published/significant - pose; -- add regression tests for target and collision-world revisions; -- decide the supported `plan()`/`_plan()` custom-action extension contract and - provide a compatibility/deprecation path before enforcing a break; -- remove or implement misleading `MotionPolicy` fields, keeping collision - semantics expressed by `DynamicCollisionMode`; -- add early cross-validation for registry/provider/planner obstacle names. +- retain `_plan()` as the new extension hook and decide whether legacy + subclasses overriding `plan()` receive a tested compatibility/deprecation + adapter or continue to fail at class-definition time; +- cross-validate provider collision entity IDs against planner-declared dynamic + obstacle names when both integrations are constructed. Phase 1 extends this + same validation to registry-derived configuration. -Exit criteria: all #474 P0 items are resolved on main and custom actions have a -documented, tested upgrade path. +Exit criteria: both remaining gates pass on `main`. Phase 1 must not depend on +an undocumented custom-action break or defer mismatched obstacle names until +planning/execution. ### Phase 1: unified integration data @@ -702,16 +738,17 @@ compiler/runtime code and produce equivalent results. Deliverables: -- stable named phases in plans/descriptors/events; +- expose the existing named plan trajectory segments through optional demo + trace metadata without adding segment-level recovery; - reusable `DynamicSettleMonitor` shared by reset and demo paths; - Gym observation, buffered command, and environment-clock ports; - thin `AtomicDemoBridge` yielding lazy `DemoSegment`s; - exact `BaseEnv.step_dt` timing validation; -- runtime metadata for calls, phases, effects, recovery, scene revisions, - settling, and validation. +- runtime metadata for calls, trajectory segments, effects, recovery, scene + revisions, settling, and validation. -Exit criteria: no demo command bypasses `env.step()`, and phase/post-policy -behavior contains no hard-coded trajectory index. +Exit criteria: no demo command bypasses `env.step()`, and no post-policy, +effect, or trace integration contains a hard-coded trajectory index. ### Phase 5: Expert Program version 1 and repeated-cube vertical slice @@ -739,7 +776,7 @@ Deliverables: - articulation/link/affordance registry integration; - reusable articulation-operation semantic call, compiler, effect monitor, and - named phases; + named trajectory segments; - configuration-based Open Drawer migration; - migrate additional sequential tasks to reveal missing reusable grounders, monitors, and validators. @@ -788,7 +825,8 @@ independent of adoption of the new path. - object-centric place conversion from one immutable snapshot and verified held state; - effect monitor state transitions and timeout/recovery feedback; -- named phase validation and exact step-duration conversion; +- trajectory-segment coverage/name validation and exact step-duration + conversion; - Action Bank compatibility adapters where introduced. ### Integration tests with fake ports @@ -801,7 +839,8 @@ independent of adoption of the new path. ### Simulation tests -- three-segment repeated cube pick/place with free-fall re-observation; +- three-program/demo-segment repeated cube pick/place with free-fall + re-observation; - moving target and dynamic collision recovery with the `safe` preset; - grasp/release/handover effect monitors; - settling success and timeout metadata; @@ -833,9 +872,10 @@ The design is complete when all of the following hold: - [ ] Custom actions have a documented and tested compatibility path. - [ ] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass through `env.step()`. -- [ ] Phase hooks use stable names rather than trajectory indices. +- [ ] 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 segments with settle/effect/validation metadata. + observed program/demo segments with settle/effect/validation metadata. - [ ] Multi-environment progress, effects, recovery, and failures remain independent. - [ ] Advanced users retain typed goals, invocations, policies, providers, From 02588b5f5b54142184619cfce673dbd647154232 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 21:16:24 +0800 Subject: [PATCH 03/28] feat(atomic-actions): add stable snapshot identity --- .../lab/sim/atomic_actions/affordance.py | 38 ++- embodichain/lab/sim/atomic_actions/core.py | 52 ++- embodichain/lab/sim/atomic_actions/effects.py | 12 +- embodichain/lab/sim/atomic_actions/goals.py | 43 +++ tests/sim/atomic_actions/test_affordance.py | 17 +- tests/sim/atomic_actions/test_core.py | 319 +++++++++++++++++- 6 files changed, 462 insertions(+), 19 deletions(-) diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index fdab5e91f..dbe1ffea7 100644 --- a/embodichain/lab/sim/atomic_actions/affordance.py +++ b/embodichain/lab/sim/atomic_actions/affordance.py @@ -240,17 +240,18 @@ def get_approach_direction(self, point_idx: int) -> torch.Tensor: class AssembleAffordance(Affordance): """Affordance describing how an assemble object fits onto a base object. - The base object anchors the assembly: its world pose is read at planning - time from :attr:`base_object_entity` so the target tracks a moved base. The - assemble object is the part that is picked up and placed; its target pose is - ``base_pose @ assemble_to_base_pose``. + The affordance stores the relative assembly relation. Canonical planning + supplies the base object's snapshot pose through ``AssembleGoal.base_pose``; + :attr:`base_object_entity` is retained only as a deprecated direct-core + fallback when that goal field is omitted. The assemble object's target pose + is ``base_pose @ assemble_to_base_pose``. """ base_object_label: str = "" """Label of the base object the assemble object is placed onto.""" base_object_entity: BatchEntity | None = None - """Simulation entity for the base object; its pose anchors the assembly.""" + """Legacy live base entity used only when ``AssembleGoal.base_pose`` is absent.""" assemble_object_label: str = "" """Label of the assemble object that is picked up and placed.""" @@ -274,18 +275,41 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: Returns: Assemble-object target pose with shape ``(n_envs, 4, 4)``. + + Raises: + TypeError: If either pose value is not a tensor. + ValueError: If either pose has an unsupported shape or batch size. """ + if not isinstance(base_pose, torch.Tensor): + raise TypeError("base_pose must be a torch.Tensor.") base_pose = base_pose.to(dtype=torch.float32) - if base_pose.dim() == 2: + if base_pose.shape == (4, 4): base_pose = base_pose.unsqueeze(0) + elif ( + base_pose.dim() != 3 + or base_pose.shape[0] == 0 + or base_pose.shape[-2:] != (4, 4) + ): + raise ValueError("base_pose must have shape (4, 4) or (n_envs, 4, 4).") n_envs = base_pose.shape[0] + if not isinstance(self.assemble_to_base_pose, torch.Tensor): + raise TypeError("assemble_to_base_pose must be a torch.Tensor.") rel = self.assemble_to_base_pose.to( device=base_pose.device, dtype=torch.float32 ) - if rel.dim() == 2: + if rel.shape == (4, 4): rel = rel.unsqueeze(0).repeat(n_envs, 1, 1) + elif rel.dim() != 3 or rel.shape[-2:] != (4, 4) or rel.shape[0] == 0: + raise ValueError( + "assemble_to_base_pose must have shape (4, 4), (1, 4, 4), " + "or (n_envs, 4, 4)." + ) elif rel.shape[0] == 1: rel = rel.repeat(n_envs, 1, 1) + elif rel.shape[0] != n_envs: + raise ValueError( + "assemble_to_base_pose batch size must match base_pose batch size." + ) return torch.bmm(base_pose, rel) diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index ee3b1f391..d434cc000 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -70,9 +70,15 @@ def resolve_runtime_device(device: torch.device | str) -> torch.device: return resolved -@dataclass +@dataclass(frozen=True, slots=True, eq=False) class ObjectSemantics: - """Semantic and geometric information about an interaction object.""" + """Shallow-frozen semantic information about an interaction object. + + .. attention:: + Top-level fields cannot be rebound after construction. Nested + affordance and metadata objects may remain mutable but never establish + object identity. + """ affordance: Affordance """Affordance data describing supported interactions.""" @@ -89,6 +95,9 @@ class ObjectSemantics: entity: BatchEntity | None = None """Optional simulation entity used by deterministic grounding.""" + entity_id: str | None = None + """Stable scene identifier used by snapshot grounding and explicit identity.""" + def __post_init__(self) -> None: if not isinstance(self.affordance, Affordance): raise TypeError("affordance must be an Affordance instance.") @@ -98,9 +107,39 @@ def __post_init__(self) -> None: raise TypeError("properties must be a dict.") if not isinstance(self.label, str) or not self.label: raise ValueError("label must be a non-empty string.") + if self.entity_id is not None and ( + not isinstance(self.entity_id, str) or not self.entity_id.strip() + ): + raise ValueError("entity_id must be a non-empty string when set.") self.affordance.object_label = self.label +def _legacy_object_uid(semantics: ObjectSemantics) -> str | None: + """Return a valid legacy simulation UID without alias normalization.""" + uid = getattr(semantics.entity, "uid", None) + return uid if isinstance(uid, str) and uid.strip() else None + + +def _same_object_identity( + left: ObjectSemantics, + right: ObjectSemantics, +) -> bool: + """Return whether two semantic snapshots identify the same object.""" + if left is right: + return True + if left.entity_id is not None or right.entity_id is not None: + return ( + left.entity_id is not None + and right.entity_id is not None + and left.entity_id == right.entity_id + ) + left_uid = _legacy_object_uid(left) + right_uid = _legacy_object_uid(right) + if left_uid is not None or right_uid is not None: + return left_uid is not None and right_uid is not None and left_uid == right_uid + return left.entity is not None and left.entity is right.entity + + @dataclass(frozen=True, slots=True) class SkillDescriptor: """Machine-readable metadata for one registered atomic skill.""" @@ -429,6 +468,13 @@ def _uses_collision_world( ) return available + def _scene_dependencies( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + ) -> tuple[str, ...]: + """Return scene entities whose poses materially affect this plan.""" + return collect_scene_dependencies(request.goal) + def build_plan( self, request: ResolvedActionRequest[GoalT, OptionsT], @@ -522,7 +568,7 @@ def build_plan( ), diagnostics=diagnostics, segments=tuple(segments), - scene_dependencies=collect_scene_dependencies(request.goal), + scene_dependencies=self._scene_dependencies(request), collision_world_sensitive=self._uses_collision_world( request, context, diff --git a/embodichain/lab/sim/atomic_actions/effects.py b/embodichain/lab/sim/atomic_actions/effects.py index f9c6507aa..f9c1f537b 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -68,6 +68,8 @@ def _merge_held( update_mask: torch.Tensor, ) -> HeldObjectState | None: """Apply one optional held-object update per environment.""" + from .core import _same_object_identity + if previous is None and candidate is None: return None if previous is None: @@ -85,7 +87,7 @@ def _merge_held( if ( previous_retained and candidate_applied - and previous.semantics is not candidate.semantics + and not _same_object_identity(previous.semantics, candidate.semantics) ): raise ValueError( "Cannot merge different held-object semantics for one resource " @@ -96,7 +98,7 @@ def _merge_held( return None selector = update_mask[:, None, None] return HeldObjectState( - semantics=candidate.semantics if candidate_applied else previous.semantics, + semantics=(previous.semantics if previous_retained else candidate.semantics), object_to_eef=torch.where( selector, candidate.object_to_eef, previous.object_to_eef ), @@ -111,6 +113,8 @@ def _merge_coordinated( 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: @@ -128,7 +132,7 @@ def _merge_coordinated( if ( previous_retained and candidate_applied - and previous.semantics is not candidate.semantics + and not _same_object_identity(previous.semantics, candidate.semantics) ): raise ValueError( "Cannot merge different coordinated held-object semantics for one " @@ -139,7 +143,7 @@ def _merge_coordinated( return None selector = update_mask[:, None, None] return CoordinatedHeldObjectState( - semantics=candidate.semantics if candidate_applied else previous.semantics, + 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 ), diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index bdf38811a..f8d031130 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -18,6 +18,7 @@ from __future__ import annotations +import warnings from collections.abc import Mapping, Sequence from dataclasses import dataclass, fields, is_dataclass from typing import Any, ClassVar, Protocol, TYPE_CHECKING @@ -163,13 +164,55 @@ def resolve_pose_goal( return torch.bmm(pose, relative) +def _resolve_object_pose( + semantics: ObjectSemantics, + context: PlanningContext, + *, + name: str = "object", +) -> torch.Tensor: + """Resolve an object's pose from a snapshot or the deprecated live handle.""" + from .core import ObjectSemantics + + if not isinstance(semantics, ObjectSemantics): + raise TypeError("semantics must be an ObjectSemantics instance.") + if semantics.entity_id is not None: + return resolve_pose_goal( + SceneEntityPose(semantics.entity_id), + context, + name=name, + ) + if semantics.entity is None: + raise ValueError( + f"{name} requires ObjectSemantics.entity_id or a legacy entity handle." + ) + warnings.warn( + "Live pose grounding through ObjectSemantics.entity is deprecated; " + "set entity_id and provide the entity through PlanningContext.scene.", + DeprecationWarning, + stacklevel=2, + ) + pose = semantics.entity.get_local_pose(to_matrix=True) + if not isinstance(pose, torch.Tensor): + raise TypeError(f"{name} legacy entity pose must be a torch.Tensor.") + pose = pose.to(device=context.robot.qpos.device, dtype=torch.float32) + if pose.shape == (4, 4): + pose = pose.unsqueeze(0).expand(context.batch_size, -1, -1) + elif pose.shape != (context.batch_size, 4, 4): + raise ValueError(f"{name} legacy entity pose must match planning batch size.") + return pose.clone() + + def collect_scene_dependencies(value: Any) -> tuple[str, ...]: """Collect stable scene entity identifiers referenced by a goal value.""" + from .core import ObjectSemantics + found: set[str] = set() def visit(item: Any) -> None: if isinstance(item, SceneEntityPose): found.add(item.entity_id) + elif isinstance(item, ObjectSemantics): + return elif is_dataclass(item) and not isinstance(item, type): for data_field in fields(item): visit(getattr(item, data_field.name)) diff --git a/tests/sim/atomic_actions/test_affordance.py b/tests/sim/atomic_actions/test_affordance.py index 4c05e0844..d85dbfe3e 100644 --- a/tests/sim/atomic_actions/test_affordance.py +++ b/tests/sim/atomic_actions/test_affordance.py @@ -18,9 +18,11 @@ from __future__ import annotations -import torch from unittest.mock import Mock +import pytest +import torch + from embodichain.lab.sim.atomic_actions.affordance import ( Affordance, AntipodalAffordance, @@ -193,3 +195,16 @@ def test_get_assemble_object_pose_broadcasts_batched_relative_pose(self): result = aff.get_assemble_object_pose(base_pose) assert result.shape == (n_envs, 4, 4) assert torch.allclose(result, torch.bmm(base_pose, rel)) + + def test_get_assemble_object_pose_rejects_relative_batch_mismatch(self): + aff = AssembleAffordance(assemble_to_base_pose=torch.eye(4).repeat(3, 1, 1)) + base_pose = torch.eye(4).repeat(2, 1, 1) + + with pytest.raises(ValueError, match="batch size must match"): + aff.get_assemble_object_pose(base_pose) + + def test_get_assemble_object_pose_rejects_invalid_base_shape(self): + aff = AssembleAffordance() + + with pytest.raises(ValueError, match="base_pose must have shape"): + aff.get_assemble_object_pose(torch.eye(4).repeat(2, 1, 1, 1)) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 7383cde33..2a4ae179c 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import FrozenInstanceError +from unittest.mock import Mock import pytest import torch @@ -26,15 +27,22 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionOptions, + ActionPlan, Affordance, + AtomicAction, + CoordinatedHeldObjectState, DynamicCollisionMode, EndEffectorPoseGoal, EntityState, HeldObjectState, MotionPolicy, ObjectSemantics, + PlannerDiagnostics, PlanningContext, RecoveryPolicy, + ResolvedActionBinding, + ResolvedActionRequest, RobotObservation, SceneEntityPose, SceneSnapshot, @@ -43,24 +51,53 @@ TimedTrajectory, ) from embodichain.lab.sim.atomic_actions.goals import ( + _resolve_object_pose, collect_scene_dependencies, resolve_pose_goal, ) -def _semantics(label: str = "object") -> ObjectSemantics: - return ObjectSemantics(affordance=Affordance(), geometry={}, label=label) +def _semantics( + label: str = "object", + *, + entity_id: str | None = None, +) -> ObjectSemantics: + return ObjectSemantics( + affordance=Affordance(), + geometry={}, + label=label, + entity_id=entity_id, + ) -def _held(batch_size: int = 2) -> HeldObjectState: +def _held( + batch_size: int = 2, + *, + semantics: ObjectSemantics | None = None, +) -> HeldObjectState: pose = torch.eye(4).repeat(batch_size, 1, 1) return HeldObjectState( - semantics=_semantics(), + semantics=semantics or _semantics(), object_to_eef=pose, grasp_xpos=pose, ) +def _coordinated_held( + batch_size: int = 2, + *, + semantics: ObjectSemantics | None = None, +) -> CoordinatedHeldObjectState: + pose = torch.eye(4).repeat(batch_size, 1, 1) + return CoordinatedHeldObjectState( + semantics=semantics or _semantics(), + left_object_to_eef=pose, + right_object_to_eef=pose, + left_grasp_xpos=pose, + right_grasp_xpos=pose, + ) + + def _context(scene: SceneSnapshot | None = None) -> PlanningContext: qpos = torch.zeros(2, 4) return PlanningContext( @@ -71,6 +108,42 @@ def _context(scene: SceneSnapshot | None = None) -> PlanningContext: ) +class _DependencyAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): + """Minimal action proving that build_plan delegates dependencies to its hook.""" + + skill_id = "dependency_test" + GoalType = EndEffectorPoseGoal + OptionsType = ActionOptions + manipulator_roles = () + + @property + def device(self) -> torch.device: + return torch.device("cpu") + + def _uses_collision_world( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> bool: + del request, context + return False + + def _scene_dependencies( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + ) -> tuple[str, ...]: + dependencies = set(super()._scene_dependencies(request)) + dependencies.add("extra") + return tuple(sorted(dependencies)) + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + def test_action_binding_is_role_based_and_immutable() -> None: binding = ActionBinding( manipulators={"primary": "left_arm"}, @@ -94,6 +167,23 @@ def test_invocation_rejects_values_without_goal_contract() -> None: ) +@pytest.mark.parametrize("entity_id", ["", " ", 7]) +def test_object_semantics_rejects_invalid_entity_id(entity_id: object) -> None: + with pytest.raises(ValueError, match="entity_id"): + ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity_id=entity_id, # type: ignore[arg-type] + ) + + +def test_object_semantics_identity_fields_are_frozen() -> None: + semantics = _semantics(entity_id="cube") + + with pytest.raises(FrozenInstanceError): + semantics.entity_id = "other" # type: ignore[misc] + + def test_motion_and_recovery_policy_validate_shared_parameters() -> None: policy = MotionPolicy(sample_count=24, control_dt=0.01) assert policy.sample_count == 24 @@ -164,6 +254,137 @@ def test_task_state_normalizes_held_relations_and_masks_updates() -> None: assert state.get_held_object("right_arm") is None +def test_state_delta_merges_distinct_semantics_with_same_entity_id() -> None: + previous_semantics = _semantics(entity_id="cube") + candidate_semantics = _semantics(entity_id="cube") + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + + updated = StateDelta( + held_object_updates={ + "arm": _held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, False])) + + held = updated.get_held_object("arm") + assert previous_semantics is not candidate_semantics + assert held is not None and held.semantics is previous_semantics + + +def test_state_delta_replaces_semantics_when_all_rows_are_updated() -> None: + previous_semantics = _semantics(entity_id="cube") + candidate_semantics = _semantics(entity_id="cube") + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + + updated = StateDelta( + held_object_updates={ + "arm": _held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, True])) + + held = updated.get_held_object("arm") + assert held is not None and held.semantics is candidate_semantics + + +def test_state_delta_rejects_partial_merge_of_different_entity_ids() -> None: + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=_semantics(entity_id="cube"))}, + ) + delta = StateDelta( + held_object_updates={ + "arm": _held(semantics=_semantics(entity_id="cup")), + } + ) + + with pytest.raises(ValueError, match="different held-object semantics"): + delta.apply(state, torch.tensor([True, False])) + + +def test_state_delta_does_not_match_explicit_id_to_legacy_uid() -> None: + shared_entity = Mock(uid="cube") + previous_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=shared_entity, + entity_id="cube", + ) + candidate_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=shared_entity, + ) + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + delta = StateDelta( + held_object_updates={"arm": _held(semantics=candidate_semantics)} + ) + + with pytest.raises(ValueError, match="different held-object semantics"): + delta.apply(state, torch.tensor([True, False])) + + +def test_state_delta_merges_legacy_semantics_with_same_uid() -> None: + previous_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=Mock(uid="cube"), + ) + candidate_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=Mock(uid="cube"), + ) + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + + updated = StateDelta( + held_object_updates={ + "arm": _held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, False])) + + held = updated.get_held_object("arm") + assert held is not None and held.semantics is previous_semantics + + +def test_state_delta_merges_coordinated_semantics_with_same_entity_id() -> None: + previous_semantics = _semantics(entity_id="tray") + candidate_semantics = _semantics(entity_id="tray") + key = ("left_arm", "right_arm") + state = TaskState( + batch_size=2, + device="cpu", + coordinated_held_objects={ + key: _coordinated_held(semantics=previous_semantics), + }, + ) + + updated = StateDelta( + coordinated_held_object_updates={ + key: _coordinated_held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, False])) + + held = updated.get_coordinated_held_object(*key) + assert previous_semantics is not candidate_semantics + assert held is not None and held.semantics is previous_semantics + + def test_robot_observation_owns_input_tensors() -> None: qpos = torch.zeros(2, 4) observation = RobotObservation( @@ -214,6 +435,96 @@ def test_scene_entity_pose_enforces_confidence() -> None: ) +def test_object_pose_uses_explicit_scene_id_without_live_fallback() -> None: + scene_pose = torch.eye(4).repeat(2, 1, 1) + scene_pose[:, 0, 3] = torch.tensor([0.2, 0.4]) + entity = Mock() + entity.get_local_pose.return_value = torch.full((2, 4, 4), 9.0) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=entity, + entity_id="cup", + ) + context = _context( + SceneSnapshot( + timestamp=1.0, + version=1, + entities={"cup": EntityState(scene_pose)}, + ) + ) + + resolved = _resolve_object_pose(semantics, context) + + assert torch.equal(resolved, scene_pose) + entity.get_local_pose.assert_not_called() + + +def test_object_pose_missing_explicit_scene_id_does_not_fall_back() -> None: + entity = Mock() + entity.get_local_pose.return_value = torch.eye(4).repeat(2, 1, 1) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=entity, + entity_id="missing", + ) + + with pytest.raises(KeyError, match="unknown scene entity"): + _resolve_object_pose(semantics, _context()) + entity.get_local_pose.assert_not_called() + + +def test_object_pose_legacy_entity_warns_and_broadcasts() -> None: + entity = Mock() + entity.get_local_pose.return_value = torch.eye(4) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=entity, + ) + + with pytest.warns(DeprecationWarning, match="entity_id"): + resolved = _resolve_object_pose(semantics, _context()) + + assert resolved.shape == (2, 4, 4) + entity.get_local_pose.assert_called_once_with(to_matrix=True) + + +def test_dependency_collection_does_not_descend_object_semantics() -> None: + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + properties={"unrelated_pose": SceneEntityPose("hidden")}, + entity_id="object", + ) + + assert collect_scene_dependencies(semantics) == () + + +def test_build_plan_uses_action_scene_dependency_hook() -> None: + context = _context() + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ResolvedActionBinding(), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + action = _DependencyAction() + + plan = action.build_plan( + request, + context, + success=True, + trajectory=context.robot.qpos.unsqueeze(1), + diagnostics=PlannerDiagnostics(backend="test"), + ) + + assert plan.scene_dependencies == ("extra", "tracked") + + def test_scene_snapshot_expands_global_collision_world_revision() -> None: pose = torch.eye(4).repeat(2, 1, 1) snapshot = SceneSnapshot( From f8054f75f1ccfb3433f828317d5488151fd66d78 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 21:16:58 +0800 Subject: [PATCH 04/28] refactor(atomic-actions): ground object motion from snapshots --- .../primitives/coordinated_pickment.py | 35 +- .../atomic_actions/primitives/hand_over.py | 42 ++- .../primitives/move_held_object.py | 19 +- .../sim/atomic_actions/primitives/pick_up.py | 54 ++- .../sim/atomic_actions/primitives/place.py | 89 ++++- .../atomic_action/moving_target_recovery.py | 1 + tests/sim/atomic_actions/test_actions.py | 308 ++++++++++++++++-- 7 files changed, 461 insertions(+), 87 deletions(-) diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index f59d5286c..c5d7900b0 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -34,6 +34,7 @@ from ..goals import ( ObjectActionGoal, PoseGoalValue, + _resolve_object_pose, resolve_pose_goal, validate_pose_goal, ) @@ -59,7 +60,11 @@ class CoordinatedPickGoal(ObjectActionGoal): """Target pose for the shared object, shape ``(4, 4)`` or ``(n_envs, 4, 4)``.""" object_initial_pose: PoseGoalValue | None = None - """Optional initial object pose. Defaults to ``semantics.entity`` pose.""" + """Optional initial object pose. + + When omitted, the pose is grounded through the semantic object's stable + scene identity, with its live entity retained only as a legacy fallback. + """ def __post_init__(self) -> None: ObjectActionGoal.__post_init__(self) @@ -359,6 +364,22 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + request: ResolvedActionRequest[ + CoordinatedPickGoal, + CoordinatedPickmentOptions, + ], + ) -> tuple[str, ...]: + """Track the semantic object only when it supplies the initial pose.""" + dependencies = set(super()._scene_dependencies(request)) + target = request.goal + if target.object_initial_pose is None: + entity_id = target.semantics.entity_id + if entity_id is not None: + dependencies.add(entity_id) + return tuple(sorted(dependencies)) + def _resolve_resources( self, request: ResolvedActionRequest[CoordinatedPickGoal, CoordinatedPickmentOptions], @@ -424,14 +445,12 @@ def _resolve_object_initial_pose( ), "object_initial_pose", ) - if target.semantics.entity is None: - logger.log_error( - "CoordinatedPickGoal requires object_initial_pose when " - "semantics.entity is not provided.", - ValueError, - ) return self._resolve_pose( - target.semantics.entity.get_local_pose(to_matrix=True), + _resolve_object_pose( + target.semantics, + context, + name="object_initial_pose", + ), "object_initial_pose", ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 4c0878773..120c8b5d0 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -28,7 +28,7 @@ from ..bindings import ResolvedControlPart from ..control import GRASP_COMMAND, OPEN_COMMAND -from ..core import AtomicAction, ObjectSemantics +from ..core import AtomicAction, ObjectSemantics, _same_object_identity from ..effects import StateDelta from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask @@ -149,6 +149,14 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + request: ResolvedActionRequest[GraspGoal, HandOverOptions], + ) -> tuple[str, ...]: + """Return no goal-pose dependency because handover ignores grasp_xpos.""" + del request + return () + def _resolve_resources( self, request: ResolvedActionRequest[GraspGoal, HandOverOptions], @@ -224,7 +232,13 @@ def _plan( state = context semantics = target.semantics transfer_object_to_eef = self._resolve_transfer_object_to_eef( - state, resources.transfer_arm.name + state, + resources.transfer_arm.name, + semantics, + ) + transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( + state, + resources, ) assert options.middle_object_pose is not None assert options.final_object_pose is not None @@ -241,8 +255,17 @@ def _plan( receive_approach_direction / torch.linalg.vector_norm(receive_approach_direction) ) - # force object pose to have the same rotation as the current object pose, so that the handover is feasible. - current_object_pose = target.semantics.entity.get_local_pose(to_matrix=True) + # Keep the requested object orientation consistent with the verified + # attachment and the transferring arm's current measured pose. + transfer_current_eef = self.robot.compute_fk( + qpos=transfer_start_qpos, + name=resources.transfer_arm.name, + to_matrix=True, + ) + current_object_pose = torch.bmm( + transfer_current_eef, + pose_inv(transfer_object_to_eef), + ) middle_object_pose[:, :3, :3] = current_object_pose[:, :3, :3] final_object_pose[:, :3, :3] = current_object_pose[:, :3, :3] @@ -285,9 +308,6 @@ def _plan( ), ) - transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( - state, resources - ) segments = self._compute_segment_lengths( request.motion_policy.sample_count, options ) @@ -507,7 +527,7 @@ def _validate_pose_options(options: HandOverOptions) -> None: ) def _resolve_matrix(self, matrix: torch.Tensor, name: str) -> torch.Tensor: - matrix = matrix.to(device=self.device, dtype=torch.float32) + matrix = matrix.to(device=self.device, dtype=torch.float32).clone() if matrix.shape == (4, 4): matrix = matrix.unsqueeze(0).repeat(self.n_envs, 1, 1) if matrix.shape != (self.n_envs, 4, 4): @@ -522,6 +542,7 @@ def _resolve_transfer_object_to_eef( self, state: PlanningContext, transfer_control_part: str, + target_semantics: ObjectSemantics, ) -> torch.Tensor: held = state.get_held_object(transfer_control_part) if held is None: @@ -530,6 +551,11 @@ def _resolve_transfer_object_to_eef( f"{transfer_control_part!r} (run PickUp first).", ValueError, ) + if not _same_object_identity(target_semantics, held.semantics): + raise ValueError( + "HandOver target semantics must identify the object held by " + f"transfer control part {transfer_control_part!r}." + ) return self._resolve_matrix(held.object_to_eef, "held_object.object_to_eef") def _resolve_receive_grasp( 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 6bf8743b5..917a758c7 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -24,7 +24,11 @@ import torch from embodichain.utils import logger -from embodichain.utils.math import axis_angle_to_rotation_matrix, get_relative_rotation +from embodichain.utils.math import ( + axis_angle_to_rotation_matrix, + get_relative_rotation, + pose_inv, +) from ._helpers import arm_qpos_from_state, resolve_object_target from ..control import GRASP_COMMAND @@ -138,18 +142,19 @@ def _plan( end_arm_xpos = self.robot.compute_fk( start_arm_qpos, name=control_part, to_matrix=True ) + object_to_eef = held_object.object_to_eef.to( + device=self.device, dtype=torch.float32 + ) + if object_to_eef.shape == (4, 4): + object_to_eef = object_to_eef.unsqueeze(0).repeat(self.n_envs, 1, 1) + current_object_pose = torch.bmm(end_arm_xpos, pose_inv(object_to_eef)) if options.pick_rotate_upright is not None: self._apply_configured_upright_rotation( object_target_pose, end_arm_xpos, - held_object.semantics.entity.get_local_pose(to_matrix=True), + current_object_pose, options, ) - object_to_eef = held_object.object_to_eef.to( - device=self.device, dtype=torch.float32 - ) - if object_to_eef.shape == (4, 4): - object_to_eef = object_to_eef.unsqueeze(0).repeat(self.n_envs, 1, 1) move_eef_xpos = torch.bmm(object_target_pose, object_to_eef) if options.pick_rotate_upright is None: diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index ed81535c6..89832e50a 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -41,6 +41,7 @@ from ..goals import ( ObjectActionGoal, PoseGoalValue, + _resolve_object_pose, resolve_pose_goal, validate_pose_goal, ) @@ -166,6 +167,17 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + request: ResolvedActionRequest[GraspGoal, PickUpOptions], + ) -> tuple[str, ...]: + """Include the semantic object when it has a stable scene identity.""" + dependencies = set(super()._scene_dependencies(request)) + entity_id = request.goal.semantics.entity_id + if entity_id is not None: + dependencies.add(entity_id) + return tuple(sorted(dependencies)) + def _get_full_pickup_trajectory( self, grasp_xpos: torch.Tensor, @@ -289,6 +301,11 @@ def _plan( control_part = manipulator.name state = context sem = target.semantics + object_pose = _resolve_object_pose( + sem, + context, + name="pickup_object_pose", + ) if target.grasp_xpos is None and not isinstance( sem.affordance, AntipodalAffordance ): @@ -296,17 +313,18 @@ def _plan( "PickUp requires an AntipodalAffordance when grasp_xpos is not set.", ValueError, ) - if sem.entity is None: - logger.log_error( - "PickUp requires an entity on the target semantics.", ValueError - ) start_arm_qpos = arm_qpos_from_state( state, list(manipulator.joint_ids), ) if target.grasp_xpos is None: is_success, grasp_xpos = self._resolve_grasp_pose( - sem, start_arm_qpos, manipulator, options, approach_direction + sem, + object_pose, + start_arm_qpos, + manipulator, + options, + approach_direction, ) else: grasp_xpos = resolve_pose_target( @@ -316,7 +334,9 @@ def _plan( ) if options.rotate_upright is not None: grasp_xpos = self._upright_adjusted_grasp_poses( - sem, grasp_xpos, options + grasp_xpos, + object_pose, + options, ) is_success = torch.ones(self.n_envs, dtype=torch.bool, device=self.device) grasp_success = normalize_success_mask( @@ -350,8 +370,7 @@ def _plan( name="Pick-up trajectory success", ) - obj_poses = sem.entity.get_local_pose(to_matrix=True) - object_to_eef = torch.bmm(pose_inv(obj_poses), grasp_xpos) + object_to_eef = torch.bmm(pose_inv(object_pose), grasp_xpos) held = HeldObjectState( semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos ) @@ -373,18 +392,18 @@ def _plan( def _resolve_grasp_pose( self, semantics: ObjectSemantics, + object_pose: torch.Tensor, start_qpos: torch.Tensor, manipulator: ResolvedControlPart, options: PickUpOptions, approach_direction: torch.Tensor, ) -> tuple[torch.Tensor, torch.Tensor]: - obj_poses = semantics.entity.get_local_pose(to_matrix=True) grasp_poses_result = semantics.affordance.get_valid_grasp_poses( - obj_poses=obj_poses, + obj_poses=object_pose, approach_direction=approach_direction, object_part=options.pick_object_part, ) - n_envs = obj_poses.shape[0] + n_envs = object_pose.shape[0] n_max_pose = max(r[0].shape[0] for r in grasp_poses_result) grasp_xpos_padding = torch.zeros( (n_envs, n_max_pose, 4, 4), dtype=torch.float32, device=self.device @@ -408,10 +427,9 @@ def _resolve_grasp_pose( grasp_xpos_padding[i, n_pose:] = grasp_poses[0] grasp_cost_padding[i, n_pose:] = grasp_costs[0] grasp_xpos_padding, ik_success = self._select_feasible_grasp_variants( - semantics, grasp_xpos_padding, start_qpos, - obj_poses, + object_pose, manipulator, options, approach_direction, @@ -426,7 +444,6 @@ def _resolve_grasp_pose( def _select_feasible_grasp_variants( self, - semantics: ObjectSemantics, grasp_xpos: torch.Tensor, start_qpos: torch.Tensor, object_poses: torch.Tensor, @@ -441,7 +458,9 @@ def _select_feasible_grasp_variants( mirrored_grasp_xpos[..., :3, 1] = -mirrored_grasp_xpos[..., :3, 1] selection_variants = torch.stack([grasp_xpos, mirrored_grasp_xpos], dim=2) grasp_variants = self._upright_adjusted_grasp_poses( - semantics, selection_variants, options + selection_variants, + object_poses, + options, ) pre_grasp_variants = grasp_variants.clone() @@ -576,8 +595,8 @@ def _compute_batch_candidate_ik( def _upright_adjusted_grasp_poses( self, - semantics: ObjectSemantics, grasp_xpos: torch.Tensor, + object_pose: torch.Tensor, options: PickUpOptions, ) -> torch.Tensor: """Return grasp poses after the optional upright-in-place roll adjustment.""" @@ -592,8 +611,7 @@ def _upright_adjusted_grasp_poses( upright_direction = options.obj_upright_direction.to( device=self.device, dtype=torch.float32 ) - obj_pose = semantics.entity.get_local_pose(to_matrix=True) - obj_upright = torch.matmul(obj_pose[:, :3, :3], upright_direction) + obj_upright = torch.matmul(object_pose[:, :3, :3], upright_direction) adjusted_grasp_xpos = grasp_xpos.clone() grasp_ry = adjusted_grasp_xpos[..., :3, 1] object_axes = obj_upright.reshape( diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index e809c0a2a..7218a5d27 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -18,6 +18,7 @@ from __future__ import annotations +import warnings from dataclasses import dataclass from typing import ClassVar, Literal @@ -31,7 +32,12 @@ from ..control import GRASP_COMMAND, OPEN_COMMAND from ..core import AtomicAction from ..effects import StateDelta -from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal +from ..goals import ( + PoseGoalValue, + SceneEntityPose, + resolve_pose_goal, + validate_pose_goal, +) from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..state import PlanningContext @@ -79,11 +85,12 @@ def __post_init__(self) -> None: class AssembleGoal: """Place a held assemble object onto a base object at a relative pose. - The base object pose is read at planning time from - :attr:`AssembleAffordance.base_object_entity`, and the assemble object's - target pose is ``base_pose @ assemble_to_base_pose``. The held-object - transform (``object_to_eef``) is read from :class:`PlanningContext` - for the place control part, which a prior :class:`PickUp` populates. + The preferred base object pose is a late-bound :class:`SceneEntityPose`. + Omitting it temporarily falls back to + :attr:`AssembleAffordance.base_object_entity`. The assemble object's target + pose is ``base_pose @ assemble_to_base_pose``. The held-object transform + (``object_to_eef``) is read from :class:`PlanningContext` for the place + control part, which a prior :class:`PickUp` populates. """ goal_kind: ClassVar[str] = "assemble" @@ -91,6 +98,18 @@ class AssembleGoal: affordance: AssembleAffordance """Assembly affordance anchoring the assemble object to the base object.""" + base_pose: SceneEntityPose | None = None + """Late-bound base-object pose used for snapshot-consistent planning.""" + + def __post_init__(self) -> None: + if not isinstance(self.affordance, AssembleAffordance): + raise TypeError("affordance must be an AssembleAffordance instance.") + if self.base_pose is not None and not isinstance( + self.base_pose, + SceneEntityPose, + ): + raise TypeError("base_pose must be a SceneEntityPose or None.") + @dataclass(frozen=True, slots=True, eq=False) class PlaceOptions(ActionOptions): @@ -129,9 +148,10 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): joint positions are inherited from :class:`PlanningContext`. An :class:`AssembleGoal` replaces the explicit EEF pose with an assembly - affordance: the place pose is derived from the base object's current pose - and ``assemble_to_base_pose``, converted to an EEF pose through the held - object's ``object_to_eef`` (read from :class:`PlanningContext`). + affordance: the place pose is derived from the base object's snapshot pose + (or deprecated live fallback) and ``assemble_to_base_pose``, converted to an + EEF pose through the held object's ``object_to_eef`` (read from + :class:`PlanningContext`). """ skill_id: ClassVar[str] = "place" @@ -154,6 +174,17 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + request: ResolvedActionRequest[PlaceGoal | AssembleGoal, PlaceOptions], + ) -> tuple[str, ...]: + """Include an explicitly snapshot-grounded assembly base.""" + dependencies = set(super()._scene_dependencies(request)) + target = request.goal + if isinstance(target, AssembleGoal) and target.base_pose is not None: + dependencies.add(target.base_pose.entity_id) + return tuple(sorted(dependencies)) + def _plan( self, request: ResolvedActionRequest[PlaceGoal | AssembleGoal, PlaceOptions], @@ -325,7 +356,7 @@ def _resolve_assemble_place_xpos( Place EEF poses with shape ``(n_envs, 4, 4)``. Raises: - ValueError: If no held object or no base object entity is available. + ValueError: If no held object or base-pose source is available. """ held = state.get_held_object(control_part) if held is None: @@ -335,15 +366,37 @@ def _resolve_assemble_place_xpos( ValueError, ) affordance = target.affordance - if affordance.base_object_entity is None: - logger.log_error( - "AssembleAffordance.base_object_entity must be set to assemble " - "onto a base object.", - ValueError, + if target.base_pose is not None: + base_pose = resolve_object_target( + resolve_pose_goal( + target.base_pose, + state, + name="base_pose", + ), + n_envs=self.n_envs, + device=self.device, + name="base_pose", + ) + else: + if affordance.base_object_entity is None: + logger.log_error( + "AssembleGoal requires base_pose or " + "AssembleAffordance.base_object_entity.", + ValueError, + ) + warnings.warn( + "AssembleGoal without base_pose reads " + "AssembleAffordance.base_object_entity live; provide " + "base_pose=SceneEntityPose(...) instead.", + DeprecationWarning, + stacklevel=3, + ) + base_pose = resolve_object_target( + affordance.base_object_entity.get_local_pose(to_matrix=True), + n_envs=self.n_envs, + device=self.device, + name="legacy_base_pose", ) - base_pose = affordance.base_object_entity.get_local_pose(to_matrix=True).to( - device=self.device, dtype=torch.float32 - ) assemble_object_pose = affordance.get_assemble_object_pose(base_pose) object_to_eef = resolve_object_target( held.object_to_eef, diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index a9053453d..a34efec07 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -275,6 +275,7 @@ def main() -> None: geometry={}, label="cube", entity=target, + entity_id=TARGET_ENTITY_ID, ) binding = ActionBinding( manipulators={"primary": "arm"}, diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 06703fae5..46384558b 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -29,6 +29,7 @@ ActionInvocation, Affordance, AntipodalAffordance, + AssembleAffordance, AssembleGoal, AtomicAction, AtomicActionEngine, @@ -77,6 +78,7 @@ PlanOptions, PlanResult, ) +from embodichain.utils.math import pose_inv NUM_ENVS = 2 ARM_DOF = 6 @@ -269,7 +271,7 @@ def _invocation( ) -def _semantics() -> ObjectSemantics: +def _semantics(*, entity_id: str | None = None) -> ObjectSemantics: entity = Mock() entity.get_local_pose.return_value = torch.eye(4).repeat(NUM_ENVS, 1, 1) return ObjectSemantics( @@ -277,6 +279,7 @@ def _semantics() -> ObjectSemantics: geometry={}, label="test_object", entity=entity, + entity_id=entity_id, ) @@ -362,12 +365,16 @@ def compute_fk( return generator -def _dual_context(task: TaskState | None = None) -> PlanningContext: +def _dual_context( + task: TaskState | None = None, + *, + scene: SceneSnapshot | None = None, +) -> PlanningContext: qpos = torch.zeros(NUM_ENVS, DUAL_ROBOT_DOF) return PlanningContext( robot=RobotObservation(0.0, qpos, torch.zeros_like(qpos)), task=task or TaskState.empty(NUM_ENVS, "cpu"), - scene=SceneSnapshot.empty(), + scene=SceneSnapshot.empty() if scene is None else scene, env_ids=torch.arange(NUM_ENVS), ) @@ -513,8 +520,9 @@ def test_move_joints_uses_binding_and_preserves_uncontrolled_joints() -> None: def test_pick_and_place_declare_effects_without_mutating_context() -> None: generator = _motion_generator() pick = _bind_action(generator, PickUp()) - initial = _context() - semantics = _semantics() + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + initial = _context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) + semantics = _semantics(entity_id="target") grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) pick_plan = _plan_action( @@ -559,15 +567,43 @@ def test_move_held_object_requires_projected_attachment() -> None: with pytest.raises(ValueError, match="requires an object held"): _plan_action(action, invocation, _context()) - held = _held() + semantics = _semantics() + held = _held(semantics) + held.object_to_eef[:, :3, :3] = torch.tensor( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]] + ) + held.object_to_eef[:, 0, 3] = torch.tensor([0.1, 0.2]) task = TaskState( batch_size=NUM_ENVS, device="cpu", held_objects={"arm": held}, ) - plan = _plan_action(action, invocation, _context(task)) + eef_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + eef_pose[:, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + eef_pose[:, 0, 3] = torch.tensor([0.5, 0.8]) + generator.robot.compute_fk.return_value = eef_pose + generator.robot.compute_fk.side_effect = None + action._apply_configured_upright_rotation = Mock() + configured_invocation = ActionInvocation( + skill_id="move_held_object", + goal=HeldObjectPoseGoal(torch.eye(4)), + binding=_binding(), + motion_policy=MotionPolicy(sample_count=10), + skill_options=MoveHeldObjectOptions(pick_rotate_upright=0.25), + ) + + plan = _plan_action(action, configured_invocation, _context(task)) + assert plan.plan_success.all() assert plan.expected_effects.is_empty + current_object_pose = action._apply_configured_upright_rotation.call_args.args[2] + assert torch.allclose( + current_object_pose, + torch.bmm(eef_pose, pose_inv(held.object_to_eef)), + ) + semantics.entity.get_local_pose.assert_not_called() def test_press_uses_invocation_sample_budget() -> None: @@ -711,32 +747,41 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: affordance = AntipodalAffordance() affordance.get_valid_grasp_poses = Mock() entity = Mock() - entity.get_local_pose.return_value = torch.eye(4).repeat(NUM_ENVS, 1, 1) + entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) semantics = ObjectSemantics( affordance=affordance, geometry={}, label="explicit-grasp-object", entity=entity, + entity_id="target", ) grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) grasp[:, 0, 3] = torch.tensor([0.1, 0.2]) action = _bind_action(generator, PickUp()) + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + object_pose[:, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + object_pose[:, 0, 3] = torch.tensor([0.03, 0.07]) + context = _context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) - plan = _plan_action( - action, + request = action.resolve_request( _invocation( "pick_up", GraspGoal(semantics=semantics, grasp_xpos=grasp), sample_count=20, - ), - _context(), + ) ) - projected = plan.expected_effects.apply(_context().task, plan.plan_success) + plan = action.plan(request, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) - affordance.get_valid_grasp_poses.assert_not_called() + request.goal.semantics.affordance.get_valid_grasp_poses.assert_not_called() + request.goal.semantics.entity.get_local_pose.assert_not_called() held = projected.get_held_object("arm") assert held is not None assert torch.allclose(held.grasp_xpos, grasp) + assert torch.allclose(held.object_to_eef, torch.bmm(pose_inv(object_pose), grasp)) + assert plan.scene_dependencies == ("target",) assert [segment.name for segment in plan.segments] == [ "approach", "close", @@ -754,6 +799,7 @@ def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: geometry={}, label="partially-graspable-object", entity=entity, + entity_id="target", ) action = _bind_action(generator, PickUp()) action._resolve_grasp_pose = Mock( @@ -762,7 +808,13 @@ def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: torch.eye(4).repeat(NUM_ENVS, 1, 1), ) ) - context = _context() + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) plan = _plan_action( action, @@ -795,6 +847,7 @@ def test_pick_resolves_late_bound_scene_grasp_and_declares_dependency() -> None: geometry={}, label="late-bound-grasp-object", entity=entity, + entity_id="target", ) action = _bind_action(generator, PickUp()) context = _context(scene=_target_scene(target_pose, timestamp=0.0, version=0)) @@ -838,6 +891,7 @@ def test_pick_session_replans_when_late_bound_target_moves() -> None: geometry={}, label="moving-grasp-object", entity=entity, + entity_id="target", ) engine = AtomicActionEngine( generator, @@ -882,16 +936,26 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: action = _bind_action(generator, PickUp()) invocation = ActionInvocation( skill_id="pick_up", - goal=GraspGoal(semantics=_semantics(), grasp_xpos=torch.eye(4)), + goal=GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), binding=ActionBinding( manipulators={"primary": "alternate_arm"}, end_effectors={"primary": "alternate_hand"}, ), motion_policy=MotionPolicy(sample_count=20), ) - context = _context() + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) - plan = _plan_action(action, invocation, context) + request = action.resolve_request(invocation) + 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 @@ -940,15 +1004,24 @@ def test_handover_does_not_mutate_cached_final_pose() -> None: ) assert handover_options.final_object_pose is not None original_final_pose = handover_options.final_object_pose.clone() - current_object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) - current_object_pose[:, :3, :3] = torch.diag(torch.tensor([-1.0, -1.0, 1.0])) - semantics = _semantics() - semantics.entity.get_local_pose.return_value = current_object_pose + semantics = _semantics(entity_id="handover_object") + held = _held(semantics) + held.object_to_eef[:, :3, :3] = torch.tensor( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]] + ) + held.object_to_eef[:, 0, 3] = torch.tensor([0.1, 0.2]) task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": _held(semantics)}, + held_objects={"left_arm": held}, ) + current_eef = torch.eye(4).repeat(NUM_ENVS, 1, 1) + current_eef[:, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + current_eef[:, 1, 3] = torch.tensor([0.3, 0.5]) + generator.robot.compute_fk.return_value = current_eef + generator.robot.compute_fk.side_effect = None receive_grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) action._resolve_receive_grasp = Mock( return_value=(receive_grasp, torch.ones(NUM_ENVS, dtype=torch.bool)) @@ -966,7 +1039,10 @@ def plan_from_start( action._plan_named_arm_trajectory = Mock(side_effect=plan_from_start) invocation = ActionInvocation( skill_id="hand_over", - goal=GraspGoal(semantics=semantics), + goal=GraspGoal( + semantics=semantics, + grasp_xpos=SceneEntityPose("unused_grasp_pose"), + ), binding=_dual_binding("source", "destination"), motion_policy=MotionPolicy(sample_count=30), ) @@ -974,7 +1050,18 @@ def plan_from_start( plan = _plan_action(action, invocation, _dual_context(task)) assert plan.plan_success.all() + assert plan.scene_dependencies == () + handover_object_pose = action._resolve_receive_grasp.call_args.args[1] + expected_current_object_pose = torch.bmm( + current_eef, + pose_inv(held.object_to_eef), + ) + assert torch.allclose( + handover_object_pose[:, :3, :3], + expected_current_object_pose[:, :3, :3], + ) 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", @@ -1007,7 +1094,7 @@ def fail_second_receiving_arm( return success, qpos generator.robot.compute_ik.side_effect = fail_second_receiving_arm - semantics = _semantics() + semantics = _semantics(entity_id="handover_object") task = TaskState( batch_size=NUM_ENVS, device="cpu", @@ -1039,7 +1126,8 @@ def fail_second_receiving_arm( motion_policy=MotionPolicy(sample_count=30), ) - plan = _plan_action(action, invocation, context) + request = action.resolve_request(invocation) + plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] @@ -1051,6 +1139,38 @@ def fail_second_receiving_arm( received = projected.get_held_object("right_arm") assert received is not None assert received.env_mask.tolist() == [True, False] + semantics.entity.get_local_pose.assert_not_called() + + +def test_handover_rejects_goal_for_a_different_held_object() -> 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), + ) + ), + ) + held_semantics = _semantics(entity_id="held_object") + goal_semantics = _semantics(entity_id="other_object") + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"left_arm": _held(held_semantics)}, + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=GraspGoal(semantics=goal_semantics), + binding=_dual_binding("source", "destination"), + ) + + with pytest.raises(ValueError, match="must identify the object held"): + _plan_action(action, invocation, _dual_context(task)) + + held_semantics.entity.get_local_pose.assert_not_called() + goal_semantics.entity.get_local_pose.assert_not_called() def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None: @@ -1067,8 +1187,14 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None ) affordance = AntipodalAffordance() _stub_dual_arm_grasp_poses(affordance) + entity = Mock() + entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) semantics = ObjectSemantics( - affordance=affordance, geometry={}, label="coordinated-object" + affordance=affordance, + geometry={}, + label="coordinated-object", + entity=entity, + entity_id="coordinated_object", ) invocation = ActionInvocation( skill_id="coordinated_pickment", @@ -1082,11 +1208,14 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None ) context = _dual_context() - plan = _plan_action(action, invocation, context) + request = action.resolve_request(invocation) + plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, True] assert plan.trajectory.positions.shape == (NUM_ENVS, 30, DUAL_ROBOT_DOF) + assert plan.scene_dependencies == () + request.goal.semantics.entity.get_local_pose.assert_not_called() assert projected.get_held_object("left_arm") is None assert projected.get_held_object("right_arm") is None assert isinstance( @@ -1102,6 +1231,129 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None ] +def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + object_motion_keyframes=3, + ), + ), + ) + affordance = AntipodalAffordance() + _stub_dual_arm_grasp_poses(affordance) + entity = Mock() + entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) + semantics = ObjectSemantics( + affordance=affordance, + geometry={}, + label="snapshot-coordinated-object", + entity=entity, + entity_id="target", + ) + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + object_pose[:, :3, :3] = torch.tensor( + [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]] + ) + object_pose[:, 1, 3] = torch.tensor([0.2, 0.4]) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=semantics, + object_target_pose=object_pose, + ), + binding=_dual_binding("left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + context = _dual_context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) + + request = action.resolve_request(invocation) + plan = action.plan(request, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) + + resolved_affordance = request.goal.semantics.affordance + sampled_pose = resolved_affordance.get_dual_arm_valid_grasp_poses.call_args.kwargs[ + "obj_poses" + ] + assert torch.equal(sampled_pose, object_pose) + assert plan.scene_dependencies == ("target",) + request.goal.semantics.entity.get_local_pose.assert_not_called() + held = projected.get_coordinated_held_object("left_arm", "right_arm") + assert held is not None + assert torch.allclose(held.left_object_to_eef, pose_inv(object_pose)) + assert torch.allclose(held.right_object_to_eef, pose_inv(object_pose)) + + +def test_assemble_place_uses_explicit_base_snapshot() -> None: + generator = _motion_generator() + action = _bind_action(generator, Place()) + base_entity = Mock() + base_entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) + relative_pose = torch.eye(4) + relative_pose[2, 3] = 0.05 + affordance = AssembleAffordance( + base_object_entity=base_entity, + assemble_to_base_pose=relative_pose, + ) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"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]) + context = _context( + task, + scene=SceneSnapshot( + timestamp=0.0, + version=0, + entities={"base": EntityState(base_pose)}, + ), + ) + + request = action.resolve_request( + _invocation( + "place", + AssembleGoal( + affordance=affordance, + base_pose=SceneEntityPose("base"), + ), + ) + ) + plan = action.plan(request, context) + + assert plan.plan_success.all() + assert plan.scene_dependencies == ("base",) + request.goal.affordance.base_object_entity.get_local_pose.assert_not_called() + + +def test_assemble_place_legacy_base_entity_warns() -> None: + generator = _motion_generator() + action = _bind_action(generator, Place()) + base_entity = Mock() + base_entity.get_local_pose.return_value = torch.eye(4) + affordance = AssembleAffordance(base_object_entity=base_entity) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"arm": _held()}, + ) + + request = action.resolve_request( + _invocation("place", AssembleGoal(affordance=affordance)) + ) + with pytest.warns(DeprecationWarning, match="base_pose"): + plan = action.plan(request, _context(task)) + + assert plan.scene_dependencies == () + request.goal.affordance.base_object_entity.get_local_pose.assert_called_once_with( + to_matrix=True + ) + + def test_coordinated_pick_holds_only_environment_with_ik_failure() -> None: generator = _dual_motion_generator() original_compute_ik = generator.robot.compute_ik.side_effect From 131c6f0cccc511625a25d38cd9c3e93842e05122 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 21:17:09 +0800 Subject: [PATCH 05/28] docs(atomic-actions): define snapshot bridge rollout --- agent_context/MAP.yaml | 14 + .../topics/atomic-actions/atomic-actions.md | 95 +++++- .../design/declarative_expert_program_plan.md | 319 ++++++++++++++---- .../sim/atomic_actions/builtin_actions.md | 103 ++++-- .../overview/sim/atomic_actions/index.md | 16 +- .../overview/sim/planners/curobo_planner.md | 6 + docs/source/tutorial/atomic_actions.rst | 18 +- 7 files changed, 462 insertions(+), 109 deletions(-) diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 60a5145e0..b236d210c 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -427,6 +427,8 @@ topics: - atomic actions - motion primitive - action primitive + - object semantics + - scene grounding - AtomicAction - ActionInvocation - AtomicActionEngine @@ -451,6 +453,16 @@ topics: - SceneSnapshotSupplier - SceneProvider - RigidObjectSceneProvider + - ObjectSemantics + - entity_id + - frozen ObjectSemantics + - legacy uid + - stable entity identity + - snapshot grounding + - AssembleGoal + - AssembleAffordance + - base_pose + - _scene_dependencies - collision world revision - dynamic obstacle - StateDelta @@ -485,6 +497,8 @@ topics: source_of_truth: - embodichain/lab/sim/atomic_actions/core.py - embodichain/lab/sim/atomic_actions/goals.py + - embodichain/lab/sim/atomic_actions/effects.py + - embodichain/lab/sim/atomic_actions/affordance.py - embodichain/lab/sim/atomic_actions/bindings.py - embodichain/lab/sim/atomic_actions/control.py - embodichain/lab/sim/atomic_actions/invocation.py diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 1c692b475..4213c7477 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -71,6 +71,76 @@ to the skill-specific `_plan()` hook. New actions implement `_plan()` and must not override `plan()`. `engine.plan_action(...)` is only an extension/testing escape hatch for an unregistered instance. +The `_plan()` extension boundary is an intentional hard break with no legacy +adapter. A subclass that defines `plan()` raises `TypeError` at class definition; +migrate an older custom action by renaming that implementation to `_plan()`. + +## Object identity and pose grounding + +`ObjectSemantics.entity_id` is the canonical pre-registry snapshot key. It is +optional for direct-core compatibility but, when supplied, must be a non-empty +string. Pose grounding with an explicit ID is strict: resolve it only from the +current `PlanningContext.scene`; a missing snapshot entry is an error and never +falls back to the live `entity`. Only when no ID is supplied may the core read +`ObjectSemantics.entity`; that path emits `DeprecationWarning`, reads live state, +and cannot declare a scene-motion dependency. + +`ObjectSemantics` is shallow-frozen. Top-level fields such as `entity_id`, +`entity`, and `label` cannot be rebound after construction; create a new +semantics value to change identity. Nested affordance and metadata objects may +remain mutable, but they never establish identity. + +`SceneSnapshot` owns copies of its input poses, but `EntityState.pose` tensors +are not deeply read-only. Callers must treat published snapshot values as +immutable and publish a newer scene version for changes. Enforced deep +immutability is deferred to the SceneRegistry/snapshot hardening phase. + +Stable object identity follows these exact rules: + +1. The same `ObjectSemantics` instance is identical to itself. +2. If either side has an explicit `entity_id`, both sides must have an explicit + ID and the strings must match. Never compare an explicit ID directly with a + legacy UID, even when the spellings are equal. +3. Only when both explicit IDs are absent, compare non-empty legacy + `entity.uid` values. If either side has a valid UID, both must have one and + the strings must match. +4. Only when neither side has an explicit ID or valid UID may identity fall back + to the same live entity handle. `label` is descriptive and never establishes + identity. + +This is a snapshot/identity bridge, not alias resolution. A future +`SceneRegistry` owns uniqueness, aliases, normalization, and authoritative +registry IDs. Partial-batch `StateDelta` attachment merges use the same stable +identity rules, so equivalent semantic wrappers update one held object instead +of creating label-based duplicates. + +For both individual and coordinated attachments, a same-identity partial merge +preserves scalar metadata: if any previously active environment row remains, +the merged relation keeps `previous.semantics` and selects only the per-row +mask, transforms, and grasp poses from previous/candidate values. It adopts +`candidate.semantics` only when no previously active row survives the update. +This prevents an update for some environments from silently replacing the +semantic metadata shared by untouched rows. + +Scene dependencies must match the poses each primitive actually consumes: + +| Primitive | Scene dependencies | +|---|---| +| `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`. | +| `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. | +| `Press` | A `SceneEntityPose` in `xpos`. | +| `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. | + +`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. + ## Static compilation Built-ins are already registered by their class-level stable `skill_id`; call: @@ -287,7 +357,17 @@ tutorial may derive a simple profile from limits explicitly. `GraspGoal.grasp_xpos` accepts an explicit pose tensor, a late-bound `SceneEntityPose`, or `None` for affordance sampling. A `SceneEntityPose` registers the referenced entity as a recovery dependency, allowing an executing -`PickUp` to replan when the grasp target moves. +`PickUp` to replan when the grasp target moves. `PickUp` also resolves its +semantic object's pose once per planning attempt and declares the semantic +`entity_id` because grasp sampling, upright adjustment, and the held +`object_to_eef` relation all consume that same pose. + +`AssembleGoal.base_pose=SceneEntityPose(...)` is the canonical assembly anchor +and becomes a recovery dependency. An omitted `base_pose` permits the deprecated +live `AssembleAffordance.base_object_entity` fallback for direct-core callers +only; it is not dependency-tracked. The current `assemble.py` tutorial exercises +that legacy fallback, while `moving_target_recovery.py` is the canonical +snapshot-grounded object example. ## Extension rules @@ -297,13 +377,18 @@ registers the referenced entity as a recovery dependency, allowing an executing 4. Implement `_plan()`; do not override the framework-owned `plan()` method. 5. Validate with `require_goal(request)` and consume only the resolved binding. 6. Plan from `context.robot.qpos`; never read an implicit live start state. -7. Return full-robot positions or a `TimedTrajectory` through `build_plan()`. +7. If planning consumes a semantic object's snapshot pose, override + `_scene_dependencies()`, preserve `super()` dependencies, and add exactly + that semantic ID. +8. Return full-robot positions or a `TimedTrajectory` through `build_plan()`. Build batched `list[PlanState]`, translate the policy with `request.motion_policy.to_motion_gen_options()`, and call `self.motion_generator.generate()`. Import pure operations directly from `trajectory_ops.py`. -8. Declare symbolic changes with `StateDelta`; do not mutate context or commit - physical effects during planning. -9. Keep scene stepping, controller I/O, and task-graph/MLLM logic outside the +9. Declare symbolic changes with `StateDelta`; do not mutate context or commit + physical effects during planning. For partial attachment updates, retain + previous scalar semantics while any previous row remains; merge only batched + masks/transforms and adopt candidate semantics only on full replacement. +10. Keep scene stepping, controller I/O, and task-graph/MLLM logic outside the atomic action. Put execution-loop I/O behind the runner protocols rather than calling a simulator or device from `plan()` or `ExecutionSession`. diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 1a2999d58..21510e7b4 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -107,17 +107,22 @@ than uncommitted working-tree changes. | Action Bank | Configuration plus task-specific Python node/edge functions | Keep only as a compatibility path while semantic coverage is built. | PR #475 resolved cumulative translation/rotation publication, removed the dead -`MotionPolicy.interpolation` field, and unified strategy dispatch. The -remaining #474 prerequisites on this baseline are: +`MotionPolicy.interpolation` field, and unified strategy dispatch. It also made +`AtomicAction.plan()` framework-owned and `_plan()` the only custom-action +extension hook. Rejecting a subclass that overrides `plan()` is an intentional +hard break: the project will not provide a compatibility adapter or deprecation +window for that former extension contract. Custom actions must migrate to +`_plan()` so framework-owned scene binding cannot be bypassed. + +The remaining #474 prerequisites on this baseline are: -- `AtomicAction` rejects the formerly documented `plan()` extension override - and requires `_plan()` without a compatibility window. - scene pose, semantics, affordance, and collision registration still have multiple sources of truth; - ordinary callers still see a large low-level public surface and must perform semantic transform and verifier plumbing; -- provider collision entity IDs and planner-declared dynamic obstacle names are - not cross-validated at integration construction time; +- dynamic-obstacle validation is planner-local; provider collision entity IDs + and planner-declared names are not yet fully cross-validated at integration + construction time; - `MotionPolicy` still exposes implementation-level tuning that should be hidden behind semantic presets for ordinary users. @@ -138,7 +143,9 @@ The following #471 decisions remain valid: - stable named trajectory segments for tracing instead of recomputed trajectory indices; - sequential execution first, then resource-aware parallel execution; -- continued legacy compatibility during migration. +- continued Action Bank compatibility and only the explicitly documented + direct-core fallbacks during migration. This does not include the intentional + `plan()` to `_plan()` hard break. The following parts must be adjusted: @@ -272,18 +279,62 @@ SceneEntityRef Rules: -1. An entity is registered once. Planner obstacles, scene dependencies, effect - monitors, and semantic calls consume that registration. -2. Grounding reads pose and geometry from one immutable snapshot. It must not +1. The registry ID is the authoritative entity identity used by semantic calls, + snapshots, scene dependencies, effect monitors, and planner obstacles. An + entity is registered once under that ID. +2. A simulation object's existing `uid` may be imported as a legacy alias only. + Aliases are resolved once at an integration boundary and normalized to the + registry ID; they never replace the authoritative ID. Duplicate registry IDs, + ambiguous aliases, or an alias colliding with another registry ID fail during + registry construction. +3. Grounding reads pose and geometry from one immutable snapshot. It must not mix a snapshot with a live simulation entity pose. -3. Automatic grasp selection declares a target dependency automatically. -4. Dynamic collision setup is derived and cross-validated at construction - time. The `safe` preset requests `DynamicCollisionMode.REQUIRED` when the - registry declares dynamic collision entities and fails early if the active - planner cannot satisfy it. -5. Environment scene configuration should populate the registry automatically; +4. Automatic grasp selection declares a target dependency automatically. +5. Dynamic collision setup is derived from authoritative registry IDs. Registry + construction performs the complete provider/planner cross-validation: the + registry's dynamic-collision ID set, the provider's `collision_entity_ids`, + and the planner's dynamic-obstacle names must agree after alias normalization; + every ID must have the required geometry, and the selected planner must + support the declared update mode. The current + planner-local name check remains a lower-level defensive validation, not the + integration contract. +6. The `safe` preset requests `DynamicCollisionMode.REQUIRED` when the registry + declares dynamic collision entities and fails early if the active planner + cannot satisfy it. +7. Environment scene configuration should populate the registry automatically; explicit providers are reserved for perception and hardware integration. +Before PR2A introduces this registry, PR1 provides only a core migration bridge. +`ObjectSemantics.entity_id` is a caller-supplied `SceneSnapshot` key, not yet a +registry reference. `ObjectSemantics` is shallow-frozen so top-level fields, +including `entity_id`, cannot be rebound after attachment state captures the +semantics; identity changes require a new instance. Nested affordance and +metadata objects remain mutable but never establish identity. + +For object identity, explicit and legacy namespaces stay separate. If either +side supplies `entity_id`, both sides must supply the same explicit ID; a +same-spelled simulation `entity.uid` is not sufficient. Only when both explicit +IDs are absent may the bridge compare non-empty legacy UIDs, requiring both UIDs +to exist and match. Only when neither side has an explicit ID or valid UID may +comparison fall back to the same semantic object or live entity handle. +Semantic labels are never identity. Arbitrary alias mapping, uniqueness +enforcement, and normalization to an authoritative registry ID belong to PR2A. + +For pose grounding, an explicit `entity_id` is strict: the pose comes only from +the current versioned `PlanningContext.scene`, and a missing entry is an error. +The planner never falls back to a live entity after an explicit ID fails. A live +`ObjectSemantics.entity` read remains temporarily available, with a deprecation +warning and without a scene dependency, only when no `entity_id` was supplied. +The same boundary applies to `AssembleGoal.base_pose`: the snapshot reference is +canonical, while an omitted reference permits the deprecated direct-core +`AssembleAffordance.base_object_entity` path. + +The current `SceneSnapshot` owns copies of input pose tensors, but exposed +`EntityState.pose` tensors are not deeply read-only. PR1 therefore requires +callers to treat snapshot values as immutable and uses the scene version for +publication/recovery semantics. Enforced deep immutability belongs to the PR2A +registry/snapshot hardening rather than this bridge. + ### 7.2 Robot skill profiles A `RobotSkillProfile` is reusable per embodiment and contains: @@ -314,6 +365,14 @@ EEF pose from the requested object-space target and the verified `object_to_eef` relation. Task code and configuration never perform `desired_object_pose @ object_to_eef`. +As the core migration path for assembly, `AssembleGoal` gains +`base_pose: SceneEntityPose | None`. The semantic compiler always supplies a +`SceneEntityPose` containing the authoritative base-object registry ID, so the +base pose is resolved from the same immutable snapshot and automatically becomes +a scene dependency. `None` preserves the existing live +`AssembleAffordance.base_object_entity` lookup only for legacy direct-core +callers; the semantic facade and Expert Program never emit that fallback. + The workflow compiler inspects later calls and propagates downstream object targets to pickup/grasp selection. The caller does not repeat later goals in `PickUpOptions`. @@ -326,7 +385,8 @@ Compilation has two stages. - validate references, presets, capabilities, resources, and bounded loops; - infer ordering and data/effect dependencies; - propagate downstream object goals for grasp selection; - - identify static stages versus observation-dependent boundaries; + - identify every call boundary that requires fresh observation or verified + effects, without coalescing calls in Version 1; - reject ambiguous bindings and unsupported semantic relations before execution. 2. **Runtime grounding and lowering** @@ -336,11 +396,18 @@ Compilation has two stages. - lower to a typed `ActionInvocation`; - dispatch through the canonical `SkillRuntime`. -Static `engine.compile()` is valid only when later goals do not depend on -observations or effects produced by earlier calls. `engine.start()` and observed -execution are required for grasp/release verification, moving targets, -recovery, post-settling, or any JIT-grounded goal. The default mode is `auto`: -the compiler partitions safe static stages and inserts observed boundaries. +Version 1 executes exactly one semantic call per `ExecutionSession`. The runtime +captures a fresh registry snapshot, lowers one call to one `ActionInvocation`, +constructs a one-invocation session, drives it through terminal effect +verification, commits the verified per-environment task state, and only then +advances to the next call. It never places multiple semantic calls in one +`ExecutionSession`. + +Static `engine.compile()` remains an advanced core API for explicitly +observation-independent offline planning. The Version 1 semantic runtime does +not coalesce calls into static stages; such an optimization requires a later +design proving that it preserves the call, effect, and re-observation +boundaries. ### 7.5 Skill runtime @@ -350,6 +417,7 @@ the compiler partitions safe static stages and inserts observed boundaries. - synchronous `run(...)` and non-blocking `step()` entry points; - planning-context refresh through registered observation ports; - JIT lowering of the next semantic call; +- exactly one semantic call and one invocation per `ExecutionSession`; - persistent, per-environment verified `TaskState`; - built-in effect-monitor selection and feedback to `ExecutionSession`; - uniform `SkillResult`, cancellation, timeout, and safe-stop behavior; @@ -531,12 +599,24 @@ deadlines. ### 9.3 Named atomic trajectory segments -Plans need stable semantic trajectory-segment names. Current built-ins expose: +Version 1 freezes the trajectory-segment names already emitted by current +built-ins. A successful non-empty plan exposes the following ordered names; +zero-length optional segments are omitted: -- pick: `approach`, `close`, `lift`; -- place: `approach`, `release`, `retract`; -- handover: `transfer`, `approach`, `close`, optional `hold`, `release`, and - `deliver`. +| Atomic skill ID | Ordered trajectory-segment names | +|---|---| +| `move_joints` | `move_joints` | +| `move_end_effector` | `move_end_effector` | +| `move_held_object` | `transport` | +| `pick_up` | `approach`, `close`, `lift` | +| `place` (including `AssembleGoal`) | `approach`, `release`, `retract` | +| `press` | `close`, `press`, `retract` | +| `hand_over` | `transfer`, `approach`, `close`, optional `hold`, `release`, `deliver` | +| `coordinated_pickment` | `approach`, `close`, `lift`, `move`, optional `hold` | +| `coordinated_placement` | `approach`, optional `hold`, optional `release`, `retreat` | + +These spellings are a trace/metadata contract. Renaming or removing one requires +an explicit API review and migration rather than a silent change in a primitive. Names are validated by `ActionPlan`; ranges may change after replanning when a backend returns a different sample count. Effect monitors run at the action @@ -574,9 +654,16 @@ All runtime state is indexed by stable environment IDs: - post-policy progress and segment validation; - result and metadata. -One environment may finish, recover, settle, or fail without blocking or -overwriting another. Program structure is shared, but runtime progress is -masked per environment. +Version 1 uses a shared program/call barrier for the environment batch; it does +not maintain a divergent AST program counter or a separate `ExecutionSession` +per environment. The runtime advances to the next semantic call or program +segment only when every still-eligible active row reaches the current boundary. +A slower or recovering active row therefore keeps the batch at that boundary. + +Within the shared barrier, task state, effects, recovery budgets, eligibility, +success, and failure remain independent per environment. Completed, failed, or +otherwise inactive rows emit hold behavior and cannot overwrite another row's +state while the active cohort catches up. ## 10. Action Bank migration @@ -600,7 +687,10 @@ Migration rules: working during the transition. 2. Add `EmbodiedEnvCfg.expert_program` and a CLI input such as `--expert_program`; reject simultaneous legacy and new program inputs. -3. Migrate sequential tasks first and compare generated metadata and outcomes. +3. Do not require official-task migration in PR1. Start opt-in sequential-task + migration with the repeated-cube vertical slice after the registry, compiler, + runtime, and demo bridge contracts are available, then compare generated + metadata and outcomes. 4. Add `Parallel` only with deterministic resource conflict checks, trajectory alignment, synchronization barriers, and per-environment `StateDelta` merging. @@ -642,12 +732,16 @@ Each item below should remain a focused PR with its own public-API review and tests. The dependency order is: ```text -Phase 0 correctness +Phase 0 correctness (complete) | v -SceneRegistry + RobotSkillProfile +PR1 snapshot/identity bridge | - v + +-----------------------+ + v v +PR2A SceneRegistry PR2B RobotSkillProfile + +-----------+-----------+ + v Semantic calls/compiler --> SkillRuntime/effect monitors | | +---------------+--------------+ @@ -665,7 +759,7 @@ Semantic calls/compiler --> SkillRuntime/effect monitors Action Bank deprecation ``` -### Phase 0: correctness and compatibility prerequisites +### Phase 0: correctness and core-contract decisions (complete) Landed on `main` through #475: @@ -676,34 +770,106 @@ Landed on `main` through #475: - the dead `MotionPolicy.interpolation` field is removed and strategy dispatch is unified; - one action owns one trajectory and one recovery/effect boundary, while named - `TrajectorySegment`s remain metadata. - -Remaining gates: + `TrajectorySegment`s remain metadata; +- `_plan()` is the only supported custom-action extension hook. The immediate + class-definition failure for a legacy `plan()` override is a documented, + tested, intentional hard break with no compatibility adapter; +- planner-local dynamic-obstacle name validation remains in place as a defensive + core check. Complete provider/planner cross-validation is deliberately owned + by the authoritative `SceneRegistry` integration in Phase 1. + +Exit criteria are met on `main@e445133c`. Implementation may proceed to the +focused PR1 bridge without adding a legacy `plan()` adapter or a pre-registry +duplicate of the integration-level obstacle validator. + +### PR1: core snapshot and identity bridge + +PR1 is deliberately smaller than Phase 1. It establishes the core seams that +the later registry and profile integrations consume: + +- add optional, validated `ObjectSemantics.entity_id` as the stable + `SceneSnapshot` key for canonical object grounding; +- resolve explicit IDs only from `PlanningContext.scene`, with a hard error and + no live fallback when the snapshot entry is missing; +- keep `ObjectSemantics.entity` only as a deprecated no-ID compatibility path; +- shallow-freeze `ObjectSemantics` fields so captured `entity_id` values cannot + be rebound without constructing a new semantic value; +- define stable held-object identity and partial-batch `StateDelta` merging: + if either side has an explicit `entity_id`, both explicit IDs must exist and + match; only two explicit-ID-less values may compare matching legacy + `entity.uid` strings, and only values with neither ID form may fall back to the + same semantic object or live handle; +- preserve scalar semantics during same-identity partial `StateDelta` merges: + while any previously active row remains, retain `previous.semantics` and + merge only per-environment masks, transforms, and grasp poses; adopt + `candidate.semantics` only when all previously active rows are replaced; +- add an action-owned scene-dependency hook. `PickUp` declares its semantic + object ID, coordinated pickup declares it only for the implicit initial-pose + path, and goal-owned `SceneEntityPose` values remain automatic dependencies; +- resolve each pickup object pose once per planning attempt and reuse that + tensor for grasp sampling, upright adjustment, and `object_to_eef`; +- derive held-object pose for `MoveHeldObject` and `HandOver` from the observed + EEF pose and verified `object_to_eef` instead of a live entity read; +- add `AssembleGoal.base_pose: SceneEntityPose | None`; the explicit reference + is snapshot-backed and dependency-tracked, while `None` retains the deprecated + `AssembleAffordance.base_object_entity` fallback; +- add focused tests, documentation, and one canonical snapshot-grounded moving + target tutorial. Keep `scripts/tutorials/atomic_action/assemble.py` explicitly + documented as a legacy fallback example until its later registry migration. + +PR1 does not add a `SceneRegistry`, a `SceneEntityRef` hierarchy, alias maps, +cross-source uniqueness or collision validation, a `RobotSkillProfile`, or +semantic presets. It does not require official task environments to migrate; +they remain on the compatibility path until a later opt-in vertical slice. + +Exit criteria: canonical object grounding never mixes snapshot and live poses; +explicit missing IDs fail; dependency metadata matches the poses actually +consumed; stable-identity merges are deterministic; and existing direct-core +callers remain usable only through the documented deprecated fallbacks. -- retain `_plan()` as the new extension hook and decide whether legacy - subclasses overriding `plan()` receive a tested compatibility/deprecation - adapter or continue to fail at class-definition time; -- cross-validate provider collision entity IDs against planner-declared dynamic - obstacle names when both integrations are constructed. Phase 1 extends this - same validation to registry-derived configuration. +### Phase 1: unified integration data -Exit criteria: both remaining gates pass on `main`. Phase 1 must not depend on -an undocumented custom-action break or defer mismatched obstacle names until -planning/execution. +Phase 1 is implemented as two focused follow-up PRs that join before the +semantic facade/compiler work. -### Phase 1: unified integration data +#### PR2A: SceneRegistry Deliverables: - `SceneEntityRef` hierarchy and `SceneRegistry`; -- immutable snapshot as the only grounding pose authority; -- environment-to-registry population and collision/provider derivation; -- `RobotSkillProfile`, capability-based binding, semantic tool commands, and - stable presets; +- authoritative registry IDs with simulation `uid` values accepted only as + normalized legacy aliases; +- immutable snapshots as the only grounding pose authority for the canonical + semantic/compiler path; +- opt-in environment-to-registry population and collision/provider derivation; +- complete construction-time agreement checks across registry collision IDs, + provider collision IDs, planner dynamic-obstacle names, geometry, and planner + capability; - explicit catalog-discovery versus engine-installation terminology. -Exit criteria: an object is registered once and a dynamic-object configuration -error fails before execution with an entity-centric diagnostic. +`ObjectSemantics.entity_id` and `AssembleGoal.base_pose` already provide the +lowering targets from PR1. PR2A replaces manually coordinated IDs/providers with +one authoritative registration and performs alias normalization exactly once at +the integration boundary. + +#### PR2B: RobotSkillProfile + +Deliverables: + +- `RobotSkillProfile` and reusable capability declarations; +- capability-based deterministic binding and explicit ambiguity errors; +- semantic tool commands and stable runtime/planning presets; +- profile validation against installed engine skills and robot control parts. + +PR2B may proceed in parallel with PR2A after the PR1 bridge. Neither follow-up +requires official task migration; the repeated-cube vertical slice opts in only +after the registry, profile, compiler, runtime, and demo bridge are available. + +Combined Phase 1 exit criteria: an object is registered once under an +authoritative ID, aliases cannot introduce ambiguity, dynamic-object +configuration mismatches fail before execution with an entity-centric +diagnostic, and robot capabilities resolve bindings/presets without task-owned +motion code. ### Phase 2: semantic facade and compiler @@ -726,9 +892,12 @@ effect verifier. Deliverables: - `SkillRuntime` wrapping `ExecutionRunner` for sync and step-wise use; +- exactly one semantic call lowered to one invocation in one + `ExecutionSession`; - built-in simulation effect monitors for grasp, release, and handover; - uniform per-environment `SkillResult` and persistent verified `TaskState`; -- automatic static/observed stage selection; +- a shared Version 1 program/call barrier with independent per-environment task, + effect, recovery, eligibility, and result state; - safe cancellation, timeout, and hold behavior inherited from the runner. Exit criteria: Python calls and a programmatic `SemanticCallSpec` use identical @@ -762,12 +931,13 @@ Deliverables: Exit criteria: -- three lazy segments complete in supported simulation; -- each segment re-observes the cube after free-fall settling; +- three lazy program/demo segments complete in supported simulation; +- each program/demo segment re-observes the cube after free-fall settling; - grasp and release effects are verified; - placement uses verified held-object state; - settle and validation data are present in metadata; -- multi-environment success, failure, and recovery masks remain independent; +- the environment batch advances through the shared call barrier while success, + failure, effect, recovery, and eligibility masks remain independent; - the task contains no task-specific motion-generation code. ### Phase 6: sequential skill coverage and articulated interaction @@ -818,9 +988,16 @@ independent of adoption of the new path. - strict decoder, unknown fields, schema versioning, bounded repeats, and registry reference errors; +- authoritative registry-ID normalization, legacy-`uid` alias collisions, and + complete registry/provider/planner obstacle-set agreement; - cumulative scene movement and collision dependency revision behavior; - profile capability matching, deterministic binding, and ambiguity errors; -- static versus observed stage partitioning; +- `AssembleGoal.base_pose` snapshot resolution and its automatic scene + dependency, with the `None` fallback isolated to legacy direct-core use; +- same-identity partial `StateDelta` merges retain previous scalar semantics + until every previously active row is replaced, for both individual and + coordinated attachments; +- exactly one semantic call and one invocation per `ExecutionSession`; - downstream target propagation for grasp selection; - object-centric place conversion from one immutable snapshot and verified held state; @@ -833,9 +1010,11 @@ independent of adoption of the new path. - Python facade and Expert Program lower to equivalent invocations; - runner scheduling, acknowledgement, safe stop, and cancellation are reused; -- one environment can complete while another recovers or fails; +- the Version 1 shared call barrier holds active rows together while completed, + recovering, and failed rows retain independent masks and state; - command buffering advances only through the environment clock; -- segment metadata is deterministic and serializable. +- program/demo-segment and trajectory-segment metadata are deterministic and + serializable. ### Simulation tests @@ -858,8 +1037,9 @@ The design is complete when all of the following hold: typed atomic-action core, and runtime. - [ ] A common new task using existing semantic skills needs no task-specific motion-generation code. -- [ ] Each scene entity is registered once across semantics, observation, - affordance, and collision handling. +- [ ] 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 math, planner construction, session plumbing, or custom verification. - [ ] Automatic grasping tracks target revisions and receives downstream object @@ -867,16 +1047,21 @@ The design is complete when all of the following hold: - [ ] `Place` is object-centric and consumes verified held-object state. - [ ] Built-in grasp, release, handover, and supported articulation effect monitors work in simulation. -- [ ] Repeated sub-threshold motion eventually publishes the correct scene +- [x] Repeated sub-threshold motion eventually publishes the correct scene revision. -- [ ] Custom actions have a documented and tested compatibility path. +- [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 + semantic call and re-observes before lowering the next call. - [ ] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass through `env.step()`. - [ ] 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. -- [ ] Multi-environment progress, effects, recovery, and failures remain +- [ ] 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, sessions, and planners as escape hatches. @@ -894,7 +1079,7 @@ The design is complete when all of the following hold: | Automatic binding makes surprising choices | Use capability validation and deterministic profile preferences; surface semantic ambiguity rather than silently selecting. | | Presets become opaque or unstable | Version preset semantics, emit the resolved core policies in runtime metadata, and keep typed overrides available to advanced users. | | Built-in effect monitors overfit simulation | Keep the contract backend-neutral and provide replaceable hardware implementations; record monitor evidence and thresholds. | -| Static compilation uses stale state | Default to dependency-driven `auto` partitioning and force observed boundaries after external effects or dynamic post-policies. | +| Static compilation uses stale state | Version 1 never coalesces semantic calls into one session or static stage; keep `engine.compile()` as an explicit advanced-core API until a later optimization proves equivalent observation/effect boundaries. | | Demo bridge duplicates runner logic | Keep scheduling, acknowledgement, recovery, timeout, and safe stop in `ExecutionRunner`; bridge only the Gym step boundary. | | Configuration grows into a programming language | Keep version 1 bounded and discriminated; add only registered nodes and no expressions or arbitrary DAG scheduler. | | Articulation and parallel work delay useful delivery | Ship the sequential cube vertical slice first; add reusable capabilities independently. | diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 7e7aeef73..0ad26c50a 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -196,14 +196,45 @@ entity as a recovery dependency. | Skill / field | `SceneEntityPose` accepted | Automatic scene-motion replan | |---|---:|---:| | `MoveEndEffector.xpos` | yes | yes | +| `MoveJoints.target` | no | no | | `MoveHeldObject.object_target_pose` | yes | yes | | `Place.xpos` | yes | yes | | `Press.xpos` | yes | yes | | `CoordinatedPickGoal.object_target_pose` / `object_initial_pose` | yes | yes | | `CoordinatedPlacementGoal` placing/support poses | yes | yes | | `PickUp.grasp_xpos` | yes | yes | -| `PickUp` / `HandOver` `ObjectSemantics.entity` lookup | not through `SceneEntityPose` | no automatic scene dependency | -| `AssembleGoal` base entity lookup | not through `SceneEntityPose` | latest pose is used when replanning, but base movement alone does not trigger it | +| `PickUp` `ObjectSemantics.entity_id` grounding | implicit snapshot reference | yes; always consumed for the object pose | +| 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 | + +### Object identity and grounding + +`ObjectSemantics.entity_id` is the canonical scene-snapshot key. It must be a +non-empty string when set. An explicit ID is strict: object grounding reads only +`PlanningContext.scene.entities[entity_id]`, and a missing entry is an error. It +never falls back to `ObjectSemantics.entity` after an explicit lookup fails. + +The live `entity` field remains a deprecated direct-core compatibility path only +when `entity_id` is absent. That read emits `DeprecationWarning` and cannot +create a scene-motion dependency. `collect_scene_dependencies()` intentionally +does not recurse into `ObjectSemantics`; each primitive declares a semantic ID +only when its planner actually consumes that object's snapshot pose. + +Attachment and handover identity are not based on `label`. The core resolves an +explicit `entity_id` only against another explicit ID. If either compared side +has one, both sides must have the same explicit value; an equal legacy +`entity.uid` does not match it. When both explicit IDs are absent, two non-empty +legacy UIDs may match. Only when neither side has either ID form may comparison +fall back to the same semantic object or live entity handle. Future +`SceneRegistry` integration will own arbitrary alias normalization; this core +bridge does not. + +`ObjectSemantics` is shallow-frozen. Its top-level fields, including +`entity_id`, cannot be rebound after construction; create a new semantics value +to change identity. Nested affordance and metadata objects remain mutable but +do not participate in identity. ### Parameter ownership @@ -307,7 +338,7 @@ bound manipulator. | Skill ID | `pick_up` | | Goal | `GraspGoal(semantics=..., grasp_xpos=None)` | | Binding | manipulator + end effector role `primary` | -| Precondition | `ObjectSemantics.entity` is set; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | +| Precondition | `ObjectSemantics.entity_id` resolves in the planning snapshot; the deprecated live `entity` fallback remains temporarily; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | | Effect | write `HeldObjectState` for the bound manipulator and clear overlapping coordinated attachment state | | Verification | the attachment effect must be verified during closed-loop execution | @@ -318,6 +349,12 @@ dependency, so material target motion invalidates and replans an executing reachability, and stores the selected `object_to_eef` transform in the expected held-object state. Later object-centric skills reuse that transform. +Set `ObjectSemantics.entity_id` to the same stable ID used by the scene +snapshot. `PickUp` resolves that object pose once per planning attempt, uses the +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. + `PickUp` requires `open` and `grasp` commands on the bound end-effector profile. Important `PickUpOptions` fields: @@ -330,11 +367,13 @@ Important `PickUpOptions` fields: | `downstream_object_target_poses` | Optional future reachability constraints used in grasp selection | | `obj_upright_direction`, `rotate_upright` | Optional orientation-selection behavior | -Reading `ObjectSemantics.entity` remains a live planning lookup rather than an -automatic dependency. Use an explicit `SceneEntityPose` in `grasp_xpos` when -object motion should trigger dynamic-goal replanning. +`ObjectSemantics.entity` without an ID is a deprecated compatibility path. Its +live pose does not create an automatic scene dependency. -**Example:** `scripts/tutorials/atomic_action/pickup.py` +**Example:** `scripts/tutorials/atomic_action/pickup.py` currently exercises the +deprecated entity-only fallback. For canonical snapshot grounding and moving +target recovery, see +`scripts/tutorials/atomic_action/moving_target_recovery.py`. (builtin-move-held-object)= @@ -343,6 +382,9 @@ object motion should trigger dynamic-goal replanning. Moves an already attached object to an object-frame target while keeping the hand closed. The caller specifies the desired **object pose**, not an EEF pose; the action derives `target_object_pose @ object_to_eef` from verified task state. +When upright transport needs the current object orientation, it derives it from +the observed EEF pose and verified `object_to_eef` relation rather than reading +a live scene entity. | Contract | Value | |---|---| @@ -400,26 +442,27 @@ The bound end-effector profile must provide `open` and `grasp`. Important ### Assembly through `Place` -`Place` also accepts `AssembleGoal(affordance=...)`. There is no separate -assembly skill: it derives the assemble-object target from the base object's -live pose and reuses the normal place/release segments. +`Place` also accepts +`AssembleGoal(affordance=..., base_pose=SceneEntityPose("base"))`. There is no +separate assembly skill: it derives the assemble-object target from the base +object's snapshot pose and reuses the normal place/release segments. ```text base_object_pose @ assemble_to_base_pose = assemble_object_target_pose assemble_object_target_pose @ held.object_to_eef = release_eef_pose ``` -The `AssembleAffordance` identifies the base and assemble objects, stores the -relative pose, and must provide `base_object_entity`. A prior verified `PickUp` -must have populated the held object's `object_to_eef` transform. Planning then -declares the same detach effect as a normal place. - -The base entity's current pose is read each time `plan()` runs. Because the -goal does not yet encode that entity through `SceneEntityPose`, base movement by -itself does not invalidate an executing plan; another recovery trigger is -required before the newer pose is resolved. +The `AssembleAffordance` stores the relative assembly pose. A prior verified +`PickUp` must have populated the held object's `object_to_eef` transform. +`base_pose` is resolved from each planning snapshot and automatically becomes a +recovery dependency. Omitting it temporarily falls back to the affordance's +`base_object_entity` with a deprecation warning; that fallback is not a scene +dependency. -**Example:** `scripts/tutorials/atomic_action/assemble.py` +**Example:** `scripts/tutorials/atomic_action/assemble.py` currently exercises +the legacy `base_object_entity` fallback and is not the canonical `base_pose` +form. It remains a compatibility example until the registry-backed tutorial +migration. (builtin-press)= @@ -458,7 +501,7 @@ both hands -> lift -> move object -> hold**. | Skill ID | `coordinated_pickment` | | Goal | `CoordinatedPickGoal` | | Binding | manipulator + end effector roles `left` and `right` | -| Precondition | `ObjectSemantics.entity` is set and the affordance is an `AntipodalAffordance` | +| Precondition | an `AntipodalAffordance`; when `object_initial_pose` is omitted, `ObjectSemantics.entity_id` resolves in the snapshot or the deprecated no-ID live fallback is available | | Goal geometry | shared-object target pose and optional initial object pose; left/right grasps are sampled from the affordance | | Effect | clear individual left/right attachments and create `CoordinatedHeldObjectState[(left, right)]` | | Verification | coordinated attachment must be externally verified | @@ -471,9 +514,13 @@ lowest-cost grasp on each side. The derived `object_to_eef` transforms are stored in the projected `CoordinatedHeldObjectState` and reused by later object-centric skills. -The object target and optional initial pose may use `SceneEntityPose`. When no -initial pose is supplied, `ObjectSemantics.entity` provides the object's current -pose. +The object target and optional initial pose may use `SceneEntityPose`. Those +references declare their own scene dependencies. When `object_initial_pose` is +omitted, the action grounds the initial pose from +`ObjectSemantics.entity_id` and declares that ID as a dependency; the deprecated +no-ID `entity` fallback is live and therefore cannot trigger scene-motion +replanning. Supplying `object_initial_pose` disables this implicit semantic +dependency because the explicit pose value is authoritative. Both bound end-effector profiles must provide `open` and `grasp`. Important `CoordinatedPickmentOptions` fields group into: @@ -549,9 +596,11 @@ 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 also queries the semantic object's live -orientation when replanning and preserves it at the supplied middle/final -positions. +`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. 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 6b38c9164..c4b2f7ab2 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -366,6 +366,10 @@ The similarly named `AtomicAction.plan()` method is not a fourth application entry point. It is a framework-owned template method called by the engine after resolving an invocation; skill implementations provide `_plan()`: +This is a deliberate hard extension boundary. Defining `plan()` on a subclass +raises `TypeError` at class definition and has no compatibility adapter. Migrate +an older custom action by renaming its implementation to `_plan()`. + | API | Intended caller | Behavior | |---|---|---| | `AtomicAction.plan(request, context)` | `AtomicActionEngine` | Binds the current collision scene into a copied policy, then delegates to `_plan()` | @@ -624,10 +628,11 @@ resets the new revision's local recovery counters, emits ```{attention} Automatic dynamic-goal invalidation is dependency-driven. A goal must contain a -`SceneEntityPose` for the session to track that scene entity. A primitive that -directly queries a simulation entity during planning will use its latest pose -when planning happens, but that query alone does not trigger scene-motion -replanning. +`SceneEntityPose`, or an object-centric primitive must explicitly declare the +`ObjectSemantics.entity_id` whose snapshot pose it consumes. `PickUp` and the +implicit-initial-pose path of coordinated pickup declare that dependency +automatically. The deprecated live-entity fallback does not trigger +scene-motion replanning. Dynamic collision invalidation is provider-driven. Only registered, pose-updatable collision entities are supported; adding/removing obstacles or @@ -696,7 +701,8 @@ A new primitive should: 4. put reusable embodiment commands on control-part profiles and generic motion/recovery choices in invocation policies; 5. implement side-effect-free `_plan(request, context)` using the engine-owned - planning services; do not override the framework-owned public `plan()`; + planning services; do not override the framework-owned public `plan()`—the + class definition is rejected if it does; 6. return full-robot timed motion, per-environment planning success, optional named segment metadata, diagnostics, and uncommitted effects; 7. add registration coverage, contract tests, execution/recovery tests, a diff --git a/docs/source/overview/sim/planners/curobo_planner.md b/docs/source/overview/sim/planners/curobo_planner.md index 915f7f098..24f3193c1 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -150,6 +150,12 @@ or live in an offset base frame, also declare their names in `"cuboid"` or `"mesh"` representation because sphere fitting expands one object into multiple independently named obstacles. +`CuroboWorldCfg` validates this planner-local registration at construction: +obstacle names must be unique, and every dynamic obstacle name must match the +`uid` (or generated fallback name) of an entry in `rigid_objects`. The later +`SceneRegistry` integration additionally cross-validates those names with the +scene provider rather than duplicating them in task code. + ### Shared and per-environment collision worlds `CuroboWorldCfg.multi_env` controls collision-world batching only. Robot start diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index f9d57c009..1bba99004 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -85,6 +85,11 @@ the protected ``_plan()`` hook instead. Similarly, ``engine.plan_action()`` is reserved for extensions and isolated tests that need to plan an unregistered instance. +This extension contract is intentionally strict: a subclass that defines +``plan()`` raises ``TypeError`` at class definition. There is no legacy adapter; +custom actions must rename that implementation to ``_plan()`` so the +framework-owned collision-scene preparation cannot be bypassed. + Runnable examples ----------------- @@ -361,10 +366,12 @@ The session replans from its latest context and emits an ``invocation_revised`` event. ``skill_id`` and ``invocation_id`` must still identify the active logical call. -Only entities referenced through ``SceneEntityPose`` become automatic -scene-motion dependencies. A skill may query a simulation entity's live pose -when it plans, but that query alone does not cause an executing session to -replan when the entity moves. +Entities referenced through ``SceneEntityPose`` become automatic scene-motion +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. Task-state effects ------------------ @@ -397,7 +404,8 @@ Define an action-owned frozen goal dataclass with a stable ``goal_kind``. Then define typed runtime options when needed, implement the protected ``_plan(request, context)`` hook, and declare the stable skill metadata. Do not override the inherited public ``plan()`` method because it binds the latest -collision scene first. +collision scene first. Legacy custom actions that implemented ``plan()`` must +rename it to ``_plan()``; defining ``plan()`` is rejected immediately. Return scalar or per-environment planner success through ``build_plan``. The framework normalizes the mask and holds failed rows at the observed qpos, so a From 1c596fe419574ed092e7dc0193f0e0f87c82f652 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 22:27:12 +0800 Subject: [PATCH 06/28] feat(sim): add authoritative scene registry --- embodichain/lab/sim/atomic_actions/state.py | 29 +- embodichain/lab/sim/planners/base_planner.py | 16 + .../lab/sim/planners/curobo/curobo_planner.py | 120 +- .../lab/sim/planners/curobo/curobo_yaml.py | 46 +- .../lab/sim/planners/motion_generator.py | 121 +- embodichain/lab/sim/skills/__init__.py | 51 + embodichain/lab/sim/skills/scene.py | 1342 +++++++++++++++++ tests/sim/atomic_actions/test_core.py | 48 + tests/sim/planners/test_curobo_planner.py | 177 +++ .../planners/test_motion_generator_batched.py | 139 ++ tests/sim/skills/__init__.py | 19 + tests/sim/skills/test_scene.py | 868 +++++++++++ .../skills/test_scene_curobo_integration.py | 115 ++ 13 files changed, 3048 insertions(+), 43 deletions(-) create mode 100644 embodichain/lab/sim/skills/__init__.py create mode 100644 embodichain/lab/sim/skills/scene.py create mode 100644 tests/sim/skills/__init__.py create mode 100644 tests/sim/skills/test_scene.py create mode 100644 tests/sim/skills/test_scene_curobo_integration.py diff --git a/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index 985599ac3..69cf8d044 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -18,9 +18,10 @@ from __future__ import annotations +from collections.abc import Iterator, Mapping from dataclasses import dataclass, field from types import MappingProxyType -from typing import Mapping, TYPE_CHECKING +from typing import TYPE_CHECKING import torch @@ -423,6 +424,30 @@ def __post_init__(self) -> None: object.__setattr__(self, "pose", self.pose.clone()) +class _ImmutableEntityMapping(Mapping[str, EntityState]): + """Own entity states and return defensive copies on every public read.""" + + __slots__ = ("_states",) + + def __init__(self, states: Mapping[str, EntityState]) -> None: + self._states = MappingProxyType( + { + entity_id: EntityState(state.pose, confidence=state.confidence) + for entity_id, state in states.items() + } + ) + + def __getitem__(self, entity_id: str) -> EntityState: + state = self._states[entity_id] + return EntityState(state.pose, confidence=state.confidence) + + def __iter__(self) -> Iterator[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.""" @@ -487,7 +512,7 @@ def __post_init__(self) -> None: "collision_entity_ids reference missing scene entities: " f"{sorted(missing)}." ) - object.__setattr__(self, "entities", MappingProxyType(normalized)) + object.__setattr__(self, "entities", _ImmutableEntityMapping(normalized)) object.__setattr__(self, "collision_entity_ids", collision_entity_ids) def collision_world_revisions(self, batch_size: int) -> tuple[int, ...]: diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index c6f0eecae..d58f7b019 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -22,6 +22,7 @@ from abc import ABC, abstractmethod from collections.abc import Mapping from dataclasses import MISSING +from typing import Literal from embodichain.utils import logger from embodichain.utils import configclass @@ -178,6 +179,21 @@ def __init__(self, cfg: BasePlannerCfg): supports_collision_world_updates: bool = False """Whether per-plan dynamic obstacle poses can update the collision world.""" + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical entity IDs accepted for dynamic pose updates.""" + return () + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every entity ID represented in the planner collision world.""" + return () + + @property + def collision_world_batch_mode(self) -> Literal["shared", "per_env"] | None: + """Return the planner collision world's batch-sharing mode, if any.""" + return None + def supports_move_type(self, move_type: MoveType) -> bool: """Return whether the planner accepts a movement target type directly. diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index 51766737f..6259134de 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -40,7 +40,7 @@ from dataclasses import dataclass from pathlib import Path from types import SimpleNamespace -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Literal import torch import yaml @@ -136,6 +136,25 @@ def __deepcopy__(self, memo: dict) -> "_RigidObjectRefList": # noqa: ARG002 return _RigidObjectRefList(self) +class _RigidObjectRefMapping(dict): + """Registry IDs mapped to live objects without deepcopying their handles.""" + + def __deepcopy__(self, memo: dict) -> "_RigidObjectRefMapping": # noqa: ARG002 + return _RigidObjectRefMapping(self) + + +def _named_rigid_objects( + rigid_objects: list[RigidObject] | Mapping[str, RigidObject] | None, +) -> list[tuple[str, RigidObject]]: + """Return canonical cuRobo obstacle names paired with their live objects.""" + if isinstance(rigid_objects, Mapping): + return list(rigid_objects.items()) + return [ + (getattr(obj, "uid", None) or f"obstacle_{index}", obj) + for index, obj in enumerate(rigid_objects or ()) + ] + + @configclass class CuroboWorldCfg: """Static collision-world configuration for the cuRobo backend. @@ -144,16 +163,19 @@ class CuroboWorldCfg: meshes (see :attr:`rigid_objects`); there is no external scene-YAML path. """ - rigid_objects: list[RigidObject] | None = None - """Live :class:`RigidObject` obstacles to bake into the auto-generated world YAML. + rigid_objects: list[RigidObject] | Mapping[str, RigidObject] | None = None + """Live :class:`RigidObject` obstacles to bake into the generated world YAML. The adapter reads each object's mesh (``get_vertices`` / ``get_triangles``) and world pose (``get_local_pose``) and writes a cuRobo V2 scene YAML (cached - on disk by content hash). Poses are written in the cuRobo world/base frame, - so this is exact when the robot base sits at the simulator world origin. For - obstacles that move or live in an offset base frame, also list their names in - :attr:`dynamic_obstacle_names` to update poses at plan time. ``None`` yields an - initially empty collision world. + on disk by content hash). A mapping is the registry-backed path: its keys are + authoritative obstacle IDs even when they differ from ``RigidObject.uid``. + The list form remains available for advanced callers and derives names from + ``uid`` (or ``obstacle_`` when absent). Poses are written in the cuRobo + world/base frame, so this is exact when the robot base sits at the simulator + world origin. For obstacles that move or live in an offset base frame, also + list their canonical names in :attr:`dynamic_obstacle_names` to update poses + at plan time. ``None`` yields an initially empty collision world. """ obstacle_representation: str = "sphere" @@ -178,7 +200,7 @@ class CuroboWorldCfg: """ dynamic_obstacle_names: list[str] = [] - """Registered rigid-object names whose poses may be updated between plans.""" + """Canonical obstacle IDs whose poses may be updated between plans.""" multi_env: bool = False """Whether cuRobo allocates one collision-world instance per environment. @@ -211,22 +233,45 @@ class CuroboWorldCfg: """ def __post_init__(self) -> None: - dynamic_names = list(self.dynamic_obstacle_names) - if len(set(dynamic_names)) != len(dynamic_names) or not all( - isinstance(name, str) and name for name in dynamic_names + if isinstance(self.dynamic_obstacle_names, (str, bytes)): + raise TypeError( + "dynamic_obstacle_names must be an iterable of obstacle IDs, " + "not a string." + ) + try: + dynamic_names = list(self.dynamic_obstacle_names) + except TypeError as exc: + raise TypeError( + "dynamic_obstacle_names must be an iterable of obstacle IDs." + ) from exc + if not all( + isinstance(name, str) and name and name == name.strip() + for name in dynamic_names ): raise ValueError( - "dynamic_obstacle_names must contain unique non-empty names." + "dynamic_obstacle_names must contain unique non-empty names " + "without outer whitespace." + ) + if len(set(dynamic_names)) != len(dynamic_names): + raise ValueError( + "dynamic_obstacle_names must contain unique non-empty names " + "without outer whitespace." ) - rigid_objects = list(self.rigid_objects or ()) - rigid_names = [ - getattr(obj, "uid", None) or f"obstacle_{index}" - for index, obj in enumerate(rigid_objects) - ] - if not all(isinstance(name, str) and name for name in rigid_names): + if self.rigid_objects is not None and not isinstance( + self.rigid_objects, + (list, Mapping), + ): + raise TypeError("rigid_objects must be a list, mapping, or None.") + named_rigid_objects = _named_rigid_objects(self.rigid_objects) + rigid_names = [name for name, _ in named_rigid_objects] + if not all( + isinstance(name, str) and name and name == name.strip() + for name in rigid_names + ): raise ValueError( - "CuroboWorldCfg.rigid_objects must have non-empty string names." + "CuroboWorldCfg.rigid_objects must have non-empty string obstacle " + "IDs without outer whitespace." ) if len(set(rigid_names)) != len(rigid_names): raise ValueError( @@ -243,7 +288,10 @@ def __post_init__(self) -> None: # Wrap live RigidObjects so the @configclass field-deepcopy (run right # after this by custom_post_init) shares references instead of trying to # pickle non-pickleable C++ dexsim handles held by each RigidObject. - if self.rigid_objects is not None and not isinstance( + if isinstance(self.rigid_objects, Mapping): + if not isinstance(self.rigid_objects, _RigidObjectRefMapping): + self.rigid_objects = _RigidObjectRefMapping(self.rigid_objects) + elif self.rigid_objects is not None and not isinstance( self.rigid_objects, _RigidObjectRefList ): self.rigid_objects = _RigidObjectRefList(self.rigid_objects) @@ -446,7 +494,7 @@ class CuroboPlanOptions(PlanOptions): """EmbodiChain control-part name to plan for.""" dynamic_obstacle_poses: dict[str, torch.Tensor] | None = None - """Per-obstacle world poses ``(B, 4, 4)`` keyed by configured name.""" + """World poses ``(B, 4, 4)`` keyed by canonical dynamic-obstacle ID.""" max_attempts: int | None = None """Per-plan override of ``CuroboPlannerCfg.max_attempts``.""" @@ -493,8 +541,8 @@ def _validate_dynamic_obstacles( """Validate dynamic-obstacle pose names and shapes. Args: - poses: Mapping of obstacle name -> pose tensor. ``None`` is a no-op. - allowed_names: Obstacle names declared in :class:`CuroboWorldCfg`. + poses: Mapping of canonical obstacle ID -> pose tensor. ``None`` is a no-op. + allowed_names: Canonical IDs declared in :class:`CuroboWorldCfg`. Raises: ValueError: If a name is not configured, or a pose is not ``(B, 4, 4)``. @@ -790,6 +838,23 @@ def preserve_plan_samples(self) -> bool: """ return self.cfg.preserve_plan_samples + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical registry IDs accepted for dynamic pose updates.""" + return tuple(self.cfg.world.dynamic_obstacle_names) + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every obstacle ID represented in the generated world.""" + return tuple( + name for name, _ in _named_rigid_objects(self.cfg.world.rigid_objects) + ) + + @property + def collision_world_batch_mode(self) -> Literal["shared", "per_env"]: + """Return the configured collision-world batching policy.""" + return "per_env" if self.cfg.world.multi_env else "shared" + def __init__(self, cfg: CuroboPlannerCfg) -> None: super().__init__(cfg) self.cfg: CuroboPlannerCfg = cfg @@ -1667,8 +1732,7 @@ def _world_yaml_cache_key(self, world_cfg: CuroboWorldCfg) -> str: hasher.update(str(auto.surface_radius).encode("utf-8")) hasher.update(str(auto.iterations).encode("utf-8")) hasher.update(str(auto.collision_sphere_buffer).encode("utf-8")) - for idx, obj in enumerate(world_cfg.rigid_objects or []): - name = getattr(obj, "uid", None) or f"obstacle_{idx}" + for name, obj in _named_rigid_objects(world_cfg.rigid_objects): hasher.update(name.encode("utf-8")) vertices = obj.get_vertices(env_ids=[0], scale=True)[0] faces = obj.get_triangles(env_ids=[0])[0] @@ -2248,8 +2312,8 @@ def update_dynamic_obstacles( """Update named dynamic obstacle poses on cached cuRobo collision worlds. Args: - poses: Mapping of obstacle name -> ``(B, 4, 4)`` world pose. ``None`` - is a no-op. + poses: Mapping of canonical obstacle ID -> ``(B, 4, 4)`` world pose. + ``None`` is a no-op. backend: Specific cached backend to update. If ``None``, updates all cached backends. sim_base_pose_inv: Precomputed inverse of the live sim base pose for diff --git a/embodichain/lab/sim/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/planners/curobo/curobo_yaml.py index 1b24eec68..f470f8e4b 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/planners/curobo/curobo_yaml.py @@ -28,7 +28,8 @@ from __future__ import annotations -from typing import TYPE_CHECKING, Sequence +from collections.abc import Mapping, Sequence +from typing import TYPE_CHECKING import torch @@ -539,7 +540,7 @@ def _mesh_to_obstacle_entry( def generate_curobo_world_yaml( - rigid_objects: Sequence[RigidObject], + rigid_objects: Sequence[RigidObject] | Mapping[str, RigidObject], output_path: str, *, representation: str = "cuboid", @@ -552,7 +553,7 @@ def generate_curobo_world_yaml( collision_sphere_buffer: float = 0.0, device: str = "cuda:0", ) -> str: - """Generate a cuRobo V2 scene (world) YAML from a sequence of ``RigidObject``. + """Generate a cuRobo V2 scene (world) YAML from live ``RigidObject`` handles. Each object's mesh (``get_vertices`` / ``get_triangles``) and world pose (``get_local_pose``) are converted into cuRobo obstacle entries under a single @@ -568,7 +569,9 @@ def generate_curobo_world_yaml( :meth:`~embodichain.lab.sim.planners.curobo.curobo_planner.CuroboPlanner.update_dynamic_obstacles`. Args: - rigid_objects: ``RigidObject`` instances to bake into the collision world. + rigid_objects: Objects to bake into the collision world. Mapping keys are + authoritative obstacle IDs. A sequence derives each name from the + object's ``uid`` (or ``obstacle_`` when absent). output_path: Destination YAML file path. representation: ``"cuboid"`` (default, AABB->OBB, no CUDA), ``"mesh"`` (exact triangle mesh, no CUDA), or ``"sphere"`` (cuRobo sphere fit, @@ -594,29 +597,48 @@ def generate_curobo_world_yaml( import yaml - rigid_objects = list(rigid_objects) - if not rigid_objects: + registry_backed = isinstance(rigid_objects, Mapping) + if registry_backed: + named_rigid_objects = list(rigid_objects.items()) + else: + named_rigid_objects = [ + (getattr(obj, "uid", None) or f"obstacle_{idx}", obj) + for idx, obj in enumerate(rigid_objects) + ] + if not named_rigid_objects: raise ValueError("rigid_objects must contain at least one RigidObject.") data: dict[str, dict[str, object]] = {} used_names: set[str] = set() - for idx, obj in enumerate(rigid_objects): - name = getattr(obj, "uid", None) or f"obstacle_{idx}" + for name, obj in named_rigid_objects: + if not isinstance(name, str) or not name or name != name.strip(): + raise ValueError( + "Obstacle IDs must be non-empty strings without outer whitespace." + ) if name in used_names: raise ValueError( - f"Duplicate obstacle name {name!r}; RigidObject uids must be unique." + f"Duplicate obstacle name {name!r}; obstacle IDs must be unique." ) used_names.add(name) vertices = obj.get_vertices(env_ids=[env_id], scale=True)[0] faces = obj.get_triangles(env_ids=[env_id])[0] - pose = obj.get_local_pose(to_matrix=False)[env_id] - - if vertices is None or faces is None or vertices.numel() == 0: + if ( + vertices is None + or faces is None + or vertices.numel() == 0 + or faces.numel() == 0 + ): + if registry_backed: + raise ValueError( + f"Registry-backed obstacle {name!r} has no mesh geometry; " + "the declared collision world cannot omit it." + ) logger.log_warning( f"RigidObject {name!r} has no mesh geometry; skipping collision export." ) continue + pose = obj.get_local_pose(to_matrix=False)[env_id] entries = _mesh_to_obstacle_entry( name, diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index e602dd6dc..0c3e9a4a4 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -168,6 +168,72 @@ def supports_dynamic_collision_world(self) -> bool: """ return getattr(self.planner, "supports_collision_world_updates", False) is True + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical dynamic-obstacle IDs declared by the planner.""" + entity_ids = getattr(self.planner, "dynamic_collision_entity_ids", ()) + return self._validate_collision_entity_ids( + entity_ids, + field_name="dynamic_collision_entity_ids", + ) + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every canonical entity ID in the planner collision world.""" + entity_ids = getattr(self.planner, "collision_world_entity_ids", ()) + return self._validate_collision_entity_ids( + entity_ids, + field_name="collision_world_entity_ids", + ) + + @staticmethod + def _validate_collision_entity_ids( + entity_ids: object, + *, + field_name: str, + ) -> tuple[str, ...]: + """Validate one planner-owned canonical collision-ID declaration.""" + if not isinstance(entity_ids, tuple) or not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in entity_ids + ): + raise TypeError( + f"Planner.{field_name} must be a tuple of " + "non-empty strings without outer whitespace." + ) + if len(set(entity_ids)) != len(entity_ids): + raise ValueError(f"Planner.{field_name} must contain unique IDs.") + return entity_ids + + @staticmethod + def _validate_collision_pose_keys( + poses: Mapping[object, object], + *, + field_name: str, + ) -> set[str]: + """Validate exact canonical IDs on one obstacle-pose mapping.""" + entity_ids = tuple(poses) + if not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in entity_ids + ): + raise TypeError( + f"{field_name} keys must be non-empty strings without outer " + "whitespace." + ) + return set(entity_ids) + + @property + def collision_world_batch_mode(self) -> Literal["shared", "per_env"] | None: + """Return the backend's dynamic collision-world batch-sharing mode.""" + mode = getattr(self.planner, "collision_world_batch_mode", None) + if mode not in (None, "shared", "per_env"): + raise ValueError( + "Planner.collision_world_batch_mode must be 'shared', 'per_env', " + "or None." + ) + return mode + def bind_collision_world( self, plan_opts: PlanOptions | None, @@ -192,15 +258,68 @@ def bind_collision_world( "collision-world updates.", ValueError, ) + configured_ids = self.dynamic_collision_entity_ids + received_ids = tuple(obstacle_poses) + if not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in received_ids + ): + raise TypeError( + "obstacle_poses keys must be non-empty strings without outer " + "whitespace." + ) + missing = sorted(set(configured_ids).difference(received_ids)) + extra = sorted(set(received_ids).difference(configured_ids)) + if missing or extra: + logger.log_error( + "Dynamic collision obstacle IDs do not match the planner " + f"configuration; missing={missing}, extra={extra}.", + ValueError, + ) options = ( deepcopy(plan_opts) if plan_opts is not None else self.planner.default_plan_options() ) - return self.planner.with_collision_world( + existing_poses = getattr(options, "dynamic_obstacle_poses", None) + if existing_poses is not None: + if not isinstance(existing_poses, Mapping): + raise TypeError( + "plan_opts.dynamic_obstacle_poses must be a mapping or None." + ) + existing_ids = self._validate_collision_pose_keys( + existing_poses, + field_name="plan_opts.dynamic_obstacle_poses", + ) + existing_extra = sorted(existing_ids.difference(configured_ids)) + if existing_extra: + raise ValueError( + "Caller planning options contain dynamic collision IDs that " + f"are not configured by the planner: {existing_extra}." + ) + bound = self.planner.with_collision_world( options, obstacle_poses=obstacle_poses, ) + if hasattr(bound, "dynamic_obstacle_poses"): + bound_poses = bound.dynamic_obstacle_poses + if bound_poses is None: + bound_ids: set[str] = set() + elif not isinstance(bound_poses, Mapping): + raise TypeError("Bound dynamic_obstacle_poses must be a mapping.") + else: + bound_ids = self._validate_collision_pose_keys( + bound_poses, + field_name="Bound dynamic_obstacle_poses", + ) + bound_missing = sorted(set(configured_ids).difference(bound_ids)) + bound_extra = sorted(bound_ids.difference(configured_ids)) + if bound_missing or bound_extra: + raise ValueError( + "Bound dynamic collision obstacle IDs do not match the planner " + f"configuration; missing={bound_missing}, extra={bound_extra}." + ) + return bound def resolve_plan_options( self, diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py new file mode 100644 index 000000000..f07a9222b --- /dev/null +++ b/embodichain/lab/sim/skills/__init__.py @@ -0,0 +1,51 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Semantic-skill integration contracts built on the atomic-action core.""" + +from __future__ import annotations + +from .scene import ( + RegistrySceneProvider, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneDynamics, + SceneEntityRef, + SceneEntityRegistration, + SceneEntityStateProvider, + SceneGeometryProvider, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + +__all__ = [ + "RegistrySceneProvider", + "SceneAffordanceRef", + "SceneArticulationRef", + "SceneCollisionRole", + "SceneCollisionWorldMode", + "SceneDynamics", + "SceneEntityRef", + "SceneEntityRegistration", + "SceneEntityStateProvider", + "SceneGeometryProvider", + "SceneLinkRef", + "SceneObjectRef", + "SceneRegistry", +] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py new file mode 100644 index 000000000..62d71ec61 --- /dev/null +++ b/embodichain/lab/sim/skills/scene.py @@ -0,0 +1,1342 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Authoritative scene identity and registration value contracts.""" + +from __future__ import annotations + +from collections.abc import Iterable, Iterator, Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass, replace +from enum import Enum +import math +from types import MappingProxyType +from typing import Any, Protocol, TYPE_CHECKING, TypeVar, runtime_checkable + +import torch + +from embodichain.lab.sim.common import BatchEntity +from embodichain.lab.sim.atomic_actions import ( + Affordance, + EntityState, + SceneProvider, + SceneSnapshot, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.planners import MotionGenerator + from embodichain.lab.sim.sim_manager import SimulationManager + + +RefT = TypeVar("RefT", bound="SceneEntityRef") + + +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(): + raise ValueError(f"{name} must be a non-empty string without outer whitespace.") + + +@dataclass(frozen=True, slots=True) +class SceneEntityRef: + """Typed reference to one authoritative scene-registry entity. + + Args: + entity_id: Globally stable canonical registry identifier. + """ + + entity_id: str + """Globally stable authoritative registry identifier.""" + + def __post_init__(self) -> None: + _validate_identifier(self.entity_id, "entity_id") + + +@dataclass(frozen=True, slots=True) +class SceneObjectRef(SceneEntityRef): + """Reference to one object registered in the semantic scene.""" + + +@dataclass(frozen=True, slots=True) +class SceneArticulationRef(SceneEntityRef): + """Reference to one articulation registered in the semantic scene.""" + + +@dataclass(frozen=True, slots=True) +class SceneLinkRef(SceneEntityRef): + """Reference to one registered articulation link.""" + + +@dataclass(frozen=True, slots=True) +class SceneAffordanceRef(SceneEntityRef): + """Reference to one registered interaction affordance.""" + + +class SceneDynamics(str, Enum): + """Physical mobility classification owned by a scene registration.""" + + UNKNOWN = "unknown" + STATIC = "static" + KINEMATIC = "kinematic" + DYNAMIC = "dynamic" + + +class SceneCollisionRole(str, Enum): + """How an entity participates in the planner collision world.""" + + NONE = "none" + STATIC = "static" + DYNAMIC = "dynamic" + + +class SceneCollisionWorldMode(str, Enum): + """Batch-sharing policy for a dynamic planner collision world.""" + + SHARED = "shared" + PER_ENV = "per_env" + + +@runtime_checkable +class SceneEntityStateProvider(Protocol): + """Observe one registered entity for an ordered environment batch.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + """Return the entity state whose rows follow ``env_ids``. + + Args: + timestamp: Observation timestamp supplied by the integration. + env_ids: Stable ordered environment correlation IDs. + + Returns: + Current pose and confidence for the registered entity. + """ + + +@runtime_checkable +class SceneGeometryProvider(Protocol): + """Provide one entity's planner-facing collision geometry descriptor.""" + + def get_geometry(self) -> object: + """Return the planner-facing geometry descriptor. + + Returns: + Backend-consumable geometry or a live simulation entity. + """ + + +@dataclass(frozen=True, slots=True, eq=False) +class SceneEntityRegistration: + """Immutable integration metadata for one authoritative scene entity. + + Parent relationships, simulator-native names, pose sources, geometry, and + affordances belong to the registry registration rather than the lightweight + reference copied into semantic calls. + + Args: + ref: Canonical typed reference. + state_provider: Optional dynamic pose/confidence 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``. + dynamics: Physical mobility classification. + geometry_provider: Planner-facing collision geometry source. + collision_role: Static, dynamic, or no planner collision role. + semantic_type: Optional application semantic type. + affordance: Affordance value for an affordance registration. + relative_pose: Optional parent-relative affordance transform. + """ + + ref: SceneEntityRef + """Canonical typed reference owned by the registry.""" + + state_provider: SceneEntityStateProvider | None = None + """Explicit dynamic pose/confidence source.""" + + aliases: tuple[str, ...] = () + """External or legacy names normalized once at the registry boundary.""" + + parent: SceneEntityRef | None = None + """Canonical parent reference for a link or affordance.""" + + native_name: str | None = None + """Backend-local link or affordance name under ``parent``.""" + + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + """Static, kinematic, dynamic, or unknown mobility classification.""" + + geometry_provider: SceneGeometryProvider | None = None + """Collision geometry source required for planner collision roles.""" + + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + """Static/dynamic planner-obstacle role, or ``none``.""" + + semantic_type: str | None = None + """Optional application semantic type such as ``container`` or ``tool``.""" + + affordance: Affordance | None = None + """Affordance value owned by a :class:`SceneAffordanceRef` registration.""" + + 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): + raise TypeError("ref must be a SceneEntityRef.") + if self.state_provider is not None and not isinstance( + self.state_provider, + SceneEntityStateProvider, + ): + raise TypeError("state_provider must implement SceneEntityStateProvider.") + + if isinstance(self.aliases, (str, bytes)): + raise TypeError("aliases must be an iterable of identifiers, not a string.") + try: + aliases = tuple(self.aliases) + except TypeError as exc: + raise TypeError("aliases must be an iterable of identifiers.") from exc + for alias in aliases: + _validate_identifier(alias, "alias") + aliases = tuple(alias for alias in aliases if alias != self.ref.entity_id) + if len(set(aliases)) != len(aliases): + raise ValueError("aliases must be unique.") + object.__setattr__(self, "aliases", aliases) + + if self.parent is not None and not isinstance(self.parent, SceneEntityRef): + raise TypeError("parent must be a SceneEntityRef or None.") + if self.native_name is not None: + _validate_identifier(self.native_name, "native_name") + if not isinstance(self.dynamics, SceneDynamics): + raise TypeError("dynamics must be a SceneDynamics value.") + if not isinstance(self.collision_role, SceneCollisionRole): + raise TypeError("collision_role must be a SceneCollisionRole value.") + if self.geometry_provider is not None and not isinstance( + self.geometry_provider, + SceneGeometryProvider, + ): + raise TypeError("geometry_provider must implement SceneGeometryProvider.") + if self.semantic_type is not 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.") + 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.") + if self.relative_pose.shape != (4, 4): + raise ValueError("relative_pose must have shape (4, 4).") + object.__setattr__(self, "relative_pose", self.relative_pose.clone()) + if self.state_provider is not None and self.relative_pose is not None: + raise ValueError( + "state_provider and relative_pose are mutually exclusive pose sources." + ) + + self._validate_reference_contract() + if ( + self.collision_role is not SceneCollisionRole.NONE + and self.geometry_provider is None + ): + raise ValueError( + f"Collision entity {self.ref.entity_id!r} requires geometry_provider." + ) + + def _validate_reference_contract(self) -> None: + """Validate fields whose meaning follows from the typed ref.""" + if isinstance(self.ref, (SceneObjectRef, SceneArticulationRef)): + if self.parent is not None: + raise ValueError( + "Object and articulation registrations cannot have a parent." + ) + if self.native_name is not None: + raise ValueError( + "Object and articulation registrations cannot have native_name." + ) + if self.state_provider is None: + raise ValueError( + "Object and articulation registrations require state_provider." + ) + if self.relative_pose is not None: + raise ValueError( + "Object and articulation registrations cannot use relative_pose." + ) + if self.affordance is not None: + raise ValueError( + "Affordance values require a SceneAffordanceRef registration." + ) + return + + if isinstance(self.ref, SceneLinkRef): + if ( + not isinstance(self.parent, SceneArticulationRef) + or self.native_name is None + ): + raise ValueError("Link registrations require parent and native_name.") + if self.state_provider is None: + raise ValueError("Link registrations require state_provider.") + if self.relative_pose is not None: + raise ValueError("Link registrations cannot use relative_pose.") + if self.affordance is not None: + raise ValueError( + "Affordance values require a SceneAffordanceRef registration." + ) + return + + if isinstance(self.ref, SceneAffordanceRef): + if ( + not isinstance( + self.parent, + (SceneObjectRef, SceneArticulationRef, SceneLinkRef), + ) + or self.native_name is None + ): + raise ValueError( + "Affordance registrations require parent and native_name." + ) + if self.affordance is None: + raise ValueError("Affordance registrations require affordance.") + if self.state_provider is None and self.relative_pose is None: + raise ValueError( + "Affordance registrations require state_provider or relative_pose." + ) + 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.") + + +def _copy_registration( + registration: SceneEntityRegistration, +) -> SceneEntityRegistration: + """Copy registry metadata without cloning live providers or entities.""" + relative_pose = registration.relative_pose + return replace( + registration, + affordance=_copy_affordance(registration.affordance), + relative_pose=relative_pose.clone() if relative_pose is not None else None, + ) + + +def _copy_affordance(affordance: Affordance | None) -> Affordance | None: + """Own mutable affordance metadata while preserving live entity handles.""" + if affordance is None: + return None + memo: dict[int, object] = {} + visited: set[int] = set() + + def visit(value: object) -> None: + value_id = id(value) + if value_id in visited: + return + visited.add(value_id) + if isinstance(value, BatchEntity): + memo[value_id] = value + return + if is_dataclass(value) and not isinstance(value, type): + for data_field in fields(value): + nested = getattr(value, data_field.name) + if data_field.name == "_generator" and nested is not None: + memo[id(nested)] = None + else: + visit(nested) + return + if isinstance(value, Mapping): + for key, nested in value.items(): + visit(key) + visit(nested) + return + if isinstance(value, (list, tuple, set, frozenset)): + for nested in value: + visit(nested) + + visit(affordance) + try: + return 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 + + +@dataclass(frozen=True, slots=True, eq=False, init=False) +class SceneRegistry: + """Immutable authoritative catalog of semantic scene entities. + + Canonical identifiers occupy one flat, globally unique namespace. Aliases + are accepted only at lookup and integration boundaries and always resolve + to a canonical typed reference before they leave the registry. + + Args: + registrations: Complete scene registrations. The iterable is copied and + cannot be extended after construction. + collision_world_mode: Explicit dynamic-collision batch policy. It may be + omitted for a single environment, which resolves to ``shared``. A + multi-environment dynamic world must select a mode explicitly. + """ + + _registrations: tuple[SceneEntityRegistration, ...] = field(repr=False) + _registrations_by_id: Mapping[str, SceneEntityRegistration] = field(repr=False) + _aliases: Mapping[str, str] = 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) + collision_world_mode: SceneCollisionWorldMode | None + + def __init__( + self, + registrations: Iterable[SceneEntityRegistration] = (), + *, + collision_world_mode: SceneCollisionWorldMode | None = None, + ) -> None: + 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." + ) + try: + supplied = tuple(registrations) + except TypeError as exc: + raise TypeError("registrations must be an iterable.") from exc + if not all(isinstance(item, SceneEntityRegistration) for item in supplied): + raise TypeError( + "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) + object.__setattr__(self, "_registrations", owned) + object.__setattr__( + self, + "_registrations_by_id", + MappingProxyType(by_id), + ) + object.__setattr__(self, "_aliases", MappingProxyType(aliases)) + object.__setattr__( + self, + "_collision_world_entity_ids", + tuple( + item.ref.entity_id + for item in owned + if item.collision_role is not SceneCollisionRole.NONE + ), + ) + object.__setattr__( + self, + "_dynamic_collision_entity_ids", + tuple( + item.ref.entity_id + for item in owned + if item.collision_role is SceneCollisionRole.DYNAMIC + ), + ) + object.__setattr__( + self, + "_static_collision_entity_ids", + tuple( + item.ref.entity_id + for item in owned + if item.collision_role is SceneCollisionRole.STATIC + ), + ) + 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_refs(self) -> tuple[SceneEntityRef, ...]: + """Return canonical typed references in registration order.""" + return tuple(item.ref for item in self._registrations) + + @property + def aliases(self) -> Mapping[str, str]: + """Return the immutable alias-to-canonical-ID index.""" + return self._aliases + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every canonical ID represented in the planner world.""" + return self._collision_world_entity_ids + + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical IDs whose planner poses update dynamically.""" + return self._dynamic_collision_entity_ids + + @property + def static_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical IDs baked into the static planner world.""" + return self._static_collision_entity_ids + + def __len__(self) -> int: + return len(self._registrations) + + def __iter__(self) -> Iterator[SceneEntityRef]: + return iter(self.entity_refs) + + def __getitem__( + self, + identifier: str | SceneEntityRef, + ) -> SceneEntityRegistration: + return self.lookup(identifier) + + def resolve( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + ) -> RefT: + """Resolve a canonical ID or alias to a typed canonical reference. + + Args: + identifier: Canonical ID, alias, or already typed canonical ref. + expected_type: Required reference class for typed lookup. + + Returns: + Registry-owned canonical reference. + + Raises: + 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] + + def lookup( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + ) -> SceneEntityRegistration: + """Return an owned registration after canonical typed resolution. + + Args: + identifier: Canonical ID, alias, or typed canonical reference. + expected_type: Required reference class. + + Returns: + A structurally independent copy of the matching registration. + """ + ref = self.resolve(identifier, expected_type=expected_type) + return _copy_registration(self._registrations_by_id[ref.entity_id]) + + def make_scene_provider( + self, + *, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + batch_size: int | None = None, + ) -> RegistrySceneProvider: + """Create an independent provider without planner cross-validation. + + This factory is intended for perception and direct-core consumers. The + canonical planning path must use :meth:`make_planning_scene_provider` + so planner IDs, capabilities, and collision-world mode cannot drift. + + Args: + translation_threshold: Accumulated translation needed to publish a + material scene change. + rotation_threshold: Accumulated rotation needed to publish a + material scene change. + batch_size: Optional fixed integration batch size. Supplying it + validates the collision-world mode immediately and binds the + provider to that row count. + + Returns: + A new provider with independent revisions and published baselines. + """ + return RegistrySceneProvider( + self, + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + batch_size=batch_size, + ) + + def make_planning_scene_provider( + self, + motion_generator: MotionGenerator, + *, + batch_size: int, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + ) -> RegistrySceneProvider: + """Create a provider after complete planner/registry validation. + + Args: + motion_generator: Motion generator that will consume dynamic poses. + batch_size: Number of execution environments. + translation_threshold: Accumulated translation needed to publish a + material scene change. + rotation_threshold: Accumulated rotation needed to publish a + material scene change. + + Returns: + A new independently stateful, planner-validated scene provider. + """ + provider = self.make_scene_provider( + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + batch_size=batch_size, + ) + self.validate_collision_integration( + motion_generator, + batch_size=batch_size, + scene_provider=provider, + ) + return provider + + def collision_geometry_by_id( + self, + role: SceneCollisionRole | None = None, + ) -> Mapping[str, object]: + """Materialize planner geometry under canonical registry IDs. + + Args: + role: Optional exact collision-role filter. Without a filter, all + static and dynamic collision registrations are included. + Registrations whose role is :attr:`SceneCollisionRole.NONE` + never enter the planner collision world. + + Returns: + Fresh immutable canonical-ID-to-geometry mapping. + """ + if role is not None and not isinstance(role, SceneCollisionRole): + raise TypeError("role must be a SceneCollisionRole or None.") + geometry: dict[str, object] = {} + for registration in self._registrations: + provider = registration.geometry_provider + if provider is None: + continue + if role is None: + if registration.collision_role is SceneCollisionRole.NONE: + continue + elif registration.collision_role is not role: + continue + entity_id = registration.ref.entity_id + descriptor = provider.get_geometry() + if descriptor is None: + raise ValueError( + f"Collision geometry provider for scene entity " + f"{entity_id!r} returned None." + ) + geometry[entity_id] = descriptor + return MappingProxyType(geometry) + + def validate_collision_integration( + self, + motion_generator: MotionGenerator, + *, + batch_size: int, + scene_provider: SceneProvider | None = None, + ) -> SceneCollisionWorldMode | None: + """Validate registry/planner agreement before dynamic planning. + + Args: + motion_generator: Motion generator whose planner consumes obstacles. + batch_size: Number of execution environments. + scene_provider: Optional external perception or hardware provider. + Its concrete ``collision_entity_ids`` must agree exactly with + the registry and planner declarations. + + Returns: + Effective dynamic collision mode, or ``None`` without dynamic IDs. + """ + effective_mode = self.resolve_collision_world_mode(batch_size=batch_size) + try: + planner_dynamic_ids = motion_generator.dynamic_collision_entity_ids + planner_world_ids = motion_generator.collision_world_entity_ids + supports_updates = motion_generator.supports_dynamic_collision_world + planner_mode = motion_generator.collision_world_batch_mode + except AttributeError as exc: + raise TypeError( + "motion_generator must expose collision-world integration properties." + ) from exc + planner_dynamic_ids = self._validate_integration_ids( + planner_dynamic_ids, + field_name="motion_generator.dynamic_collision_entity_ids", + ) + planner_world_ids = self._validate_integration_ids( + planner_world_ids, + field_name="motion_generator.collision_world_entity_ids", + ) + registry_dynamic_ids = set(self.dynamic_collision_entity_ids) + planner_dynamic_id_set = set(planner_dynamic_ids) + if registry_dynamic_ids != planner_dynamic_id_set: + raise ValueError( + "Dynamic collision entity mismatch: registry missing from planner " + f"{sorted(registry_dynamic_ids - planner_dynamic_id_set)}, planner " + "missing from registry " + f"{sorted(planner_dynamic_id_set - registry_dynamic_ids)}. Planner IDs " + "must use authoritative registry IDs, not aliases." + ) + registry_world_ids = set(self.collision_world_entity_ids) + planner_world_id_set = set(planner_world_ids) + if registry_world_ids != planner_world_id_set: + raise ValueError( + "Collision world entity mismatch: registry missing from planner " + f"{sorted(registry_world_ids - planner_world_id_set)}, planner " + "missing from registry " + f"{sorted(planner_world_id_set - registry_world_ids)}. Planner IDs " + "must use authoritative registry IDs, not aliases." + ) + if scene_provider is not None: + if not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider.") + provider_ids = getattr(scene_provider, "collision_entity_ids", None) + provider_ids = self._validate_integration_ids( + provider_ids, + field_name="scene_provider.collision_entity_ids", + ) + provider_id_set = set(provider_ids) + if registry_dynamic_ids != provider_id_set: + raise ValueError( + "Dynamic collision entity mismatch: registry missing from " + "provider " + f"{sorted(registry_dynamic_ids - provider_id_set)}, provider " + "missing from registry " + f"{sorted(provider_id_set - registry_dynamic_ids)}. Provider IDs " + "must use authoritative registry IDs, not aliases." + ) + collision_geometry = self.collision_geometry_by_id() + if set(collision_geometry) != registry_world_ids: + raise ValueError( + "Collision geometry IDs do not match authoritative registry " + f"world IDs {sorted(registry_world_ids)}." + ) + if not registry_dynamic_ids: + return None + if supports_updates is not True: + raise ValueError( + "The selected motion generator does not support dynamic collision " + f"updates required by {sorted(registry_dynamic_ids)}." + ) + assert effective_mode is not None + if planner_mode != effective_mode.value: + raise ValueError( + "Dynamic collision world mode mismatch: registry requires " + f"{effective_mode.value!r}, planner declares {planner_mode!r}." + ) + return effective_mode + + @staticmethod + def _validate_integration_ids( + value: object, + *, + field_name: str, + ) -> tuple[str, ...]: + """Validate one canonical collision-ID declaration at a boundary.""" + if not isinstance(value, tuple) or not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in value + ): + raise TypeError( + f"{field_name} must be a tuple of non-empty canonical IDs " + "without outer whitespace." + ) + if len(set(value)) != len(value): + raise ValueError(f"{field_name} must contain unique IDs.") + return value + + def resolve_collision_world_mode( + self, + *, + batch_size: int, + ) -> SceneCollisionWorldMode | None: + """Resolve the configured collision mode for an execution batch. + + Args: + batch_size: Number of execution environments. + + Returns: + The effective mode, or ``None`` when no dynamic collision entity is + registered. + """ + return self._effective_collision_world_mode(batch_size) + + def _effective_collision_world_mode( + self, + batch_size: int, + ) -> SceneCollisionWorldMode | None: + """Resolve E without reading any live state or planner integration.""" + if isinstance(batch_size, bool) or not isinstance(batch_size, int): + raise TypeError("batch_size must be an integer.") + if batch_size <= 0: + raise ValueError("batch_size must be positive.") + if not self.dynamic_collision_entity_ids: + return None + if self.collision_world_mode is not None: + return self.collision_world_mode + if batch_size == 1: + return SceneCollisionWorldMode.SHARED + raise ValueError( + "Multi-environment dynamic collision requires an explicit " + "collision_world_mode of 'shared' or 'per_env'." + ) + + @classmethod + def from_simulation( + cls, + simulation: SimulationManager, + *, + rigid_objects: Mapping[str, str] | None = None, + articulations: Mapping[str, str] | None = None, + collision_roles: Mapping[str, SceneCollisionRole] | None = None, + geometry_providers: Mapping[str, SceneGeometryProvider] | None = None, + collision_world_mode: SceneCollisionWorldMode | None = None, + ) -> SceneRegistry: + """Opt explicitly selected simulation entities into a registry. + + ``rigid_objects`` and ``articulations`` map authoritative registry IDs + to simulation UIDs. UIDs become aliases automatically; unlisted + simulation entities are never imported. Collision participation + defaults to :attr:`SceneCollisionRole.NONE`. + + Args: + simulation: Simulation manager used only for explicit UID lookup. + rigid_objects: Canonical object IDs mapped to simulation UIDs. + articulations: Canonical articulation IDs mapped to simulation UIDs. + collision_roles: Optional collision roles keyed by canonical ID. + geometry_providers: Optional geometry overrides keyed by canonical + ID. Selected rigid objects otherwise expose their live handles. + collision_world_mode: Optional dynamic collision batch-sharing mode. + + Returns: + Immutable registry containing only the explicitly selected entities. + """ + object_ids = cls._normalize_simulation_mapping( + rigid_objects, + name="rigid_objects", + ) + articulation_ids = cls._normalize_simulation_mapping( + articulations, + name="articulations", + ) + duplicate_ids = set(object_ids).intersection(articulation_ids) + if duplicate_ids: + raise ValueError( + "Simulation registry IDs must be globally unique across entity " + f"types: {sorted(duplicate_ids)}." + ) + all_ids = set(object_ids).union(articulation_ids) + roles = dict(collision_roles or {}) + geometry = dict(geometry_providers or {}) + for mapping_name, values in ( + ("collision_roles", roles), + ("geometry_providers", geometry), + ): + unknown = set(values).difference(all_ids) + if unknown: + raise KeyError( + f"{mapping_name} reference unselected registry IDs: " + f"{sorted(unknown)}." + ) + + registrations: list[SceneEntityRegistration] = [] + for registry_id, uid in object_ids.items(): + entity = cls._get_simulation_entity( + simulation, + getter_name="get_rigid_object", + registry_id=registry_id, + uid=uid, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneObjectRef(registry_id), + state_provider=_SimulationEntityStateProvider(entity), + aliases=(uid,), + geometry_provider=geometry.get( + registry_id, + _SimulationEntityGeometryProvider(entity), + ), + collision_role=roles.get( + registry_id, + SceneCollisionRole.NONE, + ), + ) + ) + for registry_id, uid in articulation_ids.items(): + entity = cls._get_simulation_entity( + simulation, + getter_name="get_articulation", + registry_id=registry_id, + uid=uid, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneArticulationRef(registry_id), + state_provider=_SimulationEntityStateProvider(entity), + aliases=(uid,), + geometry_provider=geometry.get(registry_id), + collision_role=roles.get( + registry_id, + SceneCollisionRole.NONE, + ), + ) + ) + return cls( + registrations, + collision_world_mode=collision_world_mode, + ) + + @staticmethod + def _normalize_simulation_mapping( + mapping: Mapping[str, str] | None, + *, + name: str, + ) -> dict[str, str]: + if mapping is None: + return {} + if not isinstance(mapping, Mapping): + raise TypeError(f"{name} must be a mapping from registry ID to UID.") + normalized = dict(mapping) + for registry_id, uid in normalized.items(): + _validate_identifier(registry_id, f"{name} registry ID") + _validate_identifier(uid, f"{name} UID") + return normalized + + @staticmethod + def _get_simulation_entity( + simulation: SimulationManager, + *, + getter_name: str, + registry_id: str, + uid: str, + ) -> Any: + getter = getattr(simulation, getter_name, None) + if not callable(getter): + raise TypeError(f"simulation must provide {getter_name}().") + entity = getter(uid) + if entity is None: + raise KeyError( + f"Simulation UID {uid!r} selected for registry entity " + f"{registry_id!r} was not found." + ) + return entity + + +@dataclass(frozen=True, slots=True) +class _SimulationEntityStateProvider: + """Read poses from one explicitly selected simulation entity.""" + + entity: Any + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + pose = self.entity.get_local_pose(to_matrix=True) + if not isinstance(pose, torch.Tensor): + raise TypeError("Simulation entity get_local_pose() must return a tensor.") + return EntityState(pose) + + +@dataclass(frozen=True, slots=True) +class _SimulationEntityGeometryProvider: + """Expose a selected live rigid object as planner geometry input.""" + + entity: Any + + def get_geometry(self) -> object: + return self.entity + + +class RegistrySceneProvider(SceneProvider): + """Stateful scene provider derived from an immutable registry. + + Instances are created by :meth:`SceneRegistry.make_scene_provider`; each + instance owns its revision counters and material-pose baselines. + + Args: + registry: Immutable catalog that owns entity registrations. + translation_threshold: Accumulated translation needed to publish a + material scene change. + rotation_threshold: Accumulated rotation needed to publish a material + scene change. + batch_size: Optional fixed execution batch size. Factory-created + planning providers bind this value before their first observation. + """ + + def __init__( + self, + registry: SceneRegistry, + *, + translation_threshold: float, + rotation_threshold: float, + batch_size: int | None = None, + ) -> None: + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + for name, value in ( + ("translation_threshold", translation_threshold), + ("rotation_threshold", rotation_threshold), + ): + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(float(value)) + or value < 0.0 + ): + raise ValueError(f"{name} must be finite and non-negative.") + self.registry = registry + self.translation_threshold = float(translation_threshold) + self.rotation_threshold = float(rotation_threshold) + self.collision_entity_ids = registry.dynamic_collision_entity_ids + self._expected_batch_size = batch_size + self._last_timestamp: float | None = None + self._env_ids: torch.Tensor | None = None + self._published_poses: dict[str, torch.Tensor] = {} + self._published_confidences: dict[str, float] = {} + self._scene_version = 0 + self._collision_revisions: list[int] = [] + self._effective_collision_world_mode = ( + registry.resolve_collision_world_mode(batch_size=batch_size) + if batch_size is not None + else None + ) + + @property + def collision_world_mode(self) -> SceneCollisionWorldMode | None: + """Return the configured or first-snapshot-resolved collision mode.""" + return ( + self._effective_collision_world_mode + if self._effective_collision_world_mode is not None + else self.registry.collision_world_mode + ) + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + """Observe all canonical entities and advance material revisions. + + Args: + timestamp: Non-negative monotonic observation timestamp. + env_ids: Stable ordered correlation IDs for every environment row. + + Returns: + An immutable snapshot keyed only by canonical registry IDs. + """ + if ( + isinstance(timestamp, bool) + or not isinstance(timestamp, (int, float)) + or not math.isfinite(float(timestamp)) + or timestamp < 0.0 + ): + raise ValueError("timestamp must be finite and non-negative.") + if self._last_timestamp is not None and timestamp < self._last_timestamp: + raise ValueError("Scene provider timestamps must be monotonic.") + 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.") + + batch_size = int(env_ids.numel()) + if ( + self._expected_batch_size is not None + and batch_size != self._expected_batch_size + ): + raise ValueError( + "Scene provider batch size must remain equal to its configured " + f"batch_size={self._expected_batch_size}; got {batch_size}." + ) + effective_mode = self.registry.resolve_collision_world_mode( + batch_size=batch_size + ) + stable_ids = env_ids.detach().to("cpu") + if self._env_ids is None: + self._env_ids = stable_ids.clone() + self._collision_revisions = [0] * batch_size + self._effective_collision_world_mode = effective_mode + elif not torch.equal(stable_ids, self._env_ids): + raise ValueError("Scene provider env_ids must remain stable and ordered.") + + states = self._observe_states( + 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() + } + if self._published_poses: + changed_by_entity = { + entity_id: self._pose_change_mask( + self._published_poses[entity_id], + current_pose, + ) + for entity_id, current_pose in poses.items() + } + confidence_changed = any( + 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() + ): + self._scene_version += 1 + collision_changed = torch.zeros(batch_size, dtype=torch.bool) + for entity_id in self.collision_entity_ids: + collision_changed |= changed_by_entity[entity_id] + for row in collision_changed.nonzero(as_tuple=False).flatten().tolist(): + self._collision_revisions[row] += 1 + + for entity_id, changed in changed_by_entity.items(): + if changed.any(): + published_pose = self._published_poses[entity_id] + changed_on_published_device = changed.to(published_pose.device) + current_pose = poses[entity_id].to( + device=published_pose.device, + dtype=published_pose.dtype, + ) + published_pose[changed_on_published_device] = current_pose[ + changed_on_published_device + ] + self._published_confidences = confidences.copy() + else: + self._published_poses = { + entity_id: pose.clone() for entity_id, pose in poses.items() + } + self._published_confidences = confidences.copy() + + self._last_timestamp = float(timestamp) + return SceneSnapshot( + timestamp=float(timestamp), + version=self._scene_version, + entities=states, + collision_world_revision=tuple(self._collision_revisions), + collision_entity_ids=self.collision_entity_ids, + ) + + def _observe_states( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, EntityState]: + """Observe explicit sources before deriving relative affordance poses.""" + batch_size = int(env_ids.numel()) + states: dict[str, EntityState] = {} + relative_registrations: list[SceneEntityRegistration] = [] + for registration in self.registry._registrations: + entity_id = registration.ref.entity_id + if registration.state_provider is None: + relative_registrations.append(registration) + continue + state = registration.state_provider.observe( + timestamp=timestamp, + env_ids=env_ids.clone(), + ) + if not isinstance(state, EntityState): + raise TypeError( + f"State provider for {entity_id!r} must return EntityState." + ) + states[entity_id] = EntityState( + self._normalize_pose(state.pose, batch_size, entity_id), + confidence=state.confidence, + ) + + for registration in relative_registrations: + entity_id = registration.ref.entity_id + assert registration.parent is not None + assert registration.relative_pose is not None + parent_state = states[registration.parent.entity_id] + relative_pose = registration.relative_pose.to( + device=parent_state.pose.device, + dtype=parent_state.pose.dtype, + ) + pose = torch.matmul(parent_state.pose, relative_pose) + states[entity_id] = EntityState( + pose, + confidence=parent_state.confidence, + ) + return states + + @staticmethod + def _normalize_pose( + pose: torch.Tensor, + batch_size: int, + entity_id: str, + ) -> torch.Tensor: + if pose.shape == (4, 4): + return pose.unsqueeze(0).expand(batch_size, -1, -1).clone() + if pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Scene entity {entity_id!r} pose must have shape (4, 4) or " + f"({batch_size}, 4, 4)." + ) + return pose.clone() + + def _pose_change_mask( + self, + previous: torch.Tensor, + current: torch.Tensor, + ) -> torch.Tensor: + """Return CPU rows changed against the last material publication.""" + current = current.to(device=previous.device, dtype=previous.dtype) + translation = torch.linalg.vector_norm( + current[:, :3, 3] - previous[:, :3, 3], + dim=1, + ) + relative_rotation = torch.bmm( + previous[:, :3, :3].transpose(1, 2), + current[:, :3, :3], + ) + cosine = ( + (relative_rotation.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) / 2.0 + ).clamp(-1.0, 1.0) + rotation = torch.acos(cosine) + return ( + ( + (translation > self.translation_threshold) + | (rotation > self.rotation_threshold) + ) + .detach() + .to("cpu") + ) + + +__all__ = [ + "RegistrySceneProvider", + "SceneAffordanceRef", + "SceneArticulationRef", + "SceneCollisionRole", + "SceneCollisionWorldMode", + "SceneDynamics", + "SceneEntityRef", + "SceneEntityRegistration", + "SceneEntityStateProvider", + "SceneGeometryProvider", + "SceneLinkRef", + "SceneObjectRef", + "SceneRegistry", +] diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 2a4ae179c..618da4545 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -544,6 +544,54 @@ def test_scene_snapshot_expands_global_collision_world_revision() -> None: assert torch.equal(obstacle_poses["obstacle"], pose) +def test_scene_snapshot_owns_entity_state_storage() -> None: + pose = torch.eye(4) + state = EntityState(pose) + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"object": state}, + ) + + pose.fill_(2.0) + state.pose.fill_(3.0) + + assert torch.equal(snapshot.entities["object"].pose, torch.eye(4)) + + +def test_scene_snapshot_entity_reads_are_defensive() -> None: + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"object": EntityState(torch.eye(4))}, + ) + + first_read = snapshot.entities["object"] + first_read.pose.fill_(7.0) + + assert torch.equal(snapshot.entities["object"].pose, torch.eye(4)) + with pytest.raises(TypeError): + snapshot.entities["other"] = EntityState(torch.eye(4)) # type: ignore[index] + + +def test_scene_snapshot_collision_pose_reads_are_defensive() -> None: + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"obstacle": EntityState(torch.eye(4))}, + collision_entity_ids=("obstacle",), + ) + + obstacle_poses = snapshot.collision_obstacle_poses( + batch_size=1, + device=torch.device("cpu"), + dtype=torch.float32, + ) + obstacle_poses["obstacle"].fill_(5.0) + + assert torch.equal(snapshot.entities["obstacle"].pose, torch.eye(4)) + + def test_scene_snapshot_rejects_unknown_collision_entity() -> None: with pytest.raises(ValueError, match="missing scene entities"): SceneSnapshot( diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index ba86c7d4a..b1f5ec92e 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -27,6 +27,7 @@ import importlib import logging import math +from types import SimpleNamespace import pytest import torch @@ -229,6 +230,28 @@ def test_curobo_world_cfg_accepts_registered_dynamic_obstacle(): assert cfg.dynamic_obstacle_names == ["known"] +def test_curobo_world_cfg_mapping_uses_registry_id_for_dynamic_obstacle(): + obstacle = type("NamedObstacle", (), {"uid": "legacy_uid"})() + + cfg = CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names=["registry_cube"], + ) + + assert cfg.dynamic_obstacle_names == ["registry_cube"] + assert cfg.rigid_objects["registry_cube"] is obstacle + + +def test_curobo_world_cfg_mapping_does_not_accept_object_uid_as_alias(): + obstacle = type("NamedObstacle", (), {"uid": "legacy_uid"})() + + with pytest.raises(ValueError, match="not present in rigid_objects"): + CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names=["legacy_uid"], + ) + + def test_curobo_world_cfg_rejects_unregistered_dynamic_obstacle(): obstacle = type("NamedObstacle", (), {"uid": "known"})() @@ -249,6 +272,28 @@ def test_curobo_world_cfg_rejects_duplicate_dynamic_obstacle_names(): ) +def test_curobo_world_cfg_rejects_outer_whitespace_in_obstacle_ids(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(ValueError, match="without outer whitespace"): + CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names=[" registry_cube"], + ) + with pytest.raises(ValueError, match="without outer whitespace"): + CuroboWorldCfg(rigid_objects={" registry_cube": obstacle}) + + +def test_curobo_world_cfg_rejects_string_dynamic_obstacle_collection(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(TypeError, match="not a string"): + CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names="registry_cube", # type: ignore[arg-type] + ) + + def test_curobo_world_cfg_rejects_duplicate_rigid_object_names(): obstacle_type = type("NamedObstacle", (), {"uid": "duplicate"}) @@ -256,6 +301,26 @@ def test_curobo_world_cfg_rejects_duplicate_rigid_object_names(): CuroboWorldCfg(rigid_objects=[obstacle_type(), obstacle_type()]) +@pytest.mark.parametrize( + ("multi_env", "expected_mode"), + [(False, "shared"), (True, "per_env")], +) +def test_curobo_planner_exposes_collision_world_contract(multi_env, expected_mode): + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg( + rigid_objects={"registry_cube": object()}, + dynamic_obstacle_names=["registry_cube"], + multi_env=multi_env, + ), + ) + + assert planner.dynamic_collision_entity_ids == ("registry_cube",) + assert planner.collision_world_entity_ids == ("registry_cube",) + assert planner.collision_world_batch_mode == expected_mode + + def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): planner = object.__new__(CuroboPlanner) configured_pose = torch.eye(4).unsqueeze(0) @@ -551,6 +616,88 @@ def test_generate_cuboid_world_yaml_assembles_schema(tmp_path): assert data["cuboid"]["demo_block"]["pose"][:3] == pytest.approx([0.45, 0.0, 0.18]) +def test_generate_world_yaml_uses_mapping_key_instead_of_object_uid(tmp_path): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + output_path = tmp_path / "registry_world.yml" + + generate_curobo_world_yaml( + {"registry_cube": rigid_object}, + str(output_path), + representation="cuboid", + ) + data = yaml.safe_load(output_path.read_text(encoding="utf-8")) + + assert set(data["cuboid"]) == {"registry_cube"} + + +def test_world_yaml_cache_key_includes_registry_id(): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg(rigid_objects={"registry_cube": rigid_object}), + ) + registry_key = planner._world_yaml_cache_key(planner.cfg.world) + planner.cfg.world = CuroboWorldCfg( + rigid_objects={"renamed_registry_cube": rigid_object} + ) + + renamed_key = planner._world_yaml_cache_key(planner.cfg.world) + + assert registry_key != renamed_key + + +def test_dynamic_update_uses_registry_id_in_curobo_backend(): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg( + rigid_objects={"registry_cube": rigid_object}, + obstacle_representation="cuboid", + dynamic_obstacle_names=["registry_cube"], + ), + ) + planner._curobo_device = torch.device("cpu") + planner._bindings = SimpleNamespace(Pose=lambda **kwargs: kwargs) + updates = [] + collision_checker = SimpleNamespace( + update_obstacle_pose=lambda name, pose, env_idx: updates.append( + (name, pose, env_idx) + ) + ) + backend = SimpleNamespace( + batch_size=1, + profile=SimpleNamespace(sim_base_to_curobo_base=None), + sim_base_to_curobo_base_matrix=None, + planner=SimpleNamespace(scene_collision_checker=collision_checker), + ) + identity = torch.eye(4).unsqueeze(0) + + planner.update_dynamic_obstacles( + {"registry_cube": identity}, + backend=backend, + sim_base_pose_inv=identity, + ) + + assert [(name, env_idx) for name, _, env_idx in updates] == [("registry_cube", 0)] + + def test_generate_mesh_world_yaml_assembles_schema(tmp_path): rigid_object = _FakeRigidObject( "demo_block", @@ -604,6 +751,21 @@ def test_generate_world_yaml_rejects_empty_input(tmp_path): generate_curobo_world_yaml([], str(tmp_path / "world.yml")) +def test_registry_world_yaml_rejects_empty_geometry_instead_of_skipping(tmp_path): + rigid_object = _FakeRigidObject( + "legacy_uid", + torch.zeros((0, 3), dtype=torch.float32), + torch.zeros((0, 3), dtype=torch.int64), + _identity_pose(), + ) + + with pytest.raises(ValueError, match="Registry-backed obstacle.*no mesh"): + generate_curobo_world_yaml( + {"registry_cube": rigid_object}, + str(tmp_path / "world.yml"), + ) + + def test_generate_world_yaml_rejects_duplicate_names(tmp_path): pose = _identity_pose() first = _FakeRigidObject( @@ -626,6 +788,21 @@ def test_generate_world_yaml_rejects_duplicate_names(tmp_path): ) +def test_generate_world_yaml_rejects_outer_whitespace_in_mapping_id(tmp_path): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + + with pytest.raises(ValueError, match="without outer whitespace"): + generate_curobo_world_yaml( + {" registry_cube": rigid_object}, + str(tmp_path / "world.yml"), + ) + + def test_generated_cuboid_yaml_loads_in_curobo_scene_cfg(tmp_path): pytest.importorskip("curobo") from curobo._src.geom.types import SceneCfg diff --git a/tests/sim/planners/test_motion_generator_batched.py b/tests/sim/planners/test_motion_generator_batched.py index a2adbdb49..32d18fd9e 100644 --- a/tests/sim/planners/test_motion_generator_batched.py +++ b/tests/sim/planners/test_motion_generator_batched.py @@ -129,6 +129,7 @@ def test_direct_cartesian_planner_requires_joint_fallback_inputs(): def test_bind_collision_world_copies_caller_options() -> None: planner = Mock() planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("obstacle",) original = PlanOptions() obstacle_pose = torch.eye(4).unsqueeze(0) @@ -152,6 +153,143 @@ def bind(options, *, obstacle_poses): planner.with_collision_world.assert_called_once() +@pytest.mark.parametrize( + ("configured_ids", "obstacle_poses", "expected"), + [ + (("cube", "tray"), {"cube": torch.eye(4).unsqueeze(0)}, "missing"), + ( + ("cube",), + { + "cube": torch.eye(4).unsqueeze(0), + "tray": torch.eye(4).unsqueeze(0), + }, + "extra", + ), + ], +) +def test_bind_collision_world_requires_exact_planner_entity_ids( + configured_ids, obstacle_poses, expected +) -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = configured_ids + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(ValueError, match=expected): + generator.bind_collision_world(None, obstacle_poses=obstacle_poses) + + planner.with_collision_world.assert_not_called() + + +def test_bind_collision_world_rejects_extra_ids_in_caller_options() -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("cube",) + generator = object.__new__(MotionGenerator) + generator.planner = planner + options = PlanOptions() + options.dynamic_obstacle_poses = {"legacy_cube": torch.eye(4).unsqueeze(0)} + + with pytest.raises(ValueError, match="Caller planning options.*legacy_cube"): + generator.bind_collision_world( + options, + obstacle_poses={"cube": torch.eye(4).unsqueeze(0)}, + ) + + planner.with_collision_world.assert_not_called() + + +def test_bind_collision_world_rejects_ids_injected_by_backend() -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("cube",) + + def bind(options, *, obstacle_poses): + options.dynamic_obstacle_poses = { + **obstacle_poses, + "legacy_cube": torch.eye(4).unsqueeze(0), + } + return options + + planner.with_collision_world.side_effect = bind + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(ValueError, match="Bound dynamic collision.*legacy_cube"): + generator.bind_collision_world( + PlanOptions(), + obstacle_poses={"cube": torch.eye(4).unsqueeze(0)}, + ) + + +def test_bind_collision_world_allows_none_for_empty_configured_world() -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = () + planner.default_plan_options.return_value = PlanOptions() + + def bind(options, *, obstacle_poses): + assert obstacle_poses == {} + options.dynamic_obstacle_poses = None + return options + + planner.with_collision_world.side_effect = bind + generator = object.__new__(MotionGenerator) + generator.planner = planner + + bound = generator.bind_collision_world(None, obstacle_poses={}) + + assert bound.dynamic_obstacle_poses is None + + +def test_bind_collision_world_rejects_non_string_option_keys() -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = () + generator = object.__new__(MotionGenerator) + generator.planner = planner + options = PlanOptions() + options.dynamic_obstacle_poses = {1: torch.eye(4).unsqueeze(0)} + + with pytest.raises(TypeError, match="keys must be non-empty strings"): + generator.bind_collision_world(options, obstacle_poses={}) + + planner.with_collision_world.assert_not_called() + + +def test_motion_generator_exposes_collision_integration_metadata() -> None: + planner = Mock() + planner.dynamic_collision_entity_ids = ("cube", "tray") + planner.collision_world_entity_ids = ("cube", "tray", "table") + planner.collision_world_batch_mode = "per_env" + generator = object.__new__(MotionGenerator) + generator.planner = planner + + assert generator.dynamic_collision_entity_ids == ("cube", "tray") + assert generator.collision_world_entity_ids == ("cube", "tray", "table") + assert generator.collision_world_batch_mode == "per_env" + + +@pytest.mark.parametrize( + ("entity_ids", "error_type", "match"), + [ + (("cube", "cube"), ValueError, "unique"), + ((" cube",), TypeError, "outer whitespace"), + ], +) +def test_motion_generator_rejects_invalid_collision_entity_metadata( + entity_ids, error_type, match +) -> None: + planner = Mock() + planner.dynamic_collision_entity_ids = entity_ids + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(error_type, match=match): + _ = generator.dynamic_collision_entity_ids + + def test_bind_collision_world_rejects_unsupported_planner() -> None: planner = Mock() planner.supports_collision_world_updates = False @@ -171,6 +309,7 @@ def test_bind_collision_world_rejects_unsupported_planner() -> None: def test_bind_collision_world_uses_backend_default_options() -> None: planner = Mock() planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("obstacle",) defaults = PlanOptions() planner.default_plan_options.return_value = defaults planner.with_collision_world.return_value = defaults diff --git a/tests/sim/skills/__init__.py b/tests/sim/skills/__init__.py new file mode 100644 index 000000000..8dc25c19d --- /dev/null +++ b/tests/sim/skills/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# 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 semantic-skill integration contracts.""" + +from __future__ import annotations diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py new file mode 100644 index 000000000..89c7b34fc --- /dev/null +++ b/tests/sim/skills/test_scene.py @@ -0,0 +1,868 @@ +# ---------------------------------------------------------------------------- +# 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 authoritative semantic-scene registrations.""" + +from __future__ import annotations + +from dataclasses import FrozenInstanceError + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import Affordance, EntityState, SceneSnapshot +from embodichain.lab.sim.skills import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneEntityRegistration, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + + +class _StateProvider: + """Return one fixed identity pose for registration validation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp + return EntityState(torch.eye(4).repeat(env_ids.numel(), 1, 1)) + + +class _GeometryProvider: + """Return one opaque collision-geometry descriptor.""" + + def get_geometry(self) -> object: + return {"kind": "box"} + + +class _EmptyGeometryProvider: + """Satisfy the geometry protocol but fail to materialize a descriptor.""" + + def get_geometry(self) -> object: + return None + + +class _MutableStateProvider: + """Expose a mutable pose while recording provider calls.""" + + 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 _MotionGenerator: + """Minimal dynamic-collision integration surface.""" + + def __init__( + self, + *, + entity_ids: tuple[str, ...], + world_entity_ids: tuple[str, ...] | None = None, + supports_updates: bool = True, + batch_mode: str | None = "per_env", + ) -> None: + self.dynamic_collision_entity_ids = entity_ids + self.collision_world_entity_ids = ( + entity_ids if world_entity_ids is None else world_entity_ids + ) + self.supports_dynamic_collision_world = supports_updates + self.collision_world_batch_mode = batch_mode + + +class _ExternalSceneProvider: + """External provider with an explicit concrete collision declaration.""" + + def __init__(self, entity_ids: tuple[str, ...]) -> None: + self.collision_entity_ids = entity_ids + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + del timestamp, env_ids + raise NotImplementedError + + +class _SimulationEntity: + """Simulation entity pose source used by the opt-in adapter tests.""" + + def __init__(self, pose: torch.Tensor) -> None: + self.pose = pose + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix is True + return self.pose + + +class _Simulation: + """Minimal simulation lookup surface with selected and unselected assets.""" + + def __init__(self) -> None: + self.rigid_objects = { + "sim_cube": _SimulationEntity(torch.eye(4)), + "ignored": _SimulationEntity(torch.eye(4) * 2.0), + } + self.articulations = { + "sim_drawer": _SimulationEntity(torch.eye(4)), + } + + def get_rigid_object(self, uid: str) -> _SimulationEntity | None: + return self.rigid_objects.get(uid) + + def get_articulation(self, uid: str) -> _SimulationEntity | None: + return self.articulations.get(uid) + + +@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"): + SceneObjectRef(entity_id) + + +def test_scene_entity_refs_are_typed_and_immutable() -> None: + object_ref = SceneObjectRef("cube") + + assert object_ref != SceneArticulationRef("cube") + with pytest.raises(FrozenInstanceError): + object_ref.entity_id = "other" # type: ignore[misc] + + +def test_registration_normalizes_self_alias_without_rewriting_names() -> None: + registration = SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("cube", "sim_cube"), + ) + + assert registration.aliases == ("sim_cube",) + + +def test_registration_rejects_duplicate_aliases() -> None: + with pytest.raises(ValueError, match="aliases"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("sim_cube", "sim_cube"), + ) + + +def test_registration_rejects_string_as_alias_collection() -> None: + with pytest.raises(TypeError, match="aliases.*not a string"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases="sim_cube", # type: ignore[arg-type] + ) + + +def test_root_registration_requires_explicit_state_provider() -> None: + with pytest.raises(ValueError, match="state_provider"): + SceneEntityRegistration(ref=SceneObjectRef("cube")) + + +def test_link_registration_requires_parent_and_native_name() -> None: + with pytest.raises(ValueError, match="parent and native_name"): + SceneEntityRegistration( + ref=SceneLinkRef("drawer_handle_link"), + state_provider=_StateProvider(), + ) + + +def test_affordance_registration_owns_parent_relation_and_pose() -> None: + relative_pose = torch.eye(4) + registration = SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + parent=SceneLinkRef("drawer_handle_link"), + native_name="handle", + affordance=Affordance(), + relative_pose=relative_pose, + ) + relative_pose.fill_(4.0) + + assert registration.relative_pose is not None + assert torch.equal(registration.relative_pose, torch.eye(4)) + + +def test_affordance_registration_rejects_two_pose_sources() -> None: + with pytest.raises(ValueError, match="mutually exclusive"): + SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + state_provider=_StateProvider(), + parent=SceneLinkRef("drawer_handle_link"), + native_name="handle", + affordance=Affordance(), + relative_pose=torch.eye(4), + ) + + +def test_collision_registration_requires_geometry_provider() -> None: + with pytest.raises(ValueError, match="geometry_provider"): + SceneEntityRegistration( + ref=SceneObjectRef("obstacle"), + state_provider=_StateProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ) + + registration = SceneEntityRegistration( + ref=SceneObjectRef("obstacle"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ) + assert registration.geometry_provider is not None + + +def test_registry_resolves_aliases_to_typed_canonical_refs() -> None: + cube_ref = SceneObjectRef("cube") + drawer_ref = SceneArticulationRef("drawer") + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=cube_ref, + state_provider=_StateProvider(), + aliases=("sim_cube",), + ), + SceneEntityRegistration( + ref=drawer_ref, + state_provider=_StateProvider(), + aliases=("sim_drawer",), + ), + ) + ) + + assert registry.resolve("sim_cube", expected_type=SceneObjectRef) is cube_ref + assert registry.lookup("sim_drawer").ref is drawer_ref + assert registry.aliases == { + "sim_cube": "cube", + "sim_drawer": "drawer", + } + + with pytest.raises(TypeError, match="SceneObjectRef"): + registry.resolve("sim_cube", expected_type=SceneArticulationRef) + with pytest.raises(TypeError, match="SceneArticulationRef"): + registry.resolve(SceneArticulationRef("cube")) + + +def test_registry_enforces_one_flat_global_id_namespace() -> None: + registrations = ( + SceneEntityRegistration( + ref=SceneObjectRef("shared"), + state_provider=_StateProvider(), + ), + SceneEntityRegistration( + ref=SceneArticulationRef("shared"), + state_provider=_StateProvider(), + ), + ) + + with pytest.raises(ValueError, match="Duplicate canonical"): + SceneRegistry(registrations) + + +def test_registry_rejects_alias_collision_with_canonical_id() -> None: + with pytest.raises(ValueError, match="collides with canonical"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("drawer",), + ), + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=_StateProvider(), + ), + ) + ) + + +def test_registry_rejects_ambiguous_aliases_across_types() -> None: + with pytest.raises(ValueError, match="ambiguous"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("legacy",), + ), + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=_StateProvider(), + aliases=("legacy",), + ), + ) + ) + + +def test_registry_requires_registered_exact_typed_parent() -> None: + link_registration = SceneEntityRegistration( + ref=SceneLinkRef("drawer_link"), + parent=SceneArticulationRef("drawer"), + native_name="link", + state_provider=_StateProvider(), + ) + + with pytest.raises(ValueError, match="unregistered parent"): + SceneRegistry((link_registration,)) + with pytest.raises(TypeError, match="registered as SceneObjectRef"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("drawer"), + state_provider=_StateProvider(), + ), + link_registration, + ) + ) + + +@pytest.mark.parametrize( + "ref_type", + [SceneLinkRef, SceneAffordanceRef], +) +def test_registry_rejects_duplicate_parent_native_member(ref_type: type) -> None: + parent = SceneArticulationRef("drawer") + + def member_registration(entity_id: str) -> SceneEntityRegistration: + if ref_type is SceneLinkRef: + return SceneEntityRegistration( + ref=SceneLinkRef(entity_id), + parent=parent, + native_name="handle", + state_provider=_StateProvider(), + ) + return SceneEntityRegistration( + ref=SceneAffordanceRef(entity_id), + parent=parent, + native_name="handle", + affordance=Affordance(), + relative_pose=torch.eye(4), + ) + + with pytest.raises(ValueError, match="native_name.*already registered"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=parent, + state_provider=_StateProvider(), + ), + member_registration("first"), + member_registration("second"), + ) + ) + + +def test_registry_is_structurally_immutable_and_owns_relative_pose() -> None: + parent = SceneObjectRef("drawer") + relative_pose = torch.eye(4) + affordance_registration = SceneEntityRegistration( + ref=SceneAffordanceRef("handle"), + parent=parent, + native_name="handle", + affordance=Affordance(), + relative_pose=relative_pose, + ) + registrations = [ + SceneEntityRegistration( + ref=parent, + state_provider=_StateProvider(), + ), + affordance_registration, + ] + registry = SceneRegistry(registrations) + + registrations.clear() + relative_pose.fill_(3.0) + assert len(registry) == 2 + returned_pose = registry.lookup("handle").relative_pose + assert returned_pose is not None + assert torch.equal(returned_pose, torch.eye(4)) + returned_pose.fill_(5.0) + assert torch.equal(registry.lookup("handle").relative_pose, torch.eye(4)) + with pytest.raises(TypeError): + registry.aliases["new"] = "drawer" # type: ignore[index] + with pytest.raises(FrozenInstanceError): + registry.collision_world_mode = SceneCollisionWorldMode.SHARED # type: ignore[misc] + + +def test_registry_owns_and_defensively_copies_affordance_metadata() -> None: + parent = SceneObjectRef("drawer") + affordance = Affordance(custom_config={"limits": {"opening": 0.3}}) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=parent, + state_provider=_StateProvider(), + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("handle"), + parent=parent, + native_name="handle", + affordance=affordance, + relative_pose=torch.eye(4), + ), + ) + ) + + affordance.custom_config["limits"]["opening"] = 0.8 + public_affordance = registry.lookup("handle").affordance + assert public_affordance is not None + assert public_affordance.custom_config["limits"]["opening"] == 0.3 + + public_affordance.custom_config["limits"]["opening"] = 1.0 + second_read = registry.lookup("handle").affordance + assert second_read is not None + assert second_read.custom_config["limits"]["opening"] == 0.3 + + +def test_registry_provider_uses_canonical_ids_and_derives_relative_pose() -> None: + parent_pose = torch.eye(4).repeat(2, 1, 1) + parent_pose[:, 0, 3] = torch.tensor([1.0, 2.0]) + relative_pose = torch.eye(4) + relative_pose[1, 3] = 0.25 + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("drawer"), + state_provider=_MutableStateProvider(parent_pose), + aliases=("sim_drawer",), + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("handle"), + parent=SceneObjectRef("drawer"), + native_name="handle", + affordance=Affordance(), + relative_pose=relative_pose, + ), + ) + ) + + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor([10, 20], dtype=torch.long), + ) + + assert set(snapshot.entities) == {"drawer", "handle"} + assert "sim_drawer" not in snapshot.entities + assert torch.equal( + snapshot.entities["handle"].pose, + torch.matmul(parent_pose, relative_pose), + ) + + +def test_registry_providers_have_independent_revisions() -> None: + state_provider = _MutableStateProvider(torch.eye(4)) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=state_provider, + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + first_provider = registry.make_scene_provider() + second_provider = registry.make_scene_provider() + env_ids = torch.tensor([0, 1], dtype=torch.long) + first_provider.snapshot(timestamp=0.0, env_ids=env_ids) + moved = torch.eye(4).repeat(2, 1, 1) + moved[1, 0, 3] = 0.1 + state_provider.pose = moved + + changed = first_provider.snapshot(timestamp=1.0, env_ids=env_ids) + independent_initial = second_provider.snapshot(timestamp=1.0, env_ids=env_ids) + + assert changed.version == 1 + assert changed.collision_world_revisions(2) == (0, 1) + assert independent_initial.version == 0 + assert independent_initial.collision_world_revisions(2) == (0, 0) + + +def test_registry_provider_accumulates_subthreshold_motion_per_row() -> None: + pose = torch.eye(4).repeat(2, 1, 1) + state_provider = _MutableStateProvider(pose) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=state_provider, + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + provider = registry.make_scene_provider(translation_threshold=0.01) + env_ids = torch.tensor([4, 8], dtype=torch.long) + provider.snapshot(timestamp=0.0, env_ids=env_ids) + first_motion = pose.clone() + first_motion[1, 0, 3] = 0.006 + state_provider.pose = first_motion + + below_threshold = provider.snapshot(timestamp=1.0, env_ids=env_ids) + second_motion = first_motion.clone() + second_motion[1, 0, 3] = 0.012 + state_provider.pose = second_motion + accumulated_change = provider.snapshot(timestamp=2.0, env_ids=env_ids) + + assert below_threshold.version == 0 + assert below_threshold.collision_world_revisions(2) == (0, 0) + assert accumulated_change.version == 1 + assert accumulated_change.collision_world_revisions(2) == (0, 1) + + +def test_multi_env_dynamic_collision_requires_explicit_mode_before_observation() -> ( + None +): + state_provider = _MutableStateProvider(torch.eye(4)) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=state_provider, + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ) + ) + + with pytest.raises(ValueError, match="explicit collision_world_mode"): + registry.make_scene_provider(batch_size=2) + provider = registry.make_scene_provider() + with pytest.raises(ValueError, match="explicit collision_world_mode"): + provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0, 1], dtype=torch.long), + ) + assert state_provider.calls == 0 + + +def test_single_env_dynamic_collision_defaults_to_shared_mode() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ) + ) + provider = registry.make_scene_provider(batch_size=1) + + assert provider.collision_world_mode is SceneCollisionWorldMode.SHARED + + snapshot = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + assert provider.collision_entity_ids == ("cube",) + assert snapshot.collision_world_revisions(1) == (0,) + + +def test_collision_integration_requires_exact_canonical_ids_and_mode() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + aliases=("sim_cube",), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + assert ( + registry.validate_collision_integration( + _MotionGenerator(entity_ids=("cube",)), # type: ignore[arg-type] + batch_size=2, + scene_provider=_ExternalSceneProvider(("cube",)), + ) + is SceneCollisionWorldMode.PER_ENV + ) + with pytest.raises(ValueError, match="authoritative registry IDs"): + registry.validate_collision_integration( + _MotionGenerator(entity_ids=("sim_cube",)), # type: ignore[arg-type] + batch_size=2, + ) + with pytest.raises(ValueError, match="does not support"): + registry.validate_collision_integration( + _MotionGenerator( # type: ignore[arg-type] + entity_ids=("cube",), + supports_updates=False, + ), + batch_size=2, + ) + with pytest.raises(ValueError, match="mode mismatch"): + registry.validate_collision_integration( + _MotionGenerator( # type: ignore[arg-type] + entity_ids=("cube",), + batch_mode="shared", + ), + batch_size=2, + ) + + +def test_collision_integration_requires_exact_full_world_ids() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + SceneEntityRegistration( + ref=SceneObjectRef("table"), + aliases=("legacy_table",), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.STATIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + assert registry.collision_world_entity_ids == ("cube", "table") + assert ( + registry.validate_collision_integration( + _MotionGenerator( + entity_ids=("cube",), + world_entity_ids=("cube", "table"), + ), # type: ignore[arg-type] + batch_size=2, + ) + is SceneCollisionWorldMode.PER_ENV + ) + with pytest.raises(ValueError, match="Collision world.*authoritative registry IDs"): + registry.validate_collision_integration( + _MotionGenerator( + entity_ids=("cube",), + world_entity_ids=("cube", "legacy_table"), + ), # type: ignore[arg-type] + batch_size=2, + ) + + +def test_static_only_collision_world_does_not_require_dynamic_updates() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("table"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.STATIC, + ), + ) + ) + + assert ( + registry.validate_collision_integration( + _MotionGenerator( + entity_ids=(), + world_entity_ids=("table",), + supports_updates=False, + batch_mode=None, + ), # type: ignore[arg-type] + batch_size=2, + ) + is None + ) + + +def test_collision_integration_rejects_external_provider_id_drift() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + with pytest.raises(ValueError, match="provider.*authoritative registry IDs"): + registry.validate_collision_integration( + _MotionGenerator(entity_ids=("cube",)), # type: ignore[arg-type] + batch_size=2, + scene_provider=_ExternalSceneProvider(("legacy_cube",)), + ) + + +def test_planning_provider_factory_validates_before_returning_provider() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + provider = registry.make_planning_scene_provider( + _MotionGenerator(entity_ids=("cube",)), # type: ignore[arg-type] + batch_size=2, + ) + assert provider.collision_entity_ids == ("cube",) + + with pytest.raises(ValueError, match="entity mismatch"): + registry.make_planning_scene_provider( + _MotionGenerator(entity_ids=("other",)), # type: ignore[arg-type] + batch_size=2, + ) + + with pytest.raises(ValueError, match="configured batch_size=2"): + provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + +def test_collision_geometry_is_materialized_under_canonical_ids() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("dynamic_cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + SceneEntityRegistration( + ref=SceneObjectRef("static_table"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.STATIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.SHARED, + ) + + all_geometry = registry.collision_geometry_by_id() + dynamic_geometry = registry.collision_geometry_by_id(SceneCollisionRole.DYNAMIC) + + assert set(all_geometry) == {"dynamic_cube", "static_table"} + assert set(dynamic_geometry) == {"dynamic_cube"} + with pytest.raises(TypeError): + all_geometry["other"] = {} # type: ignore[index] + + +def test_collision_integration_rejects_empty_dynamic_geometry() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_EmptyGeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ) + ) + + with pytest.raises(ValueError, match="scene entity 'cube'.*None"): + registry.validate_collision_integration( + _MotionGenerator( # type: ignore[arg-type] + entity_ids=("cube",), + batch_mode="shared", + ), + batch_size=1, + ) + + +def test_from_simulation_is_explicit_and_uses_uid_only_as_alias() -> None: + simulation = _Simulation() + + registry = SceneRegistry.from_simulation( + simulation, # type: ignore[arg-type] + rigid_objects={"cube": "sim_cube"}, + ) + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + assert len(registry) == 1 + assert registry.resolve("sim_cube") == SceneObjectRef("cube") + assert registry.lookup("cube").collision_role is SceneCollisionRole.NONE + assert registry.dynamic_collision_entity_ids == () + assert registry.collision_geometry_by_id() == {} + assert set(snapshot.entities) == {"cube"} + assert "ignored" not in snapshot.entities + + +def test_from_simulation_derives_live_geometry_only_for_explicit_collision_role() -> ( + None +): + simulation = _Simulation() + registry = SceneRegistry.from_simulation( + simulation, # type: ignore[arg-type] + rigid_objects={"cube": "sim_cube"}, + collision_roles={"cube": SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.SHARED, + ) + + assert registry.dynamic_collision_entity_ids == ("cube",) + assert registry.collision_geometry_by_id() == { + "cube": simulation.rigid_objects["sim_cube"] + } + + +def test_from_simulation_allows_geometry_provider_override() -> None: + registry = SceneRegistry.from_simulation( + _Simulation(), # type: ignore[arg-type] + rigid_objects={"cube": "sim_cube"}, + collision_roles={"cube": SceneCollisionRole.STATIC}, + geometry_providers={"cube": _GeometryProvider()}, + ) + + assert registry.collision_geometry_by_id() == {"cube": {"kind": "box"}} + + +def test_from_simulation_requires_selected_uid_to_exist() -> None: + with pytest.raises(KeyError, match="missing"): + SceneRegistry.from_simulation( + _Simulation(), # type: ignore[arg-type] + articulations={"drawer": "missing"}, + ) diff --git a/tests/sim/skills/test_scene_curobo_integration.py b/tests/sim/skills/test_scene_curobo_integration.py new file mode 100644 index 000000000..13facf1f3 --- /dev/null +++ b/tests/sim/skills/test_scene_curobo_integration.py @@ -0,0 +1,115 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Cross-layer CPU tests for registry-backed cuRobo obstacle identity.""" + +from __future__ import annotations + +import torch + +from embodichain.lab.sim.planners import ( + CuroboPlanOptions, + CuroboPlanner, + CuroboPlannerCfg, + CuroboWorldCfg, + MotionGenerator, +) +from embodichain.lab.sim.skills import ( + SceneCollisionRole, + SceneCollisionWorldMode, + SceneRegistry, +) + + +class _RigidObject: + """Minimal live rigid-object geometry and pose surface.""" + + def __init__(self) -> None: + self.uid = "legacy_cube" + self.pose = torch.eye(4).repeat(2, 1, 1) + + def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: + assert to_matrix is True + return self.pose + + def get_vertices( + self, + env_ids: list[int], + *, + scale: bool, + ) -> list[torch.Tensor]: + assert env_ids == [0] + assert scale is True + return [torch.zeros(8, 3)] + + def get_triangles(self, env_ids: list[int]) -> list[torch.Tensor]: + assert env_ids == [0] + return [torch.zeros(12, 3, dtype=torch.long)] + + +class _Simulation: + """Resolve one rigid object through its simulation-native UID.""" + + def __init__(self, rigid_object: _RigidObject) -> None: + self.rigid_object = rigid_object + + def get_rigid_object(self, uid: str) -> _RigidObject | None: + return self.rigid_object if uid == self.rigid_object.uid else None + + +def test_registry_id_remains_authoritative_through_curobo_binding() -> None: + rigid_object = _RigidObject() + registry = SceneRegistry.from_simulation( + _Simulation(rigid_object), # type: ignore[arg-type] + rigid_objects={"cube": rigid_object.uid}, + collision_roles={"cube": SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + mode = registry.resolve_collision_world_mode(batch_size=2) + geometry = registry.collision_geometry_by_id() + world_cfg = CuroboWorldCfg( + rigid_objects=geometry, # type: ignore[arg-type] + obstacle_representation="cuboid", + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), + multi_env=mode is SceneCollisionWorldMode.PER_ENV, + ) + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg(robot_uid="unused", world=world_cfg) + motion_generator = object.__new__(MotionGenerator) + motion_generator.planner = planner + provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=2, + ) + + snapshot = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0, 1], dtype=torch.long), + ) + obstacle_poses = snapshot.collision_obstacle_poses( + batch_size=2, + device=torch.device("cpu"), + dtype=torch.float32, + ) + bound = motion_generator.bind_collision_world( + CuroboPlanOptions(), + obstacle_poses=obstacle_poses, + ) + + assert set(world_cfg.rigid_objects or {}) == {"cube"} + assert set(obstacle_poses) == {"cube"} + assert set(bound.dynamic_obstacle_poses or {}) == {"cube"} + assert rigid_object.uid not in obstacle_poses From 7879d7aae3fecbebeedfce4d88f929bc9ab0afba Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 22:30:41 +0800 Subject: [PATCH 07/28] docs(sim): document scene registry integration --- agent_context/MAP.yaml | 40 +++ .../topics/atomic-actions/atomic-actions.md | 110 ++++++-- .../topics/motion-planning/motion-planning.md | 41 +++ .../design/declarative_expert_program_plan.md | 146 ++++++++--- .../embodichain/embodichain.lab.sim.rst | 14 +- .../embodichain.lab.sim.skills.rst | 72 ++++++ .../overview/sim/atomic_actions/index.md | 51 +++- docs/source/overview/sim/index.rst | 10 + .../overview/sim/planners/curobo_planner.md | 90 +++++-- docs/source/overview/sim/scene_registry.md | 244 ++++++++++++++++++ docs/source/tutorial/atomic_actions.rst | 39 ++- 11 files changed, 756 insertions(+), 101 deletions(-) create mode 100644 docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst create mode 100644 docs/source/overview/sim/scene_registry.md diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index b236d210c..3c26821ce 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -280,6 +280,8 @@ topics: - toppra - motion generator - trajectory planning + - curobo collision world + - dynamic collision integration - 运动生成 - 运动规划 - 轨迹规划 @@ -296,6 +298,16 @@ topics: - waypoint - velocity - acceleration + - canonical obstacle ID + - logical collision source ID + - physical YAML obstacle name + - sphere derived obstacle names + - empty collision mesh + - dynamic_collision_entity_ids + - collision_world_entity_ids + - collision_world_batch_mode + - collision_geometry_by_id + - make_planning_scene_provider paths: - topics/motion-planning/motion-planning.md source_of_truth: @@ -304,9 +316,11 @@ topics: - embodichain/lab/sim/planners/curobo/curobo_planner.py - embodichain/lab/sim/planners/curobo/curobo_yaml.py - embodichain/lab/sim/planners/motion_generator.py + - embodichain/lab/sim/skills/scene.py related_topics: - robot-system - ik-solvers + - atomic-actions status: active - id: rl-learning @@ -429,6 +443,8 @@ topics: - action primitive - object semantics - scene grounding + - scene registry + - semantic scene - AtomicAction - ActionInvocation - AtomicActionEngine @@ -453,6 +469,28 @@ topics: - SceneSnapshotSupplier - SceneProvider - RigidObjectSceneProvider + - SceneRegistry + - RegistrySceneProvider + - SceneEntityRef + - SceneObjectRef + - SceneArticulationRef + - SceneLinkRef + - SceneAffordanceRef + - SceneEntityRegistration + - SceneCollisionRole + - SceneCollisionWorldMode + - from_simulation + - validate_collision_integration + - make_planning_scene_provider + - collision_geometry_by_id + - collision_world_entity_ids + - canonical scene ID + - logical collision source ID + - sphere derived obstacle names + - empty collision mesh + - flat entity namespace + - native_name + - parent native source identity - ObjectSemantics - entity_id - frozen ObjectSemantics @@ -514,6 +552,8 @@ topics: - embodichain/lab/sim/atomic_actions/trajectory_ops.py - embodichain/lab/sim/atomic_actions/primitives/ - embodichain/lab/sim/atomic_actions/__init__.py + - embodichain/lab/sim/skills/scene.py + - embodichain/lab/sim/skills/__init__.py related_topics: - motion-planning - robot-system diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 4213c7477..80c21d65c 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -77,23 +77,81 @@ migrate an older custom action by renaming that implementation to `_plan()`. ## Object identity and pose grounding -`ObjectSemantics.entity_id` is the canonical pre-registry snapshot key. It is -optional for direct-core compatibility but, when supplied, must be a non-empty -string. Pose grounding with an explicit ID is strict: resolve it only from the -current `PlanningContext.scene`; a missing snapshot entry is an error and never -falls back to the live `entity`. Only when no ID is supplied may the core read -`ObjectSemantics.entity`; that path emits `DeprecationWarning`, reads live state, -and cannot declare a scene-motion dependency. +`ObjectSemantics.entity_id` is the typed core's canonical snapshot-key lowering +target. The registry-backed path obtains it from a resolved `SceneEntityRef`. +It remains optional for advanced direct-core compatibility but, when supplied, +must be a non-empty string. Pose grounding with an explicit ID is strict: +resolve it only from the current `PlanningContext.scene`; a missing snapshot +entry is an error and never falls back to the live `entity`. Only when no ID is +supplied may the core read `ObjectSemantics.entity`; that path emits +`DeprecationWarning`, reads live state, and cannot declare a scene-motion +dependency. `ObjectSemantics` is shallow-frozen. Top-level fields such as `entity_id`, `entity`, and `label` cannot be rebound after construction; create a new semantics value to change identity. Nested affordance and metadata objects may remain mutable, but they never establish identity. -`SceneSnapshot` owns copies of its input poses, but `EntityState.pose` tensors -are not deeply read-only. Callers must treat published snapshot values as -immutable and publish a newer scene version for changes. Enforced deep -immutability is deferred to the SceneRegistry/snapshot hardening phase. +`SceneSnapshot` owns copies of input entity states and returns a defensive +`EntityState`/pose copy on every public mapping lookup. Mutating an input tensor +or a previously returned pose cannot change the published snapshot. Publish a +new scene version for every material dynamic-state change. + +## Scene registry integration + +`embodichain.lab.sim.skills.SceneRegistry` is the canonical integration catalog. +It owns immutable registration metadata: typed identity, aliases, pose source, +parent relationships, backend-local names, dynamics, geometry, collision role, +semantic type, and affordance data. A `SceneSnapshot` does not duplicate that +catalog; it contains only versioned dynamic pose/confidence and collision +revision state. + +All object, articulation, link, and affordance IDs occupy one flat globally +unique namespace. Store link/affordance ancestry in +`SceneEntityRegistration.parent`, not by nesting or qualifying the ID. String +lookups may resolve aliases once to a canonical typed reference. An already +typed ref must contain a canonical ID and match the registered ref class. +Duplicate IDs, ambiguous aliases, alias/canonical collisions, missing parents, +and type mismatches fail at construction or lookup. Within one reference type, +the same `(parent, native_name)` physical source cannot be assigned multiple +canonical IDs; the same local name remains valid under different parents or +for different reference types. + +`SceneRegistry.from_simulation()` is explicit opt-in. Its `rigid_objects` and +`articulations` mappings are `registry_id -> simulation_uid`; selected UIDs are +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. + +`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 +canonical IDs only. The provider requires stable ordered `env_ids` and +monotonic timestamps, derives relative affordance poses from the same +observation, compares movement against the last materially published pose, and +maintains per-row collision revisions. Plain `make_scene_provider()` is only +for perception and advanced direct-core consumers without planner agreement. + +For an external perception/hardware provider, call +`registry.validate_collision_integration(..., scene_provider=provider)` +directly. The registry's complete `STATIC ∪ DYNAMIC` ID set must exactly +match `MotionGenerator.collision_world_entity_ids`; separately, the registry, +provider, and planner dynamic ID sets must match exactly in the canonical +namespace. The planner must support live updates for a non-empty dynamic set, +and planner/registry batch mode must agree. With dynamic entities, one +environment may infer `SHARED`; multiple environments must explicitly select +`SceneCollisionWorldMode.SHARED` or `PER_ENV`. + +Construct a registry-backed cuRobo world with +`registry.collision_geometry_by_id()`. Its default mapping includes only +`STATIC` and `DYNAMIC` registrations and excludes `NONE`. Mapping keys are +canonical logical/source IDs for cache identity and full-world validation. With +`cuboid` or `mesh`, they are also the physical YAML and runtime-update keys. +Static `sphere` sources expand to backend names such as `id_0`; dynamic sphere +configuration is rejected, while cache/full-world identity stays on `id`. +Registry mappings fail fast when a source lacks geometry required by the chosen +representation. List-valued cuRobo worlds and `RigidObjectSceneProvider` remain +advanced direct-core paths. Stable object identity follows these exact rules: @@ -108,11 +166,10 @@ Stable object identity follows these exact rules: to the same live entity handle. `label` is descriptive and never establishes identity. -This is a snapshot/identity bridge, not alias resolution. A future -`SceneRegistry` owns uniqueness, aliases, normalization, and authoritative -registry IDs. Partial-batch `StateDelta` attachment merges use the same stable -identity rules, so equivalent semantic wrappers update one held object instead -of creating label-based duplicates. +The direct-core identity rules do not perform alias resolution; normalization +belongs only to `SceneRegistry`. Partial-batch `StateDelta` attachment merges +use the same stable identity rules, so equivalent semantic wrappers update one +held object instead of creating label-based duplicates. For both individual and coordinated attachments, a same-identity partial merge preserves scalar metadata: if any previously active environment row remains, @@ -251,11 +308,14 @@ acknowledgement timeout in their transport/controller layer. boundary used by execution adapters. `SceneSnapshot.collision_entity_ids` identifies obstacle poses consumed by a planner, while `collision_world_revision` is either global or per environment. A newer -revision invalidates only affected batch rows. `RigidObjectSceneProvider` -tracks live simulation objects, filters sub-threshold pose noise, advances the -general scene version, and maintains per-environment collision revisions. -Thresholds are measured from the last materially published pose per entity and -environment, so repeated sub-threshold motion eventually becomes observable. +revision invalidates only affected batch rows. `RegistrySceneProvider` is the +canonical provider and derives its entity/collision sets from one immutable +`SceneRegistry`. It filters sub-threshold pose noise, advances the general scene +version, and maintains per-environment collision revisions. Thresholds are +measured from the last materially published pose per entity and environment, so +repeated sub-threshold motion eventually becomes observable. +`RigidObjectSceneProvider` retains that lower-level revision behavior for +advanced direct-core integrations. For lightweight sources that do not need environment correlation IDs, `SimulationExecutionAdapter` also accepts a mutually exclusive `SceneSnapshotSupplier(timestamp)` callback. @@ -271,6 +331,14 @@ parameters to each skill. Add/remove/geometry mutations are not yet supported by this pose-update path; providers should revision only pose-updatable registered obstacles. +`BasePlanner.collision_world_entity_ids`, `dynamic_collision_entity_ids`, and +`collision_world_batch_mode` expose the backend's complete world, dynamic +subset, and batching contract. `MotionGenerator` validates and forwards those +properties for `SceneRegistry.make_planning_scene_provider()`. External +providers call `validate_collision_integration(..., scene_provider=...)`. +These construction checks are separate from per-plan +`bind_collision_world()`. + Runnable closed-loop examples live under `scripts/tutorials/atomic_action/`: `tracking_error_recovery.py`, `moving_target_recovery.py`, and `dynamic_obstacle_recovery.py`. Each injects one disturbance, reports the diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 6b10e5524..b5edd4a8b 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -97,6 +97,19 @@ Learning-based EEF waypoint planner. Franka Panda only. ### CuroboPlanner collision worlds +`CuroboWorldCfg.rigid_objects` accepts either a mapping or a sequence. Use +`Mapping[registry_id, RigidObject]` for a registry-backed integration. The +mapping key is the authoritative logical/source obstacle ID used by the +content-cache key, `collision_world_entity_ids`, and registry validation. For +`cuboid` and `mesh`, it is also the physical YAML obstacle name and dynamic +update key. For `sphere`, one static source expands to physical YAML names such +as `registry_id_0`; dynamic sphere configuration is rejected, while cache and +full-world identity remain keyed by `registry_id`. A registry mapping whose +source lacks mesh geometry required by the selected representation fails fast +instead of silently dropping the source. The sequence form is an advanced +direct-core path that derives names from each object's `uid` or an +`obstacle_` fallback. + `CuroboWorldCfg.multi_env` controls collision-world batching, not whether robot states or goals are batched: @@ -126,10 +139,33 @@ them into `CuroboPlanOptions.dynamic_obstacle_poses`. to the backend hook. Atomic actions use that facade from their framework-owned `plan()` template when a `SceneSnapshot` declares collision entities; individual skills must not construct backend obstacle options themselves. +`BasePlanner.collision_world_entity_ids`, `dynamic_collision_entity_ids`, and +`collision_world_batch_mode` expose the complete canonical world, its dynamic +subset, and the `"shared"` / `"per_env"` mode. `MotionGenerator` validates and +forwards those properties to the integration layer. For cuRobo, the complete +set is every mapping key (or inferred sequence name), while the dynamic set is +exactly `CuroboWorldCfg.dynamic_obstacle_names`. Sphere-expanded physical YAML +names are not part of either logical ID declaration. `CuroboWorldCfg` rejects duplicate obstacle names and requires every `dynamic_obstacle_name` to match an object registered in `rigid_objects`, so a planner-local mismatch fails before backend construction. +For the canonical path, pass `SceneRegistry.collision_geometry_by_id()` into +`CuroboWorldCfg.rigid_objects`, derive dynamic names from the registry, and call +`SceneRegistry.make_planning_scene_provider(motion_generator, batch_size=...)` +before execution. The geometry mapping excludes `NONE` registrations. The +factory first requires the registry's complete `STATIC ∪ DYNAMIC` set to +equal `MotionGenerator.collision_world_entity_ids`, then requires exact +registry/derived-provider/planner dynamic-subset agreement. It also checks +update capability for a non-empty dynamic set and the same collision-world +batch mode. An external perception/hardware provider instead uses +`validate_collision_integration(..., scene_provider=provider)`. + +One environment may infer `SHARED`; a multi-environment registry with dynamic +entities must explicitly choose `SHARED` or `PER_ENV`. Alias normalization +happens before planner construction, so planner IDs must never be simulator +UIDs unless that string is also the chosen canonical registry ID. + `MotionGenerator.resolve_plan_options()` is the corresponding option-ownership boundary. It copies caller-supplied typed options, otherwise obtains backend defaults; for TOPPRA it maps the requested sample count and generic @@ -271,3 +307,8 @@ The decorator checks that every `PlanState` in `target_states` shares the same l - **Fork safety with GPU sim** — `ToppraPlannerCfg.mp_context=None` defaults to `spawn` on GPU to avoid fork-after-CUDA-init hazards. Force `fork` only when the sim device is CPU or you have verified it is safe. - **cuRobo shared-world mismatch** — World-frame poses may differ solely because replicated arenas are offset. Compare poses after robot-base rebasing: keep `multi_env=False` if they match, and enable it only when robot-relative layouts differ. - **Dynamic obstacles silently stale** — A planner participates in atomic-action collision revision recovery only when it declares `supports_collision_world_updates=True`; its hook must bind every `collision_entity_id` pose into the current planning attempt. +- **Registry/planner identity drift** — Registry-backed cuRobo worlds must use a + canonical-ID mapping, not a list whose names are inferred from UIDs. Validate + exact full registry/planner collision-world agreement, dynamic + registry/provider/planner agreement, and batch-mode agreement through + `SceneRegistry` before starting execution. diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 21510e7b4..1bee119b8 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,6 +1,7 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: design plan +- Status: implementation in progress; Phase 0 and PR1 complete, PR2A implemented + on the feature branch - Baseline: `main@e445133c79c8b32019dab1c844b799b43a1658d6` - Last updated: 2026-08-10 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), @@ -90,8 +91,11 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is updated against committed `main@e445133c` after PR #475 rather -than uncommitted working-tree changes. +This plan is updated against committed `main@e445133c` after PR #475. The +implementation series is stacked from that baseline: PR1 is complete on +`refactor/atomic-actions-phase0`, and PR2A is implemented by the +`feat/atomic-action-pr2a-scene-registry` change. Neither status statement +implies that the stacked changes have landed on `main`. | Capability | Current main | Design consequence | |---|---|---| @@ -114,7 +118,7 @@ hard break: the project will not provide a compatibility adapter or deprecation window for that former extension contract. Custom actions must migrate to `_plan()` so framework-owned scene binding cannot be bypassed. -The remaining #474 prerequisites on this baseline are: +The remaining #474 prerequisites on `main` are: - scene pose, semantics, affordance, and collision registration still have multiple sources of truth; @@ -126,6 +130,12 @@ The remaining #474 prerequisites on this baseline are: - `MotionPolicy` still exposes implementation-level tuning that should be hidden behind semantic presets for ordinary users. +PR1 closes the snapshot-grounding and stable-identity bridge. PR2A closes the +first and third gaps for registry-backed integrations by introducing one +authoritative registration boundary, a registry-derived scene provider, and +construction-time collision-world validation. The semantic facade and named +presets remain later-phase work. + One #474 finding has changed since its review branch: the ambiguous `collision_check` switch has been replaced by `DynamicCollisionMode.OFF`, `AUTO`, and `REQUIRED` on current main. The semantic preset layer should build @@ -287,26 +297,61 @@ Rules: registry ID; they never replace the authoritative ID. Duplicate registry IDs, ambiguous aliases, or an alias colliding with another registry ID fail during registry construction. -3. Grounding reads pose and geometry from one immutable snapshot. It must not - mix a snapshot with a live simulation entity pose. + For links and affordances, one typed `(parent, native_name)` physical source + may have only one canonical ID; changing the canonical spelling does not + create a second entity. +3. Grounding reads dynamic pose/confidence from one immutable snapshot and + static geometry/affordance metadata from the immutable registry that + produced it. It must not mix a snapshot with a live simulation entity pose. 4. Automatic grasp selection declares a target dependency automatically. -5. Dynamic collision setup is derived from authoritative registry IDs. Registry - construction performs the complete provider/planner cross-validation: the - registry's dynamic-collision ID set, the provider's `collision_entity_ids`, - and the planner's dynamic-obstacle names must agree after alias normalization; - every ID must have the required geometry, and the selected planner must - support the declared update mode. The current +5. Collision setup is derived from authoritative registry IDs. + `collision_geometry_by_id()` derives planner geometry while excluding + non-collision registrations, and `make_planning_scene_provider()` performs + the complete provider/planner cross-validation. The registry's full + `STATIC ∪ DYNAMIC` collision ID set must exactly equal the planner's complete + collision-world ID set. Within it, the registry's dynamic subset, the + provider's `collision_entity_ids`, and the planner's dynamic-obstacle IDs must + also agree exactly. Every collision ID must have the required geometry, and + the selected planner must support the declared dynamic update mode. Aliases + are resolved before these contracts are constructed, never inside the + planner. The current planner-local name check remains a lower-level defensive validation, not the integration contract. 6. The `safe` preset requests `DynamicCollisionMode.REQUIRED` when the registry declares dynamic collision entities and fails early if the active planner cannot satisfy it. -7. Environment scene configuration should populate the registry automatically; - explicit providers are reserved for perception and hardware integration. - -Before PR2A introduces this registry, PR1 provides only a core migration bridge. -`ObjectSemantics.entity_id` is a caller-supplied `SceneSnapshot` key, not yet a -registry reference. `ObjectSemantics` is shallow-frozen so top-level fields, +7. Environment scene configuration opts into registry population explicitly; + it is not inferred by scanning the simulation. Explicit providers remain + available for perception and hardware integration. + +PR2A fixes three public identity and collision-world choices: + +- **Authoritative planner IDs:** registry-backed cuRobo configuration passes an + explicit `registry_id -> RigidObject` mapping derived by + `collision_geometry_by_id()`. Mapping keys are canonical logical/source IDs + for cache identity and the complete registry/planner collision-world + contract. Cuboid and mesh worlds also use them unchanged as physical YAML + obstacle names and runtime pose-update keys. A static sphere source expands + to derived physical names such as `registry_id_0`; dynamic sphere worlds are + rejected. A registry mapping with geometry missing for the selected + representation fails fast instead of silently omitting that source. The list + form remains an advanced direct-core path and continues to derive names from + simulator UIDs or fallback names. +- **Flat reference IDs:** object, articulation, link, and affordance IDs share + one globally unique flat namespace. Link and affordance ancestry is stored in + `SceneEntityRegistration.parent`; callers do not encode hierarchy into an ID. + Within one reference type, the same `(parent, native_name)` cannot be assigned + more than one canonical ID. +- **Explicit vectorized-world semantics:** one-environment dynamic collision + setup may infer a shared collision world. A multi-environment registry with + dynamic collision entities must explicitly select shared or per-environment + collision worlds, and integration validation requires the planner mode to + match that selection. + +PR1 provides the core migration bridge consumed by the registry. +`ObjectSemantics.entity_id` remains a string lowering target in the typed core; +the canonical semantic path obtains that value from a resolved +`SceneEntityRef`. `ObjectSemantics` is shallow-frozen so top-level fields, including `entity_id`, cannot be rebound after attachment state captures the semantics; identity changes require a new instance. Nested affordance and metadata objects remain mutable but never establish identity. @@ -329,11 +374,14 @@ The same boundary applies to `AssembleGoal.base_pose`: the snapshot reference is canonical, while an omitted reference permits the deprecated direct-core `AssembleAffordance.base_object_entity` path. -The current `SceneSnapshot` owns copies of input pose tensors, but exposed -`EntityState.pose` tensors are not deeply read-only. PR1 therefore requires -callers to treat snapshot values as immutable and uses the scene version for -publication/recovery semantics. Enforced deep immutability belongs to the PR2A -registry/snapshot hardening rather than this bridge. +PR2A hardens `SceneSnapshot` at the public boundary. Construction owns a copy of +every dynamic `EntityState`, and every public entity lookup returns a defensive +copy, so mutating an input state or a previously returned pose cannot mutate the +published snapshot. The registry continues to own static integration metadata, +including typed identity, aliases, parent relationships, geometry, collision +role, dynamics classification, semantic type, and affordances. A snapshot owns +only versioned dynamic pose/confidence plus collision revision metadata; it does +not duplicate the registration catalog. ### 7.2 Robot skill profiles @@ -735,11 +783,12 @@ tests. The dependency order is: Phase 0 correctness (complete) | v -PR1 snapshot/identity bridge +PR1 snapshot/identity bridge (complete) | +-----------------------+ v v PR2A SceneRegistry PR2B RobotSkillProfile + (implemented) (next) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -778,11 +827,11 @@ Landed on `main` through #475: core check. Complete provider/planner cross-validation is deliberately owned by the authoritative `SceneRegistry` integration in Phase 1. -Exit criteria are met on `main@e445133c`. Implementation may proceed to the -focused PR1 bridge without adding a legacy `plan()` adapter or a pre-registry -duplicate of the integration-level obstacle validator. +Exit criteria are met on `main@e445133c`; this remains the foundation for the +completed PR1 bridge. That bridge adds neither a legacy `plan()` adapter nor a +pre-registry duplicate of the integration-level obstacle validator. -### PR1: core snapshot and identity bridge +### PR1: core snapshot and identity bridge (complete) PR1 is deliberately smaller than Phase 1. It establishes the core seams that the later registry and profile integrations consume: @@ -822,17 +871,18 @@ cross-source uniqueness or collision validation, a `RobotSkillProfile`, or semantic presets. It does not require official task environments to migrate; they remain on the compatibility path until a later opt-in vertical slice. -Exit criteria: canonical object grounding never mixes snapshot and live poses; -explicit missing IDs fail; dependency metadata matches the poses actually -consumed; stable-identity merges are deterministic; and existing direct-core -callers remain usable only through the documented deprecated fallbacks. +Exit criteria are met on `refactor/atomic-actions-phase0`: canonical object +grounding never mixes snapshot and live poses; explicit missing IDs fail; +dependency metadata matches the poses actually consumed; stable-identity merges +are deterministic; and existing direct-core callers remain usable only through +the documented deprecated fallbacks. ### Phase 1: unified integration data Phase 1 is implemented as two focused follow-up PRs that join before the semantic facade/compiler work. -#### PR2A: SceneRegistry +#### PR2A: SceneRegistry (implemented on the feature branch) Deliverables: @@ -842,16 +892,35 @@ Deliverables: - immutable snapshots as the only grounding pose authority for the canonical semantic/compiler path; - opt-in environment-to-registry population and collision/provider derivation; -- complete construction-time agreement checks across registry collision IDs, - provider collision IDs, planner dynamic-obstacle names, geometry, and planner - capability; +- complete construction-time agreement checks between registry and planner + full collision-world IDs, plus registry/provider/planner dynamic subsets, + geometry, mode, and planner capability; - explicit catalog-discovery versus engine-installation terminology. +The implemented scope also records the A+C+E decisions from the PR2A API +review: + +- registry-backed cuRobo worlds use a canonical-ID mapping, while the list form + remains an advanced direct-core escape hatch; +- all reference IDs are globally unique and flat, with link/affordance parent + relations retained only by their registrations; +- one-environment dynamic worlds may default to shared, while vectorized + dynamic worlds require an explicit shared/per-environment choice. + `ObjectSemantics.entity_id` and `AssembleGoal.base_pose` already provide the lowering targets from PR1. PR2A replaces manually coordinated IDs/providers with one authoritative registration and performs alias normalization exactly once at the integration boundary. +PR2A exit criteria are met by the feature change: registry-ID and alias +collisions fail at construction, typed lookups cannot silently change entity +kind, one typed parent/native physical source cannot be registered twice, +registry-derived snapshots contain only canonical keys, independent providers +keep independent revision state, full registry/planner collision-world IDs and +dynamic registry/provider/planner subsets are validated before execution, the +collision-world batch mode agrees, and cuRobo uses canonical mapping keys end +to end as logical source IDs (and as physical keys for cuboid/mesh worlds). + #### PR2B: RobotSkillProfile Deliverables: @@ -988,8 +1057,9 @@ independent of adoption of the new path. - strict decoder, unknown fields, schema versioning, bounded repeats, and registry reference errors; -- authoritative registry-ID normalization, legacy-`uid` alias collisions, and - complete registry/provider/planner obstacle-set agreement; +- authoritative registry-ID normalization, legacy-`uid` alias collisions, + typed parent/native-source collisions, complete registry/planner collision- + world agreement, and registry/provider/planner dynamic-subset agreement; - cumulative scene movement and collision dependency revision behavior; - profile capability matching, deterministic binding, and ambiguity errors; - `AssembleGoal.base_pose` snapshot resolution and its automatic scene diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.rst index 716df56ef..0f1390158 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.rst @@ -10,9 +10,9 @@ The ``sim`` package is EmbodiChain's simulation core. It is organized around the :class:`SimulationManager` (the DexSim scene handle), the scene-object hierarchy (lights, rigid/soft/cloth bodies, articulations, robots, gizmos, constraints), the sensor suite (cameras, stereo cameras, contact sensors), IK -solvers and motion planners, the atomic-action motion-primitive layer, a -reusable workspace-analysis and sampling toolkit, and the shared configuration -types and utilities that wire all of these together. +solvers and motion planners, the semantic scene registry, the atomic-action +motion-primitive layer, a reusable workspace-analysis and sampling toolkit, and +the shared configuration types and utilities that wire all of these together. .. rubric:: Submodules @@ -132,6 +132,14 @@ Planners embodichain.lab.sim.planners +Semantic Scene Integration +-------------------------- + +.. toctree:: + :maxdepth: 1 + + embodichain.lab.sim.skills + Atomic Actions -------------- diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst new file mode 100644 index 000000000..44ac3c38c --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -0,0 +1,72 @@ +embodichain.lab.sim.skills +========================== + +.. automodule:: embodichain.lab.sim.skills + + .. rubric:: Scene integration contracts + + .. autosummary:: + + SceneRegistry + RegistrySceneProvider + SceneEntityRegistration + SceneEntityRef + SceneObjectRef + SceneArticulationRef + SceneLinkRef + SceneAffordanceRef + SceneEntityStateProvider + SceneGeometryProvider + SceneDynamics + SceneCollisionRole + SceneCollisionWorldMode + +.. currentmodule:: embodichain.lab.sim.skills + +Registry and provider +--------------------- + +.. autoclass:: SceneRegistry + :members: + +.. autoclass:: RegistrySceneProvider + :members: + +Registration contracts +---------------------- + +.. autoclass:: SceneEntityRegistration + :members: + +.. autoclass:: SceneEntityStateProvider + :members: + +.. autoclass:: SceneGeometryProvider + :members: + +References and enums +-------------------- + +.. autoclass:: SceneEntityRef + :members: + +.. autoclass:: SceneObjectRef + :members: + +.. autoclass:: SceneArticulationRef + :members: + +.. autoclass:: SceneLinkRef + :members: + +.. autoclass:: SceneAffordanceRef + :members: + +.. autoclass:: SceneDynamics + :members: + +.. autoclass:: SceneCollisionRole + :members: + +.. autoclass:: SceneCollisionWorldMode + :members: diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index c4b2f7ab2..331cff6b4 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -33,8 +33,8 @@ and whole-body control are not implemented by this module yet. +---------------+----------------+ +---------------+----------------+ | | v | - agent adapter: schema validation, | - scene grounding, capability binding | + semantic adapter: schema validation, | + SceneRegistry grounding, capability binding | | | +------------------+------------------+ | @@ -75,11 +75,11 @@ The boundary is deliberate: |---|---|---| | Task intent and sequencing | Action Agent, task graph, or user-authored application | Selects skills, goals, and execution order | | Invocation construction | Agent adapter or user-authored code/config loader | Produces the same typed `ActionInvocation`; the engine has no agent-only interface | -| Perception and grounding | Agent adapter or user application | Builds scene snapshots and resource bindings, or supplies already-grounded values directly | +| Perception and grounding | `SceneRegistry` on the canonical path; adapter or user application on the advanced path | Normalizes aliases to canonical typed references and publishes snapshots, or supplies already-grounded values directly | | Deterministic motion planning | Atomic action module | Produces an `ActionPlan` from an invocation and context | | Motion-generation resources | `AtomicActionEngine` | Owns one robot, motion generator, planner backend, device, trajectory builder, and control-part command profiles | | Recovery state | `ExecutionSession` | Consumes fresh contexts, emits at most one `JointCommand` per tick, and owns bounded recovery/revision state | -| Scene observation | `SceneProvider` | Captures ordered entities plus monotonic global or per-environment collision-world revisions | +| 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`, `CommandSink`, and `ExecutionClock` adapters | Isolates observation, command transport, and time/physics advancement from planning and session state | | Physical-effect verification | Application observer | Verifies grasp, release, handover, and other symbolic effects | @@ -317,6 +317,10 @@ entity poses from the current `SceneSnapshot` into copied backend options, then calls the skill-specific `_plan()` hook. Individual skills therefore do not own dynamic-obstacle parameters or mutate caller-owned motion policies. +For a canonical integration, construct the snapshot provider and collision +world from one {doc}`SceneRegistry <../scene_registry>`. Direct use of +`RigidObjectSceneProvider` remains an advanced-core path. + Registration means that an implementation is installed, not that every robot can execute it. Required roles, control parts, profiles, and task-state preconditions are validated while an invocation is resolved and planned. Agent @@ -512,7 +516,8 @@ while session.status is ExecutionStatus.RUNNING: ``` For most applications, use `ExecutionRunner` to keep scheduling and controller -acknowledgement handling outside the session: +acknowledgement handling outside the session. The following snippet shows the +advanced direct-core provider path: ```python scene_provider = RigidObjectSceneProvider({"moving_tray": moving_tray}) @@ -568,17 +573,38 @@ per-environment action scheduling belongs in a higher-level scheduler rather tha this atomic-action session. `SceneProvider.snapshot(timestamp=..., env_ids=...)` is the scene-observation -boundary. `SceneSnapshot.collision_entity_ids` identifies obstacle poses -consumed by a planner, while `collision_world_revision` can be global or -per-environment. `RigidObjectSceneProvider` tracks live simulation objects, -filters sub-threshold pose noise, and advances those revisions. Its threshold -baseline is the last materially published pose for each entity/environment, so -cumulative sub-threshold motion cannot remain hidden indefinitely. Backends opt -in through `supports_collision_world_updates` and `with_collision_world()`; +boundary. On the canonical planning path, +`SceneRegistry.make_planning_scene_provider()` derives an independent provider +and eagerly validates its collision contract against the motion generator. +Its snapshots expose canonical registry IDs only. +The registry owns static identity, aliases, geometry, affordances, hierarchy, +and collision roles; `SceneSnapshot` owns versioned dynamic pose/confidence and +collision revisions. Snapshot states are defensively copied on construction and +public read. + +`SceneSnapshot.collision_entity_ids` identifies obstacle poses consumed by a +planner, while `collision_world_revision` can be global or per-environment. +Registry-derived providers filter sub-threshold pose noise and advance those +revisions from the last materially published pose, so cumulative motion cannot +remain hidden indefinitely. Backends opt in through +`supports_collision_world_updates` and `with_collision_world()`; `MotionGenerator.bind_collision_world()` owns that backend boundary, and cuRobo maps the snapshot poses to `CuroboPlanOptions.dynamic_obstacle_poses`. A newer revision invalidates only affected rows before synchronized cohort replanning. +`make_planning_scene_provider()` requires two exact canonical-ID agreements: +the registry's complete `STATIC ∪ DYNAMIC` set must equal the planner's +complete collision-world set, and the registry, derived provider, and planner +dynamic subsets must equal one another. It also requires planner update support +for a non-empty dynamic subset and matching shared or per-environment world +semantics. A one-environment registry may infer `SHARED`; a multi-environment +dynamic registry must choose `SHARED` or `PER_ENV` explicitly. External +perception/hardware providers use +`validate_collision_integration(..., scene_provider=...)` directly. Plain +`make_scene_provider()` and `RigidObjectSceneProvider` are perception or +advanced direct-core paths without eager planner agreement. See +{doc}`../scene_registry` for setup. + `MotionPolicy.dynamic_collision_mode` controls this live-scene path. `AUTO` (the default) consumes collision entities when the selected motion strategy and planner support them, `OFF` ignores snapshot collision entities and their @@ -713,6 +739,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}`../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/index.rst b/docs/source/overview/sim/index.rst index 168fad8db..e7aa753b5 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -43,6 +43,8 @@ The simulation stack can be read from the bottom up: |-- planners | |-- joint-space and Cartesian trajectory generation | `-- time parameterization and sampling utilities + |-- scene registry + | `-- canonical semantic identity, snapshots, and collision integration `-- atomic actions `-- reusable manipulation primitives built from assets, solvers, and planners @@ -84,6 +86,11 @@ Submodule Relationships timing, and feasibility handling. - Use robot state and solver results to produce trajectories that can be replayed in the manager loop. + * - Scene registry + - Owns canonical typed entity IDs, aliases, pose sources, geometry, + affordances, hierarchy, and collision roles. + - Publishes registry-derived snapshots for atomic actions and validates + dynamic collision-world agreement with planners. * - Atomic actions - Package complete manipulation primitives such as move, pick, and place. - Compose semantic targets, solvers, planners, and robot control into @@ -120,6 +127,8 @@ Choosing Where to Start kinematics. - Use :doc:`planners/index` when a target pose or joint goal must become a time-ordered trajectory. +- Use :doc:`scene_registry` when semantic calls, snapshots, and planner + obstacles must share one authoritative entity namespace. - Use :doc:`atomic actions ` when building scripted manipulation from reusable motion primitives. @@ -145,4 +154,5 @@ See Also viser_visualization.md solvers/index planners/index + scene_registry.md atomic_actions/index diff --git a/docs/source/overview/sim/planners/curobo_planner.md b/docs/source/overview/sim/planners/curobo_planner.md index 24f3193c1..72a512290 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -59,6 +59,9 @@ locks both fingers at `0.04`, so use the same simulated finger state or include the fingers in the planned control part. A mismatch means cuRobo validates a different collision geometry from the one replayed in DexSim. +Assuming the scene has been registered as shown in +{doc}`../scene_registry`, construct the planner world from that catalog: + ~~~python from embodichain.lab.sim.planners import ( CuroboPlannerCfg, @@ -66,13 +69,27 @@ from embodichain.lab.sim.planners import ( MotionGenCfg, MotionGenerator, ) +from embodichain.lab.sim.skills import SceneCollisionWorldMode + +collision_mode = registry.resolve_collision_world_mode( + batch_size=robot.num_instances, +) planner_cfg = CuroboPlannerCfg( robot_uid="my_franka", planner_type="curobo", - world=CuroboWorldCfg(rigid_objects=[demo_block]), + world=CuroboWorldCfg( + rigid_objects=registry.collision_geometry_by_id(), + obstacle_representation="cuboid", + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), + multi_env=collision_mode is SceneCollisionWorldMode.PER_ENV, + ), ) motion_generator = MotionGenerator(MotionGenCfg(planner_cfg=planner_cfg)) +scene_provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, +) ~~~ cuRobo's Python logger defaults to error-only output. Set @@ -135,26 +152,55 @@ second one-time warmup and its graph-resident memory, but still no subprocess or second CUDA context. The collision world is always auto-generated from live `RigidObject` meshes via -`CuroboWorldCfg.rigid_objects`: the adapter reads each object's mesh -(`get_vertices` / `get_triangles`) and world pose (`get_local_pose`) and writes a -cached cuRobo scene YAML on the first plan, using +`CuroboWorldCfg.rigid_objects`. The canonical, registry-backed form is a mapping +from authoritative registry ID to live object; the adapter reads each object's +mesh (`get_vertices` / `get_triangles`) and world pose (`get_local_pose`) and +writes a cached cuRobo scene YAML on the first plan, using `CuroboWorldCfg.obstacle_representation` (`"sphere"` by default for fast collision queries; use `"cuboid"` for a local-frame AABB placed as an OBB via the object pose, or `"mesh"` for the exact triangle mesh). Generated poses are authored in the cuRobo base/world frame, so this is exact -when the robot base sits at the simulator world origin. For obstacles that move -or live in an offset base frame, also declare their names in +when the robot base sits at the simulator world origin. The mapping key, rather +than `RigidObject.uid`, is the canonical logical/source ID used by cache +identity and collision-world validation. For `"cuboid"` and `"mesh"`, that ID +is also used unchanged as the physical YAML obstacle name and runtime update +key. For obstacles that move or live in an offset base frame, also declare their +canonical IDs in `CuroboWorldCfg.dynamic_obstacle_names` and update poses at plan time through `CuroboPlanOptions.dynamic_obstacle_poses` (provision `CuroboWorldCfg.collision_cache` before planning). Dynamic updates require the `"cuboid"` or `"mesh"` representation because sphere fitting expands one object -into multiple independently named obstacles. +into physical YAML obstacles named `_0`, `_1`, and +so on; dynamic sphere configuration is rejected. These derived names are +backend details. The cache and registry/planner full-world contract continue to +use the unexpanded canonical source ID. + +Registry-backed mappings fail fast if a selected source has no mesh geometry +required by the chosen representation. This prevents a canonical collision ID +from being silently skipped during YAML generation. The advanced sequence form +retains its lower-level behavior independently of this registry contract. + +`CuroboPlanner.collision_world_entity_ids` reports every configured logical +source ID: each mapping key on the registry path, or each inferred name on the +advanced sequence path. It deliberately does not expose sphere-expanded +physical YAML names. `dynamic_collision_entity_ids` reports exactly the +configured dynamic subset. Static entries therefore participate in +construction-time identity validation even though they do not receive per-plan +pose updates. `CuroboWorldCfg` validates this planner-local registration at construction: -obstacle names must be unique, and every dynamic obstacle name must match the -`uid` (or generated fallback name) of an entry in `rigid_objects`. The later -`SceneRegistry` integration additionally cross-validates those names with the -scene provider rather than duplicating them in task code. +obstacle IDs must be unique, and every dynamic obstacle ID must match an entry +in `rigid_objects`. A sequence of objects is retained only as an advanced +direct-core path; it derives names from each `uid` or an `obstacle_` +fallback. Do not use that form for a registry-backed world. + +The {doc}`../scene_registry` integration performs two higher-level checks before +execution. First, all registry `STATIC ∪ DYNAMIC` IDs must exactly equal +`MotionGenerator.collision_world_entity_ids`. Second, registry, derived scene +provider, and planner dynamic-ID subsets must exactly agree. The planner must +also support pose updates and its shared/per-environment batch mode must agree +with the registry. Aliases are normalized at the registry boundary; cuRobo +never translates a canonical ID back to a simulator UID. ### Shared and per-environment collision worlds @@ -188,21 +234,25 @@ pose differs by environment must also: 3. Have its current `(B, 4, 4)` simulator-world poses passed through `CuroboPlanOptions.dynamic_obstacle_poses` when planning. -For example: +For a registry-backed world, derive both the geometry mapping and dynamic ID +list from the same catalog: ```python world_cfg = CuroboWorldCfg( - rigid_objects=[block], + rigid_objects=registry.collision_geometry_by_id(), obstacle_representation="cuboid", - dynamic_obstacle_names=["block"], + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), multi_env=True, ) +current_snapshot = scene_provider.snapshot(timestamp=now, env_ids=env_ids) plan_options = CuroboPlanOptions( control_part="arm", - dynamic_obstacle_poses={ - "block": block.get_local_pose(to_matrix=True), # (B, 4, 4) - }, + dynamic_obstacle_poses=current_snapshot.collision_obstacle_poses( + batch_size=robot.num_instances, + device=robot.device, + dtype=robot.get_qpos().dtype, + ), ) ``` @@ -213,6 +263,12 @@ does not insert new geometry at runtime. Independent worlds replicate scene data and collision caches across the batch, so retain the shared default when the rebased layouts are identical. +For a registry-backed integration, a single-environment dynamic world may infer +the registry's shared mode. A multi-environment registry with dynamic collision +entities must explicitly choose shared or per-environment semantics, then set +`multi_env=False` or `True` to match. The registry validator rejects a mismatch +before planning. + (curobo-auto-generated-robot-yaml)= ## Auto-generated robot YAML diff --git a/docs/source/overview/sim/scene_registry.md b/docs/source/overview/sim/scene_registry.md new file mode 100644 index 000000000..74f693c15 --- /dev/null +++ b/docs/source/overview/sim/scene_registry.md @@ -0,0 +1,244 @@ +(scene-registry)= + +# Scene registry + +```{currentmodule} embodichain.lab.sim.skills +``` + +`SceneRegistry` is the canonical integration boundary between semantic scene +identity, atomic-action snapshots, and planner collision worlds. Register an +entity once under an authoritative ID, resolve external names at that boundary, +then use only the canonical ID in semantic calls, snapshots, dependencies, and +dynamic-obstacle configuration. + +The registry is an immutable catalog. A {class}`RegistrySceneProvider` created +from it owns changing observation state, publication baselines, and revisions. +This separation lets multiple runtimes share one catalog without sharing their +revision counters. + +## What the registry owns + +Each {class}`SceneEntityRegistration` contains static integration metadata: + +- a typed canonical reference; +- aliases for simulator, perception, or hardware names; +- an explicit pose/confidence provider; +- optional parent and backend-local name; +- dynamics and planner collision role; +- optional geometry, semantic type, and affordance data. + +A `SceneSnapshot` contains only versioned dynamic pose/confidence values and +collision-world revisions. Snapshot construction copies every entity state, and +each public entity lookup returns a defensive copy. Mutating an original tensor +or a previously returned value therefore cannot change a published snapshot. + +References use one flat, globally unique namespace: + +```text +SceneEntityRef ++-- SceneObjectRef ++-- SceneArticulationRef ++-- SceneLinkRef +`-- SceneAffordanceRef +``` + +Do not encode hierarchy into link or affordance IDs. Store ancestry in +`SceneEntityRegistration.parent` and the backend-local member name in +`native_name`. A link parent must be an articulation; an affordance parent may +be an object, articulation, or link. The registry rejects duplicate canonical +IDs, ambiguous aliases, aliases that collide with another canonical ID, +unregistered parents, and typed-reference mismatches. Within one reference +type, a `(parent, native_name)` pair identifies one physical source and cannot +be registered under multiple canonical IDs. The same local name may still be +used under different parents or by different reference types. + +String lookups may use an alias and are normalized once: + +```python +cube = registry.resolve("sim_cube", expected_type=SceneObjectRef) +assert cube.entity_id == "cube" +``` + +An already typed reference is expected to contain a canonical ID. It cannot use +an alias or silently change entity kind. + +## Explicit simulation opt-in + +Use {meth}`SceneRegistry.from_simulation` to select simulator entities +explicitly. Mapping keys are authoritative registry IDs and values are existing +simulation UIDs. The UIDs are installed as legacy aliases; unlisted simulation +entities are not scanned or imported. + +```python +from embodichain.lab.sim.skills import SceneObjectRef, SceneRegistry + +registry = SceneRegistry.from_simulation( + sim, + rigid_objects={ + "cube": "sim_cube", + "tray": "task_tray_0", + }, + articulations={"drawer": "cabinet_articulation"}, +) + +cube = registry.resolve("cube", expected_type=SceneObjectRef) +assert registry.resolve("sim_cube", expected_type=SceneObjectRef) == cube +``` + +For perception or hardware, construct registrations with an implementation of +{class}`SceneEntityStateProvider` instead. Collision registrations also require +a {class}`SceneGeometryProvider`; the geometry belongs to the catalog even +though the snapshot contains only its current pose and confidence. + +## Publish canonical snapshots + +For an atomic-action planning runtime, create the provider through +{meth}`SceneRegistry.make_planning_scene_provider` and pass it to +`SimulationExecutionAdapter`: + +```python +provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, +) +adapter = SimulationExecutionAdapter( + sim, + robot, + scene_provider=provider, +) +``` + +This factory constructs a fresh provider and eagerly validates the complete +registry/provider/planner collision contract. Use +{meth}`SceneRegistry.make_scene_provider` only for perception and advanced +direct-core consumers that do not need planner agreement. Every factory call +returns an independent provider. Its snapshots contain canonical registry IDs +only; aliases never leak into `SceneSnapshot.entities` or +`collision_entity_ids`. + +The provider observes entities in the supplied `env_ids` order. Those IDs must +remain stable and ordered for the provider lifetime, and timestamps must be +monotonic. Translation and rotation thresholds are measured from the last +materially published pose per entity and environment, so repeated +sub-threshold motion eventually publishes a new scene version. Dynamic +collision entities additionally advance per-environment collision revisions. + +Parent-relative affordances are derived from the parent pose inside the same +observation. Their static relative transforms remain registry metadata. + +## Validate the complete collision world + +Collision setup has one canonical namespace. For a registry-backed +cuRobo world, derive both the explicit `registry_id -> RigidObject` mapping and +the dynamic-obstacle ID list from the registry: + +```python +from embodichain.lab.sim.planners import ( + CuroboPlannerCfg, + CuroboWorldCfg, + MotionGenCfg, + MotionGenerator, +) +from embodichain.lab.sim.skills import ( + SceneCollisionRole, + SceneCollisionWorldMode, + SceneRegistry, +) + +registry = SceneRegistry.from_simulation( + sim, + rigid_objects={"cube": "sim_cube"}, + collision_roles={"cube": SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.PER_ENV, +) + +world = CuroboWorldCfg( + rigid_objects=registry.collision_geometry_by_id(), + obstacle_representation="cuboid", + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), + multi_env=True, +) +motion_generator = MotionGenerator( + MotionGenCfg( + planner_cfg=CuroboPlannerCfg( + robot_uid=robot.uid, + planner_type="curobo", + world=world, + ) + ) +) + +provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, +) +``` + +`collision_geometry_by_id()` derives the cuRobo mapping from the catalog. By +default it includes only `STATIC` and `DYNAMIC` registrations and excludes +`NONE`; an optional exact role filter is available when a backend needs one +subset. `from_simulation()` automatically exposes a selected live rigid object +as its geometry source. Articulations and manually constructed collision +registrations still need an appropriate explicit geometry provider. + +The registry validator checks two nested identity contracts before execution: + +1. The registry's complete `STATIC ∪ DYNAMIC` collision ID set exactly equals + `MotionGenerator.collision_world_entity_ids`. This rejects a missing static + obstacle as well as planner geometry not owned by the registry. +2. The registry's `DYNAMIC` subset exactly equals both the provider's + `collision_entity_ids` and + `MotionGenerator.dynamic_collision_entity_ids`. +3. Every ID in that complete collision world has materialized registered + geometry. +4. The planner supports dynamic collision-world updates when that subset is + non-empty. +5. The planner's shared/per-environment mode equals the registry mode. + +Every collision registration has already proved that geometry exists. Planner +IDs are canonical logical/source IDs, not aliases. For cuRobo `cuboid` and +`mesh` worlds, each mapping key is also the physical YAML obstacle key and the +runtime pose-update key. A `sphere` world instead expands one canonical source +ID into physical YAML names such as `cube_0`, `cube_1`, and so on. Those derived +names are backend details: cache identity and the full-world contract remain +keyed by the canonical source ID, and dynamic sphere obstacles are rejected. + +A registry-backed mapping also fails fast when a selected collision source has +no mesh geometry required by its representation. It never silently omits that +canonical ID from generated planner geometry. + +When an external perception or hardware provider supplies snapshots, validate +that provider's dynamic subset explicitly instead of constructing a +registry-derived one. The complete registry/planner world check still applies: + +```python +registry.validate_collision_integration( + motion_generator, + batch_size=batch_size, + scene_provider=external_scene_provider, +) +``` + +{class}`SceneCollisionWorldMode` follows this rule: + +| Batch and collision setup | Required registry choice | cuRobo setting | +|---|---|---| +| No dynamic collision entities | No mode required | Planner-specific | +| One environment | Omitted mode resolves to `SHARED`; explicit mode also allowed | Match the effective mode | +| Multiple environments | Explicit `SHARED` or `PER_ENV` is required | `multi_env=False` or `True`, respectively | + +Choose `SHARED` only when obstacle poses are equal after rebasing every +environment into its robot-base frame. Choose `PER_ENV` for independently +randomized robot-relative layouts. + +## Advanced direct-core paths + +`RigidObjectSceneProvider` and a list-valued `CuroboWorldCfg.rigid_objects` +remain available to advanced callers that intentionally assemble the atomic +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. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 1bba99004..46f81a635 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -11,7 +11,8 @@ combines that snapshot with the latest For the complete architecture and ownership model, see :doc:`/overview/sim/atomic_actions/index`. For the capability matrix and visual demonstrations of every built-in skill, see -:doc:`/overview/sim/atomic_actions/builtin_actions`. +:doc:`/overview/sim/atomic_actions/builtin_actions`. Canonical scene identity and +snapshot/provider setup are documented in :doc:`/overview/sim/scene_registry`. The contracts deliberately separate six concerns: @@ -258,7 +259,6 @@ must be resolved from the latest scene snapshot: from embodichain.lab.sim.atomic_actions import ( EndEffectorPoseGoal, RecoveryPolicy, - RigidObjectSceneProvider, SceneEntityPose, ) @@ -280,8 +280,16 @@ must be resolved from the latest scene snapshot: SimulationExecutionAdapter, TaskState, ) + from embodichain.lab.sim.skills import SceneRegistry - scene_provider = RigidObjectSceneProvider({"moving_tray": moving_tray}) + registry = SceneRegistry.from_simulation( + sim, + rigid_objects={"moving_tray": moving_tray.uid}, + ) + scene_provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, + ) adapter = SimulationExecutionAdapter( sim, robot, @@ -324,13 +332,24 @@ for comparison: python scripts/tutorials/atomic_action/moving_target_recovery.py --headless --auto_play --device cpu -For collision-aware execution, list pose-updatable obstacles in -``RigidObjectSceneProvider.collision_entity_ids`` and configure matching -dynamic obstacle names on a supporting planner such as cuRobo. The provider -advances per-environment collision-world revisions when an obstacle moves; -the session invalidates affected rows and the framework binds the latest poses -before replanning. Pose thresholds use the last materially published pose as -their baseline, so cumulative sub-threshold motion is eventually reported: +For collision-aware execution, register each pose-updatable obstacle with +``SceneCollisionRole.DYNAMIC`` and configure the same canonical registry IDs as +the planner's dynamic obstacle names. Derive the cuRobo object mapping with +``registry.collision_geometry_by_id()`` and construct the runtime provider with +``registry.make_planning_scene_provider(motion_generator, batch_size=...)``. +That one factory call checks that the registry's complete ``STATIC ∪ +DYNAMIC`` set exactly matches the planner's complete collision world, then +checks that the registry, provider, and planner dynamic subsets exactly match. +It also checks planner capability and shared/per-environment world mode. One +environment may infer a shared world; a multi-environment dynamic registry must choose +``SceneCollisionWorldMode.SHARED`` or ``PER_ENV`` explicitly. See +:doc:`/overview/sim/scene_registry` for the complete cuRobo mapping example. + +The provider advances per-environment collision-world revisions when an +obstacle moves; the session invalidates affected rows and the framework binds +the latest poses before replanning. Pose thresholds use the last materially +published pose as their baseline, so cumulative sub-threshold motion is +eventually reported: .. code-block:: bash From 794bc62778c0fe0a2c423f8eab27c1136aeb23b6 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 23:40:03 +0800 Subject: [PATCH 08/28] feat(sim): add declarative robot skill profiles --- agent_context/MAP.yaml | 39 + .../topics/atomic-actions/atomic-actions.md | 151 +- .../design/declarative_expert_program_plan.md | 172 +- .../embodichain.lab.sim.atomic_actions.rst | 51 + .../embodichain.lab.sim.skills.rst | 76 + .../overview/sim/atomic_actions/index.md | 40 +- .../atomic_actions/robot_skill_profiles.md | 281 +++ docs/source/overview/sim/index.rst | 10 + .../lab/sim/atomic_actions/__init__.py | 26 + embodichain/lab/sim/atomic_actions/control.py | 31 +- embodichain/lab/sim/atomic_actions/core.py | 22 + embodichain/lab/sim/atomic_actions/engine.py | 95 +- .../primitives/coordinated_pickment.py | 38 +- .../primitives/coordinated_placement.py | 54 +- .../atomic_actions/primitives/hand_over.py | 63 +- .../primitives/move_end_effector.py | 21 + .../primitives/move_held_object.py | 38 +- .../atomic_actions/primitives/move_joints.py | 21 + .../sim/atomic_actions/primitives/pick_up.py | 43 +- .../sim/atomic_actions/primitives/place.py | 41 +- .../sim/atomic_actions/primitives/press.py | 38 +- .../lab/sim/atomic_actions/requirements.py | 380 ++++ embodichain/lab/sim/skills/__init__.py | 36 + embodichain/lab/sim/skills/profiles.py | 1804 +++++++++++++++++ tests/sim/atomic_actions/test_control.py | 25 + tests/sim/skills/test_profiles.py | 1207 +++++++++++ 26 files changed, 4733 insertions(+), 70 deletions(-) create mode 100644 docs/source/overview/sim/atomic_actions/robot_skill_profiles.md create mode 100644 embodichain/lab/sim/atomic_actions/requirements.py create mode 100644 embodichain/lab/sim/skills/profiles.py create mode 100644 tests/sim/skills/test_profiles.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 3c26821ce..2f49e4aad 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -445,6 +445,11 @@ topics: - scene grounding - scene registry - semantic scene + - robot skill profile + - resource graph + - resource DAG + - semantic skill catalog + - capability binding - AtomicAction - ActionInvocation - AtomicActionEngine @@ -506,6 +511,38 @@ topics: - StateDelta - held_objects - ActionBinding + - ActionBindingRoute + - SkillBindingContract + - SkillResourceSlot + - SkillEndpointRequirement + - DisjointSlotEndpoints + - DisjointResourceSlots + - RobotSkillProfile + - BoundRobotSkillProfile + - RobotResource + - ResourceEndpoint + - ResourceEndpointAdapter + - ControlPartEndpoint + - ControlPartEndpointAdapter + - EndpointResolution + - ResolvedResourceEndpoint + - ResourceBinding + - ResourceClaim + - ResolvedRobotResource + - ResolvedSkillBinding + - SkillPolicyPreset + - binding_contract + - engine.skills + - skill_profile + - command_profiles + - action_control_profiles + - endpoint_adapters + - endpoint snapshot + - requires_command_profile + - claim_tokens + - capability + - whole body resource + - leaf resource claim - SceneEntityPose - dynamic goal - error recovery @@ -541,6 +578,7 @@ topics: - embodichain/lab/sim/atomic_actions/control.py - embodichain/lab/sim/atomic_actions/invocation.py - embodichain/lab/sim/atomic_actions/policies.py + - embodichain/lab/sim/atomic_actions/requirements.py - embodichain/lab/sim/atomic_actions/runtime.py - embodichain/lab/sim/atomic_actions/state.py - embodichain/lab/sim/atomic_actions/plans.py @@ -553,6 +591,7 @@ topics: - embodichain/lab/sim/atomic_actions/primitives/ - embodichain/lab/sim/atomic_actions/__init__.py - embodichain/lab/sim/skills/scene.py + - embodichain/lab/sim/skills/profiles.py - embodichain/lab/sim/skills/__init__.py related_topics: - motion-planning diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 80c21d65c..9698d0f98 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -38,7 +38,7 @@ recompute private sample splits in callers. Each `AtomicActionEngine` exclusively owns one `ActionPlanningServices` instance, which contains its robot, one `MotionGenerator`/planner backend, and -control-part command profiles. `MotionGenerator.generate()` is the only +the legacy core's control-part command profiles. `MotionGenerator.generate()` is the only stateful motion-planning entry point. `MotionPolicy.to_motion_gen_options()` passes the invocation's `strategy` directly into `MotionGenOptions`; it is either `"motion_gen"` or `"ik_interp"`. Target shaping, world-frame pose translation, @@ -75,6 +75,126 @@ The `_plan()` extension boundary is an intentional hard break with no legacy adapter. A subclass that defines `plan()` raises `TypeError` at class definition; migrate an older custom action by renaming that implementation to `_plan()`. +## Robot skill profiles and resource binding + +`embodichain.lab.sim.skills.RobotSkillProfile` is the authoritative +embodiment-level catalog for semantic resource binding. Its resource model is a +generic DAG, not a fixed arm/tool schema: + +- `RobotResource.resource_id` is a stable logical ID. `endpoints` maps + skill-local endpoint protocol names such as `motion` or `grasp` to + `ResourceEndpoint` values, and `members` declares physical composition. +- `members` determines transitive claim closure only. It does not inherit or + synthesize endpoint capabilities. A whole-body composite must declare its own + whole-body capability and endpoint explicitly. +- `ResourceEndpoint` is the extension boundary for controller kinds. An exact + endpoint-type `ResourceEndpointAdapter` resolves each declaration against the + engine into an `EndpointResolution`: lowering values, an optional generic + command-profile key, joint IDs, adapter-defined claim tokens, and exclusivity. + `ControlPartEndpointAdapter` is installed by default for + `ControlPartEndpoint`; integrations pass additional `endpoint_adapters` to + profile or engine binding for mobile bases, whole-body controllers, or other + endpoint kinds. Registration is by exact endpoint type, and the built-in + adapter cannot be overridden; distinct controller semantics use a distinct + endpoint subtype. +- Resources, profiles, and resolved bindings own independent endpoint + snapshots. A custom endpoint whose nested payload cannot be deep-copied must + override `snapshot()` and return a new value of its exact type. +- Binding snapshots adapter output as a `ResolvedResourceEndpoint`, including + its resolved commands and claims. An exclusive resolution must declare at + least one joint ID or claim token; a deliberately non-exclusive endpoint may + omit both. +- A leaf must expose at least one endpoint. Member references must exist and the + graph must be acyclic. On engine binding, physical leaves must own disjoint + robot joints and adapter claim tokens; a composite endpoint may control only + joints already covered by its transitive members. + +Skills own the robot-independent side of the contract. A concrete +`AtomicAction` opts into semantic discovery by declaring a +`SkillBindingContract` in its own class body. The contract contains +skill-local `SkillResourceSlot` values; every slot requires named +`SkillEndpointRequirement` values with all-of capabilities, optional typed +semantic commands, and an optional `ActionBindingRoute`. Selecting one resource +per slot keeps related endpoints together, so a manipulation participant cannot +silently combine one arm with an unrelated tool. Endpoint views within that +resource may overlap by default, which permits an arm, mobile base, and +whole-body view to describe the same physical system. Add +`DisjointSlotEndpoints` to a slot only when selected endpoint views must be +physically disjoint. `DisjointResourceSlots` separately expresses pairwise +claim separation between selected participant resources. + +`ActionBindingRoute` is only a transition adapter into the current core's +`manipulators` and `end_effectors` maps. Contract routes must cover the action's +declared core roles exactly. `BoundRobotSkillProfile.resolve()` returns a +`ResolvedSkillBinding` that retains the selected logical resources, the lowered +concrete `ActionBinding`, each resource's resolved endpoint data, and one +combined `ResourceClaim`. Direct-core callers may still construct +`ActionBinding` themselves, but that path does not perform profile capability +matching. + +Discovery boundaries are distinct: + +- `engine.actions` contains every installed action instance and is the + direct-core registry. +- `engine.skills` contains descriptors only for installed, `agent_visible` + actions whose concrete class explicitly declares a binding contract. A + subclass does not inherit semantic exposure implicitly. +- `engine.skill_profile.skills` filters `engine.skills` again to contracts with + at least one valid assignment on the bound robot. Registering or replacing an + action invalidates the engine's bound profile; an independently retained + `BoundRobotSkillProfile` also rejects use after the engine skill catalog + changes and must be rebound. + +Binding and policy authority is split deliberately: + +- the action class owns its slot/endpoint/command requirement contract; +- 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; +- 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 + lowering metadata; +- the engine owns installed actions, one planner backend, and the legacy + control-part command profiles used by the current action core. + +Constructing `AtomicActionEngine(..., skill_profile=profile)` makes the +profile's generic `command_profiles` the single authoritative constructor +source; passing `control_profiles` at the same time is rejected. +`command_profiles` values currently use `ControlPartCommandProfile` as their +immutable command container, but their mapping keys are generic profile IDs +rather than necessarily being control-part names. +`ControlPartEndpointAdapter` plus `RobotSkillProfile.action_control_profiles()` +is only the bridge that lowers applicable endpoint commands into the current +core's control-part-keyed profiles. Binding a profile to an already constructed +engine instead requires equivalent bridge commands to have been installed +already. A profile `JointPositionCommand` is one-dimensional and sized to the +adapter-resolved endpoint joint IDs; invocation `ActionControlOverrides` remain +the authority for one revision's per-environment replacements. Resolving a +custom endpoint's commands does not by itself add their controller transport to +the current action core. + +Resolution selects a sole valid assignment automatically. If several remain, +it uses only a complete, currently valid per-skill default or enough explicit +slot selections; partial defaults and mapping/lexical order never disambiguate. +Preset lookup order is explicit preset, per-skill preset, then profile default, +and every returned preset is an owned snapshot. Planner-pinned presets must +match the engine's configured planner. + +`ResourceClaim` contains transitive leaf-resource IDs, sorted concrete joint +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, +joint-mask command merger, or concurrency guarantee yet. `ExecutionSession` +and `ExecutionRunner` still emit, cancel, and hold full-robot joint commands. A +custom mobile/base endpoint can bind and participate in capability matching +once its adapter resolves it, including a controller claim token, but that does +not create a reusable navigation skill, planner/controller path, or command +transport. Do not treat successful binding or a non-conflicting claim as proof +of safe parallel or mobile execution. + ## Object identity and pose grounding `ObjectSemantics.entity_id` is the typed core's canonical snapshot-key lowering @@ -367,13 +487,13 @@ offsets, and grasp selection behavior. An action constructor may accept There is no `ActionCfg` or built-in `*Cfg` layer. `engine.register(action)` is reserved for custom skill implementations. A -built-in can be replaced only with explicit `replace=True`. Registration means -an implementation is installed; it does not prove that the current embodiment -has compatible control parts, profiles, bindings, or task state. Capability -adapters must filter registered descriptors before exposing skills to an Agent. -The module-level `register_action()` API is a process-wide extension-type -discovery catalog only; it neither binds actions nor changes an engine's -default built-in set. +built-in can be replaced only with explicit `replace=True`. Registration puts +the implementation in `engine.actions`; it does not prove that the current +embodiment supports it. Semantic exposure additionally requires a concrete +class-local `binding_contract` for `engine.skills` and a valid profile assignment +for `engine.skill_profile.skills`. The module-level `register_action()` API is a +process-wide extension-type discovery catalog only; it neither binds actions nor +changes an engine's default built-in set. `ExecutionRunnerCfg` is intentionally separate from action options. It configures controller acknowledgement deadlines, scheduler cadence, and final @@ -386,8 +506,9 @@ and resolve immutable `ResolvedControlPart` values containing full-robot joint indices. Built-ins use the binding as the only source for participating arm and hand names; attachment state and `StateDelta` keys use the bound manipulator. -Embodiment-specific joint commands do not belong to Action options. Register -them once by actual control-part name: +Embodiment-specific joint commands do not belong to Action options. A caller +using the legacy direct-core path without a `RobotSkillProfile` registers them +by actual control-part name: ```python engine = AtomicActionEngine( @@ -406,7 +527,11 @@ Actions request semantic commands (`open`, `grasp`, or a named joint target) from the `ResolvedControlPart`. `ActionControlOverrides` may replace commands by semantic binding role for one invocation revision. Joint limits constrain commands but do not define semantic open/grasp states; a robot integration or -tutorial may derive a simple profile from limits explicitly. +tutorial may derive a simple profile from limits explicitly. Profile-based +integrations instead own commands under generic `command_profiles` IDs and let +endpoint declarations/adapters resolve those IDs; only +`action_control_profiles()` converts applicable control-part endpoints back to +the legacy core mapping. ## Built-ins @@ -441,7 +566,9 @@ snapshot-grounded object example. 1. Define a frozen action-owned goal dataclass with `goal_kind`. 2. Define a frozen `ActionOptions` subclass only when runtime behavior exists. -3. Declare `skill_id`, `GoalType`, `OptionsType`, and required semantic roles. +3. Declare `skill_id`, `GoalType`, `OptionsType`, and required core roles. Also + declare a class-local `SkillBindingContract` when the skill should appear in + `engine.skills`; route every current core role exactly once. 4. Implement `_plan()`; do not override the framework-owned `plan()` method. 5. Validate with `require_goal(request)` and consume only the resolved binding. 6. Plan from `context.robot.qpos`; never read an implicit live start state. diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 1bee119b8..5a69e51ec 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,7 +1,7 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: implementation in progress; Phase 0 and PR1 complete, PR2A implemented - on the feature branch +- Status: implementation in progress; Phase 0 and PR1 complete, PR2A and PR2B + implemented on stacked feature branches - Baseline: `main@e445133c79c8b32019dab1c844b799b43a1658d6` - Last updated: 2026-08-10 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), @@ -66,7 +66,7 @@ sessions, or verifiers. verification, and recovery path. 3. Make object identity, observation, geometry, affordance, and collision data come from one scene registry. -4. Infer robot-part bindings and stable runtime policies from reusable profiles; +4. Infer robot-resource bindings and stable runtime policies from reusable profiles; require explicit choices only when the request is genuinely ambiguous. 5. Preserve lazy observation and per-environment recovery for programs whose later goals depend on earlier physical effects. @@ -93,9 +93,10 @@ sessions, or verifiers. This plan is updated against committed `main@e445133c` after PR #475. The implementation series is stacked from that baseline: PR1 is complete on -`refactor/atomic-actions-phase0`, and PR2A is implemented by the -`feat/atomic-action-pr2a-scene-registry` change. Neither status statement -implies that the stacked changes have landed on `main`. +`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 | |---|---|---| @@ -124,6 +125,8 @@ The remaining #474 prerequisites on `main` are: multiple sources of truth; - ordinary callers still see a large low-level public surface and must perform semantic transform and verifier plumbing; +- robot capability declarations, resource selection, semantic commands, and + stable policies do not yet have an embodiment-owned source of truth; - dynamic-obstacle validation is planner-local; provider collision entity IDs and planner-declared names are not yet fully cross-validated at integration construction time; @@ -133,8 +136,10 @@ The remaining #474 prerequisites on `main` are: PR1 closes the snapshot-grounding and stable-identity bridge. PR2A closes the first and third gaps for registry-backed integrations by introducing one authoritative registration boundary, a registry-derived scene provider, and -construction-time collision-world validation. The semantic facade and named -presets remain later-phase work. +construction-time collision-world validation. PR2B closes the robot-profile +gap on its stacked branch with generic resources, deterministic binding, +profile-owned commands, and named policy presets. The semantic facade remains +later-phase work. One #474 finding has changed since its review branch: the ambiguous `collision_check` switch has been replaced by `DynamicCollisionMode.OFF`, @@ -385,29 +390,107 @@ not duplicate the registration catalog. ### 7.2 Robot skill profiles -A `RobotSkillProfile` is reusable per embodiment and contains: - -- capability declarations for arms, grippers, hands, and tools; -- mappings from semantic roles to compatible control parts; -- semantic commands such as `open`, `grasp`, `release`, and `ready`; -- available planners/motion strategies and their constraints; -- default grasp, effect-monitor, and runtime preset selections; -- optional preference rules when more than one binding is valid. - -The compiler resolves the only valid binding automatically. If two arms are -equally valid and the profile has no deterministic preference, validation asks -for a semantic choice such as `arm: left`; it never asks the task to construct -an `ActionBinding`. +A `RobotSkillProfile` is reusable per embodiment, but its resource model is not +an `arm + tool` schema. It contains a generic resource DAG: + +- each `RobotResource` has a stable logical ID, zero or more named execution + endpoints, and optional member resources; +- each endpoint declares open, namespaced capabilities explicitly and lowers + through a `ResourceEndpoint` implementation; `ControlPartEndpoint` is the + current joint/control-part declaration, while registered + `ResourceEndpointAdapter`s resolve any endpoint kind into generic + `EndpointResolution` metadata (binding values, commands, physical claim + tokens, and optional joint IDs) without changing the graph, matcher, or slot + model. Adapters register by exact endpoint type; the built-in control-part + adapter is not overrideable, and different controller semantics use a new + endpoint subtype; +- members describe physical composition and claim closure, not capability + inheritance. A composite must explicitly declare `motion.whole_body`; it + does not acquire that capability because it contains a base, torso, or arms; +- semantic control commands such as `open`, `grasp`, or a future `stop` remain + 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; +- per-skill defaults map every skill-local slot to one resource ID. + +Resource and endpoint declarations are owned snapshots. A custom endpoint with +non-trivial nested payloads implements `snapshot()` to return an independent +value of its exact type, so caller-owned mutation cannot rewrite a bound +profile. + +Skills own the robot-independent half of the contract. A concrete atomic action +must explicitly publish a `SkillBindingContract`; inheriting the default +`primary` role or inheriting another action's contract does not expose a new +semantic skill. The contract declares skill-local participant slots and the +endpoint requirements inside each participant. For example, `pick_up` has one +`primary` participant with a `motion` endpoint and a `grasp` endpoint. A profile +can satisfy it with `left_actor`, whose endpoints lower to `left_arm` and +`left_hand`. Selecting the participant as one unit prevents invalid cross-side +combinations such as `left_arm + right_hand`. + +Endpoint names are local protocols, not global robot-part categories. A future +`navigate` skill can require `body.motion: motion.base.se2`; a +`whole_body_reach` skill can require `body.motion: motion.whole_body`. Neither +requires new `RobotSkillProfile` fields. The current `ActionBindingRoute` is a +transition adapter from generic endpoints to the core's existing +`manipulators`/`end_effectors` maps; those maps are not part of the Profile +resource model. + +Binding follows strict rules: + +1. Filter each slot by endpoint presence, all required capabilities, typed + semantic commands, explicit caller selection, and installed endpoint + support. +2. Apply explicit physical-claim constraints. Built-in manipulation contracts + declare their `motion` and `grasp` views disjoint, while coupled whole-body + views may overlap when the skill omits that constraint. Multi-participant + contracts such as handover use pairwise-disjoint resource claims. +3. No candidates means the skill is unsupported on this profile and is omitted + from the profile-backed semantic catalog. +4. One complete candidate is selected automatically. +5. Multiple candidates are resolved only by a complete, still-valid per-skill + default or enough explicit slot selections. Partial defaults, mapping order, + and lexical order never break ambiguity. + +`ResourceClaim` contains transitive leaf-resource IDs, concrete joint IDs, and +adapter-defined physical/controller claim tokens. It makes `whole_body` +conflict with `base`, `torso`, or a contained arm even when the underlying +`Robot.control_parts` names are different, and lets a non-joint base adapter +claim a controller without inventing joints. PR2B +exposes deterministic claim/conflict data only. Current runners emit and hold +full-robot commands, so claims do not imply safe parallel execution. Parallel +scheduling still requires one coordinator, joint-mask command merge, planner +serialization or isolation, cancellation semantics, and inter-trajectory +collision checks. + +`AtomicActionEngine.actions` remains the direct-core implementation registry. +`engine.skills` contains only installed, agent-visible actions whose concrete +class explicitly declares a binding contract. A bound profile filters that +catalog again by the current robot resources. Constructing an engine with +`skill_profile=...` installs the profile's command snapshots as the single +authoritative source and binds the validated profile after built-ins load. +Known FK/IK capabilities on the control-part adapter are checked against the +selected control part's configured solver; Cartesian motion is not equated with +solver presence because native planners may provide it directly. Profile joint +commands must be one-dimensional and broadcastable; per-environment values +belong in invocation overrides. ### 7.3 Semantic call specification Version 1 should provide first-class calls for: -- `Pick(object, grasp?, arm?)`; -- `Place(object, pose?|on?|in?, arm?)`; -- `HandOver(object, receiver?, final_target?)`; +- `Pick(object, grasp?, resources?)`; +- `Place(object, pose?|on?|in?, resources?)`; +- `HandOver(object, receiver?, 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. + `Place` consumes verified held-object state. The compiler computes the release EEF pose from the requested object-space target and the verified `object_to_eef` relation. Task code and configuration never perform @@ -765,10 +848,11 @@ OperateArticulation( ) ``` -Its compiler selects an affordance pose, binds an arm/tool, builds the approach -and constrained operation, and installs an articulation effect monitor. Once -implemented once in the shared layer, Open Drawer variants should differ only -in scene/affordance data, target state, presets, and validators. +Its compiler selects an affordance pose, resolves one participant resource and +its required motion/interaction endpoints, builds the approach and constrained +operation, and installs an articulation effect monitor. Once implemented once +in the shared layer, Open Drawer variants should differ only in +scene/affordance data, target state, resource defaults, presets, and validators. This is the precise meaning of "almost no action-layer code": task expansion is configuration-only when a compatible semantic capability already exists; new @@ -788,7 +872,7 @@ PR1 snapshot/identity bridge (complete) +-----------------------+ v v PR2A SceneRegistry PR2B RobotSkillProfile - (implemented) (next) + (implemented) (implemented) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -921,14 +1005,31 @@ dynamic registry/provider/planner subsets are validated before execution, the collision-world batch mode agrees, and cuRobo uses canonical mapping keys end to end as logical source IDs (and as physical keys for cuboid/mesh worlds). -#### PR2B: RobotSkillProfile +#### PR2B: RobotSkillProfile (implemented on the feature branch) Deliverables: -- `RobotSkillProfile` and reusable capability declarations; -- capability-based deterministic binding and explicit ambiguity errors; -- semantic tool commands and stable runtime/planning presets; -- profile validation against installed engine skills and robot control parts. +- a generic `RobotResource` DAG whose named `ResourceEndpoint`s are not tied to + arm/tool categories, plus a formal `ResourceEndpointAdapter` registry and + `EndpointResolution` protocol; `ControlPartEndpointAdapter` is the first + implementation; +- action-owned `SkillBindingContract`s with participant-local endpoint, + capability, typed-command, lowering-route, and disjoint-claim requirements; +- capability-based candidate filtering, complete per-skill defaults, explicit + selection overrides, and deterministic ambiguity/unsupported diagnostics; +- profile-owned semantic commands plus immutable, versioned planning/recovery/ + runner presets; +- validation against installed agent-visible engine skills, robot control + parts, joint ownership, endpoint overlap, configured solvers, commands, and + presets; +- immutable leaf/joint/adapter-token `ResourceClaim` data and explicit + same-slot endpoint disjointness for future conflict analysis without claiming + that the current full-robot command runner supports 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. PR2B may proceed in parallel with PR2A after the PR1 bridge. Neither follow-up requires official task migration; the repeated-cube vertical slice opts in only @@ -1107,6 +1208,9 @@ The design is complete when all of the following hold: typed atomic-action core, and runtime. - [ ] 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 + require new arm/tool-shaped profile fields. - [ ] 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. 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 c2a3d4dde..5345daee4 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 @@ -32,6 +32,18 @@ embodichain.lab.sim.atomic_actions ActionPlan CompiledTrajectory + .. rubric:: Semantic resource contracts + + .. autosummary:: + + SkillDescriptor + SkillBindingContract + SkillResourceSlot + SkillEndpointRequirement + ActionBindingRoute + DisjointSlotEndpoints + DisjointResourceSlots + .. rubric:: Execution contracts .. autosummary:: @@ -89,6 +101,45 @@ embodichain.lab.sim.atomic_actions .. currentmodule:: embodichain.lab.sim.atomic_actions +Semantic resource contracts +--------------------------- + +.. autoclass:: SkillDescriptor + :members: + +.. autoclass:: SkillBindingContract + :members: + +.. autoclass:: SkillResourceSlot + :members: + +.. autoclass:: SkillEndpointRequirement + :members: + +.. autoclass:: ActionBindingRoute + :members: + +.. autoclass:: DisjointSlotEndpoints + :members: + +.. autoclass:: DisjointResourceSlots + :members: + +Standard capability identifiers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. autodata:: JOINT_POSITION_CAPABILITY + +.. autodata:: CARTESIAN_POSE_CAPABILITY + +.. autodata:: FORWARD_KINEMATICS_CAPABILITY + +.. autodata:: INVERSE_KINEMATICS_CAPABILITY + +.. autodata:: BATCH_INVERSE_KINEMATICS_CAPABILITY + +.. autodata:: GRASP_CAPABILITY + Planning and state ------------------ 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 44ac3c38c..1b3022fe8 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -21,8 +21,84 @@ embodichain.lab.sim.skills SceneCollisionRole SceneCollisionWorldMode + .. rubric:: Robot skill profiles + + .. autosummary:: + + RobotSkillProfile + BoundRobotSkillProfile + RobotResource + ResourceEndpoint + ResourceEndpointAdapter + EndpointResolution + ControlPartEndpoint + ControlPartEndpointAdapter + ResourceBinding + ResolvedResourceEndpoint + ResolvedRobotResource + ResolvedSkillBinding + ResourceClaim + SkillPolicyPreset + ProfileValidationError + UnsupportedSkillError + AmbiguousSkillBindingError + .. currentmodule:: embodichain.lab.sim.skills +Robot resources and profiles +---------------------------- + +.. autoclass:: RobotSkillProfile + :members: + +.. autoclass:: BoundRobotSkillProfile + :members: + +.. autoclass:: RobotResource + :members: + +.. autoclass:: ResourceEndpoint + :members: + +.. autoclass:: ResourceEndpointAdapter + :members: + +.. autoclass:: EndpointResolution + :members: + +.. autoclass:: ControlPartEndpoint + :members: + +.. autoclass:: ControlPartEndpointAdapter + :members: + +.. autoclass:: ResourceBinding + :members: + +.. autoclass:: ResolvedResourceEndpoint + :members: + +.. autoclass:: ResolvedRobotResource + :members: + +.. autoclass:: ResolvedSkillBinding + :members: + +.. autoclass:: ResourceClaim + :members: + +.. autoclass:: SkillPolicyPreset + :members: + +Profile errors +-------------- + +.. autoclass:: ProfileValidationError + +.. autoclass:: UnsupportedSkillError + +.. autoclass:: AmbiguousSkillBindingError + Registry and provider --------------------- diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 331cff6b4..321940c1f 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -6,6 +6,7 @@ :hidden: builtin_actions +robot_skill_profiles ``` ```{currentmodule} embodichain.lab.sim.atomic_actions @@ -185,6 +186,14 @@ base class and no closed union that must change whenever a skill is added. ### Semantic resource binding +The canonical semantic path uses a +{doc}`RobotSkillProfile ` to match skill-local slots and +endpoint capabilities against a generic robot resource graph. It validates +participant pairing, typed commands, physical claims, complete defaults, and +policy presets before lowering the selected endpoints to the current core +binding. The `ActionBinding` description below is the resulting direct-core +contract and remains available for advanced manual callers. + A **role** is an action-owned semantic participant slot: it describes the job a robot resource performs in that action, not the identity of the resource. Each `AtomicAction` declares its required slots through `manipulator_roles` and @@ -219,11 +228,12 @@ manipulator's IK/TCP frame remains part of the robot and solver configuration. The engine validates every name and resolves its full-robot joint indices before calling the action planner. -The validation boundary is intentionally narrow: the engine verifies required -roles, `control_parts` membership, resolvable joint indices, command type, and -command dimensions. The Agent adapter or application binder remains responsible -for capability compatibility, such as pairing an arm with the hand mounted on -it and choosing a semantic command supported by that tool. +For a manually constructed `ActionBinding`, the validation boundary remains +intentionally narrow: the engine verifies required roles, `control_parts` +membership, resolvable joint indices, command type, and command dimensions. A +bound `RobotSkillProfile` adds capability matching, participant endpoint +pairing, command requirements, joint-claim checks, and deterministic +disambiguation before it produces that same core value. Role names should describe action responsibilities rather than robot-specific joint, link, or model names. Single-resource skills use `primary`; handover uses @@ -240,8 +250,14 @@ manipulator control-part name. ### Control-part semantic commands -Register embodiment commands once when constructing the engine. The keys are -concrete names from `robot.control_parts`; the command names remain semantic: +On the canonical semantic path, declare embodiment commands on the +{doc}`RobotSkillProfile ` and pass the profile through the +engine's `skill_profile` argument. For a direct-core integration, register the +same command profiles explicitly when constructing the engine. Profile command +IDs are generic and selected by endpoint adapters; the built-in control-part +adapter defaults them to concrete `robot.control_parts` names. Direct-core +engine keys are always concrete control-part names. The command names remain +semantic: ```python engine = AtomicActionEngine( @@ -322,10 +338,12 @@ world from one {doc}`SceneRegistry <../scene_registry>`. Direct use of `RigidObjectSceneProvider` remains an advanced-core path. Registration means that an implementation is installed, not that every robot -can execute it. Required roles, control parts, profiles, and task-state -preconditions are validated while an invocation is resolved and planned. Agent -adapters must additionally filter the catalog by `agent_visible` and -embodiment capability instead of exposing every `engine.actions` entry blindly. +can execute it. `engine.actions` contains direct-core implementations; +`engine.skills` contains installed, agent-visible implementations with an +explicit generic binding contract; and `engine.skill_profile.skills` applies +embodiment capability filtering. Required task-state preconditions remain +runtime conditions and are validated while an invocation is resolved and +planned. Use invocation `skill_options` whenever behavior varies per call. Two variants with the same stable skill ID therefore share one built-in implementation: diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md new file mode 100644 index 000000000..b5af6a58b --- /dev/null +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -0,0 +1,281 @@ +(robot-skill-profiles)= + +# Robot skill profiles + +```{currentmodule} embodichain.lab.sim.skills +``` + +A {class}`RobotSkillProfile` describes how robot-independent atomic-skill +requirements map onto one robot embodiment. Configure the robot's resources, +semantic commands, default choices, and policy presets once; task code can then +select skill-local participants instead of constructing an `ActionBinding` from +robot-specific control-part names. + +The model is deliberately generic. It does not define global `arm` and `tool` +fields. Each atomic skill publishes its own participant slots and endpoint +requirements, while a robot resource may expose any endpoints appropriate to +that embodiment: manipulation motion and grasping, a mobile base, a torso, or a +whole-body controller. + +## Contracts on the two sides + +An atomic action owns a +{class}`~embodichain.lab.sim.atomic_actions.SkillBindingContract`: + +- a {class}`~embodichain.lab.sim.atomic_actions.SkillResourceSlot` names each + skill-local participant, such as `primary`, `source`, or `destination`; +- a {class}`~embodichain.lab.sim.atomic_actions.SkillEndpointRequirement` + declares the all-of capabilities and typed semantic commands needed from that + participant; +- an optional + {class}`~embodichain.lab.sim.atomic_actions.ActionBindingRoute` lowers a + generic endpoint into the current atomic-action core; and +- {class}`~embodichain.lab.sim.atomic_actions.DisjointSlotEndpoints` declares + endpoint views that must not share physical channels within one participant; + coupled whole-body views may overlap when the skill does not declare this + constraint; and +- {class}`~embodichain.lab.sim.atomic_actions.DisjointResourceSlots` requires + multi-participant skills to select physically disjoint resources. + +The robot side supplies {class}`RobotResource` values. A resource exposes named +{class}`ResourceEndpoint` values and may contain other resources through +`members`. Members form a directed acyclic graph and describe the physical +claim; endpoint capabilities are always explicit and are never inherited or +inferred from names. {class}`ControlPartEndpoint` is the built-in joint-backed +endpoint type, not the resource schema itself. + +```text +skill contract robot profile + +slot primary resource left_participant ++-- endpoint motion <--------------> +-- endpoint motion -> left_arm +`-- endpoint grasp <--------------> `-- endpoint grasp -> left_hand + capabilities + commands + members/physical claim +``` + +Binding the profile to an engine resolves each endpoint through a registered +{class}`ResourceEndpointAdapter` and validates physical claims, known +solver-backed kinematics capabilities, command types and dimensions, complete +defaults, policy presets, and installed skill contracts. The resulting +{class}`BoundRobotSkillProfile` exposes only installed, agent-visible skills +with at least one valid resource assignment. + +Endpoint and resource declarations are snapshotted when owned by a resource, +profile, or resolved binding. Custom endpoint types whose payloads cannot be +deep-copied must override {meth}`ResourceEndpoint.snapshot` and return a new +value of the same exact type. + +## Configure a manipulation participant + +The following profile groups two physical leaves into one participant. The +`motion` and `grasp` endpoint names come from the built-in manipulation +contracts; they are local protocol names, not global robot-resource categories. + +```python +import torch + +from embodichain.lab.sim.atomic_actions import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + ControlPartCommandProfile, + MotionPolicy, +) +from embodichain.lab.sim.skills import ( + ControlPartEndpoint, + ResourceBinding, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) + +left_motion_capabilities = frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + } +) + +profile = RobotSkillProfile( + profile_id="example_robot", + resources={ + # Physical leaves own disjoint robot joints. + "left_arm_leaf": RobotResource( + resource_id="left_arm_leaf", + endpoints={"control": ControlPartEndpoint("left_arm")}, + ), + "left_hand_leaf": RobotResource( + resource_id="left_hand_leaf", + endpoints={"control": ControlPartEndpoint("left_hand")}, + ), + # A skill selects this participant as one indivisible resource. + "left_participant": RobotResource( + resource_id="left_participant", + endpoints={ + "motion": ControlPartEndpoint( + "left_arm", + capabilities=left_motion_capabilities, + ), + "grasp": ControlPartEndpoint( + "left_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + members=("left_arm_leaf", "left_hand_leaf"), + ), + }, + command_profiles={ + "left_hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.04, 0.04]), + grasp=torch.tensor([0.0, 0.0]), + ), + }, + defaults={ + "pick_up": ResourceBinding( + resources={"primary": "left_participant"}, + ), + }, + presets={ + "default": SkillPolicyPreset( + preset_id="default", + motion_policy=MotionPolicy(strategy="ik_interp"), + ), + }, + default_preset="default", +) +``` + +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 +its members. Two physical leaf resources may not claim the same joint; model a +shared physical part once and reference that leaf from multiple composites. + +`command_profiles` are generic IDs selected by endpoint adapters; the built-in +control-part adapter defaults the ID to its `control_part`, and the engine +installs those profiles into the current action core automatically. +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. + +## Bind, discover, and resolve + +Pass the profile to +{class}`~embodichain.lab.sim.atomic_actions.AtomicActionEngine`. The engine +installs its command profiles and binds it after loading built-in actions: + +```python +from embodichain.lab.sim.atomic_actions import AtomicActionEngine + +engine = AtomicActionEngine(motion_generator, skill_profile=profile) +bound = engine.skill_profile +assert bound is not None + +# This is the embodiment-filtered semantic catalog, not every installed action. +assert "pick_up" in bound.skills + +resolved = bound.resolve("pick_up") +assert resolved.resource_ids == {"primary": "left_participant"} +binding = resolved.action_binding +preset = bound.preset(skill_id="pick_up") +``` + +{meth}`BoundRobotSkillProfile.resolve` returns a {class}`ResolvedSkillBinding` +containing the selected logical resources, their adapter-resolved endpoints, +their combined {class}`ResourceClaim`, and the current-core `ActionBinding`. A +semantic compiler uses that binding and the selected preset when constructing +an invocation; profile resolution does not plan or execute the action itself. + +If exactly one assignment is valid, resolution selects it. If several remain, +the caller must provide enough skill-local selections or the profile must define +a complete per-skill default: + +```python +left = bound.resolve("pick_up", selections={"primary": "left_participant"}) +candidates = bound.candidates("pick_up") +``` + +Incomplete defaults are rejected when the profile is bound. Without an +unambiguous choice, resolution raises {class}`AmbiguousSkillBindingError` rather +than selecting a resource by declaration order. An unsupported selection raises +{class}`UnsupportedSkillError` with endpoint, capability, command, or claim +rejection details. + +`engine.actions` remains the direct-core implementation registry. +`engine.skills` is the installed semantic catalog before embodiment filtering, +and `bound.skills` is the profile-supported catalog. Registering or replacing an +action invalidates the bound profile; bind it again before discovery or +resolution. + +## Extend the graph beyond manipulation + +Resource and capability identifiers are open strings. A joint-driven mobile +robot can model a base and a whole-body controller without changing the profile +schema: + +```python +base = RobotResource( + resource_id="base", + endpoints={ + "motion": ControlPartEndpoint( + "base", + capabilities=frozenset({"motion.planar_pose"}), + ) + }, +) +torso = RobotResource( + resource_id="torso", + endpoints={"motion": ControlPartEndpoint("torso")}, +) +whole_body = RobotResource( + resource_id="whole_body", + endpoints={ + "motion": ControlPartEndpoint( + "full_body", + capabilities=frozenset({"motion.whole_body"}), + ) + }, + members=("base", "torso", "left_arm_leaf", "right_arm_leaf"), +) +``` + +Here `base`, `torso`, and `full_body` must be real, non-empty robot control +parts, and the `full_body` joint set must be covered by the listed members. A +future locomotion or whole-body skill can require the corresponding endpoint +and capability in its own binding contract. Existing built-in actions do not +consume these example capabilities. + +Non-joint controllers add one endpoint declaration type and one adapter. The +adapter returns {class}`EndpointResolution` with a command-profile key, +supported binding values, joint IDs when applicable, and adapter-defined claim +tokens. The generic graph, matching, command, default, and conflict code does +not change. For example, a twist controller can return +`claim_tokens={"controller:base"}` with no joint IDs. Exclusive endpoints must +provide joint IDs or claim tokens; a read-only or otherwise shareable virtual +endpoint must opt into `exclusive=False` explicitly. + +Adapters are registered by exact endpoint type. The built-in +{class}`ControlPartEndpointAdapter` cannot be overridden; define a distinct +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. + +{class}`ActionBindingRoute` remains a transition into the current core's +`manipulator` and `end_effector` maps. A new non-core controller therefore also +needs one reusable atomic skill/runtime integration for its route and command +transport. Once that shared capability exists, new tasks and robot variants +reuse it through profile and task configuration rather than task-specific +motion 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 +current action plans and commands still contain full-robot joint positions, and +the runtime does not merge concurrent command streams. +``` + +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 e7aa753b5..20d25c7a5 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -45,6 +45,8 @@ The simulation stack can be read from the bottom up: | `-- time parameterization and sampling utilities |-- scene registry | `-- 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 @@ -91,6 +93,11 @@ Submodule Relationships affordances, hierarchy, and collision roles. - Publishes registry-derived snapshots for atomic actions and validates dynamic collision-world agreement with planners. + * - Robot skill profiles + - Describe embodiment resources as a generic graph with explicit + endpoints, capabilities, semantic commands, defaults, and presets. + - Match skill-local participants to robot resources and lower validated + selections to the current atomic-action binding contract. * - Atomic actions - Package complete manipulation primitives such as move, pick, and place. - Compose semantic targets, solvers, planners, and robot control into @@ -129,6 +136,9 @@ Choosing Where to Start time-ordered trajectory. - Use :doc:`scene_registry` when semantic calls, snapshots, and planner obstacles must share one authoritative entity namespace. +- 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 ` when building scripted manipulation from reusable motion primitives. diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 86e00f80e..a5d7ea729 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -70,6 +70,20 @@ TrajectorySegment, ) from .policies import DynamicCollisionMode, MotionPolicy, RecoveryPolicy +from .requirements import ( + ActionBindingRoute, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from .runtime import ActionPlanningServices from .primitives import ( AssembleGoal, @@ -136,6 +150,7 @@ __all__ = [ "ActionBinding", + "ActionBindingRoute", "ActionControlOverrides", "ActionGoal", "ActionInvocation", @@ -149,6 +164,8 @@ "AtomicAction", "AtomicActionEngine", "BUILTIN_ACTION_TYPES", + "BATCH_INVERSE_KINEMATICS_CAPABILITY", + "CARTESIAN_POSE_CAPABILITY", "CompiledTrajectory", "CommandAcknowledgement", "CommandAckStatus", @@ -165,6 +182,8 @@ "CoordinatedPlacementGoal", "CoordinatedPlacementOptions", "DynamicCollisionMode", + "DisjointResourceSlots", + "DisjointSlotEndpoints", "EndEffectorPoseGoal", "EntityState", "EffectVerificationRequest", @@ -178,15 +197,19 @@ "ExecutionStatus", "ExecutionTick", "GRASP_COMMAND", + "GRASP_CAPABILITY", "GraspGoal", "HandOver", "HandOverOptions", "HeldObjectPoseGoal", "HeldObjectState", + "FORWARD_KINEMATICS_CAPABILITY", + "INVERSE_KINEMATICS_CAPABILITY", "InteractionPoints", "JointPositionGoal", "JointCommand", "JointPositionCommand", + "JOINT_POSITION_CAPABILITY", "MotionPolicy", "MonotonicExecutionClock", "MoveEndEffector", @@ -225,6 +248,9 @@ "SceneSnapshotSupplier", "SceneEntityPose", "SkillDescriptor", + "SkillBindingContract", + "SkillEndpointRequirement", + "SkillResourceSlot", "StateDelta", "SimulationExecutionAdapter", "TaskState", diff --git a/embodichain/lab/sim/atomic_actions/control.py b/embodichain/lab/sim/atomic_actions/control.py index 80be94029..d36720c17 100644 --- a/embodichain/lab/sim/atomic_actions/control.py +++ b/embodichain/lab/sim/atomic_actions/control.py @@ -44,6 +44,10 @@ class ControlCommand(ABC): def snapshot(self) -> ControlCommand: """Return an independently owned copy of this command.""" + @abstractmethod + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether ``other`` has exactly the same command semantics.""" + @dataclass(frozen=True, slots=True, eq=False, init=False) class JointPositionCommand(ControlCommand): @@ -78,6 +82,12 @@ def snapshot(self) -> JointPositionCommand: """Return an independently owned command snapshot.""" return JointPositionCommand(self._positions) + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether ``other`` owns identical joint positions.""" + return isinstance(other, JointPositionCommand) and self._positions.equal( + other._positions + ) + def resolve( self, *, @@ -131,11 +141,19 @@ def _snapshot_commands( raise TypeError(f"{field_name} must be a mapping.") snapshots: dict[str, ControlCommand] = {} for name, command in commands.items(): - if not isinstance(name, str) or not name.strip(): - raise ValueError(f"{field_name} keys must be non-empty strings.") + if not isinstance(name, str) or not name or name != name.strip(): + raise ValueError( + f"{field_name} keys must be non-empty strings without outer " + "whitespace." + ) if not isinstance(command, ControlCommand): raise TypeError(f"{field_name} values must be ControlCommand instances.") - snapshots[name] = command.snapshot() + snapshot = command.snapshot() + if not isinstance(snapshot, ControlCommand): + raise TypeError( + f"{field_name}[{name!r}].snapshot() must return a ControlCommand." + ) + snapshots[name] = snapshot return MappingProxyType(snapshots) @@ -186,8 +204,11 @@ def _snapshot_role_commands( raise TypeError(f"{field_name} must be a mapping.") snapshots: dict[str, Mapping[str, ControlCommand]] = {} for role, commands in values.items(): - if not isinstance(role, str) or not role.strip(): - raise ValueError(f"{field_name} roles must be non-empty strings.") + if not isinstance(role, str) or not role or role != role.strip(): + raise ValueError( + f"{field_name} roles must be non-empty strings without outer " + "whitespace." + ) snapshots[role] = _snapshot_commands( commands, field_name=f"{field_name}[{role!r}]", diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index d434cc000..112d1a203 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -46,6 +46,7 @@ normalize_success_mask, ) from .policies import DynamicCollisionMode +from .requirements import SkillBindingContract if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -150,6 +151,8 @@ class SkillDescriptor: manipulator_roles: tuple[str, ...] = () end_effector_roles: tuple[str, ...] = () agent_visible: bool = True + binding_contract: SkillBindingContract | None = None + """Explicit generic resource contract used by the semantic skill layer.""" def __post_init__(self) -> None: if not isinstance(self.skill_id, str) or not self.skill_id: @@ -172,6 +175,16 @@ def __post_init__(self) -> None: ): raise ValueError(f"{field_name} must contain unique non-empty roles.") object.__setattr__(self, field_name, roles) + if self.binding_contract is not None: + if not isinstance(self.binding_contract, SkillBindingContract): + raise TypeError( + "SkillDescriptor.binding_contract must be a " + "SkillBindingContract or None." + ) + self.binding_contract.validate_action_roles( + manipulator_roles=self.manipulator_roles, + end_effector_roles=self.end_effector_roles, + ) class AtomicAction(Generic[GoalT, OptionsT], ABC): @@ -200,6 +213,14 @@ class AtomicAction(Generic[GoalT, OptionsT], ABC): agent_visible: ClassVar[bool] = True """Whether an Action Agent should expose this skill by default.""" + binding_contract: ClassVar[SkillBindingContract | None] = None + """Explicit robot-independent requirements for semantic discovery. + + Concrete action classes must declare this attribute in their own class + body to opt into the semantic catalog. Inheriting another action's contract + does not silently expose a new skill identifier. + """ + def __init_subclass__(cls, **kwargs: Any) -> None: """Reject skill classes that bypass framework-owned scene binding.""" super().__init_subclass__(**kwargs) @@ -295,6 +316,7 @@ def descriptor(cls) -> SkillDescriptor: manipulator_roles=cls.manipulator_roles, end_effector_roles=cls.end_effector_roles, agent_visible=cls.agent_visible, + binding_contract=cls.__dict__.get("binding_contract"), ) def resolve_request( diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 080b3ef37..9dc97d646 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -18,11 +18,12 @@ from __future__ import annotations +from types import MappingProxyType from typing import Iterable, Mapping, TYPE_CHECKING import torch -from .core import AtomicAction +from .core import AtomicAction, SkillDescriptor from .control import ControlPartCommandProfile from .invocation import ActionInvocation, ResolvedActionRequest from .plans import ActionPlan, CompiledTrajectory, TimedTrajectory @@ -32,6 +33,12 @@ if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.planners import MotionGenerator + from embodichain.lab.sim.skills import ( + BoundRobotSkillProfile, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotSkillProfile, + ) from .execution import ExecutionSession @@ -88,6 +95,10 @@ def __init__( control_profiles: Mapping[str, ControlPartCommandProfile] | None = None, *, load_builtins: bool = True, + skill_profile: RobotSkillProfile | None = None, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, ) -> None: """Initialize one engine and bind its built-in action implementations. @@ -96,14 +107,39 @@ def __init__( control_profiles: Semantic commands keyed by robot control-part name. load_builtins: Whether to instantiate and register every built-in action. Disable this for isolated tests or fully custom engines. + skill_profile: Optional authoritative robot skill profile. Its + command profiles are installed automatically and validated + after built-in actions are loaded. ``control_profiles`` and + ``skill_profile`` are mutually exclusive. + endpoint_adapters: Optional exact-type endpoint adapters used when + binding ``skill_profile``. Invalid without a profile. """ + if endpoint_adapters is not None and skill_profile is None: + raise ValueError("endpoint_adapters requires skill_profile.") + if skill_profile is not None: + from embodichain.lab.sim.skills import RobotSkillProfile + + if not isinstance(skill_profile, RobotSkillProfile): + raise TypeError("skill_profile must be a RobotSkillProfile or None.") + if control_profiles is not None: + raise ValueError( + "control_profiles and skill_profile are mutually exclusive; " + "the profile is the authoritative semantic-command source." + ) + control_profiles = skill_profile.action_control_profiles() self._planning_services = ActionPlanningServices( motion_generator, control_profiles=control_profiles, ) self._actions: dict[str, AtomicAction] = {} + self._skill_profile: BoundRobotSkillProfile | None = None if load_builtins: self._load_builtin_actions() + if skill_profile is not None: + self._skill_profile = skill_profile.bind( + self, + endpoint_adapters=endpoint_adapters, + ) @property def motion_generator(self) -> MotionGenerator: @@ -135,6 +171,62 @@ def actions(self) -> dict[str, AtomicAction]: """Registered action instances keyed by stable skill identifier.""" return dict(self._actions) + @property + def skills(self) -> Mapping[str, SkillDescriptor]: + """Return explicitly declared, agent-visible installed skill metadata. + + Process-wide type discovery, engine installation, and semantic exposure + are separate boundaries. Only an action installed in this engine whose + concrete class explicitly declares a generic binding contract appears + here. Direct-core callers may continue to use every entry in + :attr:`actions`. + """ + return MappingProxyType( + { + skill_id: descriptor + for skill_id, action in self._actions.items() + if (descriptor := action.descriptor()).agent_visible + and descriptor.binding_contract is not None + } + ) + + @property + def skill_profile(self) -> BoundRobotSkillProfile | None: + """Return the currently bound semantic robot profile, when configured.""" + return self._skill_profile + + def bind_skill_profile( + self, + profile: RobotSkillProfile, + *, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + ) -> BoundRobotSkillProfile: + """Validate and bind a profile after custom action installation. + + The engine's immutable control-part profiles must already contain the + profile commands lowered into the current action core. Generic + non-core endpoint commands remain on resolved endpoints. Prefer the + constructor's ``skill_profile`` argument when no custom actions need + to be installed first. + + Args: + profile: Authoritative robot resource and policy profile. + endpoint_adapters: Optional exact-type endpoint adapters used for + custom controller declarations. + + Returns: + Validated profile bound to this engine and its installed actions. + """ + from embodichain.lab.sim.skills import RobotSkillProfile + + if not isinstance(profile, RobotSkillProfile): + raise TypeError("profile must be a RobotSkillProfile.") + bound = profile.bind(self, endpoint_adapters=endpoint_adapters) + self._skill_profile = bound + return bound + def register(self, action: AtomicAction, *, replace: bool = False) -> None: """Register one action instance using its descriptor. @@ -159,6 +251,7 @@ def register(self, action: AtomicAction, *, replace: bool = False) -> None: ) action._bind(self._planning_services) self._actions[descriptor.skill_id] = action + self._skill_profile = None def _load_builtin_actions(self) -> None: """Create and bind fresh built-in action instances for this engine.""" diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index c5d7900b0..83558c2bb 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -28,7 +28,7 @@ from ..affordance import AntipodalAffordance from ..bindings import ResolvedControlPart -from ..control import GRASP_COMMAND, OPEN_COMMAND +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics from ..effects import StateDelta from ..goals import ( @@ -40,6 +40,16 @@ ) from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask +from ..requirements import ( + ActionBindingRoute, + DisjointResourceSlots, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import CoordinatedHeldObjectState, PlanningContext from ..trajectory_ops import interpolate_joint_trajectory, translate_pose_world @@ -342,6 +352,32 @@ class CoordinatedPickment( OptionsType: ClassVar[type] = CoordinatedPickmentOptions manipulator_roles: ClassVar[tuple[str, ...]] = ("left", "right") end_effector_roles: ClassVar[tuple[str, ...]] = ("left", "right") + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=tuple( + SkillResourceSlot( + slot_id=role, + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({INVERSE_KINEMATICS_CAPABILITY}), + route=ActionBindingRoute("manipulator", role), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + route=ActionBindingRoute("end_effector", role), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ) + for role in ("left", "right") + ), + constraints=(DisjointResourceSlots(("left", "right")),), + ) _assemble_segment = _DualArmHelpers._assemble_segment _expand_qpos = _DualArmHelpers._expand_qpos diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index bba2c2739..e7945d018 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -27,13 +27,23 @@ from ._helpers import resolve_object_target from ..bindings import ResolvedControlPart -from ..control import GRASP_COMMAND, OPEN_COMMAND +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, normalize_success_mask from ..policies import MotionPolicy +from ..requirements import ( + ActionBindingRoute, + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( build_pose_plan_states, @@ -132,6 +142,48 @@ class CoordinatedPlacement( OptionsType: ClassVar[type] = CoordinatedPlacementOptions manipulator_roles: ClassVar[tuple[str, ...]] = ("placing", "support") end_effector_roles: ClassVar[tuple[str, ...]] = ("placing", "support") + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="placing", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + route=ActionBindingRoute("manipulator", "placing"), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + route=ActionBindingRoute("end_effector", "placing"), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + SkillResourceSlot( + slot_id="support", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + route=ActionBindingRoute("manipulator", "support"), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={GRASP_COMMAND: JointPositionCommand}, + route=ActionBindingRoute("end_effector", "support"), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + constraints=(DisjointResourceSlots(("placing", "support")),), + ) def __init__( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 120c8b5d0..4e6b87e3c 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -27,12 +27,23 @@ from embodichain.utils.math import pose_inv from ..bindings import ResolvedControlPart -from ..control import GRASP_COMMAND, OPEN_COMMAND +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics, _same_object_identity from ..effects import StateDelta from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..policies import MotionPolicy +from ..requirements import ( + ActionBindingRoute, + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( build_pose_plan_states, @@ -137,6 +148,56 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): OptionsType: ClassVar[type] = HandOverOptions manipulator_roles: ClassVar[tuple[str, ...]] = ("source", "destination") end_effector_roles: ClassVar[tuple[str, ...]] = ("source", "destination") + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="source", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + route=ActionBindingRoute("manipulator", "source"), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + route=ActionBindingRoute("end_effector", "source"), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + SkillResourceSlot( + slot_id="destination", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + route=ActionBindingRoute("manipulator", "destination"), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + route=ActionBindingRoute("end_effector", "destination"), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + constraints=(DisjointResourceSlots(("source", "destination")),), + ) def __init__( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py index d5ae8f76a..d842db5b3 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py @@ -27,6 +27,13 @@ from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan +from ..requirements import ( + ActionBindingRoute, + CARTESIAN_POSE_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import PlanningContext from ..trajectory_ops import ( build_pose_plan_states, @@ -58,6 +65,20 @@ class MoveEndEffector(AtomicAction[EndEffectorPoseGoal, MoveEndEffectorOptions]) skill_id: ClassVar[str] = "move_end_effector" GoalType: ClassVar[type] = EndEffectorPoseGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + route=ActionBindingRoute("manipulator", "primary"), + ), + ), + ), + ), + ) OptionsType: ClassVar[type] = MoveEndEffectorOptions manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) 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 917a758c7..9fca6f256 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -31,11 +31,21 @@ ) from ._helpers import arm_qpos_from_state, resolve_object_target -from ..control import GRASP_COMMAND +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 +from ..requirements import ( + ActionBindingRoute, + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import PlanningContext from ..trajectory_ops import build_pose_plan_states @@ -89,6 +99,32 @@ class MoveHeldObject(AtomicAction[HeldObjectPoseGoal, MoveHeldObjectOptions]): OptionsType: ClassVar[type] = MoveHeldObjectOptions manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + route=ActionBindingRoute("manipulator", "primary"), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={GRASP_COMMAND: JointPositionCommand}, + route=ActionBindingRoute("end_effector", "primary"), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) def __init__( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index c2693d091..a06eed0fc 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -26,6 +26,13 @@ from ..core import AtomicAction from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan +from ..requirements import ( + ActionBindingRoute, + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import PlanningContext from ..trajectory_ops import ( build_joint_plan_states, @@ -75,6 +82,20 @@ class MoveJoints(AtomicAction[JointPositionGoal, MoveJointsOptions]): OptionsType: ClassVar[type] = MoveJointsOptions manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) agent_visible: ClassVar[bool] = False + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + route=ActionBindingRoute("manipulator", "primary"), + ), + ), + ), + ), + ) def __init__( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index 89832e50a..34c183814 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -35,7 +35,7 @@ from ._helpers import arm_qpos_from_state from ..affordance import AntipodalAffordance from ..bindings import ResolvedControlPart -from ..control import GRASP_COMMAND, OPEN_COMMAND +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics from ..effects import StateDelta from ..goals import ( @@ -48,6 +48,17 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..policies import MotionPolicy +from ..requirements import ( + ActionBindingRoute, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( build_pose_plan_states, @@ -155,6 +166,36 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): OptionsType: ClassVar[type] = PickUpOptions manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + route=ActionBindingRoute("manipulator", "primary"), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + route=ActionBindingRoute("end_effector", "primary"), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) def __init__( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index 7218a5d27..3edf8ce70 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -29,7 +29,7 @@ from ._helpers import arm_qpos_from_state, resolve_object_target from ..affordance import AssembleAffordance -from ..control import GRASP_COMMAND, OPEN_COMMAND +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction from ..effects import StateDelta from ..goals import ( @@ -40,6 +40,16 @@ ) from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan +from ..requirements import ( + ActionBindingRoute, + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import PlanningContext from ..trajectory_ops import ( build_pose_plan_states, @@ -162,6 +172,35 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): OptionsType: ClassVar[type] = PlaceOptions manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + route=ActionBindingRoute("manipulator", "primary"), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + route=ActionBindingRoute("end_effector", "primary"), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) def __init__( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index a57e44bb8..eadcb425f 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -26,11 +26,21 @@ from embodichain.utils import logger from ._helpers import arm_qpos_from_state -from ..control import GRASP_COMMAND +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 +from ..requirements import ( + ActionBindingRoute, + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import PlanningContext from ..trajectory_ops import ( build_joint_plan_states, @@ -73,6 +83,32 @@ class Press(AtomicAction[PressGoal, PressOptions]): OptionsType: ClassVar[type] = PressOptions manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + route=ActionBindingRoute("manipulator", "primary"), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={GRASP_COMMAND: JointPositionCommand}, + route=ActionBindingRoute("end_effector", "primary"), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) def __init__( self, diff --git a/embodichain/lab/sim/atomic_actions/requirements.py b/embodichain/lab/sim/atomic_actions/requirements.py new file mode 100644 index 000000000..1e12aa610 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/requirements.py @@ -0,0 +1,380 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Robot-independent resource requirements published by atomic skills.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from types import MappingProxyType +from typing import Literal, Mapping + +from .control import ControlCommand + +JOINT_POSITION_CAPABILITY = "motion.joint_position" +"""Capability for planning and executing joint-position motion.""" + +CARTESIAN_POSE_CAPABILITY = "motion.cartesian_pose" +"""Capability for planning and executing Cartesian-pose motion.""" + +FORWARD_KINEMATICS_CAPABILITY = "kinematics.forward" +"""Capability for resolving forward kinematics for an endpoint.""" + +INVERSE_KINEMATICS_CAPABILITY = "kinematics.inverse" +"""Capability for resolving inverse kinematics for an endpoint.""" + +BATCH_INVERSE_KINEMATICS_CAPABILITY = "kinematics.batch_inverse" +"""Capability for resolving batched inverse kinematics for an endpoint.""" + +GRASP_CAPABILITY = "interaction.grasp" +"""Capability for commanding a grasping end effector.""" + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one strict, whitespace-free identifier.""" + if not isinstance(value, 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 _normalize_identifiers( + values: frozenset[str], + *, + field_name: str, +) -> frozenset[str]: + """Validate one immutable identifier set.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must be an iterable of strings, not a string.") + try: + normalized = frozenset(values) + except TypeError as exc: + raise TypeError(f"{field_name} must be an iterable of strings.") from exc + for value in normalized: + _validate_identifier(value, field_name=field_name) + return normalized + + +@dataclass(frozen=True, slots=True) +class ActionBindingRoute: + """Lower one generic resource endpoint into the current action core. + + This is deliberately a transition adapter. Robot resources and skill-local + slots remain generic; only this route names the two maps currently exposed + by :class:`~embodichain.lab.sim.atomic_actions.ActionBinding`. + """ + + target: Literal["manipulator", "end_effector"] + """Current core binding namespace.""" + + role: str + """Action-local role within the selected namespace.""" + + def __post_init__(self) -> None: + if self.target not in ("manipulator", "end_effector"): + raise ValueError( + "ActionBindingRoute.target must be 'manipulator' or 'end_effector'." + ) + _validate_identifier(self.role, field_name="ActionBindingRoute.role") + + @property + def key(self) -> tuple[str, str]: + """Return the normalized core target key.""" + return self.target, self.role + + +def _normalize_required_commands( + values: Mapping[str, type[ControlCommand]], +) -> Mapping[str, type[ControlCommand]]: + """Validate and freeze endpoint command requirements.""" + if not isinstance(values, Mapping): + raise TypeError("required_commands must be a mapping.") + normalized: dict[str, type[ControlCommand]] = {} + for name, command_type in values.items(): + _validate_identifier(name, field_name="required command names") + if not isinstance(command_type, type) or not issubclass( + command_type, ControlCommand + ): + raise TypeError( + "required_commands values must be ControlCommand subclasses." + ) + normalized[name] = command_type + return MappingProxyType(normalized) + + +@dataclass(frozen=True, slots=True) +class SkillEndpointRequirement: + """Capabilities and commands required from one slot-local endpoint.""" + + endpoint_id: str + """Endpoint selector local to the containing participant slot.""" + + capabilities: frozenset[str] = frozenset() + """Open, namespaced all-of capability identifiers.""" + + required_commands: Mapping[str, type[ControlCommand]] = field(default_factory=dict) + """Semantic command names and their required typed command contracts.""" + + route: ActionBindingRoute | None = None + """Optional lowering route into the current atomic-action core.""" + + def __post_init__(self) -> None: + _validate_identifier( + self.endpoint_id, + field_name="SkillEndpointRequirement.endpoint_id", + ) + object.__setattr__( + self, + "capabilities", + _normalize_identifiers( + self.capabilities, + field_name="SkillEndpointRequirement.capabilities", + ), + ) + object.__setattr__( + self, + "required_commands", + _normalize_required_commands(self.required_commands), + ) + if self.route is not None and not isinstance(self.route, ActionBindingRoute): + raise TypeError("route must be an ActionBindingRoute or None.") + + +@dataclass(frozen=True, slots=True) +class DisjointSlotEndpoints: + """Require selected endpoints within one participant to be disjoint.""" + + endpoint_ids: tuple[str, ...] + + def __post_init__(self) -> None: + if isinstance(self.endpoint_ids, (str, bytes)): + raise TypeError("endpoint_ids must be an iterable of endpoint IDs.") + try: + endpoint_ids = tuple(self.endpoint_ids) + except TypeError as exc: + raise TypeError( + "endpoint_ids must be an iterable of endpoint IDs." + ) from exc + if len(endpoint_ids) < 2: + raise ValueError("DisjointSlotEndpoints requires at least two endpoints.") + for endpoint_id in endpoint_ids: + _validate_identifier( + endpoint_id, + field_name="DisjointSlotEndpoints.endpoint_ids", + ) + if len(set(endpoint_ids)) != len(endpoint_ids): + raise ValueError("DisjointSlotEndpoints.endpoint_ids must be unique.") + object.__setattr__(self, "endpoint_ids", endpoint_ids) + + +@dataclass(frozen=True, slots=True) +class SkillResourceSlot: + """One skill-local participant selected as an indivisible resource unit.""" + + slot_id: str + """Skill-local participant name, such as ``primary`` or ``source``.""" + + endpoints: tuple[SkillEndpointRequirement, ...] + """Endpoint requirements that the selected robot resource must satisfy.""" + + constraints: tuple[DisjointSlotEndpoints, ...] = () + """Physical constraints among endpoint views in this participant.""" + + def __post_init__(self) -> None: + _validate_identifier(self.slot_id, field_name="SkillResourceSlot.slot_id") + if isinstance(self.endpoints, (str, bytes)): + raise TypeError( + "SkillResourceSlot.endpoints must be an iterable of endpoint " + "requirements." + ) + try: + endpoints = tuple(self.endpoints) + except TypeError as exc: + raise TypeError( + "SkillResourceSlot.endpoints must be an iterable of endpoint " + "requirements." + ) from exc + if not endpoints or not all( + isinstance(endpoint, SkillEndpointRequirement) for endpoint in endpoints + ): + raise ValueError( + "SkillResourceSlot.endpoints must contain at least one " + "SkillEndpointRequirement." + ) + endpoint_ids = [endpoint.endpoint_id for endpoint in endpoints] + if len(set(endpoint_ids)) != len(endpoint_ids): + raise ValueError( + f"Skill resource slot {self.slot_id!r} contains duplicate endpoint " + "identifiers." + ) + object.__setattr__(self, "endpoints", endpoints) + if isinstance(self.constraints, (str, bytes)): + raise TypeError( + "SkillResourceSlot.constraints must be an iterable of endpoint " + "constraints." + ) + try: + constraints = tuple(self.constraints) + except TypeError as exc: + raise TypeError( + "SkillResourceSlot.constraints must be an iterable of endpoint " + "constraints." + ) from exc + if not all( + isinstance(constraint, DisjointSlotEndpoints) for constraint in constraints + ): + raise TypeError( + "SkillResourceSlot.constraints values must be " + "DisjointSlotEndpoints instances." + ) + known_endpoints = set(endpoint_ids) + for constraint in constraints: + unknown = sorted(set(constraint.endpoint_ids) - known_endpoints) + if unknown: + raise ValueError( + f"Slot {self.slot_id!r} constraint references unknown endpoints " + f"{unknown}; known endpoints are {sorted(known_endpoints)}." + ) + object.__setattr__(self, "constraints", constraints) + + +@dataclass(frozen=True, slots=True) +class DisjointResourceSlots: + """Require selected slots to have pairwise-disjoint physical claims.""" + + slots: tuple[str, ...] + + def __post_init__(self) -> None: + if isinstance(self.slots, (str, bytes)): + raise TypeError("DisjointResourceSlots.slots must be an iterable.") + try: + slots = tuple(self.slots) + except TypeError as exc: + raise TypeError("DisjointResourceSlots.slots must be an iterable.") from exc + if len(slots) < 2: + raise ValueError("DisjointResourceSlots requires at least two slots.") + for slot in slots: + _validate_identifier(slot, field_name="DisjointResourceSlots.slots") + if len(set(slots)) != len(slots): + raise ValueError("DisjointResourceSlots.slots must be unique.") + object.__setattr__(self, "slots", slots) + + +@dataclass(frozen=True, slots=True) +class SkillBindingContract: + """Complete robot-independent binding contract for one atomic skill. + + ``slots=()`` explicitly declares that a skill consumes no robot resource. + ``None`` on :class:`~embodichain.lab.sim.atomic_actions.SkillDescriptor` + instead means that no semantic binding contract was declared. + """ + + slots: tuple[SkillResourceSlot, ...] = () + constraints: tuple[DisjointResourceSlots, ...] = () + + def __post_init__(self) -> None: + if isinstance(self.slots, (str, bytes)): + raise TypeError("slots must be an iterable of SkillResourceSlot values.") + try: + slots = tuple(self.slots) + except TypeError as exc: + raise TypeError( + "slots must be an iterable of SkillResourceSlot values." + ) from exc + if not all(isinstance(slot, SkillResourceSlot) for slot in slots): + raise TypeError("slots values must be SkillResourceSlot instances.") + slot_ids = [slot.slot_id for slot in slots] + if len(set(slot_ids)) != len(slot_ids): + raise ValueError("SkillBindingContract slot identifiers must be unique.") + if isinstance(self.constraints, (str, bytes)): + raise TypeError( + "constraints must be an iterable of DisjointResourceSlots values." + ) + try: + constraints = tuple(self.constraints) + except TypeError as exc: + raise TypeError( + "constraints must be an iterable of DisjointResourceSlots values." + ) from exc + if not all( + isinstance(constraint, DisjointResourceSlots) for constraint in constraints + ): + raise TypeError( + "constraints values must be DisjointResourceSlots instances." + ) + known_slots = set(slot_ids) + for constraint in constraints: + unknown = sorted(set(constraint.slots) - known_slots) + if unknown: + raise ValueError( + f"Resource constraint references unknown slots {unknown}; " + f"known slots are {sorted(known_slots)}." + ) + routes = [ + endpoint.route.key + for slot in slots + for endpoint in slot.endpoints + if endpoint.route is not None + ] + if len(set(routes)) != len(routes): + raise ValueError("Action binding routes must target unique core roles.") + object.__setattr__(self, "slots", slots) + object.__setattr__(self, "constraints", constraints) + + @property + def slot_ids(self) -> tuple[str, ...]: + """Return required slot identifiers in declaration order.""" + return tuple(slot.slot_id for slot in self.slots) + + def validate_action_roles( + self, + *, + manipulator_roles: tuple[str, ...], + end_effector_roles: tuple[str, ...], + ) -> None: + """Require lowering routes to cover the current core roles exactly.""" + expected = {("manipulator", role) for role in manipulator_roles} + expected.update(("end_effector", role) for role in end_effector_roles) + actual = { + endpoint.route.key + for slot in self.slots + for endpoint in slot.endpoints + if endpoint.route is not None + } + if actual != expected: + missing = sorted(expected - actual) + extra = sorted(actual - expected) + raise ValueError( + "Skill binding routes do not exactly cover the action roles: " + f"missing={missing}, extra={extra}." + ) + + +__all__ = [ + "ActionBindingRoute", + "BATCH_INVERSE_KINEMATICS_CAPABILITY", + "CARTESIAN_POSE_CAPABILITY", + "DisjointResourceSlots", + "DisjointSlotEndpoints", + "FORWARD_KINEMATICS_CAPABILITY", + "GRASP_CAPABILITY", + "INVERSE_KINEMATICS_CAPABILITY", + "JOINT_POSITION_CAPABILITY", + "SkillBindingContract", + "SkillEndpointRequirement", + "SkillResourceSlot", +] diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index f07a9222b..7a990fb28 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -18,6 +18,25 @@ from __future__ import annotations +from .profiles import ( + AmbiguousSkillBindingError, + BoundRobotSkillProfile, + ControlPartEndpoint, + ControlPartEndpointAdapter, + EndpointResolution, + ProfileValidationError, + ResolvedResourceEndpoint, + ResolvedRobotResource, + ResolvedSkillBinding, + ResourceBinding, + ResourceClaim, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, + UnsupportedSkillError, +) from .scene import ( RegistrySceneProvider, SceneAffordanceRef, @@ -35,7 +54,22 @@ ) __all__ = [ + "AmbiguousSkillBindingError", + "BoundRobotSkillProfile", + "ControlPartEndpoint", + "ControlPartEndpointAdapter", + "EndpointResolution", + "ProfileValidationError", "RegistrySceneProvider", + "ResolvedRobotResource", + "ResolvedResourceEndpoint", + "ResolvedSkillBinding", + "ResourceBinding", + "ResourceClaim", + "ResourceEndpoint", + "ResourceEndpointAdapter", + "RobotResource", + "RobotSkillProfile", "SceneAffordanceRef", "SceneArticulationRef", "SceneCollisionRole", @@ -48,4 +82,6 @@ "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "SkillPolicyPreset", + "UnsupportedSkillError", ] diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py new file mode 100644 index 000000000..0e1d1a8c9 --- /dev/null +++ b/embodichain/lab/sim/skills/profiles.py @@ -0,0 +1,1804 @@ +# ---------------------------------------------------------------------------- +# 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 robot resources, skill binding, and policy presets.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from copy import deepcopy +from dataclasses import dataclass, field +from itertools import product +from types import MappingProxyType +from typing import ClassVar, Mapping, TYPE_CHECKING + +from embodichain.lab.sim.atomic_actions.bindings import ActionBinding +from embodichain.lab.sim.atomic_actions.control import ( + ControlCommand, + ControlPartCommandProfile, + JointPositionCommand, +) +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.requirements import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + SkillBindingContract, + SkillResourceSlot, +) +from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg + +if TYPE_CHECKING: + from embodichain.lab.sim.atomic_actions.engine import AtomicActionEngine + + +class ProfileValidationError(ValueError): + """Raised when a robot skill profile disagrees with its engine or robot.""" + + +class UnsupportedSkillError(ValueError): + """Raised when no robot-resource assignment can satisfy a skill.""" + + +class AmbiguousSkillBindingError(ValueError): + """Raised when multiple assignments remain without a complete default.""" + + +_SOLVER_BACKED_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + } +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one strict, whitespace-free identifier.""" + if not isinstance(value, 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 _normalize_identifier_set( + values: frozenset[str], + *, + field_name: str, +) -> frozenset[str]: + """Validate one immutable set of identifiers.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must be an iterable of strings, not a string.") + try: + normalized = frozenset(values) + except TypeError as exc: + raise TypeError(f"{field_name} must be an iterable of strings.") from exc + for value in normalized: + _validate_identifier(value, field_name=field_name) + return normalized + + +def _snapshot_endpoint_commands( + values: Mapping[str, ControlCommand], + *, + field_name: str, +) -> Mapping[str, ControlCommand]: + """Validate, snapshot, and freeze commands exposed by one endpoint.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + snapshots: dict[str, ControlCommand] = {} + for command_name, command in values.items(): + _validate_identifier(command_name, field_name=f"{field_name} keys") + if not isinstance(command, ControlCommand): + raise TypeError(f"{field_name} values must be ControlCommand instances.") + snapshot = command.snapshot() + if not isinstance(snapshot, ControlCommand): + raise TypeError( + f"{field_name}[{command_name!r}].snapshot() must return a " + "ControlCommand." + ) + snapshots[command_name] = snapshot + return MappingProxyType(snapshots) + + +@dataclass(frozen=True, slots=True, kw_only=True) +class ResourceEndpoint(ABC): + """Extensible execution endpoint in a robot resource graph. + + Endpoint subclasses add controller-specific addressing data. Capabilities + stay on this common base so skill matching does not depend on any one + controller kind. + """ + + capabilities: frozenset[str] = frozenset() + """Open, namespaced capabilities provided by this exact endpoint.""" + + def __post_init__(self) -> None: + object.__setattr__( + self, + "capabilities", + _normalize_identifier_set( + self.capabilities, + field_name="ResourceEndpoint.capabilities", + ), + ) + + def snapshot(self) -> ResourceEndpoint: + """Return an independently owned endpoint declaration. + + Endpoint subclasses with payloads that cannot be deep-copied must + override this method and return a new value of their exact type. + """ + return deepcopy(self) + + +@dataclass(frozen=True, slots=True) +class ControlPartEndpoint(ResourceEndpoint): + """One named execution endpoint backed by a robot control part. + + Capabilities are explicit and never inferred from the endpoint name, joint + count, other endpoints, or composite resource members. + """ + + control_part: str + """Key from the bound robot's ``control_parts`` mapping.""" + + command_profile: str | None = None + """Optional generic command-profile ID; defaults to ``control_part``.""" + + def __post_init__(self) -> None: + ResourceEndpoint.__post_init__(self) + _validate_identifier( + self.control_part, + field_name="ControlPartEndpoint.control_part", + ) + if self.command_profile is not None: + _validate_identifier( + self.command_profile, + field_name="ControlPartEndpoint.command_profile", + ) + + +@dataclass(frozen=True, slots=True) +class EndpointResolution: + """Adapter-produced physical and lowering metadata for one endpoint.""" + + binding_values: Mapping[str, str] = field(default_factory=dict) + """Values supported for each current or future binding namespace.""" + + command_profile_key: str | None = None + """Profile key that owns semantic commands for this endpoint, when any.""" + + requires_command_profile: bool = False + """Whether a missing ``command_profile_key`` entry invalidates binding.""" + + claim_tokens: frozenset[str] = frozenset() + """Adapter-defined physical/controller claims beyond robot joint IDs.""" + + joint_ids: tuple[int, ...] = () + """Ordered robot joint IDs controlled by the endpoint, when applicable.""" + + exclusive: bool = True + """Whether this execution endpoint must declare a physical claim.""" + + def __post_init__(self) -> None: + object.__setattr__( + self, + "binding_values", + _normalize_named_mapping( + self.binding_values, + field_name="EndpointResolution.binding_values", + ), + ) + if self.command_profile_key is not None: + _validate_identifier( + self.command_profile_key, + field_name="EndpointResolution.command_profile_key", + ) + if not isinstance(self.requires_command_profile, bool): + raise TypeError("requires_command_profile must be a bool.") + if self.requires_command_profile and self.command_profile_key is None: + raise ValueError( + "requires_command_profile needs a non-None command_profile_key." + ) + object.__setattr__( + self, + "claim_tokens", + _normalize_identifier_set( + self.claim_tokens, + field_name="EndpointResolution.claim_tokens", + ), + ) + joint_ids = tuple(self.joint_ids) + 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( + "EndpointResolution.joint_ids must be non-negative integers." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ValueError("EndpointResolution.joint_ids must be unique.") + object.__setattr__(self, "joint_ids", joint_ids) + if not isinstance(self.exclusive, bool): + raise TypeError("EndpointResolution.exclusive must be a bool.") + if self.exclusive and not joint_ids and not self.claim_tokens: + raise ValueError( + "An exclusive EndpointResolution must declare joint_ids or " + "claim_tokens." + ) + + +class ResourceEndpointAdapter(ABC): + """Resolve one endpoint kind without coupling profiles to its controller.""" + + adapter_id: ClassVar[str] + """Stable adapter identifier used in diagnostics and resolved metadata.""" + + endpoint_type: ClassVar[type[ResourceEndpoint]] + """Exact endpoint declaration type accepted by this adapter.""" + + @abstractmethod + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + """Validate and resolve one endpoint against an action engine. + + Args: + endpoint: Endpoint declaration of :attr:`endpoint_type`. + engine: Engine whose robot, planner, and command profiles are bound. + + Returns: + Physical claims and supported lowering metadata. + """ + + +class ControlPartEndpointAdapter(ResourceEndpointAdapter): + """Resolve joint-backed :class:`ControlPartEndpoint` declarations.""" + + adapter_id: ClassVar[str] = "control_part" + endpoint_type: ClassVar[type[ResourceEndpoint]] = ControlPartEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + """Resolve a robot control part and verify its standard capabilities.""" + if not isinstance(endpoint, ControlPartEndpoint): + raise TypeError("ControlPartEndpointAdapter requires ControlPartEndpoint.") + control_parts = getattr(engine.robot, "control_parts", None) + if not isinstance(control_parts, Mapping): + raise ProfileValidationError( + "ControlPartEndpoint requires Robot.control_parts." + ) + if endpoint.control_part not in control_parts: + available = sorted(str(name) for name in control_parts) + raise ProfileValidationError( + f"ControlPartEndpoint references unknown control part " + f"{endpoint.control_part!r}; Robot.control_parts contains " + f"{available}." + ) + joint_ids = tuple(engine.robot.get_joint_ids(name=endpoint.control_part)) + if not joint_ids: + raise ProfileValidationError( + f"Control part {endpoint.control_part!r} contains no joints." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ProfileValidationError( + f"Control part {endpoint.control_part!r} contains duplicate joint IDs." + ) + declared = endpoint.capabilities & _SOLVER_BACKED_CAPABILITIES + if declared: + get_solver = getattr(engine.robot, "get_solver", None) + if not callable(get_solver): + raise ProfileValidationError( + f"Control part {endpoint.control_part!r} declares solver-backed " + f"capabilities {sorted(declared)}, but the robot exposes no " + "get_solver()." + ) + try: + solver = get_solver(name=endpoint.control_part) + except Exception as exc: + raise ProfileValidationError( + f"Could not validate solver-backed capabilities for control " + f"part {endpoint.control_part!r}: {exc}" + ) from exc + if solver is None: + raise ProfileValidationError( + f"Control part {endpoint.control_part!r} declares solver-backed " + f"capabilities {sorted(declared)}, but has no configured solver." + ) + return EndpointResolution( + binding_values={ + "manipulator": endpoint.control_part, + "end_effector": endpoint.control_part, + }, + command_profile_key=( + endpoint.control_part + if endpoint.command_profile is None + else endpoint.command_profile + ), + requires_command_profile=endpoint.command_profile is not None, + claim_tokens=frozenset({f"robot.control_part:{endpoint.control_part}"}), + joint_ids=joint_ids, + ) + + +@dataclass(frozen=True, slots=True) +class ResolvedResourceEndpoint: + """Endpoint declaration resolved by one registered adapter.""" + + endpoint: ResourceEndpoint + adapter_id: str + binding_values: Mapping[str, str] = field(default_factory=dict) + command_profile_key: str | None = None + requires_command_profile: bool = False + commands: Mapping[str, ControlCommand] = field(default_factory=dict) + claim_tokens: frozenset[str] = frozenset() + joint_ids: tuple[int, ...] = () + exclusive: bool = True + + def __post_init__(self) -> None: + if not isinstance(self.endpoint, ResourceEndpoint): + raise TypeError("endpoint must be a ResourceEndpoint.") + endpoint_snapshot = self.endpoint.snapshot() + if ( + type(endpoint_snapshot) is not type(self.endpoint) + or endpoint_snapshot is self.endpoint + ): + raise TypeError( + "endpoint.snapshot() must return an independently owned value of " + "the same endpoint type." + ) + object.__setattr__(self, "endpoint", endpoint_snapshot) + _validate_identifier( + self.adapter_id, + field_name="ResolvedResourceEndpoint.adapter_id", + ) + resolution = EndpointResolution( + binding_values=self.binding_values, + command_profile_key=self.command_profile_key, + requires_command_profile=self.requires_command_profile, + claim_tokens=self.claim_tokens, + joint_ids=self.joint_ids, + exclusive=self.exclusive, + ) + object.__setattr__(self, "binding_values", resolution.binding_values) + object.__setattr__( + self, + "command_profile_key", + resolution.command_profile_key, + ) + object.__setattr__( + self, + "requires_command_profile", + resolution.requires_command_profile, + ) + object.__setattr__( + self, + "commands", + _snapshot_endpoint_commands( + self.commands, + field_name="ResolvedResourceEndpoint.commands", + ), + ) + object.__setattr__(self, "claim_tokens", resolution.claim_tokens) + object.__setattr__(self, "joint_ids", resolution.joint_ids) + object.__setattr__(self, "exclusive", resolution.exclusive) + + @property + def capabilities(self) -> frozenset[str]: + """Return capabilities declared by the source endpoint.""" + return self.endpoint.capabilities + + def conflicts_with(self, other: ResolvedResourceEndpoint) -> bool: + """Return whether two endpoints address overlapping physical channels.""" + if not isinstance(other, ResolvedResourceEndpoint): + raise TypeError("other must be a ResolvedResourceEndpoint.") + return bool( + self.claim_tokens & other.claim_tokens + or set(self.joint_ids) & set(other.joint_ids) + ) + + +def _normalize_endpoints( + values: Mapping[str, ResourceEndpoint], +) -> Mapping[str, ResourceEndpoint]: + """Validate and freeze resource endpoint declarations.""" + if not isinstance(values, Mapping): + raise TypeError("RobotResource.endpoints must be a mapping.") + normalized: dict[str, ResourceEndpoint] = {} + for endpoint_id, endpoint in values.items(): + _validate_identifier(endpoint_id, field_name="resource endpoint identifiers") + if not isinstance(endpoint, ResourceEndpoint): + raise TypeError( + "RobotResource.endpoints values must be ResourceEndpoint " "instances." + ) + snapshot = endpoint.snapshot() + if type(snapshot) is not type(endpoint) or snapshot is endpoint: + raise TypeError( + f"Endpoint {endpoint_id!r}.snapshot() must return an independently " + f"owned {type(endpoint).__name__}." + ) + normalized[endpoint_id] = snapshot + return MappingProxyType(normalized) + + +@dataclass(frozen=True, slots=True) +class RobotResource: + """Generic leaf or composite resource in one robot's resource DAG. + + A resource may expose any number of named endpoints. For example, one + manipulation participant may expose ``motion`` and ``grasp`` endpoints, + while a mobile base or whole-body controller may expose only ``motion``. + ``members`` describes physical claim composition and does not inherit + endpoint capabilities. + """ + + resource_id: str + endpoints: Mapping[str, ResourceEndpoint] = field(default_factory=dict) + members: tuple[str, ...] = () + + def __post_init__(self) -> None: + _validate_identifier(self.resource_id, field_name="RobotResource.resource_id") + object.__setattr__(self, "endpoints", _normalize_endpoints(self.endpoints)) + if isinstance(self.members, (str, bytes)): + raise TypeError( + "RobotResource.members must be an iterable of strings, not a string." + ) + try: + members = tuple(self.members) + except TypeError as exc: + raise TypeError( + "RobotResource.members must be an iterable of strings." + ) from exc + for member in members: + _validate_identifier(member, field_name="RobotResource.members") + if len(set(members)) != len(members): + raise ValueError("RobotResource.members must be unique.") + if self.resource_id in members: + raise ValueError("A robot resource cannot contain itself.") + if not members and not self.endpoints: + raise ValueError( + "A leaf RobotResource must expose at least one execution endpoint." + ) + object.__setattr__(self, "members", members) + + def snapshot(self) -> RobotResource: + """Return an independently owned resource declaration.""" + return RobotResource( + resource_id=self.resource_id, + endpoints=self.endpoints, + members=self.members, + ) + + +@dataclass(frozen=True, slots=True) +class ResourceBinding: + """Generic mapping from skill-local slots to robot resource IDs.""" + + resources: Mapping[str, str] + + def __post_init__(self) -> None: + if not isinstance(self.resources, Mapping): + raise TypeError("ResourceBinding.resources must be a mapping.") + normalized: dict[str, str] = {} + for slot_id, resource_id in self.resources.items(): + _validate_identifier(slot_id, field_name="ResourceBinding slot IDs") + _validate_identifier(resource_id, field_name="ResourceBinding resource IDs") + normalized[slot_id] = resource_id + object.__setattr__(self, "resources", MappingProxyType(normalized)) + + +@dataclass(frozen=True, slots=True, init=False) +class SkillPolicyPreset: + """Versioned planning, recovery, and runner policy bundle.""" + + preset_id: str + schema_version: int + _motion_policy: MotionPolicy + _recovery_policy: RecoveryPolicy + _runner_cfg: ExecutionRunnerCfg + + def __init__( + self, + preset_id: str, + schema_version: int = 1, + motion_policy: MotionPolicy | None = None, + recovery_policy: RecoveryPolicy | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> None: + """Own one policy bundle without exposing mutable nested configuration.""" + _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: + raise ValueError( + "Unsupported SkillPolicyPreset.schema_version " + f"{schema_version}; supported versions are [1]." + ) + selected_motion = MotionPolicy() if motion_policy is None else motion_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_recovery, RecoveryPolicy): + raise TypeError("recovery_policy must be a RecoveryPolicy.") + if not isinstance(selected_runner, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg.") + object.__setattr__(self, "preset_id", preset_id) + object.__setattr__(self, "schema_version", schema_version) + object.__setattr__(self, "_motion_policy", deepcopy(selected_motion)) + object.__setattr__(self, "_recovery_policy", deepcopy(selected_recovery)) + object.__setattr__(self, "_runner_cfg", deepcopy(selected_runner)) + + @property + def motion_policy(self) -> MotionPolicy: + """Return an independently owned motion policy.""" + return deepcopy(self._motion_policy) + + @property + def recovery_policy(self) -> RecoveryPolicy: + """Return an independently owned recovery policy.""" + return deepcopy(self._recovery_policy) + + @property + def runner_cfg(self) -> ExecutionRunnerCfg: + """Return an independently owned runner configuration.""" + return deepcopy(self._runner_cfg) + + def snapshot(self) -> SkillPolicyPreset: + """Return an independently owned preset value.""" + return SkillPolicyPreset( + preset_id=self.preset_id, + schema_version=self.schema_version, + motion_policy=self.motion_policy, + recovery_policy=self.recovery_policy, + runner_cfg=self.runner_cfg, + ) + + +@dataclass(frozen=True, slots=True) +class ResourceClaim: + """Physical leaf and joint claim used for deterministic conflict checks.""" + + leaf_resource_ids: frozenset[str] + joint_ids: tuple[int, ...] + claim_tokens: frozenset[str] = frozenset() + + def __post_init__(self) -> None: + object.__setattr__( + self, + "leaf_resource_ids", + _normalize_identifier_set( + self.leaf_resource_ids, + field_name="ResourceClaim.leaf_resource_ids", + ), + ) + joint_ids = tuple(self.joint_ids) + 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("ResourceClaim.joint_ids must be non-negative integers.") + if tuple(sorted(set(joint_ids))) != joint_ids: + raise ValueError("ResourceClaim.joint_ids must be sorted and unique.") + object.__setattr__(self, "joint_ids", joint_ids) + object.__setattr__( + self, + "claim_tokens", + _normalize_identifier_set( + self.claim_tokens, + field_name="ResourceClaim.claim_tokens", + ), + ) + + def conflicts_with(self, other: ResourceClaim) -> bool: + """Return whether two claims overlap in a leaf or concrete joint.""" + if not isinstance(other, ResourceClaim): + raise TypeError("other must be a ResourceClaim.") + return bool( + self.leaf_resource_ids & other.leaf_resource_ids + or self.claim_tokens & other.claim_tokens + or set(self.joint_ids) & set(other.joint_ids) + ) + + @classmethod + def combine(cls, claims: tuple[ResourceClaim, ...]) -> ResourceClaim: + """Return the union of zero or more resource claims.""" + leaves: set[str] = set() + joints: set[int] = set() + tokens: set[str] = set() + for claim in claims: + if not isinstance(claim, ResourceClaim): + raise TypeError("claims values must be ResourceClaim instances.") + leaves.update(claim.leaf_resource_ids) + joints.update(claim.joint_ids) + tokens.update(claim.claim_tokens) + return cls( + frozenset(leaves), + tuple(sorted(joints)), + frozenset(tokens), + ) + + +@dataclass(frozen=True, slots=True) +class ResolvedRobotResource: + """Robot-validated resource with concrete endpoint joint IDs and claim.""" + + resource_id: str + endpoints: Mapping[str, ResolvedResourceEndpoint] + members: tuple[str, ...] + claim: ResourceClaim + + def __post_init__(self) -> None: + _validate_identifier( + self.resource_id, + field_name="ResolvedRobotResource.resource_id", + ) + if not isinstance(self.endpoints, Mapping): + raise TypeError("endpoints must be a mapping.") + normalized_endpoints: dict[str, ResolvedResourceEndpoint] = {} + for endpoint_id, endpoint in self.endpoints.items(): + _validate_identifier(endpoint_id, field_name="resolved endpoint IDs") + if not isinstance(endpoint, ResolvedResourceEndpoint): + raise TypeError( + "ResolvedRobotResource.endpoints values must be " + "ResolvedResourceEndpoint instances." + ) + normalized_endpoints[endpoint_id] = endpoint + object.__setattr__( + self, + "endpoints", + MappingProxyType(normalized_endpoints), + ) + if isinstance(self.members, (str, bytes)): + raise TypeError("members must be an iterable of resource IDs.") + members = tuple(self.members) + for member in members: + _validate_identifier(member, field_name="resolved resource members") + if len(set(members)) != len(members): + raise ValueError("Resolved resource members must be unique.") + object.__setattr__(self, "members", members) + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + endpoint_joints = { + joint_id + for endpoint in normalized_endpoints.values() + for joint_id in endpoint.joint_ids + } + missing_claim_joints = sorted(endpoint_joints - set(self.claim.joint_ids)) + if missing_claim_joints: + raise ValueError( + "Resolved resource claim does not cover endpoint joints " + f"{missing_claim_joints}." + ) + endpoint_tokens = { + token + for endpoint in normalized_endpoints.values() + for token in endpoint.claim_tokens + } + missing_claim_tokens = sorted(endpoint_tokens - self.claim.claim_tokens) + if missing_claim_tokens: + raise ValueError( + "Resolved resource claim does not cover endpoint claim tokens " + f"{missing_claim_tokens}." + ) + if not members: + if self.claim.leaf_resource_ids != frozenset({self.resource_id}): + raise ValueError( + "A resolved leaf resource claim must contain exactly its own " + "resource ID." + ) + if set(self.claim.joint_ids) != endpoint_joints: + raise ValueError( + "A resolved leaf resource claim must contain exactly its " + "endpoint joints." + ) + if self.claim.claim_tokens != frozenset(endpoint_tokens): + raise ValueError( + "A resolved leaf resource claim must contain exactly its " + "endpoint claim tokens." + ) + + @property + def endpoint_joint_ids(self) -> Mapping[str, tuple[int, ...]]: + """Return ordered joint IDs for each resolved endpoint.""" + return MappingProxyType( + { + endpoint_id: endpoint.joint_ids + for endpoint_id, endpoint in self.endpoints.items() + } + ) + + +@dataclass(frozen=True, slots=True) +class ResolvedSkillBinding: + """One generic resource assignment lowered for the current action core.""" + + skill_id: str + resources: Mapping[str, ResolvedRobotResource] + action_binding: ActionBinding + claim: ResourceClaim + + def __post_init__(self) -> None: + _validate_identifier(self.skill_id, field_name="ResolvedSkillBinding.skill_id") + if not isinstance(self.resources, Mapping): + raise TypeError("resources must be a mapping.") + normalized: dict[str, ResolvedRobotResource] = {} + for slot_id, resource in self.resources.items(): + _validate_identifier(slot_id, field_name="resolved skill slot IDs") + if not isinstance(resource, ResolvedRobotResource): + raise TypeError( + "ResolvedSkillBinding.resources values must be " + "ResolvedRobotResource instances." + ) + normalized[slot_id] = resource + object.__setattr__(self, "resources", MappingProxyType(normalized)) + if not isinstance(self.action_binding, ActionBinding): + raise TypeError("action_binding must be an ActionBinding.") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + + @property + def resource_ids(self) -> Mapping[str, str]: + """Return the selected logical resource ID for each skill-local slot.""" + return MappingProxyType( + { + slot_id: resource.resource_id + for slot_id, resource in self.resources.items() + } + ) + + +def _normalize_resources( + values: Mapping[str, RobotResource], +) -> Mapping[str, RobotResource]: + """Validate profile resource ownership and mapping keys.""" + if not isinstance(values, Mapping): + raise TypeError("RobotSkillProfile.resources must be a mapping.") + normalized: dict[str, RobotResource] = {} + for resource_id, resource in values.items(): + _validate_identifier(resource_id, field_name="profile resource IDs") + if not isinstance(resource, RobotResource): + raise TypeError( + "RobotSkillProfile.resources values must be RobotResource instances." + ) + if resource_id != resource.resource_id: + raise ValueError( + f"Resource mapping key {resource_id!r} does not match " + f"RobotResource.resource_id {resource.resource_id!r}." + ) + normalized[resource_id] = resource.snapshot() + return MappingProxyType(normalized) + + +def _normalize_command_profiles( + values: Mapping[str, ControlPartCommandProfile], +) -> Mapping[str, ControlPartCommandProfile]: + """Own generic endpoint command-profile snapshots by stable profile ID.""" + if not isinstance(values, Mapping): + raise TypeError("command_profiles must be a mapping.") + normalized: dict[str, ControlPartCommandProfile] = {} + for profile_id, profile in values.items(): + _validate_identifier(profile_id, field_name="command profile IDs") + if not isinstance(profile, ControlPartCommandProfile): + raise TypeError( + "command_profiles values must be ControlPartCommandProfile instances." + ) + normalized[profile_id] = profile.snapshot() + return MappingProxyType(normalized) + + +def _normalize_defaults( + values: Mapping[str, ResourceBinding], +) -> Mapping[str, ResourceBinding]: + """Validate and freeze per-skill complete default bindings.""" + if not isinstance(values, Mapping): + raise TypeError("defaults must be a mapping.") + normalized: dict[str, ResourceBinding] = {} + for skill_id, binding in values.items(): + _validate_identifier(skill_id, field_name="default skill IDs") + if not isinstance(binding, ResourceBinding): + raise TypeError("defaults values must be ResourceBinding instances.") + normalized[skill_id] = binding + return MappingProxyType(normalized) + + +def _normalize_presets( + values: Mapping[str, SkillPolicyPreset], +) -> Mapping[str, SkillPolicyPreset]: + """Validate preset keys and own independent snapshots.""" + if not isinstance(values, Mapping): + raise TypeError("presets must be a mapping.") + normalized: dict[str, SkillPolicyPreset] = {} + for preset_id, preset in values.items(): + _validate_identifier(preset_id, field_name="preset IDs") + if not isinstance(preset, SkillPolicyPreset): + raise TypeError("presets values must be SkillPolicyPreset instances.") + if preset_id != preset.preset_id: + raise ValueError( + f"Preset mapping key {preset_id!r} does not match preset_id " + f"{preset.preset_id!r}." + ) + normalized[preset_id] = preset.snapshot() + return MappingProxyType(normalized) + + +def _normalize_named_mapping( + values: Mapping[str, str], + *, + field_name: str, +) -> Mapping[str, str]: + """Validate and freeze one identifier-to-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(): + _validate_identifier(key, field_name=f"{field_name} keys") + _validate_identifier(value, field_name=f"{field_name} values") + normalized[key] = value + return MappingProxyType(normalized) + + +def _normalize_endpoint_adapters( + values: Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None, +) -> Mapping[type[ResourceEndpoint], ResourceEndpointAdapter]: + """Install the built-in adapter plus exact-type endpoint extensions.""" + normalized: dict[type[ResourceEndpoint], ResourceEndpointAdapter] = { + ControlPartEndpoint: ControlPartEndpointAdapter() + } + if values is not None: + if not isinstance(values, Mapping): + raise TypeError("endpoint_adapters must be a mapping or None.") + for endpoint_type, adapter in values.items(): + if not isinstance(endpoint_type, type) or not issubclass( + endpoint_type, ResourceEndpoint + ): + raise TypeError( + "endpoint_adapters keys must be ResourceEndpoint subclasses." + ) + if endpoint_type is ControlPartEndpoint: + raise ValueError( + "The built-in ControlPartEndpoint adapter cannot be overridden; " + "declare a distinct ResourceEndpoint subtype for custom " + "controller semantics." + ) + if not isinstance(adapter, ResourceEndpointAdapter): + raise TypeError( + "endpoint_adapters values must be ResourceEndpointAdapter " + "instances." + ) + declared_endpoint_type = getattr(adapter, "endpoint_type", None) + if not isinstance(declared_endpoint_type, type) or not issubclass( + declared_endpoint_type, ResourceEndpoint + ): + raise TypeError( + f"Endpoint adapter {type(adapter).__name__} must declare a " + "ResourceEndpoint subclass as endpoint_type." + ) + if declared_endpoint_type is not endpoint_type: + raise ValueError( + f"Endpoint adapter {type(adapter).__name__} declares " + f"endpoint_type {declared_endpoint_type.__name__}, but is " + f"registered for {endpoint_type.__name__}." + ) + adapter_id = getattr(adapter, "adapter_id", None) + _validate_identifier( + adapter_id, + field_name="ResourceEndpointAdapter.adapter_id", + ) + normalized[endpoint_type] = adapter + adapter_ids = [ + getattr(adapter, "adapter_id", None) for adapter in normalized.values() + ] + if len(set(adapter_ids)) != len(adapter_ids): + raise ValueError("Installed ResourceEndpointAdapter IDs must be unique.") + return MappingProxyType(normalized) + + +@dataclass(frozen=True, slots=True) +class RobotSkillProfile: + """Reusable declarative skill integration for one robot embodiment.""" + + profile_id: str + resources: Mapping[str, RobotResource] + command_profiles: Mapping[str, ControlPartCommandProfile] = field( + default_factory=dict + ) + defaults: Mapping[str, ResourceBinding] = field(default_factory=dict) + presets: Mapping[str, SkillPolicyPreset] = field(default_factory=dict) + default_preset: str | None = None + skill_presets: Mapping[str, str] = field(default_factory=dict) + + def __post_init__(self) -> None: + _validate_identifier(self.profile_id, field_name="RobotSkillProfile.profile_id") + resources = _normalize_resources(self.resources) + object.__setattr__(self, "resources", resources) + object.__setattr__( + self, + "command_profiles", + _normalize_command_profiles(self.command_profiles), + ) + object.__setattr__(self, "defaults", _normalize_defaults(self.defaults)) + presets = _normalize_presets(self.presets) + object.__setattr__(self, "presets", presets) + if self.default_preset is not None: + _validate_identifier( + self.default_preset, + field_name="RobotSkillProfile.default_preset", + ) + if self.default_preset not in presets: + raise ValueError( + f"Unknown default preset {self.default_preset!r}; available " + f"presets are {sorted(presets)}." + ) + skill_presets = _normalize_named_mapping( + self.skill_presets, + field_name="skill_presets", + ) + unknown_presets = sorted(set(skill_presets.values()) - set(presets)) + if unknown_presets: + raise ValueError( + f"skill_presets references unknown presets {unknown_presets}." + ) + object.__setattr__(self, "skill_presets", skill_presets) + self._validate_resource_graph(resources) + self.action_control_profiles() + + def action_control_profiles(self) -> Mapping[str, ControlPartCommandProfile]: + """Lower endpoint command profiles for the current action core. + + Returns: + Owned command profiles keyed by concrete robot control-part name. + + Raises: + ValueError: If two endpoint declarations assign non-equivalent + commands with the same semantic name to one control part. + """ + commands_by_control_part: dict[str, dict[str, ControlCommand]] = {} + for resource in self.resources.values(): + for endpoint in resource.endpoints.values(): + if type(endpoint) is not ControlPartEndpoint: + continue + profile_id = ( + endpoint.control_part + if endpoint.command_profile is None + else endpoint.command_profile + ) + profile = self.command_profiles.get(profile_id) + if profile is None: + continue + merged = commands_by_control_part.setdefault( + endpoint.control_part, + {}, + ) + for command_name, command in profile.commands.items(): + previous = merged.get(command_name) + if previous is not None and not previous.equivalent_to(command): + raise ValueError( + f"Control part {endpoint.control_part!r} receives " + f"non-equivalent {command_name!r} commands from profile " + f"{profile_id!r}." + ) + merged[command_name] = command + return MappingProxyType( + { + control_part: ControlPartCommandProfile(commands=commands) + for control_part, commands in commands_by_control_part.items() + } + ) + + @staticmethod + def _validate_resource_graph(resources: Mapping[str, RobotResource]) -> None: + """Reject unknown members and cycles in the resource DAG.""" + for resource in resources.values(): + unknown = sorted(set(resource.members) - set(resources)) + if unknown: + raise ValueError( + f"Robot resource {resource.resource_id!r} references unknown " + f"members {unknown}." + ) + + visiting: list[str] = [] + visited: set[str] = set() + + def visit(resource_id: str) -> None: + if resource_id in visited: + return + if resource_id in visiting: + cycle_start = visiting.index(resource_id) + cycle = visiting[cycle_start:] + [resource_id] + raise ValueError( + "Robot resource graph contains a cycle: " + " -> ".join(cycle) + ) + visiting.append(resource_id) + for member in resources[resource_id].members: + visit(member) + visiting.pop() + visited.add(resource_id) + + for resource_id in resources: + visit(resource_id) + + def bind( + self, + engine: AtomicActionEngine, + *, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + ) -> BoundRobotSkillProfile: + """Validate this profile against one fully configured action engine. + + Args: + engine: Installed atomic-action engine for the target robot. + endpoint_adapters: Optional exact endpoint-type adapters. Explicit + entries extend the non-overridable built-in control-part adapter. + + Returns: + Robot-, engine-, and adapter-validated profile view. + """ + return BoundRobotSkillProfile( + self, + engine, + endpoint_adapters=endpoint_adapters, + ) + + +class BoundRobotSkillProfile: + """Robot- and engine-validated view of a :class:`RobotSkillProfile`.""" + + def __init__( + self, + profile: RobotSkillProfile, + engine: AtomicActionEngine, + *, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + ) -> None: + from embodichain.lab.sim.atomic_actions.engine import AtomicActionEngine + + if not isinstance(profile, RobotSkillProfile): + raise TypeError("profile must be a RobotSkillProfile.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + self._profile = profile + self._engine = engine + self._endpoint_adapters = _normalize_endpoint_adapters(endpoint_adapters) + self._validate_presets() + self._resources = self._resolve_resources() + self._validate_engine_control_profiles() + self._validate_leaf_ownership() + self._installed_skills = MappingProxyType(dict(engine.skills)) + self._validate_named_skill_configuration() + self._validate_defaults() + self._skills = MappingProxyType( + { + skill_id: descriptor + for skill_id, descriptor in self._installed_skills.items() + if self._assignments(descriptor.binding_contract, {}) + } + ) + + @property + def profile_id(self) -> str: + """Return the stable profile identifier.""" + return self._profile.profile_id + + @property + def resources(self) -> Mapping[str, ResolvedRobotResource]: + """Return resolved generic robot resources keyed by logical ID.""" + return self._resources + + @property + def skills(self) -> Mapping[str, SkillDescriptor]: + """Return installed semantic skills fully supported by this profile.""" + self._assert_catalog_current() + return self._skills + + def preset( + self, + preset_id: str | None = None, + *, + skill_id: str | None = None, + ) -> SkillPolicyPreset: + """Resolve an explicit, per-skill, or profile-default policy preset.""" + selected = preset_id + if skill_id is not None: + descriptor = self._require_installed_skill(skill_id) + if skill_id not in self._skills: + raise UnsupportedSkillError( + self._unsupported_message( + skill_id, + descriptor.binding_contract, + {}, + ) + ) + if selected is None: + selected = self._profile.skill_presets.get(skill_id) + if selected is None: + selected = self._profile.default_preset + if selected is None: + raise KeyError( + "No policy preset was selected and no default is configured." + ) + try: + preset = self._profile.presets[selected] + except KeyError as exc: + raise KeyError( + f"Unknown policy preset {selected!r}; available presets are " + f"{sorted(self._profile.presets)}." + ) from exc + return preset.snapshot() + + def candidates( + self, + skill_id: str, + selections: Mapping[str, str] | None = None, + ) -> tuple[ResourceBinding, ...]: + """Return every valid complete resource assignment deterministically.""" + descriptor = self._require_installed_skill(skill_id) + normalized = self._normalize_selections(descriptor, selections) + return tuple( + ResourceBinding( + resources={ + slot_id: resource.resource_id + for slot_id, resource in assignment.items() + } + ) + for assignment in self._assignments( + descriptor.binding_contract, + normalized, + ) + ) + + def resolve( + self, + skill_id: str, + selections: Mapping[str, str] | None = None, + ) -> ResolvedSkillBinding: + """Resolve one skill with strict capability matching and disambiguation.""" + descriptor = self._require_installed_skill(skill_id) + normalized = self._normalize_selections(descriptor, selections) + contract = descriptor.binding_contract + assignments = self._assignments(contract, normalized) + if not assignments: + raise UnsupportedSkillError( + self._unsupported_message(skill_id, contract, normalized) + ) + if len(assignments) == 1: + assignment = assignments[0] + else: + default = self._profile.defaults.get(skill_id) + assignment = None + if default is not None: + selected_ids = dict(default.resources) + selected_ids.update(normalized) + for candidate in assignments: + if all( + candidate[slot_id].resource_id == resource_id + for slot_id, resource_id in selected_ids.items() + ): + assignment = candidate + break + if assignment is None: + rendered = [ + "{" + + ", ".join( + f"{slot}={resource.resource_id}" + for slot, resource in candidate.items() + ) + + "}" + for candidate in assignments + ] + raise AmbiguousSkillBindingError( + f"Skill {skill_id!r} has {len(assignments)} valid resource " + f"bindings: {rendered}. Configure a complete per-skill " + "default or provide enough explicit slot selections." + ) + return self._lower_binding(skill_id, contract, assignment) + + def _require_installed_skill(self, skill_id: str) -> SkillDescriptor: + """Return one installed explicit descriptor or fail at the right boundary.""" + self._assert_catalog_current() + _validate_identifier(skill_id, field_name="skill_id") + descriptor = self._installed_skills.get(skill_id) + if descriptor is None: + raise KeyError( + f"Skill {skill_id!r} is not an installed, agent-visible skill with " + "an explicit binding contract." + ) + return descriptor + + def _assert_catalog_current(self) -> None: + """Prevent stale contracts after engine registration or replacement.""" + if dict(self._engine.skills) != dict(self._installed_skills): + raise RuntimeError( + "AtomicActionEngine semantic skills changed after the robot skill " + "profile was bound; bind the profile again before discovery or " + "resolution." + ) + + def _normalize_selections( + self, + descriptor: SkillDescriptor, + selections: Mapping[str, str] | None, + ) -> Mapping[str, str]: + """Validate caller selections against one skill's local slots.""" + normalized = _normalize_named_mapping( + {} if selections is None else selections, + field_name="selections", + ) + contract = descriptor.binding_contract + assert contract is not None + unknown_slots = sorted(set(normalized) - set(contract.slot_ids)) + if unknown_slots: + raise ValueError( + f"Skill {descriptor.skill_id!r} selections contain unknown slots " + f"{unknown_slots}; expected a subset of {list(contract.slot_ids)}." + ) + unknown_resources = sorted(set(normalized.values()) - set(self._resources)) + if unknown_resources: + raise ValueError( + f"Selections reference unknown resources {unknown_resources}; " + f"available resources are {sorted(self._resources)}." + ) + return normalized + + def _validate_presets(self) -> None: + """Validate planner-pinned presets against the selected engine backend.""" + configured = self._engine.planning_services.planner_name + for preset in self._profile.presets.values(): + required = preset.motion_policy.planner + if required is not None and required != configured: + raise ProfileValidationError( + f"Preset {preset.preset_id!r} requires planner {required!r}, " + f"but this engine uses {configured!r}." + ) + + def _validate_engine_control_profiles(self) -> None: + """Require current-core endpoint commands to be installed on the engine.""" + engine_profiles = self._engine.control_profiles + try: + expected_control_profiles = self._profile.action_control_profiles() + except (TypeError, ValueError) as exc: + raise ProfileValidationError( + f"Could not lower profile commands to action control parts: {exc}" + ) from exc + for control_part, expected in expected_control_profiles.items(): + installed = engine_profiles.get(control_part) + if installed is None: + raise ProfileValidationError( + f"Profile command set for control part {control_part!r} is not " + "installed on the AtomicActionEngine." + ) + for command_name, command in expected.commands.items(): + installed_command = installed.commands.get(command_name) + if installed_command is None: + raise ProfileValidationError( + f"Engine control profile {control_part!r} is missing profile " + f"command {command_name!r}." + ) + if not command.equivalent_to(installed_command): + raise ProfileValidationError( + f"Engine command {control_part!r}.{command_name} is not " + "semantically equivalent to the profile-owned command." + ) + for resource in self._resources.values(): + for endpoint in resource.endpoints.values(): + if not endpoint.commands: + continue + control_parts = { + value + for target, value in endpoint.binding_values.items() + if target in {"manipulator", "end_effector"} + } + for control_part in control_parts: + installed = engine_profiles.get(control_part) + if installed is None: + raise ProfileValidationError( + f"Endpoint command profile " + f"{endpoint.command_profile_key!r} for control part " + f"{control_part!r} is not installed on the " + "AtomicActionEngine." + ) + for command_name, command in endpoint.commands.items(): + installed_command = installed.commands.get(command_name) + if installed_command is None: + raise ProfileValidationError( + f"Engine control profile {control_part!r} is missing " + f"profile command {command_name!r}." + ) + if not command.equivalent_to(installed_command): + raise ProfileValidationError( + f"Engine command {control_part!r}.{command_name} is " + "not semantically equivalent to the profile-owned " + "command." + ) + + def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: + """Resolve adapter endpoints, graph closure, commands, and claims.""" + resolved_endpoints: dict[str, dict[str, ResolvedResourceEndpoint]] = {} + direct_joints: dict[str, set[int]] = {} + direct_tokens: dict[str, set[str]] = {} + for resource_id, resource in self._profile.resources.items(): + resource_endpoints: dict[str, ResolvedResourceEndpoint] = {} + for endpoint_id, endpoint in resource.endpoints.items(): + adapter = self._endpoint_adapters.get(type(endpoint)) + if adapter is None: + raise ProfileValidationError( + f"Resource {resource_id!r} endpoint {endpoint_id!r} uses " + f"unsupported endpoint type {type(endpoint).__name__}; " + "register a ResourceEndpointAdapter for that exact type." + ) + try: + resolution = adapter.resolve(endpoint, engine=self._engine) + except Exception as exc: + raise ProfileValidationError( + f"Endpoint adapter {adapter.adapter_id!r} failed for resource " + f"{resource_id!r} endpoint {endpoint_id!r}: {exc}" + ) from exc + if not isinstance(resolution, EndpointResolution): + raise ProfileValidationError( + f"Endpoint adapter {adapter.adapter_id!r} returned " + f"{type(resolution).__name__}, expected EndpointResolution." + ) + command_profile = ( + None + if resolution.command_profile_key is None + else self._profile.command_profiles.get( + resolution.command_profile_key + ) + ) + if resolution.requires_command_profile and command_profile is None: + raise ProfileValidationError( + f"Endpoint adapter {adapter.adapter_id!r} resolved resource " + f"{resource_id!r} endpoint {endpoint_id!r} to required " + f"command profile {resolution.command_profile_key!r}, but " + "the RobotSkillProfile does not define it." + ) + resource_endpoints[endpoint_id] = ResolvedResourceEndpoint( + endpoint=endpoint, + adapter_id=adapter.adapter_id, + binding_values=resolution.binding_values, + command_profile_key=resolution.command_profile_key, + requires_command_profile=resolution.requires_command_profile, + commands=( + {} if command_profile is None else command_profile.commands + ), + claim_tokens=resolution.claim_tokens, + joint_ids=resolution.joint_ids, + exclusive=resolution.exclusive, + ) + resolved_endpoints[resource_id] = resource_endpoints + direct_joints[resource_id] = { + joint_id + for endpoint in resource_endpoints.values() + for joint_id in endpoint.joint_ids + } + direct_tokens[resource_id] = { + token + for endpoint in resource_endpoints.values() + for token in endpoint.claim_tokens + } + + leaf_cache: dict[str, frozenset[str]] = {} + joint_cache: dict[str, frozenset[int]] = {} + token_cache: dict[str, frozenset[str]] = {} + + def resolve_claim( + resource_id: str, + ) -> tuple[frozenset[str], frozenset[int], frozenset[str]]: + cached_leaves = leaf_cache.get(resource_id) + if cached_leaves is not None: + return ( + cached_leaves, + joint_cache[resource_id], + token_cache[resource_id], + ) + resource = self._profile.resources[resource_id] + if not resource.members: + leaves = frozenset({resource_id}) + joints = frozenset(direct_joints[resource_id]) + tokens = frozenset(direct_tokens[resource_id]) + else: + leaves_set: set[str] = set() + member_joints: set[int] = set() + member_tokens: set[str] = set() + for member in resource.members: + member_leaves, nested_joints, nested_tokens = resolve_claim(member) + leaves_set.update(member_leaves) + member_joints.update(nested_joints) + member_tokens.update(nested_tokens) + uncovered = direct_joints[resource_id] - member_joints + if uncovered: + raise ProfileValidationError( + f"Composite resource {resource_id!r} endpoints control joints " + f"{sorted(uncovered)} not claimed by its members." + ) + leaves = frozenset(leaves_set) + joints = frozenset(member_joints | direct_joints[resource_id]) + tokens = frozenset(member_tokens | direct_tokens[resource_id]) + leaf_cache[resource_id] = leaves + joint_cache[resource_id] = joints + token_cache[resource_id] = tokens + return leaves, joints, tokens + + resolved: dict[str, ResolvedRobotResource] = {} + for resource_id, resource in self._profile.resources.items(): + leaves, joints, tokens = resolve_claim(resource_id) + resolved[resource_id] = ResolvedRobotResource( + resource_id=resource_id, + endpoints=resolved_endpoints[resource_id], + members=resource.members, + claim=ResourceClaim( + leaves, + tuple(sorted(joints)), + tokens, + ), + ) + self._validate_command_shapes(resolved_endpoints) + return MappingProxyType(resolved) + + def _validate_command_shapes( + self, + endpoints_by_resource: Mapping[str, Mapping[str, ResolvedResourceEndpoint]], + ) -> None: + """Validate profile joint commands against every referenced endpoint DOF.""" + checked: set[tuple[str, int]] = set() + for endpoints in endpoints_by_resource.values(): + for endpoint in endpoints.values(): + if not endpoint.commands: + continue + dof = len(endpoint.joint_ids) + profile_label = endpoint.command_profile_key or endpoint.adapter_id + key = (profile_label, dof) + if key in checked: + continue + checked.add(key) + for command_name, command in endpoint.commands.items(): + if not isinstance(command, JointPositionCommand): + continue + positions = command.positions + if positions.dim() != 1: + raise ProfileValidationError( + f"Profile command {profile_label!r}." + f"{command_name} must be one-dimensional and " + "broadcastable across environments; use invocation " + "overrides for per-environment commands." + ) + if positions.shape[-1] != dof: + raise ProfileValidationError( + f"Command {profile_label!r}.{command_name} has " + f"{positions.shape[-1]} joints, expected {dof}." + ) + + def _validate_leaf_ownership(self) -> None: + """Require physical leaf resources to own disjoint adapter claims.""" + leaves = [ + resource for resource in self._resources.values() if not resource.members + ] + for index, left in enumerate(leaves): + for right in leaves[index + 1 :]: + overlapping_joints = sorted( + set(left.claim.joint_ids) & set(right.claim.joint_ids) + ) + overlapping_tokens = sorted( + left.claim.claim_tokens & right.claim.claim_tokens + ) + if overlapping_joints or overlapping_tokens: + raise ProfileValidationError( + f"Leaf resources {left.resource_id!r} and " + f"{right.resource_id!r} overlap on robot joints " + f"{overlapping_joints} or adapter claims " + f"{overlapping_tokens}. " + "Model one physical leaf and reference it from composites." + ) + + def _validate_named_skill_configuration(self) -> None: + """Reject defaults and preset selections for absent semantic skills.""" + configured_skill_ids = set(self._profile.defaults) | set( + self._profile.skill_presets + ) + unknown = sorted(configured_skill_ids - set(self._installed_skills)) + if unknown: + raise ProfileValidationError( + f"Profile references skills not installed with explicit contracts: " + f"{unknown}." + ) + + def _validate_defaults(self) -> None: + """Require every configured default to be complete and currently valid.""" + for skill_id, default in self._profile.defaults.items(): + descriptor = self._installed_skills[skill_id] + contract = descriptor.binding_contract + assert contract is not None + expected = set(contract.slot_ids) + actual = set(default.resources) + if actual != expected: + raise ProfileValidationError( + f"Default binding for skill {skill_id!r} must cover exactly " + f"{sorted(expected)}; missing={sorted(expected - actual)}, " + f"extra={sorted(actual - expected)}." + ) + unknown_resources = sorted( + set(default.resources.values()) - set(self._resources) + ) + if unknown_resources: + raise ProfileValidationError( + f"Default binding for skill {skill_id!r} references unknown " + f"resources {unknown_resources}." + ) + assignments = self._assignments(contract, default.resources) + if len(assignments) != 1: + raise ProfileValidationError( + f"Default binding for skill {skill_id!r} does not satisfy its " + "capabilities, commands, endpoints, and resource constraints." + ) + + def _assignments( + self, + contract: SkillBindingContract | None, + selections: Mapping[str, str], + ) -> tuple[dict[str, ResolvedRobotResource], ...]: + """Enumerate valid complete assignments in declaration order.""" + if contract is None: + return () + if not contract.slots: + return ({},) + slot_candidates: list[tuple[ResolvedRobotResource, ...]] = [] + for slot in contract.slots: + selected = selections.get(slot.slot_id) + candidates = tuple( + resource + for resource in self._resources.values() + if (selected is None or resource.resource_id == selected) + and self._resource_matches(resource, slot) + ) + if not candidates: + return () + slot_candidates.append(candidates) + assignments: list[dict[str, ResolvedRobotResource]] = [] + for combination in product(*slot_candidates): + assignment = { + slot.slot_id: resource + for slot, resource in zip(contract.slots, combination, strict=True) + } + if self._constraints_match(contract, assignment): + assignments.append(assignment) + return tuple(assignments) + + def _resource_matches( + self, + resource: ResolvedRobotResource, + slot: SkillResourceSlot, + ) -> bool: + """Return whether one resource satisfies all slot-local endpoints.""" + matched_endpoints: dict[str, ResolvedResourceEndpoint] = {} + for requirement in slot.endpoints: + endpoint = resource.endpoints.get(requirement.endpoint_id) + if endpoint is None: + return False + if not requirement.capabilities.issubset(endpoint.capabilities): + return False + if ( + requirement.route is not None + and requirement.route.target not in endpoint.binding_values + ): + return False + for command_name, command_type in requirement.required_commands.items(): + command = endpoint.commands.get(command_name) + if not isinstance(command, command_type): + return False + matched_endpoints[requirement.endpoint_id] = endpoint + for constraint in slot.constraints: + if isinstance(constraint, DisjointSlotEndpoints): + endpoints = [ + matched_endpoints[endpoint_id] + for endpoint_id in constraint.endpoint_ids + ] + for index, left in enumerate(endpoints): + if any( + left.conflicts_with(right) for right in endpoints[index + 1 :] + ): + return False + return True + + @staticmethod + def _constraints_match( + contract: SkillBindingContract, + assignment: Mapping[str, ResolvedRobotResource], + ) -> bool: + """Apply declared graph/claim constraints to one assignment.""" + for constraint in contract.constraints: + if isinstance(constraint, DisjointResourceSlots): + resources = [assignment[slot] for slot in constraint.slots] + for index, left in enumerate(resources): + if any( + left.claim.conflicts_with(right.claim) + for right in resources[index + 1 :] + ): + return False + return True + + def _unsupported_message( + self, + skill_id: str, + contract: SkillBindingContract | None, + selections: Mapping[str, str], + ) -> str: + """Render deterministic per-slot rejection reasons.""" + if contract is None: + return f"Skill {skill_id!r} has no explicit binding contract." + lines = [f"Skill {skill_id!r} has no compatible resource binding."] + every_slot_has_candidate = True + for slot in contract.slots: + selected = selections.get(slot.slot_id) + lines.append(f"slot {slot.slot_id!r}:") + slot_has_candidate = False + for resource in self._resources.values(): + if selected is not None and resource.resource_id != selected: + continue + reasons = self._rejection_reasons(resource, slot) + status = "compatible" if not reasons else "; ".join(reasons) + slot_has_candidate |= not reasons + lines.append(f" {resource.resource_id}: {status}") + every_slot_has_candidate &= slot_has_candidate + if contract.constraints and every_slot_has_candidate: + lines.append( + "All individually compatible combinations violate constraints." + ) + return "\n".join(lines) + + def _rejection_reasons( + self, + resource: ResolvedRobotResource, + slot: SkillResourceSlot, + ) -> tuple[str, ...]: + """Explain why one resource fails one slot requirement.""" + reasons: list[str] = [] + matched_endpoints: dict[str, ResolvedResourceEndpoint] = {} + for requirement in slot.endpoints: + endpoint = resource.endpoints.get(requirement.endpoint_id) + if endpoint is None: + reasons.append(f"missing endpoint {requirement.endpoint_id!r}") + continue + missing_capabilities = sorted( + requirement.capabilities - endpoint.capabilities + ) + if missing_capabilities: + reasons.append( + f"endpoint {requirement.endpoint_id!r} missing capabilities " + f"{missing_capabilities}" + ) + if ( + requirement.route is not None + and requirement.route.target not in endpoint.binding_values + ): + reasons.append( + f"endpoint {requirement.endpoint_id!r} adapter " + f"{endpoint.adapter_id!r} cannot lower to binding target " + f"{requirement.route.target!r}" + ) + for command_name, command_type in requirement.required_commands.items(): + command = endpoint.commands.get(command_name) + if command is None: + reasons.append( + f"endpoint {requirement.endpoint_id!r} missing command " + f"{command_name!r}" + ) + elif not isinstance(command, command_type): + reasons.append( + f"command {command_name!r} is {type(command).__name__}, " + f"expected {command_type.__name__}" + ) + matched_endpoints[requirement.endpoint_id] = endpoint + for constraint in slot.constraints: + if not isinstance(constraint, DisjointSlotEndpoints): + continue + endpoint_ids = constraint.endpoint_ids + for index, left_id in enumerate(endpoint_ids): + left = matched_endpoints.get(left_id) + if left is None: + continue + for right_id in endpoint_ids[index + 1 :]: + right = matched_endpoints.get(right_id) + if right is None or not left.conflicts_with(right): + continue + overlapping_joints = sorted( + set(left.joint_ids) & set(right.joint_ids) + ) + overlapping_tokens = sorted(left.claim_tokens & right.claim_tokens) + reasons.append( + f"endpoints {left_id!r} and {right_id!r} overlap on joints " + f"{overlapping_joints} or adapter claims " + f"{overlapping_tokens}" + ) + return tuple(reasons) + + @staticmethod + def _lower_binding( + skill_id: str, + contract: SkillBindingContract | None, + assignment: Mapping[str, ResolvedRobotResource], + ) -> ResolvedSkillBinding: + """Lower generic endpoints through the temporary current-core routes.""" + assert contract is not None + manipulators: dict[str, str] = {} + end_effectors: dict[str, str] = {} + for slot in contract.slots: + resource = assignment[slot.slot_id] + for requirement in slot.endpoints: + if requirement.route is None: + continue + endpoint = resource.endpoints[requirement.endpoint_id] + target = ( + manipulators + if requirement.route.target == "manipulator" + else end_effectors + ) + target[requirement.route.role] = endpoint.binding_values[ + requirement.route.target + ] + return ResolvedSkillBinding( + skill_id=skill_id, + resources=assignment, + action_binding=ActionBinding( + manipulators=manipulators, + end_effectors=end_effectors, + ), + claim=ResourceClaim.combine( + tuple(resource.claim for resource in assignment.values()) + ), + ) + + +__all__ = [ + "AmbiguousSkillBindingError", + "BoundRobotSkillProfile", + "ControlPartEndpoint", + "ControlPartEndpointAdapter", + "EndpointResolution", + "ProfileValidationError", + "ResourceEndpoint", + "ResourceEndpointAdapter", + "ResolvedRobotResource", + "ResolvedResourceEndpoint", + "ResolvedSkillBinding", + "ResourceBinding", + "ResourceClaim", + "RobotResource", + "RobotSkillProfile", + "SkillPolicyPreset", + "UnsupportedSkillError", +] diff --git a/tests/sim/atomic_actions/test_control.py b/tests/sim/atomic_actions/test_control.py index ee056aeaa..32507b02f 100644 --- a/tests/sim/atomic_actions/test_control.py +++ b/tests/sim/atomic_actions/test_control.py @@ -27,11 +27,24 @@ ActionBinding, ActionControlOverrides, ActionPlanningServices, + ControlCommand, ControlPartCommandProfile, JointPositionCommand, ) +class _BrokenSnapshotCommand(ControlCommand): + """Command double whose snapshot violates the public command contract.""" + + def snapshot(self) -> ControlCommand: + """Return an invalid snapshot for validation coverage.""" + return "invalid" # type: ignore[return-value] + + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether another command has this test-only type.""" + return isinstance(other, _BrokenSnapshotCommand) + + def _services() -> ActionPlanningServices: robot = Mock() robot.device = torch.device("cpu") @@ -73,6 +86,18 @@ def test_joint_position_command_rejects_incompatible_control_part() -> None: command.resolve(n_envs=1, control_dof=3, device="cpu") +def test_control_profile_rejects_invalid_command_snapshot_type() -> None: + with pytest.raises(TypeError, match="snapshot.*ControlCommand"): + ControlPartCommandProfile(commands={"stop": _BrokenSnapshotCommand()}) + + +def test_control_profile_rejects_command_name_outer_whitespace() -> None: + with pytest.raises(ValueError, match="outer whitespace"): + ControlPartCommandProfile( + commands={" stop ": JointPositionCommand(torch.zeros(1))} + ) + + def test_control_profile_is_resolved_from_robot_control_part() -> None: resolved = _services().resolve_binding( ActionBinding( diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py new file mode 100644 index 000000000..0f7be1fb8 --- /dev/null +++ b/tests/sim/skills/test_profiles.py @@ -0,0 +1,1207 @@ +# ---------------------------------------------------------------------------- +# 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 generic robot resources and declarative skill profiles.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionBindingRoute, + ActionOptions, + ActionPlan, + AtomicAction, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + BUILTIN_ACTION_TYPES, + CARTESIAN_POSE_CAPABILITY, + ControlCommand, + ControlPartCommandProfile, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + GRASP_COMMAND, + INVERSE_KINEMATICS_CAPABILITY, + JOINT_POSITION_CAPABILITY, + JointPositionCommand, + JointPositionGoal, + MotionPolicy, + OPEN_COMMAND, + ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.skills import ( + AmbiguousSkillBindingError, + ControlPartEndpoint, + ControlPartEndpointAdapter, + EndpointResolution, + ProfileValidationError, + ResourceBinding, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, + UnsupportedSkillError, +) + +_JOINT_IDS = { + "left_arm": [0, 1], + "left_hand": [2], + "right_arm": [3, 4], + "right_hand": [5], + "base": [6, 7], + "torso": [8], + "full_body": [0, 1, 3, 4, 6, 7, 8], +} + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } +) + + +def _command_profiles() -> dict[str, ControlPartCommandProfile]: + return { + hand: ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), + grasp=torch.tensor([1.0]), + ) + for hand in ("left_hand", "right_hand") + } + + +def _engine( + *, + control_profiles: dict[str, ControlPartCommandProfile] | None = None, + load_builtins: bool = True, +) -> AtomicActionEngine: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 9 + robot.control_parts = {name: object() for name in _JOINT_IDS} + robot.get_qpos.return_value = torch.zeros(2, 9) + robot.get_qvel.return_value = torch.zeros(2, 9) + robot.get_joint_ids.side_effect = lambda name: list(_JOINT_IDS[name]) + robot.get_solver.side_effect = lambda name=None: ( + object() if name in {"left_arm", "right_arm"} else None + ) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + return AtomicActionEngine( + generator, + control_profiles=control_profiles, + load_builtins=load_builtins, + ) + + +def _resources(*, include_right: bool = True) -> dict[str, RobotResource]: + resources = { + "left_arm": RobotResource( + "left_arm", + endpoints={"control": ControlPartEndpoint("left_arm")}, + ), + "left_hand": RobotResource( + "left_hand", + endpoints={"control": ControlPartEndpoint("left_hand")}, + ), + "left_actor": RobotResource( + "left_actor", + endpoints={ + "motion": ControlPartEndpoint( + "left_arm", capabilities=_MOTION_CAPABILITIES + ), + "grasp": ControlPartEndpoint( + "left_hand", capabilities=frozenset({GRASP_CAPABILITY}) + ), + }, + members=("left_arm", "left_hand"), + ), + "base": RobotResource( + "base", + endpoints={ + "motion": ControlPartEndpoint( + "base", capabilities=frozenset({"motion.base.se2"}) + ) + }, + ), + "torso": RobotResource( + "torso", + endpoints={"control": ControlPartEndpoint("torso")}, + ), + } + if include_right: + resources.update( + { + "right_arm": RobotResource( + "right_arm", + endpoints={"control": ControlPartEndpoint("right_arm")}, + ), + "right_hand": RobotResource( + "right_hand", + endpoints={"control": ControlPartEndpoint("right_hand")}, + ), + "right_actor": RobotResource( + "right_actor", + endpoints={ + "motion": ControlPartEndpoint( + "right_arm", capabilities=_MOTION_CAPABILITIES + ), + "grasp": ControlPartEndpoint( + "right_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + members=("right_arm", "right_hand"), + ), + } + ) + whole_body_members = ["base", "torso", "left_arm"] + if include_right: + whole_body_members.append("right_arm") + if include_right: + resources["whole_body"] = RobotResource( + "whole_body", + endpoints={ + "motion": ControlPartEndpoint( + "full_body", capabilities=frozenset({"motion.whole_body"}) + ) + }, + members=tuple(whole_body_members), + ) + return resources + + +def _profile( + *, + defaults: dict[str, ResourceBinding] | None = None, + resources: dict[str, RobotResource] | None = None, + command_profiles: dict[str, ControlPartCommandProfile] | None = None, +) -> RobotSkillProfile: + return RobotSkillProfile( + profile_id="test_robot", + resources=_resources() if resources is None else resources, + command_profiles=( + _command_profiles() if command_profiles is None else command_profiles + ), + defaults={} if defaults is None else defaults, + ) + + +class _WholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): + skill_id: ClassVar[str] = "whole_body_reach" + GoalType: ClassVar[type] = JointPositionGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.whole_body"}), + route=ActionBindingRoute("manipulator", "primary"), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + +class _NavigateAction(AtomicAction[JointPositionGoal, ActionOptions]): + skill_id: ClassVar[str] = "navigate" + GoalType: ClassVar[type] = JointPositionGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.base.se2"}), + route=ActionBindingRoute("manipulator", "primary"), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + +@dataclass(frozen=True, slots=True) +class _BaseVelocityEndpoint(ResourceEndpoint): + """Future non-joint endpoint used to prove the resource API stays generic.""" + + controller_id: str + + +@dataclass(frozen=True, slots=True) +class _MutableMetadataEndpoint(ResourceEndpoint): + """Endpoint with mutable metadata used to verify ownership snapshots.""" + + controller_id: str + aliases: list[str] + + +@dataclass(frozen=True, slots=True) +class _TwistCommand(ControlCommand): + """Test-only non-joint command for a mobile controller.""" + + value: tuple[float, float, float] + + def snapshot(self) -> _TwistCommand: + """Return an independently owned immutable command.""" + return _TwistCommand(tuple(self.value)) + + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether another twist command has the same value.""" + return isinstance(other, _TwistCommand) and self.value == other.value + + +class _BaseVelocityEndpointAdapter(ResourceEndpointAdapter): + """Resolve the test mobile controller without profile-resolver changes.""" + + adapter_id: ClassVar[str] = "test.base_velocity" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + """Resolve one mobile controller to a generic exclusive claim.""" + del engine + assert isinstance(endpoint, _BaseVelocityEndpoint) + return EndpointResolution( + command_profile_key=endpoint.controller_id, + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +class _VelocityNavigateAction(AtomicAction[JointPositionGoal, ActionOptions]): + """Semantic test skill consuming a non-core controller endpoint.""" + + skill_id: ClassVar[str] = "navigate_velocity" + GoalType: ClassVar[type] = JointPositionGoal + manipulator_roles: ClassVar[tuple[str, ...]] = () + end_effector_roles: ClassVar[tuple[str, ...]] = () + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.base.velocity"}), + required_commands={"stop": _TwistCommand}, + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + +class _RoutedVelocityAction(AtomicAction[JointPositionGoal, ActionOptions]): + """Test skill requiring a current-core route from a custom endpoint.""" + + skill_id: ClassVar[str] = "navigate_velocity_routed" + GoalType: ClassVar[type] = JointPositionGoal + manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + end_effector_roles: ClassVar[tuple[str, ...]] = () + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.base.velocity"}), + route=ActionBindingRoute("manipulator", "primary"), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + +def test_engine_skills_only_exposes_visible_explicit_installed_contracts() -> None: + engine = _engine(control_profiles=_command_profiles()) + expected = { + action_type.skill_id + for action_type in BUILTIN_ACTION_TYPES + if action_type.agent_visible + } + + assert set(engine.skills) == expected + assert "move_joints" in engine.actions + assert "move_joints" not in engine.skills + + +def test_new_skill_subclass_must_redeclare_binding_contract() -> None: + base_contract = BUILTIN_ACTION_TYPES[0].descriptor().binding_contract + + class Derived(BUILTIN_ACTION_TYPES[0]): + skill_id: ClassVar[str] = "derived_without_explicit_contract" + + assert base_contract is not None + assert Derived.descriptor().binding_contract is None + + +def test_descriptor_contract_must_exactly_cover_current_core_roles() -> None: + class InvalidRouteAction(AtomicAction[JointPositionGoal, ActionOptions]): + skill_id: ClassVar[str] = "invalid_route" + GoalType: ClassVar[type] = JointPositionGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract() + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + with pytest.raises(ValueError, match="do not exactly cover"): + InvalidRouteAction.descriptor() + + +def test_profile_owns_input_mappings_and_command_tensors() -> None: + resources = _resources() + open_positions = torch.tensor([0.0]) + profiles = { + "left_hand": ControlPartCommandProfile.joint_positions(open=open_positions) + } + profile = _profile(resources=resources, command_profiles=profiles) + + resources.clear() + profiles.clear() + open_positions.fill_(9.0) + + assert "left_actor" in profile.resources + command = profile.command_profiles["left_hand"].commands[OPEN_COMMAND] + assert isinstance(command, JointPositionCommand) + assert command.positions.tolist() == [0.0] + + +def test_profile_owns_custom_endpoint_nested_payloads() -> None: + source_aliases = ["base"] + resource = RobotResource( + "mobile_base", + endpoints={ + "motion": _MutableMetadataEndpoint( + "base_controller", + aliases=source_aliases, + ) + }, + ) + profile = RobotSkillProfile( + "mobile", + resources={"mobile_base": resource}, + ) + + source_aliases.append("source_mutation") + resource_endpoint = resource.endpoints["motion"] + assert isinstance(resource_endpoint, _MutableMetadataEndpoint) + resource_endpoint.aliases.append("resource_mutation") + profile_endpoint = profile.resources["mobile_base"].endpoints["motion"] + assert isinstance(profile_endpoint, _MutableMetadataEndpoint) + + assert profile_endpoint.aliases == ["base"] + + +def test_resource_graph_rejects_unknown_member_and_cycle() -> None: + with pytest.raises(ValueError, match="unknown members"): + RobotSkillProfile( + "unknown_member", + resources={ + "group": RobotResource("group", members=("missing",)), + }, + ) + + with pytest.raises(ValueError, match="contains a cycle"): + RobotSkillProfile( + "cycle", + resources={ + "a": RobotResource("a", members=("b",)), + "b": RobotResource("b", members=("a",)), + }, + ) + + +def test_identifier_sets_do_not_accept_one_string_as_characters() -> None: + with pytest.raises(TypeError, match="not a string"): + ControlPartEndpoint("left_arm", capabilities="motion.cartesian_pose") + with pytest.raises(TypeError, match="not a string"): + RobotResource( + "left_arm", + endpoints={"control": ControlPartEndpoint("left_arm")}, + members="left_arm", + ) + with pytest.raises(TypeError, match="iterable of endpoint IDs"): + DisjointSlotEndpoints("motion") + + +def test_slot_constraint_rejects_unknown_endpoint() -> None: + with pytest.raises(ValueError, match="unknown endpoints"): + SkillResourceSlot( + "primary", + endpoints=(SkillEndpointRequirement("motion"),), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ) + + +def test_bind_rejects_unknown_control_part() -> None: + resources = _resources() + resources["camera_gimbal"] = RobotResource( + "camera_gimbal", + endpoints={"motion": ControlPartEndpoint("missing")}, + ) + + with pytest.raises(ProfileValidationError, match="unknown control part 'missing'"): + _profile(resources=resources).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_resource_graph_accepts_extensible_endpoint_before_adapter_installation() -> ( + None +): + resource = RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_controller", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + profile = RobotSkillProfile("mobile", resources={"mobile_base": resource}) + + assert ( + profile.resources["mobile_base"].endpoints["motion"] + == resource.endpoints["motion"] + ) + with pytest.raises(ProfileValidationError, match="ResourceEndpointAdapter"): + profile.bind(_engine(control_profiles={}, load_builtins=False)) + + +def test_custom_endpoint_adapter_resolves_commands_and_physical_claim() -> None: + resource = RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + profile = RobotSkillProfile( + "mobile", + resources={"mobile_base": resource}, + command_profiles={ + "base_velocity": ControlPartCommandProfile( + commands={"stop": _TwistCommand((0.0, 0.0, 0.0))} + ) + }, + ) + engine = _engine(control_profiles={}, load_builtins=False) + engine.register(_VelocityNavigateAction()) + + bound = engine.bind_skill_profile( + profile, + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + resolved = bound.resolve("navigate_velocity") + endpoint = resolved.resources["body"].endpoints["motion"] + + assert endpoint.adapter_id == "test.base_velocity" + assert isinstance(endpoint.commands["stop"], _TwistCommand) + assert resolved.claim.claim_tokens == frozenset({"controller:base_velocity"}) + assert resolved.action_binding.manipulators == {} + assert resolved.action_binding.end_effectors == {} + + +def test_engine_constructor_forwards_custom_endpoint_adapters() -> None: + source = _engine(control_profiles={}, load_builtins=False) + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + }, + ) + + engine = AtomicActionEngine( + source.motion_generator, + load_builtins=False, + skill_profile=profile, + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + + assert engine.skill_profile is not None + assert engine.skill_profile.resources["mobile_base"].claim.claim_tokens == ( + frozenset({"controller:base_velocity"}) + ) + + +def test_custom_endpoint_claim_tokens_protect_distinct_leaf_aliases() -> None: + endpoint = _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + profile = RobotSkillProfile( + "aliased_mobile", + resources={ + "base_a": RobotResource("base_a", endpoints={"motion": endpoint}), + "base_b": RobotResource("base_b", endpoints={"motion": endpoint}), + }, + ) + + with pytest.raises(ProfileValidationError, match="adapter claims"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + + +def test_missing_adapter_binding_target_filters_skill_with_diagnostic() -> None: + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + }, + ) + engine = _engine(control_profiles={}, load_builtins=False) + engine.register(_RoutedVelocityAction()) + bound = profile.bind( + engine, + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + + assert "navigate_velocity_routed" not in bound.skills + with pytest.raises(UnsupportedSkillError, match="cannot lower.*manipulator"): + bound.resolve("navigate_velocity_routed") + + +def test_exclusive_custom_endpoint_requires_a_physical_claim() -> None: + class EmptyClaimAdapter(ResourceEndpointAdapter): + adapter_id: ClassVar[str] = "test.empty_claim" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution() + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={"motion": _BaseVelocityEndpoint("base_velocity")}, + ) + }, + ) + + with pytest.raises( + ProfileValidationError, + match="test.empty_claim.*mobile_base.*motion.*joint_ids or claim_tokens", + ): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: EmptyClaimAdapter()}, + ) + + +def test_nonexclusive_custom_endpoint_may_omit_a_physical_claim() -> None: + class VirtualEndpointAdapter(ResourceEndpointAdapter): + adapter_id: ClassVar[str] = "test.virtual" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution(exclusive=False) + + profile = RobotSkillProfile( + "virtual", + resources={ + "virtual_channel": RobotResource( + "virtual_channel", + endpoints={"motion": _BaseVelocityEndpoint("virtual")}, + ) + }, + ) + + bound = profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: VirtualEndpointAdapter()}, + ) + + assert not bound.resources["virtual_channel"].endpoints["motion"].exclusive + + +def test_endpoint_adapter_registration_validates_declared_type() -> None: + class MissingMetadataAdapter(ResourceEndpointAdapter): + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution(exclusive=False) + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={"motion": _BaseVelocityEndpoint("base_velocity")}, + ) + }, + ) + + with pytest.raises(TypeError, match="must declare.*endpoint_type"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: MissingMetadataAdapter()}, + ) + + +def test_builtin_control_part_adapter_cannot_be_overridden() -> None: + with pytest.raises(ValueError, match="cannot be overridden"): + _profile().bind( + _engine(control_profiles=_command_profiles()), + endpoint_adapters={ControlPartEndpoint: ControlPartEndpointAdapter()}, + ) + + +def test_endpoint_adapter_must_return_endpoint_resolution() -> None: + class WrongReturnAdapter(ResourceEndpointAdapter): + adapter_id: ClassVar[str] = "test.wrong_return" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return object() # type: ignore[return-value] + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={"motion": _BaseVelocityEndpoint("base_velocity")}, + ) + }, + ) + + with pytest.raises(ProfileValidationError, match="expected EndpointResolution"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: WrongReturnAdapter()}, + ) + + +def test_bind_rejects_overlapping_physical_leaves() -> None: + resources = _resources() + resources["left_arm_alias"] = RobotResource( + "left_arm_alias", + endpoints={"control": ControlPartEndpoint("left_arm")}, + ) + + with pytest.raises(ProfileValidationError, match="overlap on robot joints"): + _profile(resources=resources).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_bind_rejects_composite_endpoint_outside_member_claim() -> None: + resources = _resources() + resources["bad_composite"] = RobotResource( + "bad_composite", + endpoints={"motion": ControlPartEndpoint("right_arm")}, + members=("left_arm",), + ) + + with pytest.raises(ProfileValidationError, match="not claimed by its members"): + _profile(resources=resources).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_bind_rejects_profile_commands_not_installed_on_engine() -> None: + with pytest.raises(ProfileValidationError, match="is not installed"): + _profile().bind(_engine(control_profiles={})) + + +def test_explicit_endpoint_command_profile_must_exist() -> None: + profile = RobotSkillProfile( + "missing_commands", + resources={ + "arm": RobotResource( + "arm", + endpoints={ + "motion": ControlPartEndpoint( + "left_arm", + command_profile="missing_profile", + ) + }, + ) + }, + ) + + with pytest.raises(ProfileValidationError, match="required command profile"): + profile.bind(_engine(control_profiles={}, load_builtins=False)) + + +def test_bind_rejects_engine_command_payload_that_differs_from_profile() -> None: + engine_profiles = _command_profiles() + engine_profiles["left_hand"] = ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.5]), + grasp=torch.tensor([1.0]), + ) + + with pytest.raises(ProfileValidationError, match="not semantically equivalent"): + _profile().bind(_engine(control_profiles=engine_profiles)) + + +def test_profile_rejects_conflicting_endpoint_command_profiles() -> None: + resources = { + "hand": RobotResource( + "hand", + endpoints={ + "first": ControlPartEndpoint( + "left_hand", + command_profile="first_hand", + ), + "second": ControlPartEndpoint( + "left_hand", + command_profile="second_hand", + ), + }, + ) + } + with pytest.raises(ValueError, match="non-equivalent 'grasp'"): + RobotSkillProfile( + "conflicting_commands", + resources=resources, + command_profiles={ + "first_hand": ControlPartCommandProfile.joint_positions( + grasp=torch.tensor([0.5]) + ), + "second_hand": ControlPartCommandProfile.joint_positions( + grasp=torch.tensor([1.0]) + ), + }, + ) + + +def test_bind_rejects_joint_command_with_wrong_endpoint_dof() -> None: + profiles = _command_profiles() + profiles["left_hand"] = ControlPartCommandProfile.joint_positions( + open=torch.zeros(2), + grasp=torch.ones(2), + ) + + with pytest.raises(ProfileValidationError, match="2 joints, expected 1"): + _profile(command_profiles=profiles).bind(_engine(control_profiles=profiles)) + + +def test_profile_commands_must_be_broadcastable_across_environments() -> None: + profiles = _command_profiles() + profiles["left_hand"] = ControlPartCommandProfile.joint_positions( + open=torch.zeros(2, 1), + grasp=torch.ones(2, 1), + ) + + with pytest.raises(ProfileValidationError, match="must be one-dimensional"): + _profile(command_profiles=profiles).bind(_engine(control_profiles=profiles)) + + +def test_bind_rejects_unverified_standard_solver_capability() -> None: + engine = _engine(control_profiles=_command_profiles()) + engine.robot.get_solver.side_effect = lambda name=None: None + + with pytest.raises(ProfileValidationError, match="has no configured solver"): + _profile().bind(engine) + + +def test_unique_capability_binding_lowers_to_exact_action_binding() -> None: + profile = _profile(resources=_resources(include_right=False)) + bound = profile.bind(_engine(control_profiles=_command_profiles())) + + resolved = bound.resolve("pick_up") + + assert resolved.resource_ids == {"primary": "left_actor"} + assert resolved.action_binding.manipulators == {"primary": "left_arm"} + assert resolved.action_binding.end_effectors == {"primary": "left_hand"} + assert resolved.claim.leaf_resource_ids == frozenset({"left_arm", "left_hand"}) + assert resolved.claim.joint_ids == (0, 1, 2) + + +def test_ambiguous_binding_requires_complete_per_skill_default() -> None: + engine = _engine(control_profiles=_command_profiles()) + bound = _profile().bind(engine) + + with pytest.raises(AmbiguousSkillBindingError, match="2 valid resource bindings"): + bound.resolve("pick_up") + + selected = _profile( + defaults={ + "pick_up": ResourceBinding({"primary": "right_actor"}), + } + ).bind(engine) + assert selected.resolve("pick_up").resource_ids == {"primary": "right_actor"} + + +@pytest.mark.parametrize( + "default", + [ + ResourceBinding({}), + ResourceBinding({"primary": "left_actor", "stale": "right_actor"}), + ], +) +def test_profile_rejects_partial_or_extra_default_slots( + default: ResourceBinding, +) -> None: + with pytest.raises(ProfileValidationError, match="must cover exactly"): + _profile(defaults={"pick_up": default}).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_explicit_slot_selection_overrides_profile_default() -> None: + bound = _profile( + defaults={ + "pick_up": ResourceBinding({"primary": "left_actor"}), + } + ).bind(_engine(control_profiles=_command_profiles())) + + resolved = bound.resolve("pick_up", {"primary": "right_actor"}) + + assert resolved.resource_ids == {"primary": "right_actor"} + + +def test_missing_required_command_filters_skill_and_reports_reason() -> None: + profiles = _command_profiles() + profiles = { + name: ControlPartCommandProfile.joint_positions(grasp=torch.tensor([1.0])) + for name in profiles + } + bound = _profile(command_profiles=profiles).bind(_engine(control_profiles=profiles)) + + assert "pick_up" not in bound.skills + with pytest.raises(UnsupportedSkillError, match="missing command 'open'"): + bound.resolve("pick_up") + + +def test_one_participant_cannot_use_overlapping_required_endpoints() -> None: + resources = _resources(include_right=False) + resources["left_actor"] = RobotResource( + "left_actor", + endpoints={ + "motion": ControlPartEndpoint( + "left_arm", capabilities=_MOTION_CAPABILITIES + ), + "grasp": ControlPartEndpoint( + "left_arm", capabilities=frozenset({GRASP_CAPABILITY}) + ), + }, + members=("left_arm",), + ) + profiles = _command_profiles() + profiles["left_arm"] = ControlPartCommandProfile.joint_positions( + open=torch.zeros(2), + grasp=torch.ones(2), + ) + bound = _profile(resources=resources, command_profiles=profiles).bind( + _engine(control_profiles=profiles) + ) + + assert "pick_up" not in bound.skills + with pytest.raises(UnsupportedSkillError, match="overlap on joints"): + bound.resolve("pick_up") + + +def test_coupled_endpoint_views_are_allowed_without_disjoint_constraint() -> None: + class CoupledWholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): + skill_id: ClassVar[str] = "coupled_whole_body" + GoalType: ClassVar[type] = JointPositionGoal + manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + route=ActionBindingRoute("manipulator", "primary"), + ), + SkillEndpointRequirement( + "posture", + capabilities=frozenset({"control.posture"}), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + resources = _resources() + resources["coupled_body"] = RobotResource( + "coupled_body", + endpoints={ + "motion": ControlPartEndpoint( + "full_body", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + "posture": ControlPartEndpoint( + "full_body", + capabilities=frozenset({"control.posture"}), + ), + }, + members=("base", "torso", "left_arm", "right_arm"), + ) + engine = _engine(control_profiles=_command_profiles(), load_builtins=False) + engine.register(CoupledWholeBodyAction()) + bound = _profile(resources=resources).bind(engine) + + assert bound.resolve("coupled_whole_body").resource_ids == {"body": "coupled_body"} + + +def test_disjoint_slot_constraint_rejects_one_actor_for_two_participants() -> None: + profile = _profile(resources=_resources(include_right=False)) + bound = profile.bind(_engine(control_profiles=_command_profiles())) + + assert "hand_over" not in bound.skills + with pytest.raises(UnsupportedSkillError, match="violate constraints"): + bound.resolve("hand_over") + + +def test_composite_claim_conflicts_with_nested_arm_but_not_hand() -> None: + bound = _profile().bind(_engine(control_profiles=_command_profiles())) + + whole_body = bound.resources["whole_body"].claim + left_actor = bound.resources["left_actor"].claim + left_hand = bound.resources["left_hand"].claim + + assert whole_body.conflicts_with(left_actor) + assert not whole_body.conflicts_with(left_hand) + + +def test_generic_profile_supports_base_and_whole_body_without_arm_tool_fields() -> None: + engine = _engine(control_profiles=_command_profiles(), load_builtins=False) + engine.register(_WholeBodyAction()) + engine.register(_NavigateAction()) + bound = _profile().bind(engine) + + whole_body = bound.resolve("whole_body_reach") + navigation = bound.resolve("navigate") + + assert set(bound.skills) == {"navigate", "whole_body_reach"} + assert whole_body.resource_ids == {"body": "whole_body"} + assert whole_body.action_binding.manipulators == {"primary": "full_body"} + assert whole_body.claim.leaf_resource_ids == frozenset( + {"base", "torso", "left_arm", "right_arm"} + ) + assert navigation.resource_ids == {"body": "base"} + assert navigation.action_binding.manipulators == {"primary": "base"} + + +def test_presets_are_versioned_snapshots_and_validate_planner() -> None: + preset = SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(planner="stub_planner", sample_count=80), + ) + profile = RobotSkillProfile( + "presets", + resources=_resources(), + command_profiles=_command_profiles(), + presets={"safe": preset}, + default_preset="safe", + skill_presets={"pick_up": "safe"}, + ) + bound = profile.bind(_engine(control_profiles=_command_profiles())) + + first = bound.preset(skill_id="pick_up") + second = bound.preset() + + assert first is not second + assert first.schema_version == 1 + assert first.motion_policy.sample_count == 80 + mutable_runner = first.runner_cfg + mutable_runner.command_timeout = 99.0 + assert bound.preset().runner_cfg.command_timeout == 1.0 + with pytest.raises(KeyError, match="not an installed"): + 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) + + incompatible = RobotSkillProfile( + "bad_preset", + resources=_resources(), + command_profiles=_command_profiles(), + presets={ + "other": SkillPolicyPreset( + "other", + motion_policy=MotionPolicy(planner="other_planner"), + ) + }, + ) + with pytest.raises(ProfileValidationError, match="requires planner"): + incompatible.bind(_engine(control_profiles=_command_profiles())) + + +def test_profile_rejects_default_for_uninstalled_skill() -> None: + with pytest.raises(ProfileValidationError, match="not installed"): + _profile(defaults={"missing": ResourceBinding({"primary": "left_actor"})}).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_engine_can_install_profile_as_authoritative_command_source() -> None: + source_engine = _engine(control_profiles=_command_profiles()) + profile = _profile(defaults={"pick_up": ResourceBinding({"primary": "left_actor"})}) + + engine = AtomicActionEngine(source_engine.motion_generator, skill_profile=profile) + + assert engine.skill_profile is not None + assert engine.skill_profile.resolve("pick_up").resource_ids == { + "primary": "left_actor" + } + assert set(engine.control_profiles) == {"left_hand", "right_hand"} + + +def test_engine_rejects_endpoint_adapters_without_skill_profile() -> None: + source = _engine(control_profiles={}, load_builtins=False) + + with pytest.raises(ValueError, match="requires skill_profile"): + AtomicActionEngine( + source.motion_generator, + load_builtins=False, + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + + +def test_bound_profile_rejects_stale_engine_skill_catalog() -> None: + engine = _engine(control_profiles=_command_profiles()) + bound = _profile().bind(engine) + action_type = BUILTIN_ACTION_TYPES[0] + + class Replacement(action_type): + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "primary", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + route=ActionBindingRoute("manipulator", "primary"), + ), + ), + ), + ) + ) + + engine.register(Replacement(), replace=True) + + assert engine.skill_profile is None + with pytest.raises(RuntimeError, match="changed after"): + _ = bound.skills + + +__all__ = [] From 1683b86417422c7bb17ca6d378cb45b0de2f0b4c Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 01:46:16 +0800 Subject: [PATCH 09/28] refactor(atomic-actions): generalize runtime endpoints Make endpoint bindings, timed command frames, transports, routing, safe holds, and profile lowering controller-agnostic. Preserve joint trajectories as optional feedback artifacts and add staged, same-address invocation revision semantics for mobile and whole-body safety. --- .agents/skills/add-atomic-action/SKILL.md | 149 +++- agent_context/MAP.yaml | 20 +- .../topics/atomic-actions/atomic-actions.md | 320 +++++---- .../design/declarative_expert_program_plan.md | 137 +++- .../embodichain.lab.sim.atomic_actions.rst | 52 +- .../sim/atomic_actions/builtin_actions.md | 164 +++-- .../overview/sim/atomic_actions/index.md | 333 +++++---- .../atomic_actions/robot_skill_profiles.md | 42 +- docs/source/tutorial/atomic_actions.rst | 183 +++-- .../lab/sim/atomic_actions/__init__.py | 33 +- .../lab/sim/atomic_actions/bindings.py | 549 ++++++++++----- embodichain/lab/sim/atomic_actions/control.py | 95 ++- embodichain/lab/sim/atomic_actions/core.py | 386 ++++++++-- embodichain/lab/sim/atomic_actions/engine.py | 73 +- .../lab/sim/atomic_actions/execution.py | 573 ++++++++++----- .../lab/sim/atomic_actions/invocation.py | 20 +- embodichain/lab/sim/atomic_actions/plans.py | 242 ++++++- .../primitives/coordinated_pickment.py | 63 +- .../primitives/coordinated_placement.py | 57 +- .../atomic_actions/primitives/hand_over.py | 57 +- .../primitives/move_end_effector.py | 12 +- .../primitives/move_held_object.py | 20 +- .../atomic_actions/primitives/move_joints.py | 15 +- .../sim/atomic_actions/primitives/pick_up.py | 42 +- .../sim/atomic_actions/primitives/place.py | 23 +- .../sim/atomic_actions/primitives/press.py | 21 +- .../lab/sim/atomic_actions/requirements.py | 67 +- embodichain/lab/sim/atomic_actions/runner.py | 175 +++-- embodichain/lab/sim/atomic_actions/runtime.py | 340 +++++---- .../sim/atomic_actions/runtime_commands.py | 481 +++++++++++++ .../lab/sim/atomic_actions/sim_adapter.py | 152 +++- .../lab/sim/atomic_actions/transports.py | 489 +++++++++++++ embodichain/lab/sim/skills/profiles.py | 228 +++--- .../multi_segments/cube_pick_place.py | 21 +- .../tableware/blocks_ranking_rgb.py | 21 +- .../tableware/stack_blocks_two.py | 21 +- examples/sim/planners/curobo_planner.py | 6 +- .../move_end_effector_benchmark.py | 7 +- .../move_held_object_benchmark.py | 25 +- .../atomic_action/move_joints_benchmark.py | 10 +- .../atomic_action/pickup_benchmark.py | 10 +- .../atomic_action/place_benchmark.py | 17 +- .../atomic_action/press_benchmark.py | 18 +- scripts/tutorials/atomic_action/assemble.py | 16 +- .../atomic_action/coordinated_pickment.py | 13 +- .../atomic_action/coordinated_placement.py | 49 +- .../dynamic_obstacle_recovery.py | 50 +- scripts/tutorials/atomic_action/hand_over.py | 28 +- .../atomic_action/move_end_effector.py | 6 +- .../atomic_action/move_held_object.py | 23 +- .../tutorials/atomic_action/move_joints.py | 6 +- .../atomic_action/moving_target_recovery.py | 9 +- scripts/tutorials/atomic_action/pickup.py | 7 +- scripts/tutorials/atomic_action/place.py | 16 +- scripts/tutorials/atomic_action/press.py | 15 +- tests/sim/atomic_actions/test_actions.py | 244 +++++-- tests/sim/atomic_actions/test_control.py | 114 ++- tests/sim/atomic_actions/test_core.py | 664 +++++++++++++++++- .../test_curobo_motion_strategy_e2e.py | 12 +- .../test_endpoint_runtime_e2e.py | 535 ++++++++++++++ tests/sim/atomic_actions/test_engine.py | 128 +++- .../sim/atomic_actions/test_engine_per_env.py | 409 ++++++++++- .../test_motion_strategy_e2e.py | 14 +- tests/sim/atomic_actions/test_runner.py | 199 +++++- .../atomic_actions/test_runtime_commands.py | 379 ++++++++++ tests/sim/atomic_actions/test_sim_adapter.py | 189 ++++- tests/sim/atomic_actions/test_transports.py | 522 ++++++++++++++ tests/sim/planners/test_curobo_planner.py | 16 +- tests/sim/skills/test_profiles.py | 366 +++++++--- 69 files changed, 7822 insertions(+), 1976 deletions(-) create mode 100644 embodichain/lab/sim/atomic_actions/runtime_commands.py create mode 100644 embodichain/lab/sim/atomic_actions/transports.py create mode 100644 tests/sim/atomic_actions/test_endpoint_runtime_e2e.py create mode 100644 tests/sim/atomic_actions/test_runtime_commands.py create mode 100644 tests/sim/atomic_actions/test_transports.py diff --git a/.agents/skills/add-atomic-action/SKILL.md b/.agents/skills/add-atomic-action/SKILL.md index da144d6e6..ee825ecfe 100644 --- a/.agents/skills/add-atomic-action/SKILL.md +++ b/.agents/skills/add-atomic-action/SKILL.md @@ -20,15 +20,19 @@ Inspect only the files relevant to the requested skill: |---|---| | Base action and descriptors | `embodichain/lab/sim/atomic_actions/core.py` | | Goals and dynamic pose references | `embodichain/lab/sim/atomic_actions/goals.py` | -| Role-to-resource binding | `embodichain/lab/sim/atomic_actions/bindings.py` | +| Skill endpoint requirements | `embodichain/lab/sim/atomic_actions/requirements.py` | +| Resolved endpoint bindings and targets | `embodichain/lab/sim/atomic_actions/bindings.py` | | Invocation, options, and resolved request | `embodichain/lab/sim/atomic_actions/invocation.py` | | Control-part semantic commands | `embodichain/lab/sim/atomic_actions/control.py` | | Invocation policies | `embodichain/lab/sim/atomic_actions/policies.py` | | Robot/task/scene state | `embodichain/lab/sim/atomic_actions/state.py` | | Dynamic scene provider contract | `embodichain/lab/sim/atomic_actions/scene.py` | | Effects and plans | `embodichain/lab/sim/atomic_actions/effects.py`, `plans.py` | +| Runtime command frames and payloads | `embodichain/lab/sim/atomic_actions/runtime_commands.py` | +| Endpoint command transports | `embodichain/lab/sim/atomic_actions/transports.py` | | Trajectory helpers | `embodichain/lab/sim/atomic_actions/trajectory_ops.py` | | Engine-owned planning resources | `embodichain/lab/sim/atomic_actions/runtime.py` | +| Declarative robot resources and adapters | `embodichain/lab/sim/skills/profiles.py` | | Reference implementations | `embodichain/lab/sim/atomic_actions/primitives/` | | Static compiler and execution session | `engine.py`, `execution.py` | | Controller-facing execution ports | `runner.py`, `sim_adapter.py` | @@ -92,24 +96,33 @@ class PushOptions(ActionOptions): push_distance: float = 0.05 ``` -Do not put arm/hand names, hand qpos, or named robot postures in options. Bind -participants with `ActionBinding`. Register embodiment-specific commands such -as `open`, `grasp`, or `ready` on `ControlPartCommandProfile`; use +Do not put arm/hand names, hand qpos, or named robot postures in options. +Declare robot-independent participant slots and endpoints with +`SkillBindingContract`; the engine or a bound robot skill profile produces the +engine-owned `ActionBinding`. Register embodiment-specific commands such as +`open`, `grasp`, or `ready` on `ControlPartCommandProfile`; use `ActionControlOverrides` only for one invocation revision. ## 3. Implement the planner -Inherit `AtomicAction[PushGoal, PushOptions]` directly. Declare stable metadata and resolve -resources from semantic binding roles. +Inherit `AtomicAction[PushGoal, PushOptions]` directly. Declare stable metadata +and an explicit, robot-independent endpoint contract. Every concrete action +class must declare `binding_contract` in its own class body; use +`SkillBindingContract()` for a skill that consumes no robot resource. ```python from typing import ClassVar from embodichain.lab.sim.atomic_actions import ( - ResolvedActionRequest, ActionPlan, AtomicAction, + CARTESIAN_POSE_CAPABILITY, + JointPositionTarget, PlanningContext, + ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, ) from embodichain.lab.sim.atomic_actions.trajectory_ops import ( @@ -122,7 +135,19 @@ class Push(AtomicAction[PushGoal, PushOptions]): skill_id: ClassVar[str] = "push" GoalType: ClassVar[type] = PushGoal OptionsType: ClassVar[type] = PushOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + ), + ), + ), + ) def __init__(self, default_options: PushOptions | None = None) -> None: super().__init__(default_options) @@ -133,11 +158,13 @@ class Push(AtomicAction[PushGoal, PushOptions]): context: PlanningContext, ) -> ActionPlan: goal = self.require_goal(request) - options = request.skill_options - manipulator = request.binding.manipulator("primary") - control_part = manipulator.name - joint_ids = list(manipulator.joint_ids) + motion_target = request.binding.endpoint( + "primary", "motion" + ).require_target(JointPositionTarget) + control_part = motion_target.control_part + joint_ids = list(motion_target.joint_ids) start_qpos = context.robot.qpos[:, joint_ids] + target_poses = goal.contact_pose # Build planner states and generate controlled-joint motion using # request.motion_policy. Embed it into full robot DoF. @@ -168,6 +195,11 @@ Follow these invariants: - Let the engine supply `self.robot` and `self.motion_generator`; use `_on_bind()` only for robot/device-dependent setup. +- Keep slot and endpoint IDs semantic and robot-independent. Declare all-of + capabilities, required typed commands, and disjointness constraints in the + `SkillBindingContract`; do not infer resources from endpoint names. +- Resolve an endpoint with `request.binding.endpoint(slot_id, endpoint_id)` and + call `require_target(ExpectedTarget)` before using target-specific fields. - Import pure target-shaping, interpolation, pose-translation, and full-robot embedding helpers directly from `atomic_actions.trajectory_ops`; keep stateful planning inside `MotionGenerator`. @@ -176,8 +208,8 @@ Follow these invariants: `plan()` method; the latter injects the latest dynamic obstacle poses into a copied planner policy. - Plan from `context.robot.qpos`, never an implicit live robot start state. -- Return full-robot `(B, N, robot.dof)` motion as a tensor or - `TimedTrajectory` with matching `env_ids`. +- For joint-backed motion, return full-robot `(B, N, robot.dof)` motion as a + tensor or `TimedTrajectory` with matching `env_ids` through `build_plan()`. - Preserve row-local planner success. `build_plan()` normalizes the mask and replaces unsuccessful trajectory rows with the context's observed qpos. - Preserve backend timing/derivatives when available. @@ -196,7 +228,60 @@ Follow these invariants: `collision_entity_ids`; supported planners receive those entity poses through the framework-owned `plan()` entry point. -## 4. Register and invoke +## 4. Emit generic runtime commands when needed + +Use `build_command_plan()` when a skill targets a mobile base, whole-body +controller, tool, or another non-joint transport. Build immutable endpoint +commands; keep live controller and device handles in the transport: + +```python +target = request.binding.endpoint("primary", "tool").require_target(ToolTarget) +frames = tuple( + RuntimeCommandFrame( + commands=(EndpointCommand(target=target, payload=ToolPayload(value)),), + 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,), + request.motion_policy.control_dt, + device=context.robot.qpos.device, + ), + ) + for value in command_values +) +return self.build_command_plan( + request, + context, + success=success, + commands=TimedCommandSequence(frames=frames, env_ids=context.env_ids), +) +``` + +For a new transport kind: + +1. Define an immutable `RuntimeEndpointTarget` and `RuntimeCommandPayload` with + the same stable `transport_id`; both must return independently owned + snapshots. Payloads also expose `batch_size` and `device`. If target-specific + addressing or safe hold depends on fields beyond the exact target type, + `transport_id`, and `target_id`, override `address_fingerprint` to include + those immutable fields; frames, replans, and revisions preserve it. +2. If declarative robot profiles select it, define a `ResourceEndpoint` and an + exact-type `ResourceEndpointAdapter` that returns `EndpointResolution` with + the runtime target and physical claim metadata. +3. Implement `EndpointCommandTransport.send()`, `hold()`, and `cancel()`, then + register it in `EndpointCommandRouter` used as the `ExecutionRunner` command + sink. The router validates payload types before dispatch. + +The default command-plan feedback mode is timed and `joint_trajectory` is +optional. Use joint-position feedback only when a matching full-robot +`joint_trajectory` is supplied. Test target/payload snapshot ownership, frame +batch/device consistency, routing, acknowledgement, hold, and cancel behavior. + +## 5. Register and invoke Register an instance by its class-level `skill_id`: @@ -213,10 +298,14 @@ register_action(Push) Construct a grounded invocation explicitly: ```python +binding = engine.bind_control_parts( + "push", + {"primary": {"motion": "left_arm"}}, +) invocation = ActionInvocation( skill_id="push", goal=PushGoal(contact_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=60), recovery_policy=RecoveryPolicy(max_replans=2), ) @@ -228,26 +317,33 @@ For dynamic scene updates or online error recovery, create a session with through `ExecutionRunner`. Use non-blocking `runner.step()` in an existing event loop or `runner.run_until_blocked()` in a simple application. -## 5. Export and document +`engine.bind_control_parts()` is the explicit direct-core path for joint-backed +control parts. When a `RobotSkillProfile` is installed, prefer +`engine.skill_profile.resolve("push", selections).action_binding` so capability, +command, resource-claim, and custom-adapter validation remain declarative. + +## 6. Export and document Export the goal, options, and action from: 1. `embodichain/lab/sim/atomic_actions/primitives/__init__.py` 2. `embodichain/lab/sim/atomic_actions/__init__.py` -Add the stable skill ID, goal, roles, and effect to +Add the stable skill ID, goal, binding slots/endpoints, and effect to `docs/source/overview/sim/atomic_actions/builtin_actions.md`. Update API docs for new public classes. Do not create a compatibility re-export module or a closed built-in-goal union. -## 6. Test behavior +## 7. Test behavior Add pure pytest tests under `tests/sim/atomic_actions/`. Cover: -- descriptor `skill_id`, `GoalType`, and required roles; -- invalid goal and missing binding rejection; +- descriptor `skill_id`, `GoalType`, and explicit binding contract; +- invalid goal, wrong binding owner, and missing/extra endpoint rejection; - per-environment planning success/failure masks; - full-robot trajectory shape, `env_ids`, timing, and failed-row hold behavior; +- generic command target/payload ownership, frame batch/device consistency, and + optional `joint_trajectory` behavior when the skill emits command frames; - side-effect-free context handling; - masked `StateDelta` application for task effects; - `SceneEntityPose` replanning when the action accepts a dynamic goal; @@ -264,9 +360,12 @@ then use the `pre-commit-check` skill before committing. |---|---| | Inherit another action | Inherit `AtomicAction` directly; compose helpers. | | Add one generic target with many optional fields | Define a narrow action-owned goal. | -| Put hardware names in the goal | Bind semantic roles through `ActionBinding`. | -| Put arm/hand control-part names in skill options | Use `ActionBinding` as their only source. | -| Bind a joint, link, TCP frame, or arbitrary name | Every binding value must be a key in `RobotCfg.control_parts`. | +| Put hardware names in the goal | Declare semantic slots/endpoints and resolve an engine-owned binding. | +| Put arm/hand control-part names in skill options | Read typed runtime targets from bound endpoints. | +| Declare legacy role tuples on the action | Declare a class-local `SkillBindingContract`. | +| Use role-specific binding accessors | Use `binding.endpoint(...).require_target(...)`. | +| Construct a binding from role dictionaries | Use a bound skill profile, or `engine.bind_control_parts()` for the direct joint path. | +| Pass an arbitrary joint/link/TCP name to the direct path | `bind_control_parts()` values must be keys in `RobotCfg.control_parts`; add an endpoint adapter for another resource kind. | | Put hand qpos or named robot postures in skill options | Register semantic commands on the concrete control-part profile. | | Put planner/recovery knobs in skill options | Move them to invocation policies. | | Pass a motion generator to each action | Pass it once to `AtomicActionEngine`; construct actions from default options only. | @@ -277,4 +376,6 @@ then use the `pre-commit-check` skill before committing. | Mutate held state after planning | Declare a `StateDelta`. | | Treat `plan_success` as physical success | Verify effects during execution. | | Step the simulator from the action | Emit plans; connect execution through `ExecutionRunner`. | +| Put live controller handles in targets or payloads | Keep immutable addressing/data in values and own handles in the transport. | +| Force a non-joint endpoint into a fake trajectory | Emit typed frames with `build_command_plan()` and install its transport. | | Override public `plan()` | Implement `_plan()` so scene binding cannot be bypassed. | diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 2f49e4aad..d8648c4b6 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -511,7 +511,9 @@ topics: - StateDelta - held_objects - ActionBinding - - ActionBindingRoute + - EndpointBinding + - RuntimeEndpointTarget + - JointPositionTarget - SkillBindingContract - SkillResourceSlot - SkillEndpointRequirement @@ -551,6 +553,20 @@ topics: - ControlPartCommandProfile - ActionControlOverrides - JointPositionCommand + - RuntimeCommandPayload + - JointPositionPayload + - EndpointCommand + - RuntimeCommandFrame + - TimedCommandSequence + - EndpointCommandTransport + - EndpointCommandRouter + - endpoint transport + - transport_id + - target_id + - safe stop + - cancel then hold + - ActionPlan.commands + - joint_trajectory - invocation revision - MotionPolicy - MotionPolicy.strategy @@ -580,6 +596,8 @@ topics: - embodichain/lab/sim/atomic_actions/policies.py - embodichain/lab/sim/atomic_actions/requirements.py - embodichain/lab/sim/atomic_actions/runtime.py + - embodichain/lab/sim/atomic_actions/runtime_commands.py + - embodichain/lab/sim/atomic_actions/transports.py - embodichain/lab/sim/atomic_actions/state.py - embodichain/lab/sim/atomic_actions/plans.py - embodichain/lab/sim/atomic_actions/execution.py diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 9698d0f98..4c56e299c 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -14,40 +14,48 @@ There is no `ActionTarget`, `WorldState`, `ActionResult`, `execute()`, or `ActionInvocation` separates: - an action-owned typed goal (`goal_kind` is its stable discriminator); -- `ActionBinding`, which maps semantic roles to names from the engine robot's - `control_parts` mapping; +- an engine-owned `ActionBinding`, which covers the skill contract by exact + `(slot_id, endpoint_id)` keys and terminates every endpoint at an immutable + `RuntimeEndpointTarget`; - reusable `MotionPolicy` planner/timing choices; - bounded `RecoveryPolicy` thresholds and retry budgets; -- optional typed `skill_options` and role-scoped `control_overrides` for one - invocation revision. +- optional typed `skill_options` and endpoint-scoped `control_overrides` for + one invocation revision. `PlanningContext` separates measured `RobotObservation`, verified symbolic `TaskState`, versioned `SceneSnapshot`, and environment IDs. An `ActionPlan` -contains per-environment planning success, one full-robot `TimedTrajectory`, -action-level recovery and scene-invalidation metadata, planner diagnostics, -named `TrajectorySegment` ranges, and an uncommitted `StateDelta`. Segments are -inspection/tracing metadata inside one trajectory; they are not independently -replannable execution boundaries. - -`AtomicAction.build_plan()` normalizes the success mask and freezes unsuccessful -trajectory rows at the context's observed qpos; skill implementations should -return row-local success instead of duplicating failure-row masking. +contains per-environment planning success, an authoritative +`TimedCommandSequence` in `commands`, an optional full-robot `TimedTrajectory` +in `joint_trajectory`, action-level recovery and scene-invalidation metadata, +planner diagnostics, named `TrajectorySegment` frame ranges, and an uncommitted +`StateDelta`. Segments are inspection/tracing metadata inside one command +sequence; they are not independently replannable execution boundaries. + +`AtomicAction.build_plan()` is the planner-backed joint convenience path: it +normalizes the success mask, freezes unsuccessful trajectory rows at the +context's observed qpos, and lowers the trajectory through bound +`JointPositionTarget` values. `AtomicAction.build_command_plan()` is the generic +extension boundary for transport-neutral command sequences. Both mask failed +rows; skill implementations should return row-local success instead of +duplicating that work. Use `plan.segment(name)` for action-local half-open ranges and `compiled.segment(action_index, name)` for concatenated coordinates; do not recompute private sample splits in callers. Each `AtomicActionEngine` exclusively owns one `ActionPlanningServices` instance, which contains its robot, one `MotionGenerator`/planner backend, and -the legacy core's control-part command profiles. `MotionGenerator.generate()` is the only -stateful motion-planning entry point. `MotionPolicy.to_motion_gen_options()` -passes the invocation's `strategy` directly into `MotionGenOptions`; it is either -`"motion_gen"` or `"ik_interp"`. Target shaping, world-frame pose translation, -hand/joint interpolation used by composite actions, and full-robot trajectory -embedding are pure functions in `trajectory_ops.py`. Actions retain only an -owned copy of typed default options and borrow engine services. Engine -construction creates and binds a fresh instance of every type in -`BUILTIN_ACTION_TYPES`; use `load_builtins=False` only for isolated tests or a -fully custom action set. A bound action cannot be reused by another engine. +its direct control-part command-profile snapshot. It also issues an opaque +binding-owner ID, so an `ActionBinding` cannot cross engine instances. +`MotionGenerator.generate()` is the only stateful motion-planning entry point. +`MotionPolicy.to_motion_gen_options()` passes the invocation's `strategy` +directly into `MotionGenOptions`; it is either `"motion_gen"` or `"ik_interp"`. +Target shaping, world-frame pose translation, hand/joint interpolation used by +composite actions, and full-robot trajectory embedding are pure functions in +`trajectory_ops.py`. Actions retain only an owned copy of typed default options +and borrow engine services. Engine construction creates and binds a fresh +instance of every type in `BUILTIN_ACTION_TYPES`; use `load_builtins=False` only +for isolated tests or a fully custom action set. A bound action cannot be +reused by another engine. ## Engine entry points @@ -89,14 +97,14 @@ generic DAG, not a fixed arm/tool schema: whole-body capability and endpoint explicitly. - `ResourceEndpoint` is the extension boundary for controller kinds. An exact endpoint-type `ResourceEndpointAdapter` resolves each declaration against the - engine into an `EndpointResolution`: lowering values, an optional generic - command-profile key, joint IDs, adapter-defined claim tokens, and exclusivity. - `ControlPartEndpointAdapter` is installed by default for - `ControlPartEndpoint`; integrations pass additional `endpoint_adapters` to - profile or engine binding for mobile bases, whole-body controllers, or other - endpoint kinds. Registration is by exact endpoint type, and the built-in - adapter cannot be overridden; distinct controller semantics use a distinct - endpoint subtype. + engine into an `EndpointResolution`: a `RuntimeEndpointTarget`, an optional + generic command-profile key, joint IDs, adapter-defined claim tokens, and + exclusivity. `ControlPartEndpointAdapter` is installed by default for + `ControlPartEndpoint` and produces a `JointPositionTarget`. Integrations pass + additional `endpoint_adapters` to profile or engine binding for mobile bases, + whole-body controllers, or other endpoint kinds. Registration is by exact + endpoint type, and the built-in adapter cannot be overridden; distinct + controller semantics use a distinct endpoint subtype. - Resources, profiles, and resolved bindings own independent endpoint snapshots. A custom endpoint whose nested payload cannot be deep-copied must override `snapshot()` and return a new value of its exact type. @@ -114,23 +122,32 @@ Skills own the robot-independent side of the contract. A concrete `SkillBindingContract` in its own class body. The contract contains skill-local `SkillResourceSlot` values; every slot requires named `SkillEndpointRequirement` values with all-of capabilities, optional typed -semantic commands, and an optional `ActionBindingRoute`. Selecting one resource -per slot keeps related endpoints together, so a manipulation participant cannot -silently combine one arm with an unrelated tool. Endpoint views within that -resource may overlap by default, which permits an arm, mobile base, and +semantic commands, and no fixed arm/tool role or route layer. Selecting one +resource per slot keeps related endpoints together, so a participant cannot +silently combine endpoint views from unrelated resources. Endpoint views within +that resource may overlap by default, which permits an arm, mobile base, and whole-body view to describe the same physical system. Add `DisjointSlotEndpoints` to a slot only when selected endpoint views must be physically disjoint. `DisjointResourceSlots` separately expresses pairwise claim separation between selected participant resources. -`ActionBindingRoute` is only a transition adapter into the current core's -`manipulators` and `end_effectors` maps. Contract routes must cover the action's -declared core roles exactly. `BoundRobotSkillProfile.resolve()` returns a -`ResolvedSkillBinding` that retains the selected logical resources, the lowered -concrete `ActionBinding`, each resource's resolved endpoint data, and one -combined `ResourceClaim`. Direct-core callers may still construct -`ActionBinding` themselves, but that path does not perform profile capability -matching. +Profile binding lowers every selected endpoint directly into an +`EndpointBinding`. Its `target` supplies immutable runtime addressing +(`transport_id`, `target_id`); its semantic commands, capabilities, and claim +tokens remain attached to the same endpoint. `BoundRobotSkillProfile.resolve()` +returns a `ResolvedSkillBinding` that retains the selected logical resources, +the engine-owned `ActionBinding`, each resource's resolved endpoint data, and +one combined `ResourceClaim`. + +Advanced callers without a profile use +`engine.bind_control_parts(skill, endpoints)` with an exact nested +`slot -> endpoint -> control_part` mapping. The engine accepts an installed +skill ID or an explicit action instance later passed to `plan_action()`, checks +contract coverage, control-part existence, required commands, ownership, and +disjointness, then emits the same generic `ActionBinding` with +`JointPositionTarget` endpoints. Callers do not construct bindings manually, +and this path deliberately does not perform profile resource discovery or +capability matching. Discovery boundaries are distinct: @@ -155,9 +172,12 @@ Binding and policy authority is split deliberately: - 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 - lowering metadata; -- the engine owns installed actions, one planner backend, and the legacy - control-part command profiles used by the current action core. + immutable runtime-target lowering; +- runtime payload types own immutable command values, while + `EndpointCommandTransport` implementations own live controller/client state + and execute only payloads whose `transport_id` matches their targets; +- the engine owns installed actions, one planner backend, its binding identity, + and direct control-part command-profile snapshots. Constructing `AtomicActionEngine(..., skill_profile=profile)` makes the profile's generic `command_profiles` the single authoritative constructor @@ -166,14 +186,15 @@ source; passing `control_profiles` at the same time is rejected. immutable command container, but their mapping keys are generic profile IDs rather than necessarily being control-part names. `ControlPartEndpointAdapter` plus `RobotSkillProfile.action_control_profiles()` -is only the bridge that lowers applicable endpoint commands into the current -core's control-part-keyed profiles. Binding a profile to an already constructed -engine instead requires equivalent bridge commands to have been installed -already. A profile `JointPositionCommand` is one-dimensional and sized to the -adapter-resolved endpoint joint IDs; invocation `ActionControlOverrides` remain -the authority for one revision's per-environment replacements. Resolving a -custom endpoint's commands does not by itself add their controller transport to -the current action core. +provides the direct control-part lookup used by built-in joint planners when an +engine is constructed from a profile; it is not a binding route. Binding a +profile to an already constructed engine instead requires equivalent direct +control-part commands to have been installed already. Profile resolution still +places all resolved semantic commands, including commands for custom endpoint +types, on their `EndpointBinding`. A profile `JointPositionCommand` is +one-dimensional and sized to the adapter-resolved endpoint joint IDs; +invocation `ActionControlOverrides` remain the authority for one revision's +per-environment endpoint-command replacements. Resolution selects a sole valid assignment automatically. If several remain, it uses only a complete, currently valid per-skill default or enough explicit @@ -187,13 +208,13 @@ 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, -joint-mask command merger, or concurrency guarantee yet. `ExecutionSession` -and `ExecutionRunner` still emit, cancel, and hold full-robot joint commands. A -custom mobile/base endpoint can bind and participate in capability matching -once its adapter resolves it, including a controller claim token, but that does -not create a reusable navigation skill, planner/controller path, or command -transport. Do not treat successful binding or a non-conflicting claim as proof -of safe parallel or mobile execution. +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 +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 +not proof that a planner/controller path or safe concurrent execution exists. ## Object identity and pose grounding @@ -328,8 +349,12 @@ compiled = engine.compile(invocations, context=None) Compilation does not step simulation. It concatenates timed trajectories and applies successful expected effects only to `compiled.projected_context`, so a -following action can be checked against hypothetical state. Failed rows hold -their last successful qpos. +following action can be checked against hypothetical state. Because +`CompiledTrajectory` is a joint-trajectory result, every action plan in a +compiled sequence must own `joint_trajectory`; `compile()` rejects a generic +runtime-command plan without one. Use `start()` plus an execution runner for +plans whose authoritative `commands` target non-joint transports. Failed joint +rows hold their last successful qpos. Use invocation `skill_options` for multiple variants with the same stable `skill_id`; do not create per-variant built-in instances. @@ -357,13 +382,22 @@ result = runner.step(effect_success=None) ``` `ExecutionSession` owns deterministic planning progress and recovery state. It -emits at most one `JointCommand` per tick. The command's per-environment -`hold_duration` schedules the next feedback cycle from `TimedTrajectory.dt`: -command `i` carries the arrival interval `dt[:, i + 1]` leading to the next -waypoint. The final command reuses its own interval as a settling window. The -session monitors: - -- joint tracking error against the previous command; +emits at most one synchronized `RuntimeCommandFrame` per tick from the plan's +authoritative `TimedCommandSequence`. A frame contains one or more +`EndpointCommand` values, a shared environment batch and active mask, and a +per-environment `hold_duration`. Every command pairs a +`RuntimeEndpointTarget` with a `RuntimeCommandPayload`; their `transport_id` +values must match, destinations must be unique within the frame, and joint +targets may not overlap. `ExecutionFeedbackMode.JOINT_POSITION` requires an +owned `joint_trajectory` and joint-position targets/payloads; generic command +plans default to timed completion and retain external semantic-effect +verification. Framework authorization replaces every emitted target with its +binding-owned snapshot and rejects unbound destinations, target substitution, +and endpoint claim conflicts. A plan's non-empty frames and its recovery +replans retain a stable destination set. Empty failed plans retain previously +active targets so the caller can still hold them. The session monitors: + +- joint tracking error against the previous command in joint-position mode; - translation/rotation drift of referenced scene entities; - per-environment collision-world revision changes for collision-sensitive actions; @@ -387,42 +421,67 @@ policy, binding, or control command during execution, submit a strictly newer revision explicitly: ```python -session.revise_current(revised_invocation) +runner.revise_current(revised_invocation) ``` The replacement must keep the active `skill_id` and `invocation_id`. The session resolves a new snapshot, resets that revision's recovery budgets, and -replans from the latest context. +replans from the latest context. Once runtime destinations are owned, the +replacement must preserve a non-empty destination set and every target address +fingerprint; changing a base, whole-body, arm, controller, or safe-hold +footprint requires a new invocation. The runner snapshots the revision, keeps +the current frame deadline, then observes and installs it at the next due +boundary. Pending physical effects must be verified first, or the caller must +cancel and start a new invocation. A caller that owns manual session ticks may +use `session.revise_current(..., context=fresh_context)` directly. `ExecutionRunner` owns the controller-facing lifecycle around a session: - `ObservationProvider.observe(task_state)` supplies a fresh, monotonically timestamped `PlanningContext` when a feedback cycle is due; -- `CommandSink.send/hold/cancel` returns a `CommandAcknowledgement` with - `accepted`, `rejected`, or `timed_out` status; +- `CommandSink.send(frame)`, `hold(targets, context)`, and `cancel(targets)` + return a `CommandAcknowledgement` with `accepted`, `rejected`, or + `timed_out` status; +- `EndpointCommandRouter` is the standard mixed-controller sink. It preflights + every frame, groups commands or targets by exact transport ID, dispatches to + registered `EndpointCommandTransport` implementations, and accepts only when + every addressed transport accepts; - `ExecutionClock` supplies monotonic time and backend waiting; - non-blocking `step()` dispatches only when the current command's `hold_duration` has elapsed; +- `revise_current()` stages an owned same-address revision, preserves the active + frame deadline, and replans it from the next due observation; - `run_until_blocked()` is a convenience loop that waits through the clock and stops at a terminal state or an unhandled effect-verification boundary; the runner remembers that boundary so a later verifier call can resume it; -- cancellation, observation/session exceptions, and negative acknowledgements - enter a best-effort cancel-then-hold path. +- before dispatch, the runner records every target that may become armed by + `(transport_id, target_id)`; cancellation, observation/session/controller + exceptions, and negative acknowledgements enter target-scoped safe stop: + cancel all recorded targets first, then hold them from a fresh observation or + the last validated context when one is available. + +Every transport must actively neutralize inactive rows for each addressed +target. Omission is unsafe for persistent controllers: position transports +hold those rows and velocity transports normally command zero velocity. `TimedTrajectory.dt[:, i]` is the interval leading to sample `i`. -`ExecutionSession` dispatches sample zero immediately, then maps each following -arrival interval to the preceding command's `JointCommand.hold_duration`. The -final sample uses its own interval again as a settling window before terminal -validation. Batched execution currently advances at a synchronized barrier -using the longest active row interval. - -`SimulationExecutionAdapter` implements observation, command, and clock ports -for a `SimulationManager`/`Robot` pair. Its `sleep()` advances an integral +The built-in joint lowerer dispatches sample zero immediately, then maps each +following arrival interval to the preceding `RuntimeCommandFrame`'s +`hold_duration`. The final frame uses its own interval again as a settling +window before terminal validation. Generic action implementations set frame +hold durations directly. Batched execution currently advances at a +synchronized barrier using the longest active row interval. + +`SimulationExecutionAdapter` implements observation and clock ports plus the +exact `robot.joint_position` endpoint transport for a +`SimulationManager`/`Robot` pair. It can serve directly as the command sink for +joint-only plans or be registered in an `EndpointCommandRouter` beside mobile, +whole-body, or device-specific transports. Its `sleep()` advances an integral number of physics steps, so simulation execution does not depend on wall time. Stable context IDs are correlation identifiers; the adapter maps command rows to simulation robot indices rather than using those IDs as array indices. -Real-device adapters should implement the same protocols and enforce the passed -acknowledgement timeout in their transport/controller layer. +Real-device transports should implement `EndpointCommandTransport` and enforce +the passed acknowledgement timeout in their controller/client layer. `SceneProvider.snapshot(timestamp=..., env_ids=...)` is the scene-observation boundary used by execution adapters. `SceneSnapshot.collision_entity_ids` @@ -500,14 +559,20 @@ configures controller acknowledgement deadlines, scheduler cadence, and final safe-hold behavior for one runner instance; it does not change skill planning semantics and does not belong in `ActionInvocation` or an invocation revision. -Every `ActionBinding` value is a `RobotCfg.control_parts` key. It is not a link, -TCP-frame, joint, or scene-object name. Planning services validate those names -and resolve immutable `ResolvedControlPart` values containing full-robot joint -indices. Built-ins use the binding as the only source for participating arm and -hand names; attachment state and `StateDelta` keys use the bound manipulator. - -Embodiment-specific joint commands do not belong to Action options. A caller -using the legacy direct-core path without a `RobotSkillProfile` registers them +`ActionBinding` is an engine-owned tuple of `EndpointBinding` values, not a map +of arm/tool roles. Each endpoint is addressed by the contract's exact +`(slot_id, endpoint_id)` key and contains its logical `resource_id`, adapter ID, +capabilities, semantic commands, claims, and immutable runtime target. A +`RuntimeEndpointTarget` is controller addressing, not a live controller: its +`transport_id` selects a transport and its `target_id` selects the destination +within that transport. `JointPositionTarget` is the built-in target for a named +`RobotCfg.control_parts` entry and additionally owns its full-robot joint IDs. +Built-in joint primitives explicitly require that target type when they need +IK, joint interpolation, or current attachment keys; a custom mobile or +whole-body skill is not required to masquerade as an arm or hand. + +Embodiment-specific semantic commands do not belong to action options. A caller +using direct control-part binding without a `RobotSkillProfile` registers them by actual control-part name: ```python @@ -523,29 +588,31 @@ engine = AtomicActionEngine( ) ``` -Actions request semantic commands (`open`, `grasp`, or a named joint target) -from the `ResolvedControlPart`. `ActionControlOverrides` may replace commands -by semantic binding role for one invocation revision. Joint limits constrain -commands but do not define semantic open/grasp states; a robot integration or -tutorial may derive a simple profile from limits explicitly. Profile-based -integrations instead own commands under generic `command_profiles` IDs and let -endpoint declarations/adapters resolve those IDs; only -`action_control_profiles()` converts applicable control-part endpoints back to -the legacy core mapping. +Actions request semantic commands (`open`, `grasp`, or a named target) from an +`EndpointBinding`; `joint_positions()` is the typed convenience for a +`JointPositionCommand`. `ActionControlOverrides` may replace commands under the +exact `slot -> endpoint -> command` path for one invocation revision. Joint +limits constrain commands but do not define semantic open/grasp states; a robot +integration or tutorial may derive a simple profile from limits explicitly. +Profile-based integrations instead own commands under generic +`command_profiles` IDs and let endpoint declarations/adapters resolve those +IDs. `action_control_profiles()` additionally exposes applicable control-part +commands to the built-in joint planning helpers; custom endpoint commands stay +on their resolved endpoint. ## Built-ins -| Skill ID | Goal type | Roles | +| Skill ID | Goal type | Required slot endpoints | |---|---|---| -| `move_end_effector` | `EndEffectorPoseGoal` | manipulator `primary` | -| `move_joints` | `JointPositionGoal` (`target` is explicit qpos or a profile command name) | manipulator `primary` | -| `pick_up` | `GraspGoal` | manipulator/end effector `primary` | -| `move_held_object` | `HeldObjectPoseGoal` | manipulator/end effector `primary` | -| `place` | `PlaceGoal`, `AssembleGoal` | manipulator/end effector `primary` | -| `press` | `PressGoal` | manipulator/end effector `primary` | -| `coordinated_pickment` | `CoordinatedPickGoal` | `left`, `right` | -| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing`, `support` | -| `hand_over` | `GraspGoal` | `source`, `destination` | +| `move_end_effector` | `EndEffectorPoseGoal` | `primary.motion` | +| `move_joints` | `JointPositionGoal` (`target` is explicit qpos or a profile command name) | `primary.motion` | +| `pick_up` | `GraspGoal` | `primary.motion`, `primary.grasp` | +| `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | +| `place` | `PlaceGoal`, `AssembleGoal` | `primary.motion`, `primary.grasp` | +| `press` | `PressGoal` | `primary.motion`, `primary.grasp` | +| `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` | `GraspGoal.grasp_xpos` accepts an explicit pose tensor, a late-bound `SceneEntityPose`, or `None` for affordance sampling. A `SceneEntityPose` @@ -566,20 +633,29 @@ snapshot-grounded object example. 1. Define a frozen action-owned goal dataclass with `goal_kind`. 2. Define a frozen `ActionOptions` subclass only when runtime behavior exists. -3. Declare `skill_id`, `GoalType`, `OptionsType`, and required core roles. Also - declare a class-local `SkillBindingContract` when the skill should appear in - `engine.skills`; route every current core role exactly once. +3. Declare `skill_id`, `GoalType`, `OptionsType`, and a class-local + `SkillBindingContract` when the skill should appear in `engine.skills`. + Express only semantic slots, endpoint requirements, capabilities, required + commands, and any real disjointness constraints; do not introduce arm/tool + roles for a mobile-base or whole-body endpoint. 4. Implement `_plan()`; do not override the framework-owned `plan()` method. -5. Validate with `require_goal(request)` and consume only the resolved binding. +5. Validate with `require_goal(request)` and consume endpoints only through + `request.binding.endpoint(slot_id, endpoint_id)`. Require a concrete target + subtype only when the planner or payload implementation genuinely needs it. 6. Plan from `context.robot.qpos`; never read an implicit live start state. 7. If planning consumes a semantic object's snapshot pose, override `_scene_dependencies()`, preserve `super()` dependencies, and add exactly that semantic ID. -8. Return full-robot positions or a `TimedTrajectory` through `build_plan()`. - Build batched `list[PlanState]`, translate the policy with - `request.motion_policy.to_motion_gen_options()`, and call - `self.motion_generator.generate()`. Import pure operations directly from - `trajectory_ops.py`. +8. For planner-backed joint motion, return full-robot positions or a + `TimedTrajectory` through `build_plan()`: build batched `list[PlanState]`, + translate the policy with `request.motion_policy.to_motion_gen_options()`, + call `self.motion_generator.generate()`, and import pure operations directly + from `trajectory_ops.py`. For mobile, whole-body, or other controller-native + motion, build `EndpointCommand` frames and a `TimedCommandSequence`, then use + `build_command_plan()`. A new transport family must define matching + `RuntimeEndpointTarget` and `RuntimeCommandPayload` types with the same + `transport_id`, plus an `EndpointCommandTransport` registered in the runner's + router. 9. Declare symbolic changes with `StateDelta`; do not mutate context or commit physical effects during planning. For partial attachment updates, retain previous scalar semantics while any previous row remains; merge only batched diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 5a69e51ec..bc70bd30d 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,9 +1,9 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: implementation in progress; Phase 0 and PR1 complete, PR2A and PR2B - implemented on stacked feature branches -- Baseline: `main@e445133c79c8b32019dab1c844b799b43a1658d6` -- Last updated: 2026-08-10 +- Status: implementation in progress; Phase 0 and PR1 complete, and PR2A, + PR2B, and PR2C implemented on stacked feature branches +- 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: @@ -235,10 +235,13 @@ callers. #### Core/advanced layer -The current `ActionGoal`, `ActionInvocation`, `ActionBinding`, policies, -`PlanningContext`, `ActionPlan`, `ExecutionSession`, `ExecutionRunner`, and -provider protocols remain available for framework authors and unusual -integrations. They are no longer prerequisites for ordinary task authoring. +The current `ActionGoal`, `ActionInvocation`, generic endpoint +`ActionBinding`, policies, `PlanningContext`, `ActionPlan`, +`ExecutionSession`, `ExecutionRunner`, and provider protocols remain available +for framework authors and unusual integrations. `ActionPlan.commands` is the +runtime authority; a joint-backed plan may additionally retain a +`TimedTrajectory` for joint feedback and offline compilation. These contracts +are no longer prerequisites for ordinary task authoring. ### 6.2 Proposed package ownership @@ -255,7 +258,9 @@ embodichain/lab/sim/skills/ effects.py # built-in EffectMonitor contracts/implementations embodichain/lab/sim/atomic_actions/ - ... # existing typed core and built-in atomic planners + runtime_commands.py # transport-neutral endpoint payloads and timed frames + transports.py # endpoint transport protocol and exact-ID router + ... # typed core and built-in atomic planners embodichain/lab/gym/envs/expert_program/ cfg.py # strict @configclass schema @@ -399,11 +404,11 @@ an `arm + tool` schema. It contains a generic resource DAG: through a `ResourceEndpoint` implementation; `ControlPartEndpoint` is the current joint/control-part declaration, while registered `ResourceEndpointAdapter`s resolve any endpoint kind into generic - `EndpointResolution` metadata (binding values, commands, physical claim - tokens, and optional joint IDs) without changing the graph, matcher, or slot - model. Adapters register by exact endpoint type; the built-in control-part - adapter is not overrideable, and different controller semantics use a new - endpoint subtype; + `EndpointResolution` metadata (a typed runtime target, command-profile key, + physical claim tokens, and optional joint IDs) without changing the graph, + matcher, or slot model. Adapters register by exact endpoint type; the + built-in control-part adapter is not overrideable, and different controller + semantics use a new endpoint subtype; - members describe physical composition and claim closure, not capability inheritance. A composite must explicitly declare `motion.whole_body`; it does not acquire that capability because it contains a base, torso, or arms; @@ -432,10 +437,10 @@ combinations such as `left_arm + right_hand`. Endpoint names are local protocols, not global robot-part categories. A future `navigate` skill can require `body.motion: motion.base.se2`; a `whole_body_reach` skill can require `body.motion: motion.whole_body`. Neither -requires new `RobotSkillProfile` fields. The current `ActionBindingRoute` is a -transition adapter from generic endpoints to the core's existing -`manipulators`/`end_effectors` maps; those maps are not part of the Profile -resource model. +requires new `RobotSkillProfile` fields. Profile resolution lowers every +required endpoint directly into an engine-owned `ActionBinding` keyed by +`(slot_id, endpoint_id)` and carrying its typed runtime target; there is no +arm/tool-shaped intermediate binding layer. Binding follows strict rules: @@ -458,11 +463,11 @@ adapter-defined physical/controller claim tokens. It makes `whole_body` conflict with `base`, `torso`, or a contained arm even when the underlying `Robot.control_parts` names are different, and lets a non-joint base adapter claim a controller without inventing joints. PR2B -exposes deterministic claim/conflict data only. Current runners emit and hold -full-robot commands, so claims do not imply safe parallel execution. Parallel -scheduling still requires one coordinator, joint-mask command merge, planner -serialization or isolation, cancellation semantics, and inter-trajectory -collision checks. +exposes deterministic claim/conflict data only. PR2C runners emit endpoint +command frames and transports own target-scoped safe holds, but claims still do +not imply safe parallel execution. Parallel scheduling still requires one +coordinator, deterministic command arbitration/merge, planner serialization or +isolation, cancellation semantics, and inter-trajectory collision checks. `AtomicActionEngine.actions` remains the direct-core implementation registry. `engine.skills` contains only installed, agent-visible actions whose concrete @@ -701,8 +706,8 @@ Gym-aware runtime ports: - observation provider: captures a current planning context from the environment and scene registry; -- command sink: buffers the next full-robot command for the environment action - manager; +- command sink: buffers the next transport-neutral endpoint-command frame for + the environment action manager; - clock: advances only when the demo executor calls `env.step()`; - metadata sink: records compiler decisions, action trajectory segments, effects, recovery, scene revisions, and post-policy results. @@ -717,11 +722,11 @@ normally, then resume with a fresh observation. `BaseEnv.step_dt` is the authoritative control cadence. Semantic task configuration does not expose `control_dt`. -Version 1 should require every emitted `JointCommand.hold_duration` to be -representable by an integer number of environment steps, preferably one step -per yielded command. An incompatible command is rejected with a clear timing -error; it is not silently resampled. Explicit timed-command resampling can be a -later, separately tested feature. +Version 1 should require every emitted +`RuntimeCommandFrame.hold_duration` to be representable by an integer number +of environment steps, preferably one step per yielded frame. An incompatible +frame is rejected with a clear timing error; it is not silently resampled. +Explicit timed-command resampling can be a later, separately tested feature. Recovery timeout and retry budgets are scoped to the enclosing action attempt. A `TrajectorySegment` does not start an independent timer or own a recovery @@ -873,6 +878,10 @@ PR1 snapshot/identity bridge (complete) v v PR2A SceneRegistry PR2B RobotSkillProfile (implemented) (implemented) + | | + | v + | PR2C Runtime Endpoints + | (in progress) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -963,8 +972,9 @@ the documented deprecated fallbacks. ### Phase 1: unified integration data -Phase 1 is implemented as two focused follow-up PRs that join before the -semantic facade/compiler work. +Phase 1 is implemented as three focused follow-up PRs. PR2A and PR2B branch +from the PR1 foundation; PR2C follows PR2B and joins PR2A before the semantic +facade/compiler work. #### PR2A: SceneRegistry (implemented on the feature branch) @@ -1014,7 +1024,7 @@ Deliverables: `EndpointResolution` protocol; `ControlPartEndpointAdapter` is the first implementation; - action-owned `SkillBindingContract`s with participant-local endpoint, - capability, typed-command, lowering-route, and disjoint-claim requirements; + capability, typed-command, and disjoint-claim requirements; - capability-based candidate filtering, complete per-skill defaults, explicit selection overrides, and deterministic ambiguity/unsupported diagnostics; - profile-owned semantic commands plus immutable, versioned planning/recovery/ @@ -1023,8 +1033,8 @@ Deliverables: parts, joint ownership, endpoint overlap, configured solvers, commands, and presets; - immutable leaf/joint/adapter-token `ResourceClaim` data and explicit - same-slot endpoint disjointness for future conflict analysis without claiming - that the current full-robot command runner supports safe parallel execution. + 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 @@ -1035,11 +1045,61 @@ PR2B may proceed in parallel with PR2A after the PR1 bridge. Neither follow-up requires official task migration; the repeated-cube vertical slice opts in only after the registry, profile, compiler, runtime, and demo bridge are available. +#### PR2C: generic runtime endpoints (implemented on the feature branch) + +PR2C removes the temporary arm/tool lowering seam and makes the profile's +generic endpoint model executable end to end: + +- `ActionBinding` is an engine-owned collection keyed only by + `(slot_id, endpoint_id)`; `ActionBindingRoute`, arm/tool role maps, and the + intermediate resolved-control-part binding types are removed as an + intentional clean break; +- every resolved profile endpoint owns a typed immutable + `RuntimeEndpointTarget`, while `EndpointCommand` combines that destination + with a transport-specific `RuntimeCommandPayload`; +- `RuntimeCommandFrame` synchronizes per-environment endpoint commands and + timing, and `TimedCommandSequence` becomes the authoritative runtime content + of `ActionPlan`; +- `EndpointCommandTransport` and `EndpointCommandRouter` perform exact-ID + registration, preflight payload validation, transport grouping, + acknowledgement aggregation, cancellation, and transport-owned safe holds; +- the framework authorizes planned commands against binding-owned targets and + physical claims, requires stable destinations across frames and recovery + replans, and retains previously active targets when a failed plan is empty; +- transports actively neutralize inactive environment rows for every addressed + target instead of treating an omitted write as a safe state; +- `SimulationExecutionAdapter` implements the built-in joint-position + transport and writes or holds only the joints claimed by each addressed + endpoint; +- joint-backed planners retain an optional full-robot `TimedTrajectory` for + existing joint feedback and `engine.compile()`, while non-joint plans use + timed completion plus the existing semantic-effect verification boundary; +- full-body joint control and a custom planar-velocity endpoint are exercised + from binding/profile resolution through planning, session execution, routing, + completion, and safe hold without arm/tool-shaped fields. +- an explicit invocation revision declares the same non-empty runtime + destination set and preserves each target's address/safe-hold fingerprint. + The runner keeps the active frame deadline and replans from a fresh due-time + observation; a pending physical effect must be verified first. Changing a + base, arm, whole-body, controller destination, or hold footprint starts a new + invocation rather than hot-switching controller ownership in place. + +PR2C does not add parallel scheduling, claim merging, transport rollback, or a +generic endpoint-feedback evaluator. It also does not add cross-destination +hot revision. Those require separate contracts. + +PR2C exit criteria: an installed custom endpoint kind needs one reusable +endpoint declaration/adapter, payload, transport, and shared atomic skill, but +no core binding or runner changes; whole-body joint endpoints use the same +path; unknown transports and incompatible payloads fail before dispatch; and +cancel/hold behavior remains transport-owned and auditable. + Combined Phase 1 exit criteria: an object is registered once under an authoritative ID, aliases cannot introduce ambiguity, dynamic-object configuration mismatches fail before execution with an entity-centric -diagnostic, and robot capabilities resolve bindings/presets without task-owned -motion code. +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. ### Phase 2: semantic facade and compiler @@ -1211,6 +1271,9 @@ The design is complete when all of the following hold: - [x] Robot capability binding is expressed through generic participant resources and endpoints, so mobile-base and whole-body skills do not require new arm/tool-shaped profile fields. +- [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 across semantics, observation, affordance, and collision handling; simulation `uid` values are legacy aliases only. 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 5345daee4..b2b0236d9 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 @@ -9,8 +9,9 @@ embodichain.lab.sim.atomic_actions ActionGoal ActionBinding - ResolvedActionBinding - ResolvedControlPart + EndpointBinding + RuntimeEndpointTarget + JointPositionTarget ControlCommand JointPositionCommand ControlPartCommandProfile @@ -27,8 +28,14 @@ embodichain.lab.sim.atomic_actions PlanningContext StateDelta TimedTrajectory + RuntimeCommandPayload + JointPositionPayload + EndpointCommand + RuntimeCommandFrame + TimedCommandSequence TrajectorySegment PlannerDiagnostics + ExecutionFeedbackMode ActionPlan CompiledTrajectory @@ -40,7 +47,6 @@ embodichain.lab.sim.atomic_actions SkillBindingContract SkillResourceSlot SkillEndpointRequirement - ActionBindingRoute DisjointSlotEndpoints DisjointResourceSlots @@ -57,6 +63,8 @@ embodichain.lab.sim.atomic_actions RunnerStatus ObservationProvider CommandSink + EndpointCommandTransport + EndpointCommandRouter CommandAcknowledgement CommandAckStatus CommandDispatch @@ -65,7 +73,6 @@ embodichain.lab.sim.atomic_actions SimulationExecutionAdapter ExecutionTick EffectVerificationRequest - JointCommand ExecutionEvent ExecutionEventKind ExecutionStatus @@ -116,9 +123,6 @@ Semantic resource contracts .. autoclass:: SkillEndpointRequirement :members: -.. autoclass:: ActionBindingRoute - :members: - .. autoclass:: DisjointSlotEndpoints :members: @@ -146,10 +150,13 @@ Planning and state .. autoclass:: ActionBinding :members: -.. autoclass:: ResolvedActionBinding +.. autoclass:: EndpointBinding + :members: + +.. autoclass:: RuntimeEndpointTarget :members: -.. autoclass:: ResolvedControlPart +.. autoclass:: JointPositionTarget :members: .. autoclass:: ControlCommand @@ -202,6 +209,24 @@ Planning and state .. autoclass:: TimedTrajectory :members: +.. autoclass:: RuntimeCommandPayload + :members: + +.. autoclass:: JointPositionPayload + :members: + +.. autoclass:: EndpointCommand + :members: + +.. autoclass:: RuntimeCommandFrame + :members: + +.. autoclass:: TimedCommandSequence + :members: + +.. autoclass:: ExecutionFeedbackMode + :members: + .. autoclass:: ActionPlan :members: @@ -230,6 +255,12 @@ Engine and execution .. autoclass:: CommandSink :members: +.. autoclass:: EndpointCommandTransport + :members: + +.. autoclass:: EndpointCommandRouter + :members: + .. autoclass:: ExecutionClock :members: @@ -260,9 +291,6 @@ Engine and execution .. autoclass:: ExecutionTick :members: -.. autoclass:: JointCommand - :members: - .. autoclass:: ExecutionEvent :members: diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 0ad26c50a..9783df20f 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -21,9 +21,10 @@ the built-in catalog. Generic motion and recovery choices belong to the invocation, and per-call primitive behavior belongs to `skill_options`. Registration only installs an implementation. Whether a built-in is executable -for a particular call still depends on its binding roles, the robot's control -parts, semantic command profiles, and task-state preconditions. Action Agent -adapters must also honor `agent_visible` and filter by embodiment capability. +for a particular call still depends on its `SkillBindingContract`, the selected +resource endpoints, semantic command profiles, and task-state preconditions. +Action Agent adapters must also honor `agent_visible` and filter by embodiment +capability. ```{note} The current manipulation primitives consume semantic `open` and `grasp` @@ -135,27 +136,27 @@ The animations below are the focused simulator demos under ## Capability matrix -| Skill ID | Accepted goal | Required binding roles | Required profile commands | Required task state | Expected task effect | +| Skill ID | Accepted goal | Required endpoints | Required profile commands | Required task state | Expected task effect | |---|---|---|---|---|---| -| `move_end_effector` | `EndEffectorPoseGoal` | manipulator `primary` | none | none | none | -| `move_joints` | `JointPositionGoal` | manipulator `primary` | named target only: command matching `target` | none | none | -| `pick_up` | `GraspGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | semantic object/entity | attach object to `primary` manipulator | -| `move_held_object` | `HeldObjectPoseGoal` | manipulator + end effector `primary` | primary: `grasp` | object held by `primary` | preserve attachment | -| `place` | `PlaceGoal`, `AssembleGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | `AssembleGoal` requires an object held by `primary`; ordinary `PlaceGoal` has no planner-enforced attachment precondition | detach object | -| `press` | `PressGoal` | manipulator + end effector `primary` | primary: `grasp` | none | none | -| `coordinated_pickment` | `CoordinatedPickGoal` | manipulator + end effector `left`, `right` | both: `open`, `grasp` | semantic object/entity | create coordinated attachment; clear individual attachments | -| `coordinated_placement` | `CoordinatedPlacementGoal` | manipulator + end effector `placing`, `support` | placing: `open`, `grasp`; support: `grasp` | one individually held object per arm | optionally detach placing object; preserve support attachment | -| `hand_over` | `GraspGoal` | manipulator + end effector `source`, `destination` | both: `open`, `grasp` | object held by source arm | transfer attachment to destination arm | - -### Binding role meanings - -Roles are action-local semantic participant slots. They are keys declared by an -action, while the corresponding `ActionBinding` values are concrete -`Robot.control_parts` keys. A role that appears in both binding maps identifies -the manipulator and actuated hand/tool serving the same functional participant; -it does not make the two maps interchangeable. - -| Role | Used by | Meaning | +| `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 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 | + +### Participant slot meanings + +Slots are action-local semantic participants declared by +`SkillBindingContract`. Each slot contains endpoint requirements such as +`motion` and `grasp`; the profile binder matches their capabilities and typed +commands to a robot resource, then adapters produce the generic +`EndpointBinding` values owned by `ActionBinding`. + +| Slot | Used by | Meaning | |---|---|---| | `primary` | Single-participant skills | Principal participant for this invocation; it has no inherent left/right or default-robot meaning | | `source` | `hand_over` | Participant that initially holds and transfers the object | @@ -164,10 +165,12 @@ it does not make the two maps interchangeable. | `placing` | `coordinated_placement` | Participant that aligns and optionally releases the placing object | | `support` | `coordinated_placement` | Participant that keeps holding and positioning the support object | -The action's `manipulator_roles` and `end_effector_roles` declarations determine -which entries are required. The engine checks that those entries exist and that -every value resolves through `Robot.control_parts`; the caller or capability -binder must select a physically compatible arm and hand/tool combination. +Each endpoint requirement declares an open capability set and optional typed +semantic commands. Intra-slot and inter-slot disjointness constraints express +physical compatibility without global arm/tool categories. The built-in +control-part adapter resolves current joint-backed endpoints through +`Robot.control_parts`; custom adapters may instead return mobile, whole-body, or +other runtime targets. `MoveJoints` is intentionally `agent_visible=False`: it is useful for home, recovery, calibration, and scripted postures, but is not exposed to an Action @@ -241,9 +244,10 @@ do not participate in identity. Use this rule when configuring a built-in or adding a new one: - the **goal** carries only the requested outcome; -- the **binding** carries semantic-role mappings to control-part names selected - for this call; every value must be a key in the engine robot's - `control_parts` mapping; +- the skill's **binding contract** declares participant slots, endpoint + capabilities, required typed commands, and physical disjointness; +- the engine-owned **binding** carries adapter-resolved `EndpointBinding` + snapshots and immutable runtime targets selected for this call; - typed **skill options** carry segment-specific behavior that may vary by invocation; an action may provide defaults; - the engine's **control-part profiles** carry embodiment-specific semantic @@ -252,32 +256,35 @@ Use this rule when configuring a built-in or adding a new one: collision choice, and planner options; - `RecoveryPolicy` carries all replan/retry thresholds and budgets. -All built-ins resolve participating arm and hand names exclusively from -`ActionBinding`. The engine then resolves the selected control part's profile +All built-ins resolve their `motion` and `grasp` endpoints exclusively from the +generic `ActionBinding`. The built-in control-part adapter resolves joint IDs and checks each joint-position command against its DoF. Invocation-level -`ActionControlOverrides` may replace a command by binding role for one explicit -revision. +`ActionControlOverrides` may replace a command by `(slot, endpoint)` for one +explicit revision. ### Planning and effect semantics -Every action returns a per-environment `plan_success` mask and one or more -full-robot trajectories. `plan_success=True` means motion planning succeeded; -it does not prove contact or object transfer. Actions that change attachment -state declare a `StateDelta`. Offline `compile()` projects it hypothetically; -closed-loop execution commits it only after external effect verification. +Every action returns a per-environment `plan_success` mask and an +`ActionPlan.commands` sequence of `RuntimeCommandFrame` values. Current +joint-planned built-ins also retain `ActionPlan.joint_trajectory` for joint +feedback, inspection, and static projection. `plan_success=True` means planning +succeeded; it does not prove contact or object transfer. Actions that change +attachment state declare a `StateDelta`. Offline `compile()` projects it +hypothetically; closed-loop execution commits it only after external effect +verification. (builtin-move-end-effector)= ## `MoveEndEffector` -Plans a free-space motion for a bound manipulator to reach one EEF pose or an -ordered set of pose waypoints. +Plans a free-space motion for the bound `primary.motion` endpoint to reach one +EEF pose or an ordered set of pose waypoints. | Contract | Value | |---|---| | Skill ID | `move_end_effector` | | Goal | `EndEffectorPoseGoal(xpos=...)` | -| Binding | manipulator role `primary` | +| Binding contract | `primary.motion` with Cartesian-pose capability | | Motion | EEF planning from observed arm qpos; output expanded to full robot DoF | | Completion | `EEF_GOAL_REACHED` | | Effect | none | @@ -301,7 +308,7 @@ than an EEF pose. |---|---| | Skill ID | `move_joints` | | Goal | `JointPositionGoal(target=...)` | -| Binding | manipulator role `primary` | +| Binding contract | `primary.motion` with joint-position capability | | Motion | joint planning/interpolation from observed qpos; supports joint waypoints | | Completion | `JOINT_GOAL_REACHED` | | Effect | none | @@ -309,7 +316,7 @@ than an EEF pose. `target` accepts an explicit qpos tensor with shape `(control_dof,)`, `(B, control_dof)`, or `(B, N, control_dof)`, or a non-empty string resolved -from the bound manipulator's `ControlPartCommandProfile`. Named poses remain +from the bound `primary.motion` endpoint's command profile. Named poses remain embodiment knowledge without becoming separate goal types: ```python @@ -331,15 +338,15 @@ named_goal = JointPositionGoal(target="home") ## `PickUp` Plans **approach -> close hand -> lift** and declares the object attached to the -bound manipulator. +bound motion target. | Contract | Value | |---|---| | Skill ID | `pick_up` | | Goal | `GraspGoal(semantics=..., grasp_xpos=None)` | -| Binding | manipulator + end effector role `primary` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | | Precondition | `ObjectSemantics.entity_id` resolves in the planning snapshot; the deprecated live `entity` fallback remains temporarily; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | -| Effect | write `HeldObjectState` for the bound manipulator and clear overlapping coordinated attachment state | +| Effect | write `HeldObjectState` for the bound motion target and clear overlapping coordinated attachment state | | Verification | the attachment effect must be verified during closed-loop execution | `grasp_xpos` may be `(4, 4)`, `(B, 4, 4)`, or a `SceneEntityPose`. A scene @@ -355,7 +362,7 @@ 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. -`PickUp` requires `open` and `grasp` commands on the bound end-effector profile. +`PickUp` requires typed `open` and `grasp` commands on `primary.grasp`. Important `PickUpOptions` fields: | Field | Purpose | @@ -390,16 +397,16 @@ a live scene entity. |---|---| | Skill ID | `move_held_object` | | Goal | `HeldObjectPoseGoal(object_target_pose=...)` | -| Binding | manipulator + end effector role `primary` | -| Precondition | a `HeldObjectState` exists for the bound manipulator, normally from `PickUp` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| Precondition | a `HeldObjectState` exists for the bound motion target, normally from `PickUp` | | Motion | single object-centric transport segment with closed-hand qpos | | Effect | none; the existing attachment is preserved | | Dynamic target | explicit pose or `SceneEntityPose` | -The bound end-effector profile must provide `grasp`; optional upright-transport -settings belong to `MoveHeldObjectOptions`. The arm and hand are selected by -`ActionBinding`; generic timing and trajectory sampling remain in -`MotionPolicy`. +The bound `primary.grasp` endpoint must provide `grasp`; optional +upright-transport settings belong to `MoveHeldObjectOptions`. The participant's +motion and grasp endpoints are selected through `ActionBinding`; generic timing +and trajectory sampling remain in `MotionPolicy`. **Example:** `scripts/tutorials/atomic_action/move_held_object.py` @@ -415,8 +422,8 @@ one. |---|---| | Skill ID | `place` | | Goal | `PlaceGoal(xpos=..., tcp_symmetry="none")` | -| Binding | manipulator + end effector role `primary` | -| State | consumes the bound manipulator's attachment when present | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| State | consumes the bound motion target's attachment when present | | Effect | detach the object and clear overlapping coordinated attachment state | | Verification | release must be verified during closed-loop execution | | Dynamic target | explicit pose/waypoints or `SceneEntityPose` | @@ -426,7 +433,7 @@ translation remain physically equivalent. The action selects the closer orientation variant from the observed starting state and uses it consistently across all waypoints. -The bound end-effector profile must provide `open` and `grasp`. Important +The bound `primary.grasp` endpoint must provide `open` and `grasp`. Important `PlaceOptions` fields: | Field | Purpose | @@ -476,16 +483,16 @@ arm should retreat along its planned path after reaching the target. |---|---| | Skill ID | `press` | | Goal | `PressGoal(xpos=...)` | -| Binding | manipulator + end effector role `primary` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | | Motion | close, press, joint-space return | | Effect | none; existing attachment state is unchanged | | Dynamic target | explicit pose or `SceneEntityPose` | -The bound end-effector profile must provide `grasp`, while -`PressOptions.hand_interp_steps` controls the close interpolation. The arm and -hand control parts come from `ActionBinding`. Contact detection is not itself a -symbolic effect in the current action; applications that require force/contact -confirmation should verify it externally. +The bound `primary.grasp` endpoint must provide `grasp`, while +`PressOptions.hand_interp_steps` controls the close interpolation. Both +endpoints come from the generic `ActionBinding`. Contact detection is not +itself a symbolic effect in the current action; applications that require +force/contact confirmation should verify it externally. **Example:** `scripts/tutorials/atomic_action/press.py` @@ -500,7 +507,7 @@ both hands -> lift -> move object -> hold**. |---|---| | Skill ID | `coordinated_pickment` | | Goal | `CoordinatedPickGoal` | -| Binding | manipulator + end effector roles `left` and `right` | +| Binding contract | disjoint `left` and `right` slots, each with disjoint `motion` and `grasp` endpoints | | Precondition | an `AntipodalAffordance`; when `object_initial_pose` is omitted, `ObjectSemantics.entity_id` resolves in the snapshot or the deprecated no-ID live fallback is available | | Goal geometry | shared-object target pose and optional initial object pose; left/right grasps are sampled from the affordance | | Effect | clear individual left/right attachments and create `CoordinatedHeldObjectState[(left, right)]` | @@ -522,7 +529,7 @@ no-ID `entity` fallback is live and therefore cannot trigger scene-motion replanning. Supplying `object_initial_pose` disables this implicit semantic dependency because the explicit pose value is authoritative. -Both bound end-effector profiles must provide `open` and `grasp`. Important +Both bound grasp endpoints must provide `open` and `grasp`. Important `CoordinatedPickmentOptions` fields group into: - `pre_grasp_distance` and `lift_height`; @@ -530,8 +537,9 @@ Both bound end-effector profiles must provide `open` and `grasp`. Important - `approach_direction`, `left_to_right_arm_direction`, and `middle_empty_ratio` for affordance-based left/right grasp sampling. -The left/right arms and hands come exclusively from the corresponding binding -roles. Coordinated dual-arm planning with `strategy="motion_gen"` is not +The left/right motion and grasp endpoints come exclusively from the +corresponding participant slots. Coordinated dual-arm planning with +`strategy="motion_gen"` is not supported by the cuRobo backend; use the supported IK/interpolation path for this primitive. @@ -548,7 +556,7 @@ hold -> optionally release the placing hand -> retreat the placing arm**. |---|---| | Skill ID | `coordinated_placement` | | Goal | `CoordinatedPlacementGoal` | -| Binding | manipulator + end effector roles `placing` and `support` | +| Binding contract | disjoint `placing` and `support` slots, each with disjoint `motion` and `grasp` endpoints | | Precondition | separate `HeldObjectState` entries exist for both bound arms | | Goal geometry | placing/support object target poses, optional height offsets, optional release override | | Effect | preserve support attachment; remove or preserve placing attachment according to `release`; clear overlapping coordinated state | @@ -557,15 +565,15 @@ Both object targets may use `SceneEntityPose`, so either can participate in dynamic-goal invalidation. Goal-level height/release values override `CoordinatedPlacementOptions` for that invocation. -The placing profile must provide `open` and `grasp`; the support profile must -provide `grasp`. Important `CoordinatedPlacementOptions` fields group into: +The `placing.grasp` endpoint must provide `open` and `grasp`; `support.grasp` +must provide `grasp`. Important `CoordinatedPlacementOptions` fields group into: - default `release`, placing/support height offsets, and `lift_height`; - `hand_interp_steps`, `hold_steps`, and `retreat_steps`. -The placing/support arms and hands come exclusively from the corresponding -binding roles. The same cuRobo restriction as coordinated pickment applies to dual-arm -`strategy="motion_gen"` planning. +The placing/support motion and grasp endpoints come exclusively from the +corresponding participant slots. The same cuRobo restriction as coordinated +pickment applies to dual-arm `strategy="motion_gen"` planning. **Example:** `scripts/tutorials/atomic_action/coordinated_placement.py` @@ -581,16 +589,16 @@ retreats -> destination delivers**. |---|---| | Skill ID | `hand_over` | | Goal | `GraspGoal(semantics=...)` | -| Binding | manipulator + end effector roles `source` and `destination` | +| Binding contract | disjoint `source` and `destination` slots, each with disjoint `motion` and `grasp` endpoints | | Precondition | source arm has a verified `HeldObjectState`; semantic object supports destination grasp selection | | Effect | remove source attachment and create destination `HeldObjectState` | | Verification | attachment transfer must be externally verified | -Both source and destination end-effector profiles must provide `open` and -`grasp`. `HandOverOptions` owns the destination grasp region and approach +Both source and destination grasp endpoints must provide `open` and `grasp`. +`HandOverOptions` owns the destination grasp region and approach direction, middle/final object poses, and segment distances/counts. The -source/destination arm and hand control parts come exclusively from the -corresponding `ActionBinding` roles. +source/destination motion and grasp endpoints come exclusively from the +corresponding generic `ActionBinding` slots. The middle and final poses are currently option tensors rather than `SceneEntityPose` goal fields. Consequently, handover supports tracking-error diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 321940c1f..0ae3d06b3 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -13,16 +13,18 @@ robot_skill_profiles ``` Atomic actions are the typed planning and execution boundary between a semantic -task request and robot joint commands. A caller describes **what** should happen -with an action-owned goal, grounds semantic roles onto robot resources, and -supplies the latest measured context. The action returns a full-robot, -time-aware plan without stepping simulation or claiming that a physical effect -has occurred. +task request and runtime endpoint commands. A caller describes **what** should +happen with an action-owned goal, selects resources for the skill's participant +slots, and supplies the latest measured context. The action returns a +transport-neutral, time-aware plan without stepping simulation or claiming that +a physical effect has occurred. ```{note} -The current built-ins focus on arm-and-gripper manipulation. They already emit -full-robot-DoF trajectories, but dexterous-hand policies, lower-body locomotion, -and whole-body control are not implemented by this module yet. +The current built-ins focus on arm-and-gripper manipulation and retain an +optional full-robot joint trajectory for planning feedback and inspection. The +binding and runtime-command contracts are not limited to joints: locomotion, +whole-body, or other controllers add capabilities, endpoint adapters, command +payloads, and transports without adding fixed resource categories to the core. ``` ## Architecture and responsibility boundary @@ -35,7 +37,7 @@ and whole-body control are not implemented by this module yet. | | v | semantic adapter: schema validation, | - SceneRegistry grounding, capability binding | + SceneRegistry grounding, endpoint binding | | | +------------------+------------------+ | @@ -60,14 +62,17 @@ and whole-body control are not implemented by this module yet. one ActionPlan fixed projection observed closed loop | | v v - CompiledTrajectory JointCommand + events + CompiledTrajectory RuntimeCommandFrame + events | v ExecutionRunner observe / schedule / dispatch | v - ObservationProvider + CommandSink + Clock + ObservationProvider + EndpointCommandRouter + Clock + | + v + EndpointCommandTransport(s) ``` The boundary is deliberate: @@ -79,19 +84,21 @@ The boundary is deliberate: | Perception and grounding | `SceneRegistry` on the canonical path; adapter or user application on the advanced path | Normalizes aliases to canonical typed references and publishes snapshots, or supplies already-grounded values directly | | Deterministic motion planning | Atomic action module | Produces an `ActionPlan` from an invocation and context | | Motion-generation resources | `AtomicActionEngine` | Owns one robot, motion generator, planner backend, device, trajectory builder, and control-part command profiles | -| Recovery state | `ExecutionSession` | Consumes fresh contexts, emits at most one `JointCommand` per tick, and owns bounded recovery/revision state | +| Recovery state | `ExecutionSession` | Consumes fresh contexts, emits at most one `RuntimeCommandFrame` per tick, and owns bounded recovery/revision state | | 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`, `CommandSink`, and `ExecutionClock` adapters | Isolates observation, command transport, and time/physics advancement from planning and session state | +| 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 | `ExecutionRunner.step()` is non-blocking. Its convenience `run_until_blocked()` loop waits or advances simulation through an injected clock. Observation errors, rejected or timed-out commands, session failures, and explicit cancellation trigger a best-effort cancel-then-hold sequence. -`SimulationExecutionAdapter` implements all three ports for a simulation robot; -real hardware integrations implement the same protocols without changing -action planning or recovery state. +`SimulationExecutionAdapter` provides observation, clock, and the built-in +`robot.joint_position` transport for a simulation robot. Register it with an +`EndpointCommandRouter`; real hardware integrations provide transports for the +same or additional endpoint kinds without changing action planning or recovery +state. ### Caller entry points @@ -101,10 +108,14 @@ semantic skill call that an adapter validates, grounds, and converts into an Python or load it from an application-owned configuration layer: ```python +binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, +) manual_invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), recovery_policy=RecoveryPolicy(max_replans=2), ) @@ -116,10 +127,10 @@ live_session = engine.start((manual_invocation,), latest_context) ``` A manual caller may bypass the semantic-schema adapter only when its target and -robot-resource binding are already grounded. Scene-relative goals still need a -current `PlanningContext`, and object names or semantic roles still need to be -resolved by the user application (or by reusing the same grounding adapter as -the Agent path). +robot-resource endpoints are already grounded. Scene-relative goals still need +a current `PlanningContext`, and object names or participant selections still +need to be resolved by the user application (or by reusing the same grounding +adapter as the Agent path). Both paths converge at `ActionInvocation + PlanningContext`. They therefore use the same goal validation, capability checks, planning backend, execution @@ -134,14 +145,14 @@ Application code normally chooses between these three public entry points: | API | Choose it when | Returns | State and observation behavior | |---|---|---|---| | `engine.plan(invocation, context)` | You need to inspect or plan exactly one registered action | `ActionPlan` | Reads one context; does not project its terminal qpos or expected task effect for another action | -| `engine.compile(invocations, context)` | All goals for an ordered static sequence are known before execution | `CompiledTrajectory` | Plans in order and propagates hypothetical qpos and expected effects through `projected_context`; never observes execution | +| `engine.compile(invocations, context)` | All goals are known and every action provides an inspectable joint trajectory | `CompiledTrajectory` | Plans in order and propagates hypothetical qpos and expected effects through `projected_context`; never observes execution | | `engine.start(invocations, context)` | Commands must be issued incrementally from fresh observations with bounded recovery | `ExecutionSession` | `tick(latest_context)` consumes measured state, emits at most one command, requests effect verification, and can replan | The short selection rule is: ```text one action to inspect or plan -> plan -one or more actions in a fixed scene -> compile +joint-trajectory actions in a fixed scene -> compile observed execution and error recovery -> start, then tick ``` @@ -158,6 +169,11 @@ observe a new `PlanningContext`, and plan or compile the next stage. Use `start()` when that observe/replan loop should be managed continuously by an `ExecutionSession`. +`compile()` is intentionally an offline **joint-trajectory** projection API. It +rejects an `ActionPlan` whose optional `joint_trajectory` is absent. Generic +non-joint command plans remain valid for `plan()` and `start()`; composing their +hypothetical state requires a future endpoint-specific projection contract. + ## Core contracts The public contracts separate values with different owners and lifetimes. This @@ -167,15 +183,17 @@ from leaking into an Action Agent schema. | Contract | Contains | Does not contain | |---|---|---| | `ActionGoal` | Action-specific desired outcome, such as an EEF pose or object pose | Arm names, planner instances, recovery counters | -| `ActionBinding` | Semantic-role mappings to keys from the engine robot's `control_parts`, such as `primary -> left_arm` and `primary -> left_hand` | Link/TCP names, arbitrary scene objects, motion settings, or task geometry | +| `SkillBindingContract` | Skill-local participant slots, required endpoint capabilities and commands, and disjointness constraints | Concrete robot resources, controller handles, or transport configuration | +| `ActionBinding` / `EndpointBinding` | Engine-owned endpoint snapshots keyed by `(slot_id, endpoint_id)`, including capabilities, semantic commands, claims, and an immutable runtime target | Live controllers, planner settings, task geometry, or caller-owned mutable mappings | | `ActionOptions` / built-in `*Options` | Frozen invocation-varying skill behavior: segment counts, offsets, grasp-selection rules | Robot resource names, hand qpos, planner backend | -| `ControlPartCommandProfile` | Embodiment-specific semantic commands such as `open`, `grasp`, and `ready`, keyed by actual control-part name | Action roles, task goals, recovery state | -| `ActionControlOverrides` | Optional role-scoped command replacements for one invocation revision | Persistent robot configuration | +| `ControlPartCommandProfile` | Embodiment-specific semantic commands such as `open`, `grasp`, and `ready`, keyed by actual control-part name | Skill slots/endpoints, task goals, recovery state | +| `ActionControlOverrides` | Optional `(slot, endpoint)`-scoped command replacements for one invocation revision | Persistent robot configuration | | `MotionPolicy` | Motion strategy, sample count, timing, limits, dynamic-collision mode, typed planner options | Skill semantics or robot-resource names | | `RecoveryPolicy` | Action replan/retry budgets, tracking and dynamic-goal thresholds, action-attempt timeout | Controller state or mutable counters | | `ExecutionRunnerCfg` | Runner-level acknowledgement deadlines, minimum feedback cadence, and completion hold policy | Skill behavior, planning resources, or invocation revision data | | `PlanningContext` | Measured `RobotObservation`, verified `TaskState`, versioned `SceneSnapshot`, stable environment IDs | Hypothetical simulator mutation | -| `ActionPlan` | Per-environment result, one scene-bound timed trajectory, named segments, action-level recovery metadata, diagnostics, expected `StateDelta` | Proof that a grasp/release/contact physically succeeded; independently recoverable segment boundaries | +| `ActionPlan` | Per-environment result, `TimedCommandSequence`, optional joint trajectory, named segments, action-level recovery metadata, diagnostics, expected `StateDelta` | Proof that a grasp/release/contact physically succeeded; independently recoverable segment boundaries | +| `RuntimeCommandFrame` | Synchronized endpoint commands, active rows, stable environment IDs, and per-row hold duration | Live transport or controller objects | `MotionPolicy.strategy` accepts exactly `"motion_gen"` or `"ik_interp"`; the same value is forwarded to `MotionGenOptions.strategy` without an adapter layer. @@ -184,69 +202,78 @@ Goals follow the structural `ActionGoal` protocol: each action owns one or more frozen dataclasses with a stable `goal_kind`. There is no shared `ActionTarget` base class and no closed union that must change whenever a skill is added. -### Semantic resource binding +### Skill contracts and endpoint binding The canonical semantic path uses a {doc}`RobotSkillProfile ` to match skill-local slots and endpoint capabilities against a generic robot resource graph. It validates participant pairing, typed commands, physical claims, complete defaults, and -policy presets before lowering the selected endpoints to the current core -binding. The `ActionBinding` description below is the resulting direct-core -contract and remains available for advanced manual callers. - -A **role** is an action-owned semantic participant slot: it describes the job a -robot resource performs in that action, not the identity of the resource. Each -`AtomicAction` declares its required slots through `manipulator_roles` and -`end_effector_roles`; the same declarations are exposed through its -`SkillDescriptor` so an Agent adapter or manual caller can construct a complete -binding before planning. - -Role names are local to both the skill and the resource category. For example, -`primary` in `manipulators` and `primary` in `end_effectors` are two separate -slots. Using the same role name expresses that the selected arm and hand/tool -serve the same functional participant in the action: +policy presets before producing the engine-owned `ActionBinding` used by an +invocation. + +Each `AtomicAction` declares one explicit `SkillBindingContract`. A **slot** is +an action-local participant such as `primary`, `source`, or `destination`. Each +slot contains one or more named endpoint requirements. An endpoint name is also +local to the skill contract: current manipulation skills use `motion` and +`grasp`, while a future navigation or whole-body skill can declare different +names and open, namespaced capabilities. There are no global `manipulator`, +`end_effector`, `base`, or `whole_body` fields to extend. + +For example, `PickUp` requires `primary.motion` with its motion capabilities and +`primary.grasp` with the `interaction.grasp` capability plus typed `open` and +`grasp` commands. Its contract also requires those two endpoint views to have +disjoint physical claims. A profile can satisfy that contract with a composite +participant resource whose endpoints resolve to an arm and hand. Another skill +may deliberately permit overlapping views of one coupled whole-body +controller. + +The canonical path resolves the skill through a bound profile: + +```python +resolved = engine.skill_profile.resolve( + "pick_up", + selections={"primary": "left_participant"}, +) +binding = resolved.action_binding +``` + +Advanced direct-core code can select joint-backed endpoints by actual +`Robot.control_parts` names. Use the engine helper rather than constructing an +`ActionBinding` manually; the helper validates the installed skill's contract, +resolves joint indices and commands, and stamps the engine ownership identity: ```python -binding = ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, +binding = engine.bind_control_parts( + "pick_up", + { + "primary": { + "motion": "left_arm", + "grasp": "left_hand", + } + }, ) ``` -In this example, `primary` is the role and `left_arm` / `left_hand` are the -bound resources. `primary` does not mean left, right, the first configured -arm, or a globally preferred arm; it simply denotes the principal participant -of a single-participant skill. Changing the values can bind the same action to -another compatible arm and tool without changing its goal or implementation. - -Every bound value is the name of a control part declared by the engine-owned -robot. Both `left_arm` and `left_hand` must therefore be keys in -`robot.control_parts` (originating from `RobotCfg.control_parts`). They are not -joint names, link names, TCP frame names, or scene-object identifiers. -`end_effectors` specifically selects the actuated tool/hand control part; the -manipulator's IK/TCP frame remains part of the robot and solver configuration. -The engine validates every name and resolves its full-robot joint indices -before calling the action planner. - -For a manually constructed `ActionBinding`, the validation boundary remains -intentionally narrow: the engine verifies required roles, `control_parts` -membership, resolvable joint indices, command type, and command dimensions. A -bound `RobotSkillProfile` adds capability matching, participant endpoint -pairing, command requirements, joint-claim checks, and deterministic -disambiguation before it produces that same core value. - -Role names should describe action responsibilities rather than robot-specific -joint, link, or model names. Single-resource skills use `primary`; handover uses +The resulting `ActionBinding` is generic. Each `EndpointBinding` records its +`slot_id`, `endpoint_id`, logical `resource_id`, adapter ID, capabilities, +commands, claim tokens, and a typed `RuntimeEndpointTarget`. A target contains +only immutable addressing information such as transport ID and destination ID; +the live simulator entity, hardware client, or controller belongs to the +registered transport. Profile endpoint adapters can therefore return a mobile, +whole-body, joint-position, or custom target without changing `ActionBinding`. + +Slot names describe action responsibilities rather than robot-specific joint, +link, or model names. Single-participant skills use `primary`; handover uses `source` and `destination`; coordinated placement uses `placing` and `support`. The current coordinated-pick contract uses `left` and `right` because its goal -geometry also distinguishes left/right grasps. New skills should prefer -functional roles unless a spatial distinction is intrinsic to their semantics. +geometry distinguishes left/right grasps. New skills should prefer functional +slot names unless a spatial distinction is intrinsic to their semantics. -All built-ins resolve participating arm and hand control parts from the binding. -They obtain hardware-specific `open` and `grasp` commands from the resolved -end-effector profile; no action or option duplicates arm names, hand names, or -hand qpos. Attachment state and expected effects are keyed by the bound -manipulator control-part name. +Current built-ins resolve joint-backed `motion` and `grasp` endpoints from the +binding. They obtain hardware-specific `open` and `grasp` commands from the +resolved grasp endpoint; no action or option duplicates arm names, hand names, +or hand qpos. Their attachment state and expected effects are currently keyed +by the motion endpoint's control-part target. ### Control-part semantic commands @@ -275,12 +302,13 @@ engine = AtomicActionEngine( ``` `MoveJoints(JointPositionGoal("ready"))` resolves `ready` from its bound -manipulator. Manipulation primitives resolve `open` and/or `grasp` from their -bound end effectors. A one-dimensional `JointPositionCommand` broadcasts over -the planning batch; a two-dimensional value must match the selected batch. +`primary.motion` endpoint. Manipulation primitives resolve `open` and/or +`grasp` from their bound grasp endpoints. A one-dimensional +`JointPositionCommand` broadcasts over the planning batch; a two-dimensional +value must match the selected batch. -For a one-off change, override by action role rather than by concrete robot -name: +For a one-off change, override by action-local slot and endpoint rather than by +concrete robot name: ```python invocation = ActionInvocation( @@ -288,9 +316,11 @@ invocation = ActionInvocation( goal=goal, binding=binding, control_overrides=ActionControlOverrides( - end_effectors={ + endpoints={ "primary": { - "grasp": JointPositionCommand(object_specific_grasp_qpos), + "grasp": { + "grasp": JointPositionCommand(object_specific_grasp_qpos), + } } } ), @@ -298,9 +328,9 @@ invocation = ActionInvocation( ) ``` -The engine merges the override after resolving `primary` and captures the -result in `ResolvedActionRequest`. Automatic recovery for revision 1 sees the -same command snapshot. Joint limits remain constraints; they do not define the +The engine merges the override into `primary.grasp` and captures the result in +`ResolvedActionRequest`. Automatic recovery for revision 1 sees the same +command snapshot. Joint limits remain constraints; they do not define the semantic meaning of `open` or `grasp`. Tutorials may explicitly derive a simple profile from limits, while a robot integration should normally provide calibrated commands. @@ -397,8 +427,9 @@ 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 | -| `session.revise_current(invocation)` | Runtime orchestrator or Action Agent | Replaces the active logical call with a newer revision and replans from the latest observed context | -| `runner.step(effect_success=...)` | Non-blocking controller integration | Observes and dispatches only when the next timed command is due | +| `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.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 | @@ -411,23 +442,31 @@ Use `engine.plan()` when one registered action needs to be inspected, tested, or integrated into application-owned orchestration: ```python +binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, +) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), ) plan = engine.plan(invocation, latest_context) if plan.plan_success.all(): - positions = plan.trajectory.positions + command_frames = plan.commands.frames + if plan.joint_trajectory is not None: + positions = plan.joint_trajectory.positions ``` -The result contains that action's trajectory, named segment ranges, -diagnostics, action-level recovery metadata, and uncommitted expected effects. -`plan()` does not automatically create a next context. If another action must -be planned against this action's hypothetical result, use `compile()` instead -of manually reproducing its state projection rules. +The result always contains that action's transport-neutral command sequence. A +joint-planned action may additionally retain `joint_trajectory` for feedback, +inspection, and static qpos projection. The plan also contains named segment +ranges, diagnostics, action-level recovery metadata, and uncommitted expected +effects. `plan()` does not automatically create a next context. If another +action must be planned against this action's hypothetical result, use +`compile()` instead of manually reproducing its state projection rules. `AtomicAction.build_plan()` normalizes scalar or per-environment planner success and replaces unsuccessful rows with the context's observed joint position. @@ -444,14 +483,13 @@ still replans and retries the enclosing action as one unit. ## Static compilation -`compile()` plans invocations in order. For every successful action it projects -the terminal qpos and expected task-state effect into a new context so the next -action can be checked against a hypothetical result. The observed context and -simulator remain unchanged. +`compile()` plans joint-trajectory invocations in order. For every successful +action it projects the terminal qpos and expected task-state effect into a new +context so the next action can be checked against a hypothetical result. The +observed context and simulator remain unchanged. ```python from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -459,7 +497,10 @@ from embodichain.lab.sim.atomic_actions import ( ) engine = AtomicActionEngine(motion_generator) -binding = ActionBinding(manipulators={"primary": "left_arm"}) +binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, +) motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) approach = ActionInvocation( @@ -513,7 +554,10 @@ moving_goal = ActionInvocation( minimum_confidence=0.8, ) ), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ), recovery_policy=RecoveryPolicy( max_replans=3, max_action_retries=2, @@ -529,7 +573,7 @@ session = engine.start((moving_goal,), latest_context) while session.status is ExecutionStatus.RUNNING: tick = session.tick(latest_context) if tick.command is not None: - send_joint_command(tick.command) + dispatch_runtime_frame(tick.command) latest_context = observe_context() ``` @@ -540,11 +584,12 @@ advanced direct-core provider path: ```python scene_provider = RigidObjectSceneProvider({"moving_tray": moving_tray}) adapter = SimulationExecutionAdapter(sim, robot, scene_provider=scene_provider) +router = EndpointCommandRouter((adapter,)) initial_context = adapter.observe( TaskState.empty(robot.get_qpos().shape[0], robot.device) ) session = engine.start((moving_goal,), initial_context) -runner = ExecutionRunner(session, adapter, adapter, clock=adapter) +runner = ExecutionRunner(session, adapter, router, clock=adapter) result = runner.run_until_blocked() ``` @@ -553,17 +598,32 @@ pass a `scene_supplier(timestamp)` callback instead. `scene_provider` and `scene_supplier` are mutually exclusive. `ExecutionRunner.step()` is the non-blocking entry point for an application -that already owns its event loop. It observes only when the previous command's -`hold_duration` has elapsed, dispatches active commands through `CommandSink`, -and records accepted, rejected, or timed-out acknowledgements. Cancellation, +that already owns its event loop. It observes only when the previous +`RuntimeCommandFrame.hold_duration` has elapsed, dispatches active endpoint +commands through `EndpointCommandRouter`, and records accepted, rejected, or +timed-out acknowledgements. The router preflights a whole frame, groups commands +by exact `transport_id`, and aggregates transport acknowledgements, so an +unknown or incompatible transport cannot cause partial dispatch. Cancellation, observation/session exceptions, and negative acknowledgements enter a -best-effort cancel-then-hold path. - -`TimedTrajectory.dt[:, i]` is the interval leading to sample `i`. -`ExecutionSession` maps each following arrival interval onto the preceding -command's post-dispatch hold, while the final sample reuses its own interval as -a settling window before terminal validation. A batched runner uses the longest -active row interval as its synchronized barrier. +best-effort cancel-then-hold path for every armed runtime target. + +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 +or claim tokens. Non-empty frames and recovery plans keep one stable +destination set. If a failed replan emits no frames, the session retains the +previous targets so the runner can still request a transport-owned hold. + +An inactive row is not equivalent to omitting a write: each transport must +actively neutralize inactive rows for every addressed target. The simulation +joint-position transport holds observed positions for those rows; a velocity +transport would normally send zero velocity. + +Each `RuntimeCommandFrame` carries the delay before the next frame. A batched +runner uses the longest active row duration as its synchronized barrier. The +joint-trajectory lowering helper derives these holds from trajectory arrival +intervals; non-joint planners set them directly when building their +`TimedCommandSequence`. `SimulationExecutionAdapter.sleep()` converts that interval to an integral number of physics steps instead of using wall-clock sleep. Stable `env_ids` remain correlation identifiers and are not used as simulator array indices. @@ -638,14 +698,12 @@ varies only the measured context. Mutable goal values such as tensors and metadata containers are copied, while simulator-backed `BatchEntity` handles retain their runtime identity. -Each emitted `JointCommand` carries a per-environment `hold_duration` derived -from the plan's `TimedTrajectory.dt`. The application control loop must respect -that timing after dispatching the command and before requesting the next -observation. `dt[:, i]` is the arrival interval leading to waypoint `i`, so the -first waypoint is dispatched immediately and command `i` carries `dt[:, i + 1]` -until the next waypoint is due. The final command reuses `dt[:, -1]` as a -settling window. For a synchronized batch, the caller should wait for the -longest duration among active rows. A passive hold command has zero duration. +Each emitted `RuntimeCommandFrame` carries a per-environment `hold_duration`. +The application control loop must respect that timing after dispatch and before +requesting the next observation. For a synchronized batch, the caller waits +for the longest duration among active rows. Safe stop is a separate transport +lifecycle: the runner cancels every armed target and then asks each transport to +hold that target from the latest observed context. Use an explicit newer revision when the application or Action Agent decides to change runtime behavior: @@ -662,13 +720,22 @@ revised = ActionInvocation( invocation_id=current.invocation_id, revision=current.revision + 1, ) -session.revise_current(revised) +runner.revise_current(revised) ``` `skill_id` and `invocation_id` must still identify the active logical call. Revision replacement preserves verified task state and environment eligibility, resets the new revision's local recovery counters, emits -`INVOCATION_REVISED`, and replans from the latest context. +`INVOCATION_REVISED`, and replans from the latest context. Once the current +action owns runtime destinations, the revision must declare the same non-empty +destination set and preserve every target's exact address fingerprint, including +its safe-hold footprint. Switching to a base, another arm, or another controller +is a new invocation boundary. `runner.revise_current()` stages the owned request, +keeps the current frame deadline, and plans only after collecting the next due +observation. A physical effect awaiting verification cannot be abandoned by a +revision; verify it first, or cancel and start a new invocation. Callers that +drive `ExecutionSession.tick()` directly can use `session.revise_current()` and +should pass their fresh context explicitly. ```{attention} Automatic dynamic-goal invalidation is dependency-driven. A goal must contain a @@ -685,7 +752,7 @@ changing their geometry requires rebuilding the planner world. ## Planning success versus physical success -`ActionPlan.plan_success` only means a valid trajectory was produced for an +`ActionPlan.plan_success` only means a valid command plan was produced for an environment row. Pick, place, handover, and coordinated skills also return an uncommitted `StateDelta` describing the attachment state expected after execution. @@ -700,7 +767,7 @@ if tick.pending_effect is not None: tick = session.tick(latest_context, effect_success=effect_success) ``` -This prevents a collision-free plan or well-tracked trajectory from being +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. @@ -716,7 +783,7 @@ and embodiment capabilities, then produce the typed invocation: MLLM SkillCallSpec -> schema validation -> object / scene grounding - -> capability and role binding + -> participant and endpoint capability binding -> safe skill-option selection -> semantic command selection (never raw qpos) -> ActionInvocation @@ -740,15 +807,17 @@ A new primitive should: 1. define a frozen, action-owned goal dataclass with a stable `goal_kind`; 2. define a frozen `ActionOptions` subclass only for behavior that can vary per invocation; -3. declare `skill_id`, `GoalType`, `OptionsType`, required semantic roles, and - agent visibility; +3. declare `skill_id`, `GoalType`, `OptionsType`, an explicit + `SkillBindingContract`, and agent visibility; 4. put reusable embodiment commands on control-part profiles and generic motion/recovery choices in invocation policies; 5. implement side-effect-free `_plan(request, context)` using the engine-owned planning services; do not override the framework-owned public `plan()`—the class definition is rejected if it does; -6. return full-robot timed motion, per-environment planning success, optional - named segment metadata, diagnostics, and uncommitted effects; +6. return a `TimedCommandSequence`, per-environment planning success, optional + joint-trajectory and named-segment metadata, diagnostics, and uncommitted + effects; joint planners can use `build_plan()`, while other endpoint types + use `build_command_plan()`; 7. add registration coverage, contract tests, execution/recovery tests, a runnable example, and documentation. 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 b5af6a58b..f03d453e2 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -27,9 +27,6 @@ An atomic action owns a - a {class}`~embodichain.lab.sim.atomic_actions.SkillEndpointRequirement` declares the all-of capabilities and typed semantic commands needed from that participant; -- an optional - {class}`~embodichain.lab.sim.atomic_actions.ActionBindingRoute` lowers a - generic endpoint into the current atomic-action core; and - {class}`~embodichain.lab.sim.atomic_actions.DisjointSlotEndpoints` declares endpoint views that must not share physical channels within one participant; coupled whole-body views may overlap when the skill does not declare this @@ -184,7 +181,8 @@ preset = bound.preset(skill_id="pick_up") {meth}`BoundRobotSkillProfile.resolve` returns a {class}`ResolvedSkillBinding` containing the selected logical resources, their adapter-resolved endpoints, -their combined {class}`ResourceClaim`, and the current-core `ActionBinding`. A +their combined {class}`ResourceClaim`, and an engine-owned generic +{class}`~embodichain.lab.sim.atomic_actions.ActionBinding`. A semantic compiler uses that binding and the selected preset when constructing an invocation; profile resolution does not plan or execute the action itself. @@ -248,13 +246,15 @@ and capability in its own binding contract. Existing built-in actions do not consume these example capabilities. Non-joint controllers add one endpoint declaration type and one adapter. The -adapter returns {class}`EndpointResolution` with a command-profile key, -supported binding values, joint IDs when applicable, and adapter-defined claim +adapter returns {class}`EndpointResolution` with a typed immutable +{class}`~embodichain.lab.sim.atomic_actions.RuntimeEndpointTarget`, an optional +command-profile key, joint IDs when applicable, and adapter-defined claim tokens. The generic graph, matching, command, default, and conflict code does -not change. For example, a twist controller can return -`claim_tokens={"controller:base"}` with no joint IDs. Exclusive endpoints must -provide joint IDs or claim tokens; a read-only or otherwise shareable virtual -endpoint must opt into `exclusive=False` explicitly. +not change. For example, a twist controller can return a target addressed to a +`base_velocity` transport and `claim_tokens={"controller:base"}` with no joint +IDs. Exclusive endpoints must provide joint IDs or claim tokens; a read-only or +otherwise shareable virtual endpoint must opt into `exclusive=False` +explicitly. Adapters are registered by exact endpoint type. The built-in {class}`ControlPartEndpointAdapter` cannot be overridden; define a distinct @@ -262,19 +262,25 @@ 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. -{class}`ActionBindingRoute` remains a transition into the current core's -`manipulator` and `end_effector` maps. A new non-core controller therefore also -needs one reusable atomic skill/runtime integration for its route and command -transport. Once that shared capability exists, new tasks and robot variants -reuse it through profile and task configuration rather than task-specific -motion code. +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 +shared atomic skill that emits +{class}`~embodichain.lab.sim.atomic_actions.RuntimeCommandFrame` values, and an +{class}`~embodichain.lab.sim.atomic_actions.EndpointCommandTransport` registered +with {class}`~embodichain.lab.sim.atomic_actions.EndpointCommandRouter`. The +core binding, session, runner, and router do not need controller-specific +changes. Once that shared capability exists, new tasks and robot variants reuse +it through profile and task configuration rather than task-specific motion +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 -current action plans and commands still contain full-robot joint positions, and -the runtime does not merge concurrent command streams. +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. ``` See {doc}`index` for the direct atomic-action core and diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 46f81a635..6f7de6cb2 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -14,11 +14,13 @@ demonstrations of every built-in skill, see :doc:`/overview/sim/atomic_actions/builtin_actions`. Canonical scene identity and snapshot/provider setup are documented in :doc:`/overview/sim/scene_registry`. -The contracts deliberately separate six concerns: +The contracts deliberately separate seven concerns: * a **goal** describes what should happen; -* an **ActionBinding** maps semantic roles such as ``primary`` or ``source`` to - names declared in the engine robot's ``control_parts`` mapping; +* a **SkillBindingContract** declares action-local participant slots, endpoint + capabilities, typed commands, and physical disjointness; +* an engine-owned **ActionBinding** contains adapter-resolved + **EndpointBinding** snapshots and immutable runtime targets for one call; * a **ControlPartCommandProfile** maps embodiment-specific meanings such as ``open``, ``grasp``, or ``ready`` to typed commands; * typed **ActionOptions** contain behavior that may vary for one skill call; @@ -27,19 +29,32 @@ The contracts deliberately separate six concerns: * a **PlanningContext** contains measured robot state, verified task state, and a versioned scene snapshot. -Binding values are keys from ``RobotCfg.control_parts``. They are not joint, -link, TCP-frame, or scene-object names. The engine validates them and resolves -their full-robot joint indices before planning. The ``end_effectors`` map names -an actuated hand/tool control part rather than an IK end frame. +Slots such as ``primary`` or ``source`` name participants only within one skill. +Each slot exposes skill-local endpoint protocols such as ``motion`` and +``grasp``. There are no global arm, hand, mobile-base, or whole-body binding +fields. A profile matches endpoint capabilities to generic robot resources and +uses an endpoint adapter to create the runtime target. -A role is an action-defined semantic participant slot, not a control part. In -``{"primary": "left_arm"}``, ``primary`` means the principal participant of -that single-participant action, while ``left_arm`` is the concrete control-part -key. It has no inherent left/right or default-arm meaning. Actions publish their -required slots through ``manipulator_roles`` and ``end_effector_roles``. When a -role such as ``primary`` occurs in both maps, the entries select the arm and -hand/tool serving the same functional participant, but the caller is still -responsible for choosing a physically compatible pair. +For advanced direct-core use, joint-backed endpoint selections are concrete +``RobotCfg.control_parts`` keys, not joint, link, TCP-frame, or scene-object +names. Build them through :meth:`~embodichain.lab.sim.atomic_actions.AtomicActionEngine.bind_control_parts`: + +.. code-block:: python + + binding = engine.bind_control_parts( + "pick_up", + { + "primary": { + "motion": "left_arm", + "grasp": "left_hand", + } + }, + ) + +The helper validates the installed skill contract, resolves joint indices and +commands, and returns the engine-owned generic binding. Profile endpoint +adapters may instead resolve locomotion, whole-body, or custom controller +targets without changing ``ActionBinding``. The engine exclusively owns the ``MotionGenerator``, shared trajectory builder, and control-part profiles. It creates and binds all built-in actions by default; @@ -66,7 +81,7 @@ Application code normally uses one of three engine entry points: - ``ActionPlan`` - Reads one context and does not project a next context * - ``engine.compile()`` - - Planning a fixed sequence whose goals are already known + - Planning a fixed sequence whose goals are known and whose plans retain joint trajectories - ``CompiledTrajectory`` - Propagates hypothetical qpos and expected effects, without observing execution * - ``engine.start()`` @@ -74,10 +89,11 @@ Application code normally uses one of three engine entry points: - ``ExecutionSession`` - ``tick()`` consumes measured context, emits commands, requests effect verification, and can replan -As a short rule: use ``plan`` for one action, ``compile`` for a static action -sequence, and ``start`` followed by ``tick`` for observed execution and error -recovery. None of these APIs steps the simulator directly. The application -sends commands returned by an execution session and supplies new observations. +As a short rule: use ``plan`` for one action, ``compile`` for a static +joint-trajectory sequence, and ``start`` followed by ``tick`` for observed +execution and error recovery. None of these APIs steps the simulator directly. +The application sends commands returned by an execution session and supplies +new observations. ``AtomicAction.plan(request, context)`` is different from ``engine.plan()``. It is the framework-owned template method called by the engine, not an @@ -152,9 +168,10 @@ engine is built: ) ``PickUp``, ``Place``, and the other manipulation skills resolve ``open`` and -``grasp`` from their bound end effector. ``MoveJoints`` resolves a string target -from its bound manipulator. Joint limits validate possible commands, but do not -define their semantic meaning; supply calibrated robot commands in production. +``grasp`` from their bound grasp endpoints. ``MoveJoints`` resolves a string +target from ``primary.motion``. Joint limits validate possible commands, but do +not define their semantic meaning; supply calibrated robot commands in +production. Planning one action ------------------- @@ -166,22 +183,27 @@ application-owned orchestration: .. code-block:: python from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, ) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), ) plan = engine.plan(invocation, latest_context) if plan.plan_success.all(): - trajectory = plan.trajectory.positions + command_frames = plan.commands.frames + if plan.joint_trajectory is not None: + trajectory = plan.joint_trajectory.positions diagnostics = plan.diagnostics segments = plan.segments @@ -190,22 +212,24 @@ sequence, call ``compiled.segment(action_index, name)`` to get the corresponding range in concatenated-trajectory coordinates. This is preferable to repeating a primitive's private sample-split formula in application or tutorial code. -The returned :class:`~embodichain.lab.sim.atomic_actions.ActionPlan` describes -only that invocation. Its expected effects are not committed, and ``plan`` does -not produce a projected context for a following action. Use ``compile`` when -the engine should propagate hypothetical state through a sequence. +The returned :class:`~embodichain.lab.sim.atomic_actions.ActionPlan` always owns +a transport-neutral ``commands`` sequence. Joint-planned actions may also retain +``joint_trajectory`` for feedback, inspection, and static qpos projection. The +plan describes only that invocation: expected effects are not committed, and +``plan`` does not produce a projected context for a following action. Use +``compile`` when the engine should propagate hypothetical state through a +sequence. Static compilation ------------------ Use :meth:`~embodichain.lab.sim.atomic_actions.AtomicActionEngine.compile` when -the scene is treated as fixed and all goals in a sequence are known during -planning: +the scene is treated as fixed, all goals in a sequence are known during +planning, and every action retains ``joint_trajectory``: .. code-block:: python from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -213,7 +237,10 @@ planning: ) engine = AtomicActionEngine(motion_generator) - binding = ActionBinding(manipulators={"primary": "left_arm"}) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ) motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) approach = ActionInvocation( @@ -242,6 +269,10 @@ planning: state. Calling it with one invocation is valid, but ``plan`` is simpler when a projected context and sequence-shaped result are unnecessary. +This is intentionally an offline joint-trajectory projection API. It rejects a +generic command plan without ``joint_trajectory``; such plans remain valid for +``plan`` and closed-loop ``start``/``tick`` execution. + Do not compile across a point where later targets depend on physical execution. The coordinated-placement tutorial, for example, compiles both pick-ups, executes them, rebuilds held-object state from measured poses, and then compiles @@ -267,7 +298,10 @@ must be resolved from the latest scene snapshot: goal=EndEffectorPoseGoal( xpos=SceneEntityPose("moving_tray", relative_pose=tray_to_tcp) ), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ), recovery_policy=RecoveryPolicy( max_replans=3, tracking_error_threshold=0.05, @@ -276,6 +310,7 @@ must be resolved from the latest scene snapshot: ) from embodichain.lab.sim.atomic_actions import ( + EndpointCommandRouter, ExecutionRunner, SimulationExecutionAdapter, TaskState, @@ -298,7 +333,8 @@ 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) - runner = ExecutionRunner(session, adapter, adapter, clock=adapter) + router = EndpointCommandRouter((adapter,)) + runner = ExecutionRunner(session, adapter, router, clock=adapter) result = runner.run_until_blocked() For a lightweight scene source that does not need environment correlation IDs, @@ -306,10 +342,15 @@ pass a ``scene_supplier(timestamp)`` callback instead. ``scene_provider`` and ``scene_supplier`` are mutually exclusive. The session owns planning progress and bounded recovery. The runner owns the -outer lifecycle: it requests fresh observations, schedules each command from -the :class:`~embodichain.lab.sim.atomic_actions.TimedTrajectory` time deltas, -checks controller acknowledgements, and performs cancel-then-hold on failure. -The simulation adapter advances physics instead of sleeping in wall-clock time. +outer lifecycle: it requests fresh observations, schedules each +:class:`~embodichain.lab.sim.atomic_actions.RuntimeCommandFrame` from its +``hold_duration``, checks controller acknowledgements, and performs +cancel-then-hold on failure. ``EndpointCommandRouter`` preflights the whole +frame, groups endpoint commands by exact transport ID, and aggregates their +acknowledgements. Unknown or incompatible transports are rejected before any +partial dispatch. Safe stop cancels every armed runtime target, then asks its +transport to hold from the latest observed context. The simulation adapter +advances physics instead of sleeping in wall-clock time. ``ExecutionRunnerCfg`` contains runner-level transport and scheduling settings; it is not an atomic-action option and is not replaced by invocation revision. @@ -379,11 +420,24 @@ control command while the action is active, submit a strictly newer revision: invocation_id=invocation.invocation_id, revision=invocation.revision + 1, ) - session.revise_current(revised) + runner.revise_current(revised) The session replans from its latest context and emits an ``invocation_revised`` event. ``skill_id`` and ``invocation_id`` must still -identify the active logical call. +identify the active logical call, and the replacement must preserve the +current non-empty runtime destination set and exact target address fingerprints. +Use a new invocation when changing from an arm endpoint to a base, whole-body +controller, or another controller. The runner keeps the current frame deadline, +then observes fresh state and installs the revision at that due boundary. It +rejects revision while a physical effect is awaiting verification; verify the +effect first, or cancel and start a new invocation. A manually ticked session +can call ``session.revise_current(revised, context=fresh_context)`` directly. + +Every emitted command is authorized against the binding-owned target and +physical claims. Non-empty plan frames and recovery replans keep a stable +destination set. Transports must actively neutralize inactive batch rows for +every addressed target; simply skipping those rows can leave a persistent +controller command active. Entities referenced through ``SceneEntityPose`` become automatic scene-motion dependencies. Object-centric skills may additionally declare an explicit @@ -437,6 +491,21 @@ A minimal implementation looks like: from dataclasses import dataclass from typing import ClassVar + import torch + + from embodichain.lab.sim.atomic_actions import ( + CARTESIAN_POSE_CAPABILITY, + ActionOptions, + ActionPlan, + AtomicAction, + JointPositionTarget, + PlanningContext, + ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + ) + @dataclass(frozen=True, slots=True) class PushGoal: goal_kind: ClassVar[str] = "push" @@ -450,7 +519,21 @@ A minimal implementation looks like: skill_id: ClassVar[str] = "push" GoalType: ClassVar[type] = PushGoal OptionsType: ClassVar[type] = PushOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + {CARTESIAN_POSE_CAPABILITY} + ), + ), + ), + ), + ), + ) def __init__(self, default_options: PushOptions | None = None) -> None: super().__init__(default_options) @@ -462,8 +545,11 @@ A minimal implementation looks like: ) -> ActionPlan: goal = self.require_goal(request) options = request.skill_options - # Resolve the bound resource, plan from context.robot.qpos, and - # return a full-robot TimedTrajectory or position tensor. + motion = request.binding.endpoint("primary", "motion") + motion_target = motion.require_target(JointPositionTarget) + # Plan from context.robot.qpos using motion_target.joint_ids. + # The joint helper lowers the result into RuntimeCommandFrame values + # and retains the trajectory for joint-position feedback. return self.build_plan( request, context, @@ -471,6 +557,13 @@ A minimal implementation looks like: trajectory=full_robot_positions, ) +For a non-joint endpoint, define a typed ``RuntimeEndpointTarget`` and matching +``RuntimeCommandPayload``, have the profile endpoint adapter produce that +target, and call ``build_command_plan(commands=TimedCommandSequence(...))``. +Register the matching ``EndpointCommandTransport`` with the runner's router. +The skill contract, resource graph, binding, runner, and recovery model do not +gain controller-specific fields. + Do not step simulation, mutate ``PlanningContext``, commit ``StateDelta``, or expose planner-specific configuration through the goal. See the in-repository ``add-atomic-action`` skill for the complete checklist. diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index a5d7ea729..607470da6 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -34,7 +34,12 @@ AssembleAffordance, InteractionPoints, ) -from .bindings import ActionBinding, ResolvedActionBinding, ResolvedControlPart +from .bindings import ( + ActionBinding, + EndpointBinding, + JointPositionTarget, + RuntimeEndpointTarget, +) from .control import ( ActionControlOverrides, ControlCommand, @@ -58,20 +63,19 @@ ExecutionSession, ExecutionStatus, ExecutionTick, - JointCommand, ) from .goals import ActionGoal, ObjectActionGoal, PoseGoalValue, SceneEntityPose from .invocation import ActionInvocation, ActionOptions, ResolvedActionRequest from .plans import ( ActionPlan, CompiledTrajectory, + ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, ) from .policies import DynamicCollisionMode, MotionPolicy, RecoveryPolicy from .requirements import ( - ActionBindingRoute, BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, @@ -85,6 +89,14 @@ SkillResourceSlot, ) from .runtime import ActionPlanningServices +from .runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + RuntimeCommandPayload, + TimedCommandSequence, +) +from .transports import EndpointCommandRouter, EndpointCommandTransport from .primitives import ( AssembleGoal, BUILTIN_ACTION_TYPES, @@ -150,7 +162,6 @@ __all__ = [ "ActionBinding", - "ActionBindingRoute", "ActionControlOverrides", "ActionGoal", "ActionInvocation", @@ -185,10 +196,15 @@ "DisjointResourceSlots", "DisjointSlotEndpoints", "EndEffectorPoseGoal", + "EndpointBinding", + "EndpointCommand", + "EndpointCommandRouter", + "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", "EffectVerifier", "ExecutionClock", + "ExecutionFeedbackMode", "ExecutionEvent", "ExecutionEventKind", "ExecutionRunner", @@ -207,8 +223,9 @@ "INVERSE_KINEMATICS_CAPABILITY", "InteractionPoints", "JointPositionGoal", - "JointCommand", "JointPositionCommand", + "JointPositionPayload", + "JointPositionTarget", "JOINT_POSITION_CAPABILITY", "MotionPolicy", "MonotonicExecutionClock", @@ -237,9 +254,10 @@ "RigidObjectSceneProvider", "RigidObjectSceneProviderCfg", "ResolvedActionRequest", - "ResolvedActionBinding", - "ResolvedControlPart", "RobotObservation", + "RuntimeCommandFrame", + "RuntimeCommandPayload", + "RuntimeEndpointTarget", "RunnerStatus", "RunnerStep", "RunnerStepCallback", @@ -254,6 +272,7 @@ "StateDelta", "SimulationExecutionAdapter", "TaskState", + "TimedCommandSequence", "TimedTrajectory", "TrajectorySegment", "get_registered_actions", diff --git a/embodichain/lab/sim/atomic_actions/bindings.py b/embodichain/lab/sim/atomic_actions/bindings.py index 5257c5035..d56713580 100644 --- a/embodichain/lab/sim/atomic_actions/bindings.py +++ b/embodichain/lab/sim/atomic_actions/bindings.py @@ -14,191 +14,295 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Semantic-role to robot control-part bindings for atomic actions.""" +"""Generic runtime endpoint bindings consumed by atomic actions.""" from __future__ import annotations +from abc import ABC, abstractmethod +from collections.abc import Hashable +from copy import deepcopy from dataclasses import dataclass, field from types import MappingProxyType -from typing import Mapping +from typing import Mapping, TypeVar import torch -from .control import ControlCommand, JointPositionCommand +from .control import ControlCommand -def _normalize_resource_map( - values: Mapping[str, str], +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate and return one strict identifier.""" + if not isinstance(value, 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 _normalize_identifiers( + values: frozenset[str], *, field_name: str, -) -> Mapping[str, str]: - """Validate and freeze a semantic-role resource mapping.""" +) -> frozenset[str]: + """Validate and freeze an identifier set.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must be an iterable of strings.") + try: + normalized = frozenset(values) + except TypeError as exc: + raise TypeError(f"{field_name} must be an iterable of strings.") from exc + for value in normalized: + _validate_identifier(value, field_name=field_name) + return normalized + + +def _snapshot_commands( + values: Mapping[str, ControlCommand], +) -> Mapping[str, ControlCommand]: + """Validate semantic endpoint commands and own their snapshots.""" if not isinstance(values, Mapping): - raise TypeError(f"{field_name} must be a mapping.") - normalized: dict[str, str] = {} - for role, resource in values.items(): - if not isinstance(role, str) or not role.strip(): - raise ValueError(f"{field_name} roles must be non-empty strings.") - if not isinstance(resource, str) or not resource.strip(): - raise ValueError(f"{field_name} resources must be non-empty strings.") - normalized[role] = resource - return MappingProxyType(normalized) + raise TypeError("EndpointBinding.commands must be a mapping.") + commands: dict[str, ControlCommand] = {} + for name, command in values.items(): + _validate_identifier(name, field_name="EndpointBinding command names") + if not isinstance(command, ControlCommand): + raise TypeError( + "EndpointBinding.commands values must be ControlCommand instances." + ) + snapshot = command.snapshot() + if type(snapshot) is not type(command) or snapshot is command: + raise TypeError( + "ControlCommand.snapshot() must return an independently owned " + "value of the same command type." + ) + commands[name] = snapshot + return MappingProxyType(commands) -@dataclass(frozen=True, slots=True) -class ActionBinding: - """Bind semantic action roles to names from ``Robot.control_parts``. - - A role such as ``primary``, ``source`` or ``destination`` is an - action-defined semantic participant slot. It describes the responsibility - a resource has within that action and is not itself a robot resource. - Actions publish their required slots through ``manipulator_roles`` and - ``end_effector_roles``. Role names are scoped independently to those two - maps, so matching names associate an arm and hand/tool with the same - functional participant without making the maps interchangeable. - - ``primary`` has no inherent left/right, ordering, or default-control-part - meaning. Only the compiler or application binding layer needs to map it to - concrete robot control-part names such as ``left_arm`` and ``left_hand``. - - Every mapping value is a key from the current robot's ``control_parts`` - configuration. This value object validates the mapping shape; the - :class:`~embodichain.lab.sim.atomic_actions.AtomicActionEngine` validates - the names against its owned robot before planning. ``end_effectors`` refers - to actuated tool/hand control parts, not TCP or kinematic frame names. +def _validate_target_fingerprint( + target: RuntimeEndpointTarget, + *, + field_name: str, +) -> Hashable: + """Return one hashable, snapshot-stable target address fingerprint.""" + fingerprint = target.address_fingerprint + try: + hash(fingerprint) + except TypeError as exc: + raise TypeError(f"{field_name} must be hashable.") from exc + return fingerprint + + +class RuntimeEndpointTarget(ABC): + """Stable controller destination produced by an endpoint adapter. + + Targets contain immutable addressing data only. Live controllers, sockets, + simulator entities, and other process-owned handles belong to an + endpoint-command transport rather than this value. """ - manipulators: Mapping[str, str] = field(default_factory=dict) - """Manipulator control-part names keyed by semantic role.""" - - end_effectors: Mapping[str, str] = field(default_factory=dict) - """Tool or hand control-part names keyed by semantic role.""" + @property + @abstractmethod + def transport_id(self) -> str: + """Return the registered transport kind used by this target.""" - def __post_init__(self) -> None: - object.__setattr__( - self, - "manipulators", - _normalize_resource_map(self.manipulators, field_name="manipulators"), - ) - object.__setattr__( - self, - "end_effectors", - _normalize_resource_map(self.end_effectors, field_name="end_effectors"), - ) + @property + @abstractmethod + def target_id(self) -> str: + """Return the destination identifier within its transport.""" - def manipulator(self, role: str = "primary") -> str: - """Return the manipulator control-part name bound to ``role``. + @property + def address_fingerprint(self) -> Hashable: + """Return the stable controller-address and safe-hold fingerprint. + + The default covers the exact target type and transport-scoped + destination. Target types whose hold footprint depends on additional + immutable addressing fields must override this property and include + those fields. Replans and explicit revisions may replace payloads, but + they may not change this fingerprint in place. + """ + return type(self), self.transport_id, self.target_id - Args: - role: Semantic manipulator role. + def snapshot(self) -> RuntimeEndpointTarget: + """Return an independently owned target snapshot.""" + return deepcopy(self) - Returns: - Key from the current robot's ``control_parts`` mapping. - Raises: - KeyError: If the requested role is not bound. - """ - try: - return self.manipulators[role] - except KeyError as exc: - raise KeyError(f"No manipulator is bound to role {role!r}.") from exc +@dataclass(frozen=True, slots=True) +class JointPositionTarget(RuntimeEndpointTarget): + """Joint-position destination backed by one robot control part.""" - def end_effector(self, role: str = "primary") -> str: - """Return the tool/hand control-part name bound to ``role``. + TRANSPORT_ID = "robot.joint_position" - Args: - role: Semantic end-effector role. + control_part: str + joint_ids: tuple[int, ...] - Returns: - Key from the current robot's ``control_parts`` mapping. + def __post_init__(self) -> None: + _validate_identifier( + self.control_part, + field_name="JointPositionTarget.control_part", + ) + joint_ids = tuple(self.joint_ids) + if not joint_ids or not all( + isinstance(joint_id, int) + and not isinstance(joint_id, bool) + and joint_id >= 0 + for joint_id in joint_ids + ): + raise ValueError( + "JointPositionTarget.joint_ids must contain non-negative integers." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ValueError("JointPositionTarget.joint_ids must be unique.") + object.__setattr__(self, "joint_ids", joint_ids) - Raises: - KeyError: If the requested role is not bound. - """ - try: - return self.end_effectors[role] - except KeyError as exc: - raise KeyError(f"No end effector is bound to role {role!r}.") from exc + @property + def transport_id(self) -> str: + """Return the built-in joint-position transport identifier.""" + return self.TRANSPORT_ID + @property + def target_id(self) -> str: + """Return the robot control-part destination.""" + return self.control_part -@dataclass(frozen=True, slots=True) -class ResolvedControlPart: - """One engine-validated robot control part. + @property + def address_fingerprint(self) -> Hashable: + """Return the destination plus the joints that must remain holdable.""" + return ( + type(self), + self.transport_id, + self.target_id, + self.joint_ids, + ) - Instances are produced by engine-owned planning services. They keep - robot-specific indices out of :class:`ActionBinding` and agent-facing - invocation schemas. - """ - name: str - """Key from ``Robot.control_parts``.""" +TargetT = TypeVar("TargetT", bound=RuntimeEndpointTarget) - joint_ids: tuple[int, ...] - """Full-robot joint indices belonging to this control part.""" +@dataclass(frozen=True, slots=True) +class EndpointBinding: + """One action-local endpoint resolved to a runtime controller target.""" + + slot_id: str + endpoint_id: str + resource_id: str + adapter_id: str + target: RuntimeEndpointTarget + capabilities: frozenset[str] = frozenset() commands: Mapping[str, ControlCommand] = field(default_factory=dict) - """Engine-profile commands, including invocation-level overrides.""" + claim_tokens: frozenset[str] = frozenset() + joint_ids: tuple[int, ...] = () def __post_init__(self) -> None: - if not isinstance(self.name, str) or not self.name.strip(): - raise ValueError("ResolvedControlPart.name must be a non-empty string.") + _validate_identifier(self.slot_id, field_name="EndpointBinding.slot_id") + _validate_identifier( + self.endpoint_id, + field_name="EndpointBinding.endpoint_id", + ) + _validate_identifier( + self.resource_id, + field_name="EndpointBinding.resource_id", + ) + _validate_identifier(self.adapter_id, field_name="EndpointBinding.adapter_id") + if not isinstance(self.target, RuntimeEndpointTarget): + raise TypeError("EndpointBinding.target must be a RuntimeEndpointTarget.") + target = self.target.snapshot() + if type(target) is not type(self.target) or target is self.target: + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently " + "owned value of the same target type." + ) + _validate_identifier( + target.transport_id, + field_name="RuntimeEndpointTarget.transport_id", + ) + _validate_identifier( + target.target_id, + field_name="RuntimeEndpointTarget.target_id", + ) + source_fingerprint = _validate_target_fingerprint( + self.target, + field_name="RuntimeEndpointTarget.address_fingerprint", + ) + target_fingerprint = _validate_target_fingerprint( + target, + field_name="RuntimeEndpointTarget.snapshot().address_fingerprint", + ) + if target_fingerprint != source_fingerprint: + raise ValueError( + "RuntimeEndpointTarget.snapshot() must preserve its address " + "fingerprint." + ) + object.__setattr__(self, "target", target) + object.__setattr__( + self, + "capabilities", + _normalize_identifiers( + self.capabilities, + field_name="EndpointBinding.capabilities", + ), + ) + object.__setattr__(self, "commands", _snapshot_commands(self.commands)) + object.__setattr__( + self, + "claim_tokens", + _normalize_identifiers( + self.claim_tokens, + field_name="EndpointBinding.claim_tokens", + ), + ) joint_ids = tuple(self.joint_ids) - if not joint_ids or not all( - isinstance(joint_id, int) and joint_id >= 0 for joint_id in joint_ids + 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( - "ResolvedControlPart.joint_ids must contain non-negative integers." + "EndpointBinding.joint_ids must contain non-negative integers." ) if len(set(joint_ids)) != len(joint_ids): - raise ValueError("ResolvedControlPart.joint_ids must be unique.") - object.__setattr__(self, "joint_ids", joint_ids) - if not isinstance(self.commands, Mapping): - raise TypeError("ResolvedControlPart.commands must be a mapping.") - commands: dict[str, ControlCommand] = {} - for name, command in self.commands.items(): - if not isinstance(name, str) or not name.strip(): - raise ValueError("Control command names must be non-empty strings.") - if not isinstance(command, ControlCommand): - raise TypeError( - "ResolvedControlPart.commands values must be ControlCommand " - "instances." + raise ValueError("EndpointBinding.joint_ids must be unique.") + if isinstance(target, JointPositionTarget): + if joint_ids and joint_ids != target.joint_ids: + raise ValueError( + "EndpointBinding.joint_ids must match its JointPositionTarget." ) - commands[name] = command.snapshot() - object.__setattr__(self, "commands", MappingProxyType(commands)) + joint_ids = target.joint_ids + object.__setattr__(self, "joint_ids", joint_ids) @property - def dof(self) -> int: - """Return the number of joints in this control part.""" - return len(self.joint_ids) + def key(self) -> tuple[str, str]: + """Return the action-local ``(slot, endpoint)`` key.""" + return self.slot_id, self.endpoint_id - def with_command_overrides( - self, - overrides: Mapping[str, ControlCommand], - ) -> ResolvedControlPart: - """Return a snapshot with role-local semantic command overrides.""" - merged = dict(self.commands) - merged.update(overrides) - return ResolvedControlPart( - name=self.name, - joint_ids=self.joint_ids, - commands=merged, - ) + @property + def destination_key(self) -> tuple[str, str]: + """Return the transport-scoped physical destination key.""" + return self.target.transport_id, self.target.target_id + + def require_target(self, target_type: type[TargetT]) -> TargetT: + """Return the runtime target after an explicit type check.""" + if not isinstance(target_type, type) or not issubclass( + target_type, RuntimeEndpointTarget + ): + raise TypeError("target_type must be a RuntimeEndpointTarget subclass.") + if not isinstance(self.target, target_type): + raise TypeError( + f"Endpoint {self.slot_id}.{self.endpoint_id} uses " + f"{type(self.target).__name__}, expected {target_type.__name__}." + ) + return self.target.snapshot() def command(self, name: str) -> ControlCommand: - """Return an owned semantic command snapshot. - - Args: - name: Semantic command name, for example ``open`` or ``grasp``. - - Raises: - KeyError: If this control part does not define ``name``. - """ + """Return one owned semantic-command snapshot.""" try: command = self.commands[name] except KeyError as exc: raise KeyError( - f"Control part {self.name!r} has no command {name!r}. " - f"Available commands: {sorted(self.commands)}." + f"Endpoint {self.slot_id}.{self.endpoint_id} has no command " + f"{name!r}; available commands are {sorted(self.commands)}." ) from exc return command.snapshot() @@ -211,82 +315,141 @@ def joint_positions( dtype: torch.dtype | None = None, ) -> torch.Tensor: """Resolve a named joint-position command for a planning batch.""" - try: - command = self.commands[name] - except KeyError as exc: - raise KeyError( - f"Control part {self.name!r} has no command {name!r}. " - f"Available commands: {sorted(self.commands)}." - ) from exc + from .control import JointPositionCommand + + target = self.require_target(JointPositionTarget) + command = self.command(name) if not isinstance(command, JointPositionCommand): raise TypeError( - f"Control command {name!r} on {self.name!r} is " - f"{type(command).__name__}, not JointPositionCommand." + f"Endpoint command {name!r} is {type(command).__name__}, not " + "JointPositionCommand." ) return command.resolve( n_envs=n_envs, - control_dof=self.dof, + control_dof=len(target.joint_ids), device=device, dtype=dtype, ) + def with_commands( + self, + overrides: Mapping[str, ControlCommand], + ) -> EndpointBinding: + """Return an endpoint snapshot with semantic-command overrides.""" + merged = dict(self.commands) + merged.update(overrides) + return EndpointBinding( + slot_id=self.slot_id, + endpoint_id=self.endpoint_id, + resource_id=self.resource_id, + adapter_id=self.adapter_id, + target=self.target, + capabilities=self.capabilities, + commands=merged, + claim_tokens=self.claim_tokens, + joint_ids=self.joint_ids, + ) -def _normalize_resolved_map( - values: Mapping[str, ResolvedControlPart], - *, - field_name: str, -) -> Mapping[str, ResolvedControlPart]: - """Validate and freeze a resolved semantic-role mapping.""" - if not isinstance(values, Mapping): - raise TypeError(f"{field_name} must be a mapping.") - normalized: dict[str, ResolvedControlPart] = {} - for role, resource in values.items(): - if not isinstance(role, str) or not role.strip(): - raise ValueError(f"{field_name} roles must be non-empty strings.") - if not isinstance(resource, ResolvedControlPart): - raise TypeError( - f"{field_name} values must be ResolvedControlPart instances." - ) - normalized[role] = resource - return MappingProxyType(normalized) + def snapshot(self) -> EndpointBinding: + """Return an independently owned endpoint-binding snapshot.""" + return EndpointBinding( + slot_id=self.slot_id, + endpoint_id=self.endpoint_id, + resource_id=self.resource_id, + adapter_id=self.adapter_id, + target=self.target, + capabilities=self.capabilities, + commands=self.commands, + claim_tokens=self.claim_tokens, + joint_ids=self.joint_ids, + ) @dataclass(frozen=True, slots=True) -class ResolvedActionBinding: - """Runtime control parts resolved from an :class:`ActionBinding`.""" +class ActionBinding: + """Engine-owned generic endpoint bindings for one atomic action call.""" - manipulators: Mapping[str, ResolvedControlPart] = field(default_factory=dict) - end_effectors: Mapping[str, ResolvedControlPart] = field(default_factory=dict) + owner_id: str + endpoints: tuple[EndpointBinding, ...] = () def __post_init__(self) -> None: - object.__setattr__( - self, - "manipulators", - _normalize_resolved_map( - self.manipulators, field_name="resolved manipulators" - ), + _validate_identifier(self.owner_id, field_name="ActionBinding.owner_id") + if isinstance(self.endpoints, (str, bytes)): + raise TypeError("ActionBinding.endpoints must be an iterable.") + try: + endpoints = tuple(self.endpoints) + except TypeError as exc: + raise TypeError("ActionBinding.endpoints must be an iterable.") from exc + if not all(isinstance(endpoint, EndpointBinding) for endpoint in endpoints): + raise TypeError( + "ActionBinding.endpoints values must be EndpointBinding instances." + ) + keys = [endpoint.key for endpoint in endpoints] + if len(set(keys)) != len(keys): + raise ValueError("ActionBinding endpoint keys must be unique.") + snapshots = tuple(endpoint.snapshot() for endpoint in endpoints) + object.__setattr__(self, "endpoints", snapshots) + + @property + def endpoint_keys(self) -> tuple[tuple[str, str], ...]: + """Return action-local endpoint keys in binding order.""" + return tuple(endpoint.key for endpoint in self.endpoints) + + @property + def targets(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return unique owned runtime targets in binding order.""" + targets: list[RuntimeEndpointTarget] = [] + seen: set[tuple[str, str]] = set() + for endpoint in self.endpoints: + if endpoint.destination_key in seen: + continue + seen.add(endpoint.destination_key) + targets.append(endpoint.target.snapshot()) + return tuple(targets) + + def endpoint( + self, + slot_id: str, + endpoint_id: str, + ) -> EndpointBinding: + """Return one action-local resolved endpoint.""" + key = (slot_id, endpoint_id) + for endpoint in self.endpoints: + if endpoint.key == key: + return endpoint.snapshot() + raise KeyError( + f"No endpoint is bound to {slot_id}.{endpoint_id}; available endpoints " + f"are {list(self.endpoint_keys)}." ) - object.__setattr__( - self, - "end_effectors", - _normalize_resolved_map( - self.end_effectors, field_name="resolved end_effectors" + + def with_command_overrides( + self, + overrides: Mapping[tuple[str, str], Mapping[str, ControlCommand]], + ) -> ActionBinding: + """Return a binding snapshot with endpoint-scoped command overrides.""" + if not isinstance(overrides, Mapping): + raise TypeError("overrides must be a mapping.") + unknown = set(overrides).difference(self.endpoint_keys) + if unknown: + raise KeyError( + f"Command overrides reference unbound endpoints {sorted(unknown)}." + ) + return ActionBinding( + owner_id=self.owner_id, + endpoints=tuple( + ( + endpoint.with_commands(overrides[endpoint.key]) + if endpoint.key in overrides + else endpoint + ) + for endpoint in self.endpoints ), ) - def manipulator(self, role: str = "primary") -> ResolvedControlPart: - """Return the resolved manipulator for ``role``.""" - try: - return self.manipulators[role] - except KeyError as exc: - raise KeyError(f"No manipulator is bound to role {role!r}.") from exc - - def end_effector(self, role: str = "primary") -> ResolvedControlPart: - """Return the resolved tool/hand control part for ``role``.""" - try: - return self.end_effectors[role] - except KeyError as exc: - raise KeyError(f"No end effector is bound to role {role!r}.") from exc - -__all__ = ["ActionBinding", "ResolvedActionBinding", "ResolvedControlPart"] +__all__ = [ + "ActionBinding", + "EndpointBinding", + "JointPositionTarget", + "RuntimeEndpointTarget", +] diff --git a/embodichain/lab/sim/atomic_actions/control.py b/embodichain/lab/sim/atomic_actions/control.py index d36720c17..03648c79a 100644 --- a/embodichain/lab/sim/atomic_actions/control.py +++ b/embodichain/lab/sim/atomic_actions/control.py @@ -149,9 +149,10 @@ def _snapshot_commands( if not isinstance(command, ControlCommand): raise TypeError(f"{field_name} values must be ControlCommand instances.") snapshot = command.snapshot() - if not isinstance(snapshot, ControlCommand): + if type(snapshot) is not type(command) or snapshot is command: raise TypeError( - f"{field_name}[{name!r}].snapshot() must return a ControlCommand." + f"{field_name}[{name!r}].snapshot() must return an independently " + "owned value of the same ControlCommand type." ) snapshots[name] = snapshot return MappingProxyType(snapshots) @@ -194,68 +195,84 @@ def snapshot(self) -> ControlPartCommandProfile: return ControlPartCommandProfile(commands=self.commands) -def _snapshot_role_commands( - values: Mapping[str, Mapping[str, ControlCommand]], +def _snapshot_endpoint_commands( + values: Mapping[str, Mapping[str, Mapping[str, ControlCommand]]], *, field_name: str, -) -> Mapping[str, Mapping[str, ControlCommand]]: - """Validate and freeze role-scoped invocation command overrides.""" +) -> Mapping[str, Mapping[str, Mapping[str, ControlCommand]]]: + """Validate and freeze slot/endpoint-scoped command overrides.""" if not isinstance(values, Mapping): raise TypeError(f"{field_name} must be a mapping.") - snapshots: dict[str, Mapping[str, ControlCommand]] = {} - for role, commands in values.items(): - if not isinstance(role, str) or not role or role != role.strip(): + slots: dict[str, Mapping[str, Mapping[str, ControlCommand]]] = {} + for slot_id, endpoints in values.items(): + if not isinstance(slot_id, str) or not slot_id or slot_id != slot_id.strip(): raise ValueError( - f"{field_name} roles must be non-empty strings without outer " + f"{field_name} slot IDs must be non-empty strings without outer " "whitespace." ) - snapshots[role] = _snapshot_commands( - commands, - field_name=f"{field_name}[{role!r}]", - ) - return MappingProxyType(snapshots) + if not isinstance(endpoints, Mapping): + raise TypeError(f"{field_name}[{slot_id!r}] must be a mapping.") + endpoint_snapshots: dict[str, Mapping[str, ControlCommand]] = {} + for endpoint_id, commands in endpoints.items(): + if ( + not isinstance(endpoint_id, str) + or not endpoint_id + or endpoint_id != endpoint_id.strip() + ): + raise ValueError( + f"{field_name} endpoint IDs must be non-empty strings without " + "outer whitespace." + ) + endpoint_snapshots[endpoint_id] = _snapshot_commands( + commands, + field_name=f"{field_name}[{slot_id!r}][{endpoint_id!r}]", + ) + slots[slot_id] = MappingProxyType(endpoint_snapshots) + return MappingProxyType(slots) @dataclass(frozen=True, slots=True) class ActionControlOverrides: - """Per-invocation semantic command overrides keyed by binding role. + """Per-invocation semantic commands keyed by slot and endpoint. - The outer keys are action roles such as ``primary``, ``source`` or - ``destination``. The inner keys are semantic command names. The engine - applies these values after resolving the role to a concrete control part, - and the resulting commands are captured in the invocation revision's - immutable planning snapshot. + The first two keys match a skill's ``(slot_id, endpoint_id)`` contract. + The innermost mapping contains semantic command names. Overrides are + captured in the invocation revision's immutable planning snapshot. """ - manipulators: Mapping[str, Mapping[str, ControlCommand]] = field( - default_factory=dict - ) - end_effectors: Mapping[str, Mapping[str, ControlCommand]] = field( - default_factory=dict + endpoints: Mapping[ + str, + Mapping[str, Mapping[str, ControlCommand]], + ] = field( + default_factory=dict, ) def __post_init__(self) -> None: object.__setattr__( self, - "manipulators", - _snapshot_role_commands( - self.manipulators, - field_name="manipulators", - ), - ) - object.__setattr__( - self, - "end_effectors", - _snapshot_role_commands( - self.end_effectors, - field_name="end_effectors", + "endpoints", + _snapshot_endpoint_commands( + self.endpoints, + field_name="endpoints", ), ) @property def is_empty(self) -> bool: """Whether this invocation defines no command overrides.""" - return not self.manipulators and not self.end_effectors + return not self.endpoints + + def as_flat_mapping( + self, + ) -> Mapping[tuple[str, str], Mapping[str, ControlCommand]]: + """Return immutable overrides keyed by ``(slot_id, endpoint_id)``.""" + return MappingProxyType( + { + (slot_id, endpoint_id): commands + for slot_id, endpoints in self.endpoints.items() + for endpoint_id, commands in endpoints.items() + } + ) __all__ = [ diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index 112d1a203..707ed9233 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -29,6 +29,7 @@ from embodichain.lab.sim.common import BatchEntity from .affordance import Affordance +from .bindings import EndpointBinding, JointPositionTarget from .effects import StateDelta from .goals import collect_scene_dependencies from .invocation import ( @@ -40,6 +41,7 @@ ) from .plans import ( ActionPlan, + ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, @@ -47,6 +49,12 @@ ) from .policies import DynamicCollisionMode from .requirements import SkillBindingContract +from .runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + TimedCommandSequence, +) if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -148,8 +156,6 @@ class SkillDescriptor: skill_id: str goal_type: type[Any] | tuple[type[Any], ...] options_type: type[ActionOptions] - manipulator_roles: tuple[str, ...] = () - end_effector_roles: tuple[str, ...] = () agent_visible: bool = True binding_contract: SkillBindingContract | None = None """Explicit generic resource contract used by the semantic skill layer.""" @@ -168,23 +174,12 @@ def __post_init__(self) -> None: raise TypeError( "SkillDescriptor.options_type must be an ActionOptions subclass." ) - for field_name in ("manipulator_roles", "end_effector_roles"): - roles = tuple(getattr(self, field_name)) - if len(set(roles)) != len(roles) or not all( - isinstance(role, str) and role for role in roles - ): - raise ValueError(f"{field_name} must contain unique non-empty roles.") - object.__setattr__(self, field_name, roles) if self.binding_contract is not None: if not isinstance(self.binding_contract, SkillBindingContract): raise TypeError( "SkillDescriptor.binding_contract must be a " "SkillBindingContract or None." ) - self.binding_contract.validate_action_roles( - manipulator_roles=self.manipulator_roles, - end_effector_roles=self.end_effector_roles, - ) class AtomicAction(Generic[GoalT, OptionsT], ABC): @@ -204,12 +199,6 @@ class AtomicAction(Generic[GoalT, OptionsT], ABC): OptionsType: ClassVar[type[ActionOptions]] = ActionOptions """Concrete per-invocation runtime options accepted by this skill.""" - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - """Required semantic manipulator roles.""" - - end_effector_roles: ClassVar[tuple[str, ...]] = () - """Required semantic end-effector roles.""" - agent_visible: ClassVar[bool] = True """Whether an Action Agent should expose this skill by default.""" @@ -313,8 +302,6 @@ def descriptor(cls) -> SkillDescriptor: skill_id=cls.skill_id, goal_type=cls.GoalType, options_type=cls.OptionsType, - manipulator_roles=cls.manipulator_roles, - end_effector_roles=cls.end_effector_roles, agent_visible=cls.agent_visible, binding_contract=cls.__dict__.get("binding_contract"), ) @@ -351,10 +338,12 @@ def resolve_request( f"Skill {self.skill_id!r} expects goal {expected}, got " f"{type(invocation.goal).__name__}." ) - for role in self.manipulator_roles: - invocation.binding.manipulator(role) - for role in self.end_effector_roles: - invocation.binding.end_effector(role) + contract = type(self).__dict__.get("binding_contract") + if contract is None: + raise ValueError( + f"Skill {self.skill_id!r} has no explicit SkillBindingContract." + ) + self.planning_services.validate_binding(invocation.binding, contract) options = ( self._default_options if invocation.skill_options is None @@ -375,7 +364,7 @@ def resolve_request( return ResolvedActionRequest( skill_id=invocation.skill_id, goal=invocation.goal, - binding=self.planning_services.resolve_binding( + binding=self.planning_services.apply_command_overrides( invocation.binding, invocation.control_overrides, ), @@ -406,10 +395,12 @@ def require_goal( f"Skill {self.skill_id!r} received incompatible options " f"{type(request.skill_options).__name__}." ) - for role in self.manipulator_roles: - request.binding.manipulator(role) - for role in self.end_effector_roles: - request.binding.end_effector(role) + contract = type(self).__dict__.get("binding_contract") + if contract is None: + raise ValueError( + f"Skill {self.skill_id!r} has no explicit SkillBindingContract." + ) + self.planning_services.validate_binding(request.binding, contract) return request.goal def plan( @@ -428,7 +419,13 @@ def plan( """ self.require_goal(request) prepared = self._prepare_request(request, context) - return self._plan(prepared, context) + plan = self._plan(prepared, context) + if not isinstance(plan, ActionPlan): + raise TypeError("AtomicAction._plan() must return an ActionPlan.") + return replace( + plan, + commands=self._authorize_command_targets(prepared, plan.commands), + ) def _prepare_request( self, @@ -549,32 +546,71 @@ def build_plan( raise ValueError("Trajectory robot_dof must match the planning context.") timed = timed.hold_rows(success_mask, context.robot.qpos) - segments: list[TrajectorySegment] = [] - if segment_lengths is not None: - offset = 0 - for name, length in segment_lengths.items(): - if not isinstance(name, str) or not name: - raise ValueError("Trajectory segment names must be non-empty.") - if isinstance(length, bool) or not isinstance(length, int): - raise TypeError("Trajectory segment lengths must be integers.") - if length < 0: - raise ValueError("Trajectory segment lengths must be non-negative.") - if length == 0: - continue - segments.append( - TrajectorySegment( - name=name, - start=offset, - stop=offset + length, - ) - ) - offset += length - if offset != timed.waypoint_count: - raise ValueError( - "Trajectory segment lengths must sum to the trajectory " - f"waypoint count ({timed.waypoint_count}), got {offset}." - ) + commands = self._joint_command_sequence( + request, + timed, + active_mask=success_mask, + ) + return self.build_command_plan( + request, + context, + success=success_mask, + commands=commands, + expected_effects=expected_effects, + replannable=replannable, + diagnostics=diagnostics, + segment_lengths=segment_lengths, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + joint_trajectory=timed, + ) + + def build_command_plan( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + context: PlanningContext, + *, + success: bool | torch.Tensor, + commands: TimedCommandSequence, + expected_effects: StateDelta | None = None, + replannable: bool = True, + diagnostics: PlannerDiagnostics | None = None, + segment_lengths: 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. + """ + self.require_goal(request) + if not isinstance(commands, TimedCommandSequence): + raise TypeError("commands must be a TimedCommandSequence.") + if commands.batch_size != context.batch_size: + raise ValueError( + "Command sequence and planning context batch sizes must match." + ) + if not torch.equal(commands.env_ids, context.env_ids): + raise ValueError("Command sequence env_ids must match the context.") + commands = self._authorize_command_targets(request, commands) + success_mask = normalize_success_mask( + success, + n_envs=context.batch_size, + device=self.device, + name="Planning success", + ) + masked_commands = TimedCommandSequence( + frames=tuple( + frame.with_active_mask(frame.active_mask & success_mask) + for frame in commands.frames + ), + env_ids=commands.env_ids, + ) + segments = self._build_segments( + segment_lengths, + frame_count=masked_commands.frame_count, + ) if diagnostics is None: diagnostics = PlannerDiagnostics( backend=self.planning_services.planner_name @@ -582,25 +618,211 @@ def build_plan( return ActionPlan( skill_id=self.skill_id, plan_success=success_mask, - trajectory=timed, + commands=masked_commands, recovery_policy=request.recovery_policy, planned_scene_version=context.scene.version, planned_collision_world_revision=( context.scene.collision_world_revisions(context.batch_size) ), diagnostics=diagnostics, - segments=tuple(segments), + feedback_mode=feedback_mode, + joint_trajectory=joint_trajectory, + segments=segments, scene_dependencies=self._scene_dependencies(request), - collision_world_sensitive=self._uses_collision_world( - request, - context, - ), + collision_world_sensitive=self._uses_collision_world(request, context), replannable=replannable, expected_effects=expected_effects or StateDelta(), invocation_id=request.invocation_id, invocation_revision=request.revision, ) + @staticmethod + def _authorize_command_targets( + request: ResolvedActionRequest[GoalT, OptionsT], + commands: TimedCommandSequence, + ) -> TimedCommandSequence: + """Bind every emitted command to an endpoint authorized by the request. + + Actions may choose a subset of their bound endpoints for any frame, but + they cannot synthesize a destination outside the resolved resource + binding. The returned sequence replaces caller-provided target metadata + with the engine-owned binding snapshot, so transports never receive + altered joint claims or other target fields. + """ + authorized: dict[tuple[str, str], list[EndpointBinding]] = {} + for endpoint in request.binding.endpoints: + authorized.setdefault(endpoint.destination_key, []).append(endpoint) + unknown = sorted( + { + command.destination_key + for frame in commands.frames + for command in frame.commands + if command.destination_key not in authorized + } + ) + if unknown: + raise ValueError( + "Runtime commands reference destinations not authorized by the " + f"action binding: {unknown}." + ) + + frames: list[RuntimeCommandFrame] = [] + for frame in commands.frames: + endpoint_commands: list[EndpointCommand] = [] + joint_owners: dict[int, tuple[str, str]] = {} + token_owners: dict[str, tuple[str, str]] = {} + for command in frame.commands: + bound_endpoints = authorized[command.destination_key] + target = bound_endpoints[0].target + if any( + type(endpoint.target) is not type(target) + for endpoint in bound_endpoints[1:] + ): + raise ValueError( + f"Action binding destination {command.destination_key} has " + "incompatible target declarations." + ) + if type(command.target) is not type(target): + raise TypeError( + f"Runtime command destination {command.destination_key} uses " + f"target type {type(command.target).__name__}, but its bound " + f"endpoint uses {type(target).__name__}." + ) + if isinstance(target, JointPositionTarget) and command.target != target: + raise ValueError( + f"Runtime command destination {command.destination_key} " + "does not preserve its bound joint-position target." + ) + joint_ids = { + joint_id + for endpoint in bound_endpoints + for joint_id in endpoint.joint_ids + } + claim_tokens = { + token + for endpoint in bound_endpoints + for token in endpoint.claim_tokens + } + overlapping_joints = sorted(joint_ids & joint_owners.keys()) + overlapping_tokens = sorted(claim_tokens & token_owners.keys()) + if overlapping_joints or overlapping_tokens: + conflicting_destinations = sorted( + {joint_owners[joint_id] for joint_id in overlapping_joints} + | {token_owners[token] for token in overlapping_tokens} + ) + raise ValueError( + f"Runtime command destination {command.destination_key} " + f"conflicts with {conflicting_destinations} on bound joint " + f"IDs {overlapping_joints} or claim tokens " + f"{overlapping_tokens}." + ) + for joint_id in joint_ids: + joint_owners[joint_id] = command.destination_key + for token in claim_tokens: + token_owners[token] = command.destination_key + endpoint_commands.append( + EndpointCommand(target=target, payload=command.payload) + ) + frames.append( + RuntimeCommandFrame( + commands=tuple(endpoint_commands), + active_mask=frame.active_mask, + env_ids=frame.env_ids, + hold_duration=frame.hold_duration, + ) + ) + return TimedCommandSequence(frames=tuple(frames), env_ids=commands.env_ids) + + def _joint_command_sequence( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + trajectory: TimedTrajectory, + *, + active_mask: torch.Tensor, + ) -> TimedCommandSequence: + """Lower one full-robot planner trajectory to endpoint commands.""" + targets = tuple( + ( + endpoint, + endpoint.require_target(JointPositionTarget), + ) + for endpoint in request.binding.endpoints + ) + if not targets: + raise ValueError( + "Joint trajectory plans require at least one bound " + "JointPositionTarget endpoint." + ) + frames: list[RuntimeCommandFrame] = [] + for waypoint_index in range(trajectory.waypoint_count): + endpoint_commands: list[EndpointCommand] = [] + for _, target in targets: + joint_ids = list(target.joint_ids) + velocities = ( + None + if trajectory.velocities is None + else trajectory.velocities[:, waypoint_index, joint_ids] + ) + endpoint_commands.append( + EndpointCommand( + target=target, + payload=JointPositionPayload( + positions=trajectory.positions[ + :, waypoint_index, joint_ids + ], + velocities=velocities, + ), + ) + ) + next_waypoint_index = min( + waypoint_index + 1, + trajectory.waypoint_count - 1, + ) + # ``dt[:, i]`` is the arrival interval for waypoint ``i``. After + # dispatching it, wait for the next arrival interval; the terminal + # frame deliberately reuses its own interval as a settling window, + # preserving the closed-loop runner's pre-PR2C timing contract. + frames.append( + RuntimeCommandFrame( + commands=tuple(endpoint_commands), + active_mask=active_mask, + env_ids=trajectory.env_ids, + hold_duration=trajectory.dt[:, next_waypoint_index], + ) + ) + return TimedCommandSequence(frames=tuple(frames), env_ids=trajectory.env_ids) + + @staticmethod + def _build_segments( + segment_lengths: Mapping[str, int] | None, + *, + frame_count: int, + ) -> tuple[TrajectorySegment, ...]: + """Validate optional named ranges for one command sequence.""" + if segment_lengths is None: + return () + segments: list[TrajectorySegment] = [] + offset = 0 + for name, length in segment_lengths.items(): + if not isinstance(name, str) or not name: + raise ValueError("Trajectory segment names must be non-empty.") + if isinstance(length, bool) or not isinstance(length, int): + raise TypeError("Trajectory segment lengths must be integers.") + if length < 0: + raise ValueError("Trajectory segment lengths must be non-negative.") + if length == 0: + continue + segments.append( + TrajectorySegment(name=name, start=offset, stop=offset + length) + ) + offset += length + if offset != frame_count: + raise ValueError( + "Trajectory segment lengths must sum to the command frame count " + f"({frame_count}), got {offset}." + ) + return tuple(segments) + def failed_plan( self, request: ResolvedActionRequest[GoalT, OptionsT], @@ -618,23 +840,35 @@ def failed_plan( Returns: Failed action plan with an empty trajectory. """ - return self.build_plan( + success = torch.zeros(context.batch_size, dtype=torch.bool, device=self.device) + diagnostics = PlannerDiagnostics( + backend=self.planning_services.planner_name, + messages=(() if message is None else (message,)), + ) + if request.binding.endpoints and all( + isinstance(endpoint.target, JointPositionTarget) + for endpoint in request.binding.endpoints + ): + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.empty( + batch_size=context.batch_size, + robot_dof=context.robot.robot_dof, + device=self.device, + env_ids=context.env_ids, + ), + replannable=True, + diagnostics=diagnostics, + ) + return self.build_command_plan( request, context, - success=torch.zeros( - context.batch_size, dtype=torch.bool, device=self.device - ), - trajectory=TimedTrajectory.empty( - batch_size=context.batch_size, - robot_dof=context.robot.robot_dof, - device=self.device, - env_ids=context.env_ids, - ), + success=success, + commands=TimedCommandSequence(frames=(), env_ids=context.env_ids), replannable=True, - diagnostics=PlannerDiagnostics( - backend=self.planning_services.planner_name, - messages=(() if message is None else (message,)), - ), + diagnostics=diagnostics, ) @abstractmethod diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 9dc97d646..d73e9a3f5 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -23,6 +23,7 @@ import torch +from .bindings import ActionBinding from .core import AtomicAction, SkillDescriptor from .control import ControlPartCommandProfile from .invocation import ActionInvocation, ResolvedActionRequest @@ -161,6 +162,11 @@ def planning_services(self) -> ActionPlanningServices: """Engine-owned resources shared by every bound atomic action.""" return self._planning_services + @property + def binding_owner_id(self) -> str: + """Return the opaque owner identity required by action bindings.""" + return self._planning_services.binding_owner_id + @property def control_profiles(self) -> Mapping[str, ControlPartCommandProfile]: """Semantic command profiles registered for robot control parts.""" @@ -227,6 +233,43 @@ def bind_skill_profile( self._skill_profile = bound return bound + def bind_control_parts( + self, + skill: str | AtomicAction, + endpoints: Mapping[str, Mapping[str, str]], + ) -> ActionBinding: + """Build an advanced direct-core binding from control-part names. + + Args: + skill: Installed skill ID or an explicit action passed later to + :meth:`plan_action`. + endpoints: Nested ``slot_id -> endpoint_id -> control_part`` mapping. + + Returns: + Engine-owned generic endpoint binding. + """ + if isinstance(skill, str): + action = self._actions.get(skill) + if action is None: + raise KeyError(f"No atomic action registered for skill {skill!r}.") + elif isinstance(skill, AtomicAction): + action = skill + if ( + action.is_bound + and action.planning_services is not self._planning_services + ): + raise ValueError( + f"Atomic action {action.skill_id!r} belongs to another engine." + ) + else: + raise TypeError("skill must be an installed skill ID or AtomicAction.") + contract = type(action).__dict__.get("binding_contract") + if contract is None: + raise ValueError( + f"Skill {action.skill_id!r} has no explicit SkillBindingContract." + ) + return self._planning_services.bind_control_parts(contract, endpoints) + def register(self, action: AtomicAction, *, replace: bool = False) -> None: """Register one action instance using its descriptor. @@ -449,7 +492,15 @@ def compile( previous_qpos = projected.robot.qpos plan = self.plan(invocation, projected) step_success = alive & plan.plan_success.to(self.device) - trajectory = plan.trajectory.hold_rows(step_success, previous_qpos) + if plan.joint_trajectory is None: + raise ValueError( + f"Skill {plan.skill_id!r} emits non-joint runtime commands and " + "cannot be used with offline joint-trajectory compilation." + ) + trajectory = plan.joint_trajectory.hold_rows( + step_success, + previous_qpos, + ) plans.append(plan) trajectories.append(trajectory) @@ -529,15 +580,23 @@ def _validate_plan( raise ValueError( "ActionPlan.invocation_revision must preserve the request revision." ) - trajectory = plan.trajectory - if trajectory.batch_size != context.batch_size: + commands = plan.commands + if commands.batch_size != context.batch_size: raise ValueError("Action plan batch size does not match the context.") - if trajectory.robot_dof != self.robot.dof: - raise ValueError("Action plan robot_dof does not match the engine robot.") - if trajectory.positions.device != self.device: + if commands.device != self.device: raise ValueError("Action plan and engine must share a device.") - if not torch.equal(trajectory.env_ids, context.env_ids): + if not torch.equal(commands.env_ids, context.env_ids): raise ValueError("Action plan and context must share ordered env_ids.") + if plan.joint_trajectory is not None: + if plan.joint_trajectory.robot_dof != self.robot.dof: + raise ValueError( + "Action plan joint_trajectory robot_dof does not match the " + "engine robot." + ) + if plan.joint_trajectory.positions.device != self.device: + raise ValueError( + "Action plan joint_trajectory and engine must share a device." + ) if plan.planned_scene_version != context.scene.version: raise ValueError("Action plan must record the planning scene version.") collision_revision = context.scene.collision_world_revisions(context.batch_size) diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 1b95e64f9..b425b9934 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -26,7 +26,17 @@ from .effects import StateDelta from .invocation import ActionInvocation, ResolvedActionRequest -from .plans import ActionPlan, TimedTrajectory, TrajectorySegment +from .bindings import JointPositionTarget, RuntimeEndpointTarget +from .plans import ( + ActionPlan, + ExecutionFeedbackMode, + TrajectorySegment, +) +from .runtime_commands import ( + JointPositionPayload, + RuntimeCommandFrame, + TimedCommandSequence, +) from .state import EntityState, PlanningContext, SceneSnapshot, TaskState if TYPE_CHECKING: @@ -120,65 +130,14 @@ def __post_init__(self) -> None: object.__setattr__(self, "env_mask", self.env_mask.clone()) -@dataclass(frozen=True, slots=True, eq=False) -class JointCommand: - """Full-robot command produced by one session tick.""" - - positions: torch.Tensor - velocities: torch.Tensor | None - active_mask: torch.Tensor - env_ids: torch.Tensor - hold_duration: torch.Tensor - """Per-environment delay before the next observation/command cycle.""" - - def __post_init__(self) -> None: - if self.positions.dim() != 2: - raise ValueError("JointCommand.positions must have shape (B, robot_dof).") - if ( - self.velocities is not None - and self.velocities.shape != self.positions.shape - ): - raise ValueError("JointCommand.velocities must match positions shape.") - if self.active_mask.dtype != torch.bool or self.active_mask.shape != ( - self.positions.shape[0], - ): - raise ValueError("JointCommand.active_mask must be bool with shape (B,).") - if self.env_ids.dtype != torch.long or self.env_ids.shape != ( - self.positions.shape[0], - ): - raise ValueError("JointCommand.env_ids must be int64 with shape (B,).") - if not isinstance(self.hold_duration, torch.Tensor): - raise TypeError("JointCommand.hold_duration must be a torch.Tensor.") - if self.hold_duration.shape != (self.positions.shape[0],): - raise ValueError("JointCommand.hold_duration must have shape (B,).") - if ( - not torch.isfinite(self.hold_duration).all() - or (self.hold_duration < 0.0).any() - ): - raise ValueError( - "JointCommand.hold_duration must contain finite non-negative values." - ) - if self.active_mask.device != self.positions.device: - raise ValueError("JointCommand tensors must share a device.") - if self.env_ids.device != self.positions.device: - raise ValueError("JointCommand tensors must share a device.") - if self.hold_duration.device != self.positions.device: - raise ValueError("JointCommand tensors must share a device.") - object.__setattr__(self, "positions", self.positions.clone()) - if self.velocities is not None: - object.__setattr__(self, "velocities", self.velocities.clone()) - object.__setattr__(self, "active_mask", self.active_mask.clone()) - object.__setattr__(self, "env_ids", self.env_ids.clone()) - object.__setattr__(self, "hold_duration", self.hold_duration.clone()) - - @dataclass(frozen=True, slots=True, eq=False) class ExecutionTick: """Result returned after one closed-loop execution update.""" status: ExecutionStatus eligible_mask: torch.Tensor - command: JointCommand | None + command: RuntimeCommandFrame | None + hold_targets: tuple[RuntimeEndpointTarget, ...] events: tuple[ExecutionEvent, ...] task_state: TaskState pending_effect: EffectVerificationRequest | None = None @@ -192,16 +151,37 @@ def __post_init__(self) -> None: raise TypeError( "pending_effect must be an EffectVerificationRequest or None." ) + if self.command is not None and not isinstance( + self.command, + RuntimeCommandFrame, + ): + raise TypeError("command must be a RuntimeCommandFrame or None.") + if isinstance(self.hold_targets, (str, bytes)) or not all( + isinstance(target, RuntimeEndpointTarget) for target in self.hold_targets + ): + 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.") + hold_targets: list[RuntimeEndpointTarget] = [] + for target in self.hold_targets: + snapshot = target.snapshot() + if type(snapshot) is not type(target) or snapshot is target: + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently " + "owned value of the same target type." + ) + hold_targets.append(snapshot) object.__setattr__(self, "eligible_mask", self.eligible_mask.clone()) object.__setattr__(self, "events", tuple(self.events)) + object.__setattr__(self, "hold_targets", tuple(hold_targets)) class ExecutionSession: """Execute grounded invocations incrementally with bounded local recovery. The session never steps a simulator itself. Each :meth:`tick` consumes the - latest observation and scene snapshot and emits at most one full-robot - command. Expected symbolic effects are committed only after the caller + 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`. Environment eligibility and recovery budgets are tracked per row. The @@ -227,9 +207,14 @@ def __init__( self._invocation_index = 0 self._waypoint_index = 0 self._plan: ActionPlan | None = None + self._active_targets: dict[ + tuple[str, str], + RuntimeEndpointTarget, + ] = {} self._planned_scene = context.scene self._action_started_at = context.robot.timestamp - self._last_command: torch.Tensor | None = None + self._last_joint_command: torch.Tensor | None = None + self._last_joint_ids: tuple[int, ...] = () self._last_command_mask = torch.zeros( context.batch_size, dtype=torch.bool, device=context.robot.qpos.device ) @@ -264,75 +249,151 @@ def task_state(self) -> TaskState: """Verified symbolic task state accumulated by this session.""" return self._task_state - def revise_current(self, invocation: ActionInvocation) -> None: + @property + def effect_verification_pending(self) -> bool: + """Whether the current physical effect still requires verification.""" + return self._pending_effect is not None + + def revise_current( + self, + invocation: ActionInvocation, + *, + context: PlanningContext | None = None, + ) -> None: """Replace and replan the current invocation with a newer revision. The replacement is resolved into a new immutable request snapshot from - the latest observation. Retry and replan budgets restart for the new - revision, while verified task state, the current batch barrier, and - per-environment eligibility are preserved. Ordinary recovery replans - continue to reuse this snapshot until another explicit revision. + ``context`` or the session's latest observation. Retry and replan + budgets restart for the new revision, while verified task state, the + current batch barrier, and per-environment eligibility are preserved. + Ordinary recovery replans continue to reuse this snapshot until another + explicit revision. Once the action owns runtime destinations, the + replacement must preserve their exact address fingerprints; changing + controllers or safe-hold footprints requires a new invocation. Args: invocation: Grounded replacement for the currently active skill. Its ``revision`` must be strictly greater than the active one, and its ``skill_id`` and ``invocation_id`` must identify the same logical call. + context: Optional fresh observation used to ground the replacement. + A manually ticked caller may omit it to reuse + :attr:`latest_context`. Runner-driven code stages revisions on + :class:`ExecutionRunner`, which supplies a due-time observation. Raises: TypeError: If ``invocation`` is not an ActionInvocation. - RuntimeError: If the session is no longer running. + RuntimeError: If the session is no longer running or a physical + effect is awaiting verification. ValueError: If the replacement identifies another invocation or - does not advance the revision. + does not advance the revision, or if its plan changes the + active runtime target addresses. """ + replacement = self._prepare_revision(invocation) + replacement_context = self._context if context is None else context + self._install_prepared_revision(replacement, replacement_context) + + def _prepare_revision( + self, + invocation: ActionInvocation, + ) -> ResolvedActionRequest: + """Validate and snapshot one revision without planning or installing it.""" if not isinstance(invocation, ActionInvocation): raise TypeError("invocation must be an ActionInvocation.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - current = self._requests[self._invocation_index] - if invocation.skill_id != current.skill_id: - raise ValueError( - f"Revision skill_id {invocation.skill_id!r} does not match " - f"the active skill {current.skill_id!r}." - ) - if invocation.invocation_id != current.invocation_id: - raise ValueError( - "Revision invocation_id must match the active invocation_id." + if self._pending_effect is not None: + raise RuntimeError( + "Cannot revise while a physical effect is awaiting verification; " + "verify it or cancel and start a new invocation." ) - if invocation.revision <= current.revision: - raise ValueError( - f"Revision must advance beyond {current.revision}, got " - f"{invocation.revision}." + self._validate_revision_identity( + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + revision=invocation.revision, + ) + return self._engine.resolve(invocation) + + def _install_prepared_revision( + self, + replacement: ResolvedActionRequest, + context: PlanningContext, + ) -> None: + """Plan and transactionally install a previously snapshotted revision.""" + if not isinstance(replacement, ResolvedActionRequest): + 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: + raise RuntimeError( + "Cannot revise while a physical effect is awaiting verification; " + "verify it or cancel and start a new invocation." ) + self._validate_revision_identity( + skill_id=replacement.skill_id, + invocation_id=replacement.invocation_id, + revision=replacement.revision, + ) + replacement_context = self._validated_context(context) + replacement_plan = self._engine.plan_request( + replacement, + replacement_context, + ) + self._validate_destination_continuity( + replacement_plan, + ExecutionEventKind.INVOCATION_REVISED, + ) - replacement = self._engine.resolve(invocation) - replacement_plan = self._engine.plan_request(replacement, self._context) requests = list(self._requests) requests[self._invocation_index] = replacement self._requests = tuple(requests) + self._context = replacement_context self._waypoint_index = 0 self._action_retries.zero_() self._replans.zero_() self._install_plan( replacement_plan, - self._context, + replacement_context, ExecutionEventKind.INVOCATION_REVISED, ) + def _validate_revision_identity( + self, + *, + skill_id: str, + invocation_id: str | None, + revision: int, + ) -> None: + """Validate identity and ordering shared by staged and direct revisions.""" + current = self._requests[self._invocation_index] + if skill_id != current.skill_id: + raise ValueError( + f"Revision skill_id {skill_id!r} does not match " + f"the active skill {current.skill_id!r}." + ) + if invocation_id != current.invocation_id: + raise ValueError( + "Revision invocation_id must match the active invocation_id." + ) + if revision <= current.revision: + raise ValueError( + f"Revision must advance beyond {current.revision}, got " f"{revision}." + ) + @property def latest_context(self) -> PlanningContext: """Latest validated context with the session's verified task state.""" return self._context @property - def active_trajectory(self) -> TimedTrajectory: - """Return an owned snapshot of the active action trajectory. + def active_commands(self) -> TimedCommandSequence: + """Return an owned snapshot of the active action command sequence. This inspection surface is intended for diagnostics and visualization. Mutating the returned tensors cannot affect execution state. """ assert self._plan is not None - return self._plan.trajectory.snapshot() + return self._plan.commands.snapshot() def trajectory_segment(self, name: str) -> TrajectorySegment: """Return named segment metadata for the active action plan. @@ -360,33 +421,7 @@ def tick( Returns: Status, optional command, events, and current verified task state. """ - self._engine._validate_context(context) - if context.robot.timestamp < self._context.robot.timestamp: - raise ValueError("Execution tick timestamps must be monotonic.") - if context.scene.timestamp < self._context.scene.timestamp: - raise ValueError("Scene snapshot timestamps must be monotonic.") - if context.scene.version < self._context.scene.version: - raise ValueError("Scene snapshot versions must be monotonic.") - previous_collision_revision = torch.tensor( - self._context.scene.collision_world_revisions(context.batch_size), - dtype=torch.long, - device=context.robot.qpos.device, - ) - current_collision_revision = torch.tensor( - context.scene.collision_world_revisions(context.batch_size), - dtype=torch.long, - device=context.robot.qpos.device, - ) - if (current_collision_revision < previous_collision_revision).any(): - raise ValueError("Collision-world revisions must be monotonic.") - if not torch.equal(context.env_ids, self._context.env_ids): - raise ValueError("Execution tick env_ids must remain stable and ordered.") - self._context = PlanningContext( - robot=context.robot, - task=self._task_state, - scene=context.scene, - env_ids=context.env_ids, - ) + self._context = self._validated_context(context) events = self._drain_events() if self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) @@ -396,12 +431,16 @@ def tick( execution_mask = ( self._pending_effect.env_mask & self._pending & self._plan.plan_success ) - command, completion_events = self._finish_action( + command, hold_targets, completion_events = self._finish_action( execution_mask, effect_success, ) events.extend(completion_events) - return self._tick_result(command=command, events=events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) plan = self._plan execution_mask = self._pending & plan.plan_success @@ -421,8 +460,8 @@ def tick( plan = self._plan execution_mask = self._pending & self._plan.plan_success - trajectory = plan.trajectory - if self._waypoint_index < trajectory.waypoint_count: + commands = plan.commands + if self._waypoint_index < commands.frame_count: command = self._command_at(plan, self._waypoint_index, execution_mask) self._waypoint_index += 1 return self._tick_result(command=command, events=events) @@ -444,9 +483,27 @@ def tick( assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success - command = self._command_at(plan, 0, execution_mask) - self._waypoint_index = 1 - return self._tick_result(command=command, events=events) + 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.", + ) + ) + command, hold_targets, completion_events = self._finish_action( + execution_mask, + effect_success, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) events.append( self._event( @@ -456,12 +513,46 @@ def tick( ) ) - command, completion_events = self._finish_action( + command, hold_targets, completion_events = self._finish_action( execution_mask, effect_success, ) events.extend(completion_events) - return self._tick_result(command=command, events=events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + + def _validated_context(self, context: PlanningContext) -> PlanningContext: + """Validate one monotonic observation and attach verified task state.""" + self._engine._validate_context(context) + if context.robot.timestamp < self._context.robot.timestamp: + raise ValueError("Execution tick timestamps must be monotonic.") + if context.scene.timestamp < self._context.scene.timestamp: + raise ValueError("Scene snapshot timestamps must be monotonic.") + if context.scene.version < self._context.scene.version: + raise ValueError("Scene snapshot versions must be monotonic.") + previous_collision_revision = torch.tensor( + self._context.scene.collision_world_revisions(context.batch_size), + dtype=torch.long, + device=context.robot.qpos.device, + ) + current_collision_revision = torch.tensor( + context.scene.collision_world_revisions(context.batch_size), + dtype=torch.long, + device=context.robot.qpos.device, + ) + if (current_collision_revision < previous_collision_revision).any(): + raise ValueError("Collision-world revisions must be monotonic.") + if not torch.equal(context.env_ids, self._context.env_ids): + raise ValueError("Execution tick env_ids must remain stable and ordered.") + return PlanningContext( + robot=context.robot, + task=self._task_state, + scene=context.scene, + env_ids=context.env_ids, + ) def _plan_current( self, @@ -480,11 +571,27 @@ def _install_plan( event_kind: ExecutionEventKind, ) -> None: """Install an already validated plan as the current execution plan.""" + replacement_targets = { + (target.transport_id, target.target_id): target.snapshot() + for target in plan.commands.targets + } + replacement_destinations = frozenset(replacement_targets) + self._validate_destination_continuity(plan, event_kind) + if ( + event_kind + not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ) + or replacement_destinations + ): + self._active_targets = replacement_targets self._plan = plan self._waypoint_index = 0 self._planned_scene = context.scene self._action_started_at = context.robot.timestamp - self._last_command = None + self._last_joint_command = None + self._last_joint_ids = () self._last_command_mask.zero_() self._pending_effect = None planned_mask = self._pending & plan.plan_success @@ -492,6 +599,67 @@ def _install_plan( self._event(event_kind, planned_mask, "Planned from the latest context.") ) + def _validate_destination_continuity( + self, + plan: ActionPlan, + event_kind: ExecutionEventKind, + ) -> None: + """Reject in-place plans that change controller or safe-hold ownership.""" + if event_kind not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ): + return + replacement_targets = { + (target.transport_id, target.target_id): target + for target in plan.commands.targets + } + active_destinations = frozenset(self._active_targets) + replacement_destinations = frozenset(replacement_targets) + if not active_destinations: + return + if not replacement_destinations: + if event_kind is ExecutionEventKind.REPLANNED: + return + raise ValueError( + "Invocation revisions must declare the active runtime destination " + "set; an empty replacement plan cannot prove target continuity." + ) + if replacement_destinations == active_destinations: + mismatched_fingerprints = sorted( + destination + for destination in active_destinations + if replacement_targets[destination].address_fingerprint + != self._active_targets[destination].address_fingerprint + ) + if not mismatched_fingerprints: + return + prefix = ( + "Recovery replans" + if event_kind is ExecutionEventKind.REPLANNED + else "Invocation revisions" + ) + guidance = ( + "" + if event_kind is ExecutionEventKind.REPLANNED + else " Start a new invocation to change runtime target addresses." + ) + raise ValueError( + f"{prefix} must preserve each runtime target address fingerprint; " + f"changed={mismatched_fingerprints}.{guidance}" + ) + if event_kind is ExecutionEventKind.REPLANNED: + prefix = "Recovery replans" + guidance = "" + else: + prefix = "Invocation revisions" + guidance = " Start a new invocation to change runtime destinations." + raise ValueError( + f"{prefix} must preserve the active runtime destination set; " + f"previous={sorted(active_destinations)}, " + f"replacement={sorted(replacement_destinations)}.{guidance}" + ) + def _recover_if_needed( self, plan: ActionPlan, @@ -517,9 +685,18 @@ def _recover_if_needed( ExecutionEventKind.COLLISION_WORLD_CHANGED, "The collision world changed after this trajectory was planned.", ) - if self._last_command is not None: + if ( + plan.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION + and self._last_joint_command is not None + and self._last_joint_ids + ): + joint_ids = list(self._last_joint_ids) tracking_error = torch.amax( - torch.abs(self._context.robot.qpos - self._last_command), dim=1 + torch.abs( + self._context.robot.qpos[:, joint_ids] + - self._last_joint_command[:, joint_ids] + ), + dim=1, ) tracking_mask = ( execution_mask @@ -598,7 +775,7 @@ def _attempt_action_retry( ) if allowed.any(): self._action_retries[allowed] += 1 - self._replans.zero_() + self._replans[allowed] = 0 events.append( self._event( ExecutionEventKind.ACTION_RETRY, @@ -615,9 +792,20 @@ def _finish_action( self, execution_mask: torch.Tensor, effect_success: torch.Tensor | None, - ) -> tuple[JointCommand | None, list[ExecutionEvent]]: + ) -> tuple[ + RuntimeCommandFrame | None, + tuple[RuntimeEndpointTarget, ...], + list[ExecutionEvent], + ]: """Verify effects, update symbolic state, and advance the action barrier.""" assert self._plan is not None + plan_targets = self._plan.commands.targets + active_targets = ( + plan_targets + if plan_targets + else tuple(target.snapshot() for target in self._active_targets.values()) + ) + orphaned_targets = bool(active_targets) and not plan_targets events: list[ExecutionEvent] = [] planning_failed = self._pending & ~self._plan.plan_success if not execution_mask.any() and planning_failed.any(): @@ -629,8 +817,8 @@ def _finish_action( ) ) if self._status is not ExecutionStatus.RUNNING: - return None, events - return self._hold_command(), events + return None, active_targets, events + return None, active_targets, events if self._plan.expected_effects.is_empty: verified = execution_mask @@ -644,7 +832,7 @@ def _finish_action( "Expected symbolic effects require external verification.", ) ) - return self._hold_command(), events + return None, active_targets, events else: verified_input = self._normalize_mask(effect_success, "effect_success") verified = execution_mask & verified_input @@ -672,11 +860,11 @@ def _finish_action( ) ) if self._status is not ExecutionStatus.RUNNING: - return None, events - return self._hold_command(), events + return None, active_targets, events + return None, active_targets, events if self._pending.any(): - return self._hold_command(), events + return None, active_targets, events events.append( self._event( ExecutionEventKind.ACTION_COMPLETED, @@ -698,7 +886,7 @@ def _finish_action( "Invocation sequence completed.", ) ) - return None, events + return None, (active_targets if orphaned_targets else ()), events self._pending = self._eligible.clone() self._pending_effect = None @@ -706,64 +894,84 @@ def _finish_action( self._replans.zero_() self._plan_current(self._context, ExecutionEventKind.ACTION_PLANNED) events.extend(self._drain_events()) - return self._hold_command(), events + return None, active_targets, events def _command_at( self, plan: ActionPlan, waypoint_index: int, active_mask: torch.Tensor, - ) -> JointCommand: - """Build one command and retain it for tracking-error monitoring.""" - positions = plan.trajectory.positions[:, waypoint_index] - hold = self._context.robot.qpos - positions = torch.where(active_mask[:, None], positions, hold) - velocities = None - if plan.trajectory.velocities is not None: - values = plan.trajectory.velocities[:, waypoint_index] - velocities = torch.where( - active_mask[:, None], values, torch.zeros_like(values) - ) - self._last_command = positions.clone() - self._last_command_mask = active_mask.clone() - # ``dt[:, i]`` leads to waypoint ``i``. After dispatching waypoint - # ``i``, wait for ``dt[:, i + 1]`` before the next dispatch. Reuse the - # final arrival interval as its terminal settling window. - next_waypoint_index = min( - waypoint_index + 1, - plan.trajectory.waypoint_count - 1, - ) - hold_duration = plan.trajectory.dt[:, next_waypoint_index] - return JointCommand( - positions=positions, - velocities=velocities, - active_mask=active_mask, - env_ids=plan.trajectory.env_ids, - hold_duration=hold_duration, - ) + ) -> RuntimeCommandFrame: + """Return one frame and retain joint targets when feedback requires it.""" + 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_() + return frame - def _hold_command(self) -> JointCommand: - """Build a passive hold command from the latest observation.""" - return JointCommand( - positions=self._context.robot.qpos, - velocities=torch.zeros_like(self._context.robot.qpos), - active_mask=torch.zeros_like(self._eligible), - env_ids=self._context.env_ids, - hold_duration=torch.zeros( + 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=torch.float32, + dtype=self._context.robot.qpos.dtype, device=self._context.robot.qpos.device, - ), - ) - - def _terminal_error(self, plan: ActionPlan) -> torch.Tensor: - """Return per-row max joint error to the action terminal command.""" - if plan.trajectory.waypoint_count == 0: - return torch.full_like(self._eligible, float("inf"), dtype=torch.float32) - return torch.amax( - torch.abs(self._context.robot.qpos - plan.trajectory.positions[:, -1]), - dim=1, - ) + ) + 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, + ): + 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 + ) + ) + if not errors: + return torch.full_like( + self._eligible, + float("inf"), + dtype=self._context.robot.qpos.dtype, + ) + 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.""" @@ -901,14 +1109,16 @@ def _update_terminal_status(self) -> None: def _tick_result( self, *, - command: JointCommand | None, + command: RuntimeCommandFrame | None, events: list[ExecutionEvent], + hold_targets: tuple[RuntimeEndpointTarget, ...] = (), ) -> ExecutionTick: """Build an immutable tick result.""" return ExecutionTick( status=self._status, eligible_mask=self._eligible, command=command, + hold_targets=hold_targets, events=tuple(events), task_state=self._task_state, pending_effect=self._pending_effect, @@ -922,5 +1132,4 @@ def _tick_result( "ExecutionSession", "ExecutionStatus", "ExecutionTick", - "JointCommand", ] diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index b47795612..652cbf78a 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -25,7 +25,7 @@ from embodichain.lab.sim.common import BatchEntity -from .bindings import ActionBinding, ResolvedActionBinding +from .bindings import ActionBinding from .control import ActionControlOverrides from .goals import ActionGoal from .policies import MotionPolicy, RecoveryPolicy @@ -82,7 +82,7 @@ def visit(value: object) -> None: @dataclass(frozen=True, slots=True) class ActionInvocation(Generic[GoalT, OptionsT]): - """One fully typed and embodiment-bound atomic skill request. + """One fully typed and endpoint-bound atomic skill request. This is a runtime-domain object, not the JSON protocol emitted by an MLLM. An action compiler is responsible for converting a semantic ``SkillCallSpec`` @@ -96,7 +96,7 @@ class ActionInvocation(Generic[GoalT, OptionsT]): """Action-specific goal value object.""" binding: ActionBinding - """Semantic-role bindings to keys in the selected robot's control parts.""" + """Generic skill endpoint bindings owned by the selected engine.""" motion_policy: MotionPolicy = field(default_factory=MotionPolicy) """Reusable motion-generation settings.""" @@ -159,7 +159,7 @@ class ResolvedActionRequest(Generic[GoalT, OptionsT]): skill_id: str goal: GoalT - binding: ResolvedActionBinding + binding: ActionBinding motion_policy: MotionPolicy recovery_policy: RecoveryPolicy skill_options: OptionsT @@ -169,8 +169,8 @@ class ResolvedActionRequest(Generic[GoalT, OptionsT]): def __post_init__(self) -> None: if not isinstance(self.skill_id, str) or not self.skill_id.strip(): raise ValueError("skill_id must be a non-empty string.") - if not isinstance(self.binding, ResolvedActionBinding): - raise TypeError("binding must be a ResolvedActionBinding.") + if not isinstance(self.binding, ActionBinding): + 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.recovery_policy, RecoveryPolicy): @@ -188,6 +188,14 @@ def __post_init__(self) -> None: "goal", deepcopy(self.goal, _goal_snapshot_memo(self.goal)), ) + object.__setattr__( + self, + "binding", + ActionBinding( + owner_id=self.binding.owner_id, + endpoints=self.binding.endpoints, + ), + ) object.__setattr__(self, "motion_policy", deepcopy(self.motion_policy)) object.__setattr__(self, "recovery_policy", deepcopy(self.recovery_policy)) object.__setattr__(self, "skill_options", deepcopy(self.skill_options)) diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index 356a0d6f6..b423cf19e 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import dataclass, field +from enum import Enum from types import MappingProxyType from typing import Any, Mapping, Sequence @@ -26,8 +27,10 @@ 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 .state import PlanningContext @@ -109,7 +112,26 @@ def __post_init__(self) -> None: raise ValueError(f"env_ids must be int64 with shape ({batch_size},).") if self.env_ids.device != self.positions.device: raise ValueError("env_ids must share the positions device.") - object.__setattr__(self, "env_ids", self.env_ids.clone()) + if torch.unique(self.env_ids).numel() != batch_size: + raise ValueError("env_ids must contain unique values.") + object.__setattr__(self, "positions", self.positions.detach().clone()) + object.__setattr__( + self, + "velocities", + None if self.velocities is None else self.velocities.detach().clone(), + ) + object.__setattr__( + self, + "accelerations", + ( + None + if self.accelerations is None + else self.accelerations.detach().clone() + ), + ) + object.__setattr__(self, "dt", self.dt.detach().clone()) + object.__setattr__(self, "duration", self.duration.detach().clone()) + object.__setattr__(self, "env_ids", self.env_ids.detach().clone()) @property def batch_size(self) -> int: @@ -352,6 +374,13 @@ def __post_init__(self) -> None: object.__setattr__(self, "metadata", MappingProxyType(dict(self.metadata))) +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 TrajectorySegment: """Named half-open waypoint range inside an action trajectory. @@ -391,18 +420,20 @@ def contains(self, waypoint_index: int) -> bool: class ActionPlan: """Scene-bound planning result for one grounded atomic action invocation. - An action owns one trajectory and one recovery boundary. Named + An action owns one timed command sequence and one recovery boundary. Named :class:`TrajectorySegment` values describe semantic structure within that - trajectory without implying independent planning or recovery boundaries. + sequence without implying independent planning or recovery boundaries. """ skill_id: str plan_success: torch.Tensor - trajectory: TimedTrajectory + commands: TimedCommandSequence recovery_policy: RecoveryPolicy planned_scene_version: int planned_collision_world_revision: tuple[int, ...] diagnostics: PlannerDiagnostics + feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED + joint_trajectory: TimedTrajectory | None = None segments: tuple[TrajectorySegment, ...] = () scene_dependencies: tuple[str, ...] = () collision_world_sensitive: bool = False @@ -423,21 +454,179 @@ def __post_init__(self) -> None: raise TypeError("plan_success must be a torch.Tensor.") if self.plan_success.dtype != torch.bool or self.plan_success.dim() != 1: raise ValueError("plan_success must be a 1D bool tensor.") - if not isinstance(self.trajectory, TimedTrajectory): - raise TypeError("trajectory must be a TimedTrajectory.") - if self.trajectory.batch_size != self.plan_success.shape[0]: - raise ValueError("plan_success batch must match the trajectory.") - if self.trajectory.positions.device != self.plan_success.device: - raise ValueError("plan_success and trajectory must share a device.") + if not isinstance(self.commands, TimedCommandSequence): + raise TypeError("commands must be a TimedCommandSequence.") + if self.commands.batch_size != self.plan_success.shape[0]: + 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.") + 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): + target_types = { + command.destination_key: type(command.target) + for command in frame.commands + } + target_fingerprints = { + command.destination_key: command.target.address_fingerprint + for command in frame.commands + } + if expected_target_types is None: + expected_target_types = target_types + expected_target_fingerprints = target_fingerprints + continue + if target_types.keys() != expected_target_types.keys(): + raise ValueError( + "ActionPlan command frames must preserve the same destination " + f"set; frame {frame_index} differs from frame 0." + ) + mismatched_types = sorted( + destination + for destination, target_type in target_types.items() + if target_type is not expected_target_types[destination] + ) + if mismatched_types: + raise ValueError( + "ActionPlan command frames must preserve the exact target type " + f"for each destination; frame {frame_index} differs at " + f"{mismatched_types}." + ) + assert expected_target_fingerprints is not None + mismatched_fingerprints = sorted( + destination + for destination, fingerprint in target_fingerprints.items() + if fingerprint != expected_target_fingerprints[destination] + ) + if mismatched_fingerprints: + raise ValueError( + "ActionPlan command frames must preserve the target address " + f"fingerprint for each destination; frame {frame_index} " + f"differs at {mismatched_fingerprints}." + ) + if self.joint_trajectory is not None: + if not isinstance(self.joint_trajectory, TimedTrajectory): + raise TypeError("joint_trajectory must be a TimedTrajectory or None.") + if self.joint_trajectory.batch_size != self.commands.batch_size: + raise ValueError( + "joint_trajectory batch must match the command sequence." + ) + if self.joint_trajectory.waypoint_count != self.commands.frame_count: + raise ValueError( + "joint_trajectory waypoints must match command sequence frames." + ) + if not torch.equal(self.joint_trajectory.env_ids, self.commands.env_ids): + raise ValueError( + "joint_trajectory env_ids must match the command sequence." + ) + 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 + ): + raise ValueError( + "joint_position feedback requires an owned joint_trajectory." + ) + if self.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION: + if bool(self.plan_success.any().item()) and self.commands.frame_count == 0: + raise ValueError( + "joint_position feedback 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: + raise ValueError( + "joint_position feedback requires at least one endpoint " + f"command in frame {frame_index}." + ) + if any( + not isinstance(command.target, JointPositionTarget) + or not isinstance(command.payload, JointPositionPayload) + for command in frame.commands + ): + raise ValueError( + "joint_position feedback accepts only JointPositionTarget " + "and JointPositionPayload commands." + ) + 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: + raise ValueError( + "joint_position feedback requires a stable joint endpoint " + "set across every command frame." + ) if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") if self.planned_scene_version < 0: raise ValueError("planned_scene_version must be non-negative.") revisions = tuple(self.planned_collision_world_revision) - if len(revisions) != self.trajectory.batch_size: + if len(revisions) != self.commands.batch_size: raise ValueError( "planned_collision_world_revision must contain one value per " - "trajectory environment." + "command-sequence environment." ) if any( isinstance(value, bool) or not isinstance(value, int) or value < 0 @@ -462,7 +651,7 @@ def __post_init__(self) -> None: 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.trajectory.waypoint_count + waypoint_count = self.commands.frame_count segments = tuple(self.segments) if not all(isinstance(segment, TrajectorySegment) for segment in segments): raise TypeError("segments must contain only TrajectorySegment values.") @@ -473,7 +662,7 @@ def __post_init__(self) -> None: raise ValueError("ActionPlan segment names must be unique.") if waypoint_count == 0: if segments: - raise ValueError("An empty trajectory cannot contain segments.") + raise ValueError("An empty command sequence cannot contain segments.") elif ( not segments or segments[0].start != 0 @@ -484,10 +673,20 @@ def __post_init__(self) -> None: ) ): raise ValueError( - "ActionPlan segments must cover the trajectory exactly without " + "ActionPlan segments must cover the command sequence exactly without " "gaps or overlaps." ) object.__setattr__(self, "plan_success", self.plan_success.clone()) + object.__setattr__(self, "commands", self.commands.snapshot()) + object.__setattr__( + self, + "joint_trajectory", + ( + None + if self.joint_trajectory is None + else self.joint_trajectory.snapshot() + ), + ) object.__setattr__(self, "planned_collision_world_revision", revisions) object.__setattr__(self, "scene_dependencies", dependencies) object.__setattr__(self, "segments", segments) @@ -516,9 +715,10 @@ def segment(self, name: str) -> TrajectorySegment: def segment_at(self, waypoint_index: int) -> TrajectorySegment: """Return the segment containing a global action waypoint index.""" - if waypoint_index < 0 or waypoint_index >= self.trajectory.waypoint_count: + if waypoint_index < 0 or waypoint_index >= self.commands.frame_count: raise IndexError( - f"waypoint_index {waypoint_index} is outside the action trajectory." + f"waypoint_index {waypoint_index} is outside the action command " + "sequence." ) for segment in self.segments: if segment.contains(waypoint_index): @@ -554,7 +754,12 @@ def action_waypoint_offset(self, action_index: int) -> int: f"action_index {action_index} is outside the compiled sequence." ) return sum( - plan.trajectory.waypoint_count for plan in self.action_plans[:action_index] + ( + 0 + if plan.joint_trajectory is None + else plan.joint_trajectory.waypoint_count + ) + for plan in self.action_plans[:action_index] ) def segment(self, action_index: int, name: str) -> TrajectorySegment: @@ -571,6 +776,7 @@ def segment(self, action_index: int, name: str) -> TrajectorySegment: __all__ = [ "ActionPlan", "CompiledTrajectory", + "ExecutionFeedbackMode", "PlannerDiagnostics", "TimedTrajectory", "TrajectorySegment", diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 83558c2bb..b8cbb0b42 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -27,7 +27,7 @@ from embodichain.utils.math import matrix_from_quat, pose_inv, quat_from_matrix from ..affordance import AntipodalAffordance -from ..bindings import ResolvedControlPart +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics from ..effects import StateDelta @@ -41,7 +41,6 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..requirements import ( - ActionBindingRoute, DisjointResourceSlots, DisjointSlotEndpoints, GRASP_CAPABILITY, @@ -159,10 +158,10 @@ def __post_init__(self) -> None: class _CoordinatedPickResources: """Invocation-bound control parts and compatible hand commands.""" - left_arm: ResolvedControlPart - right_arm: ResolvedControlPart - left_hand: ResolvedControlPart - right_hand: ResolvedControlPart + left_arm: JointPositionTarget + right_arm: JointPositionTarget + left_hand: JointPositionTarget + right_hand: JointPositionTarget left_hand_open_qpos: torch.Tensor left_hand_close_qpos: torch.Tensor right_hand_open_qpos: torch.Tensor @@ -350,8 +349,6 @@ class CoordinatedPickment( skill_id: ClassVar[str] = "coordinated_pickment" GoalType: ClassVar[type] = CoordinatedPickGoal OptionsType: ClassVar[type] = CoordinatedPickmentOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("left", "right") - end_effector_roles: ClassVar[tuple[str, ...]] = ("left", "right") binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=tuple( SkillResourceSlot( @@ -360,7 +357,6 @@ class CoordinatedPickment( SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({INVERSE_KINEMATICS_CAPABILITY}), - route=ActionBindingRoute("manipulator", role), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -369,7 +365,6 @@ class CoordinatedPickment( OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", role), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -422,16 +417,20 @@ def _resolve_resources( ) -> _CoordinatedPickResources: """Resolve left/right roles from robot control parts.""" binding = request.binding - left_arm = binding.manipulator("left") - right_arm = binding.manipulator("right") - left_hand = binding.end_effector("left") - right_hand = binding.end_effector("right") - if left_arm.name == right_arm.name: + left_motion = binding.endpoint("left", "motion") + right_motion = binding.endpoint("right", "motion") + left_grasp = binding.endpoint("left", "grasp") + right_grasp = binding.endpoint("right", "grasp") + left_arm = left_motion.require_target(JointPositionTarget) + right_arm = right_motion.require_target(JointPositionTarget) + left_hand = left_grasp.require_target(JointPositionTarget) + right_hand = right_grasp.require_target(JointPositionTarget) + if left_arm.control_part == right_arm.control_part: raise ValueError( "CoordinatedPickment left and right roles must use different " "manipulator control parts." ) - if left_hand.name == right_hand.name: + if left_hand.control_part == right_hand.control_part: raise ValueError( "CoordinatedPickment left and right roles must use different " "end-effector control parts." @@ -441,25 +440,25 @@ def _resolve_resources( right_arm=right_arm, left_hand=left_hand, right_hand=right_hand, - left_hand_open_qpos=left_hand.joint_positions( + left_hand_open_qpos=left_grasp.joint_positions( OPEN_COMMAND, n_envs=self.n_envs, device=self.device, dtype=torch.float32, ), - left_hand_close_qpos=left_hand.joint_positions( + left_hand_close_qpos=left_grasp.joint_positions( GRASP_COMMAND, n_envs=self.n_envs, device=self.device, dtype=torch.float32, ), - right_hand_open_qpos=right_hand.joint_positions( + right_hand_open_qpos=right_grasp.joint_positions( OPEN_COMMAND, n_envs=self.n_envs, device=self.device, dtype=torch.float32, ), - right_hand_close_qpos=right_hand.joint_positions( + right_hand_close_qpos=right_grasp.joint_positions( GRASP_COMMAND, n_envs=self.n_envs, device=self.device, @@ -775,12 +774,12 @@ def _plan_synchronized_object_motion( ) left_success, left_qpos = self.robot.compute_ik( pose=left_xpos, - name=resources.left_arm.name, + name=resources.left_arm.control_part, joint_seed=left_qpos_seed, ) right_success, right_qpos = self.robot.compute_ik( pose=right_xpos, - name=resources.right_arm.name, + name=resources.right_arm.control_part, joint_seed=right_qpos_seed, ) left_success = normalize_success_mask( @@ -788,7 +787,7 @@ def _plan_synchronized_object_motion( n_envs=self.n_envs, device=self.device, name=( - f"IK success for {resources.left_arm.name} object waypoint " + f"IK success for {resources.left_arm.control_part} object waypoint " f"{waypoint_idx}" ), ) @@ -797,17 +796,17 @@ def _plan_synchronized_object_motion( n_envs=self.n_envs, device=self.device, name=( - f"IK success for {resources.right_arm.name} object waypoint " + f"IK success for {resources.right_arm.control_part} object waypoint " f"{waypoint_idx}" ), ) self._log_ik_failures( - resources.left_arm.name, + resources.left_arm.control_part, f"object waypoint {waypoint_idx}", success_mask & ~left_success, ) self._log_ik_failures( - resources.right_arm.name, + resources.right_arm.control_part, f"object waypoint {waypoint_idx}", success_mask & ~right_success, ) @@ -883,14 +882,14 @@ def _plan( ) success_mask = grasp_success.clone() success_mask, left_approach_traj = self._plan_masked_arm_trajectory( - resources.left_arm.name, + resources.left_arm.control_part, left_start_qpos, left_approach_targets, segments["approach"], success_mask, ) success_mask, right_approach_traj = self._plan_masked_arm_trajectory( - resources.right_arm.name, + resources.right_arm.control_part, right_start_qpos, right_approach_targets, segments["approach"], @@ -1026,13 +1025,13 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.left_arm.name: None, - resources.right_arm.name: None, + resources.left_arm.control_part: None, + resources.right_arm.control_part: None, }, coordinated_held_object_updates={ ( - resources.left_arm.name, - resources.right_arm.name, + resources.left_arm.control_part, + resources.right_arm.control_part, ): coordinated_held_object, }, ), diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index e7945d018..c00771e02 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -26,7 +26,7 @@ from embodichain.utils import logger from ._helpers import resolve_object_target -from ..bindings import ResolvedControlPart +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction from ..effects import StateDelta @@ -35,7 +35,6 @@ from ..plans import ActionPlan, normalize_success_mask from ..policies import MotionPolicy from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, DisjointSlotEndpoints, @@ -123,10 +122,10 @@ def __post_init__(self) -> None: class _CoordinatedPlacementResources: """Invocation-bound control parts and compatible hand commands.""" - placing_arm: ResolvedControlPart - support_arm: ResolvedControlPart - placing_hand: ResolvedControlPart - support_hand: ResolvedControlPart + placing_arm: JointPositionTarget + support_arm: JointPositionTarget + placing_hand: JointPositionTarget + support_hand: JointPositionTarget placing_hand_open_qpos: torch.Tensor placing_hand_close_qpos: torch.Tensor support_hand_close_qpos: torch.Tensor @@ -140,8 +139,6 @@ class CoordinatedPlacement( skill_id: ClassVar[str] = "coordinated_placement" GoalType: ClassVar[type] = CoordinatedPlacementGoal OptionsType: ClassVar[type] = CoordinatedPlacementOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("placing", "support") - end_effector_roles: ClassVar[tuple[str, ...]] = ("placing", "support") binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -150,7 +147,6 @@ class CoordinatedPlacement( SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "placing"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -159,7 +155,6 @@ class CoordinatedPlacement( OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "placing"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -170,13 +165,11 @@ class CoordinatedPlacement( SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "support"), ), SkillEndpointRequirement( endpoint_id="grasp", capabilities=frozenset({GRASP_CAPABILITY}), required_commands={GRASP_COMMAND: JointPositionCommand}, - route=ActionBindingRoute("end_effector", "support"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -204,16 +197,20 @@ def _resolve_resources( ) -> _CoordinatedPlacementResources: """Resolve placing/support roles from robot control parts.""" binding = request.binding - placing_arm = binding.manipulator("placing") - support_arm = binding.manipulator("support") - placing_hand = binding.end_effector("placing") - support_hand = binding.end_effector("support") - if placing_arm.name == support_arm.name: + placing_motion = binding.endpoint("placing", "motion") + support_motion = binding.endpoint("support", "motion") + placing_grasp = binding.endpoint("placing", "grasp") + support_grasp = binding.endpoint("support", "grasp") + placing_arm = placing_motion.require_target(JointPositionTarget) + support_arm = support_motion.require_target(JointPositionTarget) + placing_hand = placing_grasp.require_target(JointPositionTarget) + support_hand = support_grasp.require_target(JointPositionTarget) + if placing_arm.control_part == support_arm.control_part: raise ValueError( "CoordinatedPlacement placing and support roles must use " "different manipulator control parts." ) - if placing_hand.name == support_hand.name: + if placing_hand.control_part == support_hand.control_part: raise ValueError( "CoordinatedPlacement placing and support roles must use " "different end-effector control parts." @@ -223,19 +220,19 @@ def _resolve_resources( support_arm=support_arm, placing_hand=placing_hand, support_hand=support_hand, - placing_hand_open_qpos=placing_hand.joint_positions( + placing_hand_open_qpos=placing_grasp.joint_positions( OPEN_COMMAND, n_envs=self.n_envs, device=self.device, dtype=torch.float32, ), - placing_hand_close_qpos=placing_hand.joint_positions( + placing_hand_close_qpos=placing_grasp.joint_positions( GRASP_COMMAND, n_envs=self.n_envs, device=self.device, dtype=torch.float32, ), - support_hand_close_qpos=support_hand.joint_positions( + support_hand_close_qpos=support_grasp.joint_positions( GRASP_COMMAND, n_envs=self.n_envs, device=self.device, @@ -291,7 +288,7 @@ def _plan( device=self.device, ) segment_success, placing_approach_traj = self._plan_named_arm_trajectory( - resources.placing_arm.name, + resources.placing_arm.control_part, placing_start_qpos, torch.stack([placing_lift_xpos, placing_xpos], dim=1), segments["approach"], @@ -310,7 +307,7 @@ def _plan( ) segment_success, support_approach_traj = self._plan_named_arm_trajectory( - resources.support_arm.name, + resources.support_arm.control_part, support_start_qpos, support_xpos.unsqueeze(1), segments["approach"], @@ -368,7 +365,7 @@ def _plan( ) segment_success, placing_retreat_traj = self._plan_named_arm_trajectory( - resources.placing_arm.name, + resources.placing_arm.control_part, placing_place_qpos, placing_lift_xpos.unsqueeze(1), segments["retreat"], @@ -408,8 +405,8 @@ def _plan( dim=1, ) involved_control_parts = { - resources.placing_arm.name, - resources.support_arm.name, + resources.placing_arm.control_part, + resources.support_arm.control_part, } coordinated_removals = { key: None @@ -423,10 +420,10 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.placing_arm.name: ( + resources.placing_arm.control_part: ( None if release else placing_held_object ), - resources.support_arm.name: support_held_object, + resources.support_arm.control_part: support_held_object, }, coordinated_held_object_updates=coordinated_removals, ), @@ -510,8 +507,8 @@ def _resolve_target( HeldObjectState, HeldObjectState, ]: - placing_control_part = resources.placing_arm.name - support_control_part = resources.support_arm.name + 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) if placing_held_object is None: logger.log_error( diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 4e6b87e3c..02d8cf0ec 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -26,7 +26,7 @@ from embodichain.utils import logger from embodichain.utils.math import pose_inv -from ..bindings import ResolvedControlPart +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics, _same_object_identity from ..effects import StateDelta @@ -34,7 +34,6 @@ from ..plans import ActionPlan, normalize_success_mask from ..policies import MotionPolicy from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, DisjointSlotEndpoints, @@ -124,10 +123,10 @@ def __post_init__(self) -> None: class _HandOverResources: """Invocation-bound control parts and compatible hand commands.""" - transfer_arm: ResolvedControlPart - receive_arm: ResolvedControlPart - transfer_hand: ResolvedControlPart - receive_hand: ResolvedControlPart + transfer_arm: JointPositionTarget + receive_arm: JointPositionTarget + transfer_hand: JointPositionTarget + receive_hand: JointPositionTarget transfer_hand_open_qpos: torch.Tensor transfer_hand_close_qpos: torch.Tensor receive_hand_open_qpos: torch.Tensor @@ -146,8 +145,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): skill_id: ClassVar[str] = "hand_over" GoalType: ClassVar[type] = GraspGoal OptionsType: ClassVar[type] = HandOverOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("source", "destination") - end_effector_roles: ClassVar[tuple[str, ...]] = ("source", "destination") binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -161,7 +158,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): FORWARD_KINEMATICS_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "source"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -170,7 +166,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "source"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -181,7 +176,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "destination"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -190,7 +184,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "destination"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -224,16 +217,20 @@ def _resolve_resources( ) -> _HandOverResources: """Resolve source/destination roles from robot control parts.""" binding = request.binding - transfer_arm = binding.manipulator("source") - receive_arm = binding.manipulator("destination") - transfer_hand = binding.end_effector("source") - receive_hand = binding.end_effector("destination") - if transfer_arm.name == receive_arm.name: + transfer_motion = binding.endpoint("source", "motion") + receive_motion = binding.endpoint("destination", "motion") + transfer_grasp = binding.endpoint("source", "grasp") + receive_grasp = binding.endpoint("destination", "grasp") + transfer_arm = transfer_motion.require_target(JointPositionTarget) + receive_arm = receive_motion.require_target(JointPositionTarget) + transfer_hand = transfer_grasp.require_target(JointPositionTarget) + receive_hand = receive_grasp.require_target(JointPositionTarget) + if transfer_arm.control_part == receive_arm.control_part: raise ValueError( "HandOver source and destination must use different manipulator " "control parts." ) - if transfer_hand.name == receive_hand.name: + if transfer_hand.control_part == receive_hand.control_part: raise ValueError( "HandOver source and destination must use different end-effector " "control parts." @@ -243,25 +240,25 @@ def _resolve_resources( receive_arm=receive_arm, transfer_hand=transfer_hand, receive_hand=receive_hand, - transfer_hand_open_qpos=transfer_hand.joint_positions( + transfer_hand_open_qpos=transfer_grasp.joint_positions( OPEN_COMMAND, n_envs=self.n_envs, device=self.device, dtype=torch.float32, ), - transfer_hand_close_qpos=transfer_hand.joint_positions( + transfer_hand_close_qpos=transfer_grasp.joint_positions( GRASP_COMMAND, n_envs=self.n_envs, device=self.device, dtype=torch.float32, ), - receive_hand_open_qpos=receive_hand.joint_positions( + receive_hand_open_qpos=receive_grasp.joint_positions( OPEN_COMMAND, n_envs=self.n_envs, device=self.device, dtype=torch.float32, ), - receive_hand_close_qpos=receive_hand.joint_positions( + receive_hand_close_qpos=receive_grasp.joint_positions( GRASP_COMMAND, n_envs=self.n_envs, device=self.device, @@ -294,7 +291,7 @@ def _plan( semantics = target.semantics transfer_object_to_eef = self._resolve_transfer_object_to_eef( state, - resources.transfer_arm.name, + resources.transfer_arm.control_part, semantics, ) transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( @@ -320,7 +317,7 @@ def _plan( # attachment and the transferring arm's current measured pose. transfer_current_eef = self.robot.compute_fk( qpos=transfer_start_qpos, - name=resources.transfer_arm.name, + name=resources.transfer_arm.control_part, to_matrix=True, ) current_object_pose = torch.bmm( @@ -374,7 +371,7 @@ def _plan( ) segment_success, transfer_move_traj = self._plan_named_arm_trajectory( - resources.transfer_arm.name, + resources.transfer_arm.control_part, transfer_start_qpos, transfer_middle_eef.unsqueeze(1), segments["transfer"], @@ -391,7 +388,7 @@ def _plan( return self.failed_plan(request, context, message="Transfer move failed.") segment_success, receive_approach_traj = self._plan_named_arm_trajectory( - resources.receive_arm.name, + resources.receive_arm.control_part, receive_start_qpos, torch.stack([receive_pre_grasp_eef, receive_grasp_xpos], dim=1), segments["approach"], @@ -413,7 +410,7 @@ def _plan( receive_grasp_qpos = receive_approach_traj[:, -1] segment_success, transfer_retreat_traj = self._plan_named_arm_trajectory( - resources.transfer_arm.name, + resources.transfer_arm.control_part, transfer_hold_qpos, transfer_retreat_eef.unsqueeze(1), segments["deliver"], @@ -432,7 +429,7 @@ def _plan( ) segment_success, receive_deliver_traj = self._plan_named_arm_trajectory( - resources.receive_arm.name, + resources.receive_arm.control_part, receive_grasp_qpos, receive_final_eef.unsqueeze(1), segments["deliver"], @@ -568,8 +565,8 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.transfer_arm.name: None, - resources.receive_arm.name: held_object, + resources.transfer_arm.control_part: None, + resources.receive_arm.control_part: held_object, } ), segment_lengths=segment_lengths, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py index d842db5b3..382607b06 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py @@ -23,12 +23,12 @@ import torch +from ..bindings import JointPositionTarget from ..core import AtomicAction from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, SkillBindingContract, SkillEndpointRequirement, @@ -73,14 +73,12 @@ class MoveEndEffector(AtomicAction[EndEffectorPoseGoal, MoveEndEffectorOptions]) SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), ), ) OptionsType: ClassVar[type] = MoveEndEffectorOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) def __init__( self, @@ -95,9 +93,11 @@ def _plan( ) -> ActionPlan: """Plan an end-effector pose goal from the observed joint state.""" goal = self.require_goal(request) - manipulator = request.binding.manipulator("primary") - control_part = manipulator.name - joint_ids = list(manipulator.joint_ids) + motion_target = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + control_part = motion_target.control_part + joint_ids = list(motion_target.joint_ids) move_xpos = resolve_pose_target( resolve_pose_goal(goal.xpos, context, name="xpos"), n_envs=context.batch_size, 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 9fca6f256..7cb43860e 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -31,13 +31,13 @@ ) from ._helpers import arm_qpos_from_state, resolve_object_target +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 from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, @@ -97,8 +97,6 @@ class MoveHeldObject(AtomicAction[HeldObjectPoseGoal, MoveHeldObjectOptions]): skill_id: ClassVar[str] = "move_held_object" GoalType: ClassVar[type] = HeldObjectPoseGoal OptionsType: ClassVar[type] = MoveHeldObjectOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -112,13 +110,11 @@ class MoveHeldObject(AtomicAction[HeldObjectPoseGoal, MoveHeldObjectOptions]): FORWARD_KINEMATICS_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( endpoint_id="grasp", capabilities=frozenset({GRASP_CAPABILITY}), required_commands={GRASP_COMMAND: JointPositionCommand}, - route=ActionBindingRoute("end_effector", "primary"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -146,12 +142,14 @@ def _plan( target = self.require_goal(request) options = request.skill_options binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - control_part = manipulator.name - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) - hand_grasp_qpos = end_effector.joint_positions( + motion = binding.endpoint("primary", "motion") + grasp = binding.endpoint("primary", "grasp") + motion_target = motion.require_target(JointPositionTarget) + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, n_envs=context.batch_size, device=self.device, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index a06eed0fc..ffa7876eb 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -23,11 +23,11 @@ import torch +from ..bindings import JointPositionTarget from ..core import AtomicAction from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, JOINT_POSITION_CAPABILITY, SkillBindingContract, SkillEndpointRequirement, @@ -80,7 +80,6 @@ class MoveJoints(AtomicAction[JointPositionGoal, MoveJointsOptions]): skill_id: ClassVar[str] = "move_joints" GoalType: ClassVar[type] = JointPositionGoal OptionsType: ClassVar[type] = MoveJointsOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) agent_visible: ClassVar[bool] = False binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( @@ -90,7 +89,6 @@ class MoveJoints(AtomicAction[JointPositionGoal, MoveJointsOptions]): SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({JOINT_POSITION_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), @@ -110,10 +108,11 @@ def _plan( ) -> ActionPlan: """Plan a joint-space goal without mutating the robot or task state.""" goal = self.require_goal(request) - manipulator = request.binding.manipulator("primary") - control_part = manipulator.name - joint_ids = list(manipulator.joint_ids) - joint_dof = manipulator.dof + motion = request.binding.endpoint("primary", "motion") + motion_target = motion.require_target(JointPositionTarget) + control_part = motion_target.control_part + joint_ids = list(motion_target.joint_ids) + joint_dof = len(motion_target.joint_ids) target_qpos = resolve_joint_target( self._resolve_target_qpos( goal, @@ -157,7 +156,7 @@ def _resolve_target_qpos( """Resolve an explicit or named joint goal to a tensor.""" if isinstance(goal.target, torch.Tensor): return goal.target - return request.binding.manipulator("primary").joint_positions( + return request.binding.endpoint("primary", "motion").joint_positions( goal.target, n_envs=context.batch_size, device=self.device, diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index 34c183814..36ee0cc8d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -34,7 +34,7 @@ from ._helpers import arm_qpos_from_state from ..affordance import AntipodalAffordance -from ..bindings import ResolvedControlPart +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics from ..effects import StateDelta @@ -49,7 +49,6 @@ from ..plans import ActionPlan, normalize_success_mask from ..policies import MotionPolicy from ..requirements import ( - ActionBindingRoute, BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, DisjointSlotEndpoints, @@ -164,8 +163,6 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): skill_id: ClassVar[str] = "pick_up" GoalType: ClassVar[type] = GraspGoal OptionsType: ClassVar[type] = PickUpOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -180,7 +177,6 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): FORWARD_KINEMATICS_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -189,7 +185,6 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "primary"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -227,8 +222,8 @@ def _get_full_pickup_trajectory( motion_policy: MotionPolicy, options: PickUpOptions, approach_direction: torch.Tensor, - manipulator: ResolvedControlPart, - end_effector: ResolvedControlPart, + manipulator: JointPositionTarget, + end_effector: JointPositionTarget, hand_open_qpos: torch.Tensor, hand_grasp_qpos: torch.Tensor, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: @@ -247,7 +242,7 @@ def _get_full_pickup_trajectory( build_pose_plan_states(torch.stack([pre_grasp_xpos, grasp_xpos], dim=1)), options=motion_policy.to_motion_gen_options( start_qpos=start_arm_qpos, - control_part=manipulator.name, + control_part=manipulator.control_part, sample_count=n_approach, ), ) @@ -265,7 +260,7 @@ def _get_full_pickup_trajectory( build_pose_plan_states(lift_xpos), options=motion_policy.to_motion_gen_options( start_qpos=grasp_arm_qpos, - control_part=manipulator.name, + control_part=manipulator.control_part, sample_count=n_lift, ), ) @@ -325,21 +320,23 @@ def _plan( approach_direction ) binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - hand_open_qpos = end_effector.joint_positions( + motion = binding.endpoint("primary", "motion") + grasp = binding.endpoint("primary", "grasp") + manipulator = motion.require_target(JointPositionTarget) + end_effector = grasp.require_target(JointPositionTarget) + hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, n_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - hand_grasp_qpos = end_effector.joint_positions( + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, n_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - control_part = manipulator.name + control_part = manipulator.control_part state = context sem = target.semantics object_pose = _resolve_object_pose( @@ -435,7 +432,7 @@ def _resolve_grasp_pose( semantics: ObjectSemantics, object_pose: torch.Tensor, start_qpos: torch.Tensor, - manipulator: ResolvedControlPart, + manipulator: JointPositionTarget, options: PickUpOptions, approach_direction: torch.Tensor, ) -> tuple[torch.Tensor, torch.Tensor]: @@ -488,7 +485,7 @@ def _select_feasible_grasp_variants( grasp_xpos: torch.Tensor, start_qpos: torch.Tensor, object_poses: torch.Tensor, - manipulator: ResolvedControlPart, + manipulator: JointPositionTarget, options: PickUpOptions, approach_direction: torch.Tensor, ) -> tuple[torch.Tensor, torch.Tensor]: @@ -571,7 +568,7 @@ def _select_feasible_grasp_variants( start_xpos = self.robot.compute_fk( qpos=start_qpos, - name=manipulator.name, + name=manipulator.control_part, to_matrix=True, ) start_quat = quat_from_matrix(start_xpos[:, :3, :3]) @@ -616,22 +613,23 @@ def _compute_batch_candidate_ik( self, poses: torch.Tensor, joint_seed: torch.Tensor, - manipulator: ResolvedControlPart, + manipulator: JointPositionTarget, ) -> tuple[torch.Tensor, torch.Tensor]: """Solve candidate IK poses while preserving the candidate dimensions.""" n_envs, n_pose, n_variant = poses.shape[:3] flat_poses = poses.reshape(n_envs, n_pose * n_variant, 4, 4) if joint_seed.dim() == 2: joint_seed = joint_seed[:, None, None, :].expand(-1, n_pose, n_variant, -1) - flat_seed = joint_seed.reshape(n_envs, n_pose * n_variant, manipulator.dof) + manipulator_dof = len(manipulator.joint_ids) + flat_seed = joint_seed.reshape(n_envs, n_pose * n_variant, manipulator_dof) is_success, qpos = self.robot.compute_batch_ik( pose=flat_poses, - name=manipulator.name, + name=manipulator.control_part, joint_seed=flat_seed, ) return ( is_success.reshape(n_envs, n_pose, n_variant), - qpos.reshape(n_envs, n_pose, n_variant, manipulator.dof), + qpos.reshape(n_envs, n_pose, n_variant, manipulator_dof), ) def _upright_adjusted_grasp_poses( diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index 3edf8ce70..b4678dd02 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -29,6 +29,7 @@ from ._helpers import arm_qpos_from_state, resolve_object_target from ..affordance import AssembleAffordance +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction from ..effects import StateDelta @@ -41,7 +42,6 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, @@ -170,8 +170,6 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): AssembleGoal, ) OptionsType: ClassVar[type] = PlaceOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -185,7 +183,6 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): FORWARD_KINEMATICS_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -194,7 +191,6 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "primary"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -233,18 +229,21 @@ def _plan( target = self.require_goal(request) options = request.skill_options binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - control_part = manipulator.name - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) - hand_open_qpos = end_effector.joint_positions( + motion_target = binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + grasp = binding.endpoint("primary", "grasp") + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) + hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, n_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - hand_grasp_qpos = end_effector.joint_positions( + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, n_envs=context.batch_size, device=self.device, diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index eadcb425f..46fa15369 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -26,13 +26,13 @@ from embodichain.utils import logger from ._helpers import arm_qpos_from_state +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 from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointSlotEndpoints, GRASP_CAPABILITY, @@ -81,8 +81,6 @@ class Press(AtomicAction[PressGoal, PressOptions]): skill_id: ClassVar[str] = "press" GoalType: ClassVar[type] = PressGoal OptionsType: ClassVar[type] = PressOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -96,13 +94,11 @@ class Press(AtomicAction[PressGoal, PressOptions]): JOINT_POSITION_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( endpoint_id="grasp", capabilities=frozenset({GRASP_CAPABILITY}), required_commands={GRASP_COMMAND: JointPositionCommand}, - route=ActionBindingRoute("end_effector", "primary"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -130,12 +126,15 @@ def _plan( target = self.require_goal(request) options = request.skill_options binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - control_part = manipulator.name - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) - hand_close_qpos = end_effector.joint_positions( + motion_target = binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + grasp = binding.endpoint("primary", "grasp") + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) + hand_close_qpos = grasp.joint_positions( GRASP_COMMAND, n_envs=self.n_envs, device=self.device, diff --git a/embodichain/lab/sim/atomic_actions/requirements.py b/embodichain/lab/sim/atomic_actions/requirements.py index 1e12aa610..7b62233d2 100644 --- a/embodichain/lab/sim/atomic_actions/requirements.py +++ b/embodichain/lab/sim/atomic_actions/requirements.py @@ -20,7 +20,7 @@ from dataclasses import dataclass, field from types import MappingProxyType -from typing import Literal, Mapping +from typing import Mapping from .control import ControlCommand @@ -69,34 +69,6 @@ def _normalize_identifiers( return normalized -@dataclass(frozen=True, slots=True) -class ActionBindingRoute: - """Lower one generic resource endpoint into the current action core. - - This is deliberately a transition adapter. Robot resources and skill-local - slots remain generic; only this route names the two maps currently exposed - by :class:`~embodichain.lab.sim.atomic_actions.ActionBinding`. - """ - - target: Literal["manipulator", "end_effector"] - """Current core binding namespace.""" - - role: str - """Action-local role within the selected namespace.""" - - def __post_init__(self) -> None: - if self.target not in ("manipulator", "end_effector"): - raise ValueError( - "ActionBindingRoute.target must be 'manipulator' or 'end_effector'." - ) - _validate_identifier(self.role, field_name="ActionBindingRoute.role") - - @property - def key(self) -> tuple[str, str]: - """Return the normalized core target key.""" - return self.target, self.role - - def _normalize_required_commands( values: Mapping[str, type[ControlCommand]], ) -> Mapping[str, type[ControlCommand]]: @@ -129,9 +101,6 @@ class SkillEndpointRequirement: required_commands: Mapping[str, type[ControlCommand]] = field(default_factory=dict) """Semantic command names and their required typed command contracts.""" - route: ActionBindingRoute | None = None - """Optional lowering route into the current atomic-action core.""" - def __post_init__(self) -> None: _validate_identifier( self.endpoint_id, @@ -150,8 +119,6 @@ def __post_init__(self) -> None: "required_commands", _normalize_required_commands(self.required_commands), ) - if self.route is not None and not isinstance(self.route, ActionBindingRoute): - raise TypeError("route must be an ActionBindingRoute or None.") @dataclass(frozen=True, slots=True) @@ -324,14 +291,6 @@ def __post_init__(self) -> None: f"Resource constraint references unknown slots {unknown}; " f"known slots are {sorted(known_slots)}." ) - routes = [ - endpoint.route.key - for slot in slots - for endpoint in slot.endpoints - if endpoint.route is not None - ] - if len(set(routes)) != len(routes): - raise ValueError("Action binding routes must target unique core roles.") object.__setattr__(self, "slots", slots) object.__setattr__(self, "constraints", constraints) @@ -340,32 +299,8 @@ def slot_ids(self) -> tuple[str, ...]: """Return required slot identifiers in declaration order.""" return tuple(slot.slot_id for slot in self.slots) - def validate_action_roles( - self, - *, - manipulator_roles: tuple[str, ...], - end_effector_roles: tuple[str, ...], - ) -> None: - """Require lowering routes to cover the current core roles exactly.""" - expected = {("manipulator", role) for role in manipulator_roles} - expected.update(("end_effector", role) for role in end_effector_roles) - actual = { - endpoint.route.key - for slot in self.slots - for endpoint in slot.endpoints - if endpoint.route is not None - } - if actual != expected: - missing = sorted(expected - actual) - extra = sorted(actual - expected) - raise ValueError( - "Skill binding routes do not exactly cover the action roles: " - f"missing={missing}, extra={extra}." - ) - __all__ = [ - "ActionBindingRoute", "BATCH_INVERSE_KINEMATICS_CAPABILITY", "CARTESIAN_POSE_CAPABILITY", "DisjointResourceSlots", diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 38c504136..063bd1fa5 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -29,12 +29,14 @@ from embodichain.utils import configclass +from .bindings import RuntimeEndpointTarget from .execution import ( ExecutionSession, ExecutionStatus, ExecutionTick, - JointCommand, ) +from .invocation import ActionInvocation, ResolvedActionRequest +from .runtime_commands import RuntimeCommandFrame from .state import PlanningContext, TaskState @@ -123,15 +125,17 @@ class CommandSink(Protocol): def send( self, - command: JointCommand, + command: RuntimeCommandFrame, *, timeout: float, ) -> CommandAcknowledgement: - """Submit an active joint command and acknowledge its acceptance. + """Submit one synchronized endpoint-command frame. Args: - command: Full-robot command with an explicit active mask. Inactive - rows contain hold targets and must not retain stale commands. + command: Transport-neutral command frame with an active-row mask. + The sink must actively neutralize inactive rows for every + addressed target; omission is not a safe state for persistent + controllers. timeout: Maximum acknowledgement latency in seconds. Returns: @@ -140,24 +144,32 @@ def send( def hold( self, - command: JointCommand, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, *, timeout: float, ) -> CommandAcknowledgement: - """Hold the supplied observed position as a safety command. + """Apply transport-specific safe state to the supplied targets. Args: - command: Full-robot observed-position hold command. + targets: Runtime targets that may retain controller state. + context: Latest observation used by position-hold transports. timeout: Maximum acknowledgement latency in seconds. Returns: Transport or controller acknowledgement. """ - def cancel(self, *, timeout: float) -> CommandAcknowledgement: + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: """Cancel any controller-side command that has not completed. Args: + targets: Runtime targets whose queued work must be cancelled. timeout: Maximum acknowledgement latency in seconds. Returns: @@ -290,7 +302,8 @@ class ExecutionRunner: """Connect an execution session to observation, controller, and time ports. :meth:`step` is non-blocking. It observes and advances the session only when - the next command is due according to :attr:`JointCommand.hold_duration`. + the next command is due according to + :attr:`RuntimeCommandFrame.hold_duration`. :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. @@ -334,10 +347,16 @@ def __init__( self._message: str | None = None self._effect_context: PlanningContext | None = None self._effect_tick: ExecutionTick | None = None + self._armed_targets: dict[tuple[str, str], RuntimeEndpointTarget] = {} + self._pending_revision: ResolvedActionRequest | None = None @property def session(self) -> ExecutionSession: - """Execution session advanced by this runner.""" + """Execution session advanced by this runner. + + Call :meth:`revise_current` on the runner, rather than mutating the + session directly, while this runner owns scheduling. + """ return self._session @property @@ -358,6 +377,39 @@ def effect_verification_pending(self) -> bool: and self._effect_tick.pending_effect is not None ) + def revise_current(self, invocation: ActionInvocation) -> None: + """Stage a newer revision for the next scheduled observation boundary. + + Staging preserves the active frame deadline. When that deadline is due, + :meth:`step` observes fresh state, atomically plans and installs the + replacement, and dispatches its first command. The submitted invocation + is resolved into an owned snapshot immediately, so later caller + mutation cannot alter the staged revision. + + Args: + invocation: Strictly newer revision of the active logical call. + + Raises: + TypeError: If ``invocation`` is not an ActionInvocation. + RuntimeError: If this runner or its session is no longer running, + or if a physical effect is awaiting verification. + ValueError: If session-level revision invariants are violated. + """ + if not isinstance(invocation, ActionInvocation): + raise TypeError("invocation must be an ActionInvocation.") + if self._status is not RunnerStatus.RUNNING: + raise RuntimeError("Only a running execution runner can be revised.") + prepared = self._session._prepare_revision(invocation) + if ( + self._pending_revision is not None + and prepared.revision <= self._pending_revision.revision + ): + raise ValueError( + "A staged revision must advance beyond the pending revision " + f"{self._pending_revision.revision}, got {prepared.revision}." + ) + self._pending_revision = prepared + def step( self, *, @@ -398,6 +450,12 @@ 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 tick = self._session.tick(context, effect_success=effect_success) except Exception as exc: return self._fail( @@ -408,12 +466,18 @@ def step( dispatches: list[CommandDispatch] = [] if tick.command is not None: + self._remember_targets(tick.command.targets) operation = ( CommandOperation.SEND if bool(tick.command.active_mask.any().item()) else CommandOperation.HOLD ) - dispatch = self._dispatch(operation, tick.command) + dispatch = self._dispatch( + operation, + command=(tick.command if operation is CommandOperation.SEND else None), + targets=tick.command.targets, + context=context, + ) dispatches.append(dispatch) if not dispatch.acknowledgement.accepted: failure = dispatch.acknowledgement @@ -433,6 +497,29 @@ def step( self._command_count += 1 interval = self._command_interval(tick.command) self._next_step_at = self._clock_now() + interval + elif tick.hold_targets: + self._remember_targets(tick.hold_targets) + hold_dispatch = self._dispatch( + CommandOperation.HOLD, + targets=tick.hold_targets, + context=context, + ) + dispatches.append(hold_dispatch) + if not hold_dispatch.acknowledgement.accepted: + failure = hold_dispatch.acknowledgement + message = ( + "Controller did not accept the requested hold: " + f"{failure.status.value}." + ) + if failure.message: + message += f" {failure.message}" + return self._fail( + message, + context=context, + tick=tick, + dispatches=dispatches, + ) + self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time else: self._next_step_at = self._clock_now() @@ -440,7 +527,8 @@ def step( if self.cfg.hold_on_completion: hold_dispatch = self._dispatch( CommandOperation.HOLD, - self._hold_command(context), + targets=self._armed_target_snapshots(), + context=context, ) dispatches.append(hold_dispatch) if not hold_dispatch.acknowledgement.accepted: @@ -499,6 +587,7 @@ def cancel(self, reason: str = "Execution cancelled by caller.") -> RunnerStep: self._status = RunnerStatus.FAILED self._message = f"{reason} Safe stop acknowledgement failed." self._clear_effect_boundary() + self._pending_revision = None self._next_step_at = self._clock_now() return self._result( timestamp=self._clock_now(), @@ -630,7 +719,7 @@ def _clock_now(self) -> float: raise ValueError("ExecutionClock.now() must be finite and non-negative.") return value - def _command_interval(self, command: JointCommand) -> float: + def _command_interval(self, command: RuntimeCommandFrame) -> float: """Resolve a synchronized batch interval from per-environment durations.""" durations = ( command.hold_duration[command.active_mask] @@ -649,27 +738,31 @@ def _remaining_wait(self, now: float) -> float: def _dispatch( self, operation: CommandOperation, - command: JointCommand | None, + command: RuntimeCommandFrame | None = None, + *, + targets: tuple[RuntimeEndpointTarget, ...] = (), + context: PlanningContext | None = None, ) -> CommandDispatch: """Call one sink operation and convert exceptions to rejection acks.""" try: if operation is CommandOperation.SEND: if command is None: - raise ValueError("SEND requires a JointCommand.") + raise ValueError("SEND requires a RuntimeCommandFrame.") acknowledgement = self._command_sink.send( command, timeout=self.cfg.command_timeout, ) elif operation is CommandOperation.HOLD: - if command is None: - raise ValueError("HOLD requires a JointCommand.") + if context is None: + raise ValueError("HOLD requires a PlanningContext.") acknowledgement = self._command_sink.hold( - command, + targets, + context, timeout=self.cfg.safe_stop_timeout, ) else: acknowledgement = self._command_sink.cancel( - timeout=self.cfg.safe_stop_timeout + targets, timeout=self.cfg.safe_stop_timeout ) if not isinstance(acknowledgement, CommandAcknowledgement): raise TypeError( @@ -682,6 +775,19 @@ def _dispatch( ) return CommandDispatch(operation, acknowledgement) + def _remember_targets( + self, + targets: tuple[RuntimeEndpointTarget, ...], + ) -> None: + """Remember every controller destination armed during this run.""" + for target in targets: + key = (target.transport_id, target.target_id) + self._armed_targets[key] = target.snapshot() + + def _armed_target_snapshots(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return owned armed targets in first-use order.""" + return tuple(target.snapshot() for target in self._armed_targets.values()) + def _observe_for_stop(self) -> PlanningContext | None: """Best-effort observation used to build a cancellation hold command.""" try: @@ -698,32 +804,18 @@ def _safe_stop( context: PlanningContext | None, ) -> list[CommandDispatch]: """Attempt controller cancellation followed by an observed-position hold.""" - dispatches = [self._dispatch(CommandOperation.CANCEL, None)] + targets = self._armed_target_snapshots() + dispatches = [self._dispatch(CommandOperation.CANCEL, targets=targets)] if context is not None: dispatches.append( - self._dispatch(CommandOperation.HOLD, self._hold_command(context)) + self._dispatch( + CommandOperation.HOLD, + targets=targets, + context=context, + ) ) return dispatches - @staticmethod - def _hold_command(context: PlanningContext) -> JointCommand: - """Build an all-environment passive hold command from an observation.""" - return JointCommand( - positions=context.robot.qpos, - velocities=torch.zeros_like(context.robot.qpos), - active_mask=torch.zeros( - context.batch_size, - dtype=torch.bool, - device=context.robot.qpos.device, - ), - env_ids=context.env_ids, - hold_duration=torch.zeros( - context.batch_size, - dtype=torch.float32, - device=context.robot.qpos.device, - ), - ) - def _fail( self, message: str, @@ -738,6 +830,7 @@ def _fail( self._status = RunnerStatus.FAILED self._message = message self._clear_effect_boundary() + self._pending_revision = None self._next_step_at = self._clock_now() return self._result( timestamp=self._clock_now(), diff --git a/embodichain/lab/sim/atomic_actions/runtime.py b/embodichain/lab/sim/atomic_actions/runtime.py index b8a62d9d0..c0530db40 100644 --- a/embodichain/lab/sim/atomic_actions/runtime.py +++ b/embodichain/lab/sim/atomic_actions/runtime.py @@ -21,16 +21,18 @@ from collections.abc import Mapping from types import MappingProxyType from typing import TYPE_CHECKING +from uuid import uuid4 import torch -from .bindings import ActionBinding, ResolvedActionBinding, ResolvedControlPart -from .control import ( - ActionControlOverrides, - ControlCommand, - ControlPartCommandProfile, -) +from .bindings import ActionBinding, EndpointBinding, JointPositionTarget +from .control import ActionControlOverrides, ControlPartCommandProfile from .core import resolve_runtime_device +from .requirements import ( + DisjointResourceSlots, + DisjointSlotEndpoints, + SkillBindingContract, +) if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -38,18 +40,7 @@ class ActionPlanningServices: - """Planning resources exclusively owned by one atomic-action engine. - - An action may borrow these resources after the engine binds it, but callers - never pass a motion generator to individual actions. Keeping the generator - here gives one engine a single planner backend, robot, device, cache, and - collision-world owner. - - Args: - motion_generator: Motion generator owned by the engine. - control_profiles: Semantic command profiles keyed by names from the - owned robot's ``control_parts`` mapping. - """ + """Planning resources exclusively owned by one atomic-action engine.""" def __init__( self, @@ -59,13 +50,10 @@ def __init__( 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 self._control_profiles = self._snapshot_control_profiles( {} if control_profiles is None else control_profiles ) - self._binding_cache: dict[ - tuple[tuple[tuple[str, str], ...], tuple[tuple[str, str], ...]], - ResolvedActionBinding, - ] = {} @property def motion_generator(self) -> MotionGenerator: @@ -82,9 +70,14 @@ def device(self) -> torch.device: """Return the concrete device used for planning.""" return self._device + @property + def binding_owner_id(self) -> str: + """Return the opaque identity required by this engine's bindings.""" + return self._binding_owner_id + @property def control_profiles(self) -> Mapping[str, ControlPartCommandProfile]: - """Return owned semantic command profiles keyed by control-part name.""" + """Return owned direct-core command profiles by control-part name.""" return MappingProxyType( { name: profile.snapshot() @@ -101,110 +94,210 @@ def planner_name(self) -> str: planner_name = getattr(planner_cfg, "planner_type", None) return "unknown" if planner_name is None else str(planner_name) - def resolve_binding( + def bind_control_parts( self, - binding: ActionBinding, - control_overrides: ActionControlOverrides | None = None, - ) -> ResolvedActionBinding: - """Resolve binding names against the owned robot's control parts. + contract: SkillBindingContract, + endpoints: Mapping[str, Mapping[str, str]], + ) -> ActionBinding: + """Build a generic binding from explicit robot control-part names. - ``ActionBinding`` deliberately carries stable string references only. - This method establishes that every reference is a key in - ``Robot.control_parts`` and resolves its full-robot joint indices. - - Args: - binding: Semantic-role mapping to validate and resolve. - control_overrides: Optional per-role command replacements for this - invocation revision. - - Returns: - Immutable runtime resources for action planning. - - Raises: - TypeError: If ``binding`` or ``Robot.control_parts`` is invalid. - ValueError: If a referenced control part is unknown or empty. + This is the advanced direct-core construction path. Profile-backed + callers obtain the same :class:`ActionBinding` from + ``BoundRobotSkillProfile.resolve()``. """ - if not isinstance(binding, ActionBinding): - raise TypeError("binding must be an ActionBinding.") - cache_key = ( - tuple(sorted(binding.manipulators.items())), - tuple(sorted(binding.end_effectors.items())), - ) - resolved = self._binding_cache.get(cache_key) - if resolved is None: - control_parts = getattr(self.robot, "control_parts", None) - if not isinstance(control_parts, Mapping): - if binding.manipulators or binding.end_effectors: - raise TypeError( - "ActionBinding resources must come from " - "Robot.control_parts, but the engine robot does not " - "define a control-parts mapping." + if not isinstance(contract, SkillBindingContract): + raise TypeError("contract must be a SkillBindingContract.") + if not isinstance(endpoints, Mapping): + raise TypeError("endpoints must be a slot-to-endpoint mapping.") + expected = { + (slot.slot_id, requirement.endpoint_id): requirement + for slot in contract.slots + for requirement in slot.endpoints + } + supplied: dict[tuple[str, str], str] = {} + for slot_id, slot_endpoints in endpoints.items(): + if not isinstance(slot_id, str) or not slot_id.strip(): + raise ValueError("Binding slot IDs must be non-empty strings.") + if not isinstance(slot_endpoints, Mapping): + raise TypeError(f"Binding slot {slot_id!r} must contain a mapping.") + for endpoint_id, control_part in slot_endpoints.items(): + key = (slot_id, endpoint_id) + if key in supplied: + raise ValueError( + f"Binding endpoint {slot_id}.{endpoint_id} repeats." ) - control_parts = {} - - resolved = ResolvedActionBinding( - manipulators=self._resolve_resource_map( - binding.manipulators, - control_parts=control_parts, - resource_kind="manipulator", - ), - end_effectors=self._resolve_resource_map( - binding.end_effectors, - control_parts=control_parts, - resource_kind="end effector", - ), + if not isinstance(endpoint_id, str) or not endpoint_id.strip(): + raise ValueError("Binding endpoint IDs must be non-empty strings.") + if not isinstance(control_part, str) or not control_part.strip(): + raise ValueError("Control-part names must be non-empty strings.") + supplied[key] = control_part + if set(supplied) != set(expected): + missing = sorted(set(expected) - set(supplied)) + extra = sorted(set(supplied) - set(expected)) + raise ValueError( + "Direct binding must cover the skill contract exactly: " + f"missing={missing}, extra={extra}." ) - self._binding_cache[cache_key] = resolved - - if control_overrides is None: - return resolved - if not isinstance(control_overrides, ActionControlOverrides): - raise TypeError("control_overrides must be an ActionControlOverrides.") - if control_overrides.is_empty: - return resolved - return ResolvedActionBinding( - manipulators=self._apply_command_overrides( - resolved.manipulators, - control_overrides.manipulators, - resource_kind="manipulator", - ), - end_effectors=self._apply_command_overrides( - resolved.end_effectors, - control_overrides.end_effectors, - resource_kind="end effector", - ), - ) + if not expected: + binding = ActionBinding(owner_id=self.binding_owner_id) + self.validate_binding(binding, contract) + return binding - def _resolve_resource_map( - self, - resources: Mapping[str, str], - *, - control_parts: Mapping[str, object], - resource_kind: str, - ) -> dict[str, ResolvedControlPart]: - """Resolve one role map through ``Robot.control_parts``.""" + control_parts = getattr(self.robot, "control_parts", None) + 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: dict[str, ResolvedControlPart] = {} - for role, name in resources.items(): - if name not in control_parts: + resolved: list[EndpointBinding] = [] + for key, requirement in expected.items(): + slot_id, endpoint_id = key + control_part = supplied[key] + if control_part not in control_parts: raise ValueError( - f"ActionBinding {resource_kind} role {role!r} references " - f"control part {name!r}, but Robot.control_parts contains " - f"{available}." + f"Endpoint {slot_id}.{endpoint_id} references control part " + f"{control_part!r}, but Robot.control_parts contains {available}." ) - joint_ids = tuple(self.robot.get_joint_ids(name=name)) + joint_ids = tuple(self.robot.get_joint_ids(name=control_part)) if not joint_ids: + raise ValueError(f"Control part {control_part!r} contains no joints.") + profile = self._control_profiles.get(control_part) + commands = {} if profile is None else profile.commands + for name, command_type in requirement.required_commands.items(): + command = commands.get(name) + if not isinstance(command, command_type): + raise ValueError( + f"Endpoint {slot_id}.{endpoint_id} requires command {name!r} " + f"of type {command_type.__name__}." + ) + 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), + capabilities=requirement.capabilities, + commands=commands, + claim_tokens=frozenset({f"robot.control_part:{control_part}"}), + joint_ids=joint_ids, + ) + ) + binding = ActionBinding( + owner_id=self.binding_owner_id, + endpoints=tuple(resolved), + ) + self.validate_binding(binding, contract) + return binding + + def validate_binding( + self, + binding: ActionBinding, + contract: SkillBindingContract, + ) -> None: + """Validate endpoint coverage, ownership, capabilities, and claims.""" + if not isinstance(binding, ActionBinding): + raise TypeError("binding must be an ActionBinding.") + if binding.owner_id != self.binding_owner_id: + raise ValueError("ActionBinding belongs to another engine instance.") + expected = { + (slot.slot_id, requirement.endpoint_id): requirement + for slot in contract.slots + for requirement in slot.endpoints + } + if set(binding.endpoint_keys) != set(expected): + missing = sorted(set(expected) - set(binding.endpoint_keys)) + extra = sorted(set(binding.endpoint_keys) - set(expected)) + raise ValueError( + "ActionBinding must cover the skill contract exactly: " + f"missing={missing}, extra={extra}." + ) + for key, requirement in expected.items(): + endpoint = binding.endpoint(*key) + missing_capabilities = requirement.capabilities - endpoint.capabilities + if missing_capabilities: raise ValueError( - f"Robot control part {name!r} bound to {resource_kind} role " - f"{role!r} contains no joints." + f"Endpoint {key[0]}.{key[1]} is missing capabilities " + f"{sorted(missing_capabilities)}." ) - profile = self._control_profiles.get(name) - resolved[role] = ResolvedControlPart( - name=name, - joint_ids=joint_ids, - commands={} if profile is None else profile.commands, + for name, command_type in requirement.required_commands.items(): + command = endpoint.commands.get(name) + if not isinstance(command, command_type): + raise ValueError( + f"Endpoint {key[0]}.{key[1]} requires command {name!r} " + f"of type {command_type.__name__}." + ) + for slot in contract.slots: + for constraint in slot.constraints: + if not isinstance(constraint, DisjointSlotEndpoints): + continue + selected = [ + binding.endpoint(slot.slot_id, endpoint_id) + for endpoint_id in constraint.endpoint_ids + ] + self._validate_disjoint(selected, label=f"slot {slot.slot_id!r}") + for constraint in contract.constraints: + if not isinstance(constraint, DisjointResourceSlots): + continue + for index, left_slot in enumerate(constraint.slots): + left = [ + endpoint + for endpoint in binding.endpoints + if endpoint.slot_id == left_slot + ] + for right_slot in constraint.slots[index + 1 :]: + right = [ + endpoint + for endpoint in binding.endpoints + if endpoint.slot_id == right_slot + ] + self._validate_disjoint( + left + right, + label=f"slots {left_slot!r} and {right_slot!r}", + only_across=len(left), + ) + + def apply_command_overrides( + self, + binding: ActionBinding, + overrides: ActionControlOverrides, + ) -> ActionBinding: + """Apply endpoint-scoped commands to an owned validated binding.""" + if not isinstance(overrides, ActionControlOverrides): + raise TypeError("overrides must be an ActionControlOverrides.") + if overrides.is_empty: + return ActionBinding(binding.owner_id, binding.endpoints) + return binding.with_command_overrides(overrides.as_flat_mapping()) + + @staticmethod + def _validate_disjoint( + endpoints: list[EndpointBinding], + *, + label: str, + only_across: int | None = None, + ) -> None: + """Reject overlapping destination, claim-token, or joint ownership.""" + pairs = ( + ( + (left, right) + for left in endpoints[:only_across] + for right in endpoints[only_across:] ) - return resolved + if only_across is not None + else ( + (left, right) + for index, left in enumerate(endpoints) + for right in endpoints[index + 1 :] + ) + ) + for left, right in pairs: + same_destination = left.destination_key == right.destination_key + overlapping_tokens = left.claim_tokens & right.claim_tokens + left_joints = set(left.joint_ids) + right_joints = set(right.joint_ids) + if same_destination or overlapping_tokens or left_joints & right_joints: + raise ValueError( + f"ActionBinding violates disjoint constraint for {label}: " + f"{left.key} conflicts with {right.key}." + ) def _snapshot_control_profiles( self, @@ -240,24 +333,5 @@ def _snapshot_control_profiles( snapshots[name] = profile.snapshot() return MappingProxyType(snapshots) - @staticmethod - def _apply_command_overrides( - resources: Mapping[str, ResolvedControlPart], - overrides: Mapping[str, Mapping[str, ControlCommand]], - *, - resource_kind: str, - ) -> dict[str, ResolvedControlPart]: - """Apply role-scoped commands to already resolved control parts.""" - unknown_roles = sorted(set(overrides) - set(resources)) - if unknown_roles: - raise KeyError( - f"Command overrides reference unbound {resource_kind} roles " - f"{unknown_roles}; bound roles are {sorted(resources)}." - ) - return { - role: resource.with_command_overrides(overrides.get(role, {})) - for role, resource in resources.items() - } - __all__ = ["ActionPlanningServices"] diff --git a/embodichain/lab/sim/atomic_actions/runtime_commands.py b/embodichain/lab/sim/atomic_actions/runtime_commands.py new file mode 100644 index 000000000..aeaa3ffd1 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/runtime_commands.py @@ -0,0 +1,481 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Transport-neutral runtime command values for atomic actions.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from dataclasses import dataclass +from typing import ClassVar + +import torch + +from .bindings import JointPositionTarget, RuntimeEndpointTarget + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate and return one strict identifier.""" + if not isinstance(value, 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_target(target: RuntimeEndpointTarget) -> RuntimeEndpointTarget: + """Validate and own one runtime target snapshot.""" + if not isinstance(target, RuntimeEndpointTarget): + raise TypeError("target must be a RuntimeEndpointTarget.") + snapshot = target.snapshot() + if type(snapshot) is not type(target) or snapshot is target: + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently owned " + "value of the same target type." + ) + _validate_identifier( + snapshot.transport_id, + field_name="RuntimeEndpointTarget.transport_id", + ) + _validate_identifier( + snapshot.target_id, + field_name="RuntimeEndpointTarget.target_id", + ) + source_fingerprint = target.address_fingerprint + snapshot_fingerprint = snapshot.address_fingerprint + try: + hash(source_fingerprint) + hash(snapshot_fingerprint) + except TypeError as exc: + raise TypeError( + "RuntimeEndpointTarget.address_fingerprint must be hashable." + ) from exc + if snapshot_fingerprint != source_fingerprint: + raise ValueError( + "RuntimeEndpointTarget.snapshot() must preserve its address fingerprint." + ) + return snapshot + + +class RuntimeCommandPayload(ABC): + """Immutable-by-ownership payload submitted to one runtime transport.""" + + @property + @abstractmethod + def batch_size(self) -> int: + """Return the number of environment rows in this payload.""" + + @property + @abstractmethod + def device(self) -> torch.device: + """Return the device shared by this payload's batched values.""" + + @property + @abstractmethod + def transport_id(self) -> str: + """Return the transport kind that accepts this payload.""" + + @abstractmethod + def snapshot(self) -> RuntimeCommandPayload: + """Return an independently owned payload snapshot.""" + + +def _validate_payload_metadata(payload: RuntimeCommandPayload) -> None: + """Validate transport-neutral payload metadata.""" + if ( + not isinstance(payload.batch_size, int) + or isinstance(payload.batch_size, bool) + or payload.batch_size < 1 + ): + raise ValueError("RuntimeCommandPayload.batch_size must be a positive integer.") + if not isinstance(payload.device, torch.device): + raise TypeError("RuntimeCommandPayload.device must be a torch.device.") + _validate_identifier( + payload.transport_id, + field_name="RuntimeCommandPayload.transport_id", + ) + + +def _snapshot_payload(payload: RuntimeCommandPayload) -> RuntimeCommandPayload: + """Validate and own one runtime payload snapshot.""" + if not isinstance(payload, RuntimeCommandPayload): + raise TypeError("payload must be a RuntimeCommandPayload.") + snapshot = payload.snapshot() + if type(snapshot) is not type(payload) or snapshot is payload: + raise TypeError( + "RuntimeCommandPayload.snapshot() must return an independently owned " + "value of the same payload type." + ) + _validate_payload_metadata(snapshot) + return snapshot + + +@dataclass(frozen=True, slots=True, eq=False) +class JointPositionPayload(RuntimeCommandPayload): + """Batched joint-position targets for the built-in robot transport. + + Args: + positions: Joint positions with shape ``(batch_size, control_dof)``. + velocities: Optional joint velocities with the same shape and device. + """ + + TRANSPORT_ID: ClassVar[str] = JointPositionTarget.TRANSPORT_ID + + positions: torch.Tensor + velocities: torch.Tensor | None = None + + def __post_init__(self) -> None: + if not isinstance(self.positions, torch.Tensor): + raise TypeError("positions must be a torch.Tensor.") + if ( + self.positions.dim() != 2 + or self.positions.shape[0] < 1 + or self.positions.shape[1] < 1 + ): + raise ValueError( + "positions must have shape (batch_size, control_dof) with non-zero " + "dimensions." + ) + if not torch.isfinite(self.positions).all().item(): + raise ValueError("positions must contain only finite values.") + if self.velocities is not None: + if not isinstance(self.velocities, torch.Tensor): + raise TypeError("velocities must be a torch.Tensor or None.") + if self.velocities.shape != self.positions.shape: + raise ValueError("velocities must match positions shape.") + if self.velocities.device != self.positions.device: + raise ValueError("velocities must share the positions device.") + if not torch.isfinite(self.velocities).all().item(): + raise ValueError("velocities must contain only finite values.") + object.__setattr__(self, "positions", self.positions.clone()) + if self.velocities is not None: + object.__setattr__(self, "velocities", self.velocities.clone()) + + @property + def batch_size(self) -> int: + """Return the number of environment rows.""" + return int(self.positions.shape[0]) + + @property + def dof(self) -> int: + """Return the number of controlled joints.""" + return int(self.positions.shape[1]) + + @property + def device(self) -> torch.device: + """Return the tensor device.""" + return self.positions.device + + @property + def transport_id(self) -> str: + """Return the built-in joint-position transport identifier.""" + return self.TRANSPORT_ID + + def snapshot(self) -> JointPositionPayload: + """Return an independently owned joint payload.""" + return JointPositionPayload( + positions=self.positions, + velocities=self.velocities, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class EndpointCommand: + """One transport-compatible payload addressed to one runtime target. + + Args: + target: Immutable destination resolved from an action endpoint. + payload: Batched command value accepted by the target transport. + """ + + target: RuntimeEndpointTarget + payload: RuntimeCommandPayload + + def __post_init__(self) -> None: + target = _snapshot_target(self.target) + payload = _snapshot_payload(self.payload) + if target.transport_id != payload.transport_id: + raise ValueError( + f"Target transport {target.transport_id!r} does not accept payload " + f"transport {payload.transport_id!r}." + ) + object.__setattr__(self, "target", target) + object.__setattr__(self, "payload", payload) + + @property + def transport_id(self) -> str: + """Return the common target and payload transport identifier.""" + return self.target.transport_id + + @property + def destination_key(self) -> tuple[str, str]: + """Return the transport-scoped destination identifier.""" + return self.transport_id, self.target.target_id + + @property + def batch_size(self) -> int: + """Return the payload batch size.""" + return self.payload.batch_size + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.payload.device + + def snapshot(self) -> EndpointCommand: + """Return an independently owned endpoint command.""" + return EndpointCommand(target=self.target, payload=self.payload) + + +@dataclass(frozen=True, slots=True, eq=False) +class RuntimeCommandFrame: + """Synchronized endpoint commands for one batched runtime instant. + + Args: + commands: Commands dispatched together for this frame. + active_mask: Boolean environment rows allowed to execute commands. + Transports must actively neutralize addressed targets for false + rows rather than leaving a previously persistent command running. + env_ids: Stable environment identifiers for the batch rows. + hold_duration: Per-row delay before advancing to the next frame. + """ + + commands: tuple[EndpointCommand, ...] + active_mask: torch.Tensor + env_ids: torch.Tensor + hold_duration: torch.Tensor + + def __post_init__(self) -> None: + if isinstance(self.commands, (str, bytes)): + raise TypeError("commands must be an iterable of EndpointCommand values.") + try: + commands = tuple(self.commands) + except TypeError as exc: + raise TypeError( + "commands must be an iterable of EndpointCommand values." + ) from exc + if not all(isinstance(command, EndpointCommand) for command in commands): + raise TypeError("commands values must be EndpointCommand instances.") + + 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.shape[0] < 1 + ): + raise ValueError("env_ids must be int64 with shape (batch_size,).") + batch_size = int(self.env_ids.shape[0]) + if torch.unique(self.env_ids).numel() != batch_size: + raise ValueError("env_ids must be unique.") + if not isinstance(self.active_mask, torch.Tensor): + raise TypeError("active_mask must be a torch.Tensor.") + if self.active_mask.dtype != torch.bool or self.active_mask.shape != ( + batch_size, + ): + raise ValueError(f"active_mask must be bool with shape ({batch_size},).") + if not isinstance(self.hold_duration, torch.Tensor): + raise TypeError("hold_duration must be a torch.Tensor.") + if self.hold_duration.shape != (batch_size,): + raise ValueError(f"hold_duration must have shape ({batch_size},).") + if ( + not torch.isfinite(self.hold_duration).all().item() + or (self.hold_duration < 0.0).any().item() + ): + raise ValueError("hold_duration must contain finite non-negative values.") + if self.active_mask.device != self.env_ids.device: + raise ValueError("active_mask and env_ids must share a device.") + if self.hold_duration.device != self.env_ids.device: + raise ValueError("hold_duration and env_ids must share a device.") + + snapshots = tuple(command.snapshot() for command in commands) + destinations: set[tuple[str, str]] = set() + joint_owners: dict[int, tuple[str, str]] = {} + for command in snapshots: + if command.batch_size != batch_size: + raise ValueError( + f"Payload for destination {command.destination_key} has batch " + f"size {command.batch_size}, expected {batch_size}." + ) + if command.device != self.env_ids.device: + raise ValueError( + f"Payload for destination {command.destination_key} must share " + "the frame device." + ) + if command.destination_key in destinations: + raise ValueError( + f"RuntimeCommandFrame contains duplicate destination " + f"{command.destination_key}." + ) + destinations.add(command.destination_key) + + if isinstance(command.target, JointPositionTarget): + if not isinstance(command.payload, JointPositionPayload): + raise TypeError( + "JointPositionTarget requires a JointPositionPayload." + ) + expected_dof = len(command.target.joint_ids) + if command.payload.dof != expected_dof: + raise ValueError( + f"Joint payload for destination {command.destination_key} has " + f"DOF {command.payload.dof}, expected {expected_dof}." + ) + overlaps = sorted( + joint_id + for joint_id in command.target.joint_ids + if joint_id in joint_owners + ) + if overlaps: + owners = sorted({joint_owners[joint_id] for joint_id in overlaps}) + raise ValueError( + f"Joint destination {command.destination_key} overlaps joint " + f"IDs {overlaps} already owned by {owners}." + ) + for joint_id in command.target.joint_ids: + joint_owners[joint_id] = command.destination_key + + object.__setattr__(self, "commands", snapshots) + object.__setattr__(self, "active_mask", self.active_mask.clone()) + object.__setattr__(self, "env_ids", self.env_ids.clone()) + object.__setattr__(self, "hold_duration", self.hold_duration.clone()) + + @property + def batch_size(self) -> int: + """Return the number of environment rows.""" + return int(self.env_ids.shape[0]) + + @property + def device(self) -> torch.device: + """Return the shared frame device.""" + return self.env_ids.device + + @property + def targets(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return owned targets in command order.""" + return tuple(_snapshot_target(command.target) for command in self.commands) + + def with_active_mask(self, active_mask: torch.Tensor) -> RuntimeCommandFrame: + """Return a frame snapshot with a replacement active-row mask. + + Args: + active_mask: Boolean mask with one value per environment row. + + Returns: + Independently owned frame with unchanged commands and timing. + """ + return RuntimeCommandFrame( + commands=self.commands, + active_mask=active_mask, + env_ids=self.env_ids, + hold_duration=self.hold_duration, + ) + + def snapshot(self) -> RuntimeCommandFrame: + """Return an independently owned command frame.""" + return RuntimeCommandFrame( + commands=self.commands, + active_mask=self.active_mask, + env_ids=self.env_ids, + hold_duration=self.hold_duration, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class TimedCommandSequence: + """Ordered runtime command frames for one stable environment batch. + + ``env_ids`` is authoritative even when ``frames`` is empty, preserving the + batch size and device needed by compilation and execution boundaries. + + Args: + frames: Ordered command frames in execution order. + env_ids: Stable environment identifiers retained for empty sequences. + """ + + frames: tuple[RuntimeCommandFrame, ...] + env_ids: 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 + or self.env_ids.shape[0] < 1 + ): + raise ValueError("env_ids must be int64 with shape (batch_size,).") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + if isinstance(self.frames, (str, bytes)): + raise TypeError("frames must be an iterable of RuntimeCommandFrame values.") + try: + frames = tuple(self.frames) + except TypeError as exc: + raise TypeError( + "frames must be an iterable of RuntimeCommandFrame values." + ) from exc + if not all(isinstance(frame, RuntimeCommandFrame) for frame in frames): + raise TypeError("frames values must be RuntimeCommandFrame instances.") + snapshots: list[RuntimeCommandFrame] = [] + for index, frame in enumerate(frames): + if frame.device != self.env_ids.device: + raise ValueError(f"Frame {index} must share the sequence device.") + if not torch.equal(frame.env_ids, self.env_ids): + raise ValueError(f"Frame {index} env_ids do not match the sequence.") + snapshots.append(frame.snapshot()) + object.__setattr__(self, "frames", tuple(snapshots)) + object.__setattr__(self, "env_ids", self.env_ids.clone()) + + @property + def batch_size(self) -> int: + """Return the preserved environment batch size.""" + return int(self.env_ids.shape[0]) + + @property + def device(self) -> torch.device: + """Return the preserved batch device.""" + return self.env_ids.device + + @property + def frame_count(self) -> int: + """Return the number of command frames.""" + return len(self.frames) + + @property + def targets(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return unique owned destinations in first-use order.""" + targets: list[RuntimeEndpointTarget] = [] + seen: set[tuple[str, str]] = set() + for frame in self.frames: + for command in frame.commands: + if command.destination_key in seen: + continue + seen.add(command.destination_key) + targets.append(_snapshot_target(command.target)) + return tuple(targets) + + def snapshot(self) -> TimedCommandSequence: + """Return an independently owned timed sequence.""" + return TimedCommandSequence(frames=self.frames, env_ids=self.env_ids) + + +__all__ = [ + "EndpointCommand", + "JointPositionPayload", + "RuntimeCommandFrame", + "RuntimeCommandPayload", + "TimedCommandSequence", +] diff --git a/embodichain/lab/sim/atomic_actions/sim_adapter.py b/embodichain/lab/sim/atomic_actions/sim_adapter.py index 91f5e61fe..d53b61b69 100644 --- a/embodichain/lab/sim/atomic_actions/sim_adapter.py +++ b/embodichain/lab/sim/atomic_actions/sim_adapter.py @@ -26,11 +26,12 @@ from embodichain.utils import configclass -from .execution import JointCommand +from .bindings import JointPositionTarget, RuntimeEndpointTarget from .runner import ( CommandAcknowledgement, CommandAckStatus, ) +from .runtime_commands import JointPositionPayload, RuntimeCommandFrame from .scene import SceneProvider from .state import ( EntityState, @@ -262,6 +263,9 @@ class SimulationExecutionAdapter: initial_time: Initial elapsed simulation time in seconds. """ + transport_id = JointPositionTarget.TRANSPORT_ID + payload_type = JointPositionPayload + def __init__( self, simulation: SimulationManager, @@ -395,16 +399,15 @@ def observe(self, task_state: TaskState) -> PlanningContext: def send( self, - command: JointCommand, + command: RuntimeCommandFrame, *, timeout: float, ) -> CommandAcknowledgement: - """Write active targets and observed-position holds as one batch. + """Write joint endpoint targets and neutralize inactive rows. Args: - command: Full-robot batched command. Inactive rows already contain - observed-position holds and are written with active rows so no - environment continues tracking a stale target. + command: Joint-position endpoint frame. Inactive rows are replaced + with observed positions by this transport. timeout: Positive acknowledgement deadline. Simulation writes are synchronous, so this is validated but otherwise unused. @@ -413,16 +416,43 @@ def send( """ self._validate_timeout(timeout) try: - self._validate_command(command) - self.robot.set_qpos( - command.positions, - env_ids=self._robot_env_indices, - ) - if command.velocities is not None: - self.robot.set_qvel( - command.velocities, + self._validate_command_frame(command) + observed_positions = self.robot.get_qpos() + for endpoint_command in command.commands: + target = endpoint_command.target + payload = endpoint_command.payload + assert isinstance(target, JointPositionTarget) + assert isinstance(payload, JointPositionPayload) + joint_ids = list(target.joint_ids) + positions = torch.where( + command.active_mask[:, None], + payload.positions, + observed_positions[:, joint_ids], + ) + self.robot.set_qpos( + positions, + joint_ids=joint_ids, env_ids=self._robot_env_indices, ) + velocities = payload.velocities + if velocities is None and not command.active_mask.all().item(): + observed_velocities = self._read_optional_tensor("get_qvel") + velocities = ( + torch.zeros_like(observed_positions[:, joint_ids]) + if observed_velocities is None + else observed_velocities[:, joint_ids] + ) + if velocities is not None: + velocities = torch.where( + command.active_mask[:, None], + velocities, + torch.zeros_like(velocities), + ) + self.robot.set_qvel( + velocities, + joint_ids=joint_ids, + env_ids=self._robot_env_indices, + ) return CommandAcknowledgement.accepted_ack() except Exception as exc: return CommandAcknowledgement( @@ -432,15 +462,16 @@ def send( def hold( self, - command: JointCommand, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, *, timeout: float, ) -> CommandAcknowledgement: - """Set every represented environment to an observed-position hold. + """Set every represented joint endpoint to an observed-position hold. Args: - command: Full-robot hold positions. ``active_mask`` is intentionally - ignored because safety hold applies to every environment row. + targets: Joint-position destinations to place in a safe hold. + context: Latest observed positions and stable environment IDs. timeout: Positive acknowledgement deadline. Returns: @@ -448,14 +479,25 @@ def hold( """ self._validate_timeout(timeout) try: - self._validate_command(command) - self.robot.set_qpos( - command.positions, - env_ids=self._robot_env_indices, - ) - if command.velocities is not None: + self._validate_targets(targets) + if not isinstance(context, PlanningContext): + raise TypeError("context must be a PlanningContext.") + if not torch.equal(context.env_ids, self.env_ids): + raise ValueError("Hold context env_ids must match the adapter.") + if context.robot.qpos.shape != self.robot.get_qpos().shape: + raise ValueError("Hold context qpos shape must match the robot.") + for target in targets: + assert isinstance(target, JointPositionTarget) + joint_ids = list(target.joint_ids) + observed_positions = context.robot.qpos[:, joint_ids] + self.robot.set_qpos( + observed_positions, + joint_ids=joint_ids, + env_ids=self._robot_env_indices, + ) self.robot.set_qvel( - command.velocities, + torch.zeros_like(observed_positions), + joint_ids=joint_ids, env_ids=self._robot_env_indices, ) return CommandAcknowledgement.accepted_ack() @@ -465,10 +507,16 @@ def hold( f"{type(exc).__name__}: {exc}", ) - def cancel(self, *, timeout: float) -> CommandAcknowledgement: + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: """Acknowledge cancellation of synchronous simulation target writes. Args: + targets: Joint-position destinations whose queued work is cancelled. timeout: Positive acknowledgement deadline. Returns: @@ -476,6 +524,13 @@ def cancel(self, *, timeout: float) -> CommandAcknowledgement: actual safe target. """ self._validate_timeout(timeout) + try: + self._validate_targets(targets) + except Exception as exc: + return CommandAcknowledgement( + CommandAckStatus.REJECTED, + f"{type(exc).__name__}: {exc}", + ) return CommandAcknowledgement.accepted_ack( "Simulation commands are synchronous; no queued command remained." ) @@ -505,18 +560,43 @@ def _read_optional_proprioception_tensor( return None return value if isinstance(value, torch.Tensor) else None - def _validate_command(self, command: JointCommand) -> None: - """Validate command identity and shape against the attached robot.""" - if not isinstance(command, JointCommand): - raise TypeError("command must be a JointCommand.") - qpos = self.robot.get_qpos() - if command.positions.shape != qpos.shape: - raise ValueError( - "Command shape must match full robot qpos, " - f"got {tuple(command.positions.shape)} and {tuple(qpos.shape)}." - ) + def _validate_command_frame(self, command: RuntimeCommandFrame) -> None: + """Validate one joint-position frame against the attached robot.""" + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") if not torch.equal(command.env_ids, self.env_ids): raise ValueError("Command env_ids must match the simulation adapter.") + self._validate_targets(command.targets) + for endpoint_command in command.commands: + if not isinstance(endpoint_command.payload, JointPositionPayload): + raise TypeError( + "SimulationExecutionAdapter accepts JointPositionPayload only." + ) + + def _validate_targets( + self, + targets: tuple[RuntimeEndpointTarget, ...], + ) -> None: + """Validate joint target ownership and robot dimensions.""" + if isinstance(targets, (str, bytes)): + raise TypeError("targets must be an iterable of runtime targets.") + qpos = self.robot.get_qpos() + seen_joints: set[int] = set() + for target in targets: + if not isinstance(target, JointPositionTarget): + raise TypeError( + "SimulationExecutionAdapter accepts JointPositionTarget only." + ) + if target.transport_id != self.transport_id: + raise ValueError("Target transport does not match this adapter.") + if max(target.joint_ids) >= qpos.shape[1]: + raise ValueError( + f"Target {target.target_id!r} references a joint outside robot DOF." + ) + overlaps = seen_joints.intersection(target.joint_ids) + if overlaps: + raise ValueError(f"Joint targets overlap on IDs {sorted(overlaps)}.") + seen_joints.update(target.joint_ids) @staticmethod def _validate_timeout(timeout: float) -> None: diff --git a/embodichain/lab/sim/atomic_actions/transports.py b/embodichain/lab/sim/atomic_actions/transports.py new file mode 100644 index 000000000..18b95178a --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/transports.py @@ -0,0 +1,489 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Endpoint-command transport contracts and deterministic routing.""" + +from __future__ import annotations + +from collections.abc import Callable, Iterable, Mapping +import math +from types import MappingProxyType +from typing import TYPE_CHECKING, Protocol, runtime_checkable + +from .bindings import RuntimeEndpointTarget +from .runtime_commands import ( + EndpointCommand, + RuntimeCommandFrame, + RuntimeCommandPayload, +) + +if TYPE_CHECKING: + from .runner import CommandAcknowledgement + from .state import PlanningContext + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate and return one strict identifier.""" + if not isinstance(value, 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) -> float: + """Validate and normalize one acknowledgement timeout.""" + if isinstance(timeout, bool) or not isinstance(timeout, (int, float)): + raise TypeError("timeout must be a real number.") + normalized = float(timeout) + if not math.isfinite(normalized) or normalized <= 0.0: + raise ValueError("timeout must be finite and greater than zero.") + return normalized + + +@runtime_checkable +class EndpointCommandTransport(Protocol): + """Backend that owns one kind of runtime endpoint command. + + Implementations own live simulator entities, device clients, or controller + handles. Runtime command values retain only immutable addressing and payload + data, so they remain independent of those process-owned resources. + """ + + @property + def transport_id(self) -> str: + """Return the exact identifier used to register this transport.""" + + @property + def payload_type(self) -> type[RuntimeCommandPayload]: + """Return the runtime payload type accepted by :meth:`send`.""" + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Submit one transport-local command frame. + + Implementations must actively neutralize every inactive environment + row for every addressed target. Silently skipping an inactive row is + unsafe for persistent controllers such as base-velocity transports. + """ + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Hold transport-local targets at their observed state.""" + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Cancel outstanding commands for transport-local targets.""" + + +class EndpointCommandRouter: + """Route generic endpoint operations to exact registered transports. + + The router implements :class:`~.runner.CommandSink` structurally while + avoiding a module-load dependency on ``runner``. Acknowledgement types are + imported only when an operation is executed, which keeps the transport + boundary safe to import while the runner imports this module. + + Args: + transports: Either an exact ``transport_id -> transport`` mapping or an + iterable of transports from which that mapping is built. Mapping + keys must exactly equal each value's declared ``transport_id``. + + Raises: + TypeError: If a registration does not implement the transport contract. + ValueError: If an identifier is invalid, a mapping key is not exact, or + the same transport identifier is registered more than once. + """ + + def __init__( + self, + transports: ( + Mapping[str, EndpointCommandTransport] | Iterable[EndpointCommandTransport] + ), + ) -> None: + registrations = self._registrations(transports) + registered: dict[str, EndpointCommandTransport] = {} + payload_types: dict[str, type[RuntimeCommandPayload]] = {} + for map_key, transport in registrations: + if not isinstance(transport, EndpointCommandTransport): + raise TypeError( + "Registered values must implement EndpointCommandTransport." + ) + transport_id = _validate_identifier( + transport.transport_id, + field_name="EndpointCommandTransport.transport_id", + ) + if map_key is not None and map_key != transport_id: + raise ValueError( + f"Transport mapping key {map_key!r} must exactly match declared " + f"transport_id {transport_id!r}." + ) + if transport_id in registered: + raise ValueError( + f"Endpoint transport {transport_id!r} is registered more than once." + ) + payload_type = transport.payload_type + if not isinstance(payload_type, type) or not issubclass( + payload_type, RuntimeCommandPayload + ): + raise TypeError( + f"Transport {transport_id!r} payload_type must be a " + "RuntimeCommandPayload subclass." + ) + registered[transport_id] = transport + payload_types[transport_id] = payload_type + self._transports: Mapping[str, EndpointCommandTransport] = MappingProxyType( + registered + ) + self._payload_types: Mapping[str, type[RuntimeCommandPayload]] = ( + MappingProxyType(payload_types) + ) + + @staticmethod + def _registrations( + transports: ( + Mapping[str, EndpointCommandTransport] | Iterable[EndpointCommandTransport] + ), + ) -> tuple[tuple[str | None, EndpointCommandTransport], ...]: + """Normalize mapping and iterable registration forms.""" + if isinstance(transports, Mapping): + registrations: list[tuple[str | None, EndpointCommandTransport]] = [] + for key, transport in transports.items(): + _validate_identifier(key, field_name="Transport mapping keys") + registrations.append((key, transport)) + return tuple(registrations) + if isinstance(transports, (str, bytes)): + raise TypeError("transports must be a mapping or iterable of transports.") + try: + return tuple((None, transport) for transport in transports) + except TypeError as exc: + raise TypeError( + "transports must be a mapping or iterable of transports." + ) from exc + + @property + def transports(self) -> Mapping[str, EndpointCommandTransport]: + """Return the immutable exact transport registry.""" + return self._transports + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Route one synchronized command frame by transport identifier. + + Dispatch is preflighted before any transport is called. An unknown + transport or incompatible payload therefore rejects the whole frame + without creating a partially dispatched operation. + + Args: + frame: Generic runtime command frame to split by transport. + timeout: Maximum acknowledgement latency for each transport. + + Returns: + Aggregated acknowledgement. It is accepted only when every + addressed transport accepts its local frame. + """ + if not isinstance(frame, RuntimeCommandFrame): + raise TypeError("frame must be a RuntimeCommandFrame.") + normalized_timeout = _validate_timeout(timeout) + grouped: dict[str, list[EndpointCommand]] = {} + for command in frame.commands: + grouped.setdefault(command.transport_id, []).append(command) + + unknown = tuple( + transport_id + for transport_id in grouped + if transport_id not in self._transports + ) + if unknown: + return self._unknown_acknowledgement("send", unknown) + + incompatibilities: list[str] = [] + for transport_id, commands in grouped.items(): + payload_type = self._payload_types[transport_id] + for command in commands: + if not isinstance(command.payload, payload_type): + incompatibilities.append( + f"transport {transport_id!r} expects " + f"{payload_type.__name__}, got " + f"{type(command.payload).__name__} for target " + f"{command.target.target_id!r}" + ) + if incompatibilities: + return self._rejected_acknowledgement( + "send rejected: " + "; ".join(incompatibilities) + ) + + acknowledgements: list[tuple[str, CommandAcknowledgement]] = [] + for transport_id, commands in grouped.items(): + subframe = RuntimeCommandFrame( + commands=tuple(commands), + active_mask=frame.active_mask, + env_ids=frame.env_ids, + hold_duration=frame.hold_duration, + ) + transport = self._transports[transport_id] + acknowledgement = self._invoke_transport( + transport_id, + "send", + lambda transport=transport, subframe=subframe: transport.send( + subframe, + timeout=normalized_timeout, + ), + ) + acknowledgements.append((transport_id, acknowledgement)) + return self._aggregate_acknowledgements("send", acknowledgements) + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Route an observed-state hold request by target transport. + + Args: + targets: Runtime destinations to hold. + context: Fresh observation used by each transport to form its hold. + timeout: Maximum acknowledgement latency for each transport. + + Returns: + Aggregated acknowledgement. It is accepted only when every + addressed transport accepts its hold. + """ + normalized_timeout = _validate_timeout(timeout) + grouped = self._group_targets(targets) + unknown = tuple( + transport_id + for transport_id in grouped + if transport_id not in self._transports + ) + if unknown: + return self._unknown_acknowledgement("hold", unknown) + + acknowledgements: list[tuple[str, CommandAcknowledgement]] = [] + for transport_id, local_targets in grouped.items(): + transport = self._transports[transport_id] + acknowledgement = self._invoke_transport( + transport_id, + "hold", + lambda transport=transport, local_targets=local_targets: transport.hold( + local_targets, + context, + timeout=normalized_timeout, + ), + ) + acknowledgements.append((transport_id, acknowledgement)) + return self._aggregate_acknowledgements("hold", acknowledgements) + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Route cancellation by target transport. + + Args: + targets: Runtime destinations whose outstanding commands are + cancelled. + timeout: Maximum acknowledgement latency for each transport. + + Returns: + Aggregated acknowledgement. It is accepted only when every + addressed transport accepts cancellation. + """ + normalized_timeout = _validate_timeout(timeout) + grouped = self._group_targets(targets) + unknown = tuple( + transport_id + for transport_id in grouped + if transport_id not in self._transports + ) + if unknown: + return self._unknown_acknowledgement("cancel", unknown) + + acknowledgements: list[tuple[str, CommandAcknowledgement]] = [] + for transport_id, local_targets in grouped.items(): + transport = self._transports[transport_id] + acknowledgement = self._invoke_transport( + transport_id, + "cancel", + lambda transport=transport, local_targets=local_targets: transport.cancel( + local_targets, + timeout=normalized_timeout, + ), + ) + acknowledgements.append((transport_id, acknowledgement)) + return self._aggregate_acknowledgements("cancel", acknowledgements) + + @staticmethod + def _group_targets( + targets: tuple[RuntimeEndpointTarget, ...], + ) -> dict[str, tuple[RuntimeEndpointTarget, ...]]: + """Validate, snapshot, and group runtime targets in first-use order.""" + if isinstance(targets, (str, bytes)): + raise TypeError( + "targets must be an iterable of RuntimeEndpointTarget values." + ) + try: + source_targets = tuple(targets) + except TypeError as exc: + raise TypeError( + "targets must be an iterable of RuntimeEndpointTarget values." + ) from exc + + grouped: dict[str, list[RuntimeEndpointTarget]] = {} + for target in source_targets: + if not isinstance(target, RuntimeEndpointTarget): + raise TypeError( + "targets values must be RuntimeEndpointTarget instances." + ) + snapshot = target.snapshot() + if type(snapshot) is not type(target) or snapshot is target: + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently " + "owned value of the same target type." + ) + transport_id = _validate_identifier( + snapshot.transport_id, + field_name="RuntimeEndpointTarget.transport_id", + ) + _validate_identifier( + snapshot.target_id, + field_name="RuntimeEndpointTarget.target_id", + ) + grouped.setdefault(transport_id, []).append(snapshot) + return { + transport_id: tuple(local_targets) + for transport_id, local_targets in grouped.items() + } + + @staticmethod + def _validate_acknowledgement( + transport_id: str, + operation: str, + acknowledgement: object, + ) -> CommandAcknowledgement: + """Require transports to return the runner acknowledgement value.""" + from .runner import CommandAcknowledgement + + if not isinstance(acknowledgement, CommandAcknowledgement): + raise TypeError( + f"Transport {transport_id!r} {operation}() must return " + f"CommandAcknowledgement, got {type(acknowledgement).__name__}." + ) + return acknowledgement + + @staticmethod + def _invoke_transport( + transport_id: str, + operation: str, + invoke: Callable[[], object], + ) -> CommandAcknowledgement: + """Convert one transport-local failure without blocking other transports.""" + from .runner import CommandAcknowledgement, CommandAckStatus + + try: + acknowledgement = invoke() + return EndpointCommandRouter._validate_acknowledgement( + transport_id, + operation, + acknowledgement, + ) + except Exception as exc: + return CommandAcknowledgement( + CommandAckStatus.REJECTED, + f"Transport {transport_id!r} {operation}() failed with " + f"{type(exc).__name__}: {exc}", + ) + + @staticmethod + def _aggregate_acknowledgements( + operation: str, + acknowledgements: list[tuple[str, CommandAcknowledgement]], + ) -> CommandAcknowledgement: + """Aggregate transport acknowledgements with deterministic precedence.""" + from .runner import CommandAcknowledgement, CommandAckStatus + + failures = [ + (transport_id, acknowledgement) + for transport_id, acknowledgement in acknowledgements + if not acknowledgement.accepted + ] + if not failures: + diagnostics = "; ".join( + f"{transport_id}: {acknowledgement.message}" + for transport_id, acknowledgement in acknowledgements + if acknowledgement.message + ) + return CommandAcknowledgement.accepted_ack(diagnostics) + + status = ( + CommandAckStatus.TIMED_OUT + if any( + acknowledgement.status is CommandAckStatus.TIMED_OUT + for _, acknowledgement in failures + ) + else CommandAckStatus.REJECTED + ) + diagnostics = "; ".join( + f"transport {transport_id!r} {acknowledgement.status.value}: " + f"{acknowledgement.message or 'no diagnostic'}" + for transport_id, acknowledgement in failures + ) + return CommandAcknowledgement( + status, + f"{operation} failed: {diagnostics}", + ) + + @staticmethod + def _unknown_acknowledgement( + operation: str, + transport_ids: tuple[str, ...], + ) -> CommandAcknowledgement: + """Build a rejection for unregistered exact transport identifiers.""" + identifiers = ", ".join(repr(transport_id) for transport_id in transport_ids) + return EndpointCommandRouter._rejected_acknowledgement( + f"{operation} rejected: no transport is registered for {identifiers}." + ) + + @staticmethod + def _rejected_acknowledgement(message: str) -> CommandAcknowledgement: + """Build one rejected runner acknowledgement without an import cycle.""" + from .runner import CommandAcknowledgement, CommandAckStatus + + return CommandAcknowledgement(CommandAckStatus.REJECTED, message) + + +__all__ = ["EndpointCommandRouter", "EndpointCommandTransport"] diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 0e1d1a8c9..f5f79dbf4 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -25,7 +25,12 @@ from types import MappingProxyType from typing import ClassVar, Mapping, TYPE_CHECKING -from embodichain.lab.sim.atomic_actions.bindings import ActionBinding +from embodichain.lab.sim.atomic_actions.bindings import ( + ActionBinding, + EndpointBinding, + JointPositionTarget, + RuntimeEndpointTarget, +) from embodichain.lab.sim.atomic_actions.control import ( ControlCommand, ControlPartCommandProfile, @@ -109,10 +114,10 @@ def _snapshot_endpoint_commands( if not isinstance(command, ControlCommand): raise TypeError(f"{field_name} values must be ControlCommand instances.") snapshot = command.snapshot() - if not isinstance(snapshot, ControlCommand): + if type(snapshot) is not type(command) or snapshot is command: raise TypeError( - f"{field_name}[{command_name!r}].snapshot() must return a " - "ControlCommand." + f"{field_name}[{command_name!r}].snapshot() must return an " + "independently owned value of the same ControlCommand type." ) snapshots[command_name] = snapshot return MappingProxyType(snapshots) @@ -178,10 +183,10 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True) class EndpointResolution: - """Adapter-produced physical and lowering metadata for one endpoint.""" + """Adapter-produced runtime destination and claim metadata for one endpoint.""" - binding_values: Mapping[str, str] = field(default_factory=dict) - """Values supported for each current or future binding namespace.""" + runtime_target: RuntimeEndpointTarget + """Typed immutable destination consumed by an endpoint command transport.""" command_profile_key: str | None = None """Profile key that owns semantic commands for this endpoint, when any.""" @@ -199,14 +204,28 @@ class EndpointResolution: """Whether this execution endpoint must declare a physical claim.""" def __post_init__(self) -> None: - object.__setattr__( - self, - "binding_values", - _normalize_named_mapping( - self.binding_values, - field_name="EndpointResolution.binding_values", - ), + if not isinstance(self.runtime_target, RuntimeEndpointTarget): + raise TypeError( + "EndpointResolution.runtime_target must be a " "RuntimeEndpointTarget." + ) + target = self.runtime_target.snapshot() + if ( + type(target) is not type(self.runtime_target) + or target is self.runtime_target + ): + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently " + "owned value of the same target type." + ) + _validate_identifier( + target.transport_id, + field_name="RuntimeEndpointTarget.transport_id", + ) + _validate_identifier( + target.target_id, + field_name="RuntimeEndpointTarget.target_id", ) + object.__setattr__(self, "runtime_target", target) if self.command_profile_key is not None: _validate_identifier( self.command_profile_key, @@ -238,6 +257,11 @@ def __post_init__(self) -> None: ) if len(set(joint_ids)) != len(joint_ids): raise ValueError("EndpointResolution.joint_ids must be unique.") + if isinstance(target, JointPositionTarget) and joint_ids != target.joint_ids: + raise ValueError( + "EndpointResolution.joint_ids must exactly match its " + "JointPositionTarget." + ) object.__setattr__(self, "joint_ids", joint_ids) if not isinstance(self.exclusive, bool): raise TypeError("EndpointResolution.exclusive must be a bool.") @@ -333,10 +357,10 @@ def resolve( f"capabilities {sorted(declared)}, but has no configured solver." ) return EndpointResolution( - binding_values={ - "manipulator": endpoint.control_part, - "end_effector": endpoint.control_part, - }, + runtime_target=JointPositionTarget( + control_part=endpoint.control_part, + joint_ids=joint_ids, + ), command_profile_key=( endpoint.control_part if endpoint.command_profile is None @@ -354,7 +378,7 @@ class ResolvedResourceEndpoint: endpoint: ResourceEndpoint adapter_id: str - binding_values: Mapping[str, str] = field(default_factory=dict) + runtime_target: RuntimeEndpointTarget command_profile_key: str | None = None requires_command_profile: bool = False commands: Mapping[str, ControlCommand] = field(default_factory=dict) @@ -380,14 +404,14 @@ def __post_init__(self) -> None: field_name="ResolvedResourceEndpoint.adapter_id", ) resolution = EndpointResolution( - binding_values=self.binding_values, + runtime_target=self.runtime_target, command_profile_key=self.command_profile_key, requires_command_profile=self.requires_command_profile, claim_tokens=self.claim_tokens, joint_ids=self.joint_ids, exclusive=self.exclusive, ) - object.__setattr__(self, "binding_values", resolution.binding_values) + object.__setattr__(self, "runtime_target", resolution.runtime_target) object.__setattr__( self, "command_profile_key", @@ -420,7 +444,15 @@ def conflicts_with(self, other: ResolvedResourceEndpoint) -> bool: if not isinstance(other, ResolvedResourceEndpoint): raise TypeError("other must be a ResolvedResourceEndpoint.") return bool( - self.claim_tokens & other.claim_tokens + ( + self.runtime_target.transport_id, + self.runtime_target.target_id, + ) + == ( + other.runtime_target.transport_id, + other.runtime_target.target_id, + ) + or self.claim_tokens & other.claim_tokens or set(self.joint_ids) & set(other.joint_ids) ) @@ -1316,35 +1348,33 @@ def _validate_engine_control_profiles(self) -> None: ) for resource in self._resources.values(): for endpoint in resource.endpoints.values(): - if not endpoint.commands: + if not endpoint.commands or not isinstance( + endpoint.runtime_target, + JointPositionTarget, + ): continue - control_parts = { - value - for target, value in endpoint.binding_values.items() - if target in {"manipulator", "end_effector"} - } - for control_part in control_parts: - installed = engine_profiles.get(control_part) - if installed is None: + control_part = endpoint.runtime_target.control_part + installed = engine_profiles.get(control_part) + if installed is None: + raise ProfileValidationError( + f"Endpoint command profile " + f"{endpoint.command_profile_key!r} for control part " + f"{control_part!r} is not installed on the " + "AtomicActionEngine." + ) + for command_name, command in endpoint.commands.items(): + installed_command = installed.commands.get(command_name) + if installed_command is None: + raise ProfileValidationError( + f"Engine control profile {control_part!r} is missing " + f"profile command {command_name!r}." + ) + if not command.equivalent_to(installed_command): raise ProfileValidationError( - f"Endpoint command profile " - f"{endpoint.command_profile_key!r} for control part " - f"{control_part!r} is not installed on the " - "AtomicActionEngine." + f"Engine command {control_part!r}.{command_name} is " + "not semantically equivalent to the profile-owned " + "command." ) - for command_name, command in endpoint.commands.items(): - installed_command = installed.commands.get(command_name) - if installed_command is None: - raise ProfileValidationError( - f"Engine control profile {control_part!r} is missing " - f"profile command {command_name!r}." - ) - if not command.equivalent_to(installed_command): - raise ProfileValidationError( - f"Engine command {control_part!r}.{command_name} is " - "not semantically equivalent to the profile-owned " - "command." - ) def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: """Resolve adapter endpoints, graph closure, commands, and claims.""" @@ -1373,6 +1403,18 @@ def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: f"Endpoint adapter {adapter.adapter_id!r} returned " f"{type(resolution).__name__}, expected EndpointResolution." ) + invalid_joint_ids = sorted( + joint_id + for joint_id in resolution.joint_ids + if joint_id >= self._engine.robot.dof + ) + if invalid_joint_ids: + raise ProfileValidationError( + f"Endpoint adapter {adapter.adapter_id!r} resolved resource " + f"{resource_id!r} endpoint {endpoint_id!r} to joint IDs " + f"{invalid_joint_ids} outside robot DOF " + f"{self._engine.robot.dof}." + ) command_profile = ( None if resolution.command_profile_key is None @@ -1390,7 +1432,7 @@ def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: resource_endpoints[endpoint_id] = ResolvedResourceEndpoint( endpoint=endpoint, adapter_id=adapter.adapter_id, - binding_values=resolution.binding_values, + runtime_target=resolution.runtime_target, command_profile_key=resolution.command_profile_key, requires_command_profile=resolution.requires_command_profile, commands=( @@ -1504,7 +1546,7 @@ def _validate_command_shapes( ) def _validate_leaf_ownership(self) -> None: - """Require physical leaf resources to own disjoint adapter claims.""" + """Require physical leaves to own disjoint claims and runtime targets.""" leaves = [ resource for resource in self._resources.values() if not resource.members ] @@ -1524,6 +1566,28 @@ def _validate_leaf_ownership(self) -> None: f"{overlapping_tokens}. " "Model one physical leaf and reference it from composites." ) + left_targets = { + ( + endpoint.runtime_target.transport_id, + endpoint.runtime_target.target_id, + ) + for endpoint in left.endpoints.values() + } + right_targets = { + ( + endpoint.runtime_target.transport_id, + endpoint.runtime_target.target_id, + ) + for endpoint in right.endpoints.values() + } + overlapping_targets = sorted(left_targets & right_targets) + if overlapping_targets: + raise ProfileValidationError( + f"Leaf resources {left.resource_id!r} and " + f"{right.resource_id!r} share runtime targets " + f"{overlapping_targets}. Model one physical leaf and " + "reference it from composites." + ) def _validate_named_skill_configuration(self) -> None: """Reject defaults and preset selections for absent semantic skills.""" @@ -1611,11 +1675,6 @@ def _resource_matches( return False if not requirement.capabilities.issubset(endpoint.capabilities): return False - if ( - requirement.route is not None - and requirement.route.target not in endpoint.binding_values - ): - return False for command_name, command_type in requirement.required_commands.items(): command = endpoint.commands.get(command_name) if not isinstance(command, command_type): @@ -1701,15 +1760,6 @@ def _rejection_reasons( f"endpoint {requirement.endpoint_id!r} missing capabilities " f"{missing_capabilities}" ) - if ( - requirement.route is not None - and requirement.route.target not in endpoint.binding_values - ): - reasons.append( - f"endpoint {requirement.endpoint_id!r} adapter " - f"{endpoint.adapter_id!r} cannot lower to binding target " - f"{requirement.route.target!r}" - ) for command_name, command_type in requirement.required_commands.items(): command = endpoint.commands.get(command_name) if command is None: @@ -1739,43 +1789,61 @@ def _rejection_reasons( set(left.joint_ids) & set(right.joint_ids) ) overlapping_tokens = sorted(left.claim_tokens & right.claim_tokens) + shared_target = ( + ( + left.runtime_target.transport_id, + left.runtime_target.target_id, + ) + if ( + left.runtime_target.transport_id, + left.runtime_target.target_id, + ) + == ( + right.runtime_target.transport_id, + right.runtime_target.target_id, + ) + else None + ) reasons.append( f"endpoints {left_id!r} and {right_id!r} overlap on joints " f"{overlapping_joints} or adapter claims " - f"{overlapping_tokens}" + f"{overlapping_tokens} or share runtime target " + f"{shared_target}" ) return tuple(reasons) - @staticmethod def _lower_binding( + self, skill_id: str, contract: SkillBindingContract | None, assignment: Mapping[str, ResolvedRobotResource], ) -> ResolvedSkillBinding: - """Lower generic endpoints through the temporary current-core routes.""" + """Lower every required endpoint to one engine-owned action binding.""" assert contract is not None - manipulators: dict[str, str] = {} - end_effectors: dict[str, str] = {} + endpoints: list[EndpointBinding] = [] for slot in contract.slots: resource = assignment[slot.slot_id] for requirement in slot.endpoints: - if requirement.route is None: - continue endpoint = resource.endpoints[requirement.endpoint_id] - target = ( - manipulators - if requirement.route.target == "manipulator" - else end_effectors + endpoints.append( + EndpointBinding( + slot_id=slot.slot_id, + endpoint_id=requirement.endpoint_id, + resource_id=resource.resource_id, + adapter_id=endpoint.adapter_id, + target=endpoint.runtime_target, + capabilities=endpoint.capabilities, + commands=endpoint.commands, + claim_tokens=endpoint.claim_tokens, + joint_ids=endpoint.joint_ids, + ) ) - target[requirement.route.role] = endpoint.binding_values[ - requirement.route.target - ] return ResolvedSkillBinding( skill_id=skill_id, resources=assignment, action_binding=ActionBinding( - manipulators=manipulators, - end_effectors=end_effectors, + owner_id=self._engine.binding_owner_id, + endpoints=tuple(endpoints), ), claim=ResourceClaim.combine( tuple(resource.claim for resource in assignment.values()) 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 7d076b322..1965563b0 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -404,7 +404,6 @@ def _plan_pick_place_cycle( ) -> 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 ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -416,16 +415,26 @@ def _plan_pick_place_cycle( source_pose = self._cube.get_local_pose(to_matrix=True).to( device=self.device, dtype=torch.float32 ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + endpoints = { + "primary": { + "motion": "arm", + "grasp": "hand", + } + } + 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=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=self.PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.15, @@ -458,7 +467,7 @@ def _plan_pick_place_cycle( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=self.PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.14, diff --git a/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py index 5e09bac89..0a5d19dc7 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py +++ b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py @@ -211,7 +211,6 @@ def _plan_block_segment( ) -> tuple[torch.Tensor, Iterable[torch.Tensor], torch.Tensor]: """Plan an atomic PickUp followed by Place for one block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -233,9 +232,19 @@ def _plan_block_segment( source_pose[:, :3, :3], local_grasp_offset.unsqueeze(-1) ).squeeze(-1) grasp_pose[:, :3, 3] = source_pose[:, :3, 3] + world_grasp_offset - binding = ActionBinding( - manipulators={"primary": arm}, - end_effectors={"primary": hand}, + endpoints = { + "primary": { + "motion": arm, + "grasp": hand, + } + } + 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( ( @@ -245,7 +254,7 @@ def _plan_block_segment( self._object_semantics[uid], grasp_xpos=grasp_pose, ), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.12, @@ -277,7 +286,7 @@ def _plan_block_segment( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.15, diff --git a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py index 9001f0c73..9279e3037 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py +++ b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py @@ -133,7 +133,6 @@ def _plan_stack( ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: """Plan PickUp then Place while threading the held-object state.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -156,9 +155,19 @@ def _plan_stack( grasp_pose[:, :3, 3] = source_pose[:, :3, 3] + torch.tensor( GRASP_OFFSET, dtype=torch.float32, device=self.device ) - binding = ActionBinding( - manipulators={"primary": CONTROL_PART}, - end_effectors={"primary": HAND_CONTROL_PART}, + endpoints = { + "primary": { + "motion": CONTROL_PART, + "grasp": HAND_CONTROL_PART, + } + } + 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( ( @@ -168,7 +177,7 @@ def _plan_stack( self._stack_block_semantics, grasp_xpos=grasp_pose, ), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.12, @@ -201,7 +210,7 @@ def _plan_stack( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.10, diff --git a/examples/sim/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index b3105f248..e391852b0 100644 --- a/examples/sim/planners/curobo_planner.py +++ b/examples/sim/planners/curobo_planner.py @@ -59,7 +59,6 @@ visualization_cfg_from_args, ) from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -749,7 +748,10 @@ def main() -> None: ) ) engine = AtomicActionEngine(motion_generator) - binding = ActionBinding(manipulators={"primary": control_part}) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": control_part}}, + ) motion_policy = MotionPolicy( motion_source="motion_gen", plan_opts=CuroboPlanOptions( diff --git a/scripts/benchmark/atomic_action/move_end_effector_benchmark.py b/scripts/benchmark/atomic_action/move_end_effector_benchmark.py index 0f2f1de48..7630f9641 100644 --- a/scripts/benchmark/atomic_action/move_end_effector_benchmark.py +++ b/scripts/benchmark/atomic_action/move_end_effector_benchmark.py @@ -121,7 +121,6 @@ def _run_case( """Run one MoveEndEffector case.""" torch = ensure_torch() from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, @@ -129,6 +128,10 @@ def _run_case( reset_robot(robot, initial_qpos) target_pose = _make_pose(sim.device, pose_case.xyz) + binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( @@ -136,7 +139,7 @@ def _run_case( ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ) diff --git a/scripts/benchmark/atomic_action/move_held_object_benchmark.py b/scripts/benchmark/atomic_action/move_held_object_benchmark.py index 0f66c5b9d..35dbb0386 100644 --- a/scripts/benchmark/atomic_action/move_held_object_benchmark.py +++ b/scripts/benchmark/atomic_action/move_held_object_benchmark.py @@ -175,7 +175,6 @@ def _prepare_held_state( ): """Run PickUp precondition outside the timed MoveHeldObject block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -212,22 +211,26 @@ def _prepare_held_state( move_position = obj_pose[0, :3, 3].clone() move_position[2] = 0.36 move_target = make_pre_pick_eef_pose(robot, move_position) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + move_binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + pick_binding = atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ) result = atomic_engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "pick_up", GraspGoal(semantics=semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( approach_direction=resolve_pickup_approach_direction( @@ -269,7 +272,6 @@ def _run_case( ): """Run one MoveHeldObject benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -319,16 +321,17 @@ def _run_case( }, ) target_pose = _make_object_target_pose(sim.device, case.xyz) + binding = atomic_engine.bind_control_parts( + "move_held_object", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="move_held_object", goal=HeldObjectPoseGoal(object_target_pose=target_pose), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy( sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL ), diff --git a/scripts/benchmark/atomic_action/move_joints_benchmark.py b/scripts/benchmark/atomic_action/move_joints_benchmark.py index 82d43c164..abdfcc6bd 100644 --- a/scripts/benchmark/atomic_action/move_joints_benchmark.py +++ b/scripts/benchmark/atomic_action/move_joints_benchmark.py @@ -106,17 +106,19 @@ def _qpos(values, device): return torch.tensor(values, dtype=torch.float32, device=device) -def _targets_for_sequence(sequence_case: JointSequenceCase, device): +def _targets_for_sequence(atomic_engine, sequence_case: JointSequenceCase, device): """Build typed MoveJoints targets for a sequence case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, JointPositionGoal, MotionPolicy, ) targets = [] - binding = ActionBinding(manipulators={"primary": "arm"}) + binding = atomic_engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "arm"}}, + ) policy = MotionPolicy(sample_count=MOVE_JOINTS_SAMPLE_INTERVAL) for index, name in enumerate(sequence_case.sequence): if index == 0 and name == "ready": @@ -147,7 +149,7 @@ def _run_case( """Run one MoveJoints case.""" torch = ensure_torch() reset_robot(robot, initial_qpos) - steps = _targets_for_sequence(case, sim.device) + steps = _targets_for_sequence(atomic_engine, case, sim.device) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile(steps) ) diff --git a/scripts/benchmark/atomic_action/pickup_benchmark.py b/scripts/benchmark/atomic_action/pickup_benchmark.py index 4559d3e6e..f50885890 100644 --- a/scripts/benchmark/atomic_action/pickup_benchmark.py +++ b/scripts/benchmark/atomic_action/pickup_benchmark.py @@ -123,7 +123,6 @@ def _run_case( ): """Run one PickUp benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -175,16 +174,17 @@ def _run_case( build_gripper_collision_cfg=build_gripper_collision_cfg, build_grasp_generator_cfg=build_grasp_generator_cfg, ) + binding = atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics=semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( approach_direction=approach_direction, diff --git a/scripts/benchmark/atomic_action/place_benchmark.py b/scripts/benchmark/atomic_action/place_benchmark.py index 4c8242719..f63a7e73c 100644 --- a/scripts/benchmark/atomic_action/place_benchmark.py +++ b/scripts/benchmark/atomic_action/place_benchmark.py @@ -174,7 +174,6 @@ def _prepare_held_state( ): """Run PickUp precondition outside the timed Place block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -212,9 +211,9 @@ def _prepare_held_state( ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics=semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + binding=atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( @@ -255,7 +254,6 @@ def _run_case( ): """Run one Place benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -308,16 +306,17 @@ def _run_case( }, ) place_pose = _make_place_pose(sim.device, case.xyz) + binding = atomic_engine.bind_control_parts( + "place", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="place", goal=PlaceGoal(xpos=place_pose), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=PLACE_LIFT_HEIGHT, diff --git a/scripts/benchmark/atomic_action/press_benchmark.py b/scripts/benchmark/atomic_action/press_benchmark.py index a5687af7f..fa534703f 100644 --- a/scripts/benchmark/atomic_action/press_benchmark.py +++ b/scripts/benchmark/atomic_action/press_benchmark.py @@ -81,7 +81,6 @@ def _ensure_runtime_imports() -> None: import torch as torch_module from embodichain.lab.sim import SimulationManager as simulation_manager_cls from embodichain.lab.sim.atomic_actions import ( - ActionBinding as action_binding_cls, ActionInvocation as action_invocation_cls, AtomicActionEngine as atomic_action_engine_cls, ControlPartCommandProfile as control_part_command_profile_cls, @@ -125,7 +124,6 @@ def _ensure_runtime_imports() -> None: "SimulationManager": simulation_manager_cls, "AtomicActionEngine": atomic_action_engine_cls, "ControlPartCommandProfile": control_part_command_profile_cls, - "ActionBinding": action_binding_cls, "ActionInvocation": action_invocation_cls, "EndEffectorPoseGoal": end_effector_pose_target_cls, "MotionPolicy": motion_policy_cls, @@ -562,27 +560,31 @@ def _timed_atomic_run( press_target: torch.Tensor, ) -> tuple[float, dict[str, float], float, bool, torch.Tensor]: """Run a timed atomic-action sequence and return timing/memory/results.""" + move_binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + press_binding = atomic_engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) _reset_peak_gpu_memory() mem_before = _memory_snapshot() _sync_cuda() start = time.perf_counter() - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) result = atomic_engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "press", PressGoal(xpos=press_target), - binding, + press_binding, MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), skill_options=PressOptions( hand_interp_steps=HAND_INTERP_STEPS, diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index ac0594ef3..00ce492da 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -38,7 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AssembleAffordance, AssembleGoal, @@ -316,23 +315,28 @@ def run_assemble_demo( assemble_object_entity=can, assemble_to_base_pose=assemble_to_base, ) - binding = ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, + endpoint_mapping = {"primary": {"motion": "left_arm", "grasp": "left_hand"}} + pick_binding = engine.bind_control_parts( + "pick_up", + endpoint_mapping, + ) + place_binding = engine.bind_control_parts( + "place", + endpoint_mapping, ) compiled = engine.compile( ( ActionInvocation( "pick_up", GraspGoal(can_semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICKUP_SAMPLE_INTERVAL), skill_options=pick_up_options, ), ActionInvocation( "place", AssembleGoal(affordance=assemble_affordance), - binding, + place_binding, MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=place_options, ), diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index b3eeb15b7..a81fc81c0 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -36,7 +36,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -412,15 +411,19 @@ def run_coordinated_pickment_demo( ) start_time = time.time() + binding = engine.bind_control_parts( + "coordinated_pickment", + { + "left": {"motion": "left_arm", "grasp": "left_hand"}, + "right": {"motion": "right_arm", "grasp": "right_hand"}, + }, + ) compiled = engine.compile( ( ActionInvocation( "coordinated_pickment", pickment_target, - ActionBinding( - manipulators={"left": "left_arm", "right": "right_arm"}, - end_effectors={"left": "left_hand", "right": "right_hand"}, - ), + binding, MotionPolicy(sample_count=PICKMENT_SAMPLE_INTERVAL), skill_options=pickment_options, ), diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index b715cefda..7f3d69f07 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -38,7 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -620,6 +619,14 @@ def run_coordinated_placement_demo( sim.device, z_clearance=PAN_GRASP_Z_CLEARANCE, ) + left_pick_binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "left_arm", "grasp": "left_hand"}}, + ) + right_pick_binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "right_arm", "grasp": "right_hand"}}, + ) pick_invocations = ( ActionInvocation( skill_id="pick_up", @@ -627,10 +634,7 @@ def run_coordinated_placement_demo( semantics=bread_semantics, grasp_xpos=broadcast_pose_batch(bread_grasp_pose, num_envs=n_envs), ), - binding=ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ), + binding=left_pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=left_pick_options, ), @@ -640,10 +644,7 @@ def run_coordinated_placement_demo( semantics=pan_semantics, grasp_xpos=broadcast_pose_batch(pan_grasp_pose, num_envs=n_envs), ), - binding=ActionBinding( - manipulators={"primary": "right_arm"}, - end_effectors={"primary": "right_hand"}, - ), + binding=right_pick_binding, motion_policy=MotionPolicy(sample_count=PAN_PICK_SAMPLE_INTERVAL), skill_options=right_pick_options, ), @@ -663,8 +664,12 @@ def run_coordinated_placement_demo( if not pick_compiled.plan_success.all(): logger.log_warning("Failed to plan right pan pick-up trajectory.") return - left_pick_traj = left_pick_result.trajectory.positions - right_pick_traj = right_pick_result.trajectory.positions + left_pick_trajectory = left_pick_result.joint_trajectory + right_pick_trajectory = right_pick_result.joint_trajectory + if left_pick_trajectory is None or right_pick_trajectory is None: + raise RuntimeError("PickUp did not produce joint trajectories.") + left_pick_traj = left_pick_trajectory.positions + right_pick_traj = right_pick_trajectory.positions state = pick_compiled.projected_context bread_held_state = state.get_held_object("left_arm") if bread_held_state is None: @@ -690,7 +695,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: replay_trajectory( sim, robot, - left_pick_result.trajectory, + left_pick_trajectory, args, video_prefix="", hold_steps=0, @@ -703,7 +708,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: replay_trajectory( sim, robot, - right_pick_result.trajectory, + right_pick_trajectory, args, video_prefix="", hold_steps=0, @@ -785,21 +790,19 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: release=True, ) start_time = time.time() + placement_binding = engine.bind_control_parts( + "coordinated_placement", + { + "placing": {"motion": "left_arm", "grasp": "left_hand"}, + "support": {"motion": "right_arm", "grasp": "right_hand"}, + }, + ) placement_compiled = engine.compile( ( ActionInvocation( skill_id="coordinated_placement", goal=coordinated_target, - binding=ActionBinding( - manipulators={ - "placing": "left_arm", - "support": "right_arm", - }, - end_effectors={ - "placing": "left_hand", - "support": "right_hand", - }, - ), + binding=placement_binding, motion_policy=MotionPolicy(sample_count=COORDINATED_SAMPLE_INTERVAL), skill_options=coordinated_options, ), diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index fc679530d..03ab89f29 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -33,13 +33,14 @@ from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, + JointPositionPayload, + JointPositionTarget, MotionPolicy, RecoveryPolicy, RigidObjectSceneProvider, @@ -47,6 +48,7 @@ RunnerStep, SimulationExecutionAdapter, TaskState, + TimedCommandSequence, ) from embodichain.lab.sim.cfg import RigidBodyAttributesCfg from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg, Robot @@ -294,21 +296,35 @@ def _minimum_cuboid_clearance( return (outside_distance + inside_distance).amin(dim=1) -def _trajectory_eef_positions( +def _command_eef_positions( robot: Robot, - trajectory_positions: torch.Tensor, + commands: TimedCommandSequence, *, control_part: str, ) -> torch.Tensor: - """Convert a full-robot joint trajectory to batched EEF positions.""" - if trajectory_positions.dim() != 3: - raise ValueError("trajectory_positions must have shape (B, N, robot_dof).") - joint_ids = robot.get_joint_ids(name=control_part) - arm_trajectory = trajectory_positions[:, :, joint_ids] + """Convert one endpoint command sequence to batched EEF positions.""" + if not commands.frames: + raise ValueError("commands must contain at least one frame.") positions = [] - for waypoint_index in range(arm_trajectory.shape[1]): + for frame in commands.frames: + matching_commands = tuple( + command + for command in frame.commands + if isinstance(command.target, JointPositionTarget) + and command.target.control_part == control_part + ) + if len(matching_commands) != 1: + raise ValueError( + f"Expected one joint command for control part {control_part!r}, " + f"got {len(matching_commands)}." + ) + payload = matching_commands[0].payload + if not isinstance(payload, JointPositionPayload): + raise TypeError( + f"Control part {control_part!r} did not receive joint positions." + ) pose = robot.compute_fk( - qpos=arm_trajectory[:, waypoint_index], + qpos=payload.positions, name=control_part, to_matrix=True, ) @@ -457,10 +473,14 @@ def main() -> None: device=target_pose.device, ) engine = AtomicActionEngine(motion_generator=motion_gen) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": CONTROL_PART}}, + ) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(target_pose), - binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + binding=binding, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_COUNT, @@ -475,9 +495,9 @@ def main() -> None: ) task_state = TaskState.empty(robot.get_qpos().shape[0], robot.device) session = engine.start((invocation,), adapter.observe(task_state)) - initial_eef_path = _trajectory_eef_positions( + initial_eef_path = _command_eef_positions( robot, - session.active_trajectory.positions, + session.active_commands, control_part=CONTROL_PART, ) blocking_obstacle_pose, blocking_waypoint_index = _blocking_obstacle_pose( @@ -586,9 +606,9 @@ def on_step(step: RunnerStep) -> None: and replanned_eef_path is None and ExecutionEventKind.COLLISION_WORLD_CHANGED in observed_events ): - replanned_eef_path = _trajectory_eef_positions( + replanned_eef_path = _command_eef_positions( robot, - session.active_trajectory.positions, + session.active_commands, control_part=CONTROL_PART, ) replan_detour = _maximum_path_deviation( diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index bec85110e..042de17be 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -35,7 +35,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, AtomicActionEngine, @@ -258,31 +257,30 @@ def run_handover_demo( # wait for object to drop for _ in range(20): sim.update(step=10) + pick_binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "left_arm", "grasp": "left_hand"}}, + ) + handover_binding = engine.bind_control_parts( + "hand_over", + { + "source": {"motion": "left_arm", "grasp": "left_hand"}, + "destination": {"motion": "right_arm", "grasp": "right_hand"}, + }, + ) compiled = engine.compile( ( ActionInvocation( "pick_up", GraspGoal(object_semantics), - ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ), + pick_binding, MotionPolicy(sample_count=PICKUP_SAMPLE_INTERVAL), skill_options=pick_up_options, ), ActionInvocation( "hand_over", GraspGoal(object_semantics), - ActionBinding( - manipulators={ - "source": "left_arm", - "destination": "right_arm", - }, - end_effectors={ - "source": "left_hand", - "destination": "right_hand", - }, - ), + handover_binding, MotionPolicy(sample_count=HANDOVER_SAMPLE_INTERVAL), skill_options=handover_options, ), diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index f46dbe250..b6d203ebc 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -97,7 +96,10 @@ def main() -> None: ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(broadcast_waypoint_pose_batch(poses, n_envs)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ), motion_policy=MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ) diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index f320b6118..bff059972 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -30,7 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -148,22 +147,32 @@ def main() -> None: sim, args, "Inspect the paper cup, then press Enter to plan..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + motion_mapping = {"primary": {"motion": "arm"}} + manipulation_mapping = {"primary": {"motion": "arm", "grasp": "hand"}} + move_binding = engine.bind_control_parts( + "move_end_effector", + motion_mapping, + ) + pick_binding = engine.bind_control_parts( + "pick_up", + manipulation_mapping, + ) + held_object_binding = engine.bind_control_parts( + "move_held_object", + manipulation_mapping, ) compiled = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "pick_up", GraspGoal(semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.15, @@ -174,7 +183,7 @@ def main() -> None: ActionInvocation( "move_held_object", HeldObjectPoseGoal(object_target), - binding, + held_object_binding, MotionPolicy(sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL), ), ) diff --git a/scripts/tutorials/atomic_action/move_joints.py b/scripts/tutorials/atomic_action/move_joints.py index 7fbee6948..4bc32ea7e 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -95,7 +94,10 @@ def main() -> None: waypoints = ( torch.stack([mid, home]).unsqueeze(0).repeat(robot.get_qpos().shape[0], 1, 1) ) - binding = ActionBinding(manipulators={"primary": "arm"}) + binding = engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "arm"}}, + ) policy = MotionPolicy(sample_count=MOVE_JOINTS_SAMPLE_INTERVAL) compiled = engine.compile( ( diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index a34efec07..db9da7b13 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -31,7 +31,6 @@ from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, Affordance, AtomicActionEngine, @@ -277,10 +276,6 @@ def main() -> None: entity=target, entity_id=TARGET_ENTITY_ID, ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) engine = AtomicActionEngine( motion_generator=motion_gen, control_profiles={ @@ -290,6 +285,10 @@ def main() -> None: ) }, ) + binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) pick_invocation = ActionInvocation( skill_id="pick_up", goal=GraspGoal( diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index a9c361671..325d36b74 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -158,9 +157,9 @@ def main() -> None: ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + binding=engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index ae30526ad..0b7fc74c1 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -156,16 +155,21 @@ def main() -> None: sim, args, "Inspect the cube, then press Enter to plan PickUp -> Place..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + endpoint_mapping = {"primary": {"motion": "arm", "grasp": "hand"}} + pick_binding = engine.bind_control_parts( + "pick_up", + endpoint_mapping, + ) + place_binding = engine.bind_control_parts( + "place", + endpoint_mapping, ) compiled = engine.compile( ( ActionInvocation( "pick_up", GraspGoal(semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.15, @@ -180,7 +184,7 @@ def main() -> None: place_poses, robot.get_qpos().shape[0] ) ), - binding, + place_binding, MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=PLACE_LIFT_HEIGHT, diff --git a/scripts/tutorials/atomic_action/press.py b/scripts/tutorials/atomic_action/press.py index 59a4e5b6e..b21ef10d7 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -183,22 +182,26 @@ def main() -> None: sim, args, "Inspect the wooden block, then press Enter to plan..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + move_binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + press_binding = engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, ) compiled = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "press", PressGoal(press_target), - binding, + press_binding, MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), skill_options=PressOptions( hand_interp_steps=HAND_INTERP_STEPS, diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 46384558b..01bcdb94f 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -27,6 +27,7 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionPlan, Affordance, AntipodalAffordance, AssembleAffordance, @@ -50,6 +51,8 @@ HeldObjectPoseGoal, HeldObjectState, JointPositionGoal, + JointPositionPayload, + JointPositionTarget, MotionPolicy, MoveEndEffector, MoveEndEffectorOptions, @@ -71,6 +74,7 @@ SceneEntityPose, SceneSnapshot, TaskState, + TimedTrajectory, ) from embodichain.lab.sim.planners import ( MotionGenerator, @@ -88,6 +92,7 @@ DUAL_ROBOT_DOF = DUAL_ARM_DOF + 2 * HAND_DOF ActionT = TypeVar("ActionT", bound=AtomicAction) +_ACTION_ENGINES: dict[int, AtomicActionEngine] = {} @pytest.fixture(autouse=True) @@ -205,6 +210,7 @@ def _bind_action( load_builtins=False, ) engine.register(action) + _ACTION_ENGINES[id(action)] = engine return action @@ -250,27 +256,79 @@ def _target_scene( ) -def _binding() -> ActionBinding: - return ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, +def _binding( + action: AtomicAction, + *, + motion: str = "arm", + grasp: str = "hand", +) -> 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} + return _ACTION_ENGINES[id(action)].bind_control_parts( + action.skill_id, + { + slot.slot_id: { + endpoint.endpoint_id: endpoint_parts[endpoint.endpoint_id] + for endpoint in slot.endpoints + } + for slot in contract.slots + }, ) def _invocation( - skill_id: str, + action: AtomicAction, goal, *, sample_count: int = 20, ) -> ActionInvocation: return ActionInvocation( - skill_id=skill_id, + skill_id=action.skill_id, goal=goal, - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=sample_count), ) +def _joint_trajectory(plan: ActionPlan) -> TimedTrajectory: + """Return the owned planner trajectory for a joint-feedback plan.""" + assert plan.joint_trajectory is not None + return plan.joint_trajectory + + +def _joint_command_positions( + plan: ActionPlan, + control_part: str, +) -> torch.Tensor: + """Stack runtime joint commands sent to one concrete control part.""" + return torch.stack( + [payload.positions for payload in _joint_command_payloads(plan, control_part)], + dim=1, + ) + + +def _joint_command_payloads( + plan: ActionPlan, + control_part: str, +) -> tuple[JointPositionPayload, ...]: + """Return runtime joint payloads sent to one concrete control part.""" + payloads: list[JointPositionPayload] = [] + for frame in plan.commands.frames: + matching = [ + command + for command in frame.commands + if isinstance(command.target, JointPositionTarget) + and command.target.control_part == control_part + ] + assert len(matching) == 1 + payload = matching[0].payload + assert isinstance(payload, JointPositionPayload) + payloads.append(payload) + return tuple(payloads) + + def _semantics(*, entity_id: str | None = None) -> ObjectSemantics: entity = Mock() entity.get_local_pose.return_value = torch.eye(4).repeat(NUM_ENVS, 1, 1) @@ -380,17 +438,21 @@ def _dual_context( def _dual_binding( - first_role: str, - second_role: str, + action: AtomicAction, + first_slot: str, + second_slot: str, ) -> ActionBinding: - return ActionBinding( - manipulators={ - first_role: "left_arm", - second_role: "right_arm", - }, - end_effectors={ - first_role: "left_hand", - second_role: "right_hand", + return _ACTION_ENGINES[id(action)].bind_control_parts( + action.skill_id, + { + first_slot: { + "motion": "left_arm", + "grasp": "left_hand", + }, + second_slot: { + "motion": "right_arm", + "grasp": "right_hand", + }, }, ) @@ -475,7 +537,7 @@ def test_move_end_effector_returns_full_robot_timed_plan() -> None: plan = _plan_action( action, _invocation( - "move_end_effector", + action, EndEffectorPoseGoal(torch.eye(4)), sample_count=10, ), @@ -483,8 +545,9 @@ def test_move_end_effector_returns_full_robot_timed_plan() -> None: ) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.positions.shape == (NUM_ENVS, 10, ROBOT_DOF) - assert plan.trajectory.duration.tolist() == pytest.approx([0.15, 0.15]) + assert plan.commands.frame_count == 10 + assert [target.target_id for target in plan.commands.targets] == ["arm"] + assert _joint_trajectory(plan).duration.tolist() == pytest.approx([0.15, 0.15]) assert plan.expected_effects.is_empty @@ -509,12 +572,13 @@ def test_move_joints_uses_binding_and_preserves_uncontrolled_joints() -> None: plan = _plan_action( action, - _invocation("move_joints", JointPositionGoal("ready"), sample_count=8), + _invocation(action, JointPositionGoal("ready"), sample_count=8), context, ) - assert torch.allclose(plan.trajectory.positions[:, -1, :ARM_DOF], named["ready"]) - assert torch.all(plan.trajectory.positions[:, :, ARM_DOF:] == 0.7) + arm_positions = _joint_command_positions(plan, "arm") + assert torch.allclose(arm_positions[:, -1], named["ready"]) + assert [target.target_id for target in plan.commands.targets] == ["arm"] def test_pick_and_place_declare_effects_without_mutating_context() -> None: @@ -527,7 +591,7 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: pick_plan = _plan_action( pick, - _invocation("pick_up", GraspGoal(semantics=semantics, grasp_xpos=grasp)), + _invocation(pick, GraspGoal(semantics=semantics, grasp_xpos=grasp)), initial, ) picked_task = pick_plan.expected_effects.apply(initial.task, pick_plan.plan_success) @@ -544,7 +608,7 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: ) place_plan = _plan_action( place, - _invocation("place", PlaceGoal(torch.eye(4))), + _invocation(place, PlaceGoal(torch.eye(4))), picked_context, ) placed_task = place_plan.expected_effects.apply( @@ -559,7 +623,7 @@ def test_move_held_object_requires_projected_attachment() -> None: generator = _motion_generator() action = _bind_action(generator, MoveHeldObject()) invocation = _invocation( - "move_held_object", + action, HeldObjectPoseGoal(torch.eye(4)), sample_count=10, ) @@ -589,7 +653,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(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=10), skill_options=MoveHeldObjectOptions(pick_rotate_upright=0.25), ) @@ -612,12 +676,12 @@ def test_press_uses_invocation_sample_budget() -> None: plan = _plan_action( action, - _invocation("press", PressGoal(torch.eye(4)), sample_count=12), + _invocation(action, PressGoal(torch.eye(4)), sample_count=12), _context(), ) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.waypoint_count == 12 + assert plan.commands.frame_count == 12 assert plan.expected_effects.is_empty @@ -634,7 +698,7 @@ def test_move_joints_rejects_binding_with_wrong_goal_skill() -> None: invocation = ActionInvocation( skill_id="move_end_effector", goal=JointPositionGoal(torch.zeros(ARM_DOF)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_binding(action), ) with pytest.raises(ValueError, match="skill_id"): action.resolve_request(invocation) @@ -658,16 +722,19 @@ def test_planner_timing_is_preserved_in_simple_action() -> None: invocation = ActionInvocation( skill_id="move_joints", goal=JointPositionGoal(torch.ones(ARM_DOF)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_binding(action), motion_policy=MotionPolicy(strategy="motion_gen", sample_count=3), ) plan = _plan_action(action, invocation, _context()) - assert plan.trajectory.duration.tolist() == pytest.approx([0.3, 0.3]) - assert plan.trajectory.velocities is not None - assert torch.all(plan.trajectory.velocities[:, :, :ARM_DOF] == 0.5) - assert torch.all(plan.trajectory.velocities[:, :, ARM_DOF:] == 0.0) + trajectory = _joint_trajectory(plan) + payloads = _joint_command_payloads(plan, "arm") + assert trajectory.duration.tolist() == pytest.approx([0.3, 0.3]) + assert all(payload.velocities is not None for payload in payloads) + assert torch.all( + torch.stack([payload.velocities for payload in payloads], dim=1) == 0.5 + ) def test_move_end_effector_visits_batched_waypoints_in_order() -> None: @@ -692,7 +759,7 @@ def compute_ik( plan = _plan_action( action, _invocation( - "move_end_effector", + action, EndEffectorPoseGoal(waypoints), sample_count=9, ), @@ -725,19 +792,20 @@ def test_move_joints_visits_waypoints_and_rejects_unknown_names() -> None: plan = _plan_action( action, _invocation( - "move_joints", + action, JointPositionGoal(waypoints), sample_count=7, ), _context(), ) - assert torch.allclose(plan.trajectory.positions[:, 3, :ARM_DOF], waypoints[:, 0]) - assert torch.allclose(plan.trajectory.positions[:, -1, :ARM_DOF], waypoints[:, 1]) + arm_positions = _joint_command_positions(plan, "arm") + assert torch.allclose(arm_positions[:, 3], waypoints[:, 0]) + assert torch.allclose(arm_positions[:, -1], waypoints[:, 1]) with pytest.raises(KeyError, match="has no command"): _plan_action( action, - _invocation("move_joints", JointPositionGoal("missing")), + _invocation(action, JointPositionGoal("missing")), _context(), ) @@ -767,7 +835,7 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: request = action.resolve_request( _invocation( - "pick_up", + action, GraspGoal(semantics=semantics, grasp_xpos=grasp), sample_count=20, ) @@ -818,17 +886,19 @@ def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: plan = _plan_action( action, - _invocation("pick_up", GraspGoal(semantics=semantics), sample_count=20), + _invocation(action, GraspGoal(semantics=semantics), sample_count=20), context, ) projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] - assert not torch.allclose(plan.trajectory.positions[0], context.robot.qpos[0]) + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) assert torch.allclose( - plan.trajectory.positions[1], + trajectory.positions[1], context.robot.qpos[1].unsqueeze(0).expand(20, -1), ) + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) held = projected.get_held_object("arm") assert held is not None assert held.env_mask.tolist() == [True, False] @@ -855,7 +925,7 @@ def test_pick_resolves_late_bound_scene_grasp_and_declares_dependency() -> None: plan = _plan_action( action, _invocation( - "pick_up", + action, GraspGoal( semantics=semantics, grasp_xpos=SceneEntityPose( @@ -903,9 +973,11 @@ def test_pick_session_replans_when_late_bound_target_moves() -> None: }, load_builtins=False, ) - engine.register(PickUp()) + action = PickUp() + engine.register(action) + _ACTION_ENGINES[id(action)] = engine invocation = _invocation( - "pick_up", + action, GraspGoal( semantics=semantics, grasp_xpos=SceneEntityPose("target"), @@ -940,9 +1012,10 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: semantics=_semantics(entity_id="target"), grasp_xpos=torch.eye(4), ), - binding=ActionBinding( - manipulators={"primary": "alternate_arm"}, - end_effectors={"primary": "alternate_hand"}, + binding=_binding( + action, + motion="alternate_arm", + grasp="alternate_hand", ), motion_policy=MotionPolicy(sample_count=20), ) @@ -977,12 +1050,12 @@ def test_press_closes_hand_without_changing_projected_attachment() -> None: plan = _plan_action( action, - _invocation("press", PressGoal(torch.eye(4)), sample_count=12), + _invocation(action, PressGoal(torch.eye(4)), sample_count=12), _context(task), ) projected = plan.expected_effects.apply(task, plan.plan_success) - assert torch.all(plan.trajectory.positions[:, -1, ARM_DOF:] == 1.0) + assert torch.all(_joint_command_positions(plan, "hand")[:, -1] == 1.0) projected_held = projected.get_held_object("arm") assert projected_held is not None assert projected_held.semantics is held.semantics @@ -1043,7 +1116,7 @@ def plan_from_start( semantics=semantics, grasp_xpos=SceneEntityPose("unused_grasp_pose"), ), - binding=_dual_binding("source", "destination"), + binding=_dual_binding(action, "source", "destination"), motion_policy=MotionPolicy(sample_count=30), ) @@ -1122,7 +1195,7 @@ def fail_second_receiving_arm( invocation = ActionInvocation( skill_id="hand_over", goal=GraspGoal(semantics=semantics), - binding=_dual_binding("source", "destination"), + binding=_dual_binding(action, "source", "destination"), motion_policy=MotionPolicy(sample_count=30), ) @@ -1131,11 +1204,13 @@ def fail_second_receiving_arm( projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] - assert not torch.allclose(plan.trajectory.positions[0], context.robot.qpos[0]) + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) assert torch.allclose( - plan.trajectory.positions[1], + trajectory.positions[1], 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") assert received is not None assert received.env_mask.tolist() == [True, False] @@ -1163,7 +1238,7 @@ def test_handover_rejects_goal_for_a_different_held_object() -> None: invocation = ActionInvocation( skill_id="hand_over", goal=GraspGoal(semantics=goal_semantics), - binding=_dual_binding("source", "destination"), + binding=_dual_binding(action, "source", "destination"), ) with pytest.raises(ValueError, match="must identify the object held"): @@ -1203,7 +1278,7 @@ 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("left", "right"), + binding=_dual_binding(action, "left", "right"), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context() @@ -1213,7 +1288,13 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.positions.shape == (NUM_ENVS, 30, DUAL_ROBOT_DOF) + assert plan.commands.frame_count == 30 + assert {target.target_id for target in plan.commands.targets} == { + "left_arm", + "left_hand", + "right_arm", + "right_hand", + } assert plan.scene_dependencies == () request.goal.semantics.entity.get_local_pose.assert_not_called() assert projected.get_held_object("left_arm") is None @@ -1265,7 +1346,7 @@ def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: semantics=semantics, object_target_pose=object_pose, ), - binding=_dual_binding("left", "right"), + binding=_dual_binding(action, "left", "right"), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) @@ -1316,7 +1397,7 @@ def test_assemble_place_uses_explicit_base_snapshot() -> None: request = action.resolve_request( _invocation( - "place", + action, AssembleGoal( affordance=affordance, base_pose=SceneEntityPose("base"), @@ -1343,7 +1424,7 @@ def test_assemble_place_legacy_base_entity_warns() -> None: ) request = action.resolve_request( - _invocation("place", AssembleGoal(affordance=affordance)) + _invocation(action, AssembleGoal(affordance=affordance)) ) with pytest.warns(DeprecationWarning, match="base_pose"): plan = action.plan(request, _context(task)) @@ -1397,7 +1478,7 @@ def fail_second_environment( object_target_pose=target_pose, object_initial_pose=torch.eye(4), ), - binding=_dual_binding("left", "right"), + binding=_dual_binding(action, "left", "right"), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context() @@ -1406,11 +1487,13 @@ def fail_second_environment( projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] - assert not torch.allclose(plan.trajectory.positions[0], context.robot.qpos[0]) + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) assert torch.allclose( - plan.trajectory.positions[1], + trajectory.positions[1], context.robot.qpos[1].unsqueeze(0).repeat(30, 1), ) + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) held = projected.get_coordinated_held_object("left_arm", "right_arm") assert held is not None assert held.env_mask.tolist() == [True, False] @@ -1456,14 +1539,19 @@ def test_coordinated_pick_fails_when_affordance_has_no_grasp() -> None: object_target_pose=torch.eye(4), object_initial_pose=torch.eye(4), ), - binding=_dual_binding("left", "right"), + binding=_dual_binding(action, "left", "right"), motion_policy=MotionPolicy(sample_count=30), ) plan = _plan_action(action, invocation, _dual_context()) assert plan.plan_success.tolist() == [False, False] - assert plan.trajectory.positions.shape == (NUM_ENVS, 0, DUAL_ROBOT_DOF) + assert plan.commands.frame_count == 0 + assert _joint_trajectory(plan).positions.shape == ( + NUM_ENVS, + 0, + DUAL_ROBOT_DOF, + ) def test_coordinated_placement_projects_release_and_support_attachment() -> None: @@ -1495,7 +1583,7 @@ 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("placing", "support"), + binding=_dual_binding(action, "placing", "support"), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context(task) @@ -1504,7 +1592,13 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.positions.shape == (NUM_ENVS, 30, DUAL_ROBOT_DOF) + assert plan.commands.frame_count == 30 + assert {target.target_id for target in plan.commands.targets} == { + "left_arm", + "left_hand", + "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 @@ -1566,7 +1660,7 @@ def fail_second_support_arm( placing_object_target_pose=torch.eye(4), support_object_target_pose=torch.eye(4), ), - binding=_dual_binding("placing", "support"), + binding=_dual_binding(action, "placing", "support"), motion_policy=MotionPolicy(sample_count=30), ) @@ -1574,11 +1668,13 @@ def fail_second_support_arm( projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] - assert not torch.allclose(plan.trajectory.positions[0], context.robot.qpos[0]) + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) assert torch.allclose( - plan.trajectory.positions[1], + trajectory.positions[1], context.robot.qpos[1].unsqueeze(0).expand(30, -1), ) + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) supported = projected.get_held_object("right_arm") assert supported is not None assert supported.env_mask.tolist() == [True, True] @@ -1604,7 +1700,7 @@ def test_coordinated_actions_reject_curobo_motion_generation() -> None: object_target_pose=torch.eye(4), object_initial_pose=torch.eye(4), ), - binding=_dual_binding("left", "right"), + binding=_dual_binding(pick, "left", "right"), motion_policy=policy, ) @@ -1618,7 +1714,7 @@ def test_coordinated_actions_reject_curobo_motion_generation() -> None: placement_invocation = ActionInvocation( skill_id="coordinated_placement", goal=CoordinatedPlacementGoal(torch.eye(4), torch.eye(4)), - binding=_dual_binding("placing", "support"), + binding=_dual_binding(placement, "placing", "support"), motion_policy=policy, ) with pytest.raises(ValueError, match="not supported"): diff --git a/tests/sim/atomic_actions/test_control.py b/tests/sim/atomic_actions/test_control.py index 32507b02f..996944c09 100644 --- a/tests/sim/atomic_actions/test_control.py +++ b/tests/sim/atomic_actions/test_control.py @@ -24,12 +24,15 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionControlOverrides, ActionPlanningServices, ControlCommand, ControlPartCommandProfile, + DisjointSlotEndpoints, JointPositionCommand, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, ) @@ -45,6 +48,18 @@ def equivalent_to(self, other: ControlCommand) -> bool: return isinstance(other, _BrokenSnapshotCommand) +class _SelfSnapshotCommand(ControlCommand): + """Command double that leaks its source instance as the snapshot.""" + + def snapshot(self) -> ControlCommand: + """Return this instance in violation of ownership isolation.""" + return self + + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether another command has this test-only type.""" + return isinstance(other, _SelfSnapshotCommand) + + def _services() -> ActionPlanningServices: robot = Mock() robot.device = torch.device("cpu") @@ -67,6 +82,33 @@ def _services() -> ActionPlanningServices: ) +def _contract() -> SkillBindingContract: + """Return the endpoint contract used by the direct-binding tests.""" + return SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement(endpoint_id="motion"), + SkillEndpointRequirement( + endpoint_id="grasp", + required_commands={"grasp": JointPositionCommand}, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) + + +def _binding(services: ActionPlanningServices): + """Bind the test contract to concrete robot control parts.""" + return services.bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) + + def test_joint_position_command_broadcasts_owned_batch() -> None: source = torch.tensor([0.1, 0.2]) command = JointPositionCommand(source) @@ -91,6 +133,11 @@ def test_control_profile_rejects_invalid_command_snapshot_type() -> None: ControlPartCommandProfile(commands={"stop": _BrokenSnapshotCommand()}) +def test_control_profile_rejects_command_snapshot_alias() -> None: + with pytest.raises(TypeError, match="independently owned"): + ControlPartCommandProfile(commands={"stop": _SelfSnapshotCommand()}) + + def test_control_profile_rejects_command_name_outer_whitespace() -> None: with pytest.raises(ValueError, match="outer whitespace"): ControlPartCommandProfile( @@ -98,65 +145,76 @@ def test_control_profile_rejects_command_name_outer_whitespace() -> None: ) +def test_resource_free_contract_does_not_require_robot_control_parts() -> None: + robot = object() + generator = Mock(robot=robot, device=torch.device("cpu")) + services = ActionPlanningServices(generator) + + binding = services.bind_control_parts(SkillBindingContract(), {}) + + assert binding.owner_id == services.binding_owner_id + assert binding.endpoints == () + + def test_control_profile_is_resolved_from_robot_control_part() -> None: - resolved = _services().resolve_binding( - ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) - ) + resolved = _binding(_services()) - grasp = resolved.end_effector().joint_positions( + grasp = resolved.endpoint("primary", "grasp").joint_positions( "grasp", n_envs=2, device="cpu", ) assert grasp.tolist() == [[1.0, 1.0], [1.0, 1.0]] - with pytest.raises(KeyError, match="Available commands"): - resolved.end_effector().joint_positions( + with pytest.raises(KeyError, match="available commands"): + resolved.endpoint("primary", "grasp").joint_positions( "pinch", n_envs=2, device="cpu", ) -def test_invocation_override_replaces_only_resolved_role_snapshot() -> None: +def test_invocation_override_replaces_only_resolved_endpoint_snapshot() -> None: services = _services() override_source = torch.full((2,), 0.4) overrides = ActionControlOverrides( - end_effectors={ - "primary": {"grasp": JointPositionCommand(override_source)}, + endpoints={ + "primary": { + "grasp": {"grasp": JointPositionCommand(override_source)}, + }, } ) override_source.fill_(8.0) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) + binding = _binding(services) - overridden = services.resolve_binding(binding, overrides) - base = services.resolve_binding(binding) - overrides.end_effectors["primary"]["grasp"].positions.fill_(6.0) # type: ignore[attr-defined] + overridden = services.apply_command_overrides(binding, overrides) + base = services.apply_command_overrides(binding, ActionControlOverrides()) + overrides.endpoints["primary"]["grasp"]["grasp"].positions.fill_(6.0) # type: ignore[attr-defined] assert torch.allclose( - overridden.end_effector().joint_positions("grasp", n_envs=1, device="cpu"), + overridden.endpoint("primary", "grasp").joint_positions( + "grasp", n_envs=1, device="cpu" + ), torch.full((1, 2), 0.4), ) assert torch.equal( - base.end_effector().joint_positions("grasp", n_envs=1, device="cpu"), + base.endpoint("primary", "grasp").joint_positions( + "grasp", n_envs=1, device="cpu" + ), torch.ones(1, 2), ) -def test_override_rejects_role_not_present_in_binding() -> None: +def test_override_rejects_endpoint_not_present_in_binding() -> None: services = _services() - binding = ActionBinding(end_effectors={"primary": "hand"}) + binding = _binding(services) overrides = ActionControlOverrides( - end_effectors={ - "destination": {"open": JointPositionCommand(torch.zeros(2))}, + endpoints={ + "destination": { + "grasp": {"open": JointPositionCommand(torch.zeros(2))}, + }, } ) - with pytest.raises(KeyError, match="unbound end effector roles"): - services.resolve_binding(binding, overrides) + with pytest.raises(KeyError, match="unbound endpoints"): + services.apply_command_overrides(binding, overrides) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 618da4545..51c44393b 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 FrozenInstanceError +from dataclasses import dataclass, FrozenInstanceError from unittest.mock import Mock import pytest @@ -31,23 +31,32 @@ ActionPlan, Affordance, AtomicAction, + AtomicActionEngine, CoordinatedHeldObjectState, DynamicCollisionMode, + EndpointBinding, + EndpointCommand, EndEffectorPoseGoal, EntityState, + ExecutionFeedbackMode, HeldObjectState, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, PlannerDiagnostics, PlanningContext, RecoveryPolicy, - ResolvedActionBinding, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, + RuntimeEndpointTarget, SceneEntityPose, SceneSnapshot, + SkillBindingContract, StateDelta, TaskState, + TimedCommandSequence, TimedTrajectory, ) from embodichain.lab.sim.atomic_actions.goals import ( @@ -108,13 +117,112 @@ def _context(scene: SceneSnapshot | None = None) -> PlanningContext: ) +def _command_sequence( + *, + env_ids: torch.Tensor, + frame_count: int, + targets: tuple[JointPositionTarget, ...] | None = None, + positions: tuple[torch.Tensor, ...] | None = None, + velocities: tuple[torch.Tensor | None, ...] | None = None, +) -> TimedCommandSequence: + batch_size = int(env_ids.shape[0]) + if targets is None: + target = JointPositionTarget("arm", (0, 1)) + targets = (target,) * frame_count + if len(targets) != frame_count: + raise ValueError("targets must contain one value per command frame.") + if positions is not None and len(positions) != frame_count: + raise ValueError("positions must contain one value per command frame.") + if velocities is not None and len(velocities) != frame_count: + raise ValueError("velocities must contain one value per command frame.") + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target=targets[index], + payload=JointPositionPayload( + ( + torch.full( + (batch_size, len(targets[index].joint_ids)), + float(index + 1), + device=env_ids.device, + ) + if positions is None + else positions[index] + ), + velocities=(None if velocities is None else velocities[index]), + ), + ), + ), + active_mask=torch.ones( + batch_size, + dtype=torch.bool, + device=env_ids.device, + ), + env_ids=env_ids, + hold_duration=torch.full( + (batch_size,), + 0.1, + device=env_ids.device, + ), + ) + for index in range(frame_count) + ) + return TimedCommandSequence(frames=frames, env_ids=env_ids) + + +class _AlternateJointPositionTarget(JointPositionTarget): + """Distinct exact target type sharing joint-position transport semantics.""" + + +@dataclass(frozen=True, slots=True) +class _ClaimedTarget(RuntimeEndpointTarget): + """Non-joint target used to verify binding claim authorization.""" + + name: str + + @property + def transport_id(self) -> str: + return JointPositionTarget.TRANSPORT_ID + + @property + def target_id(self) -> str: + return self.name + + +def _action_plan( + commands: TimedCommandSequence, + *, + plan_success: torch.Tensor | None = None, + joint_trajectory: TimedTrajectory | None = None, + feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, +) -> ActionPlan: + if plan_success is None: + plan_success = torch.ones( + commands.batch_size, + dtype=torch.bool, + device=commands.device, + ) + return ActionPlan( + skill_id="test", + plan_success=plan_success, + commands=commands, + recovery_policy=RecoveryPolicy(), + planned_scene_version=0, + planned_collision_world_revision=(0,) * commands.batch_size, + diagnostics=PlannerDiagnostics(backend="test"), + feedback_mode=feedback_mode, + joint_trajectory=joint_trajectory, + ) + + class _DependencyAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): """Minimal action proving that build_plan delegates dependencies to its hook.""" skill_id = "dependency_test" GoalType = EndEffectorPoseGoal OptionsType = ActionOptions - manipulator_roles = () + binding_contract = SkillBindingContract() @property def device(self) -> torch.device: @@ -144,18 +252,82 @@ def _plan( raise NotImplementedError -def test_action_binding_is_role_based_and_immutable() -> None: +class _RawCommandAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): + """Action that deliberately bypasses build_command_plan for validation.""" + + skill_id = "raw_command_test" + GoalType = EndEffectorPoseGoal + OptionsType = ActionOptions + binding_contract = SkillBindingContract() + + @property + def device(self) -> torch.device: + return torch.device("cpu") + + def _uses_collision_world( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> bool: + del request, context + return False + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + del request + return ActionPlan( + skill_id=self.skill_id, + plan_success=torch.ones(context.batch_size, dtype=torch.bool), + commands=_command_sequence( + env_ids=context.env_ids, + frame_count=1, + ), + recovery_policy=RecoveryPolicy(), + planned_scene_version=context.scene.version, + planned_collision_world_revision=(0,) * context.batch_size, + diagnostics=PlannerDiagnostics(backend="test"), + ) + + +def test_action_binding_is_endpoint_based_and_immutable() -> None: + endpoint = EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="left_actor", + adapter_id="control_part", + target=JointPositionTarget("left_arm", (0, 1)), + capabilities=frozenset({"motion.test"}), + claim_tokens=frozenset({"robot.control_part:left_arm"}), + ) binding = ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, + owner_id="test-engine", + endpoints=(endpoint,), ) - assert binding.manipulator() == "left_arm" - assert binding.end_effector() == "left_hand" - with pytest.raises(TypeError): - binding.manipulators["primary"] = "right_arm" - with pytest.raises(KeyError, match="destination"): - binding.manipulator("destination") + resolved = binding.endpoint("primary", "motion") + target = resolved.require_target(JointPositionTarget) + assert resolved is not binding.endpoints[0] + assert resolved.target is not binding.endpoints[0].target + assert target.control_part == "left_arm" + assert target.joint_ids == (0, 1) + assert resolved.joint_ids == (0, 1) + assert resolved.capabilities == frozenset({"motion.test"}) + with pytest.raises(FrozenInstanceError): + binding.owner_id = "other-engine" # type: ignore[misc] + with pytest.raises(KeyError, match="destination.motion"): + binding.endpoint("destination", "motion") + with pytest.raises(ValueError, match="must match"): + EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="left_actor", + adapter_id="control_part", + target=JointPositionTarget("left_arm", (0, 1)), + joint_ids=(1, 2), + ) def test_invocation_rejects_values_without_goal_contract() -> None: @@ -163,7 +335,7 @@ def test_invocation_rejects_values_without_goal_contract() -> None: ActionInvocation( skill_id="move_end_effector", goal=object(), # type: ignore[arg-type] - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=ActionBinding(owner_id="test-engine"), ) @@ -504,27 +676,447 @@ def test_dependency_collection_does_not_descend_object_semantics() -> None: def test_build_plan_uses_action_scene_dependency_hook() -> None: context = _context() + generator = Mock() + generator.robot = Mock() + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _DependencyAction() + engine.register(action) request = ResolvedActionRequest( skill_id="dependency_test", goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), - binding=ResolvedActionBinding(), + binding=ActionBinding(owner_id=engine.binding_owner_id), motion_policy=MotionPolicy(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) - action = _DependencyAction() - plan = action.build_plan( + plan = action.build_command_plan( request, context, success=True, - trajectory=context.robot.qpos.unsqueeze(1), + commands=TimedCommandSequence(frames=(), env_ids=context.env_ids), diagnostics=PlannerDiagnostics(backend="test"), ) assert plan.scene_dependencies == ("extra", "tracked") +def test_build_command_plan_rejects_unbound_runtime_destination() -> None: + context = _context() + generator = Mock() + generator.robot = Mock() + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _DependencyAction() + engine.register(action) + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + + with pytest.raises(ValueError, match="not authorized"): + action.build_command_plan( + request, + context, + success=True, + commands=_command_sequence(env_ids=context.env_ids, frame_count=1), + diagnostics=PlannerDiagnostics(backend="test"), + ) + + +def test_public_plan_authorizes_raw_action_plan_destinations() -> None: + context = _context() + generator = Mock() + generator.robot = Mock() + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _RawCommandAction() + engine.register(action) + request = ResolvedActionRequest( + skill_id=action.skill_id, + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + + with pytest.raises(ValueError, match="not authorized"): + action.plan(request, context) + + +def test_command_target_authorization_rejects_altered_joint_claims() -> None: + context = _context() + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding( + owner_id="test-engine", + endpoints=( + EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="arm", + adapter_id="control_part", + target=JointPositionTarget("arm", (0, 1)), + ), + ), + ), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + frame = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("arm", (2, 3)), + payload=JointPositionPayload(torch.ones(2, 2)), + ), + ), + active_mask=torch.ones(2, dtype=torch.bool), + env_ids=context.env_ids, + hold_duration=torch.full((2,), 0.1), + ) + + with pytest.raises(ValueError, match="bound joint-position target"): + _DependencyAction._authorize_command_targets( + request, + TimedCommandSequence(frames=(frame,), env_ids=context.env_ids), + ) + + +def test_command_target_authorization_rejects_custom_claim_conflicts() -> None: + context = _context() + endpoints = tuple( + EndpointBinding( + slot_id="primary", + endpoint_id=name, + resource_id=name, + adapter_id="test.claimed", + target=_ClaimedTarget(name), + claim_tokens=frozenset({"controller:shared"}), + ) + for name in ("first", "second") + ) + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id="test-engine", endpoints=endpoints), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + frame = RuntimeCommandFrame( + commands=tuple( + EndpointCommand( + target=endpoint.target, + payload=JointPositionPayload(torch.ones(2, 1)), + ) + for endpoint in endpoints + ), + active_mask=torch.ones(2, dtype=torch.bool), + env_ids=context.env_ids, + hold_duration=torch.full((2,), 0.1), + ) + + with pytest.raises(ValueError, match="claim tokens.*controller:shared"): + _DependencyAction._authorize_command_targets( + request, + TimedCommandSequence(frames=(frame,), env_ids=context.env_ids), + ) + + +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) + trajectory_positions = torch.stack( + ( + torch.full((2, 2), 1.0), + torch.full((2, 2), 2.0), + ), + dim=1, + ) + trajectory = TimedTrajectory.from_positions( + trajectory_positions, + env_ids=env_ids, + control_dt=0.1, + ) + plan_success = torch.tensor([True, False]) + + plan = _action_plan( + commands, + plan_success=plan_success, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + payload = commands.frames[0].commands[0].payload + assert isinstance(payload, JointPositionPayload) + plan_success.zero_() + payload.positions.zero_() + commands.frames[0].active_mask.zero_() + commands.frames[0].hold_duration.zero_() + commands.env_ids.zero_() + trajectory.positions.zero_() + + owned_payload = plan.commands.frames[0].commands[0].payload + assert isinstance(owned_payload, JointPositionPayload) + assert plan.plan_success.tolist() == [True, False] + assert torch.all(owned_payload.positions == 1.0) + assert plan.commands.frames[0].active_mask.tolist() == [True, True] + assert torch.all(plan.commands.frames[0].hold_duration == 0.1) + assert plan.commands.env_ids.tolist() == [4, 7] + assert plan.joint_trajectory is not None + assert torch.equal(plan.joint_trajectory.positions, trajectory_positions) + + +def test_action_plan_allows_timed_commands_without_joint_trajectory() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + plan = _action_plan(commands) + + assert plan.commands.frame_count == 1 + assert plan.joint_trajectory is None + assert plan.feedback_mode is ExecutionFeedbackMode.TIMED + + +def test_action_plan_rejects_command_device_mismatch() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + with pytest.raises(ValueError, match="share a device"): + _action_plan( + commands, + plan_success=torch.ones(1, dtype=torch.bool, device="meta"), + ) + + +@pytest.mark.parametrize( + ("trajectory_env_ids", "trajectory_frame_count", "message"), + [ + (torch.tensor([7], dtype=torch.long), 1, "env_ids must match"), + (torch.tensor([4], dtype=torch.long), 2, "waypoints must match"), + ], +) +def test_action_plan_validates_joint_trajectory_against_commands( + trajectory_env_ids: torch.Tensor, + trajectory_frame_count: int, + message: str, +) -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + trajectory = TimedTrajectory.from_positions( + torch.ones(1, trajectory_frame_count, 2), + env_ids=trajectory_env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match=message): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_rejects_empty_commands_for_successful_rows() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = TimedCommandSequence(frames=(), env_ids=env_ids) + trajectory = TimedTrajectory.empty( + batch_size=1, + robot_dof=2, + device=env_ids.device, + env_ids=env_ids, + ) + + with pytest.raises(ValueError, match="requires command frames"): + _action_plan( + commands, + plan_success=torch.tensor([True]), + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_allows_empty_commands_when_all_rows_fail() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = TimedCommandSequence(frames=(), env_ids=env_ids) + trajectory = TimedTrajectory.empty( + batch_size=1, + robot_dof=2, + device=env_ids.device, + env_ids=env_ids, + ) + + plan = _action_plan( + commands, + plan_success=torch.tensor([False]), + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + 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: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=2, + targets=( + JointPositionTarget("arm", (0, 1)), + JointPositionTarget("other_arm", (0, 1)), + ), + ) + trajectory = ( + TimedTrajectory.from_positions( + torch.tensor([[[1.0, 1.0], [2.0, 2.0]]]), + env_ids=env_ids, + control_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, + ) + + +def test_action_plan_requires_stable_exact_target_type() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=2, + targets=( + JointPositionTarget("arm", (0, 1)), + _AlternateJointPositionTarget("arm", (0, 1)), + ), + ) + + with pytest.raises(ValueError, match="exact target type"): + _action_plan(commands) + + +def test_action_plan_requires_stable_target_address_fingerprint() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=2, + targets=( + JointPositionTarget("arm", (0, 1)), + JointPositionTarget("arm", (1, 0)), + ), + ) + + with pytest.raises(ValueError, match="target address fingerprint"): + _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_positions( + torch.ones(1, 1, 2), + env_ids=env_ids, + control_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_positions( + torch.zeros(1, 1, 2), + env_ids=env_ids, + control_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_positions( + torch.ones(1, 1, 2), + env_ids=env_ids, + control_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_positions( + torch.ones(1, 1, 2), + velocities=torch.ones(1, 1, 2), + env_ids=env_ids, + control_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( @@ -618,6 +1210,44 @@ def test_timed_trajectory_synthesizes_timing_and_holds_selected_rows() -> None: assert torch.all(held.positions[1] == -1.0) +def test_timed_trajectory_constructor_detaches_and_owns_all_tensor_fields() -> None: + positions = torch.tensor([[[1.0, 2.0], [3.0, 4.0]]], requires_grad=True) + velocities = torch.full_like(positions, 0.5, requires_grad=True) + accelerations = torch.full_like(positions, 0.25, requires_grad=True) + dt = torch.tensor([[0.0, 0.1]], requires_grad=True) + duration = torch.tensor([0.1], requires_grad=True) + env_ids = torch.tensor([4], dtype=torch.long) + inputs = { + "positions": positions, + "velocities": velocities, + "accelerations": accelerations, + "dt": dt, + "duration": duration, + "env_ids": env_ids, + } + expected = {name: value.detach().clone() for name, value in inputs.items()} + + trajectory = TimedTrajectory(**inputs) + + with torch.no_grad(): + for value in inputs.values(): + value.zero_() + for name, value in expected.items(): + owned = getattr(trajectory, name) + assert torch.equal(owned, value) + assert owned.grad_fn is None + assert not owned.requires_grad + + +def test_timed_trajectory_rejects_duplicate_environment_ids() -> None: + with pytest.raises(ValueError, match="unique"): + TimedTrajectory.from_positions( + torch.zeros(2, 1, 2), + env_ids=torch.tensor([4, 4], dtype=torch.long), + control_dt=0.1, + ) + + def test_timed_trajectory_snapshot_owns_its_tensor_storage() -> None: trajectory = TimedTrajectory.from_positions( torch.arange(12, dtype=torch.float32).reshape(1, 3, 4), diff --git a/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py b/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py index da4e87a41..7ea946be9 100644 --- a/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py +++ b/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py @@ -46,7 +46,6 @@ CuroboWorldCfg, ) from embodichain.lab.sim.atomic_actions import ( # noqa: E402 - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -128,12 +127,16 @@ def test_atomic_move_end_effector_uses_curobo_v2(): sim, robot, engine = _make_franka_curobo_engine() try: target = _reachable_target_beyond_demo_block(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target), - binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + binding=binding, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_INTERVAL, @@ -141,7 +144,10 @@ def test_atomic_move_end_effector_uses_curobo_v2(): ), ) ) - trajectory = result.trajectory.positions + plan = result.action_plans[0] + assert plan.joint_trajectory is not None + assert plan.commands.frame_count == plan.joint_trajectory.waypoint_count + trajectory = plan.joint_trajectory.positions assert result.plan_success.shape == (1,) assert bool(result.plan_success.item()) assert trajectory.shape[2] == robot.dof diff --git a/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py new file mode 100644 index 000000000..7c6dd3688 --- /dev/null +++ b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py @@ -0,0 +1,535 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""End-to-end coverage for generic atomic-action runtime endpoints.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar +from unittest.mock import Mock + +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionInvocation, + ActionOptions, + ActionPlan, + AtomicAction, + AtomicActionEngine, + CommandAcknowledgement, + EndpointCommand, + EndpointCommandRouter, + ExecutionRunner, + ExecutionStatus, + JOINT_POSITION_CAPABILITY, + JointPositionGoal, + JointPositionPayload, + JointPositionTarget, + MoveJoints, + PlanningContext, + RobotObservation, + RunnerStatus, + RuntimeCommandFrame, + RuntimeCommandPayload, + RuntimeEndpointTarget, + SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + TaskState, + TimedCommandSequence, +) +from embodichain.lab.sim.atomic_actions.invocation import ResolvedActionRequest +from embodichain.lab.sim.planners import PlanResult +from embodichain.lab.sim.skills import ( + EndpointResolution, + ResourceBinding, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, +) + + +class _Clock: + """Deterministic clock used by the runner.""" + + def __init__(self) -> None: + self.value = 0.0 + + def now(self) -> float: + """Return simulated time.""" + return self.value + + def sleep(self, duration: float) -> None: + """Advance simulated time.""" + self.value += duration + + +class _Robot: + """Small stateful robot with one whole-body control part.""" + + def __init__(self) -> None: + self.device = torch.device("cpu") + self.dof = 4 + self.control_parts = {"whole_body": object()} + self.qpos = torch.zeros(2, self.dof) + + def get_qpos(self, name: str | None = None) -> torch.Tensor: + """Return observed joint positions.""" + if name is not None and name != "whole_body": + raise KeyError(name) + return self.qpos.clone() + + def get_qvel(self, name: str | None = None) -> torch.Tensor: + """Return zero joint velocities.""" + return torch.zeros_like(self.get_qpos(name)) + + def get_joint_ids(self, name: str) -> list[int]: + """Resolve the whole-body control part.""" + if name != "whole_body": + raise KeyError(name) + return list(range(self.dof)) + + +class _Provider: + """Observe the stateful robot at the injected clock time.""" + + def __init__(self, robot: _Robot, clock: _Clock) -> None: + self.robot = robot + self.clock = clock + self.env_ids = torch.tensor([3, 7], dtype=torch.long) + + def observe(self, task_state: TaskState) -> PlanningContext: + """Return one fresh, correlated planning context.""" + qpos = self.robot.get_qpos() + 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=0), + env_ids=self.env_ids, + ) + + +def _engine(robot: _Robot) -> AtomicActionEngine: + """Build a core engine around a controllable planning stub.""" + generator = Mock() + generator.robot = robot + generator.device = robot.device + generator.planner.cfg.planner_type = "stub" + + def generate(states: list[object], *, options: object) -> PlanResult: + target = states[-1].qpos + assert isinstance(target, torch.Tensor) + start = options.start_qpos + assert isinstance(start, torch.Tensor) + positions = torch.stack((start, target), dim=1) + dt = torch.zeros(positions.shape[:2], dtype=torch.float32) + dt[:, 1] = 0.01 + return PlanResult( + success=torch.ones(positions.shape[0], dtype=torch.bool), + positions=positions, + dt=dt, + duration=dt.sum(dim=1), + ) + + generator.generate.side_effect = generate + return AtomicActionEngine(generator, load_builtins=False) + + +class _JointTransport: + """Apply joint endpoint payloads to the stateful test robot.""" + + transport_id = JointPositionTarget.TRANSPORT_ID + payload_type = JointPositionPayload + + def __init__(self, robot: _Robot) -> None: + self.robot = robot + self.sent: list[RuntimeCommandFrame] = [] + self.held: list[tuple[RuntimeEndpointTarget, ...]] = [] + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply each addressed joint subset.""" + del timeout + self.sent.append(frame.snapshot()) + for command in frame.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + joint_ids = list(command.target.joint_ids) + self.robot.qpos[:, joint_ids] = torch.where( + frame.active_mask[:, None], + command.payload.positions, + self.robot.qpos[:, joint_ids], + ) + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Hold only the joint subsets addressed by the runner.""" + del timeout + self.held.append(tuple(target.snapshot() for target in targets)) + for target in targets: + assert isinstance(target, JointPositionTarget) + joint_ids = list(target.joint_ids) + self.robot.qpos[:, joint_ids] = context.robot.qpos[:, joint_ids] + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Acknowledge synchronous cancellation.""" + del targets, timeout + return CommandAcknowledgement.accepted_ack() + + +def test_whole_body_joint_endpoint_executes_without_arm_or_tool_roles() -> None: + robot = _Robot() + engine = _engine(robot) + engine.register(MoveJoints()) + binding = engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "whole_body"}}, + ) + invocation = ActionInvocation( + skill_id="move_joints", + goal=JointPositionGoal(torch.full((2, robot.dof), 0.5)), + binding=binding, + ) + clock = _Clock() + provider = _Provider(robot, clock) + context = provider.observe(TaskState.empty(batch_size=2, device="cpu")) + + plan = engine.plan(invocation, context) + target = binding.endpoint("primary", "motion").require_target(JointPositionTarget) + assert target.control_part == "whole_body" + assert plan.joint_trajectory is not None + assert plan.commands.targets[0].target_id == "whole_body" + + transport = _JointTransport(robot) + runner = ExecutionRunner( + engine.start((invocation,), context), + provider, + EndpointCommandRouter((transport,)), + clock=clock, + ) + result = runner.run_until_blocked() + + assert result.status is RunnerStatus.COMPLETED + assert result.tick is not None + assert result.tick.status is ExecutionStatus.COMPLETED + assert len(transport.sent) == 2 + assert len(transport.held) == 1 + assert transport.held[0][0].target_id == "whole_body" + assert torch.allclose(robot.qpos, torch.full((2, robot.dof), 0.5)) + + +@dataclass(frozen=True, slots=True) +class _PlanarVelocityTarget(RuntimeEndpointTarget): + """Address one planar velocity controller.""" + + controller_id: str + + @property + def transport_id(self) -> str: + """Return the custom transport identifier.""" + return "test.planar_velocity" + + @property + def target_id(self) -> str: + """Return the controller-local target identifier.""" + return self.controller_id + + +@dataclass(frozen=True, slots=True, eq=False) +class _PlanarVelocityPayload(RuntimeCommandPayload): + """Batched ``(vx, vy, yaw_rate)`` commands.""" + + twist: torch.Tensor + + def __post_init__(self) -> None: + if not isinstance(self.twist, torch.Tensor) or self.twist.dim() != 2: + raise ValueError("twist must have shape (batch_size, 3).") + if self.twist.shape[0] < 1 or self.twist.shape[1] != 3: + raise ValueError("twist must have shape (batch_size, 3).") + object.__setattr__(self, "twist", self.twist.clone()) + + @property + def batch_size(self) -> int: + """Return the number of environment rows.""" + return int(self.twist.shape[0]) + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.twist.device + + @property + def transport_id(self) -> str: + """Return the custom transport identifier.""" + return "test.planar_velocity" + + def snapshot(self) -> _PlanarVelocityPayload: + """Return an independently owned payload.""" + return _PlanarVelocityPayload(self.twist) + + +@dataclass(frozen=True, slots=True) +class _PlanarVelocityEndpoint(ResourceEndpoint): + """Profile declaration for a planar velocity controller.""" + + controller_id: str + + +class _PlanarVelocityAdapter(ResourceEndpointAdapter): + """Resolve the custom profile endpoint to a runtime target.""" + + adapter_id: ClassVar[str] = "test.planar_velocity" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _PlanarVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + """Resolve immutable addressing and an exclusive controller claim.""" + del engine + assert isinstance(endpoint, _PlanarVelocityEndpoint) + return EndpointResolution( + runtime_target=_PlanarVelocityTarget(endpoint.controller_id), + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _DriveGoal: + """Planar velocity command used by the custom atomic action.""" + + goal_kind: ClassVar[str] = "planar_velocity" + twist: torch.Tensor + + +class _DriveVelocity(AtomicAction[_DriveGoal, ActionOptions]): + """Custom action proving non-joint commands cross the full runtime.""" + + skill_id: ClassVar[str] = "drive_velocity" + GoalType: ClassVar[type] = _DriveGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.base.planar_velocity"}), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[_DriveGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + """Emit one drive frame followed by an explicit zero-velocity frame.""" + goal = self.require_goal(request) + target = request.binding.endpoint("body", "motion").require_target( + _PlanarVelocityTarget + ) + active = torch.ones(context.batch_size, dtype=torch.bool, device=self.device) + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target=target, + payload=_PlanarVelocityPayload(twist), + ), + ), + active_mask=active, + env_ids=context.env_ids, + hold_duration=torch.full( + (context.batch_size,), + duration, + dtype=torch.float32, + device=self.device, + ), + ) + for twist, duration in ( + (goal.twist.to(self.device), 0.02), + (torch.zeros_like(goal.twist, device=self.device), 0.0), + ) + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence(frames, context.env_ids), + segment_lengths={"drive": 1, "stop": 1}, + ) + + +class _PlanarVelocityTransport: + """Record velocity frames and own the zero-velocity safe state.""" + + transport_id = "test.planar_velocity" + payload_type = _PlanarVelocityPayload + + def __init__(self) -> None: + self.sent: list[torch.Tensor] = [] + self.hold_targets: tuple[RuntimeEndpointTarget, ...] = () + self.last_twist: torch.Tensor | None = None + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record active twists and neutralize every inactive row.""" + del timeout + payload = frame.commands[0].payload + assert isinstance(payload, _PlanarVelocityPayload) + self.last_twist = torch.where( + frame.active_mask[:, None], + payload.twist, + torch.zeros_like(payload.twist), + ) + self.sent.append(self.last_twist) + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply the velocity transport's safe zero command.""" + del context, timeout + self.hold_targets = tuple(target.snapshot() for target in targets) + assert self.last_twist is not None + self.last_twist = torch.zeros_like(self.last_twist) + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Acknowledge cancellation.""" + del targets, timeout + return CommandAcknowledgement.accepted_ack() + + +def test_custom_planar_velocity_endpoint_runs_from_profile_through_router() -> None: + robot = _Robot() + engine = _engine(robot) + engine.register(_DriveVelocity()) + profile = RobotSkillProfile( + profile_id="mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _PlanarVelocityEndpoint( + "base_controller", + capabilities=frozenset({"motion.base.planar_velocity"}), + ) + }, + ) + }, + defaults={"drive_velocity": ResourceBinding({"body": "mobile_base"})}, + ) + bound = engine.bind_skill_profile( + profile, + endpoint_adapters={_PlanarVelocityEndpoint: _PlanarVelocityAdapter()}, + ) + binding = bound.resolve("drive_velocity").action_binding + goal_twist = torch.tensor([[0.5, 0.0, 0.1], [0.2, 0.0, -0.1]]) + invocation = ActionInvocation( + skill_id="drive_velocity", + goal=_DriveGoal(goal_twist), + binding=binding, + ) + clock = _Clock() + provider = _Provider(robot, clock) + context = provider.observe(TaskState.empty(batch_size=2, device="cpu")) + + plan = engine.plan(invocation, context) + assert plan.joint_trajectory is None + assert plan.segment("drive").waypoint_count == 1 + assert plan.commands.targets[0].transport_id == "test.planar_velocity" + + transport = _PlanarVelocityTransport() + runner = ExecutionRunner( + engine.start((invocation,), context), + provider, + EndpointCommandRouter((transport,)), + clock=clock, + ) + result = runner.run_until_blocked() + + assert result.status is RunnerStatus.COMPLETED + assert len(transport.sent) == 2 + assert torch.allclose(transport.sent[0], goal_twist) + assert torch.count_nonzero(transport.sent[1]) == 0 + assert transport.last_twist is not None + assert torch.count_nonzero(transport.last_twist) == 0 + assert transport.hold_targets[0].target_id == "base_controller" + + +def test_planar_velocity_transport_neutralizes_inactive_rows() -> None: + transport = _PlanarVelocityTransport() + frame = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=_PlanarVelocityTarget("base_controller"), + payload=_PlanarVelocityPayload(torch.ones(2, 3)), + ), + ), + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([0, 1]), + hold_duration=torch.zeros(2), + ) + + acknowledgement = transport.send(frame, timeout=1.0) + + assert acknowledgement.accepted + assert transport.last_twist is not None + assert torch.equal(transport.last_twist[0], torch.ones(3)) + assert torch.count_nonzero(transport.last_twist[1]) == 0 diff --git a/tests/sim/atomic_actions/test_engine.py b/tests/sim/atomic_actions/test_engine.py index e19035e56..eed03fad8 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -37,11 +37,16 @@ ControlPartCommandProfile, JointPositionCommand, JointPositionGoal, + JointPositionTarget, + JOINT_POSITION_CAPABILITY, MotionPolicy, PlanningContext, PressGoal, PressOptions, ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, register_action, get_registered_actions, unregister_action, @@ -53,7 +58,19 @@ class StubAction(AtomicAction[JointPositionGoal, ActionOptions]): skill_id: ClassVar[str] = "stub" GoalType: ClassVar[type] = JointPositionGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + ), + ), + ), + ) def _plan( self, @@ -119,12 +136,16 @@ def _engine( def _invocation( + engine: AtomicActionEngine, qpos: torch.Tensor, ) -> ActionInvocation[JointPositionGoal, ActionOptions]: return ActionInvocation( skill_id="stub", goal=JointPositionGoal(qpos), - binding=ActionBinding(manipulators={"primary": "all"}), + binding=engine.bind_control_parts( + "stub", + {"primary": {"motion": "all"}}, + ), motion_policy=MotionPolicy(sample_count=2), ) @@ -179,14 +200,24 @@ def test_engine_can_disable_builtin_loading() -> None: def test_auto_registered_builtin_accepts_per_invocation_options() -> None: - engine = _engine(load_builtins=True) + generator = _motion_generator(robot_dof=3) + generator.robot.control_parts = {"arm": object(), "hand": object()} + generator.robot.get_joint_ids.side_effect = lambda name: ( + [0, 1] if name == "arm" else [2] + ) + engine = AtomicActionEngine( + generator, + control_profiles={ + "hand": ControlPartCommandProfile.joint_positions(grasp=torch.ones(1)) + }, + ) options = PressOptions(hand_interp_steps=7) invocation = ActionInvocation( skill_id="press", goal=PressGoal(torch.eye(4)), - binding=ActionBinding( - manipulators={"primary": "all"}, - end_effectors={"primary": "all"}, + binding=engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=20), skill_options=options, @@ -204,11 +235,13 @@ def test_engine_compile_projects_terminal_state_between_actions() -> None: first = torch.ones(2, 3) second = torch.full((2, 3), 2.0) - compiled = engine.compile((_invocation(first), _invocation(second))) + compiled = engine.compile((_invocation(engine, first), _invocation(engine, second))) assert compiled.plan_success.tolist() == [True, True] assert compiled.trajectory.positions.shape == (2, 4, 3) - assert torch.equal(compiled.action_plans[1].trajectory.positions[:, 0], first) + second_trajectory = compiled.action_plans[1].joint_trajectory + assert second_trajectory is not None + assert torch.equal(second_trajectory.positions[:, 0], first) assert torch.equal(compiled.projected_context.robot.qpos, second) assert torch.count_nonzero(engine.robot.get_qpos()) == 0 assert compiled.action_waypoint_offset(1) == 2 @@ -222,12 +255,14 @@ def test_engine_compile_holds_failed_rows_for_remaining_actions() -> None: first = torch.tensor([[1.0, 1.0, 1.0], [float("nan"), 2.0, 2.0]]) second = torch.full((2, 3), 4.0) - compiled = engine.compile((_invocation(first), _invocation(second))) + compiled = engine.compile((_invocation(engine, first), _invocation(engine, second))) assert compiled.plan_success.tolist() == [True, False] assert torch.all(compiled.projected_context.robot.qpos[0] == 4.0) assert torch.all(compiled.projected_context.robot.qpos[1] == 0.0) - assert torch.all(compiled.action_plans[0].trajectory.positions[1] == 0.0) + first_trajectory = compiled.action_plans[0].joint_trajectory + assert first_trajectory is not None + assert torch.all(first_trajectory.positions[1] == 0.0) assert torch.all(compiled.trajectory.positions[1] == 0.0) @@ -244,8 +279,14 @@ def test_engine_compile_empty_sequence_is_successful_noop() -> None: def test_engine_rejects_unknown_skill() -> None: engine = _engine() + invocation = ActionInvocation( + skill_id="stub", + goal=JointPositionGoal(torch.zeros(2, 3)), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(sample_count=2), + ) with pytest.raises(KeyError, match="stub"): - engine.compile((_invocation(torch.zeros(2, 3)),)) + engine.compile((invocation,)) def test_engine_rejects_duplicate_instance_registration() -> None: @@ -285,14 +326,16 @@ def test_engine_binds_one_planning_service_to_every_action() -> None: def test_engine_resolves_action_binding_from_robot_control_parts() -> None: engine = _engine(robot_dof=3) + engine.register(StubAction()) - resolved = engine.planning_services.resolve_binding( - ActionBinding(manipulators={"primary": "all"}) + resolved = engine.bind_control_parts( + "stub", + {"primary": {"motion": "all"}}, ) + target = resolved.endpoint("primary", "motion").require_target(JointPositionTarget) - assert resolved.manipulator().name == "all" - assert resolved.manipulator().joint_ids == (0, 1, 2) - assert resolved.manipulator().dof == 3 + assert target.control_part == "all" + assert target.joint_ids == (0, 1, 2) def test_engine_resolves_invocation_control_override_into_request() -> None: @@ -304,10 +347,12 @@ def test_engine_resolves_invocation_control_override_into_request() -> None: ) engine.register(StubAction()) invocation = replace( - _invocation(torch.ones(2, 3)), + _invocation(engine, torch.ones(2, 3)), control_overrides=ActionControlOverrides( - manipulators={ - "primary": {"ready": JointPositionCommand(torch.full((3,), 0.4))} + endpoints={ + "primary": { + "motion": {"ready": JointPositionCommand(torch.full((3,), 0.4))} + } } ), revision=2, @@ -317,7 +362,9 @@ def test_engine_resolves_invocation_control_override_into_request() -> None: assert request.revision == 2 assert torch.allclose( - request.binding.manipulator().joint_positions("ready", n_envs=2, device="cpu"), + request.binding.endpoint("primary", "motion").joint_positions( + "ready", n_envs=2, device="cpu" + ), torch.full((2, 3), 0.4), ) @@ -325,15 +372,12 @@ def test_engine_resolves_invocation_control_override_into_request() -> None: def test_engine_rejects_binding_outside_robot_control_parts() -> None: engine = _engine() engine.register(StubAction()) - invocation = ActionInvocation( - skill_id="stub", - goal=JointPositionGoal(torch.zeros(2, 3)), - binding=ActionBinding(manipulators={"primary": "missing_arm"}), - motion_policy=MotionPolicy(sample_count=2), - ) with pytest.raises(ValueError, match="Robot.control_parts"): - engine.plan(invocation) + engine.bind_control_parts( + "stub", + {"primary": {"motion": "missing_arm"}}, + ) def test_engine_motion_generator_is_read_only() -> None: @@ -346,10 +390,20 @@ def test_engine_motion_generator_is_read_only() -> None: def test_engine_plan_action_supports_unregistered_configured_instance() -> None: engine = _engine() action = StubAction() + binding = engine.bind_control_parts( + action, + {"primary": {"motion": "all"}}, + ) + invocation = ActionInvocation( + skill_id="stub", + goal=JointPositionGoal(torch.ones(2, 3)), + binding=binding, + motion_policy=MotionPolicy(sample_count=2), + ) plan = engine.plan_action( action, - _invocation(torch.ones(2, 3)), + invocation, engine.initial_context(), ) @@ -358,6 +412,18 @@ def test_engine_plan_action_supports_unregistered_configured_instance() -> None: assert engine.actions == {} +def test_engine_cannot_build_binding_for_action_owned_by_another_engine() -> None: + action = StubAction() + first = _engine() + first.register(action) + + with pytest.raises(ValueError, match="belongs to another engine"): + _engine().bind_control_parts( + action, + {"primary": {"motion": "all"}}, + ) + + def test_action_cannot_be_rebound_to_another_engine() -> None: action = StubAction() _engine().register(action) @@ -368,9 +434,11 @@ def test_action_cannot_be_rebound_to_another_engine() -> None: def test_unbound_action_rejects_direct_planning() -> None: action = StubAction() + donor_engine = _engine() + donor_engine.register(StubAction()) with pytest.raises(RuntimeError, match="not bound"): - action.resolve_request(_invocation(torch.ones(2, 3))) + action.resolve_request(_invocation(donor_engine, torch.ones(2, 3))) def test_engine_rejects_plan_for_a_different_skill() -> None: @@ -388,4 +456,4 @@ def wrong_skill_plan( engine.register(action) with pytest.raises(ValueError, match="must match its request"): - engine.compile((_invocation(torch.zeros(2, 3)),)) + engine.compile((_invocation(engine, torch.zeros(2, 3)),)) diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 1458b7367..7243013fd 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -36,22 +36,30 @@ AtomicActionEngine, DynamicCollisionMode, EndEffectorPoseGoal, + EndpointBinding, + EndpointCommand, EntityState, ExecutionEventKind, ExecutionStatus, GraspGoal, HeldObjectState, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, PlanningContext, RecoveryPolicy, - ResolvedActionBinding, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, SceneEntityPose, SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, + TimedCommandSequence, TimedTrajectory, ) from embodichain.lab.sim.common import BatchEntity @@ -64,7 +72,14 @@ class DynamicAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): skill_id: ClassVar[str] = "dynamic" GoalType: ClassVar[type] = EndEffectorPoseGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=(SkillEndpointRequirement(endpoint_id="motion"),), + ), + ) + ) def __init__(self) -> None: super().__init__() @@ -93,6 +108,7 @@ class EffectAction(DynamicAction): """Dynamic test action that declares an attachment effect.""" skill_id: ClassVar[str] = "effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( self, @@ -123,6 +139,7 @@ class FailedEffectAction(EffectAction): """Effect-declaring action whose planner fails for every environment.""" skill_id: ClassVar[str] = "failed_effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( self, @@ -137,6 +154,7 @@ class NonuniformTimingAction(DynamicAction): """Test action with explicit nonuniform waypoint arrival intervals.""" skill_id: ClassVar[str] = "nonuniform_timing" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( self, @@ -168,6 +186,82 @@ def _plan( ) +class DestinationSequenceAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): + """Emit a configured destination sequence across recovery plans.""" + + skill_id: ClassVar[str] = "destination_sequence" + GoalType: ClassVar[type] = EndEffectorPoseGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement(endpoint_id="first"), + SkillEndpointRequirement(endpoint_id="second"), + ), + ), + ) + ) + + def __init__(self, destinations: tuple[str | None, ...]) -> None: + super().__init__() + self.destinations = destinations + self.plan_count = 0 + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + self.require_goal(request) + index = min(self.plan_count, len(self.destinations) - 1) + endpoint_id = self.destinations[index] + self.plan_count += 1 + if endpoint_id is None: + commands = TimedCommandSequence(frames=(), env_ids=context.env_ids) + return self.build_command_plan( + request, + context, + success=False, + commands=commands, + ) + + target = request.binding.endpoint("primary", endpoint_id).require_target( + JointPositionTarget + ) + joint_ids = list(target.joint_ids) + frame = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=target, + payload=JointPositionPayload( + positions=context.robot.qpos[:, joint_ids] + ), + ), + ), + active_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + env_ids=context.env_ids, + hold_duration=torch.zeros( + context.batch_size, + dtype=torch.float32, + device=context.robot.qpos.device, + ), + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence( + frames=(frame,), + env_ids=context.env_ids, + ), + ) + + class UncopyableEntity(BatchEntity): """Minimal live entity whose simulator identity must not be copied.""" @@ -210,6 +304,30 @@ def _engine(batch_size: int = 1) -> tuple[AtomicActionEngine, DynamicAction]: return engine, action +def _destination_engine( + destinations: tuple[str | None, ...], +) -> tuple[AtomicActionEngine, DestinationSequenceAction]: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 2 + robot.control_parts = {"arm_a": object(), "arm_b": 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_a": [0], + "arm_b": [1], + }[name] + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub" + generator.supports_dynamic_collision_world = False + engine = AtomicActionEngine(generator, load_builtins=False) + action = DestinationSequenceAction(destinations) + engine.register(action) + return engine, action + + def _context( timestamp: float, qpos: float | tuple[float, ...], @@ -274,6 +392,7 @@ def _collision_context( def _invocation( + engine: AtomicActionEngine, *, skill_id: str = "dynamic", max_replans: int = 2, @@ -286,7 +405,10 @@ def _invocation( return ActionInvocation( skill_id=skill_id, goal=EndEffectorPoseGoal(SceneEntityPose("target")), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=engine.planning_services.bind_control_parts( + DynamicAction.binding_contract, + {"primary": {"motion": "arm"}}, + ), motion_policy=MotionPolicy( sample_count=2, control_dt=control_dt, @@ -304,17 +426,50 @@ def _invocation( ) +def _destination_invocation( + engine: AtomicActionEngine, +) -> ActionInvocation[EndEffectorPoseGoal]: + return ActionInvocation( + skill_id=DestinationSequenceAction.skill_id, + goal=EndEffectorPoseGoal(SceneEntityPose("target")), + binding=engine.bind_control_parts( + DestinationSequenceAction.skill_id, + { + "primary": { + "first": "arm_a", + "second": "arm_b", + } + }, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + max_action_retries=1, + goal_translation_threshold=0.02, + ), + invocation_id="destination-call", + ) + + +def _joint_positions(command: RuntimeCommandFrame | None) -> torch.Tensor: + """Return the only joint-position payload emitted by the test action.""" + assert command is not None + assert len(command.commands) == 1 + payload = command.commands[0].payload + assert isinstance(payload, JointPositionPayload) + return payload.positions + + def test_session_completes_incremental_command_sequence() -> None: engine, _ = _engine() - session = engine.start((_invocation(),), _context(0.0, 0.0, 0.2, 0)) + session = engine.start((_invocation(engine),), _context(0.0, 0.0, 0.2, 0)) first = session.tick(_context(0.0, 0.0, 0.2, 0)) second = session.tick(_context(0.1, 0.0, 0.2, 0)) final = session.tick(_context(0.2, 0.2, 0.2, 0)) - assert first.command is not None and torch.all(first.command.positions == 0.0) + assert torch.all(_joint_positions(first.command) == 0.0) assert all(event.invocation_id == "dynamic-call" for event in first.events) - assert second.command is not None and torch.all(second.command.positions == 0.2) + assert torch.all(_joint_positions(second.command) == 0.2) assert final.status is ExecutionStatus.COMPLETED assert final.eligible_mask.tolist() == [True] @@ -323,7 +478,7 @@ def test_session_commands_schedule_arrivals_and_final_settling() -> None: engine, _ = _engine() engine.register(NonuniformTimingAction()) session = engine.start( - (_invocation(skill_id="nonuniform_timing"),), + (_invocation(engine, skill_id="nonuniform_timing"),), _context(0.0, 0.0, 0.2, 0), ) @@ -361,7 +516,7 @@ def test_request_snapshot_preserves_live_entity_identity() -> None: request = ResolvedActionRequest( skill_id="pick_up", goal=goal, - binding=ResolvedActionBinding(), + binding=ActionBinding(owner_id="snapshot-test"), motion_policy=MotionPolicy(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), @@ -381,7 +536,7 @@ def test_request_snapshot_preserves_live_entity_identity() -> None: def test_scene_motion_replans_late_bound_goal() -> None: engine, action = _engine() - session = engine.start((_invocation(),), _context(0.0, 0.0, 0.1, 0)) + session = engine.start((_invocation(engine),), _context(0.0, 0.0, 0.1, 0)) session.tick(_context(0.0, 0.0, 0.1, 0)) tick = session.tick(_context(0.1, 0.0, 0.3, 1)) @@ -394,6 +549,52 @@ def test_scene_motion_replans_late_bound_goal() -> None: assert tick.command is not None +def test_recovery_replan_rejects_runtime_destination_change() -> None: + engine, action = _destination_engine(("first", "second")) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + activated = session.tick(initial) + assert activated.command is not None + assert activated.command.commands[0].target.target_id == "arm_a" + + with pytest.raises( + ValueError, + match="Recovery replans must preserve the active runtime destination set", + ) as exc_info: + session.tick(_context(0.1, 0.0, 0.3, 1)) + + assert "arm_a" in str(exc_info.value) + assert "arm_b" in str(exc_info.value) + assert action.plan_count == 2 + + +def test_empty_failed_replan_preserves_destination_for_same_target_retry() -> None: + engine, action = _destination_engine(("first", None, "first")) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + activated = session.tick(initial) + assert activated.command is not None + assert activated.command.commands[0].target.target_id == "arm_a" + + recovered = session.tick(_context(0.1, 0.0, 0.3, 1)) + + kinds = [event.kind for event in recovered.events] + assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + assert kinds.count(ExecutionEventKind.REPLANNED) == 2 + assert action.plan_count == 3 + assert recovered.command is None + assert [target.target_id for target in recovered.hold_targets] == ["arm_a"] + + resumed = session.tick(_context(0.2, 0.0, 0.3, 1)) + assert resumed.command is not None + assert resumed.command.commands[0].target.target_id == "arm_a" + + def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: engine, action = _engine() generator = engine.motion_generator @@ -409,7 +610,7 @@ def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: (0,), ) session = engine.start( - (_invocation(strategy="motion_gen"),), + (_invocation(engine, strategy="motion_gen"),), initial, ) session.tick(initial) @@ -448,6 +649,7 @@ def test_collision_world_exhaustion_only_disables_changed_environment() -> None: session = engine.start( ( _invocation( + engine, max_replans=0, strategy="motion_gen", ), @@ -496,6 +698,7 @@ def test_dynamic_collision_off_skips_binding_and_revision_recovery() -> None: session = engine.start( ( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.OFF, ), @@ -526,6 +729,7 @@ def test_required_dynamic_collision_rejects_incompatible_strategy() -> None: with pytest.raises(ValueError, match="strategy='motion_gen'"): engine.plan( _invocation( + engine, dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), _collision_context( @@ -544,6 +748,7 @@ def test_required_dynamic_collision_rejects_missing_scene_entities() -> None: with pytest.raises(ValueError, match="scene collision entities"): engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -557,6 +762,7 @@ def test_required_dynamic_collision_rejects_unsupported_planner() -> None: with pytest.raises(ValueError, match="dynamic collision-world support"): engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -579,6 +785,7 @@ def test_required_dynamic_collision_binds_supported_scene() -> None: plan = engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -598,7 +805,7 @@ def test_resolved_goal_snapshot_is_reused_during_recovery() -> None: engine, action = _engine() target = torch.eye(4).unsqueeze(0) target[:, 0, 3] = 0.2 - base = _invocation() + base = _invocation(engine) invocation = ActionInvocation( skill_id=base.skill_id, goal=EndEffectorPoseGoal(target), @@ -623,7 +830,7 @@ def test_resolved_goal_snapshot_is_reused_during_recovery() -> None: def test_subset_replan_restarts_synchronized_active_cohort() -> None: engine, action = _engine(batch_size=2) session = engine.start( - (_invocation(),), + (_invocation(engine),), _context(0.0, (0.0, 0.0), (0.1, 0.2), 0), ) session.tick(_context(0.0, (0.0, 0.0), (0.1, 0.2), 0)) @@ -644,17 +851,18 @@ def test_subset_replan_restarts_synchronized_active_cohort() -> None: assert changed.env_mask.tolist() == [True, False] assert cohort.env_mask.tolist() == [True, True] assert replanned.eligible_mask.tolist() == [True, True] - assert replanned.command is not None - assert torch.all(replanned.command.positions == 0.0) - assert next_command.command is not None - assert torch.equal(next_command.command.positions[:, 0], torch.tensor([0.4, 0.2])) + assert torch.all(_joint_positions(replanned.command) == 0.0) + assert torch.equal( + _joint_positions(next_command.command)[:, 0], + torch.tensor([0.4, 0.2]), + ) assert action.plan_count == 2 def test_replan_exhaustion_disables_only_triggering_row() -> None: engine, _ = _engine(batch_size=2) session = engine.start( - (_invocation(max_replans=1),), + (_invocation(engine, max_replans=1),), _context(0.0, (0.0, 0.0), (0.1, 0.2), 0), ) session.tick(_context(0.0, (0.0, 0.0), (0.1, 0.2), 0)) @@ -674,9 +882,51 @@ def test_replan_exhaustion_disables_only_triggering_row() -> None: assert exhausted.command.active_mask.tolist() == [False, True] +def test_action_retry_resets_replan_budget_only_for_allowed_rows() -> None: + engine, _ = _engine(batch_size=2) + session = engine.start( + ( + _invocation( + engine, + max_replans=1, + max_action_retries=1, + ), + ), + _context(0.0, (0.0, 0.0), (0.1, 0.2), 0), + ) + session.tick(_context(0.0, (0.0, 0.0), (0.1, 0.2), 0)) + + row_b_replan = session.tick(_context(0.1, (0.0, 0.0), (0.1, 0.4), 1)) + changed = next( + event + for event in row_b_replan.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.env_mask.tolist() == [False, True] + + retry_events = session._attempt_action_retry( + torch.tensor([True, False]), + ExecutionEventKind.ACTION_TIMEOUT, + "Row A starts a new action attempt.", + ) + retried = next( + event for event in retry_events if event.kind is ExecutionEventKind.ACTION_RETRY + ) + assert retried.env_mask.tolist() == [True, False] + + row_b_exhausted = session.tick(_context(0.2, (0.0, 0.0), (0.1, 0.6), 2)) + exhausted = next( + event + for event in row_b_exhausted.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert exhausted.env_mask.tolist() == [False, True] + assert row_b_exhausted.eligible_mask.tolist() == [True, False] + + def test_session_revision_replans_from_latest_context() -> None: engine, action = _engine() - original = _invocation() + original = _invocation(engine) session = engine.start((original,), _context(0.0, 0.0, 0.1, 0)) revised_pose = torch.eye(4).unsqueeze(0) revised_pose[:, 0, 3] = 0.8 @@ -702,13 +952,12 @@ def test_session_revision_replans_from_latest_context() -> None: and event.invocation_revision == 1 for event in first.events ) - assert second.command is not None - assert torch.all(second.command.positions == 0.8) + assert torch.all(_joint_positions(second.command) == 0.8) def test_session_revision_must_advance_same_invocation() -> None: engine, _ = _engine() - original = _invocation() + original = _invocation(engine) session = engine.start((original,), _context(0.0, 0.0, 0.1, 0)) with pytest.raises(ValueError, match="must advance"): @@ -728,10 +977,84 @@ def test_session_revision_must_advance_same_invocation() -> None: ) +def test_session_revision_rejects_runtime_destination_change() -> None: + engine, action = _destination_engine(("first", "second")) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + with pytest.raises( + ValueError, + match="Invocation revisions must preserve the active runtime destination set", + ) as exc_info: + session.revise_current(replace(invocation, revision=1)) + + assert "Start a new invocation" in str(exc_info.value) + assert "arm_a" in str(exc_info.value) + assert "arm_b" in str(exc_info.value) + assert action.plan_count == 2 + + active = session.tick(initial) + assert active.command is not None + assert active.command.commands[0].target.target_id == "arm_a" + + +def test_session_revision_rejects_empty_target_plan() -> None: + engine, action = _destination_engine(("first", None)) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + with pytest.raises(ValueError, match="empty replacement plan"): + session.revise_current(replace(invocation, revision=1)) + + assert action.plan_count == 2 + active = session.tick(initial) + assert active.command is not None + assert active.command.commands[0].target.target_id == "arm_a" + + +def test_session_revision_rejects_changed_target_address_fingerprint() -> None: + engine, action = _engine() + invocation = _invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + endpoint = invocation.binding.endpoint("primary", "motion") + changed_endpoint = EndpointBinding( + slot_id=endpoint.slot_id, + endpoint_id=endpoint.endpoint_id, + resource_id=endpoint.resource_id, + adapter_id=endpoint.adapter_id, + target=JointPositionTarget(control_part="arm", joint_ids=(0,)), + capabilities=endpoint.capabilities, + commands=endpoint.commands, + claim_tokens=endpoint.claim_tokens, + joint_ids=(0,), + ) + revised = replace( + invocation, + binding=ActionBinding( + owner_id=invocation.binding.owner_id, + endpoints=(changed_endpoint,), + ), + revision=1, + ) + + with pytest.raises(ValueError, match="address fingerprint"): + session.revise_current(revised) + + assert action.plan_count == 2 + active = session.tick(initial) + assert active.command is not None + target = active.command.commands[0].target + assert isinstance(target, JointPositionTarget) + assert target.joint_ids == (0, 1) + + def test_tracking_error_fails_when_replan_budget_is_zero() -> None: engine, _ = _engine() session = engine.start( - (_invocation(max_replans=0),), + (_invocation(engine, max_replans=0),), _context(0.0, 0.0, 0.2, 0), ) session.tick(_context(0.0, 0.0, 0.2, 0)) @@ -750,6 +1073,7 @@ def test_action_timeout_retry_budget_is_bounded() -> None: session = engine.start( ( _invocation( + engine, max_action_retries=1, action_timeout=0.05, ), @@ -776,7 +1100,7 @@ def test_action_timeout_retry_budget_is_bounded() -> None: def test_session_rejects_changed_environment_identity() -> None: engine, _ = _engine() initial = _context(0.0, 0.0, 0.2, 0) - session = engine.start((_invocation(),), initial) + session = engine.start((_invocation(engine),), initial) changed = PlanningContext( robot=initial.robot, task=initial.task, @@ -790,7 +1114,7 @@ def test_session_rejects_changed_environment_identity() -> None: def test_session_rejects_regressing_scene_snapshot() -> None: engine, _ = _engine() - session = engine.start((_invocation(),), _context(1.0, 0.0, 0.2, 2)) + session = engine.start((_invocation(engine),), _context(1.0, 0.0, 0.2, 2)) with pytest.raises(ValueError, match="versions must be monotonic"): session.tick(_context(1.0, 0.0, 0.2, 1)) @@ -801,7 +1125,7 @@ def test_session_rejects_regressing_collision_world_revision() -> None: qpos = torch.zeros(1, 2) initial = _collision_context(0.0, qpos, torch.tensor([0.4]), (2,)) session = engine.start( - (_invocation(strategy="motion_gen"),), + (_invocation(engine, strategy="motion_gen"),), initial, ) regressed = _collision_context(0.1, qpos, torch.tensor([0.4]), (1,)) @@ -814,7 +1138,7 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None engine, _ = _engine() effect = EffectAction() engine.register(effect) - invocation = _invocation() + invocation = _invocation(engine) invocation = ActionInvocation( skill_id="effect", goal=invocation.goal, @@ -858,10 +1182,39 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None assert completed.task_state.get_held_object("arm") is not None +def test_session_revision_cannot_abandon_pending_effect_verification() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation(engine) + 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 + + with pytest.raises(RuntimeError, match="awaiting verification"): + 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]), + ) + assert completed.status is ExecutionStatus.COMPLETED + assert completed.task_state.get_held_object("arm") is not None + + def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: engine, _ = _engine() engine.register(EffectAction()) - base = _invocation(max_action_retries=0) + base = _invocation(engine, max_action_retries=0) invocation = ActionInvocation( skill_id="effect", goal=base.goal, @@ -888,7 +1241,7 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: engine, _ = _engine() engine.register(FailedEffectAction()) - base = _invocation(max_action_retries=0) + base = _invocation(engine, max_action_retries=0) invocation = ActionInvocation( skill_id="failed_effect", goal=base.goal, diff --git a/tests/sim/atomic_actions/test_motion_strategy_e2e.py b/tests/sim/atomic_actions/test_motion_strategy_e2e.py index dd3e7e058..eaf99f44d 100644 --- a/tests/sim/atomic_actions/test_motion_strategy_e2e.py +++ b/tests/sim/atomic_actions/test_motion_strategy_e2e.py @@ -25,7 +25,6 @@ from embodichain.lab.sim.robots import CobotMagicCfg from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -79,14 +78,16 @@ def _run_reach_test(self, strategy: str): sim, robot, engine = self._setup() try: target, arm_ids = self._reachable_target(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": self.CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target), - binding=ActionBinding( - manipulators={"primary": self.CONTROL_PART} - ), + binding=binding, motion_policy=MotionPolicy( strategy=strategy, sample_count=self.SAMPLE_INTERVAL, @@ -95,7 +96,10 @@ def _run_reach_test(self, strategy: str): ) ) assert result.plan_success.all().item(), f"{strategy} reported failure" - final_q = result.trajectory.positions[0, -1, arm_ids] + plan = result.action_plans[0] + assert plan.joint_trajectory is not None + assert plan.commands.frame_count == plan.joint_trajectory.waypoint_count + final_q = plan.joint_trajectory.positions[0, -1, arm_ids] fk = robot.compute_fk( qpos=final_q[None], name=self.CONTROL_PART, to_matrix=True )[0] diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index bfeccc04b..7fe66b73c 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -41,15 +41,22 @@ ExecutionRunner, ExecutionRunnerCfg, HeldObjectState, - JointCommand, + JOINT_POSITION_CAPABILITY, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, PlanningContext, RecoveryPolicy, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, + RuntimeEndpointTarget, RunnerStatus, SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, TimedTrajectory, @@ -115,14 +122,15 @@ def __init__(self, provider: FakeObservationProvider) -> None: self.provider = provider self.send_statuses: deque[CommandAckStatus] = deque() self.follow_commands: deque[bool] = deque() - self.sent: list[JointCommand] = [] + self.sent: list[RuntimeCommandFrame] = [] self.send_times: list[float] = [] - self.held: list[JointCommand] = [] + self.held: list[tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext]] = [] + self.cancelled: list[tuple[RuntimeEndpointTarget, ...]] = [] self.cancel_count = 0 def send( self, - command: JointCommand, + command: RuntimeCommandFrame, *, timeout: float, ) -> CommandAcknowledgement: @@ -136,22 +144,41 @@ def send( ) follows = self.follow_commands.popleft() if self.follow_commands else True if status is CommandAckStatus.ACCEPTED and follows: - self.provider.qpos = command.positions.clone() + positions = self.provider.qpos.clone() + for endpoint_command in command.commands: + target = endpoint_command.target + payload = endpoint_command.payload + assert isinstance(target, JointPositionTarget) + assert isinstance(payload, JointPositionPayload) + joint_ids = list(target.joint_ids) + positions[:, joint_ids] = torch.where( + command.active_mask[:, None], + payload.positions, + positions[:, joint_ids], + ) + self.provider.qpos = positions return CommandAcknowledgement(status) def hold( self, - command: JointCommand, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, *, timeout: float, ) -> CommandAcknowledgement: - """Record and apply a hold command.""" - self.held.append(command) - self.provider.qpos = command.positions.clone() + """Record targets and apply the supplied observed-state hold.""" + self.held.append((tuple(targets), context)) + self.provider.qpos = context.robot.qpos.clone() return CommandAcknowledgement.accepted_ack() - def cancel(self, *, timeout: float) -> CommandAcknowledgement: + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: """Record controller cancellation.""" + self.cancelled.append(tuple(targets)) self.cancel_count += 1 return CommandAcknowledgement.accepted_ack() @@ -161,7 +188,19 @@ class TimedAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): skill_id: ClassVar[str] = "timed" GoalType: ClassVar[type] = EndEffectorPoseGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + ), + ), + ) + ) def __init__(self, *, with_effect: bool = False) -> None: super().__init__() @@ -213,10 +252,19 @@ def _plan( ) +def _timed_action_binding(action: TimedAction) -> ActionBinding: + """Bind the timed action's generic motion endpoint to the fake arm.""" + return action.planning_services.bind_control_parts( + TimedAction.binding_contract, + {"primary": {"motion": "arm"}}, + ) + + def _make_runner( *, with_effect: bool = False, batch_size: int = BATCH_SIZE, + control_joint_ids: tuple[int, ...] | None = None, ) -> tuple[ ExecutionRunner, FakeClock, @@ -232,7 +280,9 @@ def _make_runner( robot.dof = ROBOT_DOF robot.control_parts = {"arm": object()} robot.get_qpos.return_value = torch.zeros(batch_size, ROBOT_DOF) - robot.get_joint_ids.return_value = list(range(ROBOT_DOF)) + robot.get_joint_ids.return_value = list( + range(ROBOT_DOF) if control_joint_ids is None else control_joint_ids + ) generator = Mock() generator.robot = robot generator.device = torch.device("cpu") @@ -247,7 +297,7 @@ def _make_runner( invocation = ActionInvocation( skill_id="timed", goal=EndEffectorPoseGoal(goal_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_timed_action_binding(action), motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), recovery_policy=RecoveryPolicy( max_replans=2, @@ -266,6 +316,30 @@ def _make_runner( return runner, clock, provider, sink, action +def test_joint_feedback_ignores_motion_outside_bound_endpoint() -> None: + runner, clock, provider, sink, action = _make_runner(control_joint_ids=(0,)) + + runner.step() + provider.qpos[:, 1] = 42.0 + clock.advance(FIRST_INTERVAL) + second = runner.step() + clock.advance(SECOND_INTERVAL) + runner.step() + clock.advance(SECOND_INTERVAL) + completed = runner.step() + + assert action.plan_count == 1 + assert len(sink.sent) == 3 + assert not any( + event.kind is ExecutionEventKind.TRACKING_ERROR + for step in (second, completed) + if step.tick is not None + for event in step.tick.events + ) + assert completed.status is RunnerStatus.COMPLETED + assert provider.qpos[0, 1].item() == 42.0 + + def test_runner_dispatches_only_when_timed_waypoint_is_due() -> None: runner, clock, _, sink, _ = _make_runner() @@ -289,13 +363,34 @@ def test_runner_dispatches_only_when_timed_waypoint_is_due() -> None: assert third.wait_duration == pytest.approx(SECOND_INTERVAL) -def test_session_active_trajectory_returns_an_owned_snapshot() -> None: +def test_runner_dispatches_transport_neutral_endpoint_frames() -> None: + runner, _, _, sink, _ = _make_runner() + + runner.step() + + frame = sink.sent[0] + assert isinstance(frame, RuntimeCommandFrame) + assert len(frame.commands) == 1 + endpoint_command = frame.commands[0] + assert isinstance(endpoint_command.target, JointPositionTarget) + assert endpoint_command.target.transport_id == "robot.joint_position" + assert endpoint_command.target.target_id == "arm" + assert endpoint_command.target.joint_ids == (0, 1) + assert isinstance(endpoint_command.payload, JointPositionPayload) + assert endpoint_command.payload.transport_id == endpoint_command.target.transport_id + + +def test_session_active_commands_return_an_owned_endpoint_snapshot() -> None: runner, _, _, _, _ = _make_runner() - trajectory = runner.session.active_trajectory - trajectory.positions.fill_(-1.0) + commands = runner.session.active_commands + payload = commands.frames[0].commands[0].payload + assert isinstance(payload, JointPositionPayload) + payload.positions.fill_(-1.0) - assert torch.all(runner.session.active_trajectory.positions >= 0.0) + current_payload = runner.session.active_commands.frames[0].commands[0].payload + assert isinstance(current_payload, JointPositionPayload) + assert torch.all(current_payload.positions >= 0.0) def test_runner_uses_the_longest_active_batch_interval_as_a_barrier() -> None: @@ -324,6 +419,11 @@ def test_runner_completes_and_holds_after_last_command_settles() -> None: assert completed.command_count == 3 assert [item.operation for item in completed.dispatches] == [CommandOperation.HOLD] assert len(sink.held) == 1 + held_targets, hold_context = sink.held[0] + assert [(target.transport_id, target.target_id) for target in held_targets] == [ + ("robot.joint_position", "arm") + ] + assert torch.equal(hold_context.robot.qpos, sink.provider.qpos) @pytest.mark.parametrize( @@ -345,6 +445,8 @@ def test_runner_safely_stops_when_command_is_not_accepted( CommandOperation.HOLD, ] assert sink.cancel_count == 1 + assert [target.target_id for target in sink.cancelled[0]] == ["arm"] + assert [target.target_id for target in sink.held[0][0]] == ["arm"] assert failed.message is not None and status.value in failed.message @@ -363,6 +465,8 @@ def test_runner_cancel_performs_cancel_then_hold() -> None: assert repeated.status is RunnerStatus.CANCELLED assert repeated.dispatches == () assert sink.cancel_count == 1 + assert sink.cancelled == [()] + assert sink.held[0][0] == () def test_runner_replans_from_observation_after_tracking_error() -> None: @@ -383,14 +487,17 @@ def test_runner_replans_from_observation_after_tracking_error() -> None: assert recovered.status is RunnerStatus.RUNNING -def test_runner_surfaces_explicit_invocation_revision() -> None: - runner, _, _, _, action = _make_runner() +def test_runner_revision_waits_for_deadline_and_plans_from_fresh_observation() -> None: + runner, clock, provider, sink, action = _make_runner() + first = runner.step() + assert first.wait_duration == pytest.approx(FIRST_INTERVAL) + revised_pose = torch.eye(4) revised_pose[0, 3] = 2.0 * TARGET_POSITION revised = ActionInvocation( skill_id="timed", goal=EndEffectorPoseGoal(revised_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_timed_action_binding(action), motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), recovery_policy=RecoveryPolicy( max_replans=2, @@ -400,11 +507,24 @@ def test_runner_surfaces_explicit_invocation_revision() -> None: revision=1, ) - runner.session.revise_current(revised) + runner.revise_current(revised) + provider.qpos.fill_(0.4) + waiting = runner.step() + + assert waiting.is_waiting is True + assert action.plan_count == 1 + assert sink.send_times == [0.0] + + clock.advance(FIRST_INTERVAL) result = runner.step() assert action.plan_count == 2 + assert result.command_count == 2 + assert sink.send_times == pytest.approx([0.0, FIRST_INTERVAL]) assert result.tick is not None + revised_payload = result.tick.command.commands[0].payload + assert isinstance(revised_payload, JointPositionPayload) + assert torch.allclose(revised_payload.positions, torch.full((1, 2), 0.4)) assert any( event.kind is ExecutionEventKind.INVOCATION_REVISED and event.invocation_revision == 1 @@ -412,6 +532,41 @@ def test_runner_surfaces_explicit_invocation_revision() -> None: ) +def test_runner_revision_rejects_pending_effect_verification() -> None: + runner, _, _, _, action = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None + assert blocked.tick.pending_effect is not None + assert runner.effect_verification_pending is True + + revised_pose = torch.eye(4) + revised_pose[0, 3] = 2.0 * TARGET_POSITION + revised = ActionInvocation( + skill_id="timed", + goal=EndEffectorPoseGoal(revised_pose), + binding=_timed_action_binding(action), + motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), + recovery_policy=RecoveryPolicy( + max_replans=2, + tracking_error_threshold=0.05, + action_timeout=10.0, + ), + revision=1, + ) + + with pytest.raises(RuntimeError, match="awaiting verification"): + 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, + ) + ) + assert completed.status is RunnerStatus.COMPLETED + + def test_runner_fails_safely_when_observation_provider_raises() -> None: runner, _, provider, sink, _ = _make_runner() provider.fail = True @@ -425,6 +580,8 @@ def test_runner_fails_safely_when_observation_provider_raises() -> None: ] assert len(sink.held) == 1 assert sink.cancel_count == 1 + assert sink.cancelled == [()] + assert sink.held[0][0] == () assert failed.message is not None and "observation unavailable" in failed.message diff --git a/tests/sim/atomic_actions/test_runtime_commands.py b/tests/sim/atomic_actions/test_runtime_commands.py new file mode 100644 index 000000000..fb0e6bd62 --- /dev/null +++ b/tests/sim/atomic_actions/test_runtime_commands.py @@ -0,0 +1,379 @@ +# ---------------------------------------------------------------------------- +# 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 value-object tests for transport-neutral runtime commands.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + RuntimeCommandPayload, + TimedCommandSequence, +) + + +@dataclass(frozen=True, slots=True) +class _TestTarget(RuntimeEndpointTarget): + """Small target used to exercise custom transports.""" + + _transport_id: str + _target_id: str + + @property + def transport_id(self) -> str: + """Return the test transport identifier.""" + return self._transport_id + + @property + def target_id(self) -> str: + """Return the test destination identifier.""" + return self._target_id + + +@dataclass(frozen=True, slots=True) +class _OpaquePayload(RuntimeCommandPayload): + """Metadata-only payload used for transport and device validation.""" + + rows: int + payload_device: torch.device + payload_transport: str + + @property + def batch_size(self) -> int: + """Return the configured row count.""" + return self.rows + + @property + def device(self) -> torch.device: + """Return the configured device.""" + return self.payload_device + + @property + def transport_id(self) -> str: + """Return the configured transport identifier.""" + return self.payload_transport + + def snapshot(self) -> _OpaquePayload: + """Return an independently owned payload.""" + return _OpaquePayload( + rows=self.rows, + payload_device=self.payload_device, + payload_transport=self.payload_transport, + ) + + +class _SelfSnapshotPayload(RuntimeCommandPayload): + """Invalid payload whose snapshot aliases the source.""" + + @property + def batch_size(self) -> int: + """Return one row.""" + return 1 + + @property + def device(self) -> torch.device: + """Return the CPU device.""" + return torch.device("cpu") + + @property + def transport_id(self) -> str: + """Return the test transport.""" + return "test.transport" + + def snapshot(self) -> _SelfSnapshotPayload: + """Incorrectly return this same payload.""" + return self + + +def _joint_command( + control_part: str, + joint_ids: tuple[int, ...], + positions: torch.Tensor, +) -> EndpointCommand: + """Build one joint endpoint command for a test.""" + return EndpointCommand( + target=JointPositionTarget(control_part, joint_ids), + payload=JointPositionPayload(positions), + ) + + +def _frame( + commands: tuple[EndpointCommand, ...], + *, + active_mask: torch.Tensor | None = None, + env_ids: torch.Tensor | None = None, + hold_duration: torch.Tensor | None = None, +) -> RuntimeCommandFrame: + """Build a two-row CPU frame with optional field replacements.""" + return RuntimeCommandFrame( + commands=commands, + active_mask=( + torch.tensor([True, False]) if active_mask is None else active_mask + ), + env_ids=torch.tensor([4, 9]) if env_ids is None else env_ids, + hold_duration=( + torch.tensor([0.0, 0.1]) if hold_duration is None else hold_duration + ), + ) + + +def test_runtime_command_payload_is_abstract() -> None: + with pytest.raises(TypeError): + RuntimeCommandPayload() # type: ignore[abstract] + + +def test_joint_position_payload_owns_tensors_and_snapshots() -> None: + positions = torch.tensor([[1.0, 2.0], [3.0, 4.0]]) + velocities = torch.tensor([[0.1, 0.2], [0.3, 0.4]]) + payload = JointPositionPayload(positions, velocities) + + positions.fill_(9.0) + velocities.fill_(8.0) + snapshot = payload.snapshot() + snapshot.positions.fill_(7.0) + assert payload.positions.tolist() == [[1.0, 2.0], [3.0, 4.0]] + assert payload.velocities is not None + assert torch.allclose( + payload.velocities, + torch.tensor([[0.1, 0.2], [0.3, 0.4]]), + ) + assert payload.batch_size == 2 + assert payload.dof == 2 + assert payload.device == torch.device("cpu") + assert payload.transport_id == JointPositionTarget.TRANSPORT_ID + + +@pytest.mark.parametrize( + "positions, message", + [ + (torch.empty(0, 2), "non-zero"), + (torch.empty(2, 0), "non-zero"), + (torch.zeros(2), "shape"), + (torch.tensor([[float("nan")]]), "finite"), + ], +) +def test_joint_position_payload_rejects_invalid_positions( + positions: torch.Tensor, + message: str, +) -> None: + with pytest.raises(ValueError, match=message): + JointPositionPayload(positions) + + +def test_joint_position_payload_validates_velocities() -> None: + positions = torch.zeros(2, 2) + with pytest.raises(ValueError, match="match positions shape"): + JointPositionPayload(positions, torch.zeros(2, 3)) + with pytest.raises(ValueError, match="finite"): + JointPositionPayload( + positions, + torch.tensor([[0.0, float("inf")], [0.0, 0.0]]), + ) + + +def test_endpoint_command_requires_matching_transport() -> None: + with pytest.raises(ValueError, match="does not accept"): + EndpointCommand( + target=_TestTarget("test.target", "base"), + payload=_OpaquePayload(2, torch.device("cpu"), "test.payload"), + ) + + +def test_endpoint_command_owns_target_and_payload_snapshots() -> None: + target = _TestTarget("test.transport", "base") + payload = _OpaquePayload(2, torch.device("cpu"), "test.transport") + command = EndpointCommand(target=target, payload=payload) + + assert command.target is not target + assert command.payload is not payload + assert command.transport_id == "test.transport" + assert command.destination_key == ("test.transport", "base") + assert command.batch_size == 2 + assert command.device == torch.device("cpu") + assert command.snapshot().payload is not command.payload + + +def test_endpoint_command_rejects_aliased_payload_snapshot() -> None: + with pytest.raises(TypeError, match="independently owned"): + EndpointCommand( + target=_TestTarget("test.transport", "base"), + payload=_SelfSnapshotPayload(), + ) + + +def test_runtime_command_frame_accepts_disjoint_joint_destinations() -> None: + frame = _frame( + ( + _joint_command("left", (0, 2), torch.zeros(2, 2)), + _joint_command("right", (1, 3), torch.ones(2, 2)), + ) + ) + + assert frame.batch_size == 2 + assert frame.device == torch.device("cpu") + assert [target.target_id for target in frame.targets] == ["left", "right"] + assert frame.active_mask.tolist() == [True, False] + assert frame.env_ids.tolist() == [4, 9] + + +def test_runtime_command_frame_rejects_payload_batch_mismatch() -> None: + with pytest.raises(ValueError, match="batch size 1, expected 2"): + _frame((_joint_command("arm", (0,), torch.zeros(1, 1)),)) + + +def test_runtime_command_frame_rejects_payload_device_mismatch() -> None: + command = EndpointCommand( + target=_TestTarget("test.transport", "base"), + payload=_OpaquePayload(2, torch.device("meta"), "test.transport"), + ) + with pytest.raises(ValueError, match="share the frame device"): + _frame((command,)) + + +def test_runtime_command_frame_rejects_duplicate_destination() -> None: + target = _TestTarget("test.transport", "base") + command = EndpointCommand( + target=target, + payload=_OpaquePayload(2, torch.device("cpu"), "test.transport"), + ) + with pytest.raises(ValueError, match="duplicate destination"): + _frame((command, command)) + + +def test_runtime_command_frame_requires_joint_payload_for_joint_target() -> None: + command = EndpointCommand( + target=JointPositionTarget("arm", (0,)), + payload=_OpaquePayload( + 2, + torch.device("cpu"), + JointPositionTarget.TRANSPORT_ID, + ), + ) + with pytest.raises(TypeError, match="requires a JointPositionPayload"): + _frame((command,)) + + +def test_runtime_command_frame_rejects_joint_target_dof_mismatch() -> None: + with pytest.raises(ValueError, match="DOF 1, expected 2"): + _frame((_joint_command("arm", (0, 1), torch.zeros(2, 1)),)) + + +def test_runtime_command_frame_rejects_overlapping_joint_ids() -> None: + with pytest.raises(ValueError, match=r"overlaps joint IDs \[2\]"): + _frame( + ( + _joint_command("left", (0, 2), torch.zeros(2, 2)), + _joint_command("right", (2, 3), torch.zeros(2, 2)), + ) + ) + + +def test_runtime_command_frame_validates_batch_metadata() -> None: + command = _joint_command("arm", (0,), torch.zeros(2, 1)) + with pytest.raises(ValueError, match="active_mask"): + _frame((command,), active_mask=torch.tensor([1, 0])) + with pytest.raises(ValueError, match="env_ids"): + _frame((command,), env_ids=torch.tensor([4.0, 9.0])) + with pytest.raises(ValueError, match="hold_duration"): + _frame((command,), hold_duration=torch.tensor([0.0, float("nan")])) + with pytest.raises(ValueError, match="non-negative"): + _frame((command,), hold_duration=torch.tensor([0.0, -0.1])) + with pytest.raises(ValueError, match="unique"): + _frame((command,), env_ids=torch.tensor([4, 4])) + + +def test_runtime_command_frame_with_active_mask_returns_owned_frame() -> None: + frame = _frame((_joint_command("arm", (0,), torch.zeros(2, 1)),)) + replacement = torch.tensor([False, True]) + updated = frame.with_active_mask(replacement) + + replacement.fill_(False) + updated.commands[0].payload.positions.fill_(4.0) + assert updated.active_mask.tolist() == [False, True] + assert frame.active_mask.tolist() == [True, False] + assert isinstance(frame.commands[0].payload, JointPositionPayload) + assert frame.commands[0].payload.positions.tolist() == [[0.0], [0.0]] + + +def test_timed_command_sequence_preserves_empty_batch_and_device() -> None: + env_ids = torch.tensor([3, 7], dtype=torch.long) + sequence = TimedCommandSequence(frames=(), env_ids=env_ids) + + env_ids.fill_(0) + assert sequence.frame_count == 0 + assert sequence.batch_size == 2 + assert sequence.device == torch.device("cpu") + assert sequence.env_ids.tolist() == [3, 7] + assert sequence.targets == () + + +def test_timed_command_sequence_requires_matching_frame_env_ids() -> None: + frame = _frame((_joint_command("arm", (0,), torch.zeros(2, 1)),)) + with pytest.raises(ValueError, match="env_ids do not match"): + TimedCommandSequence( + frames=(frame,), + env_ids=torch.tensor([4, 8], dtype=torch.long), + ) + + +def test_timed_command_sequence_owns_frames_and_returns_unique_targets() -> None: + first = _frame( + ( + _joint_command("left", (0,), torch.zeros(2, 1)), + _joint_command("right", (1,), torch.ones(2, 1)), + ) + ) + second = _frame((_joint_command("left", (0,), torch.full((2, 1), 2.0)),)) + sequence = TimedCommandSequence( + frames=(first, second), + env_ids=torch.tensor([4, 9]), + ) + snapshot = sequence.snapshot() + + snapshot.frames[0].active_mask.fill_(False) + targets = sequence.targets + assert sequence.frame_count == 2 + assert sequence.frames[0].active_mask.tolist() == [True, False] + assert [target.target_id for target in targets] == ["left", "right"] + assert targets[0] is not sequence.frames[0].commands[0].target + + +def test_timed_command_sequence_rejects_invalid_frame_values() -> None: + with pytest.raises(TypeError, match="RuntimeCommandFrame"): + TimedCommandSequence( + frames=(object(),), # type: ignore[arg-type] + env_ids=torch.tensor([0], dtype=torch.long), + ) + + +def test_timed_command_sequence_requires_nonempty_int64_batch() -> None: + with pytest.raises(ValueError, match="int64"): + TimedCommandSequence(frames=(), env_ids=torch.empty(0, dtype=torch.long)) + with pytest.raises(ValueError, match="int64"): + TimedCommandSequence(frames=(), env_ids=torch.tensor([0.0])) + with pytest.raises(ValueError, match="unique"): + TimedCommandSequence(frames=(), env_ids=torch.tensor([2, 2])) diff --git a/tests/sim/atomic_actions/test_sim_adapter.py b/tests/sim/atomic_actions/test_sim_adapter.py index 01356abde..5b29109c0 100644 --- a/tests/sim/atomic_actions/test_sim_adapter.py +++ b/tests/sim/atomic_actions/test_sim_adapter.py @@ -25,9 +25,13 @@ from embodichain.lab.sim.atomic_actions import ( CommandAckStatus, - JointCommand, + EndpointCommand, + EndpointCommandTransport, + JointPositionPayload, + JointPositionTarget, RigidObjectSceneProvider, RigidObjectSceneProviderCfg, + RuntimeCommandFrame, SceneSnapshot, SimulationExecutionAdapter, TaskState, @@ -53,10 +57,20 @@ def _command( *, env_ids: torch.Tensor | None = None, active_mask: torch.Tensor | None = None, -) -> JointCommand: - return JointCommand( - positions=torch.ones(BATCH_SIZE, ROBOT_DOF), - velocities=torch.full((BATCH_SIZE, ROBOT_DOF), 0.5), +) -> RuntimeCommandFrame: + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget( + control_part="arm", + joint_ids=tuple(range(ROBOT_DOF)), + ), + payload=JointPositionPayload( + positions=torch.ones(BATCH_SIZE, ROBOT_DOF), + velocities=torch.full((BATCH_SIZE, ROBOT_DOF), 0.5), + ), + ), + ), active_mask=( torch.tensor([True, False]) if active_mask is None else active_mask ), @@ -80,6 +94,15 @@ def test_simulation_adapter_observes_full_robot_state() -> None: assert context.scene.version == 0 +def test_simulation_adapter_is_joint_position_transport() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + + assert isinstance(adapter, EndpointCommandTransport) + assert adapter.transport_id == JointPositionTarget.TRANSPORT_ID + assert adapter.payload_type is JointPositionPayload + + @pytest.mark.parametrize("error", [AttributeError, NotImplementedError]) def test_simulation_adapter_treats_unavailable_effort_as_optional( error: type[Exception], @@ -115,10 +138,86 @@ def test_simulation_adapter_sends_active_rows_and_inactive_holds_together() -> N assert acknowledgement.status is CommandAckStatus.ACCEPTED sent_qpos = robot.set_qpos.call_args.args[0] sent_qvel = robot.set_qvel.call_args.args[0] - assert torch.equal(sent_qpos, command.positions) - assert torch.equal(sent_qvel, command.velocities) + expected_qpos = torch.zeros(BATCH_SIZE, ROBOT_DOF) + expected_qpos[0] = 1.0 + expected_qvel = torch.zeros(BATCH_SIZE, ROBOT_DOF) + expected_qvel[0] = 0.5 + assert torch.equal(sent_qpos, expected_qpos) + assert torch.equal(sent_qvel, expected_qvel) + endpoint_command = command.commands[0] + assert isinstance(endpoint_command.target, JointPositionTarget) + assert endpoint_command.target.target_id == "arm" + assert endpoint_command.target.joint_ids == tuple(range(ROBOT_DOF)) + assert isinstance(endpoint_command.payload, JointPositionPayload) assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qpos.call_args.kwargs["joint_ids"] == [0, 1, 2] assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["joint_ids"] == [0, 1, 2] + + +def test_simulation_adapter_writes_disjoint_joint_endpoints_independently() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + command = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("arm", (0, 2)), + payload=JointPositionPayload(torch.tensor([[1.0, 3.0], [4.0, 6.0]])), + ), + EndpointCommand( + target=JointPositionTarget("tool", (1,)), + payload=JointPositionPayload(torch.tensor([[2.0], [5.0]])), + ), + ), + active_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + hold_duration=torch.zeros(BATCH_SIZE), + ) + + acknowledgement = adapter.send(command, timeout=1.0) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert robot.set_qpos.call_count == 2 + arm_call, tool_call = robot.set_qpos.call_args_list + assert torch.equal( + arm_call.args[0], + torch.tensor([[1.0, 3.0], [4.0, 6.0]]), + ) + assert arm_call.kwargs == {"joint_ids": [0, 2], "env_ids": [0, 1]} + assert torch.equal(tool_call.args[0], torch.tensor([[2.0], [5.0]])) + assert tool_call.kwargs == {"joint_ids": [1], "env_ids": [0, 1]} + robot.set_qvel.assert_not_called() + + +def test_simulation_adapter_neutralizes_inactive_rows_without_velocity_payload() -> ( + None +): + simulation, robot = _simulation_and_robot() + robot.get_qvel.return_value = torch.tensor([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]]) + adapter = SimulationExecutionAdapter(simulation, robot) + command = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("arm", (0, 2)), + payload=JointPositionPayload(torch.ones(BATCH_SIZE, 2)), + ), + ), + active_mask=torch.tensor([True, False]), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + hold_duration=torch.zeros(BATCH_SIZE), + ) + + acknowledgement = adapter.send(command, timeout=1.0) + + assert acknowledgement.accepted + assert torch.equal( + robot.set_qvel.call_args.args[0], + torch.tensor([[0.1, 0.3], [0.0, 0.0]]), + ) + assert robot.set_qvel.call_args.kwargs == { + "joint_ids": [0, 2], + "env_ids": [0, 1], + } def test_simulation_adapter_send_writes_a_pure_hold_batch() -> None: @@ -129,8 +228,18 @@ def test_simulation_adapter_send_writes_a_pure_hold_batch() -> None: acknowledgement = adapter.send(command, timeout=1.0) assert acknowledgement.status is CommandAckStatus.ACCEPTED - robot.set_qpos.assert_called_once_with(command.positions, env_ids=[0, 1]) - robot.set_qvel.assert_called_once_with(command.velocities, env_ids=[0, 1]) + assert torch.equal( + robot.set_qpos.call_args.args[0], + torch.zeros(BATCH_SIZE, ROBOT_DOF), + ) + assert torch.equal( + robot.set_qvel.call_args.args[0], + torch.zeros(BATCH_SIZE, ROBOT_DOF), + ) + assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qpos.call_args.kwargs["joint_ids"] == [0, 1, 2] + assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["joint_ids"] == [0, 1, 2] def test_simulation_adapter_keeps_stable_ids_separate_from_robot_indices() -> None: @@ -147,14 +256,70 @@ def test_simulation_adapter_keeps_stable_ids_separate_from_robot_indices() -> No def test_simulation_adapter_hold_targets_every_environment() -> None: simulation, robot = _simulation_and_robot() + observed_positions = torch.full((BATCH_SIZE, ROBOT_DOF), 0.25) + robot.get_qpos.return_value = observed_positions adapter = SimulationExecutionAdapter(simulation, robot) command = _command() + context = adapter.observe(TaskState.empty(BATCH_SIZE, "cpu")) + + acknowledgement = adapter.hold(command.targets, context, timeout=1.0) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert torch.equal(robot.set_qpos.call_args.args[0], observed_positions) + assert torch.equal( + robot.set_qvel.call_args.args[0], + torch.zeros_like(observed_positions), + ) + assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qpos.call_args.kwargs["joint_ids"] == [0, 1, 2] + assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["joint_ids"] == [0, 1, 2] + + +def test_simulation_adapter_hold_scopes_write_to_target_joint_ids() -> None: + simulation, robot = _simulation_and_robot() + observed_positions = torch.tensor([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]]) + robot.get_qpos.return_value = observed_positions + adapter = SimulationExecutionAdapter(simulation, robot) + context = adapter.observe(TaskState.empty(BATCH_SIZE, "cpu")) + + acknowledgement = adapter.hold( + (JointPositionTarget("tool", (1,)),), + context, + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert torch.equal( + robot.set_qpos.call_args.args[0], + torch.tensor([[0.2], [0.5]]), + ) + assert robot.set_qpos.call_args.kwargs == { + "joint_ids": [1], + "env_ids": [0, 1], + } + assert torch.equal(robot.set_qvel.call_args.args[0], torch.zeros(BATCH_SIZE, 1)) - acknowledgement = adapter.hold(command, timeout=1.0) + +def test_simulation_adapter_cancel_validates_transport_targets() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + targets = _command().targets + + acknowledgement = adapter.cancel(targets, timeout=1.0) assert acknowledgement.status is CommandAckStatus.ACCEPTED - robot.set_qpos.assert_called_once_with(command.positions, env_ids=[0, 1]) - robot.set_qvel.assert_called_once_with(command.velocities, env_ids=[0, 1]) + assert [(target.transport_id, target.target_id) for target in targets] == [ + (JointPositionTarget.TRANSPORT_ID, "arm") + ] + robot.set_qpos.assert_not_called() + + invalid = adapter.cancel( + (JointPositionTarget("invalid", (ROBOT_DOF,)),), + timeout=1.0, + ) + assert invalid.status is CommandAckStatus.REJECTED + assert "outside robot DOF" in invalid.message def test_simulation_adapter_sleep_advances_integral_physics_steps() -> None: diff --git a/tests/sim/atomic_actions/test_transports.py b/tests/sim/atomic_actions/test_transports.py new file mode 100644 index 000000000..32b74c3a2 --- /dev/null +++ b/tests/sim/atomic_actions/test_transports.py @@ -0,0 +1,522 @@ +# ---------------------------------------------------------------------------- +# 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 routing tests for endpoint-command transports.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions.bindings import RuntimeEndpointTarget +from embodichain.lab.sim.atomic_actions.runner import ( + CommandAcknowledgement, + CommandAckStatus, + CommandSink, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + RuntimeCommandFrame, + RuntimeCommandPayload, +) +from embodichain.lab.sim.atomic_actions.transports import ( + EndpointCommandRouter, + EndpointCommandTransport, +) + + +@dataclass(frozen=True, slots=True) +class _Target(RuntimeEndpointTarget): + """Test-only runtime target.""" + + _transport_id: str + _target_id: str + + @property + def transport_id(self) -> str: + """Return the addressed transport.""" + return self._transport_id + + @property + def target_id(self) -> str: + """Return the local destination.""" + return self._target_id + + +@dataclass(frozen=True, slots=True) +class _Payload(RuntimeCommandPayload): + """Test-only payload with transport-neutral scalar data.""" + + _transport_id: str + values: torch.Tensor + + @property + def batch_size(self) -> int: + """Return the payload batch size.""" + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.values.device + + @property + def transport_id(self) -> str: + """Return the addressed transport.""" + return self._transport_id + + def snapshot(self) -> _Payload: + """Return an independently owned payload.""" + return _Payload(self._transport_id, self.values.clone()) + + +@dataclass(frozen=True, slots=True) +class _OtherPayload(RuntimeCommandPayload): + """Different payload type used to exercise compatibility checks.""" + + _transport_id: str + values: torch.Tensor + + @property + def batch_size(self) -> int: + """Return the payload batch size.""" + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.values.device + + @property + def transport_id(self) -> str: + """Return the addressed transport.""" + return self._transport_id + + def snapshot(self) -> _OtherPayload: + """Return an independently owned payload.""" + return _OtherPayload(self._transport_id, self.values.clone()) + + +class _FakeTransport: + """Recording transport with configurable acknowledgements.""" + + def __init__( + self, + transport_id: str, + *, + payload_type: type[RuntimeCommandPayload] = _Payload, + ) -> None: + self._transport_id = transport_id + self._payload_type = payload_type + self.send_ack: object = CommandAcknowledgement.accepted_ack() + self.hold_ack: object = CommandAcknowledgement.accepted_ack() + self.cancel_ack: object = CommandAcknowledgement.accepted_ack() + self.send_error: Exception | None = None + self.hold_error: Exception | None = None + self.cancel_error: Exception | None = None + self.send_calls: list[tuple[RuntimeCommandFrame, float]] = [] + self.hold_calls: list[ + tuple[tuple[RuntimeEndpointTarget, ...], object, float] + ] = [] + self.cancel_calls: list[tuple[tuple[RuntimeEndpointTarget, ...], float]] = [] + + @property + def transport_id(self) -> str: + """Return the fake registration identifier.""" + return self._transport_id + + @property + def payload_type(self) -> type[RuntimeCommandPayload]: + """Return the accepted fake payload type.""" + return self._payload_type + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record one transport-local frame.""" + self.send_calls.append((frame, timeout)) + if self.send_error is not None: + raise self.send_error + return self.send_ack # type: ignore[return-value] + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: object, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record one transport-local hold.""" + self.hold_calls.append((targets, context, timeout)) + if self.hold_error is not None: + raise self.hold_error + return self.hold_ack # type: ignore[return-value] + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record one transport-local cancellation.""" + self.cancel_calls.append((targets, timeout)) + if self.cancel_error is not None: + raise self.cancel_error + return self.cancel_ack # type: ignore[return-value] + + +def _command( + transport_id: str, + target_id: str, + *, + payload_type: type[RuntimeCommandPayload] = _Payload, +) -> EndpointCommand: + """Build one two-row endpoint command.""" + return EndpointCommand( + target=_Target(transport_id, target_id), + payload=payload_type( # type: ignore[call-arg] + transport_id, + torch.tensor([[1.0], [2.0]]), + ), + ) + + +def _frame(*commands: EndpointCommand) -> RuntimeCommandFrame: + """Build one two-row command frame.""" + return RuntimeCommandFrame( + commands=commands, + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([3, 8]), + hold_duration=torch.tensor([0.1, 0.2]), + ) + + +def test_transport_protocol_is_runtime_checkable() -> None: + assert isinstance(_FakeTransport("alpha"), EndpointCommandTransport) + assert not isinstance(object(), EndpointCommandTransport) + + +def test_router_structurally_implements_command_sink() -> None: + assert isinstance(EndpointCommandRouter([]), CommandSink) + + +def test_router_builds_owned_exact_registry_from_mapping() -> None: + alpha = _FakeTransport("alpha") + registrations = {"alpha": alpha} + router = EndpointCommandRouter(registrations) + + registrations.clear() + assert dict(router.transports) == {"alpha": alpha} + with pytest.raises(TypeError): + router.transports["beta"] = _FakeTransport("beta") # type: ignore[index] + + +def test_router_rejects_non_exact_mapping_key() -> None: + with pytest.raises(ValueError, match="exactly match"): + EndpointCommandRouter({"alias": _FakeTransport("alpha")}) + + +def test_router_rejects_duplicate_transport_registration() -> None: + with pytest.raises(ValueError, match="more than once"): + EndpointCommandRouter([_FakeTransport("alpha"), _FakeTransport("alpha")]) + + +def test_router_rejects_invalid_transport_contract_and_payload_type() -> None: + with pytest.raises(TypeError, match="EndpointCommandTransport"): + EndpointCommandRouter([object()]) # type: ignore[list-item] + + invalid_payload = _FakeTransport("alpha") + invalid_payload._payload_type = str # type: ignore[assignment] + with pytest.raises(TypeError, match="payload_type"): + EndpointCommandRouter([invalid_payload]) + + +def test_send_groups_subframes_and_preserves_frame_metadata() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + router = EndpointCommandRouter({"alpha": alpha, "beta": beta}) + frame = _frame( + _command("alpha", "a0"), + _command("beta", "b0"), + _command("alpha", "a1"), + ) + + acknowledgement = router.send(frame, timeout=0.75) + + assert acknowledgement.accepted + assert len(alpha.send_calls) == 1 + assert len(beta.send_calls) == 1 + alpha_frame, alpha_timeout = alpha.send_calls[0] + beta_frame, beta_timeout = beta.send_calls[0] + assert [command.target.target_id for command in alpha_frame.commands] == [ + "a0", + "a1", + ] + assert [command.target.target_id for command in beta_frame.commands] == ["b0"] + assert torch.equal(alpha_frame.active_mask, frame.active_mask) + assert torch.equal(alpha_frame.env_ids, frame.env_ids) + assert torch.equal(alpha_frame.hold_duration, frame.hold_duration) + assert alpha_frame.active_mask.data_ptr() != frame.active_mask.data_ptr() + assert alpha_timeout == beta_timeout == 0.75 + + +def test_send_unknown_transport_rejects_before_any_dispatch() -> None: + alpha = _FakeTransport("alpha") + router = EndpointCommandRouter([alpha]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("missing", "x0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "missing" in acknowledgement.message + assert alpha.send_calls == [] + + +def test_send_incompatible_payload_rejects_before_dispatch() -> None: + alpha = _FakeTransport("alpha", payload_type=_Payload) + router = EndpointCommandRouter([alpha]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0", payload_type=_OtherPayload)), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "_Payload" in acknowledgement.message + assert "_OtherPayload" in acknowledgement.message + assert alpha.send_calls == [] + + +def test_send_aggregates_partial_rejection_with_transport_id() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + alpha.send_ack = CommandAcknowledgement.accepted_ack("queued") + beta.send_ack = CommandAcknowledgement( + CommandAckStatus.REJECTED, + "controller busy", + ) + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "beta" in acknowledgement.message + assert "controller busy" in acknowledgement.message + assert len(alpha.send_calls) == len(beta.send_calls) == 1 + + +def test_send_timed_out_status_takes_failure_precedence() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + alpha.send_ack = CommandAcknowledgement(CommandAckStatus.REJECTED, "rejected") + beta.send_ack = CommandAcknowledgement(CommandAckStatus.TIMED_OUT, "late") + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.TIMED_OUT + assert "alpha" in acknowledgement.message + assert "beta" in acknowledgement.message + + +def test_send_converts_transport_exception_and_continues_dispatch() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + alpha.send_error = RuntimeError("send exploded") + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "RuntimeError" in acknowledgement.message + assert "send exploded" in acknowledgement.message + assert len(alpha.send_calls) == len(beta.send_calls) == 1 + + +def test_hold_groups_targets_and_forwards_observation_context() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + router = EndpointCommandRouter([alpha, beta]) + context = object() + + acknowledgement = router.hold( + ( + _Target("alpha", "a0"), + _Target("beta", "b0"), + _Target("alpha", "a1"), + ), + context, # type: ignore[arg-type] + timeout=0.4, + ) + + assert acknowledgement.accepted + alpha_targets, alpha_context, alpha_timeout = alpha.hold_calls[0] + beta_targets, beta_context, beta_timeout = beta.hold_calls[0] + assert [target.target_id for target in alpha_targets] == ["a0", "a1"] + assert [target.target_id for target in beta_targets] == ["b0"] + assert alpha_context is beta_context is context + assert alpha_timeout == beta_timeout == 0.4 + + +def test_cancel_groups_targets_and_aggregates_partial_failure() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + beta.cancel_ack = CommandAcknowledgement(CommandAckStatus.TIMED_OUT, "late") + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.cancel( + ( + _Target("beta", "b0"), + _Target("alpha", "a0"), + _Target("beta", "b1"), + ), + timeout=0.2, + ) + + assert acknowledgement.status is CommandAckStatus.TIMED_OUT + assert "beta" in acknowledgement.message + assert [target.target_id for target in beta.cancel_calls[0][0]] == ["b0", "b1"] + assert [target.target_id for target in alpha.cancel_calls[0][0]] == ["a0"] + + +@pytest.mark.parametrize("operation", ["hold", "cancel"]) +def test_safe_stop_transport_exception_does_not_block_later_transport( + operation: str, +) -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + setattr(alpha, f"{operation}_error", RuntimeError(f"{operation} exploded")) + router = EndpointCommandRouter([alpha, beta]) + targets = (_Target("alpha", "a0"), _Target("beta", "b0")) + + if operation == "hold": + acknowledgement = router.hold( + targets, + object(), # type: ignore[arg-type] + timeout=1.0, + ) + alpha_calls = alpha.hold_calls + beta_calls = beta.hold_calls + else: + acknowledgement = router.cancel(targets, timeout=1.0) + alpha_calls = alpha.cancel_calls + beta_calls = beta.cancel_calls + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "RuntimeError" in acknowledgement.message + assert f"{operation} exploded" in acknowledgement.message + assert len(alpha_calls) == len(beta_calls) == 1 + + +@pytest.mark.parametrize("operation", ["hold", "cancel"]) +def test_target_operation_unknown_transport_rejects_before_dispatch( + operation: str, +) -> None: + alpha = _FakeTransport("alpha") + router = EndpointCommandRouter([alpha]) + if operation == "hold": + acknowledgement = router.hold( + (_Target("missing", "x0"),), + object(), # type: ignore[arg-type] + timeout=1.0, + ) + calls = alpha.hold_calls + else: + acknowledgement = router.cancel( + (_Target("missing", "x0"),), + timeout=1.0, + ) + calls = alpha.cancel_calls + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "missing" in acknowledgement.message + assert calls == [] + + +@pytest.mark.parametrize("operation", ["send", "hold", "cancel"]) +def test_router_converts_invalid_return_type_and_continues_dispatch( + operation: str, +) -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + setattr(alpha, f"{operation}_ack", object()) + router = EndpointCommandRouter([alpha, beta]) + + if operation == "send": + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + beta_calls = beta.send_calls + elif operation == "hold": + acknowledgement = router.hold( + (_Target("alpha", "a0"), _Target("beta", "b0")), + object(), # type: ignore[arg-type] + timeout=1.0, + ) + beta_calls = beta.hold_calls + else: + acknowledgement = router.cancel( + (_Target("alpha", "a0"), _Target("beta", "b0")), + timeout=1.0, + ) + beta_calls = beta.cancel_calls + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "CommandAcknowledgement" in acknowledgement.message + assert len(beta_calls) == 1 + + +@pytest.mark.parametrize("timeout", [0.0, -1.0, float("inf"), float("nan")]) +@pytest.mark.parametrize("operation", ["send", "hold", "cancel"]) +def test_router_rejects_invalid_timeout(operation: str, timeout: float) -> None: + router = EndpointCommandRouter([]) + + with pytest.raises(ValueError, match="timeout"): + if operation == "send": + router.send(_frame(), timeout=timeout) + elif operation == "hold": + router.hold((), object(), timeout=timeout) # type: ignore[arg-type] + else: + router.cancel((), timeout=timeout) + + +def test_empty_operations_are_accepted() -> None: + router = EndpointCommandRouter([]) + + assert router.send(_frame(), timeout=1.0).accepted + assert router.hold((), object(), timeout=1.0).accepted # type: ignore[arg-type] + assert router.cancel((), timeout=1.0).accepted diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index b1f5ec92e..8cfd6e2cd 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -928,7 +928,6 @@ def _make_curobo_engine( def test_curobo_reuses_non_graph_backend(): from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, @@ -939,13 +938,17 @@ def test_curobo_reuses_non_graph_backend(): try: engine = _make_curobo_engine(block) target = _target_beyond_block(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": _SIM_CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=target), - ActionBinding(manipulators={"primary": _SIM_CONTROL_PART}), + binding, MotionPolicy(strategy="motion_gen", sample_count=80), ), ) @@ -964,7 +967,7 @@ def test_curobo_reuses_non_graph_backend(): ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=target), - ActionBinding(manipulators={"primary": _SIM_CONTROL_PART}), + binding, MotionPolicy(strategy="motion_gen", sample_count=80), ), ) @@ -982,7 +985,6 @@ def test_curobo_reuses_non_graph_backend(): def test_curobo_uses_accelerator_with_cpu_physics(): from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, @@ -993,13 +995,17 @@ def test_curobo_uses_accelerator_with_cpu_physics(): try: engine = _make_curobo_engine(block, use_cuda_graph=True) target = _target_beyond_block(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": _SIM_CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=target), - ActionBinding(manipulators={"primary": _SIM_CONTROL_PART}), + binding, MotionPolicy(strategy="motion_gen", sample_count=80), ), ) diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index 0f7be1fb8..2082c35fa 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -26,7 +26,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBindingRoute, ActionOptions, ActionPlan, AtomicAction, @@ -51,6 +50,10 @@ SkillEndpointRequirement, SkillResourceSlot, ) +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.skills import ( AmbiguousSkillBindingError, @@ -228,7 +231,6 @@ class _WholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): SkillEndpointRequirement( "motion", capabilities=frozenset({"motion.whole_body"}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), @@ -254,7 +256,6 @@ class _NavigateAction(AtomicAction[JointPositionGoal, ActionOptions]): SkillEndpointRequirement( "motion", capabilities=frozenset({"motion.base.se2"}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), @@ -274,6 +275,7 @@ class _BaseVelocityEndpoint(ResourceEndpoint): """Future non-joint endpoint used to prove the resource API stays generic.""" controller_id: str + claim_id: str | None = None @dataclass(frozen=True, slots=True) @@ -284,6 +286,41 @@ class _MutableMetadataEndpoint(ResourceEndpoint): aliases: list[str] +@dataclass(frozen=True, slots=True) +class _BaseVelocityTarget(RuntimeEndpointTarget): + """Typed runtime destination for the test mobile controller.""" + + controller_id: str + + @property + def transport_id(self) -> str: + """Return the fake base-velocity transport kind.""" + return "test.base_velocity" + + @property + def target_id(self) -> str: + """Return the addressed controller ID.""" + return self.controller_id + + +@dataclass(frozen=True, slots=True) +class _MutableRuntimeTarget(RuntimeEndpointTarget): + """Target with nested mutable data used to prove snapshot ownership.""" + + controller_id: str + aliases: list[str] + + @property + def transport_id(self) -> str: + """Return the fake mutable-target transport kind.""" + return "test.mutable" + + @property + def target_id(self) -> str: + """Return the addressed controller ID.""" + return self.controller_id + + @dataclass(frozen=True, slots=True) class _TwistCommand(ControlCommand): """Test-only non-joint command for a mobile controller.""" @@ -314,9 +351,13 @@ def resolve( """Resolve one mobile controller to a generic exclusive claim.""" del engine assert isinstance(endpoint, _BaseVelocityEndpoint) + claim_id = ( + endpoint.controller_id if endpoint.claim_id is None else endpoint.claim_id + ) return EndpointResolution( + runtime_target=_BaseVelocityTarget(endpoint.controller_id), command_profile_key=endpoint.controller_id, - claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + claim_tokens=frozenset({f"controller:{claim_id}"}), ) @@ -325,8 +366,6 @@ class _VelocityNavigateAction(AtomicAction[JointPositionGoal, ActionOptions]): skill_id: ClassVar[str] = "navigate_velocity" GoalType: ClassVar[type] = JointPositionGoal - manipulator_roles: ClassVar[tuple[str, ...]] = () - end_effector_roles: ClassVar[tuple[str, ...]] = () binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -350,36 +389,6 @@ def _plan( raise NotImplementedError -class _RoutedVelocityAction(AtomicAction[JointPositionGoal, ActionOptions]): - """Test skill requiring a current-core route from a custom endpoint.""" - - skill_id: ClassVar[str] = "navigate_velocity_routed" - GoalType: ClassVar[type] = JointPositionGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = () - binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( - slots=( - SkillResourceSlot( - "body", - endpoints=( - SkillEndpointRequirement( - "motion", - capabilities=frozenset({"motion.base.velocity"}), - route=ActionBindingRoute("manipulator", "primary"), - ), - ), - ), - ) - ) - - def _plan( - self, - request: ResolvedActionRequest[JointPositionGoal, ActionOptions], - context: PlanningContext, - ) -> ActionPlan: - raise NotImplementedError - - def test_engine_skills_only_exposes_visible_explicit_installed_contracts() -> None: engine = _engine(control_profiles=_command_profiles()) expected = { @@ -403,23 +412,6 @@ class Derived(BUILTIN_ACTION_TYPES[0]): assert Derived.descriptor().binding_contract is None -def test_descriptor_contract_must_exactly_cover_current_core_roles() -> None: - class InvalidRouteAction(AtomicAction[JointPositionGoal, ActionOptions]): - skill_id: ClassVar[str] = "invalid_route" - GoalType: ClassVar[type] = JointPositionGoal - binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract() - - def _plan( - self, - request: ResolvedActionRequest[JointPositionGoal, ActionOptions], - context: PlanningContext, - ) -> ActionPlan: - raise NotImplementedError - - with pytest.raises(ValueError, match="do not exactly cover"): - InvalidRouteAction.descriptor() - - def test_profile_owns_input_mappings_and_command_tensors() -> None: resources = _resources() open_positions = torch.tensor([0.0]) @@ -464,6 +456,55 @@ def test_profile_owns_custom_endpoint_nested_payloads() -> None: assert profile_endpoint.aliases == ["base"] +def test_endpoint_resolution_requires_a_runtime_target() -> None: + with pytest.raises(TypeError, match="runtime_target"): + EndpointResolution( + runtime_target=None, # type: ignore[arg-type] + exclusive=False, + ) + + +def test_endpoint_resolution_owns_runtime_target_snapshot() -> None: + aliases = ["base"] + target = _MutableRuntimeTarget("base_controller", aliases) + + resolution = EndpointResolution(runtime_target=target, exclusive=False) + aliases.append("source_mutation") + target.aliases.append("target_mutation") + + assert resolution.runtime_target is not target + assert type(resolution.runtime_target) is _MutableRuntimeTarget + assert resolution.runtime_target.aliases == ["base"] + + +@pytest.mark.parametrize("returns_self", [False, True]) +def test_endpoint_resolution_rejects_invalid_target_snapshot( + returns_self: bool, +) -> None: + @dataclass(frozen=True, slots=True) + class InvalidSnapshotTarget(RuntimeEndpointTarget): + controller_id: str + + @property + def transport_id(self) -> str: + return "test.invalid_snapshot" + + @property + def target_id(self) -> str: + return self.controller_id + + def snapshot(self) -> RuntimeEndpointTarget: + if returns_self: + return self + return _BaseVelocityTarget(self.controller_id) + + with pytest.raises(TypeError, match="same target type"): + EndpointResolution( + runtime_target=InvalidSnapshotTarget("base_controller"), + exclusive=False, + ) + + def test_resource_graph_rejects_unknown_member_and_cycle() -> None: with pytest.raises(ValueError, match="unknown members"): RobotSkillProfile( @@ -568,12 +609,112 @@ def test_custom_endpoint_adapter_resolves_commands_and_physical_claim() -> None: ) resolved = bound.resolve("navigate_velocity") endpoint = resolved.resources["body"].endpoints["motion"] + binding_endpoint = resolved.action_binding.endpoint("body", "motion") assert endpoint.adapter_id == "test.base_velocity" + assert isinstance(endpoint.runtime_target, _BaseVelocityTarget) assert isinstance(endpoint.commands["stop"], _TwistCommand) assert resolved.claim.claim_tokens == frozenset({"controller:base_velocity"}) - assert resolved.action_binding.manipulators == {} - assert resolved.action_binding.end_effectors == {} + assert resolved.action_binding.owner_id == engine.binding_owner_id + assert binding_endpoint.resource_id == "mobile_base" + assert binding_endpoint.require_target(_BaseVelocityTarget).controller_id == ( + "base_velocity" + ) + assert isinstance(binding_endpoint.command("stop"), _TwistCommand) + + +def test_custom_endpoint_joint_claim_survives_action_binding_lowering() -> None: + class JointClaimAdapter(_BaseVelocityEndpointAdapter): + """Attach robot-joint ownership to a non-joint runtime target.""" + + adapter_id: ClassVar[str] = "test.base_velocity_joint_claim" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del engine + assert isinstance(endpoint, _BaseVelocityEndpoint) + return EndpointResolution( + runtime_target=_BaseVelocityTarget(endpoint.controller_id), + command_profile_key=endpoint.controller_id, + joint_ids=(6, 7), + ) + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + }, + command_profiles={ + "base_velocity": ControlPartCommandProfile( + commands={"stop": _TwistCommand((0.0, 0.0, 0.0))} + ) + }, + ) + engine = _engine(control_profiles={}, load_builtins=False) + engine.register(_VelocityNavigateAction()) + bound = engine.bind_skill_profile( + profile, + endpoint_adapters={_BaseVelocityEndpoint: JointClaimAdapter()}, + ) + + binding_endpoint = bound.resolve("navigate_velocity").action_binding.endpoint( + "body", "motion" + ) + + assert binding_endpoint.joint_ids == (6, 7) + + +def test_custom_endpoint_joint_claim_must_fit_robot_dof() -> None: + class OutOfRangeJointClaimAdapter(_BaseVelocityEndpointAdapter): + adapter_id: ClassVar[str] = "test.out_of_range_joint_claim" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del engine + assert isinstance(endpoint, _BaseVelocityEndpoint) + return EndpointResolution( + runtime_target=_BaseVelocityTarget(endpoint.controller_id), + joint_ids=(9,), + ) + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + }, + ) + + with pytest.raises(ProfileValidationError, match="outside robot DOF 9"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={ + _BaseVelocityEndpoint: OutOfRangeJointClaimAdapter(), + }, + ) def test_engine_constructor_forwards_custom_endpoint_adapters() -> None: @@ -626,31 +767,67 @@ def test_custom_endpoint_claim_tokens_protect_distinct_leaf_aliases() -> None: ) -def test_missing_adapter_binding_target_filters_skill_with_diagnostic() -> None: +def test_distinct_physical_leaves_cannot_share_one_runtime_target() -> None: profile = RobotSkillProfile( - "mobile", + "duplicate_runtime_target", resources={ - "mobile_base": RobotResource( - "mobile_base", + "base_a": RobotResource( + "base_a", endpoints={ - "motion": _BaseVelocityEndpoint( - "base_velocity", - capabilities=frozenset({"motion.base.velocity"}), - ) + "motion": _BaseVelocityEndpoint("shared", claim_id="base_a") }, - ) + ), + "base_b": RobotResource( + "base_b", + endpoints={ + "motion": _BaseVelocityEndpoint("shared", claim_id="base_b") + }, + ), }, ) - engine = _engine(control_profiles={}, load_builtins=False) - engine.register(_RoutedVelocityAction()) - bound = profile.bind( - engine, - endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + + with pytest.raises(ProfileValidationError, match="share runtime targets"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + + +def test_endpoint_adapter_cannot_omit_runtime_target() -> None: + class MissingRuntimeTargetAdapter(ResourceEndpointAdapter): + adapter_id: ClassVar[str] = "test.missing_runtime_target" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution( + runtime_target=None, # type: ignore[arg-type] + exclusive=False, + ) + + profile = RobotSkillProfile( + "missing_runtime_target", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={"motion": _BaseVelocityEndpoint("base_velocity")}, + ) + }, ) - assert "navigate_velocity_routed" not in bound.skills - with pytest.raises(UnsupportedSkillError, match="cannot lower.*manipulator"): - bound.resolve("navigate_velocity_routed") + with pytest.raises( + ProfileValidationError, + match="test.missing_runtime_target.*mobile_base.*motion.*runtime_target", + ): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: MissingRuntimeTargetAdapter()}, + ) def test_exclusive_custom_endpoint_requires_a_physical_claim() -> None: @@ -665,7 +842,9 @@ def resolve( engine: AtomicActionEngine, ) -> EndpointResolution: del endpoint, engine - return EndpointResolution() + return EndpointResolution( + runtime_target=_BaseVelocityTarget("base_velocity") + ) profile = RobotSkillProfile( "mobile", @@ -699,7 +878,10 @@ def resolve( engine: AtomicActionEngine, ) -> EndpointResolution: del endpoint, engine - return EndpointResolution(exclusive=False) + return EndpointResolution( + runtime_target=_BaseVelocityTarget("virtual"), + exclusive=False, + ) profile = RobotSkillProfile( "virtual", @@ -728,7 +910,10 @@ def resolve( engine: AtomicActionEngine, ) -> EndpointResolution: del endpoint, engine - return EndpointResolution(exclusive=False) + return EndpointResolution( + runtime_target=_BaseVelocityTarget("base_velocity"), + exclusive=False, + ) profile = RobotSkillProfile( "mobile", @@ -912,13 +1097,21 @@ def test_bind_rejects_unverified_standard_solver_capability() -> None: def test_unique_capability_binding_lowers_to_exact_action_binding() -> None: profile = _profile(resources=_resources(include_right=False)) - bound = profile.bind(_engine(control_profiles=_command_profiles())) + engine = _engine(control_profiles=_command_profiles()) + bound = profile.bind(engine) resolved = bound.resolve("pick_up") + motion = resolved.action_binding.endpoint("primary", "motion") + grasp = resolved.action_binding.endpoint("primary", "grasp") assert resolved.resource_ids == {"primary": "left_actor"} - assert resolved.action_binding.manipulators == {"primary": "left_arm"} - assert resolved.action_binding.end_effectors == {"primary": "left_hand"} + assert resolved.action_binding.owner_id == engine.binding_owner_id + assert resolved.action_binding.endpoint_keys == ( + ("primary", "motion"), + ("primary", "grasp"), + ) + assert motion.require_target(JointPositionTarget).control_part == "left_arm" + assert grasp.require_target(JointPositionTarget).control_part == "left_hand" assert resolved.claim.leaf_resource_ids == frozenset({"left_arm", "left_hand"}) assert resolved.claim.joint_ids == (0, 1, 2) @@ -1011,7 +1204,6 @@ def test_coupled_endpoint_views_are_allowed_without_disjoint_constraint() -> Non class CoupledWholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): skill_id: ClassVar[str] = "coupled_whole_body" GoalType: ClassVar[type] = JointPositionGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -1020,7 +1212,6 @@ class CoupledWholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): SkillEndpointRequirement( "motion", capabilities=frozenset({JOINT_POSITION_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( "posture", @@ -1091,12 +1282,22 @@ def test_generic_profile_supports_base_and_whole_body_without_arm_tool_fields() assert set(bound.skills) == {"navigate", "whole_body_reach"} assert whole_body.resource_ids == {"body": "whole_body"} - assert whole_body.action_binding.manipulators == {"primary": "full_body"} + assert ( + whole_body.action_binding.endpoint("body", "motion") + .require_target(JointPositionTarget) + .control_part + == "full_body" + ) assert whole_body.claim.leaf_resource_ids == frozenset( {"base", "torso", "left_arm", "right_arm"} ) assert navigation.resource_ids == {"body": "base"} - assert navigation.action_binding.manipulators == {"primary": "base"} + assert ( + navigation.action_binding.endpoint("body", "motion") + .require_target(JointPositionTarget) + .control_part + == "base" + ) def test_presets_are_versioned_snapshots_and_validate_planner() -> None: @@ -1190,7 +1391,6 @@ class Replacement(action_type): SkillEndpointRequirement( "motion", capabilities=frozenset({JOINT_POSITION_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), From baaa55bd917deaa0642999ef1a1d03bf921dbaa0 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 02:24:26 +0800 Subject: [PATCH 10/28] feat(sim): add declarative scene affordances --- embodichain/lab/sim/skills/__init__.py | 12 + embodichain/lab/sim/skills/scene.py | 489 ++++++++++++++++++++++++- tests/sim/skills/test_scene.py | 127 ++++++- 3 files changed, 624 insertions(+), 4 deletions(-) diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 7a990fb28..c3019445b 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -38,6 +38,10 @@ UnsupportedSkillError, ) from .scene import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, RegistrySceneProvider, SceneAffordanceRef, SceneArticulationRef, @@ -45,20 +49,26 @@ SceneCollisionWorldMode, SceneDynamics, SceneEntityRef, + SceneEntityMetadata, SceneEntityRegistration, SceneEntityStateProvider, SceneGeometryProvider, SceneLinkRef, SceneObjectRef, SceneRegistry, + UnsupportedSceneAffordanceError, ) __all__ = [ + "AmbiguousSceneAffordanceError", "AmbiguousSkillBindingError", "BoundRobotSkillProfile", "ControlPartEndpoint", "ControlPartEndpointAdapter", "EndpointResolution", + "GRASP_AFFORDANCE_CAPABILITY", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", "ProfileValidationError", "RegistrySceneProvider", "ResolvedRobotResource", @@ -76,6 +86,7 @@ "SceneCollisionWorldMode", "SceneDynamics", "SceneEntityRef", + "SceneEntityMetadata", "SceneEntityRegistration", "SceneEntityStateProvider", "SceneGeometryProvider", @@ -84,4 +95,5 @@ "SceneRegistry", "SkillPolicyPreset", "UnsupportedSkillError", + "UnsupportedSceneAffordanceError", ] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py index 62d71ec61..00fa219b0 100644 --- a/embodichain/lab/sim/skills/scene.py +++ b/embodichain/lab/sim/skills/scene.py @@ -31,6 +31,7 @@ from embodichain.lab.sim.common import BatchEntity from embodichain.lab.sim.atomic_actions import ( Affordance, + AntipodalAffordance, EntityState, SceneProvider, SceneSnapshot, @@ -43,13 +44,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 +163,211 @@ 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()) + ), + ) + + @runtime_checkable class SceneEntityStateProvider(Protocol): """Observe one registered entity for an ordered environment batch.""" @@ -161,6 +420,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 +459,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 +499,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 +522,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 +546,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 +578,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 +599,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 +623,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( @@ -398,6 +720,10 @@ class SceneRegistry: _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) + _entity_metadata: tuple[SceneEntityMetadata, ...] = field(repr=False) + _affordances_by_parent_capability: Mapping[ + tuple[str, str], tuple[SceneAffordanceRef, ...] + ] = field(repr=False) collision_world_mode: SceneCollisionWorldMode | None def __init__( @@ -448,6 +774,10 @@ def __init__( aliases[alias] = canonical_id self._validate_relationships(owned, by_id) + affordances_by_parent_capability = self._index_affordances(owned, by_id) + entity_metadata = tuple( + SceneEntityMetadata.from_registration(item) for item in owned + ) object.__setattr__(self, "_registrations", owned) object.__setattr__( self, @@ -482,6 +812,12 @@ def __init__( if item.collision_role is SceneCollisionRole.STATIC ), ) + object.__setattr__( + self, + "_affordances_by_parent_capability", + MappingProxyType(affordances_by_parent_capability), + ) + object.__setattr__(self, "_entity_metadata", entity_metadata) object.__setattr__(self, "collision_world_mode", collision_world_mode) @staticmethod @@ -528,11 +864,65 @@ def _validate_relationships( ) native_members[member_key] = registration.ref.entity_id + for registration in registrations: + for capability, default_ref in registration.default_affordances.items(): + default_registration = by_id.get(default_ref.entity_id) + if default_registration is None: + raise ValueError( + f"Scene entity {registration.ref.entity_id!r} declares " + f"unknown default affordance {default_ref.entity_id!r} " + f"for capability {capability!r}." + ) + if not isinstance(default_registration.ref, SceneAffordanceRef): + raise TypeError( + f"Default affordance {default_ref.entity_id!r} is registered " + f"as {type(default_registration.ref).__name__}, not " + "SceneAffordanceRef." + ) + if default_registration.parent != registration.ref: + actual_parent = default_registration.parent + raise ValueError( + f"Default affordance {default_ref.entity_id!r} is not a " + f"direct child of {registration.ref.entity_id!r}; its parent " + f"is {None if actual_parent is None else actual_parent.entity_id!r}." + ) + if capability not in default_registration.affordance_capabilities: + raise ValueError( + f"Default affordance {default_ref.entity_id!r} does not " + f"declare capability {capability!r}." + ) + + @staticmethod + def _index_affordances( + registrations: tuple[SceneEntityRegistration, ...], + by_id: Mapping[str, SceneEntityRegistration], + ) -> dict[tuple[str, str], tuple[SceneAffordanceRef, ...]]: + """Build deterministic parent/capability reverse lookup entries.""" + del by_id + mutable: dict[tuple[str, str], list[SceneAffordanceRef]] = {} + for registration in registrations: + if not isinstance(registration.ref, SceneAffordanceRef): + continue + assert registration.parent is not None + for capability in registration.affordance_capabilities: + mutable.setdefault( + (registration.parent.entity_id, capability), [] + ).append(registration.ref) + return { + key: tuple(sorted(refs, key=lambda ref: ref.entity_id)) + for key, refs in mutable.items() + } + @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._entity_metadata + @property def entity_refs(self) -> tuple[SceneEntityRef, ...]: """Return canonical typed references in registration order.""" @@ -640,6 +1030,93 @@ 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. + """ + 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 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. + """ + parent_ref = self.resolve(parent) + _validate_identifier(capability, "affordance capability") + 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 + registration = self._registrations_by_id[selected.entity_id] + if registration.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 registration.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] + parent_registration = self._registrations_by_id[parent_ref.entity_id] + default = parent_registration.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." + ) + def make_scene_provider( self, *, @@ -1326,6 +1803,10 @@ def _pose_change_mask( __all__ = [ + "AmbiguousSceneAffordanceError", + "GRASP_AFFORDANCE_CAPABILITY", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", "RegistrySceneProvider", "SceneAffordanceRef", "SceneArticulationRef", @@ -1333,10 +1814,12 @@ def _pose_change_mask( "SceneCollisionWorldMode", "SceneDynamics", "SceneEntityRef", + "SceneEntityMetadata", "SceneEntityRegistration", "SceneEntityStateProvider", "SceneGeometryProvider", "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "UnsupportedSceneAffordanceError", ] diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py index 89c7b34fc..935dda770 100644 --- a/tests/sim/skills/test_scene.py +++ b/tests/sim/skills/test_scene.py @@ -23,8 +23,15 @@ 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.skills import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneArticulationRef, SceneCollisionRole, @@ -33,6 +40,7 @@ SceneLinkRef, SceneObjectRef, SceneRegistry, + UnsupportedSceneAffordanceError, ) @@ -146,6 +154,17 @@ 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() + + @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"): @@ -228,6 +247,112 @@ 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_collision_registration_requires_geometry_provider() -> None: with pytest.raises(ValueError, match="geometry_provider"): SceneEntityRegistration( From 2421b1f30df3ad7b08562efceae586ab27149f39 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 02:25:17 +0800 Subject: [PATCH 11/28] feat(sim): add semantic call catalog --- embodichain/lab/sim/atomic_actions/engine.py | 18 + embodichain/lab/sim/skills/__init__.py | 24 + embodichain/lab/sim/skills/calls.py | 822 +++++++++++++++++++ embodichain/lab/sim/skills/profiles.py | 13 +- tests/sim/skills/test_calls.py | 434 ++++++++++ tests/sim/skills/test_profiles.py | 16 + 6 files changed, 1326 insertions(+), 1 deletion(-) create mode 100644 embodichain/lab/sim/skills/calls.py create mode 100644 tests/sim/skills/test_calls.py diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index d73e9a3f5..0baab8ab7 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -133,6 +133,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() @@ -196,6 +197,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.""" @@ -294,6 +305,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: diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index c3019445b..9aa1cb545 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -18,6 +18,19 @@ from __future__ import annotations +from .calls import ( + DeclarativeValue, + HandOver, + Pick, + Place, + PlaceRelationTarget, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticCallSpec, + SemanticPose, + builtin_semantic_call_catalog, +) from .profiles import ( AmbiguousSkillBindingError, BoundRobotSkillProfile, @@ -65,10 +78,15 @@ "BoundRobotSkillProfile", "ControlPartEndpoint", "ControlPartEndpointAdapter", + "DeclarativeValue", "EndpointResolution", "GRASP_AFFORDANCE_CAPABILITY", + "HandOver", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", + "Pick", + "Place", + "PlaceRelationTarget", "ProfileValidationError", "RegistrySceneProvider", "ResolvedRobotResource", @@ -78,6 +96,7 @@ "ResourceClaim", "ResourceEndpoint", "ResourceEndpointAdapter", + "RegisteredSemanticCall", "RobotResource", "RobotSkillProfile", "SceneAffordanceRef", @@ -93,7 +112,12 @@ "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "SemanticCallCatalog", + "SemanticCallDescriptor", + "SemanticCallSpec", + "SemanticPose", "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..71301fe71 --- /dev/null +++ b/embodichain/lab/sim/skills/calls.py @@ -0,0 +1,822 @@ +# ---------------------------------------------------------------------------- +# 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.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 + w, x, y, z = quaternion.unbind(dim=-1) + output = torch.zeros( + quaternion.shape[0], + 4, + 4, + dtype=quaternion.dtype, + device=quaternion.device, + ) + output[:, 0, 0] = 1.0 - 2.0 * (y * y + z * z) + output[:, 0, 1] = 2.0 * (x * y - z * w) + output[:, 0, 2] = 2.0 * (x * z + y * w) + output[:, 1, 0] = 2.0 * (x * y + z * w) + output[:, 1, 1] = 1.0 - 2.0 * (x * x + z * z) + output[:, 1, 2] = 2.0 * (y * z - x * w) + output[:, 2, 0] = 2.0 * (x * z - y * w) + output[:, 2, 1] = 2.0 * (y * z + x * w) + output[:, 2, 2] = 1.0 - 2.0 * (x * x + y * y) + output[:, :3, 3] = position + output[:, 3, 3] = 1.0 + 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. + skill_id: Atomic skill identifier installed separately on an engine. + binding_contract: Robot-independent resource requirements. + 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] + skill_id: str + binding_contract: SkillBindingContract + schema_version: int = 1 + target_descriptor: SkillDescriptor | None = None + + def __post_init__(self) -> None: + _validate_identifier(self.call_id, field_name="SemanticCallDescriptor.call_id") + _validate_identifier( + self.skill_id, field_name="SemanticCallDescriptor.skill_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." + ) + _validate_static_binding_contract( + self.binding_contract, + field_name="SemanticCallDescriptor.binding_contract", + ) + 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.skill_id != expected.skill_id + or (self.binding_contract != expected.binding_contract) + or ( + 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 ( + self.target_descriptor.skill_id != self.skill_id + or self.target_descriptor.binding_contract != self.binding_contract + or not self.target_descriptor.agent_visible + or self.target_descriptor.binding_contract is None + ): + raise ValueError( + "Registered target_descriptor must be agent-visible and match " + "skill_id plus binding_contract exactly." + ) + 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) + + +@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, + skill_id=_builtin_call_target(spec_type).skill_id, + binding_contract=_builtin_call_target(spec_type).binding_contract, + ) + 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/profiles.py b/embodichain/lab/sim/skills/profiles.py index f5f79dbf4..19eb69ec4 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -1131,6 +1131,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() @@ -1147,6 +1148,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.""" @@ -1274,7 +1285,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/tests/sim/skills/test_calls.py b/tests/sim/skills/test_calls.py new file mode 100644 index 000000000..742d0058c --- /dev/null +++ b/tests/sim/skills/test_calls.py @@ -0,0 +1,434 @@ +# ---------------------------------------------------------------------------- +# 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, + skill_id=target.skill_id, + binding_contract=target.binding_contract, + 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, + skill_id="unsafe", + binding_contract=SkillBindingContract(), + ) + + +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: + with pytest.raises(ValueError, match="exact curated"): + SemanticCallDescriptor( + call_id=Pick.call_kind, + spec_type=Pick, + skill_id="move_joints", + binding_contract=SkillBindingContract(), + ) + + +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, + skill_id=source.skill_id, + binding_contract=source.binding_contract, + 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, + skill_id=target.skill_id, + binding_contract=contract, + 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, + skill_id=target.skill_id, + binding_contract=target.binding_contract, + target_descriptor=live_target, + ) diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index 2082c35fa..a1644e9e5 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -1404,4 +1404,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__ = [] From 5d6a5248887cce7b3611fb305d2310f4257bb1a6 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 02:27:19 +0800 Subject: [PATCH 12/28] feat(sim): add semantic integration manifest --- embodichain/lab/sim/skills/__init__.py | 20 + embodichain/lab/sim/skills/integration.py | 1270 +++++++++++++++++++++ tests/sim/skills/test_integration.py | 544 +++++++++ 3 files changed, 1834 insertions(+) create mode 100644 embodichain/lab/sim/skills/integration.py create mode 100644 tests/sim/skills/test_integration.py diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 9aa1cb545..dceaa94ee 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -31,6 +31,17 @@ SemanticPose, builtin_semantic_call_catalog, ) +from .integration import ( + BoundSemanticCall, + BoundSemanticIntegration, + LinkedSemanticCall, + PathPart, + SceneEntityManifest, + SceneManifest, + SemanticDiagnostic, + SemanticIntegrationManifest, + SemanticValidationError, +) from .profiles import ( AmbiguousSkillBindingError, BoundRobotSkillProfile, @@ -75,6 +86,8 @@ __all__ = [ "AmbiguousSceneAffordanceError", "AmbiguousSkillBindingError", + "BoundSemanticCall", + "BoundSemanticIntegration", "BoundRobotSkillProfile", "ControlPartEndpoint", "ControlPartEndpointAdapter", @@ -82,8 +95,10 @@ "EndpointResolution", "GRASP_AFFORDANCE_CAPABILITY", "HandOver", + "LinkedSemanticCall", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", + "PathPart", "Pick", "Place", "PlaceRelationTarget", @@ -107,15 +122,20 @@ "SceneEntityRef", "SceneEntityMetadata", "SceneEntityRegistration", + "SceneEntityManifest", "SceneEntityStateProvider", "SceneGeometryProvider", "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "SceneManifest", "SemanticCallCatalog", "SemanticCallDescriptor", "SemanticCallSpec", + "SemanticDiagnostic", + "SemanticIntegrationManifest", "SemanticPose", + "SemanticValidationError", "SkillPolicyPreset", "UnsupportedSkillError", "UnsupportedSceneAffordanceError", diff --git a/embodichain/lab/sim/skills/integration.py b/embodichain/lab/sim/skills/integration.py new file mode 100644 index 000000000..d6fe6a28c --- /dev/null +++ b/embodichain/lab/sim/skills/integration.py @@ -0,0 +1,1270 @@ +# ---------------------------------------------------------------------------- +# 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 ( + Affordance, + 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, + SceneCollisionRole, + SceneDynamics, + SceneEntityMetadata, + SceneEntityRef, + SceneEntityRegistration, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + +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: + """Provider-free static scene-entity declaration. + + Args: + ref: Canonical typed entity reference. + aliases: Boundary aliases accepted during static linking. + 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 classification. + affordance_capabilities: Semantic operations supplied by an affordance. + default_affordances: Capability-scoped direct-child defaults. + affordance_payload_type: Exact registered affordance payload type. + affordance_revision: Stable payload revision or fingerprint. + relative_pose: Flattened parent-relative homogeneous transform. + """ + + 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: + if not isinstance(self.ref, SceneEntityRef): + raise TypeError("SceneEntityManifest.ref must be a SceneEntityRef.") + if isinstance(self.aliases, (str, bytes)): + raise TypeError("aliases must be an iterable of identifiers.") + aliases = tuple(self.aliases) + for alias in aliases: + _validate_identifier(alias, field_name="scene aliases") + aliases = tuple(alias for alias in aliases if alias != self.ref.entity_id) + if len(set(aliases)) != len(aliases): + raise ValueError("SceneEntityManifest.aliases must be unique.") + object.__setattr__(self, "aliases", aliases) + if self.parent is not None and not isinstance(self.parent, SceneEntityRef): + raise TypeError("parent must be a SceneEntityRef or None.") + if self.semantic_type is not None: + _validate_identifier(self.semantic_type, field_name="semantic_type") + if isinstance(self.affordance_capabilities, (str, bytes)): + raise TypeError("affordance_capabilities must be an iterable.") + capabilities = frozenset(self.affordance_capabilities) + for capability in capabilities: + _validate_identifier(capability, field_name="affordance capabilities") + object.__setattr__(self, "affordance_capabilities", capabilities) + if not isinstance(self.default_affordances, Mapping): + raise TypeError("default_affordances must be a mapping.") + defaults: dict[str, SceneAffordanceRef] = {} + for capability, affordance in self.default_affordances.items(): + _validate_identifier(capability, field_name="default capabilities") + if type(affordance) is not SceneAffordanceRef: + raise TypeError( + "default_affordances values must be SceneAffordanceRef values." + ) + defaults[capability] = affordance + object.__setattr__( + self, + "default_affordances", + MappingProxyType(defaults), + ) + metadata = SceneEntityMetadata( + ref=self.ref, + aliases=self.aliases, + parent=self.parent, + native_name=self.native_name, + dynamics=self.dynamics, + collision_role=self.collision_role, + semantic_type=self.semantic_type, + affordance_capabilities=self.affordance_capabilities, + default_affordances=self.default_affordances, + affordance_payload_type=self.affordance_payload_type, + affordance_revision=self.affordance_revision, + relative_pose=self.relative_pose, + ) + object.__setattr__(self, "aliases", metadata.aliases) + object.__setattr__(self, "default_affordances", metadata.default_affordances) + object.__setattr__(self, "relative_pose", metadata.relative_pose) + + @classmethod + def from_registration( + cls, + registration: SceneEntityRegistration, + ) -> SceneEntityManifest: + """Project live registration metadata without reading providers.""" + if not isinstance(registration, SceneEntityRegistration): + raise TypeError("registration must be a SceneEntityRegistration.") + return cls.from_metadata(SceneEntityMetadata.from_registration(registration)) + + @classmethod + def from_metadata(cls, metadata: SceneEntityMetadata) -> SceneEntityManifest: + """Copy one provider-free registry 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, ...] + _by_id: Mapping[str, SceneEntityManifest] + _aliases: Mapping[str, str] + _affordances: Mapping[tuple[str, str], tuple[SceneAffordanceRef, ...]] + + def __init__(self, entries: Iterable[SceneEntityManifest] = ()) -> 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.") + by_id: dict[str, SceneEntityManifest] = {} + for entry in supplied: + if entry.ref.entity_id in by_id: + raise ValueError( + f"Duplicate scene manifest ID {entry.ref.entity_id!r}." + ) + by_id[entry.ref.entity_id] = entry + aliases: dict[str, str] = {} + for entry in supplied: + for alias in entry.aliases: + if alias in by_id: + raise ValueError( + f"Scene manifest alias {alias!r} collides with a canonical ID." + ) + previous = aliases.get(alias) + if previous is not None: + raise ValueError( + f"Scene manifest alias {alias!r} is ambiguous between " + f"{previous!r} and {entry.ref.entity_id!r}." + ) + aliases[alias] = entry.ref.entity_id + affordances: dict[tuple[str, str], list[SceneAffordanceRef]] = {} + native_members: dict[tuple[type[SceneEntityRef], str, str], str] = {} + for entry in supplied: + if entry.parent is not None: + parent_entry = by_id.get(entry.parent.entity_id) + if parent_entry is None: + raise ValueError( + f"Scene manifest entity {entry.ref.entity_id!r} references " + f"unknown parent {entry.parent.entity_id!r}." + ) + if type(parent_entry.ref) is not type(entry.parent): + raise TypeError( + f"Scene manifest parent {entry.parent.entity_id!r} has " + "the wrong reference type." + ) + if entry.native_name is not None and isinstance( + entry.ref, (SceneLinkRef, SceneAffordanceRef) + ): + native_key = ( + type(entry.ref), + entry.parent.entity_id, + entry.native_name, + ) + previous = native_members.get(native_key) + if previous is not None: + raise ValueError( + f"Scene manifest parent {entry.parent.entity_id!r} " + f"and native_name {entry.native_name!r} are already " + f"registered as {previous!r}." + ) + native_members[native_key] = entry.ref.entity_id + if isinstance(entry.ref, SceneAffordanceRef): + if entry.parent is None: + raise ValueError( + f"Affordance {entry.ref.entity_id!r} requires a parent." + ) + for capability in entry.affordance_capabilities: + affordances.setdefault( + (entry.parent.entity_id, capability), [] + ).append(entry.ref) + elif entry.affordance_capabilities: + raise ValueError( + "Only SceneAffordanceRef entries may declare " + "affordance_capabilities." + ) + for entry in supplied: + if isinstance(entry.ref, SceneAffordanceRef) and entry.default_affordances: + raise ValueError( + "Scene affordance entries cannot declare default_affordances." + ) + for capability, default in entry.default_affordances.items(): + default_entry = by_id.get(default.entity_id) + if default_entry is None or not isinstance( + default_entry.ref, SceneAffordanceRef + ): + raise ValueError( + f"Default affordance {default.entity_id!r} is not a " + "registered affordance entry." + ) + if default_entry.parent != entry.ref: + raise ValueError( + f"Default affordance {default.entity_id!r} is not a direct " + f"child of {entry.ref.entity_id!r}." + ) + if capability not in default_entry.affordance_capabilities: + raise ValueError( + f"Default affordance {default.entity_id!r} does not support " + f"capability {capability!r}." + ) + object.__setattr__(self, "_entries", supplied) + object.__setattr__(self, "_by_id", MappingProxyType(by_id)) + object.__setattr__(self, "_aliases", MappingProxyType(aliases)) + object.__setattr__( + self, + "_affordances", + MappingProxyType( + { + key: tuple(sorted(refs, key=lambda ref: ref.entity_id)) + for key, refs in affordances.items() + } + ), + ) + + @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 + ) + + 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._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._by_id.get(candidate_id) + if entry is None: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_entity", + path, + f"Unknown scene entity {candidate_id!r}.", + tuple(self._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._by_id[ref.entity_id] + + 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._affordances.get((parent_ref.entity_id, capability), ()) + if explicit is not None: + selected = self.resolve( + explicit, + expected_type=SceneAffordanceRef, + path=path, + ) + entry = self._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._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._by_id) + live_ids = set(live._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._by_id[entity_id] != live._by_id[entity_id]: + raise SemanticValidationError( + SemanticDiagnostic( + "scene_manifest_mismatch", + (*path, entity_id), + "Live scene metadata 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, init=False) +class BoundSemanticCall: + """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 __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.") + 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.") + manifest.scene.validate_registry(scene_registry) + 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) + if type(linked.call) is RegisteredSemanticCall: + raise SemanticValidationError( + SemanticDiagnostic( + "semantic_lowerer_not_installed", + (*path, "kind"), + f"Registered semantic call {linked.call.semantic_id!r} was " + "discovered but has no explicitly installed compiler lowerer.", + ) + ) + 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._create( + linked=linked, + binding=binding, + preset=preset, + robot_profile=self._robot_profile, + ) + + +__all__ = [ + "BoundSemanticCall", + "BoundSemanticIntegration", + "LinkedSemanticCall", + "PathPart", + "SceneEntityManifest", + "SceneManifest", + "SemanticDiagnostic", + "SemanticIntegrationManifest", + "SemanticValidationError", +] diff --git a/tests/sim/skills/test_integration.py b/tests/sim/skills/test_integration.py new file mode 100644 index 000000000..ffa790bce --- /dev/null +++ b/tests/sim/skills/test_integration.py @@ -0,0 +1,544 @@ +# ---------------------------------------------------------------------------- +# 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 ( + BoundSemanticCall, + 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, + 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_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, + skill_id=pick.skill_id, + binding_contract=pick.binding_contract, + 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_is_factory_owned_by_installed_profile() -> 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 + with pytest.raises(TypeError, match="created by"): + BoundSemanticCall() + + +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, + ) From 7b2a1e93d67a7f0e9bf1dc70e4b09f93db217b19 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 02:58:37 +0800 Subject: [PATCH 13/28] feat(sim): compile semantic skill workflows --- embodichain/lab/sim/atomic_actions/goals.py | 13 + .../atomic_actions/primitives/hand_over.py | 57 +- .../sim/atomic_actions/primitives/pick_up.py | 36 +- embodichain/lab/sim/skills/__init__.py | 30 + embodichain/lab/sim/skills/compiler.py | 1446 +++++++++++++++++ embodichain/lab/sim/skills/integration.py | 9 - embodichain/lab/sim/skills/profiles.py | 10 + embodichain/lab/sim/skills/scene.py | 52 +- tests/sim/atomic_actions/test_actions.py | 80 + tests/sim/atomic_actions/test_core.py | 1 + tests/sim/skills/test_compiler.py | 892 ++++++++++ tests/sim/skills/test_profiles.py | 22 + tests/sim/skills/test_scene.py | 68 + 13 files changed, 2691 insertions(+), 25 deletions(-) create mode 100644 embodichain/lab/sim/skills/compiler.py create mode 100644 tests/sim/skills/test_compiler.py diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index f8d031130..755d3ec24 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -69,9 +69,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/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 02d8cf0ec..72c4bb0f4 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -30,6 +30,12 @@ from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics, _same_object_identity from ..effects import StateDelta +from ..goals import ( + PoseGoalValue, + collect_scene_dependencies, + resolve_pose_goal, + validate_pose_goal, +) from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..policies import MotionPolicy @@ -60,13 +66,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 ``(n_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 ``(n_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 @@ -116,7 +124,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) @@ -207,9 +224,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, @@ -301,10 +326,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 36ee0cc8d..a5037d661 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 @@ -42,6 +42,7 @@ ObjectActionGoal, PoseGoalValue, _resolve_object_pose, + collect_scene_dependencies, resolve_pose_goal, validate_pose_goal, ) @@ -112,7 +113,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 @@ -146,10 +147,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__( @@ -212,6 +223,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( @@ -312,7 +328,19 @@ def _plan( ) -> ActionPlan: """Plan approach, close, and lift segments without committing attachment.""" target = self.require_goal(request) - options = request.skill_options + options = replace( + request.skill_options, + downstream_object_target_poses=tuple( + resolve_pose_goal( + target, + context, + name=f"downstream_object_target_poses[{index}]", + ) + for index, target in enumerate( + request.skill_options.downstream_object_target_poses + ) + ), + ) approach_direction = options.approach_direction.to( device=self.device, dtype=torch.float32 ) diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index dceaa94ee..d3b7c2ea2 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -31,6 +31,22 @@ SemanticPose, builtin_semantic_call_catalog, ) +from .compiler import ( + AnalyzedSemanticCall, + GroundedSemanticCall, + HandOverPoseProvider, + HandOverPoseTargets, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticEffectDependency, + SemanticEffectKind, + SemanticHandOverTarget, + SemanticLowering, + SemanticObjectTarget, + SemanticRelationTarget, + SemanticSkillCompiler, + SemanticWorkflow, +) from .integration import ( BoundSemanticCall, BoundSemanticIntegration, @@ -86,6 +102,7 @@ __all__ = [ "AmbiguousSceneAffordanceError", "AmbiguousSkillBindingError", + "AnalyzedSemanticCall", "BoundSemanticCall", "BoundSemanticIntegration", "BoundRobotSkillProfile", @@ -94,7 +111,10 @@ "DeclarativeValue", "EndpointResolution", "GRASP_AFFORDANCE_CAPABILITY", + "GroundedSemanticCall", "HandOver", + "HandOverPoseProvider", + "HandOverPoseTargets", "LinkedSemanticCall", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", @@ -112,6 +132,8 @@ "ResourceEndpoint", "ResourceEndpointAdapter", "RegisteredSemanticCall", + "RegisteredSemanticLowerer", + "RelationTargetGrounder", "RobotResource", "RobotSkillProfile", "SceneAffordanceRef", @@ -133,9 +155,17 @@ "SemanticCallDescriptor", "SemanticCallSpec", "SemanticDiagnostic", + "SemanticEffectDependency", + "SemanticEffectKind", + "SemanticHandOverTarget", "SemanticIntegrationManifest", + "SemanticLowering", + "SemanticObjectTarget", "SemanticPose", + "SemanticRelationTarget", + "SemanticSkillCompiler", "SemanticValidationError", + "SemanticWorkflow", "SkillPolicyPreset", "UnsupportedSkillError", "UnsupportedSceneAffordanceError", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py new file mode 100644 index 000000000..9d3914736 --- /dev/null +++ b/embodichain/lab/sim/skills/compiler.py @@ -0,0 +1,1446 @@ +# ---------------------------------------------------------------------------- +# 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, + SkillDescriptor, +) +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. + + Exactly one source is set. 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. + """ + + pose: SemanticPose | SceneEntityPose | None = None + relation: SemanticRelationTarget | None = None + handover: SemanticHandOverTarget | None = None + + def __post_init__(self) -> None: + selected = sum( + value is not None for value in (self.pose, self.relation, self.handover) + ) + if selected != 1: + raise ValueError( + "SemanticObjectTarget requires exactly one of pose, relation, " + "or handover." + ) + if self.pose is not None: + if type(self.pose) is SemanticPose: + object.__setattr__(self, "pose", self.pose.snapshot()) + elif type(self.pose) is SceneEntityPose: + object.__setattr__(self, "pose", self.pose.snapshot()) + else: + raise TypeError( + "pose must be exactly SemanticPose, SceneEntityPose, or None." + ) + if self.relation is not None and ( + type(self.relation) is not SemanticRelationTarget + ): + raise TypeError("relation must be exactly SemanticRelationTarget or None.") + if self.handover is not None and ( + type(self.handover) is not SemanticHandOverTarget + ): + raise TypeError("handover must be exactly SemanticHandOverTarget or 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_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: + 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.") + 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: + """Return the canonical linked semantic call.""" + return self.bound.linked.call + + +@dataclass(frozen=True, slots=True, init=False) +class SemanticWorkflow: + """Factory-owned immutable result of static workflow analysis.""" + + workflow_id: str + calls: tuple[AnalyzedSemanticCall, ...] + effect_dependencies: tuple[SemanticEffectDependency, ...] = () + engine_owner_id: str = field(repr=False, compare=False, default="") + skill_catalog_revision: int = field(repr=False, compare=False, default=0) + compiler_id: str = field(repr=False, compare=False, default="") + + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :class:`SemanticSkillCompiler`.""" + del args, kwargs + raise TypeError( + "SemanticWorkflow values are created by " "SemanticSkillCompiler.analyze()." + ) + + @classmethod + def _create( + cls, + *, + workflow_id: str, + calls: tuple[AnalyzedSemanticCall, ...], + effect_dependencies: tuple[SemanticEffectDependency, ...], + engine_owner_id: str, + skill_catalog_revision: int, + compiler_id: str, + ) -> SemanticWorkflow: + """Create one owned workflow after compiler analysis.""" + instance = object.__new__(cls) + object.__setattr__(instance, "workflow_id", workflow_id) + object.__setattr__(instance, "calls", calls) + object.__setattr__(instance, "effect_dependencies", effect_dependencies) + object.__setattr__(instance, "engine_owner_id", engine_owner_id) + object.__setattr__( + instance, + "skill_catalog_revision", + skill_catalog_revision, + ) + object.__setattr__(instance, "compiler_id", compiler_id) + instance.__post_init__() + return instance + + 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.engine_owner_id, field_name="engine_owner_id") + _validate_identifier(self.compiler_id, field_name="compiler_id") + if type(self.skill_catalog_revision) is not int or ( + self.skill_catalog_revision < 0 + ): + raise ValueError("skill_catalog_revision must be non-negative.") + 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: + goal_kind = getattr(type(self.goal), "goal_kind", None) + if type(goal_kind) is not str or not goal_kind: + raise TypeError("goal must implement the typed ActionGoal protocol.") + 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] + target_descriptor: ClassVar[SkillDescriptor] + + @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, init=False) +class GroundedSemanticCall: + """Factory-owned call lowered from the latest observed context.""" + + analyzed: AnalyzedSemanticCall + invocation: ActionInvocation + _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, + 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, "_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.") + 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.") + + @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}." + ) + target_descriptor = getattr(type(lowerer), "target_descriptor", None) + if type(target_descriptor) is not SkillDescriptor or ( + target_descriptor != descriptor.target_descriptor + ): + raise ValueError( + f"Lowerer {call_id!r} target_descriptor must exactly match " + "the registered catalog target." + ) + 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_registration = self._integration.scene_registry.lookup( + target.relation.affordance, + expected_type=SceneAffordanceRef, + ) + if destination_registration.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 + requires_held = type(call) in (Place, HandOver) + 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_targets = ( + self._downstream_targets(index, bound_calls) + if type(call) is Pick + else () + ) + analyzed.append( + AnalyzedSemanticCall( + index=index, + bound=bound, + effect_kind=effect_kind, + downstream_object_targets=downstream_targets, + requires_verified_held_object=requires_held, + ) + ) + return SemanticWorkflow._create( + workflow_id=workflow_id, + calls=tuple(analyzed), + effect_dependencies=tuple(dependencies), + engine_owner_id=self._integration.engine.binding_owner_id, + skill_catalog_revision=self._integration.engine.skill_catalog_revision, + 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: Factory-owned 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: + Compiler-owned 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._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._create( + 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")) + engine = self._integration.engine + if workflow.engine_owner_id != engine.binding_owner_id: + raise _diagnostic( + "semantic_workflow_owner_mismatch", + path, + "The workflow belongs to a different action engine.", + ) + if workflow.compiler_id != self._compiler_id: + raise _diagnostic( + "semantic_program_stale", + path, + "The workflow belongs to a different compiler/grounder registry.", + ) + if workflow.skill_catalog_revision != engine.skill_catalog_revision: + raise _diagnostic( + "semantic_catalog_stale", + path, + "The installed semantic skill catalog changed after workflow " + "analysis.", + ) + + @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 _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 _downstream_targets( + self, + pick_index: int, + bound_calls: list[BoundSemanticCall], + ) -> 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, + ): + 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"), + ) + targets.append( + SemanticObjectTarget( + handover=SemanticHandOverTarget( + provider_id=provider_id, + bound=bound, + ) + ) + ) + break + if type(call) is Place: + if call.at is not None: + targets.append(SemanticObjectTarget(pose=call.at)) + else: + targets.append(self._relation_target(bound)) + break + return tuple(targets) + + 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=tuple( + self._ground_object_target(target, context) + for target in analyzed.downstream_object_targets + ) + ), + ) + + 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", "object"), + ) + 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", "object"), + ) + _, 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(pose=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.") + registration = self._integration.scene_registry.lookup( + affordance_ref, + expected_type=SceneAffordanceRef, + ) + if registration.affordance is None or registration.affordance_revision is None: + raise AssertionError( + "Capability-bearing relation affordance lacks payload metadata." + ) + return SemanticObjectTarget( + relation=SemanticRelationTarget( + capability=capability, + affordance=affordance_ref, + payload_type=type(registration.affordance), + payload_revision=registration.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, + 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, + 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 index d6fe6a28c..8efc8e046 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -1212,15 +1212,6 @@ def link_call( ) ) linked = self._manifest.link_call(call, path=path) - if type(linked.call) is RegisteredSemanticCall: - raise SemanticValidationError( - SemanticDiagnostic( - "semantic_lowerer_not_installed", - (*path, "kind"), - f"Registered semantic call {linked.call.semantic_id!r} was " - "discovered but has no explicitly installed compiler lowerer.", - ) - ) installed = self._engine.skills.get(linked.descriptor.skill_id) if installed is None or installed != linked.descriptor.target_descriptor: raise SemanticValidationError( diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 19eb69ec4..a9fe70f14 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -970,6 +970,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") @@ -1003,6 +1005,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() diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py index 00fa219b0..7d613da06 100644 --- a/embodichain/lab/sim/skills/scene.py +++ b/embodichain/lab/sim/skills/scene.py @@ -33,6 +33,7 @@ Affordance, AntipodalAffordance, EntityState, + ObjectSemantics, SceneProvider, SceneSnapshot, ) @@ -690,12 +691,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) @@ -1117,6 +1124,49 @@ def resolve_affordance( "explicitly." ) + 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, *, diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 01bcdb94f..3cfa32029 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -504,6 +504,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] @@ -1145,6 +1166,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 51c44393b..eabb6359a 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -576,6 +576,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, diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py new file mode 100644 index 000000000..f9a206e5f --- /dev/null +++ b/tests/sim/skills/test_compiler.py @@ -0,0 +1,892 @@ +# ---------------------------------------------------------------------------- +# 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, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + GraspGoal, + HandOverOptions, + HeldObjectState, + ObjectSemantics, + PickUp, + PickUpOptions, + PlaceGoal, + PlanningContext, + RobotObservation, + SceneEntityPose, + SkillDescriptor, + 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 ( + GroundedSemanticCall, + HandOverPoseProvider, + HandOverPoseTargets, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticLowering, + SemanticObjectTarget, + SemanticRelationTarget, + SemanticSkillCompiler, + SemanticWorkflow, +) +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 + target_descriptor: ClassVar[SkillDescriptor] = _PICK_TARGET + + 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 + target_descriptor: ClassVar[SkillDescriptor] = _PICK_TARGET + + 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(pose=SceneEntityPose("table_top")), + final=SemanticObjectTarget( + pose=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, + skill_id=_PICK_TARGET.skill_id, + binding_contract=_PICK_TARGET.binding_contract, + 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_targets[0].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_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 len(workflow.calls[0].downstream_object_targets) == 1 + 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" + + 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" + + grounded = compiler.ground( + workflow, + 0, + partial_context, + eligible_mask=torch.tensor([True, False]), + ) + assert grounded.eligible_mask.tolist() == [True, False] + with pytest.raises(TypeError, match="created by"): + GroundedSemanticCall( + analyzed=grounded.analyzed, + invocation=grounded.invocation, + eligible_mask=torch.tensor([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_targets == () + 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_is_factory_owned_and_cannot_cross_compilers() -> None: + registry, _ = _scene_registry() + first, _ = _compiler(registry) + second, _ = _compiler(registry) + workflow = first.analyze((Pick(object=SceneObjectRef("cube")),)) + + with pytest.raises(TypeError, match="created by"): + SemanticWorkflow() + with pytest.raises(SemanticValidationError) as error: + second.ground(workflow, 0, _context(registry)) + assert error.value.diagnostic.code in { + "semantic_program_stale", + "semantic_workflow_owner_mismatch", + } diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index a1644e9e5..ac31f550e 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -1346,6 +1346,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( diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py index 935dda770..bf364f56f 100644 --- a/tests/sim/skills/test_scene.py +++ b/tests/sim/skills/test_scene.py @@ -165,6 +165,14 @@ def __deepcopy__(self, memo: dict[int, object]) -> _CopyTrackedAffordance: 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"): @@ -353,6 +361,66 @@ def test_registry_metadata_projection_does_not_copy_affordance_payload() -> None 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( From e8623ffe5ec1d794382c3884540b1d6982514587 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 03:33:12 +0800 Subject: [PATCH 14/28] refactor(atomic-actions): preserve per-environment runtime lifecycle --- agent_context/MAP.yaml | 4 + .../topics/atomic-actions/atomic-actions.md | 48 +- .../overview/sim/atomic_actions/index.md | 43 +- docs/source/tutorial/atomic_actions.rst | 76 +- .../lab/sim/atomic_actions/__init__.py | 2 + embodichain/lab/sim/atomic_actions/effects.py | 108 ++- embodichain/lab/sim/atomic_actions/engine.py | 11 +- .../lab/sim/atomic_actions/execution.py | 517 +++++++++++-- .../lab/sim/atomic_actions/policies.py | 2 +- embodichain/lab/sim/atomic_actions/runner.py | 104 ++- .../sim/atomic_actions/test_engine_per_env.py | 722 +++++++++++++++++- tests/sim/atomic_actions/test_runner.py | 216 +++++- 12 files changed, 1728 insertions(+), 125 deletions(-) diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index d8648c4b6..308d42ea3 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -467,6 +467,10 @@ topics: - PlanningContext - ExecutionSession - EffectVerificationRequest + - EffectVerificationResult + - eligible_mask + - deactivate_rows + - effect verification deadline - ExecutionRunner - ObservationProvider - CommandSink diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 4c56e299c..de796f9fb 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -378,7 +378,7 @@ runner = ExecutionRunner( command_sink, clock=execution_clock, ) -result = runner.step(effect_success=None) +result = runner.step(effect_result=None) ``` `ExecutionSession` owns deterministic planning progress and recovery state. It @@ -404,15 +404,43 @@ active targets so the caller can still hold them. The session monitors: - action-attempt timeout; - planner and semantic-effect failure. -It replans from the latest observation within per-environment budgets. The -budgets and eligibility masks are row-local, while the action waypoint cursor -is batch-synchronized: one allowed replan regenerates the active cohort and -restarts its action trajectory without charging unaffected rows. Unknown -or exhausted failures are reported as structured `ExecutionEvent` objects. A -non-empty `StateDelta` is not committed until the caller supplies an external -`effect_success` mask. While verification is outstanding, -`ExecutionTick.pending_effect` retains a typed `EffectVerificationRequest` on -every tick; `EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. +It replans from the latest observation within per-environment budgets. Pass an +owned boolean `eligible_mask` to `engine.start()` when a previous semantic call +has already deactivated rows. Eligibility can only shrink; use +`runner.deactivate_rows(mask, reason=...)` while a runner owns scheduling so its +cached effect request stays correlated. The budgets, verified task state, and +eligibility masks are row-local, while the action waypoint cursor and call +barrier are batch-synchronized. One allowed replan regenerates the still-pending +cohort without charging unaffected rows. Exhausted rows hold and never become +eligible again. + +A non-empty `StateDelta` is not committed until the caller supplies a +correlated `EffectVerificationResult`. Its disjoint `success_mask` and +`failure_mask` must be subsets of the current request mask; requested rows in +neither mask remain unresolved. Partial successes commit immediately while +unresolved rows keep the barrier pending. `EffectVerificationRequest` carries a +monotonic `verification_id`, stable `requested_at`/`deadline` values in the +robot-observation timestamp domain, and an owned effect snapshot. Mask shrinkage +creates a new ID without extending the deadline; whole-action retry creates a +new attempt. Results for an old ID are rejected. `RecoveryPolicy.action_timeout` +covers the trajectory and terminal effect wait together, and only timestamps +strictly greater than the deadline time out. While verification is outstanding, +`ExecutionTick.pending_effect` retains the request on every tick; +`EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. + +```python +request = tick.pending_effect +effect_result = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=observed_success, + failure_mask=observed_failure, +) +result = runner.step(effect_result=effect_result) +``` + +Cause events (`ACTION_PLANNING_FAILED`, `EFFECT_VERIFICATION_FAILED`, and +`EFFECT_VERIFICATION_TIMEOUT`) are distinct from the `ACTION_RETRY` recovery +event. `SESSION_COMPLETED` and `SESSION_FAILED` are distinct terminal events. Recovery replans reuse the current immutable `ResolvedActionRequest`, including its owned goal snapshot. Mutable goal values are copied, while simulator-backed diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 0ae3d06b3..56afd9a57 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -427,9 +427,11 @@ an older custom action by renaming its implementation to `_plan()`. | `AtomicAction.plan(request, context)` | `AtomicActionEngine` | Binds the current collision scene into a copied policy, then delegates to `_plan()` | | `AtomicAction._plan(request, context)` | Atomic-action implementer | Consumes the prepared immutable `ResolvedActionRequest` and returns an `ActionPlan` | | `engine.plan_action(action, invocation, context)` | Extension or isolated test | Temporarily binds and plans an unregistered action instance; built-in parameter variants should use invocation `skill_options` instead | +| `engine.start(invocations, context, eligible_mask=...)` | Runtime orchestrator | Starts a session whose owned row cohort can only shrink across action barriers and recovery | | `session.revise_current(invocation)` | Manually ticked runtime orchestrator | Replaces the active logical call with a newer same-destination revision and replans from the latest observed context | | `runner.revise_current(invocation)` | Runner-driven runtime orchestrator or Action Agent | Snapshots a revision, preserves the current frame deadline, then replans from a fresh due-time observation | -| `runner.step(effect_success=...)` | Non-blocking controller integration | Observes and routes a `RuntimeCommandFrame` only when it is due | +| `runner.deactivate_rows(mask, reason=...)` | Runner-driven runtime orchestrator | Permanently removes rows and refreshes the runner's cached effect request; prefer it over direct session mutation | +| `runner.step(effect_result=...)` | Non-blocking controller integration | Observes and routes a `RuntimeCommandFrame` only when it is due | | `runner.run_until_blocked(...)` | Simple blocking application or tutorial | Advances the injected clock until terminal or external effect verification is required | | `runner.cancel(reason)` | Explicit safe stop | Requests controller cancellation followed by an observed-position hold | @@ -607,6 +609,16 @@ unknown or incompatible transport cannot cause partial dispatch. Cancellation, observation/session exceptions, and negative acknowledgements enter a best-effort cancel-then-hold path for every armed runtime target. +Pass an owned `eligible_mask` to `engine.start()` when only a subset of rows may +enter the invocation sequence. This cohort is sticky: eligibility can only +shrink across action barriers and replans. Later failures outside the atomic +runtime should call `runner.deactivate_rows(mask, reason=...)`; the operation is +idempotent, the next command neutralizes changed rows, and removing the final +eligible row fails and terminates the session. When effect verification is +pending, deactivation narrows the request and assigns a new +`verification_id`. Do not mutate `session` directly while its runner owns +scheduling, because the runner must refresh its cached effect boundary. + The engine authorizes every emitted command against the immutable target and physical claims in the resolved binding. A command cannot address an unbound destination, substitute target metadata, or overlap another endpoint's joints @@ -757,20 +769,39 @@ environment row. Pick, place, handover, and coordinated skills also return an uncommitted `StateDelta` describing the attachment state expected after execution. -At the terminal waypoint, an `ExecutionSession` requests an external -per-environment verification mask before committing a non-empty effect: +At the terminal waypoint, an `ExecutionSession` requests an external, +correlated per-environment result before committing a non-empty effect: ```python +from embodichain.lab.sim.atomic_actions import EffectVerificationResult + tick = session.tick(latest_context) if tick.pending_effect is not None: - effect_success = verify_grasp_or_release() - tick = session.tick(latest_context, effect_success=effect_success) + request = tick.pending_effect + success_mask, failure_mask = verify_grasp_or_release(request.env_mask) + effect_result = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + ) + tick = session.tick(latest_context, effect_result=effect_result) ``` This prevents a collision-free or well-tracked command plan from being misreported as a successful grasp, release, or handover. The typed `EffectVerificationRequest` persists on subsequent ticks while waiting; -`EFFECT_VERIFICATION_REQUIRED` remains a one-time observability event. +`EFFECT_VERIFICATION_REQUIRED` remains a one-time observability event. Success +and failure masks are disjoint subsets of the request mask; omitted request rows +remain unresolved. Request IDs change after mask shrinkage or whole-action +retry, so a delayed result cannot commit a newer attempt. + +`request.deadline` is expressed in the robot-observation timestamp domain. +`RecoveryPolicy.action_timeout` covers both trajectory execution and the +terminal effect wait; a retry invalidates the old request ID. With +`ExecutionRunner.step()`, a call made before the next due cycle does not consume +its `effect_result`: schedule another call using `wait_duration`, re-read the +current request, and submit a result for that current ID. Partial resolution and +row deactivation can also replace the request before the delayed result arrives. ## Action Agent integration diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 6f7de6cb2..181472035 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -332,7 +332,12 @@ must be resolved from the latest scene snapshot: ) task = TaskState.empty(robot.get_qpos().shape[0], robot.device) initial_context = adapter.observe(task) - session = engine.start((invocation,), initial_context) + initial_eligible = determine_ready_rows(initial_context) + session = engine.start( + (invocation,), + initial_context, + eligible_mask=initial_eligible, + ) router = EndpointCommandRouter((adapter,)) runner = ExecutionRunner(session, adapter, router, clock=adapter) result = runner.run_until_blocked() @@ -359,6 +364,25 @@ For an application that already owns its event loop, call the non-blocking with ``is_waiting`` set has not consumed a new observation or effect result; use its ``wait_duration`` to schedule the next call. +``eligible_mask`` is an owned initial cohort, not a one-tick filter. Eligibility +can only shrink for the lifetime of the session and remains inactive across +action barriers and replans. If an application later loses a row, deactivate it +through the runner that owns scheduling: + +.. code-block:: python + + changed = runner.deactivate_rows( + lost_tracking_mask, + reason="object tracking was lost", + ) + +The operation is idempotent and the next command actively neutralizes changed +rows. Deactivating rows while an effect is pending narrows the request and +changes its ``verification_id``. Deactivating the last eligible row fails and +terminates the session. Do not call ``session.deactivate_rows()`` directly while +an ``ExecutionRunner`` owns the session because the runner must refresh its +cached effect boundary. + The complete simulation example starts with a visible cube directly in front of the robot, then applies a short horizontal force pulse so physics and friction slide it sideways during one ``PickUp`` invocation whose @@ -451,24 +475,58 @@ Task-state effects Pick, place, handover, and coordinated skills declare attachment changes as a :class:`~embodichain.lab.sim.atomic_actions.StateDelta`. Planning does not commit -those changes. During closed-loop execution, a non-empty effect requires an -external per-environment verification mask: +those changes. During closed-loop execution, a non-empty effect requires a +correlated per-environment verification result: .. code-block:: python + from embodichain.lab.sim.atomic_actions import EffectVerificationResult + def verify_effect(context, tick): - return verify_grasp_or_release(context) + request = tick.pending_effect + assert request is not None + success_mask, failure_mask = verify_grasp_or_release(context, request.env_mask) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + ) result = runner.run_until_blocked(effect_verifier=verify_effect) This prevents a successful trajectory plan from being mistaken for a successful physical grasp or release. If verification is asynchronous, omit the callback; ``run_until_blocked`` returns at the verification boundary and the application -can later resume with ``runner.step(effect_success=verified)`` when the next -cycle is due, or call ``run_until_blocked(effect_verifier=...)`` again. The -runner remembers the pending boundary even though the session emits its event -only once. The durable state is ``tick.pending_effect`` (an -``EffectVerificationRequest``), not the presence of that one-time event. +can later resume from the *current* pending request: + +.. code-block:: python + + request = runner.session.pending_effect + assert request is not None + success_mask, failure_mask = await_effect_observation(request.env_mask) + verified = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + ) + resumed = runner.step(effect_result=verified) + if resumed.is_waiting: + schedule_after(resumed.wait_duration) + # This call did not consume ``verified``. Re-read the current request + # and submit a result for that ID again at the due cycle. + +Alternatively, call ``run_until_blocked(effect_verifier=...)`` again. Success +and failure masks must be disjoint subsets of the request mask; rows in neither +mask remain unresolved. A result must reuse the current request's +``verification_id``. Deactivation, partial resolution, or retry can replace the +request, so re-read it before delayed submission and re-verify if its ID or mask +changed. ``request.deadline`` uses the robot-observation timestamp domain; +``RecoveryPolicy.action_timeout`` covers both trajectory execution and the +terminal effect wait. A result submitted after timeout cannot satisfy the new +retry attempt because its old ID is invalid. The runner remembers the pending +boundary even though the session emits its event only once. The durable state is +``tick.pending_effect`` (an ``EffectVerificationRequest``), not the presence of +that one-time event. Adding an action ---------------- diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 607470da6..6eec6bdac 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -58,6 +58,7 @@ ) from .execution import ( EffectVerificationRequest, + EffectVerificationResult, ExecutionEvent, ExecutionEventKind, ExecutionSession, @@ -202,6 +203,7 @@ "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", + "EffectVerificationResult", "EffectVerifier", "ExecutionClock", "ExecutionFeedbackMode", diff --git a/embodichain/lab/sim/atomic_actions/effects.py b/embodichain/lab/sim/atomic_actions/effects.py index f9c1f537b..90b80bfa5 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -18,12 +18,16 @@ from __future__ import annotations -from dataclasses import dataclass, field +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass from types import MappingProxyType -from typing import Mapping +from typing import TYPE_CHECKING import torch +from embodichain.lab.sim.common import BatchEntity + from .state import ( CoordinatedHeldObjectState, HeldObjectState, @@ -33,6 +37,86 @@ _normalize_mask, ) +if TYPE_CHECKING: + from .core import ObjectSemantics + + +def _effect_snapshot_memo(value: object) -> dict[int, object]: + """Preserve live entities and private runtime caches during effect copies.""" + memo: dict[int, object] = {} + visited: set[int] = set() + + def visit(nested: object) -> None: + nested_id = id(nested) + if nested_id in visited: + return + visited.add(nested_id) + if isinstance(nested, BatchEntity): + memo[nested_id] = nested + return + if is_dataclass(nested) and not isinstance(nested, type): + for data_field in fields(nested): + child = getattr(nested, data_field.name) + if data_field.name == "_generator" and child is not None: + memo[id(child)] = None + elif not data_field.init and child is not None: + memo[id(child)] = child + else: + visit(child) + return + if isinstance(nested, Mapping): + for key, child in nested.items(): + visit(key) + visit(child) + return + if isinstance(nested, (list, tuple, set, frozenset)): + for child in nested: + visit(child) + + visit(value) + return memo + + +def _snapshot_semantics(value: ObjectSemantics) -> ObjectSemantics: + """Copy semantic data while retaining live simulation-entity identity.""" + try: + copied = deepcopy(value, _effect_snapshot_memo(value)) + except Exception as exc: + raise TypeError( + "ObjectSemantics effect metadata must be copyable without cloning " + "live simulation entities." + ) from exc + if type(copied) is not type(value) or copied is value: + raise TypeError( + "ObjectSemantics effect snapshots must produce a distinct value " + "of the same exact type." + ) + return copied + + +def _snapshot_held(value: HeldObjectState) -> HeldObjectState: + """Return an independently owned held-object effect value.""" + return HeldObjectState( + semantics=_snapshot_semantics(value.semantics), + object_to_eef=value.object_to_eef.clone(), + grasp_xpos=value.grasp_xpos.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + + +def _snapshot_coordinated( + value: CoordinatedHeldObjectState, +) -> CoordinatedHeldObjectState: + """Return an independently owned coordinated held-object effect value.""" + return CoordinatedHeldObjectState( + semantics=_snapshot_semantics(value.semantics), + left_object_to_eef=value.left_object_to_eef.clone(), + right_object_to_eef=value.right_object_to_eef.clone(), + left_grasp_xpos=value.left_grasp_xpos.clone(), + right_grasp_xpos=value.right_grasp_xpos.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + def _with_held_mask( value: HeldObjectState, @@ -217,6 +301,26 @@ def is_empty(self) -> bool: """Whether this delta declares no symbolic state changes.""" return not self.held_object_updates and not self.coordinated_held_object_updates + def snapshot(self) -> StateDelta: + """Return an independently owned symbolic-effect snapshot. + + Live simulation entities retain identity, while semantic metadata, + affordance data, and every attachment tensor are copied. + + Returns: + Independently owned state delta. + """ + return StateDelta( + held_object_updates={ + resource: None if value is None else _snapshot_held(value) + for resource, value in self.held_object_updates.items() + }, + coordinated_held_object_updates={ + resources: (None if value is None else _snapshot_coordinated(value)) + for resources, value in self.coordinated_held_object_updates.items() + }, + ) + def apply( self, state: TaskState, diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 0baab8ab7..2c7d446df 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -546,6 +546,8 @@ def start( self, invocations: Iterable[ActionInvocation], context: PlanningContext | None = None, + *, + eligible_mask: torch.Tensor | None = None, ) -> ExecutionSession: """Start closed-loop execution for a grounded invocation sequence. @@ -553,6 +555,8 @@ def start( invocations: Grounded action requests in execution order. context: Initial measured state and scene snapshot. The engine captures one when omitted. + eligible_mask: Optional rows allowed to enter this session. Inactive + rows remain inactive across every invocation in the sequence. Returns: Stateful execution session advanced by ``session.tick(...)``. @@ -560,7 +564,12 @@ def start( from .execution import ExecutionSession initial = self.initial_context() if context is None else context - return ExecutionSession(self, tuple(invocations), initial) + return ExecutionSession( + self, + tuple(invocations), + initial, + eligible_mask=eligible_mask, + ) def _validate_context(self, context: PlanningContext) -> None: """Validate an externally supplied planning context.""" diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index b425b9934..d44f24505 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -20,6 +20,7 @@ from dataclasses import dataclass from enum import Enum +import math from typing import TYPE_CHECKING import torch @@ -60,13 +61,18 @@ class ExecutionEventKind(str, Enum): TRACKING_ERROR = "tracking_error" DYNAMIC_GOAL_CHANGED = "dynamic_goal_changed" COLLISION_WORLD_CHANGED = "collision_world_changed" + ACTION_PLANNING_FAILED = "action_planning_failed" ACTION_TIMEOUT = "action_timeout" TRAJECTORY_COMPLETED = "trajectory_completed" EFFECT_VERIFICATION_REQUIRED = "effect_verification_required" + EFFECT_VERIFICATION_FAILED = "effect_verification_failed" + EFFECT_VERIFICATION_TIMEOUT = "effect_verification_timeout" ACTION_RETRY = "action_retry" ACTION_COMPLETED = "action_completed" RECOVERY_EXHAUSTED = "recovery_exhausted" + ROWS_DEACTIVATED = "rows_deactivated" SESSION_COMPLETED = "session_completed" + SESSION_FAILED = "session_failed" @dataclass(frozen=True, slots=True, eq=False) @@ -89,6 +95,8 @@ def __post_init__(self) -> None: raise ValueError("invocation_index must be non-negative.") if self.invocation_revision < 0: raise ValueError("invocation_revision must be non-negative.") + if not isinstance(self.env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") if self.env_mask.dtype != torch.bool or self.env_mask.dim() != 1: raise ValueError("ExecutionEvent.env_mask must be a 1D bool tensor.") object.__setattr__(self, "env_mask", self.env_mask.clone()) @@ -96,17 +104,27 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True, eq=False) class EffectVerificationRequest: - """Typed boundary describing a semantic effect awaiting verification.""" + """Typed boundary describing a semantic effect awaiting verification. + ``requested_at`` and ``deadline`` use the same timestamp domain as + :class:`RobotObservation`. Request-mask shrinkage retains both values; + only a whole-action retry starts a new attempt deadline. + """ + + verification_id: int skill_id: str invocation_id: str | None invocation_revision: int invocation_index: int terminal_segment: str | None + requested_at: float + deadline: float env_mask: torch.Tensor expected_effects: StateDelta def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") if not isinstance(self.skill_id, str) or not self.skill_id: raise ValueError("skill_id must be a non-empty string.") if self.invocation_id is not None and ( @@ -121,13 +139,71 @@ def __post_init__(self) -> None: not isinstance(self.terminal_segment, str) or not self.terminal_segment ): raise ValueError("terminal_segment must be a non-empty string or None.") + if not math.isfinite(self.requested_at) or self.requested_at < 0.0: + raise ValueError("requested_at must be finite and non-negative.") + if not math.isfinite(self.deadline) or self.deadline < self.requested_at: + raise ValueError( + "deadline must be finite and no earlier than requested_at." + ) + if not isinstance(self.env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") if self.env_mask.dtype != torch.bool or self.env_mask.dim() != 1: raise ValueError("env_mask must be a 1D bool tensor.") + if not self.env_mask.any(): + raise ValueError("env_mask must contain at least one requested row.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") if self.expected_effects.is_empty: raise ValueError("Effect verification requires a non-empty StateDelta.") object.__setattr__(self, "env_mask", self.env_mask.clone()) + object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + + def snapshot(self) -> EffectVerificationRequest: + """Return a request snapshot with an independently owned row mask.""" + return EffectVerificationRequest( + verification_id=self.verification_id, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + invocation_index=self.invocation_index, + terminal_segment=self.terminal_segment, + requested_at=self.requested_at, + deadline=self.deadline, + env_mask=self.env_mask, + expected_effects=self.expected_effects, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectVerificationResult: + """Correlated per-environment update for one effect boundary. + + Rows absent from both masks remain unresolved. This lets one shared batch + barrier commit verified rows while other rows continue observing the same + physical effect. + """ + + verification_id: int + success_mask: torch.Tensor + failure_mask: torch.Tensor + + def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") + for name in ("success_mask", "failure_mask"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if self.success_mask.shape != self.failure_mask.shape: + raise ValueError("success_mask and failure_mask must have equal shapes.") + if self.success_mask.device != self.failure_mask.device: + raise ValueError("success_mask and failure_mask must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("success_mask and failure_mask must not overlap.") + object.__setattr__(self, "success_mask", self.success_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) @dataclass(frozen=True, slots=True, eq=False) @@ -162,6 +238,16 @@ def __post_init__(self) -> None: raise TypeError("hold_targets must contain RuntimeEndpointTarget values.") if self.command is not None and self.hold_targets: raise ValueError("A tick cannot send commands and request a hold together.") + if self.pending_effect is not None: + if not isinstance(self.pending_effect, EffectVerificationRequest): + raise TypeError( + "pending_effect must be an EffectVerificationRequest or None." + ) + object.__setattr__( + self, + "pending_effect", + self.pending_effect.snapshot(), + ) hold_targets: list[RuntimeEndpointTarget] = [] for target in self.hold_targets: snapshot = target.snapshot() @@ -182,11 +268,14 @@ class ExecutionSession: The session never steps a simulator itself. Each :meth:`tick` consumes the latest observation and scene snapshot and emits at most one synchronized endpoint-command frame. Expected symbolic effects are committed only after the caller - supplies ``effect_success`` for a non-empty :class:`StateDelta`. + supplies a correlated :class:`EffectVerificationResult` for a non-empty + :class:`StateDelta`. Environment eligibility and recovery budgets are tracked per row. The waypoint cursor is batch-synchronized: a recoverable row replans the active cohort from the latest observation and restarts the action trajectory. + Calls that mutate the session must be serialized by its owner; the session + does not provide thread synchronization. """ def __init__( @@ -194,6 +283,8 @@ def __init__( engine: AtomicActionEngine, invocations: tuple[ActionInvocation, ...], context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, ) -> None: if not invocations: raise ValueError("ExecutionSession requires at least one invocation.") @@ -218,16 +309,34 @@ def __init__( self._last_command_mask = torch.zeros( context.batch_size, dtype=torch.bool, device=context.robot.qpos.device ) - self._eligible = torch.ones_like(self._last_command_mask) + self._eligible = ( + torch.ones_like(self._last_command_mask) + if eligible_mask is None + else self._normalize_mask(eligible_mask, "eligible_mask") + ) self._pending = self._eligible.clone() self._action_retries = torch.zeros( context.batch_size, dtype=torch.long, device=context.robot.qpos.device ) self._replans = torch.zeros_like(self._action_retries) self._pending_effect: EffectVerificationRequest | None = None - self._status = ExecutionStatus.RUNNING + self._effect_failures = torch.zeros_like(self._eligible) + self._effect_requested_at: float | None = None + self._next_effect_verification_id = 0 + self._status = ( + ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED + ) self._queued_events: list[ExecutionEvent] = [] - self._plan_current(context, ExecutionEventKind.ACTION_PLANNED) + if self._status is ExecutionStatus.RUNNING: + self._plan_current(context, ExecutionEventKind.ACTION_PLANNED) + else: + self._queued_events.append( + self._event( + ExecutionEventKind.SESSION_FAILED, + self._eligible, + "No environment was initially eligible for execution.", + ) + ) @property def status(self) -> ExecutionStatus: @@ -254,6 +363,68 @@ def effect_verification_pending(self) -> bool: """Whether the current physical effect still requires verification.""" return self._pending_effect is not None + @property + def pending_effect(self) -> EffectVerificationRequest | None: + """Owned snapshot of the current effect boundary, when present.""" + return None if self._pending_effect is None else self._pending_effect.snapshot() + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> torch.Tensor: + """Permanently remove selected rows from this invocation sequence. + + Deactivation is sticky across action barriers and recovery replans. + The next emitted command frame marks those rows inactive so the command + sink can apply target-specific safe hold behavior. + + Args: + env_mask: Rows requested for deactivation. + reason: Human-readable event message. + + Returns: + Owned mask of rows that changed from eligible to inactive. + + Raises: + RuntimeError: If the session is already terminal. + ValueError: If ``reason`` is empty or the mask shape is invalid. + """ + if self._status is not ExecutionStatus.RUNNING: + raise RuntimeError("Only a running execution session can deactivate rows.") + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + requested = self._normalize_mask(env_mask, "env_mask") + changed = requested & self._eligible + if not changed.any(): + return changed + self._eligible &= ~changed + self._pending &= ~changed + self._effect_failures &= ~changed + self._last_command_mask &= ~changed + self._queued_events.append( + self._event(ExecutionEventKind.ROWS_DEACTIVATED, changed, reason) + ) + if self._pending_effect is not None: + assert self._plan is not None + previous_effect = self._pending_effect + remaining_effect = ( + previous_effect.env_mask & self._pending & self._plan.plan_success + ) + if torch.equal(remaining_effect, previous_effect.env_mask): + self._pending_effect = previous_effect + elif remaining_effect.any(): + self._pending_effect = self._effect_verification_request( + remaining_effect + ) + else: + self._pending_effect = None + terminal_event = self._update_terminal_status() + if terminal_event is not None: + self._queued_events.append(terminal_event) + return changed.clone() + def revise_current( self, invocation: ActionInvocation, @@ -302,10 +473,10 @@ def _prepare_revision( raise TypeError("invocation must be an ActionInvocation.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - if self._pending_effect is not None: + if self._pending_effect is not None or self._effect_failures.any(): raise RuntimeError( - "Cannot revise while a physical effect is awaiting verification; " - "verify it or cancel and start a new invocation." + "Cannot revise while physical-effect resolution is pending; " + "resolve it or cancel and start a new invocation." ) self._validate_revision_identity( skill_id=invocation.skill_id, @@ -324,10 +495,10 @@ def _install_prepared_revision( raise TypeError("replacement must be a ResolvedActionRequest.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - if self._pending_effect is not None: + if self._pending_effect is not None or self._effect_failures.any(): raise RuntimeError( - "Cannot revise while a physical effect is awaiting verification; " - "verify it or cancel and start a new invocation." + "Cannot revise while physical-effect resolution is pending; " + "resolve it or cancel and start a new invocation." ) self._validate_revision_identity( skill_id=replacement.skill_id, @@ -408,32 +579,41 @@ def tick( self, context: PlanningContext, *, - effect_success: torch.Tensor | None = None, + effect_result: EffectVerificationResult | None = None, ) -> ExecutionTick: """Advance execution by one observation/command cycle. Args: context: Latest measured robot and versioned scene state. Its task state is replaced by the session's verified task state. - effect_success: Optional per-environment semantic-effect verification - for an action waiting at its terminal waypoint. + effect_result: Optional correlated semantic-effect result for an + action waiting at its terminal waypoint. Returns: Status, optional command, events, and current verified task state. """ self._context = self._validated_context(context) events = self._drain_events() + if effect_result is not None: + if type(effect_result) is not EffectVerificationResult: + raise TypeError( + "effect_result must be exactly EffectVerificationResult or None." + ) + if self._pending_effect is None: + raise ValueError("No semantic effect is awaiting verification.") + if effect_result.verification_id != self._pending_effect.verification_id: + raise ValueError( + "effect_result verification_id does not match the pending " + "effect boundary." + ) if self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) assert self._plan is not None - if self._pending_effect is not None: - execution_mask = ( - self._pending_effect.env_mask & self._pending & self._plan.plan_success - ) + if not self._pending.any(): command, hold_targets, completion_events = self._finish_action( - execution_mask, - effect_success, + self._pending, + None, ) events.extend(completion_events) return self._tick_result( @@ -442,6 +622,104 @@ def tick( events=events, ) + if self._pending_effect is not None: + execution_mask = ( + self._pending_effect.env_mask & self._pending & self._plan.plan_success + ) + if self._action_timed_out(self._plan, execution_mask): + timed_out = execution_mask.clone() + known_failures = self._effect_failures.clone() + planning_failed = self._pending & ~self._plan.plan_success + retry_mask = timed_out | known_failures | planning_failed + self._pending_effect = None + self._effect_failures.zero_() + if known_failures.any(): + events.append( + self._event( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + known_failures, + "Expected semantic effects were not observed.", + ) + ) + if planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT, + "Effect verification exceeded the action attempt timeout.", + reason_mask=timed_out, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + assert self._plan is not None + effect_result = None + else: + command, hold_targets, completion_events = self._finish_action( + execution_mask, + effect_result, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + + if self._effect_failures.any(): + failed_effect = self._effect_failures.clone() + planning_failed = self._pending & ~self._plan.plan_success + retry_mask = failed_effect | planning_failed + self._effect_failures.zero_() + if planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + "Expected semantic effects were not observed.", + reason_mask=failed_effect, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + assert self._plan is not None + plan = self._plan execution_mask = self._pending & plan.plan_success recovery_events = self._recover_if_needed(plan, execution_mask) @@ -459,6 +737,17 @@ def tick( assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) commands = plan.commands if self._waypoint_index < commands.frame_count: @@ -483,6 +772,17 @@ def tick( assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) if plan.commands.frame_count > 0: command = self._command_at(plan, 0, execution_mask) self._waypoint_index = 1 @@ -496,7 +796,7 @@ def tick( ) command, hold_targets, completion_events = self._finish_action( execution_mask, - effect_success, + effect_result, ) events.extend(completion_events) return self._tick_result( @@ -515,7 +815,7 @@ def tick( command, hold_targets, completion_events = self._finish_action( execution_mask, - effect_success, + effect_result, ) events.extend(completion_events) return self._tick_result( @@ -594,6 +894,8 @@ def _install_plan( self._last_joint_ids = () self._last_command_mask.zero_() self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None planned_mask = self._pending & plan.plan_success self._queued_events.append( self._event(event_kind, planned_mask, "Planned from the latest context.") @@ -669,10 +971,7 @@ def _recover_if_needed( events: list[ExecutionEvent] = [] if not execution_mask.any(): return events - if ( - self._context.robot.timestamp - self._action_started_at - > plan.recovery_policy.action_timeout - ): + if self._action_timed_out(plan, execution_mask): return self._attempt_action_retry( execution_mask, ExecutionEventKind.ACTION_TIMEOUT, @@ -718,6 +1017,18 @@ def _recover_if_needed( ) return events + def _action_timed_out( + self, + plan: ActionPlan, + execution_mask: torch.Tensor, + ) -> bool: + """Return whether an active action attempt exceeded its deadline.""" + return bool( + execution_mask.any() + and self._context.robot.timestamp - self._action_started_at + > plan.recovery_policy.action_timeout + ) + def _attempt_replan( self, trigger_mask: torch.Tensor, @@ -748,7 +1059,9 @@ def _attempt_replan( self._replans[allowed] += 1 self._plan_current(self._context, ExecutionEventKind.REPLANNED) events.extend(self._drain_events()) - self._update_terminal_status() + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) return events def _attempt_action_retry( @@ -756,11 +1069,16 @@ def _attempt_action_retry( trigger_mask: torch.Tensor, reason: ExecutionEventKind, message: str, + *, + reason_mask: torch.Tensor | None = None, ) -> list[ExecutionEvent]: """Retry the current action or permanently fail exhausted rows.""" assert self._plan is not None policy = self._plan.recovery_policy - events = [self._event(reason, trigger_mask, message)] + cause_mask = trigger_mask if reason_mask is None else reason_mask + events = [self._event(reason, cause_mask, message)] + self._pending_effect = None + self._effect_failures &= ~trigger_mask allowed = trigger_mask & (self._action_retries < policy.max_action_retries) exhausted = trigger_mask & ~allowed if exhausted.any(): @@ -785,13 +1103,15 @@ def _attempt_action_retry( ) self._plan_current(self._context, ExecutionEventKind.REPLANNED) events.extend(self._drain_events()) - self._update_terminal_status() + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) return events def _finish_action( self, execution_mask: torch.Tensor, - effect_success: torch.Tensor | None, + effect_result: EffectVerificationResult | None, ) -> tuple[ RuntimeCommandFrame | None, tuple[RuntimeEndpointTarget, ...], @@ -807,22 +1127,39 @@ def _finish_action( ) orphaned_targets = bool(active_targets) and not plan_targets events: list[ExecutionEvent] = [] + if not self._pending.any(): + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + return None, hold_targets, barrier_events + planning_failed = self._pending & ~self._plan.plan_success if not execution_mask.any() and planning_failed.any(): events.extend( self._attempt_action_retry( planning_failed, - ExecutionEventKind.ACTION_RETRY, + ExecutionEventKind.ACTION_PLANNING_FAILED, "Planning failed for every pending environment.", ) ) if self._status is not ExecutionStatus.RUNNING: return None, active_targets, events + if not self._pending.any(): + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + events.extend(barrier_events) + return None, hold_targets, events return None, active_targets, events + failed_effect = torch.zeros_like(execution_mask) + unresolved = torch.zeros_like(execution_mask) + made_progress = False if self._plan.expected_effects.is_empty: verified = execution_mask - elif effect_success is None: + elif effect_result is None: if self._pending_effect is None: self._pending_effect = self._effect_verification_request(execution_mask) events.append( @@ -834,9 +1171,27 @@ def _finish_action( ) return None, active_targets, events else: - verified_input = self._normalize_mask(effect_success, "effect_success") - verified = execution_mask & verified_input - self._pending_effect = None + success_input = self._normalize_mask( + effect_result.success_mask, + "effect_result.success_mask", + ) + failure_input = self._normalize_mask( + effect_result.failure_mask, + "effect_result.failure_mask", + ) + reported = success_input | failure_input + if (reported & ~execution_mask).any(): + raise ValueError( + "Effect verification masks must be subsets of the pending " + "effect request env_mask." + ) + verified = execution_mask & success_input + failed_effect = execution_mask & failure_input + unresolved = execution_mask & ~reported + made_progress = bool(reported.any().item()) + self._effect_failures |= failed_effect + if not unresolved.any(): + self._pending_effect = None if verified.any(): self._task_state = self._plan.expected_effects.apply( @@ -849,22 +1204,67 @@ def _finish_action( env_ids=self._context.env_ids, ) self._pending &= ~verified - failed_effect = execution_mask & ~verified - retry_mask = failed_effect | planning_failed + if unresolved.any(): + if made_progress: + self._pending_effect = self._effect_verification_request(unresolved) + return None, active_targets, events + retry_mask = self._effect_failures | planning_failed if retry_mask.any(): + effect_failure_mask = self._effect_failures.clone() + self._effect_failures.zero_() + reason = ( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED + if effect_failure_mask.any() + else ExecutionEventKind.ACTION_PLANNING_FAILED + ) + reason_mask = ( + effect_failure_mask if effect_failure_mask.any() else retry_mask + ) + if effect_failure_mask.any() and planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) events.extend( self._attempt_action_retry( retry_mask, - ExecutionEventKind.ACTION_RETRY, + reason, "Planning or expected-effect verification failed.", + reason_mask=reason_mask, ) ) if self._status is not ExecutionStatus.RUNNING: return None, active_targets, events - return None, active_targets, events + if self._pending.any(): + return None, active_targets, events if self._pending.any(): return None, active_targets, events + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + events.extend(barrier_events) + return None, hold_targets, events + + def _advance_action_barrier( + self, + active_targets: tuple[RuntimeEndpointTarget, ...], + *, + orphaned_targets: bool, + ) -> tuple[tuple[RuntimeEndpointTarget, ...], list[ExecutionEvent]]: + """Complete an empty action cohort and install the next invocation.""" + if self._status is not ExecutionStatus.RUNNING or self._plan is None: + raise RuntimeError("Only a running planned action can cross its barrier.") + if self._pending.any(): + raise RuntimeError("The action barrier cannot advance with pending rows.") + self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None + events: list[ExecutionEvent] = [] events.append( self._event( ExecutionEventKind.ACTION_COMPLETED, @@ -879,22 +1279,28 @@ def _finish_action( if self._eligible.any() else ExecutionStatus.FAILED ) + terminal_kind = ( + ExecutionEventKind.SESSION_COMPLETED + if self._status is ExecutionStatus.COMPLETED + else ExecutionEventKind.SESSION_FAILED + ) events.append( self._event( - ExecutionEventKind.SESSION_COMPLETED, + terminal_kind, self._eligible, "Invocation sequence completed.", ) ) - return None, (active_targets if orphaned_targets else ()), events + return (active_targets if orphaned_targets else ()), events self._pending = self._eligible.clone() self._pending_effect = None + self._effect_failures.zero_() self._action_retries.zero_() self._replans.zero_() self._plan_current(self._context, ExecutionEventKind.ACTION_PLANNED) events.extend(self._drain_events()) - return None, active_targets, events + return active_targets, events def _command_at( self, @@ -1037,6 +1443,8 @@ def _batched_entity_pose(self, state: EntityState) -> torch.Tensor: def _normalize_mask(self, value: torch.Tensor, name: str) -> torch.Tensor: """Validate and copy a per-environment boolean mask.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") if value.dtype != torch.bool or value.shape != (self._context.batch_size,): raise ValueError( f"{name} must be bool with shape ({self._context.batch_size},)." @@ -1050,7 +1458,12 @@ def _effect_verification_request( """Describe the current action's pending semantic-effect boundary.""" assert self._plan is not None request = self._requests[self._invocation_index] + verification_id = self._next_effect_verification_id + self._next_effect_verification_id += 1 + if self._effect_requested_at is None: + self._effect_requested_at = self._context.robot.timestamp return EffectVerificationRequest( + verification_id=verification_id, skill_id=request.skill_id, invocation_id=request.invocation_id, invocation_revision=request.revision, @@ -1058,6 +1471,10 @@ def _effect_verification_request( terminal_segment=( self._plan.segments[-1].name if self._plan.segments else None ), + requested_at=self._effect_requested_at, + deadline=( + self._action_started_at + self._plan.recovery_policy.action_timeout + ), env_mask=env_mask, expected_effects=self._plan.expected_effects, ) @@ -1101,10 +1518,19 @@ def _drain_events(self) -> list[ExecutionEvent]: self._queued_events = [] return events - def _update_terminal_status(self) -> None: - """Mark the session failed when no environment can continue.""" - if not self._eligible.any(): + def _update_terminal_status(self) -> ExecutionEvent | None: + """Mark and report failure when no environment can continue.""" + if not self._eligible.any() and self._status is ExecutionStatus.RUNNING: self._status = ExecutionStatus.FAILED + self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None + return self._event( + ExecutionEventKind.SESSION_FAILED, + self._eligible, + "No environment remains eligible for execution.", + ) + return None def _tick_result( self, @@ -1127,6 +1553,7 @@ def _tick_result( __all__ = [ "EffectVerificationRequest", + "EffectVerificationResult", "ExecutionEvent", "ExecutionEventKind", "ExecutionSession", diff --git a/embodichain/lab/sim/atomic_actions/policies.py b/embodichain/lab/sim/atomic_actions/policies.py index 8f82b49a6..5ef36d4c6 100644 --- a/embodichain/lab/sim/atomic_actions/policies.py +++ b/embodichain/lab/sim/atomic_actions/policies.py @@ -162,7 +162,7 @@ class RecoveryPolicy: """Dynamic-goal rotation threshold in radians (five degrees by default).""" action_timeout: float = 30.0 - """Maximum execution time for one action attempt in seconds.""" + """Maximum time for one action attempt, including terminal effect verification.""" def __post_init__(self) -> None: if self.max_replans < 0: diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 063bd1fa5..65043e374 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -19,7 +19,7 @@ from __future__ import annotations from collections.abc import Callable -from dataclasses import dataclass +from dataclasses import dataclass, replace from enum import Enum import math import time @@ -31,6 +31,7 @@ from .bindings import RuntimeEndpointTarget from .execution import ( + EffectVerificationResult, ExecutionSession, ExecutionStatus, ExecutionTick, @@ -291,7 +292,10 @@ def is_waiting(self) -> bool: ) -EffectVerifier = Callable[[PlanningContext, ExecutionTick], torch.Tensor | None] +EffectVerifier = Callable[ + [PlanningContext, ExecutionTick], + EffectVerificationResult | None, +] """Callback that verifies a pending semantic effect for each environment.""" RunnerStepCallback = Callable[[RunnerStep], None] @@ -307,6 +311,8 @@ class ExecutionRunner: :meth:`run_until_blocked` supplies the blocking loop for tutorials and simple applications. Controller rejection, timeout, observation failure, and session exceptions all trigger a best-effort cancel-then-hold sequence. + Runner methods are designed for serialized event-loop use and are not + thread-safe. Args: session: Stateful atomic-action execution session. @@ -354,8 +360,9 @@ def __init__( def session(self) -> ExecutionSession: """Execution session advanced by this runner. - Call :meth:`revise_current` on the runner, rather than mutating the - session directly, while this runner owns scheduling. + Call :meth:`revise_current` or :meth:`deactivate_rows` on the runner, + rather than mutating the session directly, while this runner owns + scheduling. """ return self._session @@ -410,16 +417,59 @@ def revise_current(self, invocation: ActionInvocation) -> None: ) self._pending_revision = prepared + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> torch.Tensor: + """Permanently deactivate environment rows owned by this runner. + + The runner refreshes its cached effect boundary so a verifier cannot + submit a result correlated with a request that deactivation replaced. + In-flight controller work is neutralized for those rows by the next + due command frame according to the :class:`CommandSink` contract. + + Args: + env_mask: Rows requested for deactivation. + reason: Human-readable event message. + + Returns: + Owned mask of rows that changed from eligible to inactive. + + Raises: + RuntimeError: If the runner is already terminal. + TypeError: If ``env_mask`` is not a tensor. + ValueError: If the mask or reason is invalid. + """ + if self._status is not RunnerStatus.RUNNING: + raise RuntimeError("Only a running execution runner can deactivate rows.") + changed = self._session.deactivate_rows(env_mask, reason=reason) + if self._session.status is not ExecutionStatus.RUNNING: + self._pending_revision = None + pending_effect = self._session.pending_effect + if pending_effect is None: + self._clear_effect_boundary() + elif self._effect_tick is not None: + self._effect_tick = replace( + self._effect_tick, + status=self._session.status, + eligible_mask=self._session.eligible_mask, + task_state=self._session.task_state, + pending_effect=pending_effect, + ) + return changed + def step( self, *, - effect_success: torch.Tensor | None = None, + effect_result: EffectVerificationResult | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. Args: - effect_success: Optional per-environment verification mask. If this - call occurs before the next cycle is due, it is not consumed and + effect_result: Optional correlated effect result. If this call + occurs before the next cycle is due, it is not consumed and must be supplied again on a later call. Returns: @@ -456,7 +506,9 @@ def step( context, ) self._pending_revision = None - tick = self._session.tick(context, effect_success=effect_success) + tick = self._session.tick(context, effect_result=effect_result) + context = self._session.latest_context + self._last_context = context except Exception as exc: return self._fail( f"Execution session failed: {type(exc).__name__}: {exc}", @@ -520,6 +572,8 @@ def step( dispatches=dispatches, ) self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time + elif tick.pending_effect is not None: + self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time else: self._next_step_at = self._clock_now() @@ -549,7 +603,7 @@ def step( self._next_step_at = self._clock_now() elif tick.status is ExecutionStatus.FAILED: return self._fail( - "Execution session exhausted its recovery budget.", + "Execution session failed; inspect its terminal events for the cause.", context=context, tick=tick, dispatches=dispatches, @@ -616,7 +670,7 @@ def run_until_blocked( """ if max_steps <= 0: raise ValueError("max_steps must be greater than zero.") - effect_success: torch.Tensor | None = None + effect_result: EffectVerificationResult | None = None now = self._clock_now() last_result = self._result( timestamp=now, @@ -624,30 +678,12 @@ def run_until_blocked( context=self._effect_context, tick=self._effect_tick, ) - if self.effect_verification_pending: - if ( - effect_verifier is None - or self._effect_context is None - or self._effect_tick is None - ): - return last_result - try: - effect_success = effect_verifier( - self._effect_context, - self._effect_tick, - ) - except Exception as exc: - return self._fail( - f"Effect verifier failed: {type(exc).__name__}: {exc}", - context=self._effect_context, - tick=self._effect_tick, - ) - if effect_success is None: - return last_result + if self.effect_verification_pending and effect_verifier is None: + return last_result for _ in range(max_steps): - result = self.step(effect_success=effect_success) + result = self.step(effect_result=effect_result) if result.tick is not None: - effect_success = None + effect_result = None if on_step is not None: try: on_step(result) @@ -668,7 +704,7 @@ def run_until_blocked( if effect_verifier is None or result.context is None: return result try: - effect_success = effect_verifier(result.context, result.tick) + effect_result = effect_verifier(result.context, result.tick) except Exception as exc: return self._fail( f"Effect verifier failed: {type(exc).__name__}: {exc}", @@ -676,7 +712,7 @@ def run_until_blocked( tick=result.tick, dispatches=list(result.dispatches), ) - if effect_success is None: + if effect_result is None: return result if result.wait_duration > 0.0: try: diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 7243013fd..141813b98 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -40,7 +40,10 @@ EndpointCommand, EntityState, ExecutionEventKind, + ExecutionSession, ExecutionStatus, + ExecutionTick, + EffectVerificationResult, GraspGoal, HeldObjectState, JointPositionPayload, @@ -150,6 +153,23 @@ def _plan( return replace(plan, plan_success=torch.zeros_like(plan.plan_success)) +class MixedEffectAction(EffectAction): + """Effect action whose final environment row always fails planning.""" + + skill_id: ClassVar[str] = "mixed_effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + plan_success = torch.ones_like(plan.plan_success) + plan_success[-1] = False + return replace(plan, plan_success=plan_success) + + class NonuniformTimingAction(DynamicAction): """Test action with explicit nonuniform waypoint arrival intervals.""" @@ -450,6 +470,44 @@ def _destination_invocation( ) +def _effect_session( + *, + batch_size: int = 1, + max_action_retries: int = 2, + action_timeout: float = 30.0, + eligible_mask: torch.Tensor | None = None, + action: EffectAction | None = None, +) -> tuple[ExecutionSession, ExecutionTick]: + """Advance a test effect action to its verification boundary.""" + engine, _ = _engine(batch_size=batch_size) + selected_action = EffectAction() if action is None else action + engine.register(selected_action) + base = _invocation( + engine, + max_action_retries=max_action_retries, + action_timeout=action_timeout, + ) + invocation = ActionInvocation( + skill_id=selected_action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + qpos = tuple(0.0 for _ in range(batch_size)) + target = tuple(0.2 for _ in range(batch_size)) + session = engine.start( + (invocation,), + _context(0.0, qpos, target, 0), + eligible_mask=eligible_mask, + ) + session.tick(_context(0.0, qpos, target, 0)) + session.tick(_context(0.1, qpos, target, 0)) + waiting = session.tick(_context(0.2, target, target, 0)) + assert waiting.pending_effect is not None + return session, waiting + + def _joint_positions(command: RuntimeCommandFrame | None) -> torch.Tensor: """Return the only joint-position payload emitted by the test action.""" assert command is not None @@ -474,6 +532,112 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +def test_initial_eligibility_is_sticky_across_invocation_barriers() -> None: + engine, _ = _engine(batch_size=2) + invocation = _invocation(engine) + supplied_mask = torch.tensor([True, False]) + session = engine.start( + (invocation, invocation), + _context(0.0, (0.0, 0.0), (0.2, 0.2), 0), + eligible_mask=supplied_mask, + ) + supplied_mask.fill_(True) + + first = session.tick(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + barrier = session.tick(_context(0.2, (0.2, 7.0), (0.2, 0.2), 0)) + second_action = session.tick(_context(0.3, (0.2, 7.0), (0.2, 0.2), 0)) + + assert first.command is not None + assert first.command.active_mask.tolist() == [True, False] + assert barrier.status is ExecutionStatus.RUNNING + assert second_action.command is not None + assert second_action.command.active_mask.tolist() == [True, False] + assert second_action.eligible_mask.tolist() == [True, False] + + +def test_empty_initial_eligibility_fails_without_planning() -> None: + engine, action = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + + session = engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([False, False]), + ) + terminal = session.tick(initial) + + assert action.plan_count == 0 + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False] + assert any( + event.kind is ExecutionEventKind.SESSION_FAILED for event in terminal.events + ) + + +def test_initial_eligibility_is_owned_and_validated() -> None: + engine, _ = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + + with pytest.raises(TypeError, match="eligible_mask must be a torch.Tensor"): + engine.start((_invocation(engine),), initial, eligible_mask=[True, False]) + with pytest.raises(ValueError, match="bool with shape"): + engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([1, 0]), + ) + with pytest.raises(ValueError, match="bool with shape"): + engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([True]), + ) + + supplied = torch.tensor([True, False]) + session = engine.start( + (_invocation(engine),), + initial, + eligible_mask=supplied, + ) + supplied.fill_(False) + observed = session.eligible_mask + observed.fill_(False) + + assert session.eligible_mask.tolist() == [True, False] + + +def test_deactivate_rows_is_sticky_and_masks_the_next_command() -> None: + engine, _ = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + session = engine.start((_invocation(engine),), initial) + session.tick(initial) + + changed = session.deactivate_rows( + torch.tensor([False, True]), + reason="environment terminated", + ) + unchanged = session.deactivate_rows( + torch.tensor([False, True]), + reason="duplicate termination", + ) + tick = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + + assert changed.tolist() == [False, True] + assert unchanged.tolist() == [False, False] + assert tick.command is not None + assert tick.command.active_mask.tolist() == [True, False] + assert tick.eligible_mask.tolist() == [True, False] + deactivated = [ + event + for event in tick.events + if event.kind is ExecutionEventKind.ROWS_DEACTIVATED + ] + assert len(deactivated) == 1 + assert deactivated[0].env_mask.tolist() == [False, True] + assert deactivated[0].message == "environment terminated" + + def test_session_commands_schedule_arrivals_and_final_settling() -> None: engine, _ = _engine() engine.register(NonuniformTimingAction()) @@ -1154,7 +1318,11 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None still_waiting = session.tick(_context(0.25, 0.2, 0.2, 0)) completed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_success=torch.tensor([True]), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), ) assert waiting.status is ExecutionStatus.RUNNING @@ -1182,6 +1350,347 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None assert completed.task_state.get_held_object("arm") is not None +def test_initially_ineligible_rows_never_receive_effects() -> None: + session, waiting = _effect_session( + batch_size=2, + eligible_mask=torch.tensor([True, False]), + ) + request = waiting.pending_effect + assert request is not None + assert request.env_mask.tolist() == [True, False] + + completed = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = completed.task_state.get_held_object("arm") + assert completed.status is ExecutionStatus.COMPLETED + assert completed.eligible_mask.tolist() == [True, False] + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + + +def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> None: + session, waiting = _effect_session(batch_size=2) + first_request = waiting.pending_effect + assert first_request is not None + + no_progress = session.tick( + _context(0.205, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([False, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert no_progress.pending_effect is not None + assert no_progress.pending_effect.verification_id == first_request.verification_id + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = partial.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True] + assert partial.pending_effect.verification_id != first_request.verification_id + assert partial.pending_effect.requested_at == first_request.requested_at + assert partial.pending_effect.deadline == first_request.deadline + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in partial.events + ) + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + current_request = partial.pending_effect + completed = session.tick( + _context(0.23, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + current_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + completed_held = completed.task_state.get_held_object("arm") + assert completed.status is ExecutionStatus.COMPLETED + assert completed_held is not None and completed_held.env_mask is not None + assert completed_held.env_mask.tolist() == [True, True] + + +def test_effect_result_masks_are_owned_disjoint_and_request_scoped() -> None: + success = torch.tensor([True, False]) + failure = torch.tensor([False, True]) + result = EffectVerificationResult(0, success, failure) + success.fill_(False) + failure.fill_(False) + assert result.success_mask.tolist() == [True, False] + assert result.failure_mask.tolist() == [False, True] + + with pytest.raises(ValueError, match="must not overlap"): + EffectVerificationResult( + 0, + torch.tensor([True, False]), + torch.tensor([True, False]), + ) + + session, waiting = _effect_session(batch_size=2) + request = waiting.pending_effect + assert request is not None + request.env_mask.fill_(False) + published_effect = request.expected_effects.held_object_updates["arm"] + assert published_effect is not None + published_effect.object_to_eef.fill_(9.0) + published_effect.grasp_xpos.fill_(8.0) + published_effect.semantics.affordance.set_custom_config("mutated", True) + preserved = session.pending_effect + assert preserved is not None + assert preserved.env_mask.tolist() == [True, True] + preserved_effect = preserved.expected_effects.held_object_updates["arm"] + assert preserved_effect is not None + assert torch.equal(preserved_effect.object_to_eef, torch.eye(4)) + assert torch.equal(preserved_effect.grasp_xpos, torch.eye(4)) + assert preserved_effect.semantics.affordance.get_custom_config("mutated") is None + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + preserved.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + current = partial.pending_effect + assert current is not None + held = partial.task_state.get_held_object("arm") + assert held is not None + assert torch.equal(held.object_to_eef[0], torch.eye(4)) + + with pytest.raises(ValueError, match="subsets"): + session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + current.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + +def test_state_delta_snapshot_owns_effect_data_and_preserves_live_entity() -> None: + entity = UncopyableEntity() + semantics = ObjectSemantics( + affordance=Affordance(custom_config={"threshold": [1.0]}), + geometry={"size": torch.ones(3)}, + properties={"mass": torch.tensor(1.0)}, + label="snapshot-object", + entity=entity, + ) + held = HeldObjectState( + semantics=semantics, + object_to_eef=torch.eye(4), + grasp_xpos=torch.eye(4), + ) + delta = StateDelta(held_object_updates={"arm": held}) + + snapshot = delta.snapshot() + copied = snapshot.held_object_updates["arm"] + assert copied is not None + assert copied is not held + assert copied.semantics is not semantics + assert copied.semantics.entity is entity + assert copied.semantics.affordance is not semantics.affordance + assert copied.object_to_eef.data_ptr() != held.object_to_eef.data_ptr() + assert copied.grasp_xpos.data_ptr() != held.grasp_xpos.data_ptr() + + copied.object_to_eef.fill_(7.0) + copied.semantics.affordance.custom_config["threshold"].append(2.0) + copied.semantics.geometry["size"].zero_() + assert torch.equal(held.object_to_eef, torch.eye(4)) + assert semantics.affordance.custom_config["threshold"] == [1.0] + assert torch.equal(semantics.geometry["size"], torch.ones(3)) + + +def test_partial_effect_failure_waits_for_unresolved_rows_then_retries_failure() -> ( + None +): + session, waiting = _effect_session(batch_size=2, max_action_retries=1) + request = waiting.pending_effect + assert request is not None + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([False, False]), + failure_mask=torch.tensor([True, False]), + ), + ) + + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True] + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in partial.events + ) + + unresolved_request = partial.pending_effect + resolved = session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + unresolved_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = resolved.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [False, True] + failed_event = next( + event + for event in resolved.events + if event.kind is ExecutionEventKind.EFFECT_VERIFICATION_FAILED + ) + retry_event = next( + event + for event in resolved.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + assert failed_event.env_mask.tolist() == [True, False] + assert retry_event.env_mask.tolist() == [True, False] + + retry_command = session.tick(_context(0.23, (0.2, 0.2), (0.2, 0.2), 0)) + assert retry_command.command is not None + assert retry_command.command.active_mask.tolist() == [True, False] + + +def test_effect_failure_exhaustion_advances_completed_rows_without_empty_request() -> ( + None +): + session, waiting = _effect_session(batch_size=2, max_action_retries=0) + request = waiting.pending_effect + assert request is not None + + terminal = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, True]), + ), + ) + + held = terminal.task_state.get_held_object("arm") + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert terminal.command is None + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert any( + event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + and event.env_mask.tolist() == [False, True] + for event in terminal.events + ) + assert any( + event.kind is ExecutionEventKind.SESSION_COMPLETED for event in terminal.events + ) + + +def test_deactivating_last_unresolved_effect_row_advances_barrier() -> None: + session, waiting = _effect_session(batch_size=2) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert partial.pending_effect is not None + + session.deactivate_rows( + torch.tensor([False, True]), + reason="effect observation terminated", + ) + terminal = session.tick(_context(0.22, (0.2, 0.2), (0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert not any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in terminal.events + ) + + +def test_deactivating_all_effect_rows_is_terminal_and_clears_request() -> None: + session, _ = _effect_session(batch_size=2) + + changed = session.deactivate_rows( + torch.tensor([True, True]), + reason="all environments terminated", + ) + terminal = session.tick(_context(0.21, (0.2, 0.2), (0.2, 0.2), 0)) + + assert changed.tolist() == [True, True] + assert terminal.status is ExecutionStatus.FAILED + assert terminal.pending_effect is None + assert any( + event.kind is ExecutionEventKind.SESSION_FAILED for event in terminal.events + ) + assert not any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in terminal.events + ) + + +def test_effect_request_deadline_is_stable_and_accepts_result_at_boundary() -> None: + session, waiting = _effect_session(action_timeout=0.25) + request = waiting.pending_effect + assert request is not None + assert request.requested_at == pytest.approx(0.2) + assert request.deadline == pytest.approx(0.25) + + polled = session.tick(_context(0.24, 0.2, 0.2, 0)) + assert polled.pending_effect is not None + assert polled.pending_effect.verification_id == request.verification_id + assert polled.pending_effect.requested_at == request.requested_at + assert polled.pending_effect.deadline == request.deadline + + completed = session.tick( + _context(0.25, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + assert completed.status is ExecutionStatus.COMPLETED + + def test_session_revision_cannot_abandon_pending_effect_verification() -> None: engine, _ = _engine() engine.register(EffectAction()) @@ -1199,13 +1708,17 @@ def test_session_revision_cannot_abandon_pending_effect_verification() -> None: waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) assert waiting.pending_effect is not None - with pytest.raises(RuntimeError, match="awaiting verification"): + with pytest.raises(RuntimeError, match="physical-effect resolution"): session.revise_current(replace(invocation, revision=1)) assert session.effect_verification_pending is True completed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_success=torch.tensor([True]), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), ) assert completed.status is ExecutionStatus.COMPLETED assert completed.task_state.get_held_object("arm") is not None @@ -1226,9 +1739,15 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: session.tick(_context(0.0, 0.0, 0.2, 0)) session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None failed = session.tick( - _context(0.2, 0.2, 0.2, 0), - effect_success=torch.tensor([False]), + _context(0.3, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([False]), + torch.tensor([True]), + ), ) assert failed.status is ExecutionStatus.FAILED @@ -1238,6 +1757,199 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: ) +def test_pending_effect_timeout_exhausts_without_committing_late_result() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation( + engine, + max_action_retries=0, + action_timeout=0.25, + ) + invocation = ActionInvocation( + skill_id="effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None + + timed_out = session.tick( + _context(0.3, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), + ) + + kinds = {event.kind for event in timed_out.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.RECOVERY_EXHAUSTED in kinds + assert timed_out.status is ExecutionStatus.FAILED + assert timed_out.pending_effect is None + assert timed_out.task_state.get_held_object("arm") is None + + +def test_effect_timeout_exhaustion_advances_rows_already_verified() -> None: + session, waiting = _effect_session( + batch_size=2, + max_action_retries=0, + action_timeout=0.25, + ) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert partial.pending_effect is not None + + terminal = session.tick(_context(0.3, (0.2, 0.2), (0.2, 0.2), 0)) + + held = terminal.task_state.get_held_object("arm") + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + timeout_event = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT + ) + assert timeout_event.env_mask.tolist() == [False, True] + + +def test_effect_timeout_charges_concurrent_planning_failures() -> None: + session, _ = _effect_session( + batch_size=2, + max_action_retries=1, + action_timeout=0.25, + action=MixedEffectAction(), + ) + + first_retry = session.tick(_context(0.3, (0.2, 0.2), (0.2, 0.2), 0)) + retry_event = next( + event + for event in first_retry.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + planning_event = next( + event + for event in first_retry.events + if event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + ) + assert retry_event.env_mask.tolist() == [True, True] + assert planning_event.env_mask.tolist() == [False, True] + + session.tick(_context(0.4, (0.2, 0.2), (0.2, 0.2), 0)) + second_wait = session.tick(_context(0.5, (0.2, 0.2), (0.2, 0.2), 0)) + assert second_wait.pending_effect is not None + terminal = session.tick(_context(0.6, (0.2, 0.2), (0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False] + exhausted = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert exhausted.env_mask.tolist() == [True, True] + + +def test_deferred_effect_failure_charges_concurrent_planning_failures() -> None: + session, waiting = _effect_session( + batch_size=3, + max_action_retries=0, + action=MixedEffectAction(), + ) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2, 0.2), (0.2, 0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([False, False, False]), + failure_mask=torch.tensor([True, False, False]), + ), + ) + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True, False] + + session.deactivate_rows( + torch.tensor([False, True, False]), + reason="unresolved effect row terminated", + ) + terminal = session.tick(_context(0.22, (0.2, 0.2, 0.2), (0.2, 0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False, False] + planning_event = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + ) + exhausted = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert planning_event.env_mask.tolist() == [False, False, True] + assert exhausted.env_mask.tolist() == [True, False, True] + + +def test_effect_retry_invalidates_previous_verification_id() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation( + engine, + max_action_retries=1, + action_timeout=0.25, + ) + invocation = ActionInvocation( + skill_id="effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + first_wait = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert first_wait.pending_effect is not None + old_id = first_wait.pending_effect.verification_id + old_deadline = first_wait.pending_effect.deadline + + retry = session.tick(_context(0.3, 0.2, 0.2, 0)) + assert retry.command is not None + assert any(event.kind is ExecutionEventKind.ACTION_RETRY for event in retry.events) + session.tick(_context(0.4, 0.2, 0.2, 0)) + second_wait = session.tick(_context(0.5, 0.2, 0.2, 0)) + assert second_wait.pending_effect is not None + assert second_wait.pending_effect.verification_id != old_id + assert second_wait.pending_effect.deadline > old_deadline + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.55, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + old_id, + torch.tensor([True]), + torch.tensor([False]), + ), + ) + + def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: engine, _ = _engine() engine.register(FailedEffectAction()) diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index 7fe66b73c..ed9432387 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -37,9 +37,11 @@ CommandAckStatus, CommandOperation, EndEffectorPoseGoal, + EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, + ExecutionTick, HeldObjectState, JOINT_POSITION_CAPABILITY, JointPositionPayload, @@ -265,6 +267,8 @@ def _make_runner( with_effect: bool = False, batch_size: int = BATCH_SIZE, control_joint_ids: tuple[int, ...] | None = None, + max_action_retries: int = 2, + action_timeout: float = 10.0, ) -> tuple[ ExecutionRunner, FakeClock, @@ -301,8 +305,9 @@ def _make_runner( motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), recovery_policy=RecoveryPolicy( max_replans=2, + max_action_retries=max_action_retries, tracking_error_threshold=0.05, - action_timeout=10.0, + action_timeout=action_timeout, ), ) session = engine.start((invocation,), initial_context) @@ -316,6 +321,28 @@ def _make_runner( return runner, clock, provider, sink, action +def _successful_effect_result( + context: PlanningContext, + tick: ExecutionTick, +) -> EffectVerificationResult: + """Correlate a successful result with the pending effect boundary.""" + pending_effect = tick.pending_effect + assert pending_effect is not None + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + failure_mask=torch.zeros( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + ) + + def test_joint_feedback_ignores_motion_outside_bound_endpoint() -> None: runner, clock, provider, sink, action = _make_runner(control_joint_ids=(0,)) @@ -554,15 +581,12 @@ def test_runner_revision_rejects_pending_effect_verification() -> None: revision=1, ) - with pytest.raises(RuntimeError, match="awaiting verification"): + with pytest.raises(RuntimeError, match="physical-effect resolution"): runner.revise_current(revised) assert runner.effect_verification_pending is True completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, - dtype=torch.bool, - ) + effect_verifier=_successful_effect_result, ) assert completed.status is RunnerStatus.COMPLETED @@ -620,9 +644,7 @@ def test_blocking_runner_verifies_effect_before_committing_task_state() -> None: runner, _, _, _, _ = _make_runner(with_effect=True) completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, dtype=torch.bool - ) + effect_verifier=_successful_effect_result, ) assert completed.status is RunnerStatus.COMPLETED @@ -641,12 +663,182 @@ def test_blocking_runner_resumes_a_stored_effect_verification_boundary() -> None assert runner.effect_verification_pending is True completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, dtype=torch.bool - ) + effect_verifier=_successful_effect_result, ) assert runner.effect_verification_pending is False assert completed.status is RunnerStatus.COMPLETED assert completed.tick is not None assert completed.tick.task_state.get_held_object("arm") is not None + + +def test_resumed_effect_verifier_uses_a_fresh_observation() -> None: + runner, clock, _, _, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.context is not None + blocked_at = blocked.context.robot.timestamp + clock.advance(0.5) + resumed_at = clock.now() + observed_at: list[float] = [] + + def record_fresh_context( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, tick) + + completed = runner.run_until_blocked(effect_verifier=record_fresh_context) + + assert completed.status is RunnerStatus.COMPLETED + assert observed_at and observed_at[0] >= resumed_at + assert observed_at[0] > blocked_at + + +def test_partial_effect_verifier_receives_the_committed_task_state() -> None: + runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) + observations: list[list[bool] | None] = [] + + def verify_in_two_updates( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + pending_effect = tick.pending_effect + assert pending_effect is not None + held = context.task.get_held_object("arm") + observations.append( + None if held is None or held.env_mask is None else held.env_mask.tolist() + ) + if pending_effect.env_mask.tolist() == [True, True]: + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ) + assert pending_effect.env_mask.tolist() == [False, True] + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert torch.equal(context.task.held_objects["arm"].env_mask, held.env_mask) + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ) + + completed = runner.run_until_blocked(effect_verifier=verify_in_two_updates) + + assert completed.status is RunnerStatus.COMPLETED + assert observations == [None, [True, False]] + assert completed.context is not None and completed.tick is not None + assert completed.context.task is completed.tick.task_state + + +def test_runner_effect_timeout_replans_and_invalidates_cached_request() -> None: + runner, clock, _, _, action = _make_runner( + with_effect=True, + max_action_retries=1, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + plan_count = action.plan_count + clock.advance(request.deadline - clock.now() + 0.01) + + retry = runner.step() + + assert retry.status is RunnerStatus.RUNNING + assert retry.tick is not None and retry.tick.command is not None + assert runner.effect_verification_pending is False + assert action.plan_count == plan_count + 1 + kinds = {event.kind for event in retry.tick.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + assert ExecutionEventKind.REPLANNED in kinds + + +def test_runner_effect_timeout_exhaustion_cancels_and_holds() -> None: + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + max_action_retries=0, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + clock.advance(request.deadline - clock.now() + 0.01) + + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert runner.effect_verification_pending is False + assert failed.tick is not None and failed.tick.pending_effect is None + assert failed.tick.task_state.get_held_object("arm") is None + assert [item.operation for item in failed.dispatches[-2:]] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + + +def test_runner_deactivation_refreshes_cached_effect_request() -> None: + runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + old_id = blocked.tick.pending_effect.verification_id + + changed = runner.deactivate_rows( + torch.tensor([False, True]), + reason="environment terminated", + ) + refreshed = runner.run_until_blocked() + + assert changed.tolist() == [False, True] + assert refreshed.tick is not None and refreshed.tick.pending_effect is not None + assert refreshed.tick.pending_effect.env_mask.tolist() == [True, False] + assert refreshed.tick.pending_effect.verification_id != old_id + + def verify_remaining( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + pending_effect = tick.pending_effect + assert pending_effect is not None + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ) + + completed = runner.run_until_blocked(effect_verifier=verify_remaining) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None + assert completed.tick.eligible_mask.tolist() == [True, False] + + +def test_blocking_runner_fails_safely_for_a_mismatched_effect_result() -> None: + runner, _, _, sink, _ = _make_runner(with_effect=True) + + def mismatched_effect_result( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + pending_effect = tick.pending_effect + assert pending_effect is not None + return EffectVerificationResult( + verification_id=pending_effect.verification_id + 1, + success_mask=torch.ones(context.batch_size, dtype=torch.bool), + failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), + ) + + failed = runner.run_until_blocked(effect_verifier=mismatched_effect_result) + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches[-2:]] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "verification_id does not match" in failed.message From 1317f99a8db208b925c5a4fd0d371a2c495d8200 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 03:50:13 +0800 Subject: [PATCH 15/28] refactor(atomic-actions): verify effects on due observations --- .../topics/atomic-actions/atomic-actions.md | 16 +- .../overview/sim/atomic_actions/index.md | 2 +- docs/source/tutorial/atomic_actions.rst | 9 +- .../lab/sim/atomic_actions/execution.py | 11 +- embodichain/lab/sim/atomic_actions/runner.py | 66 +++--- .../atomic_action/moving_target_recovery.py | 14 +- .../sim/atomic_actions/test_engine_per_env.py | 27 +++ tests/sim/atomic_actions/test_runner.py | 199 ++++++++++++++++-- 8 files changed, 285 insertions(+), 59 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index de796f9fb..dfcd1dbb1 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -420,14 +420,24 @@ correlated `EffectVerificationResult`. Its disjoint `success_mask` and neither mask remain unresolved. Partial successes commit immediately while unresolved rows keep the barrier pending. `EffectVerificationRequest` carries a monotonic `verification_id`, stable `requested_at`/`deadline` values in the -robot-observation timestamp domain, and an owned effect snapshot. Mask shrinkage -creates a new ID without extending the deadline; whole-action retry creates a -new attempt. Results for an old ID are rejected. `RecoveryPolicy.action_timeout` +robot-observation timestamp domain, a session-local `attempt_generation`, and +an owned effect snapshot. Mask shrinkage creates a new ID without extending the +deadline or changing the generation; installing a replacement plan increments +the generation. Results for an old ID are rejected. `RecoveryPolicy.action_timeout` covers the trajectory and terminal effect wait together, and only timestamps strictly greater than the deadline time out. While verification is outstanding, `ExecutionTick.pending_effect` retains the request on every tick; `EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. +For synchronous verification, pass `effect_verifier(context, request)` to +`runner.step()` or `run_until_blocked()`. The runner calls it after the fresh +due-cycle observation and supplies its result to `session.tick()` in that same +cycle. It does not call the verifier when the observation timestamp is already +past the request deadline. A verifier must return an exact +`EffectVerificationResult`; all-false masks mean unresolved. External +asynchronous integrations instead pass `effect_result` explicitly on a due +`step()` call. + ```python request = tick.pending_effect effect_result = EffectVerificationResult( diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 56afd9a57..e033a7937 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -431,7 +431,7 @@ an older custom action by renaming its implementation to `_plan()`. | `session.revise_current(invocation)` | Manually ticked runtime orchestrator | Replaces the active logical call with a newer same-destination revision and replans from the latest observed context | | `runner.revise_current(invocation)` | Runner-driven runtime orchestrator or Action Agent | Snapshots a revision, preserves the current frame deadline, then replans from a fresh due-time observation | | `runner.deactivate_rows(mask, reason=...)` | Runner-driven runtime orchestrator | Permanently removes rows and refreshes the runner's cached effect request; prefer it over direct session mutation | -| `runner.step(effect_result=...)` | Non-blocking controller integration | Observes and routes a `RuntimeCommandFrame` only when it is due | +| `runner.step(effect_result=..., effect_verifier=...)` | Non-blocking controller integration | Observes only when due; accepts either an asynchronous correlated result or a synchronous verifier, never both | | `runner.run_until_blocked(...)` | Simple blocking application or tutorial | Advances the injected clock until terminal or external effect verification is required | | `runner.cancel(reason)` | Explicit safe stop | Requests controller cancellation followed by an observed-position hold | diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 181472035..b451fe9ce 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -482,9 +482,7 @@ correlated per-environment verification result: from embodichain.lab.sim.atomic_actions import EffectVerificationResult - def verify_effect(context, tick): - request = tick.pending_effect - assert request is not None + def verify_effect(context, request): success_mask, failure_mask = verify_grasp_or_release(context, request.env_mask) return EffectVerificationResult( verification_id=request.verification_id, @@ -495,7 +493,10 @@ correlated per-environment verification result: result = runner.run_until_blocked(effect_verifier=verify_effect) This prevents a successful trajectory plan from being mistaken for a successful -physical grasp or release. If verification is asynchronous, omit the callback; +physical grasp or release. The runner invokes this synchronous callback after a +fresh due-cycle observation and feeds its result to the session in that same +cycle. Returning all-false masks keeps the remaining rows unresolved. If +verification is asynchronous, omit the callback; ``run_until_blocked`` returns at the verification boundary and the application can later resume from the *current* pending request: diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index d44f24505..ba21c669c 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -108,7 +108,9 @@ class EffectVerificationRequest: ``requested_at`` and ``deadline`` use the same timestamp domain as :class:`RobotObservation`. Request-mask shrinkage retains both values; - only a whole-action retry starts a new attempt deadline. + only a newly installed plan starts a new attempt deadline. + ``attempt_generation`` is session-local and remains stable when partial + resolution or row deactivation replaces only the request ID. """ verification_id: int @@ -116,6 +118,7 @@ class EffectVerificationRequest: invocation_id: str | None invocation_revision: int invocation_index: int + attempt_generation: int terminal_segment: str | None requested_at: float deadline: float @@ -135,6 +138,8 @@ def __post_init__(self) -> None: raise ValueError("invocation_revision must be non-negative.") if self.invocation_index < 0: raise ValueError("invocation_index must be non-negative.") + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be a non-negative integer.") if self.terminal_segment is not None and ( not isinstance(self.terminal_segment, str) or not self.terminal_segment ): @@ -166,6 +171,7 @@ def snapshot(self) -> EffectVerificationRequest: invocation_id=self.invocation_id, invocation_revision=self.invocation_revision, invocation_index=self.invocation_index, + attempt_generation=self.attempt_generation, terminal_segment=self.terminal_segment, requested_at=self.requested_at, deadline=self.deadline, @@ -304,6 +310,7 @@ def __init__( ] = {} self._planned_scene = context.scene self._action_started_at = context.robot.timestamp + self._attempt_generation = -1 self._last_joint_command: torch.Tensor | None = None self._last_joint_ids: tuple[int, ...] = () self._last_command_mask = torch.zeros( @@ -887,6 +894,7 @@ def _install_plan( ): self._active_targets = replacement_targets self._plan = plan + self._attempt_generation += 1 self._waypoint_index = 0 self._planned_scene = context.scene self._action_started_at = context.robot.timestamp @@ -1468,6 +1476,7 @@ def _effect_verification_request( invocation_id=request.invocation_id, invocation_revision=request.revision, invocation_index=self._invocation_index, + attempt_generation=self._attempt_generation, terminal_segment=( self._plan.segments[-1].name if self._plan.segments else None ), diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 65043e374..8dac661a6 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -31,6 +31,7 @@ from .bindings import RuntimeEndpointTarget from .execution import ( + EffectVerificationRequest, EffectVerificationResult, ExecutionSession, ExecutionStatus, @@ -293,10 +294,10 @@ def is_waiting(self) -> bool: EffectVerifier = Callable[ - [PlanningContext, ExecutionTick], - EffectVerificationResult | None, + [PlanningContext, EffectVerificationRequest], + EffectVerificationResult, ] -"""Callback that verifies a pending semantic effect for each environment.""" +"""Synchronous verifier called on a fresh due-cycle observation.""" RunnerStepCallback = Callable[[RunnerStep], None] """Optional observer called after every blocking runner-loop iteration.""" @@ -464,6 +465,7 @@ def step( self, *, effect_result: EffectVerificationResult | None = None, + effect_verifier: EffectVerifier | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. @@ -471,11 +473,22 @@ def step( effect_result: Optional correlated effect result. If this call occurs before the next cycle is due, it is not consumed and must be supplied again on a later call. + effect_verifier: Optional synchronous verifier for the current + pending request. It runs after a fresh due-cycle observation + and before the session consumes the result. It is not called + after the request deadline. Mutually exclusive with + ``effect_result``. Returns: Runner status, optional session tick, controller acknowledgements, and time remaining before another update is due. """ + if effect_result is not None and effect_verifier is not None: + raise ValueError( + "effect_result and effect_verifier are mutually exclusive." + ) + if effect_verifier is not None and not callable(effect_verifier): + raise TypeError("effect_verifier must be callable or None.") now = self._clock_now() if self._status is not RunnerStatus.RUNNING: return self._result(timestamp=now) @@ -499,6 +512,25 @@ def step( ) self._last_context = context + pending_effect = self._session.pending_effect + if ( + effect_verifier is not None + and pending_effect is not None + and context.robot.timestamp <= pending_effect.deadline + ): + try: + effect_result = effect_verifier(context, pending_effect) + if type(effect_result) is not EffectVerificationResult: + raise TypeError( + "EffectVerifier must return exactly " + "EffectVerificationResult." + ) + except Exception as exc: + return self._fail( + f"Effect verifier failed: {type(exc).__name__}: {exc}", + context=context, + ) + try: if self._pending_revision is not None: self._session._install_prepared_revision( @@ -659,9 +691,10 @@ def run_until_blocked( """Run with clock-driven waiting until terminal or effect verification blocks. Args: - effect_verifier: Optional callback used after an - ``effect_verification_required`` event. Without one, the method - returns the running step so the caller can verify externally. + effect_verifier: Optional synchronous callback used on fresh + due-cycle observations while effect verification is pending. + Without one, the method returns the running boundary so the + caller can verify externally. on_step: Optional callback for tracing or tutorial visualization. max_steps: Hard bound on loop iterations. @@ -670,7 +703,6 @@ def run_until_blocked( """ if max_steps <= 0: raise ValueError("max_steps must be greater than zero.") - effect_result: EffectVerificationResult | None = None now = self._clock_now() last_result = self._result( timestamp=now, @@ -681,9 +713,7 @@ def run_until_blocked( if self.effect_verification_pending and effect_verifier is None: return last_result for _ in range(max_steps): - result = self.step(effect_result=effect_result) - if result.tick is not None: - effect_result = None + result = self.step(effect_verifier=effect_verifier) if on_step is not None: try: on_step(result) @@ -700,20 +730,8 @@ def run_until_blocked( verification_required = ( result.tick is not None and result.tick.pending_effect is not None ) - if verification_required: - if effect_verifier is None or result.context is None: - return result - try: - effect_result = effect_verifier(result.context, result.tick) - except Exception as exc: - return self._fail( - f"Effect verifier failed: {type(exc).__name__}: {exc}", - context=result.context, - tick=result.tick, - dispatches=list(result.dispatches), - ) - if effect_result is None: - return result + if verification_required and effect_verifier is None: + return result if result.wait_duration > 0.0: try: self._clock.sleep(result.wait_duration) diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index db9da7b13..4c76f5968 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -36,10 +36,11 @@ AtomicActionEngine, ControlPartCommandProfile, EntityState, + EffectVerificationRequest, + EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, - ExecutionTick, GraspGoal, MotionPolicy, ObjectSemantics, @@ -406,8 +407,8 @@ def on_step(step: RunnerStep) -> None: def verify_pickup_effect( _context: PlanningContext, - _: ExecutionTick, - ) -> torch.Tensor: + request: EffectVerificationRequest, + ) -> EffectVerificationResult: """Verify that the cube rose with, and remains near, the end effector.""" cube_position = target.get_local_pose(to_matrix=True)[:, :3, 3] eef_position = robot.compute_fk( @@ -426,7 +427,12 @@ def verify_pickup_effect( f"cube-to-EEF={held_distance.detach().cpu().tolist()} m, " f"success={success.detach().cpu().tolist()}." ) - return success + verified_success = request.env_mask & success + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=verified_success, + failure_mask=request.env_mask & ~success, + ) recording_started = start_auto_play_recording( sim, diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 141813b98..eb2dcf6cb 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -1406,6 +1406,7 @@ def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> N assert partial.pending_effect is not None assert partial.pending_effect.env_mask.tolist() == [False, True] assert partial.pending_effect.verification_id != first_request.verification_id + assert partial.pending_effect.attempt_generation == first_request.attempt_generation assert partial.pending_effect.requested_at == first_request.requested_at assert partial.pending_effect.deadline == first_request.deadline assert not any( @@ -1929,6 +1930,7 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: assert first_wait.pending_effect is not None old_id = first_wait.pending_effect.verification_id old_deadline = first_wait.pending_effect.deadline + old_generation = first_wait.pending_effect.attempt_generation retry = session.tick(_context(0.3, 0.2, 0.2, 0)) assert retry.command is not None @@ -1937,6 +1939,7 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: second_wait = session.tick(_context(0.5, 0.2, 0.2, 0)) assert second_wait.pending_effect is not None assert second_wait.pending_effect.verification_id != old_id + assert second_wait.pending_effect.attempt_generation == old_generation + 1 assert second_wait.pending_effect.deadline > old_deadline with pytest.raises(ValueError, match="verification_id"): @@ -1950,6 +1953,30 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: ) +def test_effect_request_generation_advances_after_tracking_replan() -> None: + engine, _ = _engine() + effect = EffectAction() + engine.register(effect) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=effect.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + + replanned = session.tick(_context(0.1, 1.0, 0.2, 0)) + session.tick(_context(0.2, 1.0, 0.2, 0)) + waiting = session.tick(_context(0.3, 0.2, 0.2, 0)) + + assert any(event.kind is ExecutionEventKind.REPLANNED for event in replanned.events) + assert waiting.pending_effect is not None + assert waiting.pending_effect.attempt_generation == 1 + + def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: engine, _ = _engine() engine.register(FailedEffectAction()) diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index ed9432387..58dda7380 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -37,11 +37,11 @@ CommandAckStatus, CommandOperation, EndEffectorPoseGoal, + EffectVerificationRequest, EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, - ExecutionTick, HeldObjectState, JOINT_POSITION_CAPABILITY, JointPositionPayload, @@ -323,13 +323,11 @@ def _make_runner( def _successful_effect_result( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: """Correlate a successful result with the pending effect boundary.""" - pending_effect = tick.pending_effect - assert pending_effect is not None return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.ones( context.batch_size, dtype=torch.bool, @@ -683,10 +681,10 @@ def test_resumed_effect_verifier_uses_a_fresh_observation() -> None: def record_fresh_context( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: observed_at.append(context.robot.timestamp) - return _successful_effect_result(context, tick) + return _successful_effect_result(context, request) completed = runner.run_until_blocked(effect_verifier=record_fresh_context) @@ -695,32 +693,191 @@ def record_fresh_context( assert observed_at[0] > blocked_at +def test_due_effect_verifier_consumes_fresh_observation_in_the_same_step() -> None: + runner, clock, _, _, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.context is not None + assert blocked.tick is not None and blocked.tick.pending_effect is not None + blocked_at = blocked.context.robot.timestamp + clock.advance(MINIMUM_CYCLE_TIME) + observed_at: list[float] = [] + + def verify_fresh_observation( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, request) + + completed = runner.step(effect_verifier=verify_fresh_observation) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None and completed.tick.pending_effect is None + assert completed.tick.task_state.get_held_object("arm") is not None + assert completed.context is not None + assert observed_at == [completed.context.robot.timestamp] + assert observed_at[0] > blocked_at + + +def test_effect_verifier_runs_and_succeeds_at_the_request_deadline() -> None: + runner, clock, _, _, _ = _make_runner( + with_effect=True, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + clock.advance(request.deadline - clock.now()) + observed_at: list[float] = [] + + def verify_at_deadline( + context: PlanningContext, + current_request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, current_request) + + completed = runner.step(effect_verifier=verify_at_deadline) + + assert completed.status is RunnerStatus.COMPLETED + assert observed_at == pytest.approx([request.deadline]) + + +def test_effect_verifier_is_not_called_after_deadline_and_session_retries() -> None: + runner, clock, _, _, action = _make_runner( + with_effect=True, + max_action_retries=1, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + plan_count = action.plan_count + clock.advance(request.deadline - clock.now() + MINIMUM_CYCLE_TIME) + verifier = Mock() + + retry = runner.step(effect_verifier=verifier) + + verifier.assert_not_called() + assert retry.status is RunnerStatus.RUNNING + assert retry.tick is not None and retry.tick.command is not None + assert action.plan_count == plan_count + 1 + assert { + ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT, + ExecutionEventKind.ACTION_RETRY, + ExecutionEventKind.REPLANNED, + }.issubset({event.kind for event in retry.tick.events}) + + +def test_effect_result_and_effect_verifier_are_mutually_exclusive() -> None: + runner, _, _, sink, action = _make_runner(with_effect=True) + result = EffectVerificationResult( + verification_id=0, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ) + + with pytest.raises(ValueError, match="mutually exclusive"): + runner.step( + effect_result=result, + effect_verifier=_successful_effect_result, + ) + + assert action.plan_count == 1 + assert sink.sent == [] + + +@pytest.mark.parametrize( + "invalid_result", + [None, True], + ids=["none", "wrong-type"], +) +def test_effect_verifier_invalid_result_fails_with_cancel_then_hold( + invalid_result: object | None, +) -> None: + runner, clock, _, sink, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + clock.advance(MINIMUM_CYCLE_TIME) + + def invalid_verifier( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> object | None: + del context, request + return invalid_result + + failed = runner.step(effect_verifier=invalid_verifier) + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "must return exactly EffectVerificationResult" in failed.message + + +def test_all_false_effect_updates_keep_polling_the_same_request() -> None: + runner, clock, _, sink, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + initial_request = blocked.tick.pending_effect + observed_requests: list[tuple[int, int]] = [] + + def report_no_progress( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_requests.append((request.verification_id, request.attempt_generation)) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=torch.zeros(context.batch_size, dtype=torch.bool), + failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), + ) + + clock.advance(MINIMUM_CYCLE_TIME) + first_poll = runner.step(effect_verifier=report_no_progress) + clock.advance(MINIMUM_CYCLE_TIME) + second_poll = runner.step(effect_verifier=report_no_progress) + + assert first_poll.status is RunnerStatus.RUNNING + assert second_poll.status is RunnerStatus.RUNNING + assert first_poll.tick is not None and first_poll.tick.pending_effect is not None + assert second_poll.tick is not None and second_poll.tick.pending_effect is not None + assert observed_requests == [ + (initial_request.verification_id, initial_request.attempt_generation), + (initial_request.verification_id, initial_request.attempt_generation), + ] + assert sink.cancel_count == 0 + assert second_poll.tick.task_state.get_held_object("arm") is None + + def test_partial_effect_verifier_receives_the_committed_task_state() -> None: runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) observations: list[list[bool] | None] = [] def verify_in_two_updates( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: - pending_effect = tick.pending_effect - assert pending_effect is not None held = context.task.get_held_object("arm") observations.append( None if held is None or held.env_mask is None else held.env_mask.tolist() ) - if pending_effect.env_mask.tolist() == [True, True]: + if request.env_mask.tolist() == [True, True]: return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), ) - assert pending_effect.env_mask.tolist() == [False, True] + assert request.env_mask.tolist() == [False, True] assert held is not None and held.env_mask is not None assert held.env_mask.tolist() == [True, False] assert torch.equal(context.task.held_objects["arm"].env_mask, held.env_mask) return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.tensor([False, True]), failure_mask=torch.tensor([False, False]), ) @@ -786,6 +943,7 @@ def test_runner_deactivation_refreshes_cached_effect_request() -> None: blocked = runner.run_until_blocked() assert blocked.tick is not None and blocked.tick.pending_effect is not None old_id = blocked.tick.pending_effect.verification_id + old_generation = blocked.tick.pending_effect.attempt_generation changed = runner.deactivate_rows( torch.tensor([False, True]), @@ -797,15 +955,14 @@ def test_runner_deactivation_refreshes_cached_effect_request() -> None: assert refreshed.tick is not None and refreshed.tick.pending_effect is not None assert refreshed.tick.pending_effect.env_mask.tolist() == [True, False] assert refreshed.tick.pending_effect.verification_id != old_id + assert refreshed.tick.pending_effect.attempt_generation == old_generation def verify_remaining( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: - pending_effect = tick.pending_effect - assert pending_effect is not None return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), ) @@ -822,12 +979,10 @@ def test_blocking_runner_fails_safely_for_a_mismatched_effect_result() -> None: def mismatched_effect_result( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: - pending_effect = tick.pending_effect - assert pending_effect is not None return EffectVerificationResult( - verification_id=pending_effect.verification_id + 1, + verification_id=request.verification_id + 1, success_mask=torch.ones(context.batch_size, dtype=torch.bool), failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), ) From 58bce6d987d7a975286336f8955fd70677cc56d2 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:17:03 +0800 Subject: [PATCH 16/28] feat(atomic-actions): complete verified action runtime --- .../lab/sim/atomic_actions/__init__.py | 27 +- .../lab/sim/atomic_actions/affordance.py | 303 ++++++++ .../lab/sim/atomic_actions/bindings.py | 13 + embodichain/lab/sim/atomic_actions/core.py | 46 ++ embodichain/lab/sim/atomic_actions/effects.py | 122 ++- embodichain/lab/sim/atomic_actions/engine.py | 12 +- .../lab/sim/atomic_actions/execution.py | 338 ++++++++- embodichain/lab/sim/atomic_actions/goals.py | 122 +++ .../lab/sim/atomic_actions/invocation.py | 13 + embodichain/lab/sim/atomic_actions/plans.py | 145 +++- .../sim/atomic_actions/primitives/__init__.py | 9 + .../sim/atomic_actions/primitives/_helpers.py | 38 +- .../primitives/coordinated_pickment.py | 28 +- .../primitives/coordinated_placement.py | 49 +- .../atomic_actions/primitives/hand_over.py | 36 +- .../primitives/move_held_object.py | 17 +- .../primitives/operate_articulation.py | 468 ++++++++++++ .../sim/atomic_actions/primitives/pick_up.py | 16 +- .../sim/atomic_actions/primitives/place.py | 36 +- embodichain/lab/sim/atomic_actions/runtime.py | 65 ++ embodichain/lab/sim/atomic_actions/state.py | 312 +++++++- embodichain/lab/sim/objects/articulation.py | 9 + tests/sim/atomic_actions/test_actions.py | 699 ++++++++++++++++-- .../test_articulation_effects.py | 120 +++ tests/sim/atomic_actions/test_control.py | 58 ++ tests/sim/atomic_actions/test_core.py | 175 ++++- tests/sim/atomic_actions/test_engine.py | 5 +- .../sim/atomic_actions/test_engine_per_env.py | 495 ++++++++++++- tests/sim/objects/test_articulation.py | 14 +- tests/sim/objects/test_robot.py | 20 +- 30 files changed, 3654 insertions(+), 156 deletions(-) create mode 100644 embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py create mode 100644 tests/sim/atomic_actions/test_articulation_effects.py diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 6eec6bdac..fd1e0429c 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -31,6 +31,8 @@ from .affordance import ( Affordance, AntipodalAffordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, AssembleAffordance, InteractionPoints, ) @@ -61,15 +63,23 @@ EffectVerificationResult, ExecutionEvent, ExecutionEventKind, + ExecutionPlanAttempt, ExecutionSession, ExecutionStatus, ExecutionTick, ) -from .goals import ActionGoal, ObjectActionGoal, PoseGoalValue, SceneEntityPose +from .goals import ( + ActionGoal, + ObjectActionGoal, + PoseGoalValue, + SceneArticulationOperationGeometry, + SceneEntityPose, +) from .invocation import ActionInvocation, ActionOptions, ResolvedActionRequest from .plans import ( ActionPlan, CompiledTrajectory, + EffectVerificationRequirement, ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, @@ -119,6 +129,9 @@ MoveHeldObjectOptions, MoveJoints, MoveJointsOptions, + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, PickUp, PickUpOptions, Place, @@ -152,9 +165,11 @@ SimulationExecutionAdapter, ) from .state import ( + ArticulationJointState, CoordinatedHeldObjectState, EntityState, HeldObjectState, + ObservedArticulationJointState, PlanningContext, RobotObservation, SceneSnapshot, @@ -171,6 +186,9 @@ "ActionPlanningServices", "Affordance", "AntipodalAffordance", + "ArticulationOperationAffordance", + "ArticulationOperationTarget", + "ArticulationJointState", "AssembleAffordance", "AssembleGoal", "AtomicAction", @@ -203,12 +221,14 @@ "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", + "EffectVerificationRequirement", "EffectVerificationResult", "EffectVerifier", "ExecutionClock", "ExecutionFeedbackMode", "ExecutionEvent", "ExecutionEventKind", + "ExecutionPlanAttempt", "ExecutionRunner", "ExecutionRunnerCfg", "ExecutionSession", @@ -241,6 +261,10 @@ "ObjectSemantics", "OPEN_COMMAND", "ObservationProvider", + "ObservedArticulationJointState", + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", "PickUp", "PickUpOptions", "Place", @@ -264,6 +288,7 @@ "RunnerStep", "RunnerStepCallback", "SceneProvider", + "SceneArticulationOperationGeometry", "SceneSnapshot", "SceneSnapshotSupplier", "SceneEntityPose", diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index dbe1ffea7..1a5e6c452 100644 --- a/embodichain/lab/sim/atomic_actions/affordance.py +++ b/embodichain/lab/sim/atomic_actions/affordance.py @@ -17,7 +17,11 @@ from __future__ import annotations import torch +from collections.abc import Mapping +from copy import deepcopy from dataclasses import dataclass, field +import math +from types import MappingProxyType from typing import Any, TYPE_CHECKING from embodichain.toolkits.graspkit.pg_grasp import ( @@ -236,6 +240,303 @@ def get_approach_direction(self, point_idx: int) -> torch.Tensor: ) +def _owned_se3_offset(value: torch.Tensor, *, field_name: str) -> torch.Tensor: + """Validate and own one affordance-local homogeneous transform.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.shape != (4, 4): + raise ValueError(f"{field_name} must have shape (4, 4).") + if not value.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + if not torch.isfinite(value).all(): + raise ValueError(f"{field_name} must contain only finite values.") + checked = value.to(dtype=torch.float64) + bottom = checked.new_tensor((0.0, 0.0, 0.0, 1.0)) + if not torch.allclose(checked[3], bottom, atol=1.0e-6, rtol=0.0): + raise ValueError(f"{field_name} must have bottom row [0, 0, 0, 1].") + rotation = checked[:3, :3] + if not torch.allclose( + rotation.T @ rotation, + torch.eye(3, dtype=checked.dtype, device=checked.device), + atol=1.0e-6, + rtol=0.0, + ) or not torch.isclose( + torch.linalg.det(rotation), + checked.new_tensor(1.0), + atol=1.0e-6, + rtol=0.0, + ): + raise ValueError(f"{field_name} must contain a proper SE(3) rotation.") + return value.clone() + + +def _finite_scalar(value: float, *, field_name: str) -> float: + """Return one finite non-boolean scalar as a float.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a finite scalar.") + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + return normalized + + +@dataclass(frozen=True, slots=True) +class ArticulationOperationTarget: + """Named joint target and handle-relative operation displacement. + + ``displacement`` is deliberately explicit: it is the full signed handle + stroke from the live source joint position captured during semantic + grounding to ``target_position``. Recovery replans scale this stroke by + the remaining live joint progress. + """ + + target_position: float + """Absolute desired articulation joint position.""" + + displacement: float + """Signed operation displacement from the currently observed handle pose.""" + + def __post_init__(self) -> None: + object.__setattr__( + self, + "target_position", + _finite_scalar( + self.target_position, + field_name="ArticulationOperationTarget.target_position", + ), + ) + object.__setattr__( + self, + "displacement", + _finite_scalar( + self.displacement, + field_name="ArticulationOperationTarget.displacement", + ), + ) + + def snapshot(self) -> ArticulationOperationTarget: + """Return an independently constructed immutable target.""" + return ArticulationOperationTarget(self.target_position, self.displacement) + + +@dataclass(eq=False) +class ArticulationOperationAffordance(Affordance): + """Declarative handle geometry for one articulated joint operation. + + The four offsets are expressed in the live handle frame. During semantic + grounding the approach and contact poses are ``handle @ offset``. The + operation and retract poses additionally insert a local translation of + ``operation_axis * displacement * position_scale`` before their offsets. + This keeps task code free of pose-matrix construction; the semantic + compiler copies the geometry into a late-bound atomic goal. + """ + + joint_id: str = "" + """Canonical joint identifier written to the atomic goal and effect.""" + + approach_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + contact_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + operation_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + retract_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + operation_axis: torch.Tensor = field( + default_factory=lambda: torch.tensor((1.0, 0.0, 0.0), dtype=torch.float32) + ) + """Unit operation direction expressed in the observed handle frame.""" + + position_scale: float = 1.0 + """Positive conversion from declared displacement units to pose metres.""" + + semantic_targets: Mapping[str, ArticulationOperationTarget] = field( + default_factory=dict + ) + """Optional stable target names mapped to position/displacement pairs.""" + + def __post_init__(self) -> None: + if ( + type(self.joint_id) is not str + or not self.joint_id + or self.joint_id != self.joint_id.strip() + ): + raise ValueError( + "ArticulationOperationAffordance.joint_id must be a non-empty " + "canonical identifier." + ) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + setattr( + self, + field_name, + _owned_se3_offset( + getattr(self, field_name), + field_name=f"ArticulationOperationAffordance.{field_name}", + ), + ) + axis = self.operation_axis + if not isinstance(axis, torch.Tensor): + raise TypeError( + "ArticulationOperationAffordance.operation_axis must be a tensor." + ) + if axis.shape != (3,) or not axis.is_floating_point(): + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be a " + "floating tensor with shape (3,)." + ) + if not torch.isfinite(axis).all(): + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be finite." + ) + norm = torch.linalg.vector_norm(axis) + if float(norm) <= torch.finfo(axis.dtype).eps: + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be non-zero." + ) + self.operation_axis = (axis / norm).clone() + self.position_scale = _finite_scalar( + self.position_scale, + field_name="ArticulationOperationAffordance.position_scale", + ) + if self.position_scale <= 0.0: + raise ValueError( + "ArticulationOperationAffordance.position_scale must be positive." + ) + if not isinstance(self.semantic_targets, Mapping): + raise TypeError( + "ArticulationOperationAffordance.semantic_targets must be a mapping." + ) + targets: dict[str, ArticulationOperationTarget] = {} + for target_id, target in self.semantic_targets.items(): + if ( + type(target_id) is not str + or not target_id + or target_id != target_id.strip() + ): + raise ValueError( + "Articulation operation target IDs must be non-empty canonical " + "identifiers." + ) + if type(target) is not ArticulationOperationTarget: + raise TypeError( + "semantic_targets values must be exact " + "ArticulationOperationTarget values." + ) + targets[target_id] = target.snapshot() + self.semantic_targets = MappingProxyType(targets) + + def resolve_target(self, target_id: str) -> ArticulationOperationTarget: + """Return an owned named target or raise with deterministic candidates.""" + if type(target_id) is not str or not target_id: + raise ValueError("target_id must be a non-empty string.") + try: + target = self.semantic_targets[target_id] + except KeyError as exc: + raise KeyError( + f"Unknown articulation target {target_id!r}; available targets are " + f"{sorted(self.semantic_targets)}." + ) from exc + return target.snapshot() + + def __deepcopy__(self, memo: dict[int, object]) -> ArticulationOperationAffordance: + """Copy immutable configuration despite ``MappingProxyType`` storage.""" + existing = memo.get(id(self)) + if existing is not None: + assert isinstance(existing, ArticulationOperationAffordance) + return existing + copied = ArticulationOperationAffordance( + object_label=self.object_label, + custom_config=deepcopy(self.custom_config, memo), + joint_id=self.joint_id, + approach_offset=self.approach_offset, + contact_offset=self.contact_offset, + operation_offset=self.operation_offset, + retract_offset=self.retract_offset, + operation_axis=self.operation_axis, + position_scale=self.position_scale, + semantic_targets={ + target_id: target.snapshot() + for target_id, target in self.semantic_targets.items() + }, + ) + memo[id(self)] = copied + return copied + + def ground_poses( + self, + handle_pose: torch.Tensor, + *, + displacement: float, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Ground four end-effector poses from a fresh handle observation. + + Args: + handle_pose: Live handle pose with shape ``(4, 4)`` or ``(B, 4, 4)``. + displacement: Signed displacement from this observed handle pose. + + Returns: + Approach, contact, operation, and retract pose batches. + """ + if not isinstance(handle_pose, torch.Tensor): + raise TypeError("handle_pose must be a torch.Tensor.") + if handle_pose.shape == (4, 4): + handles = handle_pose.unsqueeze(0) + elif ( + handle_pose.dim() == 3 + and handle_pose.shape[0] > 0 + and handle_pose.shape[-2:] == (4, 4) + ): + handles = handle_pose + else: + raise ValueError("handle_pose must have shape (4, 4) or (B, 4, 4).") + if not handle_pose.is_floating_point() or not torch.isfinite(handle_pose).all(): + raise ValueError("handle_pose must be a finite floating tensor.") + displacement = _finite_scalar(displacement, field_name="displacement") + offsets = tuple( + getattr(self, field_name) + .to( + device=handles.device, + dtype=handles.dtype, + ) + .unsqueeze(0) + .expand(handles.shape[0], -1, -1) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ) + ) + translation = ( + torch.eye( + 4, + dtype=handles.dtype, + device=handles.device, + ) + .unsqueeze(0) + .repeat(handles.shape[0], 1, 1) + ) + translation[:, :3, 3] = self.operation_axis.to( + device=handles.device, + dtype=handles.dtype, + ) * (displacement * self.position_scale) + approach = torch.bmm(handles, offsets[0]) + contact = torch.bmm(handles, offsets[1]) + moved_handle = torch.bmm(handles, translation) + operation = torch.bmm(moved_handle, offsets[2]) + retract = torch.bmm(moved_handle, offsets[3]) + return tuple(pose.clone() for pose in (approach, contact, operation, retract)) + + @dataclass class AssembleAffordance(Affordance): """Affordance describing how an assemble object fits onto a base object. @@ -316,6 +617,8 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: __all__ = [ "Affordance", "AntipodalAffordance", + "ArticulationOperationAffordance", + "ArticulationOperationTarget", "InteractionPoints", "AssembleAffordance", ] diff --git a/embodichain/lab/sim/atomic_actions/bindings.py b/embodichain/lab/sim/atomic_actions/bindings.py index d56713580..3043c5b5c 100644 --- a/embodichain/lab/sim/atomic_actions/bindings.py +++ b/embodichain/lab/sim/atomic_actions/bindings.py @@ -189,6 +189,9 @@ class EndpointBinding: resource_id: str adapter_id: str target: RuntimeEndpointTarget + task_state_key: str | None = None + """Symbolic task-state key; direct-core defaults to ``target.target_id``.""" + capabilities: frozenset[str] = frozenset() commands: Mapping[str, ControlCommand] = field(default_factory=dict) claim_tokens: frozenset[str] = frozenset() @@ -235,6 +238,14 @@ def __post_init__(self) -> None: "fingerprint." ) object.__setattr__(self, "target", target) + task_state_key = ( + target.target_id if self.task_state_key is None else self.task_state_key + ) + _validate_identifier( + task_state_key, + field_name="EndpointBinding.task_state_key", + ) + object.__setattr__(self, "task_state_key", task_state_key) object.__setattr__( self, "capabilities", @@ -344,6 +355,7 @@ def with_commands( resource_id=self.resource_id, adapter_id=self.adapter_id, target=self.target, + task_state_key=self.task_state_key, capabilities=self.capabilities, commands=merged, claim_tokens=self.claim_tokens, @@ -358,6 +370,7 @@ def snapshot(self) -> EndpointBinding: resource_id=self.resource_id, adapter_id=self.adapter_id, target=self.target, + task_state_key=self.task_state_key, capabilities=self.capabilities, commands=self.commands, claim_tokens=self.claim_tokens, diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index 707ed9233..a3d37d55e 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -41,6 +41,7 @@ ) from .plans import ( ActionPlan, + EffectVerificationRequirement, ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, @@ -502,9 +503,11 @@ def build_plan( success: bool | torch.Tensor, trajectory: TimedTrajectory | torch.Tensor, expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, + scene_dependency_monitor_until: Mapping[str, int] | None = None, ) -> ActionPlan: """Build a validated action plan for a primitive implementation. @@ -514,10 +517,19 @@ def build_plan( success: Per-environment planning success or scalar planner result. trajectory: Full-robot timed trajectory or position tensor. expected_effects: Symbolic effects to verify after execution. + effect_verification: Optional explicit physical-effect boundary. + Use this when verification is required without a symbolic task- + state delta. replannable: Whether the execution runtime may replan this action. diagnostics: Optional retained planner diagnostics. segment_lengths: Optional ordered mapping from semantic segment names to waypoint counts. Zero-length entries are omitted. + scene_dependency_monitor_until: Optional per-entity exclusive + waypoint-index upper bound for scene-motion invalidation. An + entity is monitored while the current waypoint index is smaller + than its bound. ``0`` disables monitoring immediately; omitted + dependencies remain monitored for the full action. Once the bound + is reached, all pose changes for that entity are ignored. Returns: Side-effect-free action plan. @@ -557,9 +569,11 @@ def build_plan( success=success_mask, commands=commands, expected_effects=expected_effects, + effect_verification=effect_verification, replannable=replannable, diagnostics=diagnostics, segment_lengths=segment_lengths, + scene_dependency_monitor_until=scene_dependency_monitor_until, feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, joint_trajectory=timed, ) @@ -572,9 +586,11 @@ def build_command_plan( success: bool | torch.Tensor, commands: TimedCommandSequence, expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, + scene_dependency_monitor_until: Mapping[str, int] | None = None, feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, joint_trajectory: TimedTrajectory | None = None, ) -> ActionPlan: @@ -583,6 +599,30 @@ def build_command_plan( Non-joint command sequences use timed completion unless a future endpoint-specific feedback evaluator is installed. Semantic effects remain externally verified through the execution session. + + Args: + request: Resolved invocation snapshot being planned. + context: Planning input used for the plan. + success: Per-environment planning success or scalar planner result. + commands: Transport-neutral command sequence for the action. + expected_effects: Symbolic effects to verify after execution. + effect_verification: Optional explicit physical-effect boundary. + replannable: Whether the execution runtime may replan this action. + diagnostics: Optional retained planner diagnostics. + segment_lengths: Optional ordered mapping from semantic segment names + to command-frame counts. Zero-length entries are omitted. + scene_dependency_monitor_until: Optional per-entity exclusive + command-frame-index upper bound for scene-motion invalidation. An + entity is monitored while the current frame index is smaller than + its bound. ``0`` disables monitoring immediately; omitted + dependencies remain monitored for the full action. Once the bound + is reached, all pose changes for that entity are ignored. + feedback_mode: Feedback contract used to determine target completion. + joint_trajectory: Optional joint trajectory retained for joint-position + feedback and inspection. + + Returns: + Side-effect-free action plan. """ self.require_goal(request) if not isinstance(commands, TimedCommandSequence): @@ -629,9 +669,15 @@ def build_command_plan( joint_trajectory=joint_trajectory, segments=segments, scene_dependencies=self._scene_dependencies(request), + scene_dependency_monitor_until=( + {} + if scene_dependency_monitor_until is None + else scene_dependency_monitor_until + ), collision_world_sensitive=self._uses_collision_world(request, context), replannable=replannable, expected_effects=expected_effects or StateDelta(), + effect_verification=effect_verification, invocation_id=request.invocation_id, invocation_revision=request.revision, ) diff --git a/embodichain/lab/sim/atomic_actions/effects.py b/embodichain/lab/sim/atomic_actions/effects.py index 90b80bfa5..eb33be348 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -29,9 +29,11 @@ from embodichain.lab.sim.common import BatchEntity from .state import ( + ArticulationJointState, CoordinatedHeldObjectState, HeldObjectState, TaskState, + _normalize_articulation_joint, _normalize_coordinated_held, _normalize_held, _normalize_mask, @@ -118,6 +120,16 @@ def _snapshot_coordinated( ) +def _snapshot_articulation_joint( + value: ArticulationJointState, +) -> ArticulationJointState: + """Return an independently owned articulation-joint effect value.""" + return ArticulationJointState( + position=value.position.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + + def _with_held_mask( value: HeldObjectState, env_mask: torch.Tensor, @@ -146,6 +158,14 @@ def _with_coordinated_mask( ) +def _with_articulation_joint_mask( + value: ArticulationJointState, + env_mask: torch.Tensor, +) -> ArticulationJointState: + """Copy an articulation-joint state with a replacement mask.""" + return ArticulationJointState(position=value.position, env_mask=env_mask) + + def _merge_held( previous: HeldObjectState | None, candidate: HeldObjectState | None, @@ -244,6 +264,48 @@ def _merge_coordinated( ) +def _merge_articulation_joint( + previous: ArticulationJointState | None, + candidate: ArticulationJointState | None, + update_mask: torch.Tensor, +) -> ArticulationJointState | None: + """Apply one optional articulation-joint update per environment.""" + if previous is None and candidate is None: + return None + if previous is None: + assert candidate is not None and candidate.env_mask is not None + env_mask = candidate.env_mask & update_mask + return ( + _with_articulation_joint_mask(candidate, env_mask) + if env_mask.any() + else None + ) + assert previous.env_mask is not None + if candidate is None: + env_mask = previous.env_mask & ~update_mask + return ( + _with_articulation_joint_mask(previous, env_mask) + if env_mask.any() + else None + ) + assert candidate.env_mask is not None + if candidate.position.shape != previous.position.shape: + raise ValueError( + "Cannot merge articulation-joint states with different joint widths." + ) + env_mask = torch.where(update_mask, candidate.env_mask, previous.env_mask) + if not env_mask.any(): + return None + return ArticulationJointState( + position=torch.where( + update_mask[:, None], + candidate.position, + previous.position, + ), + env_mask=env_mask, + ) + + @dataclass(frozen=True, slots=True, eq=False) class StateDelta: """Expected task-state changes that require post-execution verification. @@ -263,9 +325,15 @@ class StateDelta: ] = field(default_factory=dict) """Per-resource-pair coordinated attachment replacements or removals.""" + articulation_joint_updates: Mapping[ + tuple[str, str], ArticulationJointState | None + ] = field(default_factory=dict) + """Per-articulation/joint verified state replacements or removals.""" + def __post_init__(self) -> None: held = dict(self.held_object_updates) coordinated = dict(self.coordinated_held_object_updates) + articulation = dict(self.articulation_joint_updates) for resource, value in held.items(): if not isinstance(resource, str) or not resource: raise ValueError( @@ -289,17 +357,43 @@ def __post_init__(self) -> None: "coordinated_held_object_updates values must be " "CoordinatedHeldObjectState or None." ) + for key, value in articulation.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(item) is str and item and item == item.strip() for item in key + ) + ): + raise ValueError( + "articulation_joint_updates keys must be canonical " + "articulation/joint pairs." + ) + if value is not None and not isinstance(value, ArticulationJointState): + raise TypeError( + "articulation_joint_updates values must be " + "ArticulationJointState or None." + ) object.__setattr__(self, "held_object_updates", MappingProxyType(held)) object.__setattr__( self, "coordinated_held_object_updates", MappingProxyType(coordinated), ) + object.__setattr__( + self, + "articulation_joint_updates", + MappingProxyType(articulation), + ) @property def is_empty(self) -> bool: """Whether this delta declares no symbolic state changes.""" - return not self.held_object_updates and not self.coordinated_held_object_updates + return ( + not self.held_object_updates + and not self.coordinated_held_object_updates + and not self.articulation_joint_updates + ) def snapshot(self) -> StateDelta: """Return an independently owned symbolic-effect snapshot. @@ -319,6 +413,10 @@ def snapshot(self) -> StateDelta: resources: (None if value is None else _snapshot_coordinated(value)) for resources, value in self.coordinated_held_object_updates.items() }, + articulation_joint_updates={ + key: (None if value is None else _snapshot_articulation_joint(value)) + for key, value in self.articulation_joint_updates.items() + }, ) def apply( @@ -381,11 +479,33 @@ def apply( else: coordinated[resources] = merged + articulation = dict(state.articulation_joints) + for key, candidate in self.articulation_joint_updates.items(): + normalized = ( + None + if candidate is None + else _normalize_articulation_joint( + candidate, + batch_size=state.batch_size, + device=state.device, + ) + ) + merged = _merge_articulation_joint( + articulation.get(key), + normalized, + mask, + ) + if merged is None: + articulation.pop(key, None) + else: + articulation[key] = merged + return TaskState( batch_size=state.batch_size, device=state.device, held_objects=held, coordinated_held_objects=coordinated, + articulation_joints=articulation, ) diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 2c7d446df..ff1d5eb30 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -248,6 +248,8 @@ def bind_control_parts( self, skill: str | AtomicAction, endpoints: Mapping[str, Mapping[str, str]], + *, + task_state_keys: Mapping[str, str] | None = None, ) -> ActionBinding: """Build an advanced direct-core binding from control-part names. @@ -255,6 +257,10 @@ def bind_control_parts( skill: Installed skill ID or an explicit action passed later to :meth:`plan_action`. endpoints: Nested ``slot_id -> endpoint_id -> control_part`` mapping. + task_state_keys: Optional explicit stable task-state key for each + resource slot. See + :meth:`ActionPlanningServices.bind_control_parts` for inference + rules when omitted. Returns: Engine-owned generic endpoint binding. @@ -279,7 +285,11 @@ def bind_control_parts( raise ValueError( f"Skill {action.skill_id!r} has no explicit SkillBindingContract." ) - return self._planning_services.bind_control_parts(contract, endpoints) + return self._planning_services.bind_control_parts( + contract, + endpoints, + task_state_keys=task_state_keys, + ) def register(self, action: AtomicAction, *, replace: bool = False) -> None: """Register one action instance using its descriptor. diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index ba21c669c..c30691c28 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -30,9 +30,11 @@ from .bindings import JointPositionTarget, RuntimeEndpointTarget from .plans import ( ActionPlan, + EffectVerificationRequirement, ExecutionFeedbackMode, TrajectorySegment, ) +from .policies import RecoveryPolicy from .runtime_commands import ( JointPositionPayload, RuntimeCommandFrame, @@ -102,9 +104,118 @@ def __post_init__(self) -> None: object.__setattr__(self, "env_mask", self.env_mask.clone()) +@dataclass(frozen=True, slots=True, eq=False) +class ExecutionPlanAttempt: + """Owned inspection snapshot for one installed action plan. + + Recovery can install several plans for one logical invocation. This value + preserves the exact scene/collision revisions and trajectory structure of + every installation, correlated with the session-local attempt generation + and row-local recovery counters. + """ + + attempt_generation: int + event_kind: ExecutionEventKind + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + request: ResolvedActionRequest + plan: ActionPlan + + def __post_init__(self) -> None: + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be a non-negative integer.") + if self.event_kind not in { + ExecutionEventKind.ACTION_PLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ExecutionEventKind.REPLANNED, + }: + raise ValueError("event_kind must describe an installed action plan.") + if not math.isfinite(self.planned_at) or self.planned_at < 0.0: + raise ValueError("planned_at must be finite and non-negative.") + if type(self.invocation_index) is not int or self.invocation_index < 0: + raise ValueError("invocation_index must be a non-negative integer.") + if ( + not isinstance(self.planned_mask, torch.Tensor) + or self.planned_mask.dtype != torch.bool + or self.planned_mask.dim() != 1 + ): + raise ValueError("planned_mask must be a one-dimensional bool tensor.") + retries = tuple(self.action_retry_counts) + replans = tuple(self.replan_counts) + batch_size = int(self.planned_mask.numel()) + if len(retries) != batch_size or len(replans) != batch_size: + raise ValueError("Recovery counters must contain one value per row.") + if any(type(value) is not int or value < 0 for value in (*retries, *replans)): + raise ValueError("Recovery counters must be non-negative integers.") + if not isinstance(self.request, ResolvedActionRequest): + raise TypeError("request must be a ResolvedActionRequest.") + if not isinstance(self.plan, ActionPlan): + raise TypeError("plan must be an ActionPlan.") + if ( + self.request.skill_id != self.plan.skill_id + or self.request.invocation_id != self.plan.invocation_id + or self.request.revision != self.plan.invocation_revision + ): + raise ValueError("request identity must match the installed plan.") + if self.plan.plan_success.shape != self.planned_mask.shape: + raise ValueError("plan and planned_mask batch shapes must match.") + if self.plan.plan_success.device != self.planned_mask.device: + raise ValueError("plan and planned_mask must share a device.") + object.__setattr__(self, "planned_mask", self.planned_mask.clone()) + object.__setattr__(self, "action_retry_counts", retries) + object.__setattr__(self, "replan_counts", replans) + object.__setattr__(self, "request", self.request.snapshot()) + object.__setattr__(self, "plan", self.plan.snapshot()) + + def snapshot(self) -> ExecutionPlanAttempt: + """Return an independently owned plan-attempt trace.""" + return ExecutionPlanAttempt( + attempt_generation=self.attempt_generation, + event_kind=self.event_kind, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + request=self.request, + plan=self.plan, + ) + + +@dataclass(frozen=True, slots=True) +class _ExecutionPlanAttemptRecord: + """Session-private plan reference converted to an owned public snapshot.""" + + attempt_generation: int + event_kind: ExecutionEventKind + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + request: ResolvedActionRequest + plan: ActionPlan + + def snapshot(self) -> ExecutionPlanAttempt: + return ExecutionPlanAttempt( + attempt_generation=self.attempt_generation, + event_kind=self.event_kind, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + request=self.request, + plan=self.plan, + ) + + @dataclass(frozen=True, slots=True, eq=False) class EffectVerificationRequest: - """Typed boundary describing a semantic effect awaiting verification. + """Typed boundary describing a physical effect awaiting verification. ``requested_at`` and ``deadline`` use the same timestamp domain as :class:`RobotObservation`. Request-mask shrinkage retains both values; @@ -124,6 +235,7 @@ class EffectVerificationRequest: deadline: float env_mask: torch.Tensor expected_effects: StateDelta + effect_verification: EffectVerificationRequirement | None = None def __post_init__(self) -> None: if type(self.verification_id) is not int or self.verification_id < 0: @@ -158,10 +270,30 @@ def __post_init__(self) -> None: raise ValueError("env_mask must contain at least one requested row.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") - if self.expected_effects.is_empty: - raise ValueError("Effect verification requires a non-empty StateDelta.") + if ( + self.effect_verification is not None + and type(self.effect_verification) is not EffectVerificationRequirement + ): + raise TypeError( + "effect_verification must be exactly " + "EffectVerificationRequirement or None." + ) + if self.expected_effects.is_empty and self.effect_verification is None: + raise ValueError( + "Effect verification requires expected symbolic effects or an " + "explicit physical-effect requirement." + ) object.__setattr__(self, "env_mask", self.env_mask.clone()) object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + object.__setattr__( + self, + "effect_verification", + ( + None + if self.effect_verification is None + else self.effect_verification.snapshot() + ), + ) def snapshot(self) -> EffectVerificationRequest: """Return a request snapshot with an independently owned row mask.""" @@ -177,6 +309,7 @@ def snapshot(self) -> EffectVerificationRequest: deadline=self.deadline, env_mask=self.env_mask, expected_effects=self.expected_effects, + effect_verification=self.effect_verification, ) @@ -273,9 +406,9 @@ class ExecutionSession: The session never steps a simulator itself. Each :meth:`tick` consumes the latest observation and scene snapshot and emits at most one synchronized - endpoint-command frame. Expected symbolic effects are committed only after the caller - supplies a correlated :class:`EffectVerificationResult` for a non-empty - :class:`StateDelta`. + endpoint-command frame. A declared physical-effect boundary resolves only + after the caller supplies a correlated :class:`EffectVerificationResult`. + Non-empty expected symbolic effects are committed for verified rows only. Environment eligibility and recovery budgets are tracked per row. The waypoint cursor is batch-synchronized: a recoverable row replans the active @@ -330,6 +463,7 @@ def __init__( self._effect_failures = torch.zeros_like(self._eligible) self._effect_requested_at: float | None = None self._next_effect_verification_id = 0 + self._plan_attempt_records: list[_ExecutionPlanAttemptRecord] = [] self._status = ( ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED ) @@ -573,6 +707,27 @@ def active_commands(self) -> TimedCommandSequence: assert self._plan is not None return self._plan.commands.snapshot() + @property + def active_plan(self) -> ActionPlan: + """Return an independently owned snapshot of the active action plan. + + This is a read-only diagnostics boundary for runtime metadata, + visualization, and tests. Planning and recovery remain session-owned; + mutating any tensor in the returned value cannot affect execution. + """ + assert self._plan is not None + return self._plan.snapshot() + + @property + def plan_attempts(self) -> tuple[ExecutionPlanAttempt, ...]: + """Return every installed plan in deterministic recovery order. + + The initial plan has generation zero. Each invocation revision, + recovery replan, or whole-action retry appends a new generation instead + of replacing earlier scene/collision evidence. + """ + return tuple(record.snapshot() for record in self._plan_attempt_records) + def trajectory_segment(self, name: str) -> TrajectorySegment: """Return named segment metadata for the active action plan. @@ -607,7 +762,7 @@ def tick( "effect_result must be exactly EffectVerificationResult or None." ) if self._pending_effect is None: - raise ValueError("No semantic effect is awaiting verification.") + raise ValueError("No physical effect is awaiting verification.") if effect_result.verification_id != self._pending_effect.verification_id: raise ValueError( "effect_result verification_id does not match the pending " @@ -645,7 +800,7 @@ def tick( self._event( ExecutionEventKind.EFFECT_VERIFICATION_FAILED, known_failures, - "Expected semantic effects were not observed.", + "Required physical effects were not observed.", ) ) if planning_failed.any(): @@ -708,7 +863,7 @@ def tick( self._attempt_action_retry( retry_mask, ExecutionEventKind.EFFECT_VERIFICATION_FAILED, - "Expected semantic effects were not observed.", + "Required physical effects were not observed.", reason_mask=failed_effect, ) ) @@ -756,6 +911,18 @@ def tick( events=events, ) + if not execution_mask.any(): + command, hold_targets, completion_events = self._finish_action( + execution_mask, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + commands = plan.commands if self._waypoint_index < commands.frame_count: command = self._command_at(plan, self._waypoint_index, execution_mask) @@ -767,11 +934,15 @@ def tick( terminal_error > plan.recovery_policy.tracking_error_threshold ) if not_reached.any(): + max_terminal_error = float(terminal_error[not_reached].amax().item()) events.extend( self._attempt_replan( not_reached, ExecutionEventKind.TRACKING_ERROR, - "Terminal command has not been reached.", + "Terminal command has not been reached " + f"(max_error={max_terminal_error:.6f}, " + "threshold=" + f"{plan.recovery_policy.tracking_error_threshold:.6f}).", ) ) if self._status is not ExecutionStatus.RUNNING: @@ -905,6 +1076,23 @@ def _install_plan( self._effect_failures.zero_() self._effect_requested_at = None planned_mask = self._pending & plan.plan_success + self._plan_attempt_records.append( + _ExecutionPlanAttemptRecord( + attempt_generation=self._attempt_generation, + event_kind=event_kind, + planned_at=context.robot.timestamp, + invocation_index=self._invocation_index, + planned_mask=planned_mask.clone(), + action_retry_counts=tuple( + int(value) for value in self._action_retries.detach().cpu().tolist() + ), + replan_counts=tuple( + int(value) for value in self._replans.detach().cpu().tolist() + ), + request=self._requests[self._invocation_index].snapshot(), + plan=plan.snapshot(), + ) + ) self._queued_events.append( self._event(event_kind, planned_mask, "Planned from the latest context.") ) @@ -1011,17 +1199,25 @@ def _recover_if_needed( & (tracking_error > plan.recovery_policy.tracking_error_threshold) ) if tracking_mask.any(): + max_tracking_error = float(tracking_error[tracking_mask].amax().item()) return self._attempt_replan( tracking_mask, ExecutionEventKind.TRACKING_ERROR, - "Observed joint tracking error exceeded the policy threshold.", + "Observed joint tracking error exceeded the policy threshold " + f"(max_error={max_tracking_error:.6f}, " + "threshold=" + f"{plan.recovery_policy.tracking_error_threshold:.6f}).", ) - scene_mask = self._dynamic_scene_change_mask(plan) - if (execution_mask & scene_mask).any(): + scene_mask, scene_message = self._dynamic_scene_change( + plan, + execution_mask, + ) + if scene_mask.any(): + assert scene_message is not None return self._attempt_replan( - execution_mask & scene_mask, + scene_mask, ExecutionEventKind.DYNAMIC_GOAL_CHANGED, - "A referenced scene entity moved beyond the policy threshold.", + scene_message, ) return events @@ -1165,7 +1361,7 @@ def _finish_action( failed_effect = torch.zeros_like(execution_mask) unresolved = torch.zeros_like(execution_mask) made_progress = False - if self._plan.expected_effects.is_empty: + if not self._plan.requires_effect_verification: verified = execution_mask elif effect_result is None: if self._pending_effect is None: @@ -1174,7 +1370,7 @@ def _finish_action( self._event( ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED, execution_mask, - "Expected symbolic effects require external verification.", + "The action requires external physical-effect verification.", ) ) return None, active_targets, events @@ -1202,15 +1398,16 @@ def _finish_action( self._pending_effect = None if verified.any(): - self._task_state = self._plan.expected_effects.apply( - self._task_state, verified - ) - self._context = PlanningContext( - robot=self._context.robot, - task=self._task_state, - scene=self._context.scene, - env_ids=self._context.env_ids, - ) + if not self._plan.expected_effects.is_empty: + self._task_state = self._plan.expected_effects.apply( + self._task_state, verified + ) + self._context = PlanningContext( + robot=self._context.robot, + task=self._task_state, + scene=self._context.scene, + env_ids=self._context.env_ids, + ) self._pending &= ~verified if unresolved.any(): if made_progress: @@ -1387,21 +1584,47 @@ def _terminal_error(self, plan: ActionPlan) -> torch.Tensor: ) return torch.amax(torch.cat(errors, dim=1), dim=1) - def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: - """Detect material motion of entities referenced by the action goal.""" + def _dynamic_scene_change( + self, + plan: ActionPlan, + execution_mask: torch.Tensor, + ) -> tuple[torch.Tensor, str | None]: + """Detect and describe material scene-dependency invalidation.""" dependencies = plan.scene_dependencies changed = torch.zeros_like(self._eligible) if ( not dependencies or self._context.scene.version == self._planned_scene.version ): - return changed + return changed, None policy = plan.recovery_policy - for entity_id in dependencies: + details: list[str] = [] + for entity_id in sorted(dependencies): + monitor_until = plan.scene_dependency_monitor_until.get(entity_id) + if monitor_until is not None and self._waypoint_index >= monitor_until: + continue previous = self._planned_scene.entities.get(entity_id) current = self._context.scene.entities.get(entity_id) if previous is None or current is None: - changed |= self._eligible + entity_changed = execution_mask.clone() + if not entity_changed.any(): + continue + changed |= entity_changed + missing = [] + if previous is None: + missing.append("planned_scene") + if current is None: + missing.append("current_scene") + details.append( + self._scene_dependency_change_detail( + entity_id=entity_id, + monitor_until=monitor_until, + policy=policy, + max_translation=None, + max_rotation=None, + missing=",".join(missing), + ) + ) continue previous_pose = self._batched_entity_pose(previous) current_pose = self._batched_entity_pose(current) @@ -1416,10 +1639,55 @@ def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: (relative_rotation.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) / 2.0 ).clamp(-1.0, 1.0) rotation = torch.acos(cosine) - changed |= (translation > policy.goal_translation_threshold) | ( - rotation > policy.goal_rotation_threshold + entity_changed = execution_mask & ( + (translation > policy.goal_translation_threshold) + | (rotation > policy.goal_rotation_threshold) + ) + if not entity_changed.any(): + continue + changed |= entity_changed + details.append( + self._scene_dependency_change_detail( + entity_id=entity_id, + monitor_until=monitor_until, + policy=policy, + max_translation=float(translation[entity_changed].amax().item()), + max_rotation=float(rotation[entity_changed].amax().item()), + missing=None, + ) ) - return changed + if not details: + return changed, None + return ( + changed, + "Scene dependency invalidated the active plan at " + f"waypoint_index={self._waypoint_index}: " + " | ".join(details) + ".", + ) + + @staticmethod + def _scene_dependency_change_detail( + *, + entity_id: str, + monitor_until: int | None, + policy: RecoveryPolicy, + max_translation: float | None, + max_rotation: float | None, + missing: str | None, + ) -> str: + """Return one stable scene-dependency diagnostic fragment.""" + cutoff = "none" if monitor_until is None else str(monitor_until) + translation = ( + "unavailable" if max_translation is None else f"{max_translation:.6f}" + ) + rotation = "unavailable" if max_rotation is None else f"{max_rotation:.6f}" + missing_detail = "" if missing is None else f", missing={missing}" + return ( + f"entity_id={entity_id!r}, monitor_cutoff={cutoff}{missing_detail}, " + f"max_translation={translation}, " + f"translation_threshold={policy.goal_translation_threshold:.6f}, " + f"max_rotation={rotation}, " + f"rotation_threshold={policy.goal_rotation_threshold:.6f}" + ) def _collision_world_change_mask(self, plan: ActionPlan) -> torch.Tensor: """Detect collision revisions newer than the active action plan.""" @@ -1486,6 +1754,7 @@ def _effect_verification_request( ), env_mask=env_mask, expected_effects=self._plan.expected_effects, + effect_verification=self._plan.effect_verification, ) def _event( @@ -1565,6 +1834,7 @@ def _tick_result( "EffectVerificationResult", "ExecutionEvent", "ExecutionEventKind", + "ExecutionPlanAttempt", "ExecutionSession", "ExecutionStatus", "ExecutionTick", diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index 755d3ec24..cce03c73d 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -21,6 +21,7 @@ import warnings from collections.abc import Mapping, Sequence from dataclasses import dataclass, fields, is_dataclass +import math from typing import Any, ClassVar, Protocol, TYPE_CHECKING import torch @@ -86,6 +87,126 @@ def snapshot(self) -> SceneEntityPose: ) +@dataclass(frozen=True, slots=True, eq=False) +class SceneArticulationOperationGeometry: + """Late-bound handle geometry for an articulation operation. + + The offsets and operation axis are immutable grounded affordance data. The + handle itself remains a :class:`SceneEntityPose`, so every atomic plan or + recovery replan resolves it from the latest :class:`SceneSnapshot`. + """ + + handle_pose: SceneEntityPose + approach_offset: torch.Tensor + contact_offset: torch.Tensor + operation_offset: torch.Tensor + retract_offset: torch.Tensor + operation_axis: torch.Tensor + position_scale: float = 1.0 + + def __post_init__(self) -> None: + if not isinstance(self.handle_pose, SceneEntityPose): + raise TypeError("handle_pose must be a SceneEntityPose.") + object.__setattr__(self, "handle_pose", self.handle_pose.snapshot()) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + offset = getattr(self, field_name) + validate_pose_tensor(offset, field_name, allow_waypoints=False) + if offset.shape != (4, 4): + raise ValueError(f"{field_name} must have shape (4, 4).") + if not offset.is_floating_point() or not torch.isfinite(offset).all(): + raise ValueError(f"{field_name} must be a finite floating tensor.") + object.__setattr__(self, field_name, offset.clone()) + axis = self.operation_axis + if ( + not isinstance(axis, torch.Tensor) + or axis.shape != (3,) + or not axis.is_floating_point() + ): + raise ValueError("operation_axis must be a floating tensor of shape (3,).") + if not torch.isfinite(axis).all(): + raise ValueError("operation_axis must contain only finite values.") + norm = torch.linalg.vector_norm(axis) + if float(norm) <= torch.finfo(axis.dtype).eps: + raise ValueError("operation_axis must be non-zero.") + object.__setattr__(self, "operation_axis", (axis / norm).clone()) + scale = self.position_scale + if isinstance(scale, bool) or not isinstance(scale, (int, float)): + raise TypeError("position_scale must be a finite positive scalar.") + scale = float(scale) + if not math.isfinite(scale) or scale <= 0.0: + raise ValueError("position_scale must be a finite positive scalar.") + object.__setattr__(self, "position_scale", scale) + + def resolve( + self, + context: PlanningContext, + *, + displacement: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Resolve four poses using a fresh handle and row-local displacement. + + Args: + context: Latest immutable planning observation. + displacement: Remaining signed handle displacement, shape ``(B,)``. + + Returns: + Approach, contact, operation, and retract pose batches. + """ + if not isinstance(displacement, torch.Tensor): + raise TypeError("displacement must be a torch.Tensor.") + if displacement.shape != (context.batch_size,): + raise ValueError("displacement must have one scalar for each planning row.") + if ( + not displacement.is_floating_point() + or not torch.isfinite(displacement).all() + ): + raise ValueError("displacement must be a finite floating tensor.") + handle = resolve_pose_goal( + self.handle_pose, + context, + name="handle_pose", + ) + offsets = tuple( + getattr(self, field_name) + .to(device=handle.device, dtype=handle.dtype) + .unsqueeze(0) + .expand(context.batch_size, -1, -1) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ) + ) + translation = ( + torch.eye( + 4, + dtype=handle.dtype, + device=handle.device, + ) + .unsqueeze(0) + .repeat(context.batch_size, 1, 1) + ) + axis = self.operation_axis.to(device=handle.device, dtype=handle.dtype) + translation[:, :3, 3] = ( + axis.unsqueeze(0) + * displacement.to(device=handle.device, dtype=handle.dtype).unsqueeze(1) + * self.position_scale + ) + moved_handle = torch.bmm(handle, translation) + return ( + torch.bmm(handle, offsets[0]), + torch.bmm(handle, offsets[1]), + torch.bmm(moved_handle, offsets[2]), + torch.bmm(moved_handle, offsets[3]), + ) + + PoseGoalValue = torch.Tensor | SceneEntityPose """Explicit pose tensor or a pose resolved from the latest scene snapshot.""" @@ -263,6 +384,7 @@ def __post_init__(self) -> None: "ActionGoal", "ObjectActionGoal", "PoseGoalValue", + "SceneArticulationOperationGeometry", "SceneEntityPose", "collect_scene_dependencies", "resolve_pose_goal", diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index 652cbf78a..a600ff30b 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -200,6 +200,19 @@ def __post_init__(self) -> None: object.__setattr__(self, "recovery_policy", deepcopy(self.recovery_policy)) object.__setattr__(self, "skill_options", deepcopy(self.skill_options)) + def snapshot(self) -> ResolvedActionRequest[GoalT, OptionsT]: + """Return an independently owned resolved-request snapshot.""" + return ResolvedActionRequest( + skill_id=self.skill_id, + goal=self.goal, + binding=self.binding, + motion_policy=self.motion_policy, + recovery_policy=self.recovery_policy, + skill_options=self.skill_options, + invocation_id=self.invocation_id, + revision=self.revision, + ) + __all__ = [ "ActionInvocation", diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index b423cf19e..7e86f16cf 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -18,6 +18,7 @@ from __future__ import annotations +from copy import deepcopy from dataclasses import dataclass, field from enum import Enum from types import MappingProxyType @@ -370,8 +371,17 @@ class PlannerDiagnostics: def __post_init__(self) -> None: if not isinstance(self.backend, str) or not self.backend: raise ValueError("PlannerDiagnostics.backend must be non-empty.") - object.__setattr__(self, "messages", tuple(self.messages)) - object.__setattr__(self, "metadata", MappingProxyType(dict(self.metadata))) + if not isinstance(self.metadata, Mapping): + raise TypeError("PlannerDiagnostics.metadata must be a mapping.") + messages = tuple(self.messages) + if not all(type(message) is str for message in messages): + raise TypeError("PlannerDiagnostics.messages must contain strings.") + object.__setattr__(self, "messages", messages) + object.__setattr__( + self, + "metadata", + MappingProxyType(deepcopy(dict(self.metadata))), + ) class ExecutionFeedbackMode(str, Enum): @@ -381,6 +391,36 @@ class ExecutionFeedbackMode(str, Enum): TIMED = "timed" +@dataclass(frozen=True, slots=True) +class EffectVerificationRequirement: + """Explicit physical-effect verification independent of symbolic state. + + Presence of this value on an :class:`ActionPlan` forces a terminal effect + boundary even when the plan declares no :class:`StateDelta`. The open + ``kind`` identifier lets an external runtime select an appropriate + verifier without placing backend-specific callbacks in the core plan. + + Args: + kind: Stable, non-empty discriminator for the physical effect. + """ + + kind: str + + def __post_init__(self) -> None: + if ( + type(self.kind) is not str + or not self.kind + or self.kind != self.kind.strip() + ): + raise ValueError( + "kind must be a non-empty string without outer whitespace." + ) + + def snapshot(self) -> EffectVerificationRequirement: + """Return an independently owned requirement value.""" + return EffectVerificationRequirement(kind=self.kind) + + @dataclass(frozen=True, slots=True) class TrajectorySegment: """Named half-open waypoint range inside an action trajectory. @@ -423,6 +463,15 @@ class ActionPlan: An action owns one timed command sequence and one recovery boundary. Named :class:`TrajectorySegment` values describe semantic structure within that sequence without implying independent planning or recovery boundaries. + + Attributes: + scene_dependency_monitor_until: Optional exclusive waypoint-index upper + bounds for individual ``scene_dependencies``. An entity is monitored + while the current waypoint index is smaller than its bound; ``0`` + disables monitoring immediately, while an omitted entity remains + monitored for the action's full execution. Once the bound is reached, + all pose changes for that entity are ignored, regardless of whether + they were caused by the action or by an external disturbance. """ skill_id: str @@ -436,9 +485,11 @@ class ActionPlan: joint_trajectory: TimedTrajectory | None = None segments: tuple[TrajectorySegment, ...] = () scene_dependencies: tuple[str, ...] = () + scene_dependency_monitor_until: Mapping[str, int] = field(default_factory=dict) collision_world_sensitive: bool = False replannable: bool = True expected_effects: StateDelta = field(default_factory=StateDelta) + effect_verification: EffectVerificationRequirement | None = None invocation_id: str | None = None invocation_revision: int = 0 @@ -645,13 +696,37 @@ def __post_init__(self) -> None: raise ValueError( "scene_dependencies must contain unique non-empty entity ids." ) + waypoint_count = self.commands.frame_count + monitor_until = dict(self.scene_dependency_monitor_until) + if not set(monitor_until).issubset(dependencies): + raise ValueError( + "scene_dependency_monitor_until keys must be scene dependencies." + ) + for entity_id, waypoint_index in monitor_until.items(): + if ( + type(entity_id) is not str + or not entity_id + or type(waypoint_index) is not int + or not 0 <= waypoint_index <= waypoint_count + ): + raise ValueError( + "scene_dependency_monitor_until must map non-empty entity IDs " + "to waypoint indices within the command sequence." + ) if not isinstance(self.collision_world_sensitive, bool): raise TypeError("collision_world_sensitive must be a bool.") if not isinstance(self.replannable, bool): raise TypeError("replannable must be a bool.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") - waypoint_count = self.commands.frame_count + if ( + self.effect_verification is not None + and type(self.effect_verification) is not EffectVerificationRequirement + ): + raise TypeError( + "effect_verification must be exactly " + "EffectVerificationRequirement or None." + ) segments = tuple(self.segments) if not all(isinstance(segment, TrajectorySegment) for segment in segments): raise TypeError("segments must contain only TrajectorySegment values.") @@ -688,14 +763,77 @@ def __post_init__(self) -> None: ), ) object.__setattr__(self, "planned_collision_world_revision", revisions) + object.__setattr__( + self, + "diagnostics", + PlannerDiagnostics( + backend=self.diagnostics.backend, + messages=self.diagnostics.messages, + metadata=self.diagnostics.metadata, + ), + ) object.__setattr__(self, "scene_dependencies", dependencies) + object.__setattr__( + self, + "scene_dependency_monitor_until", + MappingProxyType(monitor_until), + ) object.__setattr__(self, "segments", segments) + object.__setattr__( + self, + "effect_verification", + ( + None + if self.effect_verification is None + else self.effect_verification.snapshot() + ), + ) @property def success_all(self) -> bool: """Whether every environment row planned successfully.""" return bool(self.plan_success.all().item()) + def snapshot(self) -> ActionPlan: + """Return an independently owned inspection snapshot of this plan. + + Runtime tracing and visualization need access to the exact plan that + reached an execution boundary without being able to mutate the live + session. Reconstructing the value through the public constructor also + re-applies every plan invariant and snapshots all tensor-owning nested + contracts. + + Returns: + A validated plan with independently owned tensor storage. + """ + return ActionPlan( + skill_id=self.skill_id, + plan_success=self.plan_success, + commands=self.commands, + recovery_policy=self.recovery_policy, + planned_scene_version=self.planned_scene_version, + planned_collision_world_revision=self.planned_collision_world_revision, + diagnostics=self.diagnostics, + feedback_mode=self.feedback_mode, + joint_trajectory=self.joint_trajectory, + segments=self.segments, + scene_dependencies=self.scene_dependencies, + scene_dependency_monitor_until=self.scene_dependency_monitor_until, + collision_world_sensitive=self.collision_world_sensitive, + replannable=self.replannable, + expected_effects=self.expected_effects, + effect_verification=self.effect_verification, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + ) + + @property + def requires_effect_verification(self) -> bool: + """Whether execution must verify a terminal physical effect.""" + return ( + self.effect_verification is not None or not self.expected_effects.is_empty + ) + def segment(self, name: str) -> TrajectorySegment: """Return a named trajectory segment. @@ -776,6 +914,7 @@ def segment(self, action_index: int, name: str) -> TrajectorySegment: __all__ = [ "ActionPlan", "CompiledTrajectory", + "EffectVerificationRequirement", "ExecutionFeedbackMode", "PlannerDiagnostics", "TimedTrajectory", diff --git a/embodichain/lab/sim/atomic_actions/primitives/__init__.py b/embodichain/lab/sim/atomic_actions/primitives/__init__.py index 85de2c985..563972e6a 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_actions/primitives/__init__.py @@ -41,6 +41,11 @@ MoveHeldObjectOptions, ) from .move_joints import JointPositionGoal, MoveJoints, MoveJointsOptions +from .operate_articulation import ( + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, +) from .pick_up import GraspGoal, PickUp, PickUpOptions from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions from .press import Press, PressGoal, PressOptions @@ -55,6 +60,7 @@ CoordinatedPickment, CoordinatedPlacement, HandOver, + OperateArticulation, ) """Built-in action implementations instantiated once per action engine.""" @@ -79,6 +85,9 @@ "MoveHeldObjectOptions", "MoveJoints", "MoveJointsOptions", + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", "PickUp", "PickUpOptions", "Place", diff --git a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py index fe3f4c60f..dae4c3016 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py +++ b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py @@ -22,9 +22,41 @@ from embodichain.utils import logger +from ..bindings import EndpointBinding from ..state import PlanningContext +def require_shared_task_state_key( + motion: EndpointBinding, + grasp: EndpointBinding, + *, + participant: str, +) -> str: + """Return the stable task-state key shared by one participant's endpoints. + + Args: + motion: Participant endpoint used for motion control. + grasp: Participant endpoint used for grasp control. + participant: Human-readable participant name used in validation errors. + + Returns: + Stable logical key used to address held-object task state. + + Raises: + ValueError: If the participant endpoints use different task-state keys. + """ + motion_key = motion.task_state_key + grasp_key = grasp.task_state_key + if motion_key != grasp_key: + raise ValueError( + f"{participant} motion and grasp endpoints must share one " + f"task_state_key, but got {motion_key!r} and {grasp_key!r}." + ) + if not isinstance(motion_key, str) or not motion_key: + raise ValueError(f"{participant} task_state_key must be a non-empty string.") + return motion_key + + def resolve_object_target( target: torch.Tensor, *, @@ -52,4 +84,8 @@ def arm_qpos_from_state( return context.robot.qpos[:, arm_joint_ids] -__all__ = ["arm_qpos_from_state", "resolve_object_target"] +__all__ = [ + "arm_qpos_from_state", + "require_shared_task_state_key", + "resolve_object_target", +] diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index b8cbb0b42..a528fd37d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -51,6 +51,7 @@ ) from ..state import CoordinatedHeldObjectState, PlanningContext from ..trajectory_ops import interpolate_joint_trajectory, translate_pose_world +from ._helpers import require_shared_task_state_key @dataclass(frozen=True, slots=True, eq=False) @@ -158,6 +159,8 @@ def __post_init__(self) -> None: class _CoordinatedPickResources: """Invocation-bound control parts and compatible hand commands.""" + left_task_state_key: str + right_task_state_key: str left_arm: JointPositionTarget right_arm: JointPositionTarget left_hand: JointPositionTarget @@ -425,6 +428,21 @@ def _resolve_resources( right_arm = right_motion.require_target(JointPositionTarget) left_hand = left_grasp.require_target(JointPositionTarget) right_hand = right_grasp.require_target(JointPositionTarget) + left_task_state_key = require_shared_task_state_key( + left_motion, + left_grasp, + participant="CoordinatedPickment left participant", + ) + right_task_state_key = require_shared_task_state_key( + right_motion, + right_grasp, + participant="CoordinatedPickment right participant", + ) + if left_task_state_key == right_task_state_key: + raise ValueError( + "CoordinatedPickment left and right participants must use " + "different task_state_key values." + ) if left_arm.control_part == right_arm.control_part: raise ValueError( "CoordinatedPickment left and right roles must use different " @@ -436,6 +454,8 @@ def _resolve_resources( "end-effector control parts." ) return _CoordinatedPickResources( + left_task_state_key=left_task_state_key, + right_task_state_key=right_task_state_key, left_arm=left_arm, right_arm=right_arm, left_hand=left_hand, @@ -1025,13 +1045,13 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.left_arm.control_part: None, - resources.right_arm.control_part: None, + resources.left_task_state_key: None, + resources.right_task_state_key: None, }, coordinated_held_object_updates={ ( - resources.left_arm.control_part, - resources.right_arm.control_part, + resources.left_task_state_key, + resources.right_task_state_key, ): coordinated_held_object, }, ), diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index c00771e02..cba06598e 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -25,7 +25,6 @@ from embodichain.utils import logger -from ._helpers import resolve_object_target from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction @@ -49,6 +48,7 @@ interpolate_hand_qpos, translate_pose_world, ) +from ._helpers import require_shared_task_state_key, resolve_object_target @dataclass(frozen=True, slots=True, eq=False) @@ -122,6 +122,8 @@ def __post_init__(self) -> None: class _CoordinatedPlacementResources: """Invocation-bound control parts and compatible hand commands.""" + placing_task_state_key: str + support_task_state_key: str placing_arm: JointPositionTarget support_arm: JointPositionTarget placing_hand: JointPositionTarget @@ -205,6 +207,21 @@ def _resolve_resources( support_arm = support_motion.require_target(JointPositionTarget) placing_hand = placing_grasp.require_target(JointPositionTarget) support_hand = support_grasp.require_target(JointPositionTarget) + placing_task_state_key = require_shared_task_state_key( + placing_motion, + placing_grasp, + participant="CoordinatedPlacement placing participant", + ) + support_task_state_key = require_shared_task_state_key( + support_motion, + support_grasp, + participant="CoordinatedPlacement support participant", + ) + if placing_task_state_key == support_task_state_key: + raise ValueError( + "CoordinatedPlacement placing and support participants must " + "use different task_state_key values." + ) if placing_arm.control_part == support_arm.control_part: raise ValueError( "CoordinatedPlacement placing and support roles must use " @@ -216,6 +233,8 @@ def _resolve_resources( "different end-effector control parts." ) return _CoordinatedPlacementResources( + placing_task_state_key=placing_task_state_key, + support_task_state_key=support_task_state_key, placing_arm=placing_arm, support_arm=support_arm, placing_hand=placing_hand, @@ -404,14 +423,14 @@ def _plan( ], dim=1, ) - involved_control_parts = { - resources.placing_arm.control_part, - resources.support_arm.control_part, + involved_task_state_keys = { + resources.placing_task_state_key, + resources.support_task_state_key, } coordinated_removals = { key: None for key in state.coordinated_held_objects - if not involved_control_parts.isdisjoint(key) + if not involved_task_state_keys.isdisjoint(key) } return self.build_plan( request, @@ -420,10 +439,10 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.placing_arm.control_part: ( + resources.placing_task_state_key: ( None if release else placing_held_object ), - resources.support_arm.control_part: support_held_object, + resources.support_task_state_key: support_held_object, }, coordinated_held_object_updates=coordinated_removals, ), @@ -507,20 +526,20 @@ def _resolve_target( HeldObjectState, HeldObjectState, ]: - placing_control_part = resources.placing_arm.control_part - support_control_part = resources.support_arm.control_part - placing_held_object = state.get_held_object(placing_control_part) + placing_task_state_key = resources.placing_task_state_key + support_task_state_key = resources.support_task_state_key + placing_held_object = state.get_held_object(placing_task_state_key) if placing_held_object is None: logger.log_error( - "CoordinatedPlacement requires an object held by placing control " - f"part {placing_control_part!r}.", + "CoordinatedPlacement requires an object held by placing " + f"task-state resource {placing_task_state_key!r}.", ValueError, ) - support_held_object = state.get_held_object(support_control_part) + support_held_object = state.get_held_object(support_task_state_key) if support_held_object is None: logger.log_error( - "CoordinatedPlacement requires an object held by support control " - f"part {support_control_part!r}.", + "CoordinatedPlacement requires an object held by support " + f"task-state resource {support_task_state_key!r}.", ValueError, ) placing_height_offset = ( diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 72c4bb0f4..87c98517d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -55,6 +55,7 @@ interpolate_hand_qpos, translate_pose_world, ) +from ._helpers import require_shared_task_state_key from .pick_up import GraspGoal @@ -140,6 +141,8 @@ def __post_init__(self) -> None: class _HandOverResources: """Invocation-bound control parts and compatible hand commands.""" + transfer_task_state_key: str + receive_task_state_key: str transfer_arm: JointPositionTarget receive_arm: JointPositionTarget transfer_hand: JointPositionTarget @@ -250,6 +253,21 @@ def _resolve_resources( receive_arm = receive_motion.require_target(JointPositionTarget) transfer_hand = transfer_grasp.require_target(JointPositionTarget) receive_hand = receive_grasp.require_target(JointPositionTarget) + transfer_task_state_key = require_shared_task_state_key( + transfer_motion, + transfer_grasp, + participant="HandOver source participant", + ) + receive_task_state_key = require_shared_task_state_key( + receive_motion, + receive_grasp, + participant="HandOver destination participant", + ) + if transfer_task_state_key == receive_task_state_key: + raise ValueError( + "HandOver source and destination must use different " + "task_state_key values." + ) if transfer_arm.control_part == receive_arm.control_part: raise ValueError( "HandOver source and destination must use different manipulator " @@ -261,6 +279,8 @@ def _resolve_resources( "control parts." ) return _HandOverResources( + transfer_task_state_key=transfer_task_state_key, + receive_task_state_key=receive_task_state_key, transfer_arm=transfer_arm, receive_arm=receive_arm, transfer_hand=transfer_hand, @@ -316,7 +336,7 @@ def _plan( semantics = target.semantics transfer_object_to_eef = self._resolve_transfer_object_to_eef( state, - resources.transfer_arm.control_part, + resources.transfer_task_state_key, semantics, ) transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( @@ -600,8 +620,8 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.transfer_arm.control_part: None, - resources.receive_arm.control_part: held_object, + resources.transfer_task_state_key: None, + resources.receive_task_state_key: held_object, } ), segment_lengths=segment_lengths, @@ -634,20 +654,20 @@ def _resolve_matrix(self, matrix: torch.Tensor, name: str) -> torch.Tensor: def _resolve_transfer_object_to_eef( self, state: PlanningContext, - transfer_control_part: str, + transfer_task_state_key: str, target_semantics: ObjectSemantics, ) -> torch.Tensor: - held = state.get_held_object(transfer_control_part) + held = state.get_held_object(transfer_task_state_key) if held is None: logger.log_error( - "HandOver requires an object held by transfer control part " - f"{transfer_control_part!r} (run PickUp first).", + "HandOver requires an object held by source task-state resource " + f"{transfer_task_state_key!r} (run PickUp first).", ValueError, ) if not _same_object_identity(target_semantics, held.semantics): raise ValueError( "HandOver target semantics must identify the object held by " - f"transfer control part {transfer_control_part!r}." + f"source task-state resource {transfer_task_state_key!r}." ) return self._resolve_matrix(held.object_to_eef, "held_object.object_to_eef") diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py index 7cb43860e..604795e0c 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -30,7 +30,11 @@ pose_inv, ) -from ._helpers import arm_qpos_from_state, resolve_object_target +from ._helpers import ( + arm_qpos_from_state, + require_shared_task_state_key, + resolve_object_target, +) from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, JointPositionCommand from ..core import AtomicAction @@ -146,6 +150,11 @@ def _plan( grasp = binding.endpoint("primary", "grasp") motion_target = motion.require_target(JointPositionTarget) grasp_target = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="MoveHeldObject primary participant", + ) control_part = motion_target.control_part arm_joint_ids = list(motion_target.joint_ids) hand_joint_ids = list(grasp_target.joint_ids) @@ -156,11 +165,11 @@ def _plan( dtype=context.robot.qpos.dtype, ) state = context - held_object = state.get_held_object(control_part) + held_object = state.get_held_object(task_state_key) if held_object is None: logger.log_error( - "MoveHeldObject requires an object held by control part " - f"{control_part!r} - run PickUp first.", + "MoveHeldObject requires an object held by task-state resource " + f"{task_state_key!r} - run PickUp first.", ValueError, ) object_target_pose = resolve_object_target( diff --git a/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py b/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py new file mode 100644 index 000000000..c40bcb491 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py @@ -0,0 +1,468 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Reusable contact-and-drag operation for articulated mechanisms.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import ClassVar + +import torch + +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand +from ..core import AtomicAction +from ..effects import StateDelta +from ..goals import SceneArticulationOperationGeometry +from ..invocation import ActionOptions, ResolvedActionRequest +from ..plans import ActionPlan, EffectVerificationRequirement, PlannerDiagnostics +from ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) +from ..state import ArticulationJointState, PlanningContext +from ..trajectory_ops import ( + build_pose_plan_states, + interpolate_hand_qpos, + resolve_pose_target, +) + + +def _validate_identifier(value: str, *, field_name: str) -> None: + """Validate one canonical articulation identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty canonical identifier.") + + +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulationGoal: + """Grounded interaction path and desired state for one articulation joint. + + The semantic compiler copies immutable affordance geometry and records the + live source joint position. The atomic planner combines those values with + the latest handle and joint observation, so the same resolved request can + safely replan drawers, doors, sliders, and similar interactions. + """ + + goal_kind: ClassVar[str] = "operate_articulation" + + articulation_id: str + """Canonical scene-registry articulation identifier.""" + + joint_id: str + """Canonical joint identifier within the articulation.""" + + geometry: SceneArticulationOperationGeometry + """Handle-relative geometry resolved again for every plan and replan.""" + + source_position: torch.Tensor + """Live joint position at semantic grounding, shape ``(1,)`` or ``(B, 1)``.""" + + target_position: torch.Tensor + """Absolute desired joint position, shape ``(1,)`` or ``(B, 1)``.""" + + target_displacement: float + """Signed handle displacement from source position to target position.""" + + def __post_init__(self) -> None: + _validate_identifier(self.articulation_id, field_name="articulation_id") + _validate_identifier(self.joint_id, field_name="joint_id") + if not isinstance(self.geometry, SceneArticulationOperationGeometry): + raise TypeError("geometry must be a SceneArticulationOperationGeometry.") + for field_name in ("source_position", "target_position"): + position = getattr(self, field_name) + if not isinstance(position, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if position.dim() not in (1, 2) or position.shape[-1:] != (1,): + raise ValueError(f"{field_name} must have shape (1,) or (B, 1).") + if not position.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + if not torch.isfinite(position).all(): + raise ValueError(f"{field_name} must contain only finite values.") + object.__setattr__(self, field_name, position.clone()) + displacement = self.target_displacement + if isinstance(displacement, bool) or not isinstance(displacement, (int, float)): + raise TypeError("target_displacement must be a finite scalar.") + displacement = float(displacement) + if not math.isfinite(displacement): + raise ValueError("target_displacement must be finite.") + object.__setattr__(self, "target_displacement", displacement) + + +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulationOptions(ActionOptions): + """Per-invocation contact sequencing for articulation operations.""" + + engage_steps: int = 5 + """Number of gripper-closing waypoints at the contact pose.""" + + release_steps: int = 5 + """Number of gripper-opening waypoints before retracting.""" + + def __post_init__(self) -> None: + for field_name in ("engage_steps", "release_steps"): + value = getattr(self, field_name) + if type(value) is not int or value <= 0: + raise ValueError(f"{field_name} must be a positive integer.") + + +class OperateArticulation( + AtomicAction[OperateArticulationGoal, OperateArticulationOptions] +): + """Approach, engage, move, release, and retract an articulated affordance.""" + + skill_id: ClassVar[str] = "operate_articulation" + GoalType: ClassVar[type] = OperateArticulationGoal + OptionsType: ClassVar[type] = OperateArticulationOptions + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + SkillEndpointRequirement( + endpoint_id="interaction", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + GRASP_COMMAND: JointPositionCommand, + OPEN_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "interaction")),), + ), + ), + ) + + def __init__( + self, + default_options: OperateArticulationOptions | None = None, + ) -> None: + super().__init__(default_options) + + def _on_bind(self) -> None: + """Capture immutable robot dimensions from engine-owned services.""" + self.n_envs = int(self.robot.get_qpos().shape[0]) + self.robot_dof = int(self.robot.dof) + + def _plan( + self, + request: ResolvedActionRequest[ + OperateArticulationGoal, + OperateArticulationOptions, + ], + context: PlanningContext, + ) -> ActionPlan: + """Plan the complete contact interaction from one observed context.""" + goal = self.require_goal(request) + options = request.skill_options + motion = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + interaction = request.binding.endpoint("primary", "interaction") + interaction_target = interaction.require_target(JointPositionTarget) + arm_joint_ids = list(motion.joint_ids) + interaction_joint_ids = list(interaction_target.joint_ids) + + remaining_displacement = self._remaining_displacement(goal, context) + poses = tuple( + resolve_pose_target( + pose, + n_envs=context.batch_size, + device=self.device, + ) + for pose in goal.geometry.resolve( + context, + displacement=remaining_displacement, + ) + ) + motion_counts = self._motion_sample_counts( + request.motion_policy.sample_count, + options, + ) + arm_segments: list[torch.Tensor] = [] + phase_diagnostics: dict[str, dict[str, object]] = {} + arm_start = context.robot.qpos[:, arm_joint_ids] + success = torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ) + phase_names = ("approach", "contact", "operate", "retract") + for phase_name, pose, sample_count in zip( + phase_names, + poses, + motion_counts, + strict=True, + ): + result = self.motion_generator.generate( + build_pose_plan_states(pose), + options=request.motion_policy.to_motion_gen_options( + start_qpos=arm_start, + control_part=motion.control_part, + sample_count=sample_count, + ), + ) + if result.positions is None or not isinstance(result.success, torch.Tensor): + return self.failed_plan( + request, + context, + message=( + "The articulation motion planner returned no trajectory for " + f"phase {phase_name!r}." + ), + ) + phase_success = result.success.to( + device=success.device, + dtype=torch.bool, + ) + failed_rows = ( + (~phase_success).nonzero(as_tuple=False).flatten().detach().cpu() + ) + phase_diagnostics[phase_name] = { + "success": phase_success.detach().cpu().tolist(), + "failed_rows": failed_rows.tolist(), + "waypoint_count": int(result.positions.shape[1]), + } + arm_segments.append(result.positions) + arm_start = result.positions[:, -1] + success &= phase_success + + grasp_qpos = interaction.joint_positions( + GRASP_COMMAND, + n_envs=self.n_envs, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + open_qpos = interaction.joint_positions( + OPEN_COMMAND, + n_envs=self.n_envs, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + initial_interaction = context.robot.qpos[:, interaction_joint_ids] + engage_path = interpolate_hand_qpos( + initial_interaction, + grasp_qpos, + n_waypoints=options.engage_steps, + ) + release_path = interpolate_hand_qpos( + grasp_qpos, + open_qpos, + n_waypoints=options.release_steps, + ) + + approach_arm, contact_arm, operation_arm, retract_arm = arm_segments + lengths = { + "approach": int(approach_arm.shape[1]), + "engage": int(contact_arm.shape[1] + engage_path.shape[1]), + "operate": int(operation_arm.shape[1]), + "release": int(release_path.shape[1]), + "retract": int(retract_arm.shape[1]), + } + full = torch.empty( + ( + context.batch_size, + sum(lengths.values()), + self.robot_dof, + ), + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + full[:] = context.robot.qpos.unsqueeze(1) + cursor = 0 + + def append_motion( + segment: torch.Tensor, + interaction_qpos: torch.Tensor, + ) -> None: + nonlocal cursor + count = int(segment.shape[1]) + full[:, cursor : cursor + count, arm_joint_ids] = segment + full[:, cursor : cursor + count, interaction_joint_ids] = ( + interaction_qpos.unsqueeze(1) + ) + cursor += count + + append_motion(approach_arm, initial_interaction) + append_motion(contact_arm, initial_interaction) + full[:, cursor : cursor + options.engage_steps, arm_joint_ids] = contact_arm[ + :, -1 + ].unsqueeze(1) + full[:, cursor : cursor + options.engage_steps, interaction_joint_ids] = ( + engage_path + ) + cursor += options.engage_steps + append_motion(operation_arm, grasp_qpos) + full[:, cursor : cursor + options.release_steps, arm_joint_ids] = operation_arm[ + :, -1 + ].unsqueeze(1) + full[:, cursor : cursor + options.release_steps, interaction_joint_ids] = ( + release_path + ) + cursor += options.release_steps + append_motion(retract_arm, open_qpos) + assert cursor == full.shape[1] + + target_position = goal.target_position.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + expected = StateDelta( + articulation_joint_updates={ + (goal.articulation_id, goal.joint_id): ArticulationJointState( + target_position + ) + } + ) + return self.build_plan( + request, + context, + success=success, + trajectory=full, + expected_effects=expected, + effect_verification=EffectVerificationRequirement( + kind="articulation.joint_progress" + ), + segment_lengths=lengths, + scene_dependency_monitor_until={ + goal.geometry.handle_pose.entity_id: lengths["approach"] + + lengths["engage"] + }, + diagnostics=PlannerDiagnostics( + backend=self.planning_services.planner_name, + messages=tuple( + f"Articulation motion phase {phase_name!r} failed for rows " + f"{details['failed_rows']}." + for phase_name, details in phase_diagnostics.items() + if details["failed_rows"] + ), + metadata={"motion_phases": phase_diagnostics}, + ), + ) + + @staticmethod + def _position_batch( + value: torch.Tensor, + context: PlanningContext, + *, + field_name: str, + ) -> torch.Tensor: + """Broadcast one scalar joint position to the planning batch.""" + position = value.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if position.shape == (1,): + return position.unsqueeze(0).expand(context.batch_size, -1).clone() + if position.shape != (context.batch_size, 1): + raise ValueError( + f"{field_name} must have shape (1,) or " f"({context.batch_size}, 1)." + ) + return position.clone() + + @classmethod + def _remaining_displacement( + cls, + goal: OperateArticulationGoal, + context: PlanningContext, + ) -> torch.Tensor: + """Map remaining joint stroke to a bounded signed handle displacement. + + For each row, ``remaining = target_displacement * clamp( + (target - current) / (target - source), 0, 1)``. A zero-length source + stroke, a reached target, and an overshot target all resolve to zero. + """ + observed = context.scene.get_articulation_joint_state( + goal.articulation_id, + goal.joint_id, + ) + address = (goal.articulation_id, goal.joint_id) + if observed is None: + raise ValueError( + "OperateArticulation recovery-safe planning requires a live " + f"ObservedArticulationJointState for {address!r}." + ) + current = cls._position_batch( + observed.position, + context, + field_name=f"observed articulation joint {address!r}", + ) + if observed.valid_mask is not None: + valid = observed.valid_mask.to(device=context.robot.qpos.device) + if not bool(valid.all()): + invalid_rows = (~valid).nonzero(as_tuple=False).flatten().tolist() + raise ValueError( + f"Live articulation joint {address!r} is invalid for planning " + f"rows {invalid_rows}." + ) + source = cls._position_batch( + goal.source_position, + context, + field_name="source_position", + ) + target = cls._position_batch( + goal.target_position, + context, + field_name="target_position", + ) + total = target - source + tolerance = torch.finfo(total.dtype).eps * 16.0 + nonzero_stroke = total.abs() > tolerance + fraction = torch.zeros_like(total) + fraction[nonzero_stroke] = ( + (target - current)[nonzero_stroke] / total[nonzero_stroke] + ).clamp(0.0, 1.0) + return fraction[:, 0] * goal.target_displacement + + @staticmethod + def _motion_sample_counts( + sample_count: int, + options: OperateArticulationOptions, + ) -> tuple[int, int, int, int]: + """Allocate the preset sample budget across four motion phases.""" + remaining = sample_count - options.engage_steps - options.release_steps + if remaining < 8: + raise ValueError( + "MotionPolicy.sample_count must leave at least two waypoints for " + "each articulation motion phase." + ) + base, remainder = divmod(remaining, 4) + counts = tuple(base + (1 if index < remainder else 0) for index in range(4)) + assert len(counts) == 4 and all(value >= 2 for value in counts) + return counts + + +__all__ = [ + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", +] diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index a5037d661..5e199a4ab 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -32,7 +32,7 @@ quat_from_matrix, ) -from ._helpers import arm_qpos_from_state +from ._helpers import arm_qpos_from_state, require_shared_task_state_key from ..affordance import AntipodalAffordance from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand @@ -352,6 +352,11 @@ def _plan( grasp = binding.endpoint("primary", "grasp") manipulator = motion.require_target(JointPositionTarget) end_effector = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="PickUp primary participant", + ) hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, n_envs=context.batch_size, @@ -441,7 +446,7 @@ def _plan( semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos ) coordinated_updates = { - key: None for key in state.coordinated_held_objects if control_part in key + key: None for key in state.coordinated_held_objects if task_state_key in key } return self.build_plan( request, @@ -449,10 +454,15 @@ def _plan( success=success_mask, trajectory=full, expected_effects=StateDelta( - held_object_updates={control_part: held}, + held_object_updates={task_state_key: held}, coordinated_held_object_updates=coordinated_updates, ), segment_lengths=segment_lengths, + scene_dependency_monitor_until=( + {} + if sem.entity_id is None + else {sem.entity_id: segment_lengths["approach"]} + ), ) def _resolve_grasp_pose( diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index b4678dd02..7e65ea1c7 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -27,7 +27,11 @@ from embodichain.utils import logger from embodichain.utils.math import quat_error_magnitude, quat_from_matrix -from ._helpers import arm_qpos_from_state, resolve_object_target +from ._helpers import ( + arm_qpos_from_state, + require_shared_task_state_key, + resolve_object_target, +) from ..affordance import AssembleAffordance from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand @@ -229,11 +233,15 @@ def _plan( target = self.require_goal(request) options = request.skill_options binding = request.binding - motion_target = binding.endpoint("primary", "motion").require_target( - JointPositionTarget - ) + motion = binding.endpoint("primary", "motion") grasp = binding.endpoint("primary", "grasp") + motion_target = motion.require_target(JointPositionTarget) grasp_target = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="Place primary participant", + ) control_part = motion_target.control_part arm_joint_ids = list(motion_target.joint_ids) hand_joint_ids = list(grasp_target.joint_ids) @@ -250,7 +258,7 @@ def _plan( dtype=context.robot.qpos.dtype, ) state = context - place_xpos = self._resolve_place_xpos(target, state, control_part) + place_xpos = self._resolve_place_xpos(target, state, task_state_key) if place_xpos.dim() == 3: place_xpos = place_xpos.unsqueeze(1) @@ -332,7 +340,7 @@ def _plan( full[:, n_down_actual + n_open :, hand_joint_ids] = hand_open_qpos.unsqueeze(1) coordinated_updates = { - key: None for key in state.coordinated_held_objects if control_part in key + key: None for key in state.coordinated_held_objects if task_state_key in key } return self.build_plan( request, @@ -340,7 +348,7 @@ def _plan( success=success, trajectory=full, expected_effects=StateDelta( - held_object_updates={control_part: None}, + held_object_updates={task_state_key: None}, coordinated_held_object_updates=coordinated_updates, ), segment_lengths={ @@ -354,13 +362,14 @@ def _resolve_place_xpos( self, target: PlaceGoal | AssembleGoal, state: PlanningContext, - control_part: str, + task_state_key: str, ) -> torch.Tensor: """Resolve the place EEF poses from a typed target. Args: target: Either an explicit EEF pose target or an assembly target. state: World state carrying the held-object transform. + task_state_key: Stable logical resource used for held-object state. Returns: Place EEF poses with shape ``(n_envs, 4, 4)`` or @@ -372,13 +381,13 @@ def _resolve_place_xpos( n_envs=self.n_envs, device=self.device, ) - return self._resolve_assemble_place_xpos(target, state, control_part) + return self._resolve_assemble_place_xpos(target, state, task_state_key) def _resolve_assemble_place_xpos( self, target: AssembleGoal, state: PlanningContext, - control_part: str, + task_state_key: str, ) -> torch.Tensor: """Derive the place EEF pose from an assembly affordance. @@ -389,6 +398,7 @@ def _resolve_assemble_place_xpos( Args: target: Assembly target carrying the base/assemble affordance. state: World state carrying the held-object transform. + task_state_key: Stable logical resource used for held-object state. Returns: Place EEF poses with shape ``(n_envs, 4, 4)``. @@ -396,11 +406,11 @@ def _resolve_assemble_place_xpos( Raises: ValueError: If no held object or base-pose source is available. """ - held = state.get_held_object(control_part) + held = state.get_held_object(task_state_key) if held is None: logger.log_error( - "Place with AssembleGoal requires an object held by control " - f"part {control_part!r} (run PickUp first).", + "Place with AssembleGoal requires an object held by task-state " + f"resource {task_state_key!r} (run PickUp first).", ValueError, ) affordance = target.affordance diff --git a/embodichain/lab/sim/atomic_actions/runtime.py b/embodichain/lab/sim/atomic_actions/runtime.py index c0530db40..13228ff37 100644 --- a/embodichain/lab/sim/atomic_actions/runtime.py +++ b/embodichain/lab/sim/atomic_actions/runtime.py @@ -98,12 +98,25 @@ def bind_control_parts( self, contract: SkillBindingContract, endpoints: Mapping[str, Mapping[str, str]], + *, + task_state_keys: Mapping[str, str] | None = None, ) -> ActionBinding: """Build a generic binding from explicit robot control-part names. This is the advanced direct-core construction path. Profile-backed callers obtain the same :class:`ActionBinding` from ``BoundRobotSkillProfile.resolve()``. + + Args: + contract: Typed endpoint contract for the bound skill. + endpoints: Nested ``slot_id -> endpoint_id -> control_part`` mapping. + task_state_keys: Optional explicit stable task-state key for each + resource slot. When omitted, a slot inherits its ``motion`` + endpoint's control part. A slot without ``motion`` can be + inferred only when all of its endpoints use one control part. + + Returns: + Engine-owned generic endpoint binding. """ if not isinstance(contract, SkillBindingContract): raise TypeError("contract must be a SkillBindingContract.") @@ -138,6 +151,36 @@ def bind_control_parts( "Direct binding must cover the skill contract exactly: " f"missing={missing}, extra={extra}." ) + slot_ids = {slot.slot_id for slot in contract.slots} + if task_state_keys is not None: + if not isinstance(task_state_keys, Mapping): + raise TypeError("task_state_keys must be a slot-to-key mapping.") + for slot_id, task_state_key in task_state_keys.items(): + if ( + not isinstance(slot_id, str) + or not slot_id + or slot_id != slot_id.strip() + ): + raise ValueError( + "task_state_keys slot IDs must be non-empty strings " + "without outer whitespace." + ) + if not isinstance(task_state_key, str) or not task_state_key.strip(): + raise ValueError( + "task_state_keys values must be non-empty strings." + ) + if task_state_key != task_state_key.strip(): + raise ValueError( + "task_state_keys values must not contain outer whitespace." + ) + supplied_task_slots = set(task_state_keys) + if supplied_task_slots != slot_ids: + missing = sorted(slot_ids - supplied_task_slots) + extra = sorted(supplied_task_slots - slot_ids) + raise ValueError( + "task_state_keys must cover the binding slots exactly: " + f"missing={missing}, extra={extra}." + ) if not expected: binding = ActionBinding(owner_id=self.binding_owner_id) self.validate_binding(binding, contract) @@ -147,6 +190,27 @@ def bind_control_parts( if not isinstance(control_parts, Mapping): raise TypeError("Direct control-part binding requires Robot.control_parts.") available = sorted(str(name) for name in control_parts) + resolved_task_state_keys: dict[str, str] + if task_state_keys is not None: + resolved_task_state_keys = dict(task_state_keys) + else: + resolved_task_state_keys = {} + for slot in contract.slots: + motion_key = (slot.slot_id, "motion") + if motion_key in supplied: + resolved_task_state_keys[slot.slot_id] = supplied[motion_key] + continue + slot_control_parts = { + supplied[(slot.slot_id, endpoint.endpoint_id)] + for endpoint in slot.endpoints + } + if len(slot_control_parts) != 1: + raise ValueError( + f"Direct binding slot {slot.slot_id!r} has no 'motion' " + "endpoint and spans multiple control parts; provide an " + "explicit task_state_keys entry for this slot." + ) + resolved_task_state_keys[slot.slot_id] = next(iter(slot_control_parts)) resolved: list[EndpointBinding] = [] for key, requirement in expected.items(): slot_id, endpoint_id = key @@ -175,6 +239,7 @@ def bind_control_parts( resource_id=f"direct.{slot_id}", adapter_id="control_part", target=JointPositionTarget(control_part, joint_ids), + task_state_key=resolved_task_state_keys[slot_id], capabilities=requirement.capabilities, commands=commands, claim_tokens=frozenset({f"robot.control_part:{control_part}"}), diff --git a/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index 69cf8d044..578a43d86 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -92,6 +92,80 @@ def _broadcast_pose( return value.clone() +def _broadcast_joint_position( + value: torch.Tensor, + *, + batch_size: int, + device: torch.device, + name: str, +) -> torch.Tensor: + """Resolve an optionally batched joint-position value to a task batch.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dim() == 1: + if value.numel() == 0: + raise ValueError(f"{name} must contain at least one joint value.") + value = value.unsqueeze(0).expand(batch_size, -1) + elif value.dim() != 2 or value.shape[0] != batch_size or value.shape[1] == 0: + raise ValueError( + f"{name} must have shape (n_joints,) or " f"({batch_size}, n_joints)." + ) + if not value.is_floating_point(): + raise TypeError(f"{name} must use a floating-point dtype.") + if value.device != device: + raise ValueError(f"{name} must use task-state device {device}.") + if not torch.isfinite(value).all(): + raise ValueError(f"{name} must contain only finite values.") + return value.clone() + + +@dataclass(frozen=True, slots=True, eq=False) +class ArticulationJointState: + """Verified symbolic state for one named articulation joint. + + ``position`` may describe one scalar joint or a multi-DoF joint. The + surrounding :class:`TaskState` supplies the stable articulation/joint key; + this value only owns row-local verified measurements and activity. + """ + + position: torch.Tensor + """Joint positions with shape ``(J,)`` or ``(B, J)``.""" + + env_mask: torch.Tensor | None = None + """Rows for which the verified state is present.""" + + def __post_init__(self) -> None: + if not isinstance(self.position, torch.Tensor): + raise TypeError("ArticulationJointState.position must be a tensor.") + if self.position.dim() not in (1, 2) or self.position.numel() == 0: + raise ValueError( + "ArticulationJointState.position must have shape (J,) or (B, J)." + ) + if not self.position.is_floating_point(): + raise TypeError("ArticulationJointState.position must be floating point.") + if not torch.isfinite(self.position).all(): + raise ValueError("ArticulationJointState.position must be finite.") + object.__setattr__(self, "position", self.position.clone()) + if self.env_mask is not None: + batch_size = int(self.position.shape[0]) if self.position.dim() == 2 else -1 + if batch_size <= 0: + if self.env_mask.dim() != 1 or self.env_mask.numel() == 0: + raise ValueError( + "ArticulationJointState.env_mask must be a non-empty vector." + ) + batch_size = int(self.env_mask.shape[0]) + object.__setattr__( + self, + "env_mask", + _normalize_mask( + self.env_mask, + batch_size=batch_size, + device=self.position.device, + name="ArticulationJointState.env_mask", + ), + ) + + @dataclass(frozen=True, slots=True, eq=False) class HeldObjectState: """Observed or projected relation between an object and one manipulator.""" @@ -252,6 +326,29 @@ def _normalize_coordinated_held( ) +def _normalize_articulation_joint( + value: ArticulationJointState, + *, + batch_size: int, + device: torch.device, +) -> ArticulationJointState: + """Normalize one articulation-joint state to a task-state batch.""" + return ArticulationJointState( + position=_broadcast_joint_position( + value.position, + batch_size=batch_size, + device=device, + name="ArticulationJointState.position", + ), + env_mask=_normalize_mask( + value.env_mask, + batch_size=batch_size, + device=device, + name="ArticulationJointState.env_mask", + ), + ) + + @dataclass(frozen=True, slots=True, eq=False) class TaskState: """Symbolic task state, separate from measured robot state.""" @@ -263,12 +360,17 @@ class TaskState: """Device used by per-environment masks and relation tensors.""" held_objects: Mapping[str, HeldObjectState] = field(default_factory=dict) - """Single-manipulator held-object relations keyed by control resource.""" + """Held-object relations keyed by stable logical task-state resource.""" coordinated_held_objects: Mapping[tuple[str, str], CoordinatedHeldObjectState] = ( field(default_factory=dict) ) - """Two-manipulator held-object relations keyed by ordered resource pairs.""" + """Coordinated relations keyed by ordered logical task-state resource pairs.""" + + articulation_joints: Mapping[tuple[str, str], ArticulationJointState] = field( + default_factory=dict + ) + """Verified articulation states keyed by canonical articulation and joint IDs.""" def __post_init__(self) -> None: if self.batch_size <= 0: @@ -303,6 +405,30 @@ def __post_init__(self) -> None: value, batch_size=self.batch_size, device=device ) + normalized_articulation: dict[tuple[str, str], ArticulationJointState] = {} + for key, value in self.articulation_joints.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(item) is str and item and item == item.strip() for item in key + ) + ): + raise TypeError( + "articulation_joints keys must be pairs of non-empty " + "canonical identifiers." + ) + if not isinstance(value, ArticulationJointState): + raise TypeError( + "articulation_joints values must be ArticulationJointState " + "objects." + ) + normalized_articulation[key] = _normalize_articulation_joint( + value, + batch_size=self.batch_size, + device=device, + ) + object.__setattr__(self, "device", device) object.__setattr__(self, "held_objects", MappingProxyType(normalized_held)) object.__setattr__( @@ -310,6 +436,11 @@ def __post_init__(self) -> None: "coordinated_held_objects", MappingProxyType(normalized_coordinated), ) + object.__setattr__( + self, + "articulation_joints", + MappingProxyType(normalized_articulation), + ) @classmethod def empty( @@ -340,6 +471,14 @@ def get_coordinated_held_object( """Return the relation for an ordered resource pair, if any.""" return self.coordinated_held_objects.get((first_resource, second_resource)) + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ArticulationJointState | None: + """Return verified state for one canonical articulation joint.""" + return self.articulation_joints.get((articulation_id, joint_id)) + @dataclass(frozen=True, slots=True, eq=False) class RobotObservation: @@ -424,6 +563,70 @@ def __post_init__(self) -> None: object.__setattr__(self, "pose", self.pose.clone()) +@dataclass(frozen=True, slots=True, eq=False) +class ObservedArticulationJointState: + """Live measured state for one scene articulation joint. + + This value belongs to :class:`SceneSnapshot`, not :class:`TaskState`. + ``ArticulationJointState`` records a verified symbolic effect after an + operation, while this class records the physical position used by online + grounding and recovery replans. + """ + + position: torch.Tensor + """Measured joint position with shape ``(J,)`` or ``(B, J)``.""" + + valid_mask: torch.Tensor | None = None + """Optional row-validity mask for a batched observation.""" + + def __post_init__(self) -> None: + position = self.position + if not isinstance(position, torch.Tensor): + raise TypeError("ObservedArticulationJointState.position must be a tensor.") + if position.dim() not in (1, 2) or position.numel() == 0: + raise ValueError( + "ObservedArticulationJointState.position must have shape (J,) " + "or (B, J)." + ) + if not position.is_floating_point(): + raise TypeError( + "ObservedArticulationJointState.position must be floating point." + ) + if not torch.isfinite(position).all(): + raise ValueError( + "ObservedArticulationJointState.position must contain only " + "finite values." + ) + object.__setattr__(self, "position", position.clone()) + if self.valid_mask is None: + return + valid_mask = self.valid_mask + if not isinstance(valid_mask, torch.Tensor): + raise TypeError( + "ObservedArticulationJointState.valid_mask must be a tensor or None." + ) + if position.dim() != 2: + raise ValueError( + "ObservedArticulationJointState.valid_mask requires a batched " + "position." + ) + if valid_mask.dtype != torch.bool or valid_mask.shape != (position.shape[0],): + raise ValueError( + "ObservedArticulationJointState.valid_mask must have shape (B,) " + "and dtype torch.bool." + ) + if valid_mask.device != position.device: + raise ValueError( + "ObservedArticulationJointState position and valid_mask must " + "share a device." + ) + object.__setattr__(self, "valid_mask", valid_mask.clone()) + + def snapshot(self) -> ObservedArticulationJointState: + """Return an independently owned observation value.""" + return ObservedArticulationJointState(self.position, self.valid_mask) + + class _ImmutableEntityMapping(Mapping[str, EntityState]): """Own entity states and return defensive copies on every public read.""" @@ -448,6 +651,34 @@ def __len__(self) -> int: return len(self._states) +class _ImmutableObservedArticulationJointMapping( + Mapping[tuple[str, str], ObservedArticulationJointState] +): + """Own live joint observations and copy values on every public read.""" + + __slots__ = ("_states",) + + def __init__( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> None: + self._states = MappingProxyType( + {key: state.snapshot() for key, state in states.items()} + ) + + def __getitem__( + self, + key: tuple[str, str], + ) -> ObservedArticulationJointState: + return self._states[key].snapshot() + + def __iter__(self) -> Iterator[tuple[str, str]]: + return iter(self._states) + + def __len__(self) -> int: + return len(self._states) + + @dataclass(frozen=True, slots=True, eq=False) class SceneSnapshot: """Versioned scene state used to ground dynamic goals and obstacles.""" @@ -461,6 +692,11 @@ class SceneSnapshot: collision_entity_ids: tuple[str, ...] = () """Entity IDs whose poses update a planner's dynamic collision world.""" + articulation_joints: Mapping[tuple[str, str], ObservedArticulationJointState] = ( + field(default_factory=dict) + ) + """Live physical joint observations keyed by articulation and joint ID.""" + def __post_init__(self) -> None: if self.timestamp < 0.0: raise ValueError("SceneSnapshot.timestamp must be non-negative.") @@ -498,6 +734,28 @@ def __post_init__(self) -> None: "SceneSnapshot entities must contain EntityState values." ) normalized[entity_id] = state + normalized_joints: dict[tuple[str, str], ObservedArticulationJointState] = {} + for key, state in self.articulation_joints.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(identifier) is str + and identifier + and identifier == identifier.strip() + for identifier in key + ) + ): + raise TypeError( + "SceneSnapshot articulation_joints keys must be canonical " + "(articulation_id, joint_id) pairs." + ) + if not isinstance(state, ObservedArticulationJointState): + raise TypeError( + "SceneSnapshot articulation_joints values must be " + "ObservedArticulationJointState objects." + ) + normalized_joints[key] = state collision_entity_ids = tuple(self.collision_entity_ids) if len(set(collision_entity_ids)) != len(collision_entity_ids) or not all( isinstance(entity_id, str) and entity_id @@ -513,8 +771,29 @@ def __post_init__(self) -> None: f"{sorted(missing)}." ) object.__setattr__(self, "entities", _ImmutableEntityMapping(normalized)) + object.__setattr__( + self, + "articulation_joints", + _ImmutableObservedArticulationJointMapping(normalized_joints), + ) object.__setattr__(self, "collision_entity_ids", collision_entity_ids) + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ObservedArticulationJointState | None: + """Return an owned live joint observation for a canonical address.""" + for value, field_name in ( + (articulation_id, "articulation_id"), + (joint_id, "joint_id"), + ): + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty canonical identifier." + ) + return self.articulation_joints.get((articulation_id, joint_id)) + def collision_world_revisions(self, batch_size: int) -> tuple[int, ...]: """Expand the collision revision to one value per environment. @@ -604,6 +883,18 @@ def __post_init__(self) -> None: f"Scene entity {entity_id!r} pose batch must match the " "planning context." ) + for ( + articulation_id, + joint_id, + ), state in self.scene.articulation_joints.items(): + if ( + state.position.dim() == 2 + and state.position.shape[0] != self.robot.batch_size + ): + raise ValueError( + f"Scene articulation joint ({articulation_id!r}, {joint_id!r}) " + "position batch must match the planning context." + ) if not isinstance(self.env_ids, torch.Tensor): raise TypeError("env_ids must be a torch.Tensor.") if self.env_ids.dtype != torch.long: @@ -653,6 +944,21 @@ def get_coordinated_held_object( """Return a coordinated held-object relation, if any.""" return self.task.get_coordinated_held_object(first_resource, second_resource) + @property + def articulation_joints( + self, + ) -> Mapping[tuple[str, str], ArticulationJointState]: + """Verified articulation-joint states.""" + return self.task.articulation_joints + + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ArticulationJointState | None: + """Return verified state for one canonical articulation joint.""" + return self.task.get_articulation_joint_state(articulation_id, joint_id) + def project( self, *, @@ -677,9 +983,11 @@ def project( __all__ = [ + "ArticulationJointState", "CoordinatedHeldObjectState", "EntityState", "HeldObjectState", + "ObservedArticulationJointState", "PlanningContext", "RobotObservation", "SceneSnapshot", diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 239258b13..d9128a149 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -1488,6 +1488,15 @@ def set_qf( data_type=ArticulationGPUAPIWriteType.JOINT_FORCE, ) + def get_qf(self) -> torch.Tensor: + """Get the current generalized efforts (qf) of the articulation. + + Returns: + torch.Tensor: Joint efforts with shape (N, dof), where N is the + number of environments. + """ + return self.body_data.qf + def get_qf_limits( self, joint_ids: Sequence[int] | torch.Tensor | None = None, diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 3cfa32029..b01d1891e 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -18,6 +18,7 @@ from __future__ import annotations +from dataclasses import replace from typing import TypeVar from unittest.mock import Mock @@ -61,6 +62,10 @@ MoveJoints, MoveJointsOptions, ObjectSemantics, + ObservedArticulationJointState, + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, PickUp, PickUpOptions, Place, @@ -71,6 +76,7 @@ PressGoal, PressOptions, RobotObservation, + SceneArticulationOperationGeometry, SceneEntityPose, SceneSnapshot, TaskState, @@ -256,16 +262,54 @@ def _target_scene( ) +def _articulation_geometry() -> SceneArticulationOperationGeometry: + """Build late-bound identity handle geometry for atomic tests.""" + identity = torch.eye(4) + return SceneArticulationOperationGeometry( + handle_pose=SceneEntityPose("drawer_handle"), + approach_offset=identity, + contact_offset=identity, + operation_offset=identity, + retract_offset=identity, + operation_axis=torch.tensor((1.0, 0.0, 0.0)), + ) + + +def _articulation_scene( + position: torch.Tensor, + *, + handle_x: float = 0.0, + timestamp: float = 0.0, + version: int = 0, +) -> SceneSnapshot: + """Build one live handle and articulation-joint snapshot.""" + handle = torch.eye(4).repeat(NUM_ENVS, 1, 1) + handle[:, 0, 3] = handle_x + return SceneSnapshot( + timestamp=timestamp, + version=version, + entities={"drawer_handle": EntityState(handle)}, + articulation_joints={ + ("drawer", "slide"): ObservedArticulationJointState(position) + }, + ) + + def _binding( action: AtomicAction, *, motion: str = "arm", grasp: str = "hand", + task_state_key: str | None = None, ) -> ActionBinding: """Bind one single-participant action through its owning engine.""" contract = type(action).__dict__.get("binding_contract") assert contract is not None - endpoint_parts = {"motion": motion, "grasp": grasp} + endpoint_parts = { + "motion": motion, + "grasp": grasp, + "interaction": grasp, + } return _ACTION_ENGINES[id(action)].bind_control_parts( action.skill_id, { @@ -275,6 +319,11 @@ def _binding( } for slot in contract.slots }, + task_state_keys=( + None + if task_state_key is None + else {slot.slot_id: task_state_key for slot in contract.slots} + ), ) @@ -441,6 +490,8 @@ def _dual_binding( action: AtomicAction, first_slot: str, second_slot: str, + *, + task_state_keys: dict[str, str] | None = None, ) -> ActionBinding: return _ACTION_ENGINES[id(action)].bind_control_parts( action.skill_id, @@ -454,6 +505,7 @@ def _dual_binding( "grasp": "right_hand", }, }, + task_state_keys=task_state_keys, ) @@ -472,6 +524,85 @@ def _sample(obj_poses: torch.Tensor, **_kwargs: object) -> list[dict]: affordance.get_dual_arm_valid_grasp_poses = Mock(side_effect=_sample) +def _plan_segment_contract_case(case_id: str) -> ActionPlan: + """Plan one built-in used by the Version 1 trajectory-segment contract.""" + generator = _motion_generator() + sample_count = 20 + + if case_id == "move_joints": + action = _bind_action(generator, MoveJoints()) + goal = JointPositionGoal(torch.zeros(ARM_DOF)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + if case_id == "move_end_effector": + action = _bind_action(generator, MoveEndEffector()) + goal = EndEffectorPoseGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + held_task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"arm": _held()}, + ) + if case_id == "move_held_object": + action = _bind_action(generator, MoveHeldObject()) + goal = HeldObjectPoseGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task), + ) + + if case_id == "place": + action = _bind_action(generator, Place()) + goal = PlaceGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task), + ) + + if case_id == "assemble": + action = _bind_action(generator, Place()) + goal = AssembleGoal( + affordance=AssembleAffordance( + base_object_entity=Mock(), + assemble_to_base_pose=torch.eye(4), + ), + base_pose=SceneEntityPose("base"), + ) + base_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"base": EntityState(base_pose)}, + ) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task, scene=scene), + ) + + if case_id == "press": + action = _bind_action(generator, Press()) + goal = PressGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + raise AssertionError(f"Unknown trajectory-segment contract case {case_id!r}.") + + def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: assert MoveEndEffector.GoalType is EndEffectorPoseGoal assert MoveJoints.GoalType is JointPositionGoal @@ -482,6 +613,34 @@ def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: assert CoordinatedPickment.GoalType is CoordinatedPickGoal assert CoordinatedPlacement.GoalType is CoordinatedPlacementGoal assert HandOver.GoalType is GraspGoal + assert OperateArticulation.GoalType is OperateArticulationGoal + + +@pytest.mark.parametrize( + ("case_id", "expected_names"), + ( + ("move_joints", ("move_joints",)), + ("move_end_effector", ("move_end_effector",)), + ("move_held_object", ("transport",)), + ("place", ("approach", "release", "retract")), + ("assemble", ("approach", "release", "retract")), + ("press", ("close", "press", "retract")), + ), +) +def test_builtin_trajectory_segment_names_and_ranges_are_stable( + case_id: str, + expected_names: tuple[str, ...], +) -> None: + plan = _plan_segment_contract_case(case_id) + + assert plan.success_all + assert tuple(segment.name for segment in plan.segments) == expected_names + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count @pytest.mark.parametrize( @@ -494,6 +653,7 @@ def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: CoordinatedPickmentOptions(), CoordinatedPlacementOptions(), HandOverOptions(), + OperateArticulationOptions(), ), ) def test_action_options_do_not_contain_embodiment_resources(options: object) -> None: @@ -612,13 +772,18 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: pick_plan = _plan_action( pick, - _invocation(pick, GraspGoal(semantics=semantics, grasp_xpos=grasp)), + ActionInvocation( + skill_id="pick_up", + goal=GraspGoal(semantics=semantics, grasp_xpos=grasp), + binding=_binding(pick, task_state_key="logical_arm"), + ), initial, ) picked_task = pick_plan.expected_effects.apply(initial.task, pick_plan.plan_success) - assert initial.task.get_held_object("arm") is None - assert picked_task.get_held_object("arm") is not None + assert initial.task.get_held_object("logical_arm") is None + assert picked_task.get_held_object("logical_arm") is not None + assert picked_task.get_held_object("arm") is None place = _bind_action(generator, Place()) picked_context = PlanningContext( @@ -629,15 +794,19 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: ) place_plan = _plan_action( place, - _invocation(place, PlaceGoal(torch.eye(4))), + ActionInvocation( + skill_id="place", + goal=PlaceGoal(torch.eye(4)), + binding=_binding(place, task_state_key="logical_arm"), + ), picked_context, ) placed_task = place_plan.expected_effects.apply( picked_task, place_plan.plan_success ) - assert picked_task.get_held_object("arm") is not None - assert placed_task.get_held_object("arm") is None + assert picked_task.get_held_object("logical_arm") is not None + assert placed_task.get_held_object("logical_arm") is None def test_move_held_object_requires_projected_attachment() -> None: @@ -648,6 +817,10 @@ def test_move_held_object_requires_projected_attachment() -> None: HeldObjectPoseGoal(torch.eye(4)), sample_count=10, ) + invocation = replace( + invocation, + binding=_binding(action, task_state_key="logical_arm"), + ) with pytest.raises(ValueError, match="requires an object held"): _plan_action(action, invocation, _context()) @@ -661,7 +834,7 @@ def test_move_held_object_requires_projected_attachment() -> None: task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"arm": held}, + held_objects={"logical_arm": held}, ) eef_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) eef_pose[:, :3, :3] = torch.tensor( @@ -674,7 +847,7 @@ def test_move_held_object_requires_projected_attachment() -> None: configured_invocation = ActionInvocation( skill_id="move_held_object", goal=HeldObjectPoseGoal(torch.eye(4)), - binding=_binding(action), + binding=_binding(action, task_state_key="logical_arm"), motion_policy=MotionPolicy(sample_count=10), skill_options=MoveHeldObjectOptions(pick_rotate_upright=0.25), ) @@ -683,6 +856,7 @@ def test_move_held_object_requires_projected_attachment() -> None: assert plan.plan_success.all() assert plan.expected_effects.is_empty + assert generator.robot.compute_fk.call_args.kwargs["name"] == "arm" current_object_pose = action._apply_configured_upright_rotation.call_args.args[2] assert torch.allclose( current_object_pose, @@ -706,6 +880,191 @@ def test_press_uses_invocation_sample_budget() -> None: assert plan.expected_effects.is_empty +def test_operate_articulation_builds_named_verified_interaction() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + plan = _plan_action( + action, + _invocation(action, goal, sample_count=16), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + assert plan.plan_success.tolist() == [True, True] + assert plan.commands.frame_count == 16 + assert tuple(segment.name for segment in plan.segments) == ( + "approach", + "engage", + "operate", + "release", + "retract", + ) + assert plan.requires_effect_verification + assert plan.scene_dependency_monitor_until == { + "drawer_handle": plan.segment("operate").start + } + assert plan.effect_verification is not None + assert plan.effect_verification.kind == "articulation.joint_progress" + update = plan.expected_effects.articulation_joint_updates[("drawer", "slide")] + assert update is not None + assert torch.equal(update.position, torch.tensor([0.4])) + interaction = _joint_command_positions(plan, "hand") + assert torch.all(interaction[:, -1] == 0.0) + assert torch.any(interaction == 1.0) + + +def test_operate_articulation_reports_per_phase_planning_failures() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + phase_results = [] + for index in range(4): + phase_results.append( + PlanResult( + success=torch.tensor([index != 2, True]), + positions=torch.zeros(NUM_ENVS, 3, ARM_DOF), + ) + ) + generator.generate = Mock(side_effect=phase_results) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + plan = _plan_action( + action, + _invocation(action, goal, sample_count=16), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + assert plan.plan_success.tolist() == [False, True] + assert plan.diagnostics.messages == ( + "Articulation motion phase 'operate' failed for rows [0].", + ) + phases = plan.diagnostics.metadata["motion_phases"] + assert phases["operate"] == { + "success": [False, True], + "failed_rows": [0], + "waypoint_count": 3, + } + + +def test_operate_articulation_replan_uses_fresh_handle_and_remaining_stroke() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([[0.0], [0.0]]), + target_position=torch.tensor([[0.4], [0.4]]), + target_displacement=0.4, + ) + invocation = _invocation(action, goal, sample_count=16) + initial_context = _context( + scene=_articulation_scene( + torch.tensor([[0.0], [0.0]]), + handle_x=0.3, + ) + ) + session = _ACTION_ENGINES[id(action)].start((invocation,), initial_context) + first_operation = generator.robot.compute_ik.call_args_list[2].kwargs["pose"] + assert torch.allclose(first_operation[:, 0, 3], torch.tensor([0.7, 0.7])) + session.tick(initial_context) + + generator.robot.compute_ik.reset_mock() + recovered = session.tick( + _context( + scene=_articulation_scene( + torch.tensor([[0.2], [0.4]]), + handle_x=0.55, + timestamp=1.0, + version=1, + ), + timestamp=1.0, + ), + ) + recovered_operation = generator.robot.compute_ik.call_args_list[2].kwargs["pose"] + + assert torch.allclose(recovered_operation[:, 0, 3], torch.tensor([0.75, 0.55])) + event_kinds = {event.kind for event in recovered.events} + assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in event_kinds + assert ExecutionEventKind.REPLANNED in event_kinds + assert session.trajectory_segment("operate").name == "operate" + + +def test_operate_articulation_requires_live_joint_observation() -> None: + generator = _motion_generator() + action = _bind_action(generator, OperateArticulation()) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + handle = torch.eye(4).repeat(NUM_ENVS, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer_handle": EntityState(handle)}, + ) + + with pytest.raises(ValueError, match="ObservedArticulationJointState"): + _plan_action( + action, + _invocation(action, goal, sample_count=20), + _context(scene=scene), + ) + + +def test_operate_articulation_rejects_insufficient_motion_budget() -> None: + generator = _motion_generator() + action = _bind_action(generator, OperateArticulation()) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + with pytest.raises(ValueError, match="at least two waypoints"): + _plan_action( + action, + _invocation(action, goal, sample_count=17), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + def test_strategy_and_sample_count_are_not_action_config_fields() -> None: with pytest.raises(TypeError): MoveEndEffectorOptions(strategy="motion_gen") # type: ignore[call-arg] @@ -877,6 +1236,9 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: "lift", ] assert plan.segment("close").stop == plan.segment("lift").start + assert plan.scene_dependency_monitor_until == { + "target": plan.segment("close").start + } def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: @@ -1024,7 +1386,91 @@ def test_pick_session_replans_when_late_bound_target_moves() -> None: assert ExecutionEventKind.REPLANNED in event_kinds -def test_pick_uses_binding_control_part_as_effect_resource() -> None: +@pytest.mark.parametrize( + ("waypoint_offset", "expects_replan"), + ((-1, True), (0, False)), +) +def test_pick_scene_monitoring_window_is_exclusive_at_close_boundary( + waypoint_offset: int, + expects_replan: bool, +) -> None: + """External motion replans before close, while grasp-induced motion does not.""" + generator = _motion_generator() + action = _bind_action(generator, PickUp()) + engine = _ACTION_ENGINES[id(action)] + initial_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + moved_pose = initial_pose.clone() + moved_pose[:, 1, 3] = 0.3 + invocation = _invocation( + action, + GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), + sample_count=20, + ) + task_state = TaskState.empty(batch_size=NUM_ENVS, device="cpu") + qpos = torch.zeros(NUM_ENVS, ROBOT_DOF) + + def context_at( + pose: torch.Tensor, + *, + timestamp: float, + version: int, + ) -> PlanningContext: + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=task_state, + scene=_target_scene(pose, timestamp=timestamp, version=version), + env_ids=torch.arange(NUM_ENVS), + ) + + session = engine.start( + (invocation,), context_at(initial_pose, timestamp=0.0, version=0) + ) + tick = session.tick(context_at(initial_pose, timestamp=0.0, version=0)) + close_start = session.plan_attempts[0].plan.segment("close").start + commands_to_issue = close_start + waypoint_offset + issued = 1 + while issued < commands_to_issue: + assert tick.command is not None + for command in tick.command.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + qpos[:, list(command.target.joint_ids)] = command.payload.positions + tick = session.tick( + context_at( + initial_pose, + timestamp=0.04 * issued, + version=0, + ) + ) + issued += 1 + + assert tick.command is not None + for command in tick.command.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + qpos[:, list(command.target.joint_ids)] = command.payload.positions + moved = session.tick( + context_at( + moved_pose, + timestamp=0.04 * commands_to_issue, + version=1, + ) + ) + + event_kinds = {event.kind for event in moved.events} + assert (ExecutionEventKind.DYNAMIC_GOAL_CHANGED in event_kinds) is expects_replan + assert (ExecutionEventKind.REPLANNED in event_kinds) is expects_replan + assert len(session.plan_attempts) == (2 if expects_replan else 1) + + +def test_pick_uses_logical_task_state_key_and_physical_control_target() -> None: generator = _motion_generator() action = _bind_action(generator, PickUp()) invocation = ActionInvocation( @@ -1037,6 +1483,7 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: action, motion="alternate_arm", grasp="alternate_hand", + task_state_key="logical_picker", ), motion_policy=MotionPolicy(sample_count=20), ) @@ -1052,8 +1499,74 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) - assert projected.get_held_object("alternate_arm") is not None + assert projected.get_held_object("logical_picker") is not None + assert projected.get_held_object("alternate_arm") is None assert projected.get_held_object("arm") is None + assert {target.target_id for target in plan.commands.targets} == { + "alternate_arm", + "alternate_hand", + } + + +def test_participant_motion_and_grasp_must_share_task_state_key() -> None: + generator = _motion_generator() + action = _bind_action(generator, PickUp()) + binding = _binding(action) + mismatched = ActionBinding( + owner_id=binding.owner_id, + endpoints=tuple( + ( + replace(endpoint, task_state_key="other_participant") + if endpoint.endpoint_id == "grasp" + else endpoint + ) + for endpoint in binding.endpoints + ), + ) + invocation = ActionInvocation( + skill_id="pick_up", + goal=GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), + binding=mismatched, + ) + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) + + with pytest.raises(ValueError, match="must share one task_state_key"): + _plan_action(action, invocation, context) + + +def test_handover_participants_must_use_distinct_task_state_keys() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + HandOver( + default_options=HandOverOptions( + middle_object_pose=torch.eye(4), + final_object_pose=torch.eye(4), + ) + ), + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=GraspGoal(semantics=_semantics()), + binding=_dual_binding( + action, + "source", + "destination", + task_state_keys={"source": "same", "destination": "same"}, + ), + ) + + with pytest.raises(ValueError, match="different task_state_key"): + _plan_action(action, invocation, _dual_context()) def test_press_closes_hand_without_changing_projected_attachment() -> None: @@ -1083,13 +1596,26 @@ def test_press_closes_hand_without_changing_projected_attachment() -> None: assert torch.equal(projected_held.object_to_eef, held.object_to_eef) -def test_handover_does_not_mutate_cached_final_pose() -> None: +@pytest.mark.parametrize( + ("hold_steps", "expected_segments"), + ( + (0, ("transfer", "approach", "close", "release", "deliver")), + ( + 2, + ("transfer", "approach", "close", "hold", "release", "deliver"), + ), + ), +) +def test_handover_does_not_mutate_cached_final_pose_and_omits_empty_hold( + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() handover_options = HandOverOptions( middle_object_pose=torch.eye(4), final_object_pose=torch.eye(4), hand_interp_steps=4, - hold_steps=2, + hold_steps=hold_steps, retreat_steps=5, ) action = _bind_action( @@ -1156,14 +1682,13 @@ def plan_from_start( ) assert torch.equal(handover_options.final_object_pose, original_final_pose) semantics.entity.get_local_pose.assert_not_called() - assert [segment.name for segment in plan.segments] == [ - "transfer", - "approach", - "close", - "hold", - "release", - "deliver", - ] + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_handover_replan_resolves_named_targets_from_latest_snapshot() -> None: @@ -1251,7 +1776,7 @@ def fail_second_receiving_arm( task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": _held(semantics)}, + held_objects={"logical_source": _held(semantics)}, ) action = _bind_action( generator, @@ -1275,7 +1800,15 @@ def fail_second_receiving_arm( invocation = ActionInvocation( skill_id="hand_over", goal=GraspGoal(semantics=semantics), - binding=_dual_binding(action, "source", "destination"), + binding=_dual_binding( + action, + "source", + "destination", + task_state_keys={ + "source": "logical_source", + "destination": "logical_destination", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) @@ -1291,9 +1824,10 @@ def fail_second_receiving_arm( context.robot.qpos[1].unsqueeze(0).expand(30, -1), ) assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) - received = projected.get_held_object("right_arm") + received = projected.get_held_object("logical_destination") assert received is not None assert received.env_mask.tolist() == [True, False] + assert projected.get_held_object("right_arm") is None semantics.entity.get_local_pose.assert_not_called() @@ -1328,14 +1862,24 @@ def test_handover_rejects_goal_for_a_different_held_object() -> None: goal_semantics.entity.get_local_pose.assert_not_called() -def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None: +@pytest.mark.parametrize( + ("hold_steps", "expected_segments"), + ( + (0, ("approach", "close", "lift", "move")), + (2, ("approach", "close", "lift", "move", "hold")), + ), +) +def test_coordinated_pick_returns_full_dof_plan_and_omits_empty_hold( + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() action = _bind_action( generator, CoordinatedPickment( default_options=CoordinatedPickmentOptions( hand_interp_steps=4, - hold_steps=2, + hold_steps=hold_steps, object_motion_keyframes=3, ), ), @@ -1358,7 +1902,15 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None object_target_pose=torch.eye(4), object_initial_pose=torch.eye(4), ), - binding=_dual_binding(action, "left", "right"), + binding=_dual_binding( + action, + "left", + "right", + task_state_keys={ + "left": "logical_left", + "right": "logical_right", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context() @@ -1377,19 +1929,20 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None } assert plan.scene_dependencies == () request.goal.semantics.entity.get_local_pose.assert_not_called() - assert projected.get_held_object("left_arm") is None - assert projected.get_held_object("right_arm") is None + assert projected.get_held_object("logical_left") is None + assert projected.get_held_object("logical_right") is None assert isinstance( - projected.get_coordinated_held_object("left_arm", "right_arm"), + projected.get_coordinated_held_object("logical_left", "logical_right"), CoordinatedHeldObjectState, ) - assert [segment.name for segment in plan.segments] == [ - "approach", - "close", - "lift", - "move", - "hold", - ] + assert projected.get_coordinated_held_object("left_arm", "right_arm") is None + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: @@ -1462,7 +2015,7 @@ def test_assemble_place_uses_explicit_base_snapshot() -> None: task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"arm": _held(_semantics(entity_id="assemble_object"))}, + held_objects={"logical_arm": _held(_semantics(entity_id="assemble_object"))}, ) base_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) base_pose[:, 0, 3] = torch.tensor([0.2, 0.4]) @@ -1476,12 +2029,15 @@ def test_assemble_place_uses_explicit_base_snapshot() -> None: ) request = action.resolve_request( - _invocation( - action, - AssembleGoal( - affordance=affordance, - base_pose=SceneEntityPose("base"), + replace( + _invocation( + action, + AssembleGoal( + affordance=affordance, + base_pose=SceneEntityPose("base"), + ), ), + binding=_binding(action, task_state_key="logical_arm"), ) ) plan = action.plan(request, context) @@ -1634,14 +2190,26 @@ def test_coordinated_pick_fails_when_affordance_has_no_grasp() -> None: ) -def test_coordinated_placement_projects_release_and_support_attachment() -> None: +@pytest.mark.parametrize( + ("release", "hold_steps", "expected_segments"), + ( + (False, 0, ("approach", "retreat")), + (True, 3, ("approach", "hold", "release", "retreat")), + ), +) +def test_coordinated_placement_projects_effects_and_omits_empty_segments( + release: bool, + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() action = _bind_action( generator, CoordinatedPlacement( default_options=CoordinatedPlacementOptions( + release=release, hand_interp_steps=4, - hold_steps=3, + hold_steps=hold_steps, retreat_steps=5, ), ), @@ -1655,7 +2223,10 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": placing, "right_arm": support}, + held_objects={ + "logical_placing": placing, + "logical_support": support, + }, ) invocation = ActionInvocation( skill_id="coordinated_placement", @@ -1663,7 +2234,15 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None placing_object_target_pose=torch.eye(4), support_object_target_pose=torch.eye(4), ), - binding=_dual_binding(action, "placing", "support"), + binding=_dual_binding( + action, + "placing", + "support", + task_state_keys={ + "placing": "logical_placing", + "support": "logical_support", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context(task) @@ -1679,15 +2258,23 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None "right_arm", "right_hand", } - assert projected.get_held_object("left_arm") is None - assert projected.get_held_object("right_arm") is not None - assert projected.get_held_object("right_arm").semantics is support.semantics - assert [segment.name for segment in plan.segments] == [ - "approach", - "hold", - "release", - "retreat", - ] + projected_placing = projected.get_held_object("logical_placing") + if release: + assert projected_placing is None + else: + assert projected_placing is not None + assert projected_placing.semantics is placing.semantics + assert torch.equal(projected_placing.object_to_eef, placing.object_to_eef) + assert projected.get_held_object("logical_support") is not None + assert projected.get_held_object("logical_support").semantics is support.semantics + assert projected.get_held_object("right_arm") is None + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_coordinated_placement_holds_only_environment_with_ik_failure() -> None: diff --git a/tests/sim/atomic_actions/test_articulation_effects.py b/tests/sim/atomic_actions/test_articulation_effects.py new file mode 100644 index 000000000..26603cb89 --- /dev/null +++ b/tests/sim/atomic_actions/test_articulation_effects.py @@ -0,0 +1,120 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for verified articulation state and masked symbolic effects.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + StateDelta, + TaskState, +) + + +def test_task_state_normalizes_and_owns_articulation_joint_state() -> None: + position = torch.tensor([0.35], dtype=torch.float32) + state = TaskState( + batch_size=2, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState(position), + }, + ) + + position.fill_(99.0) + observed = state.get_articulation_joint_state("drawer", "slide") + assert observed is not None + assert torch.equal(observed.position, torch.tensor([[0.35], [0.35]])) + assert torch.equal(observed.env_mask, torch.tensor([True, True])) + + +def test_state_delta_merges_articulation_rows_without_overwriting_others() -> None: + state = TaskState( + batch_size=3, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState( + torch.tensor([[0.0], [0.1], [0.2]]), + ) + }, + ) + candidate = ArticulationJointState( + torch.tensor([[0.5], [0.6], [0.7]]), + env_mask=torch.tensor([True, True, False]), + ) + + updated = StateDelta( + articulation_joint_updates={("drawer", "slide"): candidate} + ).apply(state, torch.tensor([True, False, True])) + + joint = updated.get_articulation_joint_state("drawer", "slide") + assert joint is not None + assert torch.equal(joint.position, torch.tensor([[0.5], [0.1], [0.7]])) + assert torch.equal(joint.env_mask, torch.tensor([True, True, False])) + + +def test_state_delta_removes_only_selected_articulation_rows() -> None: + state = TaskState( + batch_size=2, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + }, + ) + updated = StateDelta(articulation_joint_updates={("drawer", "slide"): None}).apply( + state, torch.tensor([False, True]) + ) + + joint = updated.get_articulation_joint_state("drawer", "slide") + assert joint is not None + assert torch.equal(joint.env_mask, torch.tensor([True, False])) + + removed = StateDelta(articulation_joint_updates={("drawer", "slide"): None}).apply( + updated, torch.tensor([True, False]) + ) + assert removed.get_articulation_joint_state("drawer", "slide") is None + + +def test_articulation_state_and_delta_validate_strictly() -> None: + with pytest.raises(TypeError, match="floating"): + ArticulationJointState(torch.tensor([1], dtype=torch.long)) + with pytest.raises(ValueError, match="finite"): + ArticulationJointState(torch.tensor([float("nan")])) + with pytest.raises(ValueError, match="articulation/joint pairs"): + StateDelta(articulation_joint_updates={("drawer", ""): None}) + with pytest.raises(TypeError, match="ArticulationJointState"): + StateDelta( + articulation_joint_updates={("drawer", "slide"): torch.tensor([0.1])} + ) + + +def test_articulation_state_delta_snapshot_is_independently_owned() -> None: + source = ArticulationJointState(torch.tensor([[0.2], [0.3]])) + delta = StateDelta(articulation_joint_updates={("drawer", "slide"): source}) + snapshot = delta.snapshot() + copied = snapshot.articulation_joint_updates[("drawer", "slide")] + + assert copied is not None + assert copied is not source + assert copied.position.data_ptr() != source.position.data_ptr() + assert torch.equal(copied.position, source.position) + + +__all__: list[str] = [] diff --git a/tests/sim/atomic_actions/test_control.py b/tests/sim/atomic_actions/test_control.py index 996944c09..f31408989 100644 --- a/tests/sim/atomic_actions/test_control.py +++ b/tests/sim/atomic_actions/test_control.py @@ -174,6 +174,64 @@ def test_control_profile_is_resolved_from_robot_control_part() -> None: ) +def test_direct_binding_shares_motion_task_state_key_across_slot_endpoints() -> None: + resolved = _binding(_services()) + + assert resolved.endpoint("primary", "motion").task_state_key == "arm" + assert resolved.endpoint("primary", "grasp").task_state_key == "arm" + + +def test_direct_binding_accepts_explicit_stable_task_state_key() -> None: + resolved = _services().bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + task_state_keys={"primary": "logical_manipulator"}, + ) + + assert {endpoint.task_state_key for endpoint in resolved.endpoints} == { + "logical_manipulator" + } + + +def test_direct_binding_without_motion_requires_unambiguous_task_state_key() -> None: + contract = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement(endpoint_id="grasp"), + SkillEndpointRequirement(endpoint_id="support"), + ), + ), + ) + ) + endpoints = {"primary": {"grasp": "hand", "support": "arm"}} + services = _services() + + with pytest.raises(ValueError, match="no 'motion'.*task_state_keys"): + services.bind_control_parts(contract, endpoints) + + resolved = services.bind_control_parts( + contract, + endpoints, + task_state_keys={"primary": "logical_manipulator"}, + ) + assert {endpoint.task_state_key for endpoint in resolved.endpoints} == { + "logical_manipulator" + } + + +def test_direct_binding_requires_exact_task_state_key_slot_coverage() -> None: + services = _services() + + with pytest.raises(ValueError, match="cover the binding slots exactly"): + services.bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + task_state_keys={}, + ) + + def test_invocation_override_replaces_only_resolved_endpoint_snapshot() -> None: services = _services() override_source = torch.full((2,), 0.4) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index eabb6359a..171ebd054 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -38,6 +38,7 @@ EndpointCommand, EndEffectorPoseGoal, EntityState, + EffectVerificationRequirement, ExecutionFeedbackMode, HeldObjectState, JointPositionPayload, @@ -196,6 +197,11 @@ def _action_plan( plan_success: torch.Tensor | None = None, joint_trajectory: TimedTrajectory | None = None, feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, + expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, + diagnostics: PlannerDiagnostics | None = None, + scene_dependencies: tuple[str, ...] = (), + scene_dependency_monitor_until: dict[str, int] | None = None, ) -> ActionPlan: if plan_success is None: plan_success = torch.ones( @@ -210,10 +216,71 @@ def _action_plan( recovery_policy=RecoveryPolicy(), planned_scene_version=0, planned_collision_world_revision=(0,) * commands.batch_size, - diagnostics=PlannerDiagnostics(backend="test"), + diagnostics=( + PlannerDiagnostics(backend="test") if diagnostics is None else diagnostics + ), feedback_mode=feedback_mode, joint_trajectory=joint_trajectory, + scene_dependencies=scene_dependencies, + scene_dependency_monitor_until=( + {} + if scene_dependency_monitor_until is None + else scene_dependency_monitor_until + ), + expected_effects=StateDelta() if expected_effects is None else expected_effects, + effect_verification=effect_verification, + ) + + +@pytest.mark.parametrize("kind", ("", " physical", "physical ", 1, True)) +def test_effect_verification_requirement_rejects_invalid_kind(kind: object) -> None: + with pytest.raises(ValueError, match="kind"): + EffectVerificationRequirement(kind=kind) # type: ignore[arg-type] + + +def test_action_plan_owns_explicit_effect_verification_requirement() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, ) + requirement = EffectVerificationRequirement(kind="articulation.joint_progress") + + plan = _action_plan(commands, effect_verification=requirement) + requirement_snapshot = plan.effect_verification + + assert plan.requires_effect_verification is True + assert requirement_snapshot is not None + assert requirement_snapshot is not requirement + assert requirement_snapshot.kind == requirement.kind + assert requirement_snapshot.snapshot() is not requirement_snapshot + + +def test_action_plan_implicitly_verifies_nonempty_state_delta() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + effects = StateDelta(held_object_updates={"arm": _held(batch_size=1)}) + + implicit = _action_plan(commands, expected_effects=effects) + no_effect = _action_plan(commands) + + assert implicit.effect_verification is None + assert implicit.requires_effect_verification is True + assert no_effect.requires_effect_verification is False + + +def test_action_plan_rejects_untyped_effect_verification_requirement() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + with pytest.raises(TypeError, match="EffectVerificationRequirement"): + _action_plan( + commands, + effect_verification=object(), # type: ignore[arg-type] + ) class _DependencyAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): @@ -704,6 +771,28 @@ def test_build_plan_uses_action_scene_dependency_hook() -> None: assert plan.scene_dependencies == ("extra", "tracked") +def test_build_segments_omits_zero_length_entry_and_preserves_offsets() -> None: + approach_length = 2 + release_length = 3 + segment_lengths = { + "approach": approach_length, + "hold": 0, + "release": release_length, + } + + segments = AtomicAction._build_segments( + segment_lengths, + frame_count=sum(segment_lengths.values()), + ) + + assert tuple( + (segment.name, segment.start, segment.stop) for segment in segments + ) == ( + ("approach", 0, approach_length), + ("release", approach_length, approach_length + release_length), + ) + + def test_build_command_plan_rejects_unbound_runtime_destination() -> None: context = _context() generator = Mock() @@ -878,6 +967,90 @@ def test_action_plan_owns_commands_and_optional_joint_trajectory() -> None: assert torch.equal(plan.joint_trajectory.positions, trajectory_positions) +def test_planner_diagnostics_and_plan_snapshots_own_nested_metadata() -> None: + nested = {"solver": {"iterations": [3, 5]}} + diagnostics = PlannerDiagnostics(backend="test", metadata=nested) + plan = _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ), + diagnostics=diagnostics, + ) + + nested["solver"]["iterations"][0] = 99 + diagnostics.metadata["solver"]["iterations"][1] = 77 + snapshot = plan.snapshot() + plan.diagnostics.metadata["solver"]["iterations"][0] = 42 + + assert snapshot.diagnostics.metadata["solver"]["iterations"] == [3, 5] + + +def test_planner_diagnostics_rejects_non_string_messages() -> None: + with pytest.raises(TypeError, match="messages must contain strings"): + PlannerDiagnostics( + backend="test", + messages=("valid", 1), # type: ignore[arg-type] + ) + + +def test_action_plan_owns_scene_dependency_monitor_cutoffs() -> None: + source = {"disabled": 0, "full_sequence": 2} + plan = _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("disabled", "full_sequence"), + scene_dependency_monitor_until=source, + ) + + source["disabled"] = 1 + source["full_sequence"] = 1 + snapshot = plan.snapshot() + + assert plan.scene_dependency_monitor_until == { + "disabled": 0, + "full_sequence": 2, + } + assert snapshot.scene_dependency_monitor_until == { + "disabled": 0, + "full_sequence": 2, + } + assert snapshot.scene_dependency_monitor_until is not ( + plan.scene_dependency_monitor_until + ) + + +@pytest.mark.parametrize("waypoint_index", (-1, 3, True, 1.5)) +def test_action_plan_rejects_invalid_scene_dependency_monitor_cutoff( + waypoint_index: object, +) -> None: + with pytest.raises(ValueError, match="waypoint indices"): + _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("tracked",), + scene_dependency_monitor_until={ + "tracked": waypoint_index # type: ignore[dict-item] + }, + ) + + +def test_action_plan_rejects_monitor_cutoff_for_non_dependency() -> None: + with pytest.raises(ValueError, match="keys must be scene dependencies"): + _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("tracked",), + scene_dependency_monitor_until={"other": 1}, + ) + + def test_action_plan_allows_timed_commands_without_joint_trajectory() -> None: commands = _command_sequence( env_ids=torch.tensor([4], dtype=torch.long), diff --git a/tests/sim/atomic_actions/test_engine.py b/tests/sim/atomic_actions/test_engine.py index eed03fad8..0cee75606 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -331,11 +331,14 @@ def test_engine_resolves_action_binding_from_robot_control_parts() -> None: resolved = engine.bind_control_parts( "stub", {"primary": {"motion": "all"}}, + task_state_keys={"primary": "logical_robot"}, ) - target = resolved.endpoint("primary", "motion").require_target(JointPositionTarget) + endpoint = resolved.endpoint("primary", "motion") + target = endpoint.require_target(JointPositionTarget) assert target.control_part == "all" assert target.joint_ids == (0, 1, 2) + assert endpoint.task_state_key == "logical_robot" def test_engine_resolves_invocation_control_override_into_request() -> None: diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index eb2dcf6cb..f71f2179f 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -20,6 +20,7 @@ from collections.abc import Sequence from dataclasses import replace +import math from typing import ClassVar from unittest.mock import Mock @@ -43,6 +44,7 @@ ExecutionSession, ExecutionStatus, ExecutionTick, + EffectVerificationRequirement, EffectVerificationResult, GraspGoal, HeldObjectState, @@ -50,6 +52,7 @@ JointPositionTarget, MotionPolicy, ObjectSemantics, + PlannerDiagnostics, PlanningContext, RecoveryPolicy, ResolvedActionRequest, @@ -138,6 +141,31 @@ def _plan( ) +class VerificationOnlyAction(DynamicAction): + """Dynamic action requiring a physical check without symbolic effects.""" + + skill_id: ClassVar[str] = "verification_only" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + pose = resolve_pose_goal(goal.xpos, context, name="xpos") + target = pose[:, 0, 3].unsqueeze(1).expand_as(context.robot.qpos) + return self.build_plan( + request, + context, + success=True, + trajectory=torch.stack([context.robot.qpos, target], dim=1), + effect_verification=EffectVerificationRequirement( + kind="physical.test_completion" + ), + ) + + class FailedEffectAction(EffectAction): """Effect-declaring action whose planner fails for every environment.""" @@ -153,6 +181,65 @@ def _plan( return replace(plan, plan_success=torch.zeros_like(plan.plan_success)) +class DiagnosticAction(DynamicAction): + """Dynamic action exposing its installed plan for snapshot isolation tests.""" + + skill_id: ClassVar[str] = "diagnostic" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def __init__(self) -> None: + super().__init__() + self.metadata = {"solver": {"iterations": [3, 5]}} + self.returned_plans: list[ActionPlan] = [] + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + plan = replace( + plan, + diagnostics=PlannerDiagnostics( + backend="diagnostic", + metadata=self.metadata, + ), + ) + self.returned_plans.append(plan) + return plan + + +class WindowedDependencyAction(DynamicAction): + """Stop monitoring a goal pose once its first command was issued.""" + + skill_id: ClassVar[str] = "windowed_dependency" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + return replace( + plan, + scene_dependency_monitor_until={"target": 1}, + ) + + +class MultiDependencyAction(DynamicAction): + """Track the goal plus one auxiliary scene dependency.""" + + skill_id: ClassVar[str] = "multi_dependency" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _scene_dependencies( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + ) -> tuple[str, ...]: + return tuple(sorted((*super()._scene_dependencies(request), "obstacle"))) + + class MixedEffectAction(EffectAction): """Effect action whose final environment row always fails planning.""" @@ -378,6 +465,41 @@ def _context( ) +def _multi_dependency_context( + timestamp: float, + *, + target_x: float, + obstacle_x: float | None, + version: int, + target_yaw: float = 0.0, +) -> PlanningContext: + """Build one-row context with an optional auxiliary dependency.""" + context = _context(timestamp, 0.0, target_x, version) + entities = dict(context.scene.entities) + target_pose = entities["target"].pose + cosine = math.cos(target_yaw) + sine = math.sin(target_yaw) + target_pose[:, 0, 0] = cosine + target_pose[:, 0, 1] = -sine + target_pose[:, 1, 0] = sine + target_pose[:, 1, 1] = cosine + entities["target"] = EntityState(target_pose) + if obstacle_x is not None: + obstacle_pose = torch.eye(4).unsqueeze(0) + obstacle_pose[:, 0, 3] = obstacle_x + entities["obstacle"] = EntityState(obstacle_pose) + return PlanningContext( + robot=context.robot, + task=context.task, + scene=SceneSnapshot( + timestamp=timestamp, + version=version, + entities=entities, + ), + env_ids=context.env_ids, + ) + + def _collision_context( timestamp: float, qpos: torch.Tensor, @@ -460,6 +582,7 @@ def _destination_invocation( "second": "arm_b", } }, + task_state_keys={"primary": "destination_resource"}, ), recovery_policy=RecoveryPolicy( max_replans=2, @@ -476,7 +599,7 @@ def _effect_session( max_action_retries: int = 2, action_timeout: float = 30.0, eligible_mask: torch.Tensor | None = None, - action: EffectAction | None = None, + action: DynamicAction | None = None, ) -> tuple[ExecutionSession, ExecutionTick]: """Advance a test effect action to its verification boundary.""" engine, _ = _engine(batch_size=batch_size) @@ -532,6 +655,109 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +def test_all_rows_planning_failure_skips_inactive_command_frames() -> None: + engine, _ = _engine() + action = FailedEffectAction() + engine.register(action) + base = _invocation(engine, max_action_retries=0) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + + failed = session.tick(_context(0.0, 0.0, 0.2, 0)) + + assert failed.command is None + assert failed.status is ExecutionStatus.FAILED + assert failed.eligible_mask.tolist() == [False] + assert any( + event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + for event in failed.events + ) + + +def test_plan_attempt_records_snapshot_nested_metadata_at_installation() -> None: + engine, _ = _engine() + action = DiagnosticAction() + engine.register(action) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + + action.metadata["solver"]["iterations"][0] = 99 + action.returned_plans[0].diagnostics.metadata["solver"]["iterations"][1] = 77 + first_read = session.plan_attempts[0] + first_read.plan.diagnostics.metadata["solver"]["iterations"][0] = 42 + second_read = session.plan_attempts[0] + + assert second_read.plan.diagnostics.metadata["solver"]["iterations"] == [3, 5] + + +def test_scene_dependency_window_ignores_expected_self_motion() -> None: + engine, _ = _engine() + action = WindowedDependencyAction() + engine.register(action) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + + first = session.tick(_context(0.0, 0.0, 0.2, 0)) + moved = session.tick(_context(0.1, 0.0, 0.8, 1)) + + assert first.command is not None + assert moved.command is not None + assert action.plan_count == 1 + assert not any( + event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED for event in moved.events + ) + + +def test_scene_dependency_window_reports_motion_before_cutoff() -> None: + engine, _ = _engine() + action = WindowedDependencyAction() + engine.register(action) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + + moved = session.tick(_context(0.1, 0.0, 0.8, 1)) + + changed = next( + event + for event in moved.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=0: " + "entity_id='target', monitor_cutoff=1, max_translation=0.600000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266." + ) + assert action.plan_count == 2 + + def test_initial_eligibility_is_sticky_across_invocation_barriers() -> None: engine, _ = _engine(batch_size=2) invocation = _invocation(engine) @@ -706,13 +932,104 @@ def test_scene_motion_replans_late_bound_goal() -> None: tick = session.tick(_context(0.1, 0.0, 0.3, 1)) kinds = {event.kind for event in tick.events} + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in kinds assert ExecutionEventKind.REPLANNED in kinds + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='target', monitor_cutoff=none, max_translation=0.200000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266." + ) assert action.plan_count == 2 assert action.requests[0] is action.requests[1] assert tick.command is not None +def test_scene_motion_diagnostic_orders_multiple_changed_entities() -> None: + engine, _ = _engine() + action = MultiDependencyAction() + engine.register(action) + initial = _multi_dependency_context( + 0.0, + target_x=0.1, + obstacle_x=0.4, + version=0, + ) + session = engine.start( + (_invocation(engine, skill_id=action.skill_id),), + initial, + ) + session.tick(initial) + + tick = session.tick( + _multi_dependency_context( + 0.1, + target_x=0.4, + obstacle_x=0.8, + version=1, + target_yaw=0.2, + ) + ) + + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='obstacle', monitor_cutoff=none, max_translation=0.400000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266 | " + "entity_id='target', monitor_cutoff=none, max_translation=0.300000, " + "translation_threshold=0.020000, max_rotation=0.200000, " + "rotation_threshold=0.087266." + ) + + +def test_scene_motion_diagnostic_identifies_missing_entity() -> None: + engine, _ = _engine() + action = MultiDependencyAction() + engine.register(action) + initial = _multi_dependency_context( + 0.0, + target_x=0.1, + obstacle_x=0.4, + version=0, + ) + session = engine.start( + (_invocation(engine, skill_id=action.skill_id),), + initial, + ) + session.tick(initial) + + tick = session.tick( + _multi_dependency_context( + 0.1, + target_x=0.1, + obstacle_x=None, + version=1, + ) + ) + + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='obstacle', monitor_cutoff=none, missing=current_scene, " + "max_translation=unavailable, translation_threshold=0.020000, " + "max_rotation=unavailable, rotation_threshold=0.087266." + ) + + def test_recovery_replan_rejects_runtime_destination_change() -> None: engine, action = _destination_engine(("first", "second")) invocation = _destination_invocation(engine) @@ -794,6 +1111,18 @@ def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: latest_obstacles = generator.bind_collision_world.call_args.kwargs["obstacle_poses"] assert latest_obstacles["obstacle"][0, 0, 3] == pytest.approx(0.6) assert tick.command is not None + attempts = session.plan_attempts + assert [attempt.attempt_generation for attempt in attempts] == [0, 1] + assert [attempt.event_kind for attempt in attempts] == [ + ExecutionEventKind.ACTION_PLANNED, + ExecutionEventKind.REPLANNED, + ] + assert [attempt.plan.planned_scene_version for attempt in attempts] == [0, 1] + assert [attempt.plan.planned_collision_world_revision for attempt in attempts] == [ + (0,), + (1,), + ] + assert [attempt.replan_counts for attempt in attempts] == [(0,), (1,)] def test_collision_world_exhaustion_only_disables_changed_environment() -> None: @@ -1107,10 +1436,15 @@ def test_session_revision_replans_from_latest_context() -> None: session.revise_current(revised) first = session.tick(_context(0.0, 0.0, 0.1, 0)) second = session.tick(_context(0.1, 0.0, 0.1, 0)) + attempts = session.plan_attempts assert action.plan_count == 2 assert action.requests[0] is not action.requests[1] assert [request.revision for request in action.requests] == [0, 1] + assert [attempt.request.revision for attempt in attempts] == [0, 1] + assert attempts[0].request is not attempts[1].request + assert attempts[1].request.motion_policy == revised.motion_policy + assert attempts[1].request.recovery_policy == revised.recovery_policy assert any( event.kind is ExecutionEventKind.INVOCATION_REVISED and event.invocation_revision == 1 @@ -1298,6 +1632,159 @@ def test_session_rejects_regressing_collision_world_revision() -> None: session.tick(regressed) +def test_explicit_verification_with_empty_delta_preserves_task_state() -> None: + session, waiting = _effect_session(action=VerificationOnlyAction()) + request = waiting.pending_effect + assert request is not None + assert request.expected_effects.is_empty + assert request.effect_verification is not None + assert request.effect_verification.kind == "physical.test_completion" + initial_task_state = waiting.task_state + + preserved = session.pending_effect + assert preserved is not None + assert preserved.effect_verification is not None + assert preserved.effect_verification is not request.effect_verification + with pytest.raises(ValueError, match="explicit physical-effect requirement"): + replace(request, effect_verification=None) + + completed = session.tick( + _context(0.21, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.pending_effect is None + assert completed.task_state is initial_task_state + assert not completed.task_state.held_objects + + +def test_explicit_verification_keeps_partial_and_retry_row_lifecycle() -> None: + session, waiting = _effect_session( + batch_size=2, + max_action_retries=1, + action=VerificationOnlyAction(), + ) + first_request = waiting.pending_effect + assert first_request is not None + initial_task_state = waiting.task_state + + retry = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, True]), + ), + ) + + assert retry.pending_effect is None + assert retry.task_state is initial_task_state + assert any( + event.kind is ExecutionEventKind.ACTION_RETRY + and event.env_mask.tolist() == [False, True] + for event in retry.events + ) + + first_command = session.tick(_context(0.22, (0.2, 0.2), (0.2, 0.2), 0)) + second_command = session.tick(_context(0.23, (0.2, 0.2), (0.2, 0.2), 0)) + second_wait = session.tick(_context(0.24, (0.2, 0.2), (0.2, 0.2), 0)) + assert first_command.command is not None + assert first_command.command.active_mask.tolist() == [False, True] + assert second_command.command is not None + assert second_command.command.active_mask.tolist() == [False, True] + second_request = second_wait.pending_effect + assert second_request is not None + assert second_request.env_mask.tolist() == [False, True] + assert second_request.verification_id != first_request.verification_id + assert second_request.deadline > first_request.deadline + + completed = session.tick( + _context(0.25, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + second_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.eligible_mask.tolist() == [True, True] + assert completed.task_state is initial_task_state + + +def test_explicit_verification_partial_success_shrinks_request_without_state_delta() -> ( + None +): + session, waiting = _effect_session( + batch_size=2, + action=VerificationOnlyAction(), + ) + first_request = waiting.pending_effect + assert first_request is not None + initial_task_state = waiting.task_state + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + second_request = partial.pending_effect + assert second_request is not None + assert second_request.env_mask.tolist() == [False, True] + assert second_request.verification_id != first_request.verification_id + assert second_request.requested_at == first_request.requested_at + assert second_request.deadline == first_request.deadline + assert second_request.effect_verification is not None + assert second_request.effect_verification.kind == "physical.test_completion" + assert partial.task_state is initial_task_state + + completed = session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + second_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.task_state is initial_task_state + + +def test_explicit_verification_empty_delta_obeys_action_timeout() -> None: + session, waiting = _effect_session( + max_action_retries=0, + action_timeout=0.25, + action=VerificationOnlyAction(), + ) + request = waiting.pending_effect + assert request is not None + initial_task_state = waiting.task_state + + timed_out = session.tick(_context(0.3, 0.2, 0.2, 0)) + + assert timed_out.status is ExecutionStatus.FAILED + assert timed_out.pending_effect is None + assert timed_out.task_state is initial_task_state + assert any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT + for event in timed_out.events + ) + assert any( + event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + for event in timed_out.events + ) + + def test_nonempty_effect_is_committed_only_after_external_verification() -> None: engine, _ = _engine() effect = EffectAction() @@ -1989,12 +2476,10 @@ def test_failed_effect_plan_retries_without_requesting_effect_verification() -> recovery_policy=base.recovery_policy, ) session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) - session.tick(_context(0.0, 0.0, 0.2, 0)) - session.tick(_context(0.1, 0.0, 0.2, 0)) - - failed = session.tick(_context(0.2, 0.0, 0.2, 0)) + failed = session.tick(_context(0.0, 0.0, 0.2, 0)) assert failed.status is ExecutionStatus.FAILED + assert failed.command is None assert failed.pending_effect is None assert not any( event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index 8a731a6f8..e294f3c0a 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -17,8 +17,10 @@ from __future__ import annotations import os -import torch +from types import SimpleNamespace + import pytest +import torch from embodichain.lab.sim import ( SimulationManager, @@ -41,6 +43,16 @@ NUM_ARENAS = 10 +def test_get_qf_returns_all_articulation_joint_efforts(): + expected_qf = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], dtype=torch.float32) + articulation = object.__new__(Articulation) + articulation._data = SimpleNamespace(qf=expected_qf) + + actual_qf = articulation.get_qf() + + assert torch.equal(actual_qf, expected_qf) + + def _link_static_friction(art: Articulation, link_name: str, env_idx: int = 0) -> float: return art._entities[env_idx].get_physical_attr(link_name).static_friction diff --git a/tests/sim/objects/test_robot.py b/tests/sim/objects/test_robot.py index 876781a60..e6e050f91 100644 --- a/tests/sim/objects/test_robot.py +++ b/tests/sim/objects/test_robot.py @@ -17,9 +17,11 @@ from __future__ import annotations import os -import torch -import pytest +from types import SimpleNamespace + import numpy as np +import pytest +import torch from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.objects import Robot @@ -49,6 +51,20 @@ } +def test_get_qf_selects_control_part_joint_efforts(): + full_qf = torch.tensor( + [[1.0, 2.0, 3.0, 4.0], [5.0, 6.0, 7.0, 8.0]], dtype=torch.float32 + ) + robot = object.__new__(Robot) + robot._data = SimpleNamespace(qf=full_qf) + robot.cfg = SimpleNamespace(control_parts={"arm": ["joint_3", "joint_1"]}) + robot._joint_ids = {"arm": [3, 1]} + + actual_qf = robot.get_qf(name="arm") + + assert torch.equal(actual_qf, full_qf[:, [3, 1]]) + + # Base test class for CPU and CUDA class BaseRobotTest: @classmethod From 974368f92967ce7ebf7d148a1c67dfc47333092c Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:18:03 +0800 Subject: [PATCH 17/28] feat(sim): add semantic runtime effects and parallelism --- .../embodichain.lab.sim.skills.rst | 104 + .../atomic_actions/robot_skill_profiles.md | 49 + embodichain/lab/sim/skills/__init__.py | 216 +- embodichain/lab/sim/skills/calls.py | 148 +- embodichain/lab/sim/skills/compiler.py | 751 +++++- embodichain/lab/sim/skills/effects.py | 2250 ++++++++++++++++ embodichain/lab/sim/skills/evidence.py | 1467 +++++++++++ embodichain/lab/sim/skills/integration.py | 152 +- embodichain/lab/sim/skills/parallel.py | 354 +++ .../lab/sim/skills/parallel_runtime.py | 1487 +++++++++++ embodichain/lab/sim/skills/profiles.py | 154 +- embodichain/lab/sim/skills/runtime.py | 2294 +++++++++++++++++ embodichain/lab/sim/skills/scene.py | 265 +- .../sim/skills/test_articulation_semantics.py | 594 +++++ tests/sim/skills/test_calls.py | 20 + tests/sim/skills/test_compiler.py | 480 +++- ...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 | 910 +++++++ tests/sim/skills/test_scene.py | 111 +- 25 files changed, 15640 insertions(+), 59 deletions(-) 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 create mode 100644 embodichain/lab/sim/skills/runtime.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 create mode 100644 tests/sim/skills/test_runtime.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 1b3022fe8..979bc9314 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -43,6 +43,38 @@ embodichain.lab.sim.skills UnsupportedSkillError AmbiguousSkillBindingError + .. rubric:: Semantic calls and runtime + + .. autosummary:: + + SemanticCallSpec + SemanticPose + Pick + Place + HandOver + OperateArticulation + RegisteredSemanticCall + SemanticCallCatalog + SemanticSkillCompiler + AtomicSkills + SkillRuntime + SkillResult + SkillCallTrace + SkillPlanAttemptTrace + SkillEffectTrace + + .. rubric:: Effects, evidence, and parallel execution + + .. autosummary:: + + SemanticEffectSpec + EffectMonitorRef + EffectMonitor + EffectEvidenceCollector + ParallelSkillRuntime + ParallelSkillResult + ParallelCommandSafetyValidator + .. currentmodule:: embodichain.lab.sim.skills Robot resources and profiles @@ -99,6 +131,78 @@ Profile errors .. autoclass:: AmbiguousSkillBindingError +Semantic calls and runtime +-------------------------- + +.. autoclass:: SemanticCallSpec + :members: + +.. autoclass:: SemanticPose + :members: + +.. autoclass:: Pick + :members: + +.. autoclass:: Place + :members: + +.. autoclass:: HandOver + :members: + +.. autoclass:: OperateArticulation + :members: + +.. autoclass:: RegisteredSemanticCall + :members: + +.. autoclass:: SemanticCallCatalog + :members: + +.. autoclass:: SemanticSkillCompiler + :members: + +.. autoclass:: AtomicSkills + :members: + +.. autoclass:: SkillRuntime + :members: + +.. autoclass:: SkillResult + :members: + +.. autoclass:: SkillCallTrace + :members: + +.. autoclass:: SkillPlanAttemptTrace + :members: + +.. autoclass:: SkillEffectTrace + :members: + +Effects, evidence, and parallel execution +----------------------------------------- + +.. autoclass:: SemanticEffectSpec + :members: + +.. autoclass:: EffectMonitorRef + :members: + +.. autoclass:: EffectMonitor + :members: + +.. autoclass:: EffectEvidenceCollector + :members: + +.. autoclass:: ParallelSkillRuntime + :members: + +.. autoclass:: ParallelSkillResult + :members: + +.. autoclass:: ParallelCommandSafetyValidator + :members: + Registry and provider --------------------- 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 f03d453e2..66488542d 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -157,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 diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index d3b7c2ea2..576d2f24a 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -21,6 +21,7 @@ from .calls import ( DeclarativeValue, HandOver, + OperateArticulation, Pick, Place, PlaceRelationTarget, @@ -39,7 +40,6 @@ RegisteredSemanticLowerer, RelationTargetGrounder, SemanticEffectDependency, - SemanticEffectKind, SemanticHandOverTarget, SemanticLowering, SemanticObjectTarget, @@ -47,6 +47,74 @@ 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, @@ -58,6 +126,29 @@ 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, @@ -78,12 +169,17 @@ UnsupportedSkillError, ) 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, @@ -98,30 +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", @@ -131,12 +314,15 @@ "ResourceClaim", "ResourceEndpoint", "ResourceEndpointAdapter", + "ResolvedCorePolicyTrace", "RegisteredSemanticCall", "RegisteredSemanticLowerer", "RelationTargetGrounder", "RobotResource", "RobotSkillProfile", "SceneAffordanceRef", + "SceneArticulationJointStateProvider", + "SceneArticulationEvidenceProvider", "SceneArticulationRef", "SceneCollisionRole", "SceneCollisionWorldMode", @@ -150,13 +336,25 @@ "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION", "SceneManifest", + "ScalarEffectClause", + "ScalarEffectEvidenceBatch", + "ScalarEffectEvidenceQuery", + "ScalarEffectObservation", + "ScalarEvidenceKind", + "ScalarExpectation", + "ScalarObservationCallback", "SemanticCallCatalog", "SemanticCallDescriptor", "SemanticCallSpec", "SemanticDiagnostic", "SemanticEffectDependency", "SemanticEffectKind", + "SemanticEffectSpec", + "SymbolicStateDomain", + "SymbolicStateKey", "SemanticHandOverTarget", "SemanticIntegrationManifest", "SemanticLowering", @@ -167,7 +365,23 @@ "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 71301fe71..7382c03d5 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 @@ -306,6 +306,36 @@ def to_matrix(self) -> torch.Tensor: output[:, 3, 3] = 1.0 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: @@ -327,6 +357,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): @@ -447,6 +492,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 @@ -608,11 +723,18 @@ def __post_init__(self) -> None: _validate_identifier( self.skill_id, field_name="SemanticCallDescriptor.skill_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." ) _validate_static_binding_contract( self.binding_contract, @@ -674,6 +796,7 @@ def __post_init__(self) -> None: Pick.call_kind, Place.call_kind, HandOver.call_kind, + OperateArticulation.call_kind, RegisteredSemanticCall.call_kind, }: raise ValueError( @@ -738,7 +861,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: @@ -774,6 +903,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 @@ -781,6 +913,7 @@ def _builtin_call_target( Pick: PickUp.descriptor(), Place: PlaceAction.descriptor(), HandOver: HandOverAction.descriptor(), + OperateArticulation: OperateArticulationAction.descriptor(), } try: return targets[spec_type] @@ -802,7 +935,7 @@ def builtin_semantic_call_catalog() -> SemanticCallCatalog: skill_id=_builtin_call_target(spec_type).skill_id, binding_contract=_builtin_call_target(spec_type).binding_contract, ) - for spec_type in (Pick, Place, HandOver) + for spec_type in (Pick, Place, HandOver, OperateArticulation) ) return SemanticCallCatalog(descriptors) @@ -810,6 +943,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 9d3914736..69cc0832b 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 -from enum import Enum from types import MappingProxyType from typing import ClassVar from uuid import uuid4 @@ -33,26 +32,53 @@ ActionInvocation, ActionOptions, Affordance, + ArticulationOperationAffordance, GraspGoal, HandOverOptions, - JointPositionTarget, HeldObjectState, PickUpOptions, PlaceGoal, PlaceOptions, + OperateArticulationGoal, PlanningContext, PoseGoalValue, + SceneArticulationOperationGeometry, SceneEntityPose, SkillDescriptor, ) 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, @@ -61,6 +87,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, @@ -87,15 +117,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,6 +259,9 @@ class AnalyzedSemanticCall: index: int bound: BoundSemanticCall effect_kind: SemanticEffectKind + 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 @@ -249,6 +273,29 @@ 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 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." + ) + 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( @@ -423,6 +470,8 @@ 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: @@ -439,12 +488,16 @@ def _create( *, 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 @@ -456,6 +509,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: @@ -479,6 +546,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. @@ -487,6 +555,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.") @@ -603,6 +672,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: @@ -626,6 +705,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], @@ -659,7 +743,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.") @@ -770,6 +860,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 @@ -780,11 +872,23 @@ def analyze( if type(call) is Pick 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, + 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, ) @@ -798,6 +902,124 @@ def analyze( compiler_id=self._compiler_id, ) + 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, @@ -844,6 +1066,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 @@ -861,9 +1089,30 @@ def ground( invocation_id=f"{workflow.workflow_id}:{call_index}", revision=revision, ) + 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, + effect_spec=effect_spec, + effect_monitor=effect_monitor, eligible_mask=eligible, ) @@ -956,6 +1205,62 @@ def _validate_context(self, context: PlanningContext) -> None: 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, @@ -1038,14 +1343,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", "object"), ) - del control_part + del task_state_key if call.at is not None: object_target = self._broadcast_pose( call.at.to_matrix(), @@ -1113,6 +1418,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, @@ -1162,6 +1597,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, @@ -1373,18 +2046,19 @@ def _require_held_object( 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: + """Resolve the logical participant key and verify held-object identity.""" + resource = analyzed.bound.binding.resources[slot_id] + endpoint = resource.endpoints.get("motion") + if endpoint is None: raise _diagnostic( - "unsupported_builtin_endpoint", + "missing_effect_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) + "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) 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: @@ -1392,7 +2066,7 @@ def _require_held_object( "verified_held_object_required", path, 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 @@ -1408,7 +2082,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( @@ -1427,6 +2101,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 8efc8e046..f98ab1fe4 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -26,6 +26,7 @@ from embodichain.lab.sim.atomic_actions import ( Affordance, AtomicActionEngine, + DynamicCollisionMode, DisjointResourceSlots, DisjointSlotEndpoints, SkillResourceSlot, @@ -33,6 +34,7 @@ from .calls import ( HandOver, + OperateArticulation, Pick, Place, RegisteredSemanticCall, @@ -51,6 +53,7 @@ SkillPolicyPreset, ) from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, GRASP_AFFORDANCE_CAPABILITY, PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, @@ -577,7 +580,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.") @@ -685,6 +694,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, @@ -781,6 +813,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, @@ -813,6 +863,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, @@ -1119,6 +1197,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, @@ -1139,6 +1223,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.""" @@ -1160,6 +1291,10 @@ def __init__( 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: @@ -1240,6 +1375,21 @@ def link_call( str(exc), ) ) from exc + 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, 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 a9fe70f14..9623e8a6d 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,13 +646,14 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True, init=False) class SkillPolicyPreset: - """Versioned planning, recovery, and runner policy bundle.""" + """Versioned planning, recovery, runner, and effect-monitor bundle.""" preset_id: str schema_version: int _motion_policy: MotionPolicy _recovery_policy: RecoveryPolicy _runner_cfg: ExecutionRunnerCfg + _effect_monitors: Mapping[str, EffectMonitorRef] def __init__( self, @@ -563,6 +662,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, ) -> None: """Own one policy bundle without exposing mutable nested configuration.""" _validate_identifier(preset_id, field_name="SkillPolicyPreset.preset_id") @@ -584,11 +684,45 @@ 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, "_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: @@ -605,6 +739,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( @@ -613,6 +757,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, ) @@ -1454,6 +1599,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=( @@ -1853,6 +2004,7 @@ def _lower_binding( resource_id=resource.resource_id, adapter_id=endpoint.adapter_id, target=endpoint.runtime_target, + 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 new file mode 100644 index 000000000..de98f5ed3 --- /dev/null +++ b/embodichain/lab/sim/skills/runtime.py @@ -0,0 +1,2294 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Canonical execution service and convenience facade for semantic skills.""" + +from __future__ import annotations + +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, replace +from enum import Enum +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, + ExecutionPlanAttempt, +) +from ..atomic_actions.plans import ExecutionFeedbackMode, TrajectorySegment +from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy +from ..atomic_actions.runner import ( + CommandSink, + ExecutionClock, + ExecutionRunner, + ExecutionRunnerCfg, + MonotonicExecutionClock, + ObservationProvider, + RunnerStatus, + RunnerStep, +) +from ..atomic_actions.state import PlanningContext, TaskState +from .calls import SemanticCallSpec +from .compiler import SemanticSkillCompiler +from .effects import ( + BinaryEffectEvidenceBatch, + EffectEvidenceBatch, + EffectMonitor, + EffectMonitorRef, + JointStateEvidenceBatch, + PoseRelationEvidenceBatch, + ScalarEffectEvidenceBatch, + SemanticEffectSpec, +) +from .scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + + +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 SkillStatus(str, Enum): + """Lifecycle state of one semantic workflow run.""" + + IDLE = "idle" + RUNNING = "running" + COMPLETED = "completed" + FAILED = "failed" + CANCELLED = "cancelled" + + +@dataclass(frozen=True, slots=True) +class SkillEndpointBindingTrace: + """JSON-safe typed projection of one resolved execution endpoint.""" + + 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 { + "planner": policy.planner, + "strategy": policy.strategy, + "sample_count": policy.sample_count, + "control_dt": _metadata_value(policy.control_dt), + "velocity_limit": _metadata_value(policy.velocity_limit), + "acceleration_limit": _metadata_value(policy.acceleration_limit), + "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 SkillPlanAttemptTrace: + """Compact, typed trace of one installed action-plan generation. + + ``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 + 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 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, 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 ( + 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 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 ( + type(self.planned_scene_version) is not int + or self.planned_scene_version < 0 + ): + 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 SkillEffectTrace: + """One monitor decision correlated with an atomic verification boundary.""" + + 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: + 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 type(value) is not str or not value: + raise ValueError(f"{name} must be a non-empty string.") + 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("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 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 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 + 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.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): + 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 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, "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 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)``. + """ + 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.""" + + +@runtime_checkable +class SkillRuntimeProvider(Protocol): + """Explicit environment adapter installed for :meth:`AtomicSkills.from_env`.""" + + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + """Build a fully connected semantic runtime for this environment.""" + + +class _PrimedObservationProvider: + """Return a JIT-grounding observation once before delegating fresh reads.""" + + 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, + ) + + +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__( + self, + 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, + ) -> 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 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 runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") + 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._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 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, + ) -> SkillRuntime: + """Construct the canonical runtime from explicit reusable ports.""" + return cls( + compiler, + observation_provider, + command_sink, + evidence_collector, + task_state=task_state, + clock=clock, + runner_cfg=runner_cfg, + ) + + @property + def compiler(self) -> SemanticSkillCompiler: + """Return the installed semantic compiler.""" + return self._compiler + + @property + def clock(self) -> ExecutionClock: + """Return the shared execution clock used by this runtime. + + Parallel coordinators use the same clock for every derived lane so a + branch cannot advance independently of the environment step grid. + """ + return self._clock + + @property + def scene_registry(self) -> SceneRegistry: + """Return the authoritative semantic scene registry.""" + return self._compiler.integration.scene_registry + + @property + def task_state(self) -> TaskState: + """Return an owned snapshot of persistent verified task state.""" + return _snapshot_task_state(self._task_state) + + def fork( + self, + command_sink: CommandSink, + *, + 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: + command_sink: Lane-local command sink. + task_state: Optional verified barrier state. The current owned + task state is used when omitted. + + Returns: + A new idle semantic runtime for one independent lane. + """ + 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, + ) + + @property + def status(self) -> SkillStatus: + """Return the current workflow status.""" + return self._status + + @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, + ) + + def start( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + """Analyze once and prepare the first call without blocking on motion. + + Args: + *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: + Immutable initial runtime result. + """ + 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: + 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." + ) + 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: 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 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: + env_mask: Rows to remove permanently from this workflow. + reason: Human-readable cancellation reason. + + Returns: + Updated immutable workflow result. + """ + 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( + "env_mask must be bool and match the runtime batch/device." + ) + 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, + ) + 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 + + @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, + calls: tuple[SemanticCallSpec, ...], + workflow: object, + *, + workflow_id: str, + eligible_mask: torch.Tensor | None, + execution_prefix_length: int, + ) -> None: + """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, + ) + 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._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.") + + self._grounded = grounded + session = self._engine.start( + (invocation,), + context, + eligible_mask=self._eligible, + ) + primed = _PrimedObservationProvider(context, self._observation_provider) + runner = ExecutionRunner( + session, + primed, + self._command_sink, + clock=self._clock, + cfg=self._runner_cfg, + ) + 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, + 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( + "Effect request invocation_id does not match the effect spec." + ) + 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, + ) + 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: + 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, + ) + + 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: + 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, + 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._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 :]), + ) + ) + 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, + call_index: int, + failed_mask: torch.Tensor, + ) -> None: + """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 _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 + + def _require_runner(self) -> ExecutionRunner: + if self._runner is None: + raise RuntimeError("No semantic call runner is active.") + return self._runner + + def _require_grounded(self) -> object: + if self._grounded is None: + raise RuntimeError("No grounded semantic call is active.") + return self._grounded + + 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 + + +class SkillScene: + """Typed convenience lookup surface backed by one immutable registry.""" + + def __init__(self, registry: SceneRegistry) -> None: + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + self._registry = registry + + @property + def registry(self) -> SceneRegistry: + """Return the authoritative scene registry.""" + return self._registry + + def entity(self, identifier: str | SceneEntityRef) -> SceneEntityRef: + """Resolve any registered semantic entity.""" + return self._registry.resolve(identifier) + + def object(self, identifier: str | SceneObjectRef) -> SceneObjectRef: + """Resolve a registered semantic object.""" + return self._registry.resolve(identifier, expected_type=SceneObjectRef) + + def articulation( + self, + identifier: str | SceneArticulationRef, + ) -> SceneArticulationRef: + """Resolve a registered articulation.""" + return self._registry.resolve(identifier, expected_type=SceneArticulationRef) + + def link(self, identifier: str | SceneLinkRef) -> SceneLinkRef: + """Resolve a registered articulation link.""" + return self._registry.resolve(identifier, expected_type=SceneLinkRef) + + def affordance( + self, + identifier: str | SceneAffordanceRef, + ) -> SceneAffordanceRef: + """Resolve a registered semantic affordance.""" + return self._registry.resolve(identifier, expected_type=SceneAffordanceRef) + + +class AtomicSkills: + """Small application-facing facade over :class:`SkillRuntime`.""" + + 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) + + @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, + ) + ) + + @classmethod + def from_env(cls, env: object, *, preset: str = "safe") -> AtomicSkills: + """Build through an explicitly installed environment integration adapter. + + 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. + """ + 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." + ) + runtime = env.create_skill_runtime(preset=preset) + if not isinstance(runtime, SkillRuntime): + raise TypeError( + "SkillRuntimeProvider.create_skill_runtime() must return " + "SkillRuntime." + ) + return cls(runtime) + + @property + def runtime(self) -> SkillRuntime: + """Return the canonical runtime for advanced step-wise use.""" + return self._runtime + + @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 start( + self, + *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, + ) + + def step(self) -> SkillResult: + """Advance non-blocking execution by one due runner cycle.""" + return self._runtime.step() + + 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, + ) + + 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__ = [ + "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 7d613da06..cac2f0181 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.") @@ -390,6 +431,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.""" @@ -413,6 +467,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``. @@ -472,6 +527,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, @@ -486,6 +544,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.") @@ -584,9 +650,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 @@ -608,6 +687,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, @@ -632,6 +716,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: @@ -1490,7 +1578,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), @@ -1512,7 +1600,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, @@ -1580,6 +1671,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.""" @@ -1636,6 +1772,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 = ( @@ -1711,6 +1849,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() @@ -1727,8 +1869,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) @@ -1754,6 +1899,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( @@ -1762,8 +1908,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, *, @@ -1853,11 +2103,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/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 742d0058c..b6d7d8502 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 @@ -113,6 +114,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 f9a206e5f..5ad526016 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -32,12 +32,14 @@ BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, ControlPartCommandProfile, + DynamicCollisionMode, EntityState, FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, GraspGoal, HandOverOptions, HeldObjectState, + MotionPolicy, ObjectSemantics, PickUp, PickUpOptions, @@ -69,6 +71,24 @@ SemanticSkillCompiler, SemanticWorkflow, ) +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, @@ -86,6 +106,8 @@ GRASP_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, + SceneCollisionRole, + SceneCollisionWorldMode, SceneEntityRegistration, SceneObjectRef, SceneRegistry, @@ -119,6 +141,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.""" @@ -234,7 +263,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 @@ -250,6 +309,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, @@ -275,12 +340,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={ @@ -304,7 +372,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", ) @@ -346,7 +414,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( @@ -370,6 +442,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) @@ -377,8 +450,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 @@ -393,10 +468,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( @@ -407,14 +485,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, ) @@ -450,7 +539,7 @@ def _held_context( object_to_eef: torch.Tensor, *, env_mask: torch.Tensor | None = None, - control_part: str = "arm", + task_state_key: str = "manipulator", robot_dof: int = 2, ) -> PlanningContext: held = HeldObjectState( @@ -464,12 +553,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) @@ -499,6 +926,33 @@ def test_analysis_is_provider_free_and_propagates_object_target() -> None: engine.resolve(grounded.invocation) +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")),)) + + grounded = compiler.ground(workflow, 0, _context(registry)) + + assert ( + grounded.invocation.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + assert ( + engine.resolve(grounded.invocation).motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + + def test_pick_relation_lookahead_stays_late_bound_scene_dependency() -> None: registry, _ = _scene_registry() compiler, engine = _compiler(registry) @@ -624,7 +1078,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", + task_state_key="left", robot_dof=4, ) handover = compiler.ground(workflow, 1, held_context) @@ -659,7 +1113,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", + task_state_key="left", robot_dof=4, ) with pytest.raises(RuntimeError, match="captured target"): 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 ffa790bce..8a32e3bfd 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,8 +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, @@ -59,6 +63,8 @@ GRASP_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, + SceneCollisionRole, + SceneCollisionWorldMode, SceneEntityRegistration, SceneObjectRef, SceneRegistry, @@ -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, @@ -378,6 +422,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")),)) @@ -506,6 +580,289 @@ def test_bound_semantic_call_is_factory_owned_by_installed_profile() -> None: BoundSemanticCall() +@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 ac31f550e..f50666d3b 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) @@ -1346,6 +1404,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 new file mode 100644 index 000000000..098f0d7cf --- /dev/null +++ b/tests/sim/skills/test_runtime.py @@ -0,0 +1,910 @@ +# ---------------------------------------------------------------------------- +# 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 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 ( + ActionInvocation, + ActionOptions, + ActionPlan, + ArticulationJointState, + AtomicAction, + AtomicActionEngine, + CommandAcknowledgement, + EffectVerificationRequirement, + EffectVerificationRequest, + EndpointBinding, + JointPositionTarget, + MotionPolicy, + PlanningContext, + RecoveryPolicy, + ResolvedActionRequest, + RobotObservation, + SceneSnapshot, + SkillBindingContract, + StateDelta, + TaskState, + TimedCommandSequence, +) +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, +) +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 + +BATCH_SIZE = 2 + + +class _Clock: + """Deterministic execution clock.""" + + def __init__(self) -> None: + self.time = 0.0 + self.sleeps: list[float] = [] + + def now(self) -> float: + return self.time + + def sleep(self, duration: float) -> None: + self.sleeps.append(duration) + self.time += duration + + +class _ObservationProvider: + """Return a new timestamped context on every external observation.""" + + def __init__(self) -> None: + self.calls = 0 + self.task_states: list[TaskState] = [] + + def observe(self, task_state: TaskState) -> PlanningContext: + self.calls += 1 + self.task_states.append(task_state) + timestamp = float(self.calls) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=torch.zeros(BATCH_SIZE, 1), + qvel=torch.zeros(BATCH_SIZE, 1), + ), + task=task_state, + scene=SceneSnapshot(timestamp=timestamp, version=self.calls), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + ) + + +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[object, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, context, timeout + self.held += 1 + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[object, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, timeout + self.cancelled += 1 + return CommandAcknowledgement.accepted_ack() + + +class _Collector: + """Fake acquisition boundary; the test monitor owns decisions.""" + + def __init__(self) -> None: + self.calls: list[tuple[int, float, torch.Tensor]] = [] + + 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}, + ) + + +@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( + planner="runtime_test", + sample_count=7, + control_dt=0.02, + velocity_limit=0.4, + acceleration_limit=0.8, + ), + 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, + ), + ) + ) + return _Grounded( + analyzed, + invocation, + spec, + monitor, + eligible_mask.clone(), + ) + + +@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 _system( + decisions: tuple[EffectMonitorDecision, ...], + *, + 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 = "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, + ) + return _System( + runtime, + compiler, + engine, + action, + observation, + sink, + collector, + clock, + ) + + +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) + + 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" + + +@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)), + ) + ) + + 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] + ) + + assert system.compiler.analyze_count == 0 + assert system.observation.calls == 0 + + +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"]["planner"] == "runtime_test" + 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_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] + + with pytest.raises(ValueError, match="keys must be scene dependencies"): + replace( + attempt, + scene_dependency_monitor_until={"other": 0}, + ) + + +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), + ) + + 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")), + ) + + 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"]["planner"] + == "runtime_test" + ) + + +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") + + 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" + + +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")) + + 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_from_env_requires_an_explicit_runtime_provider() -> None: + class AttributeBag: + compiler = object() + robot = object() + scene = object() + + with pytest.raises(TypeError, match="no semantic-skill integration adapter"): + AtomicSkills.from_env(AttributeBag()) + + +def test_from_env_delegates_preset_to_installed_provider() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + + class Provider: + def __init__(self) -> None: + self.presets: list[str] = [] + + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + self.presets.append(preset) + return system.runtime + + provider = Provider() + skills = AtomicSkills.from_env(provider, preset="precise") + + assert skills.runtime is system.runtime + assert provider.presets == ["precise"] + + +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 + + assert torch.equal(fresh.success_mask, _mask(True, True)) + assert torch.equal(fresh.calls[0].completed_mask, _mask(True, True)) + + +def test_fork_creates_an_independent_lane_on_the_shared_clock() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + lane_sink = _CommandSink() + + 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_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") + + 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() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.cancelled_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) + + +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")) + + result = system.runtime.deactivate_rows( + _mask(True, True), + reason="parallel peer failed", + ) + + 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 + + +def test_parallel_factory_analyzes_claims_and_forks_owned_shared_clock_lanes() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + + 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, + ) + + 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 bf364f56f..6b9a97d9f 100644 --- a/tests/sim/skills/test_scene.py +++ b/tests/sim/skills/test_scene.py @@ -27,6 +27,7 @@ Affordance, AntipodalAffordance, EntityState, + ObservedArticulationJointState, SceneSnapshot, ) from embodichain.lab.sim.skills import ( @@ -89,6 +90,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.""" @@ -127,13 +146,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.""" @@ -144,7 +177,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: @@ -220,6 +257,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( @@ -1025,6 +1103,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 ): From f5ec1864b1946cbd642798b12eee53038f2ae4bb Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:19:15 +0800 Subject: [PATCH 18/28] feat(gym): add declarative expert program runtime --- ...mbodichain.lab.gym.envs.expert_program.rst | 95 + .../embodichain/embodichain.lab.gym.envs.rst | 9 + .../embodichain/embodichain.utils.rst | 7 + .../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 | 251 ++ .../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 | 1240 ++++++++++ .../gym/envs/expert_program/environment.py | 831 +++++++ .../lab/gym/envs/expert_program/loader.py | 337 +++ .../lab/gym/envs/expert_program/simulation.py | 1240 ++++++++++ .../expert_program/simulation_environment.py | 1223 ++++++++++ .../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/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 | 503 ++++ tests/gym/envs/expert_program/test_decoder.py | 530 +++++ .../envs/expert_program/test_environment.py | 946 ++++++++ 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 | 1893 +++++++++++++++ .../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 + 43 files changed, 20728 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..c4d7d1f4d --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst @@ -0,0 +1,95 @@ +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 + +.. 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 + +Compilation and environment integration +--------------------------------------- + +.. autoclass:: ExpertProgramCompiler + :members: + +.. autoclass:: CompiledProgram + :members: + +.. autoclass:: ExpertProgramEnvironmentMixin + :members: + +.. autoclass:: ExpertProgramEnvironmentAdapter + :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/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 14c7e98bf..19dc53837 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`` import 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 51b2fa966..ba18a6a91 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): @@ -1248,14 +1274,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 @@ -1548,15 +1590,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) @@ -1763,13 +1808,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. @@ -1778,6 +1875,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..ca245f98b --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/__init__.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. +# ---------------------------------------------------------------------------- + +"""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, + 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, +) +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", + "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", + "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..88ba825df --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/decoder.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. +# ---------------------------------------------------------------------------- + +"""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 +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_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") + _call_context( + context.validate_semantic_call, + call, + path=call_path, + ) + if type(call) in (PickCfg, PlaceCfg, HandOverCfg): + _call_context( + context.validate_scene_reference, + call.object, + role="object", + path=(*call_path, "object"), + ) + if type(call) is PickCfg and call.grasp is not None: + _call_context( + context.validate_scene_reference, + call.grasp, + role="affordance", + path=(*call_path, "grasp"), + ) + if type(call) is PlaceCfg: + if call.on is not None: + _call_context( + context.validate_scene_reference, + call.on, + role="object_or_affordance", + path=(*call_path, "on"), + ) + if call.inside is not None: + _call_context( + context.validate_scene_reference, + call.inside, + role="object_or_affordance", + path=(*call_path, "inside"), + ) + if type(call) is OperateArticulationCfg: + _call_context( + context.validate_scene_reference, + call.articulation, + role="articulation", + path=(*call_path, "articulation"), + ) + if call.handle is not None: + _call_context( + context.validate_scene_reference, + call.handle, + role="affordance", + path=(*call_path, "handle"), + ) + 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", + "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..95b2294f3 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/environment.py @@ -0,0 +1,831 @@ +# ---------------------------------------------------------------------------- +# 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 + + +@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", +] 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..557b8f707 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -0,0 +1,1223 @@ +# ---------------------------------------------------------------------------- +# 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 +from dataclasses import replace +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( + n_envs=command.batch_size, + control_dof=payload.dof, + device=payload.device, + dtype=payload.positions.dtype, + ) + grasp_positions = grasp_command.resolve( + n_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, + ) + + +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 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, + ), + 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.") + return aligned + + 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/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..1befa0ac6 --- /dev/null +++ b/tests/gym/envs/expert_program/test_completion_metadata.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. +# ---------------------------------------------------------------------------- + +"""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( + planner="completion_trace_planner", + sample_count=9, + control_dt=STEP_DT, + ), + 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..b73266154 --- /dev/null +++ b/tests/gym/envs/expert_program/test_decoder.py @@ -0,0 +1,530 @@ +# ---------------------------------------------------------------------------- +# 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, + 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} + + +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..dcca9b241 --- /dev/null +++ b/tests/gym/envs/expert_program/test_environment.py @@ -0,0 +1,946 @@ +# ---------------------------------------------------------------------------- +# 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, +) +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) + + 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..0b8c5c186 --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -0,0 +1,1893 @@ +# ---------------------------------------------------------------------------- +# 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, +) +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(control_dt=0.01), + ), + ), + 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.dynamic_collision_entity_ids = () + generator.collision_world_entity_ids = () + generator.supports_dynamic_collision_world = False + generator.collision_world_batch_mode = 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=trajectory, + 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=trajectory, + 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_aligns_every_motion_policy_to_gym_step() -> None: + """The environment cadence replaces unrelated preset fallback timing.""" + factory, _ = _factory() + + profile = factory.create_robot_skill_profile() + + assert profile.presets["safe"].motion_policy.control_dt == pytest.approx(_STEP_DT) + + +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 2df667fd71de507703975c4b82bbf8eeefff6049 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:20:24 +0800 Subject: [PATCH 19/28] feat(agents): add strict expert program frontend --- ...mbodichain.lab.gym.envs.expert_program.rst | 11 + 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 +++++--- 6 files changed, 890 insertions(+), 64 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 c4d7d1f4d..f94f74cd8 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 @@ -48,6 +48,17 @@ and 2. Version 2 adds deterministic parallel blocks with explicit barriers. .. autofunction:: decode_expert_program +MLLM frontend +------------- + +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/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 0b8c5c186..119ecca39 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, @@ -1071,12 +1074,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}) @@ -1112,17 +1125,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 @@ -1305,60 +1322,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, @@ -1381,9 +1417,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: @@ -1395,7 +1434,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 @@ -1494,37 +1533,70 @@ def test_simulation_factory_aligns_every_motion_policy_to_gym_step() -> None: assert profile.presets["safe"].motion_policy.control_dt == pytest.approx(_STEP_DT) -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 43c34c57d7a9a141f32c45631ac705f78e102e87 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:23:28 +0800 Subject: [PATCH 20/28] feat(tasks): add declarative expert program vertical slices --- agent_context/MAP.yaml | 36 + .../topics/atomic-actions/atomic-actions.md | 174 ++++- .../design/declarative_expert_program_plan.md | 142 +++- .../sim/atomic_actions/builtin_actions.md | 49 ++ .../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 | 646 +++++------------- .../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, 2276 insertions(+), 845 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 308d42ea3..01323f9a5 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -449,6 +449,10 @@ topics: - resource graph - resource DAG - semantic skill catalog + - semantic skill runtime + - expert program + - declarative expert program + - atomic demo bridge - capability binding - AtomicAction - ActionInvocation @@ -468,6 +472,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 @@ -537,6 +561,8 @@ topics: - ResolvedRobotResource - ResolvedSkillBinding - SkillPolicyPreset + - effect_monitors + - semantic effect monitor - binding_contract - engine.skills - skill_profile @@ -613,8 +639,18 @@ 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/__init__.py + - embodichain/lab/gym/envs/expert_program/ + - embodichain/lab/gym/envs/settling.py related_topics: - motion-planning - robot-system diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index dfcd1dbb1..ec1489b17 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -168,7 +168,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 @@ -207,10 +208,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 @@ -448,6 +451,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. @@ -566,6 +605,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 @@ -651,6 +814,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` | `GraspGoal.grasp_xpos` accepts an explicit pose tensor, a late-bound `SceneEntityPose`, or `None` for affordance sampling. A `SceneEntityPose` diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index bc70bd30d..6b33529b2 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,7 +1,9 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: implementation in progress; Phase 0 and PR1 complete, and PR2A, - PR2B, and PR2C implemented on stacked feature branches +- 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), @@ -45,11 +47,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 @@ -91,7 +95,7 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is updated against committed `main@e445133c` after PR #475. The +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 @@ -881,7 +885,7 @@ PR2A SceneRegistry PR2B RobotSkillProfile | | | v | PR2C Runtime Endpoints - | (in progress) + | (implemented) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -1036,10 +1040,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. PR2B may proceed in parallel with PR2A after the PR1 bridge. Neither follow-up requires official task migration; the repeated-cube vertical slice opts in only @@ -1101,8 +1123,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`; @@ -1119,6 +1154,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; @@ -1135,6 +1180,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 @@ -1151,13 +1202,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: @@ -1172,6 +1233,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; @@ -1186,11 +1254,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. @@ -1200,6 +1279,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; @@ -1262,11 +1346,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 @@ -1274,14 +1362,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 @@ -1289,20 +1377,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 9783df20f..c14b62527 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -147,6 +147,7 @@ The animations below are the focused simulator demos under | `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 @@ -362,6 +363,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: @@ -615,6 +623,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 e033a7937..6ad3ae46c 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 @@ -803,6 +805,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 20d25c7a5..b63b9395a 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -139,6 +139,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 b451fe9ce..adceebbed 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -470,6 +470,15 @@ 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 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 1965563b0..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,457 +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, ), - ), - ) - ) - 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 9b59c7067df44126e62488b67abeedfce7903571 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 14:21:42 +0800 Subject: [PATCH 21/28] feat(benchmark): add expert program rollout validation --- .../design/declarative_expert_program_plan.md | 30 +- docs/design/expert_program_rollout_report.md | 68 + scripts/benchmark/expert_program/__init__.py | 21 + .../benchmark/expert_program/demo_success.py | 1378 +++++++++++++++++ .../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_expert_program_rollout_report.py | 94 ++ 9 files changed, 3246 insertions(+), 6 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 6b33529b2..7a2ac157c 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), @@ -1127,8 +1130,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 @@ -1279,6 +1285,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 @@ -1339,7 +1352,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 @@ -1350,6 +1364,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/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..154468f3c --- /dev/null +++ b/scripts/benchmark/expert_program/demo_success.py @@ -0,0 +1,1378 @@ +# ---------------------------------------------------------------------------- +# 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 _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: + body_error.add_note( + "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: + body_error.add_note(discard_note) + elif executor_error is not None: + executor_error.add_note(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 + close_error.add_note( + "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: + factory_error.add_note( + "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 + body_error.add_note( + "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/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 From 6a42cd6b291e87eac21f6edc614d0249bd559137 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 16:37:00 +0800 Subject: [PATCH 22/28] refactor(atomic-actions): add typed tracking contracts --- .../lab/sim/atomic_actions/__init__.py | 80 +- .../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 | 434 +++--- .../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 | 101 +- 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 +- 26 files changed, 3016 insertions(+), 410 deletions(-) create mode 100644 embodichain/lab/sim/atomic_actions/tracking.py create mode 100644 tests/sim/atomic_actions/test_tracking.py diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index fd1e0429c..8611766fb 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -80,7 +80,6 @@ ActionPlan, CompiledTrajectory, EffectVerificationRequirement, - ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, @@ -108,6 +107,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, @@ -225,7 +264,6 @@ "EffectVerificationResult", "EffectVerifier", "ExecutionClock", - "ExecutionFeedbackMode", "ExecutionEvent", "ExecutionEventKind", "ExecutionPlanAttempt", @@ -234,6 +272,9 @@ "ExecutionSession", "ExecutionStatus", "ExecutionTick", + "EndpointTrackingChannelBinding", + "EndpointTrackingFeedbackAddress", + "FeedbackTerminalAcceptance", "GRASP_COMMAND", "GRASP_CAPABILITY", "GraspGoal", @@ -243,12 +284,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", @@ -273,6 +318,8 @@ "PlannerDiagnostics", "PlanningContext", "PoseGoalValue", + "PoseTrackingMetric", + "PoseTrackingState", "Press", "PressGoal", "PressOptions", @@ -300,8 +347,37 @@ "SimulationExecutionAdapter", "TaskState", "TimedCommandSequence", + "TimedTerminalAcceptance", + "TimedTrackingSequence", "TimedTrajectory", + "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 3043c5b5c..1180244a4 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 a3d37d55e..892bec50a 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -42,7 +42,6 @@ from .plans import ( ActionPlan, EffectVerificationRequirement, - ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, @@ -56,6 +55,12 @@ RuntimeCommandFrame, TimedCommandSequence, ) +from .tracking import ( + FeedbackTerminalAcceptance, + TimedTrackingSequence, + TrackingFrame, + TrackingSetpoint, +) if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -370,6 +375,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, @@ -574,7 +580,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, ) @@ -591,14 +596,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. @@ -617,9 +621,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. @@ -647,6 +650,7 @@ def build_command_plan( ), env_ids=commands.env_ids, ) + tracking = self._tracking_sequence(request, masked_commands) segments = self._build_segments( segment_lengths, frame_count=masked_commands.frame_count, @@ -660,12 +664,13 @@ def build_command_plan( plan_success=success_mask, commands=masked_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), @@ -682,6 +687,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 ff1d5eb30..c9f6dbce2 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -30,6 +30,7 @@ from .plans import ActionPlan, CompiledTrajectory, TimedTrajectory 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 @@ -100,6 +101,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. @@ -114,6 +116,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.") @@ -131,6 +136,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 @@ -163,6 +169,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.""" @@ -617,6 +628,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 c30691c28..4876a0c25 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,10 @@ 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 @@ -888,14 +910,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 +944,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( @@ -1054,7 +1107,9 @@ def _install_plan( for target in plan.commands.targets } replacement_destinations = frozenset(replacement_targets) + 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 ( @@ -1064,14 +1119,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 @@ -1158,6 +1225,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, @@ -1180,34 +1297,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, @@ -1513,76 +1645,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 a600ff30b..cba5fac0f 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -29,6 +29,7 @@ from .control import ActionControlOverrides from .goals import ActionGoal from .policies import MotionPolicy, RecoveryPolicy +from .tracking import TrackingPolicy GoalT = TypeVar("GoalT", bound=ActionGoal) @@ -101,6 +102,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.""" @@ -131,6 +137,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( @@ -161,6 +169,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 @@ -173,6 +182,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): @@ -197,6 +208,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)) @@ -207,6 +219,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 7e86f16cf..b3c901b90 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 from types import MappingProxyType from typing import Any, Mapping, Sequence @@ -28,11 +27,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( @@ -384,13 +387,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. @@ -478,10 +474,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, ...] = () @@ -511,8 +508,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): @@ -573,101 +570,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.") @@ -753,6 +737,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", @@ -811,10 +805,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, @@ -915,7 +910,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 5ef36d4c6..c9d536b4d 100644 --- a/embodichain/lab/sim/atomic_actions/policies.py +++ b/embodichain/lab/sim/atomic_actions/policies.py @@ -152,9 +152,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.""" @@ -170,7 +167,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 578a43d86..d24812628 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -511,6 +511,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 69cc0832b..def756fc5 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -1083,6 +1083,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 f98ab1fe4..0892f2284 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -1386,6 +1386,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 9623e8a6d..2fc875d5d 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,11 +717,12 @@ 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 _motion_policy: MotionPolicy + _tracking_policy: TrackingPolicy _recovery_policy: RecoveryPolicy _runner_cfg: ExecutionRunnerCfg _effect_monitors: Mapping[str, EffectMonitorRef] @@ -660,6 +732,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, @@ -674,12 +747,19 @@ def __init__( f"{schema_version}; supported versions are [1]." ) 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): @@ -716,6 +796,7 @@ def __init__( object.__setattr__(self, "preset_id", preset_id) object.__setattr__(self, "schema_version", schema_version) 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__( @@ -734,6 +815,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.""" @@ -755,6 +841,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, @@ -1605,6 +1692,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=( @@ -2005,6 +2093,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 de98f5ed3..46fe7da6e 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), } @@ -332,7 +439,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 ), @@ -341,6 +447,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.""" @@ -349,6 +546,7 @@ class ResolvedCorePolicyTrace: preset_id: str preset_schema_version: int motion_policy: MotionPolicy + tracking_policy: TrackingPolicy recovery_policy: RecoveryPolicy endpoints: tuple[SkillEndpointBindingTrace, ...] @@ -364,6 +562,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) @@ -375,6 +575,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) @@ -386,6 +587,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: @@ -395,6 +597,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) @@ -409,6 +612,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, ) @@ -422,6 +626,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], } @@ -455,7 +660,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 @@ -545,8 +751,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 @@ -577,6 +787,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", @@ -623,7 +836,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 @@ -634,6 +848,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, ), @@ -664,7 +879,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, @@ -709,7 +925,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": { @@ -2042,6 +2259,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 @@ -2282,6 +2504,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 1befa0ac6..ce54ce7d3 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 171ebd054..8fe203f87 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -36,10 +36,11 @@ DynamicCollisionMode, EndpointBinding, EndpointCommand, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, EndEffectorPoseGoal, EntityState, EffectVerificationRequirement, - ExecutionFeedbackMode, HeldObjectState, JointPositionPayload, JointPositionTarget, @@ -58,7 +59,15 @@ StateDelta, TaskState, TimedCommandSequence, + TimedTerminalAcceptance, + TimedTrackingSequence, TimedTrajectory, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingPolicy, + TrackingProjectorRef, + TrackingSetpoint, + JointPositionTrackingState, ) from embodichain.lab.sim.atomic_actions.goals import ( _resolve_object_pose, @@ -196,7 +205,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, @@ -214,12 +224,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=( @@ -232,6 +245,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"): @@ -353,6 +401,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"), @@ -756,6 +805,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(), ) @@ -807,6 +857,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(), ) @@ -835,6 +886,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(), ) @@ -861,6 +913,7 @@ def test_command_target_authorization_rejects_altered_joint_claims() -> None: ), ), motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) @@ -901,6 +954,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(), ) @@ -945,7 +999,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) @@ -1061,7 +1114,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: @@ -1103,7 +1157,6 @@ def test_action_plan_validates_joint_trajectory_against_commands( _action_plan( commands, joint_trajectory=trajectory, - feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, ) @@ -1122,7 +1175,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, ) @@ -1140,19 +1240,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, @@ -1162,22 +1257,8 @@ def test_action_plan_requires_stable_destination_set( JointPositionTarget("other_arm", (0, 1)), ), ) - trajectory = ( - TimedTrajectory.from_positions( - torch.tensor([[[1.0, 1.0], [2.0, 2.0]]]), - env_ids=env_ids, - control_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: @@ -1210,87 +1291,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_positions( - torch.ones(1, 1, 2), - env_ids=env_ids, - control_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_positions( - torch.zeros(1, 1, 2), - env_ids=env_ids, - control_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_positions( - torch.ones(1, 1, 2), - env_ids=env_ids, - control_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_positions( - torch.ones(1, 1, 2), - velocities=torch.ones(1, 1, 2), - env_ids=env_ids, - control_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 7c6dd3688..2e15c5ea5 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 @@ -485,6 +486,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 f71f2179f..b81c96e33 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,7 +68,13 @@ StateDelta, TaskState, TimedCommandSequence, + TimedTrackingSequence, TimedTrajectory, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingPolicy, + TrackingProjectorRef, + TrackingSetpoint, ) from embodichain.lab.sim.common import BatchEntity from embodichain.lab.sim.atomic_actions.goals import resolve_pose_goal @@ -310,9 +318,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( @@ -358,7 +371,7 @@ def _plan( device=context.robot.qpos.device, ), ) - return self.build_command_plan( + plan = self.build_command_plan( request, context, success=True, @@ -367,6 +380,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): @@ -413,6 +455,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") @@ -430,7 +473,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 @@ -560,7 +603,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, ), @@ -908,6 +950,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(), ) @@ -1076,6 +1119,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 @@ -1535,6 +1595,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, ) @@ -1549,6 +1610,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( @@ -1560,7 +1651,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 58dda7380..12bec40ac 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.""" @@ -269,6 +339,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, @@ -292,7 +363,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) @@ -306,7 +377,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, ), ) @@ -356,7 +426,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 @@ -507,11 +577,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() @@ -526,7 +716,6 @@ def test_runner_revision_waits_for_deadline_and_plans_from_fresh_observation() - motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), recovery_policy=RecoveryPolicy( max_replans=2, - tracking_error_threshold=0.05, action_timeout=10.0, ), revision=1, @@ -573,7 +762,6 @@ def test_runner_revision_rejects_pending_effect_verification() -> None: motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), 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 5ad526016..8e5eb6d2e 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -50,6 +50,10 @@ SkillDescriptor, TaskState, ) +from embodichain.lab.sim.atomic_actions.tracking import ( + JointPositionTrackingMetric, + TrackingPolicy, +) from embodichain.lab.sim.skills.calls import ( HandOver, Pick, @@ -932,6 +936,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( @@ -951,6 +959,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_pick_relation_lookahead_stays_late_bound_scene_dependency() -> 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 f50666d3b..5400a54ad 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 @@ -1362,6 +1381,10 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: preset = SkillPolicyPreset( "safe", 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, + ), ) profile = RobotSkillProfile( "presets", @@ -1379,6 +1402,11 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: assert first is not second assert first.schema_version == 1 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 098f0d7cf..128f23d13 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 ( @@ -345,6 +350,7 @@ def ground( velocity_limit=0.4, acceleration_limit=0.8, ), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy( max_replans=0, max_action_retries=0, @@ -627,9 +633,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"]["planner"] == "runtime_test" 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" @@ -670,16 +683,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) @@ -694,6 +725,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 7c065ed1d556a59c83f463b74ff055fac92a6a41 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 17:02:14 +0800 Subject: [PATCH 23/28] 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 | 98 +- .../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 | 85 +- .../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, 2281 insertions(+), 176 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 ca245f98b..6a39b35e3 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -130,6 +130,11 @@ SimulationRobotSkillProfileBinding, SimulationSceneBinding, ) +from .catalog import ( + ExpertProgramIntegrationCatalog, + IntegrationFingerprintMismatch, + SimulationExpertProgramRegistration, +) from .simulation_environment import ( ControlCommandStateEvidenceTracker, MotionGeneratorFactory, @@ -139,7 +144,10 @@ SimulationPlanningObservationProvider, create_simulation_expert_program_adapter, ) -from .simulation_policies import SimulationSegmentPolicyPort +from .simulation_policies import ( + SimulationSegmentPolicyPort, + default_simulation_settle_presets, +) __all__ = [ "AcceptedRuntimeCommandObserver", @@ -183,6 +191,7 @@ "ExpertProgramEnvironmentFactory", "ExpertProgramEnvironmentMixin", "ExpertProgramIntegrationCfg", + "ExpertProgramIntegrationCatalog", "ExpertProgramRuntimeAssembly", "ExpertProgramSceneResolver", "ExpertProgramValidationContext", @@ -190,6 +199,7 @@ "HandOverCfg", "GymPlanningObservationProvider", "InvokeCfg", + "IntegrationFingerprintMismatch", "MAX_DECLARATIVE_DEPTH", "MAX_DECLARATIVE_NODES", "MAX_EXPANDED_CALLS", @@ -228,6 +238,7 @@ "SimulationArticulationLinkBinding", "SimulationExpertProgramEnvironment", "SimulationExpertProgramFactory", + "SimulationExpertProgramRegistration", "SimulationPlanningObservationProvider", "SimulationRigidObjectBinding", "SimulationResourceEndpointBinding", @@ -242,6 +253,7 @@ "ValidatorCfg", "WaitStablePostCfg", "create_simulation_expert_program_adapter", + "default_simulation_settle_presets", "decode_expert_program", "load_expert_program", "loads_expert_program_json", 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 95b2294f3..2e75c934e 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, @@ -268,6 +269,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. @@ -291,6 +294,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 @@ -319,7 +323,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): @@ -353,6 +382,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) @@ -406,6 +436,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 557b8f707..eee4660eb 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -38,7 +38,6 @@ import torch -from embodichain.lab.gym.envs.settling import DynamicSettleMonitorCfg from embodichain.lab.sim.atomic_actions import ( AtomicActionEngine, EntityState, @@ -70,11 +69,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, @@ -108,15 +104,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,7 +917,9 @@ 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 a profile whose every motion policy uses the Gym cadence.""" @@ -946,6 +932,7 @@ def create_robot_skill_profile(self) -> RobotSkillProfile: 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, @@ -958,6 +945,10 @@ def create_robot_skill_profile(self) -> RobotSkillProfile: 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( @@ -977,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, @@ -1089,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, @@ -1136,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, @@ -1158,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. @@ -1191,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, @@ -1205,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 119ecca39..c52e18b88 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,6 +73,7 @@ PlanningContext, StateDelta, TaskState, + TrackingPolicy, ) from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget @@ -102,7 +104,6 @@ SemanticObjectTarget, SemanticPose, SemanticRelationTarget, - SemanticValidationError, SkillPolicyPreset, ) from embodichain.lab.sim.skills.effects import ( @@ -685,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, @@ -697,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,6 +845,10 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: SkillPolicyPreset( "safe", 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", @@ -977,8 +978,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), ), @@ -1114,8 +1117,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), ) @@ -1525,12 +1530,16 @@ def _assert_invocation_equivalent( def test_simulation_factory_aligns_every_motion_policy_to_gym_step() -> None: - """The environment cadence replaces unrelated preset fallback timing.""" + """Cadence alignment preserves the exact registered tracking contract.""" factory, _ = _factory() profile = factory.create_robot_skill_profile() assert profile.presets["safe"].motion_policy.control_dt == pytest.approx(_STEP_DT) + assert profile.presets["safe"].tracking_policy == TrackingPolicy.joint_position( + 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( @@ -1689,8 +1698,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), @@ -1701,11 +1710,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( @@ -1722,41 +1733,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() -> ( @@ -1789,8 +1794,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 cc246bbd7ade570e365f730ec318024bea325faf Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 17:06:38 +0800 Subject: [PATCH 24/28] 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 dd0f6133a4d478ce3ca69fb68c63cb136c841b79 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 17:31:56 +0800 Subject: [PATCH 25/28] 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 | 82 ++++-- embodichain/lab/sim/skills/integration.py | 98 +++++++ embodichain/lab/sim/skills/profiles.py | 259 +++++++++++++++++- .../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 | 159 ++++++++++- 16 files changed, 943 insertions(+), 77 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 eee4660eb..6fa9f9df7 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 def756fc5..98040c412 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 dataclasses import dataclass, field +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, @@ -97,6 +99,8 @@ SceneObjectRef, ) +OptionT = TypeVar("OptionT", bound=ActionOptions) + def _validate_identifier(value: str, *, field_name: str) -> str: """Return one exact non-empty identifier.""" @@ -421,8 +425,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) @@ -1078,6 +1089,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, @@ -1323,13 +1338,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 - ) + ), ), ) @@ -1367,7 +1384,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, @@ -1411,9 +1431,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, ), @@ -1546,7 +1568,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( @@ -1567,19 +1593,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) @@ -1590,13 +1621,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 0892f2284..9acb72234 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -29,6 +29,8 @@ DynamicCollisionMode, DisjointResourceSlots, DisjointSlotEndpoints, + HandOverOptions, + PickUpOptions, SkillResourceSlot, ) @@ -717,6 +719,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, @@ -856,6 +933,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, @@ -1390,6 +1487,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 2fc875d5d..8e6fd3656 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 @@ -726,11 +926,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, @@ -741,10 +944,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]." ) selected_motion = MotionPolicy() if motion_policy is None else motion_policy selected_tracking = ( @@ -793,6 +996,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, "_motion_policy", deepcopy(selected_motion)) @@ -804,6 +1018,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: @@ -835,6 +1054,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( @@ -845,6 +1093,7 @@ def snapshot(self) -> SkillPolicyPreset: recovery_policy=self.recovery_policy, runner_cfg=self.runner_cfg, effect_monitors=self.effect_monitors, + 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 dcca9b241..9d6108af7 100644 --- a/tests/gym/envs/expert_program/test_environment.py +++ b/tests/gym/envs/expert_program/test_environment.py @@ -68,7 +68,10 @@ GRASP_CAPABILITY, JOINT_POSITION_CAPABILITY, MotionPolicy, + OperateArticulationOptions, + PickUpOptions, PlanningContext, + PlaceOptions, RobotObservation, TaskState, ) @@ -200,6 +203,10 @@ def _robot_profile( presets={ "safe": SkillPolicyPreset( "safe", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + }, motion_policy=safe_motion_policy, ) }, @@ -278,7 +285,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 c52e18b88..bc9b55680 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, @@ -1032,7 +1050,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", ) @@ -1783,7 +1813,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 8e5eb6d2e..fdac46a71 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, @@ -41,9 +42,11 @@ HeldObjectState, MotionPolicy, ObjectSemantics, + OperateArticulationOptions, PickUp, PickUpOptions, PlaceGoal, + PlaceOptions, PlanningContext, RobotObservation, SceneEntityPose, @@ -127,6 +130,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.""" @@ -177,14 +207,19 @@ class _InspectLowerer(RegisteredSemanticLowerer): schema_version: ClassVar[int] = 1 target_descriptor: ClassVar[SkillDescriptor] = _PICK_TARGET + 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( @@ -193,7 +228,6 @@ def lower( entity_id="cube", ) ), - skill_options=PickUpOptions(), ) @@ -201,12 +235,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 @@ -221,8 +251,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={}, @@ -231,11 +262,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), ) @@ -352,7 +382,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={ @@ -376,12 +410,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, @@ -412,7 +454,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}), ) @@ -457,7 +499,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 @@ -588,7 +630,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) @@ -602,7 +644,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"), @@ -627,7 +669,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( @@ -839,10 +881,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"),)) @@ -854,14 +907,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, @@ -903,7 +963,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( @@ -923,6 +993,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(), @@ -933,7 +1005,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( @@ -1048,7 +1120,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, @@ -1101,6 +1180,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) @@ -1192,7 +1273,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")),)) @@ -1208,6 +1299,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) @@ -1332,8 +1427,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, @@ -1342,7 +1443,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 8a32e3bfd..1423fb138 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( { @@ -429,7 +449,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") @@ -452,6 +472,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")),)) @@ -597,7 +709,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", @@ -632,7 +744,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"), ), @@ -668,9 +780,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"), ), @@ -694,11 +806,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() }, @@ -720,11 +832,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) @@ -744,7 +856,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"), ), @@ -772,7 +884,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"), ), @@ -791,7 +903,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"), ), @@ -822,7 +934,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), ), @@ -848,7 +960,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 5400a54ad..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,6 +1406,9 @@ 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", + 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, @@ -1400,9 +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.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) @@ -1414,8 +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=r"supported versions are \[2\]"): + SkillPolicyPreset("legacy", action_option_templates={}, schema_version=1) incompatible = RobotSkillProfile( "bad_preset", @@ -1424,6 +1460,7 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: presets={ "other": SkillPolicyPreset( "other", + action_option_templates={}, motion_policy=MotionPolicy(planner="other_planner"), ) }, @@ -1433,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", @@ -1448,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) == {} @@ -1461,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] @@ -1485,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 f167a2a68fde74e4507ed514a7ac82e17384f04e Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 18:11:00 +0800 Subject: [PATCH 26/28] feat(expert-program): own standard runtime extensions --- .../design/declarative_expert_program_plan.md | 27 + .../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 | 530 +++++++++- .../gym/envs/expert_program/environment.py | 268 +++++- .../lab/gym/envs/expert_program/extensions.py | 908 ++++++++++++++++++ .../expert_program/simulation_environment.py | 168 +--- .../lab/sim/skills/parallel_runtime.py | 58 +- tests/gym/envs/expert_program/test_bridge.py | 150 ++- tests/gym/envs/expert_program/test_catalog.py | 340 ++++++- .../envs/expert_program/test_extensions.py | 542 +++++++++++ .../test_simulation_environment.py | 844 +++++++++++++++- tests/sim/skills/test_parallel_runtime.py | 148 ++- 15 files changed, 3961 insertions(+), 252 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 7a2ac157c..e52240826 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1270,6 +1270,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/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 6a39b35e3..a3cae5178 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -135,6 +135,14 @@ IntegrationFingerprintMismatch, SimulationExpertProgramRegistration, ) +from .extensions import ( + EndpointAdapterDeclaration, + ParallelCommandSafetyValidatorFactory, + ParallelSafetyDeclaration, + RuntimeTransportDeclaration, + StandardExtensionDeclarations, + VersionedKey, +) from .simulation_environment import ( ControlCommandStateEvidenceTracker, MotionGeneratorFactory, @@ -182,6 +190,7 @@ "EXPERT_PROGRAM_SCHEMA_VERSION_V2", "EnvironmentStepClock", "EnvironmentStepTimingError", + "EndpointAdapterDeclaration", "ExpertProgramCfg", "ExpertProgramCompileError", "ExpertProgramCompiler", @@ -212,6 +221,8 @@ "ObjectNearTargetValidatorCfg", "OperateArticulationCfg", "ParallelCfg", + "ParallelCommandSafetyValidatorFactory", + "ParallelSafetyDeclaration", "PickCfg", "PlaceCfg", "PlanningObservationPort", @@ -222,6 +233,7 @@ "RepeatCfg", "RobotResourceBinding", "RuntimeCommandFrameEncoder", + "RuntimeTransportDeclaration", "RuntimeTransportActionEncoder", "SceneReferenceRole", "SceneRegistryProgramResolver", @@ -246,11 +258,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..8db7f334a 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,18 +34,36 @@ 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, RobotSkillProfile, + RegisteredSemanticCall, + ResourceEndpoint, + ResourceEndpointAdapter, SceneAffordanceRef, SceneArticulationRef, SceneEntityRef, @@ -56,6 +76,15 @@ 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 +106,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 +278,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 +290,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 +345,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 +356,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]]: @@ -491,6 +551,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 +586,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) @@ -754,6 +849,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,6 +896,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." + ) + + 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 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 _profile_with_control_dt( @@ -829,6 +1128,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 +1168,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 +1230,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 +1255,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 +1277,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 +1328,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 +1346,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 +1369,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 +1397,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,11 +1414,58 @@ 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, diff --git a/embodichain/lab/gym/envs/expert_program/environment.py b/embodichain/lab/gym/envs/expert_program/environment.py index 2e75c934e..580e7b61b 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 @@ -271,6 +310,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. @@ -295,6 +336,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 @@ -331,6 +373,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( @@ -382,16 +504,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 @@ -473,6 +596,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." @@ -482,12 +606,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, @@ -499,6 +642,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, @@ -528,6 +677,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( @@ -535,6 +685,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 " @@ -545,6 +696,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 " @@ -560,10 +712,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 = ( @@ -573,6 +745,7 @@ def _assemble_execution_runtime( observation_provider=observation_provider, ) ) + self._validate_registration_ownership() if not isinstance( accepted_command_observer, AcceptedRuntimeCommandObserver, @@ -586,14 +759,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, @@ -607,6 +822,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, ) @@ -634,7 +851,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( @@ -684,12 +902,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] @@ -723,6 +942,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, @@ -730,6 +956,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", @@ -761,6 +988,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( @@ -773,10 +1022,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 6fa9f9df7..abcacd901 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -58,7 +58,6 @@ ControlPartCommandProfile, JointPositionCommand, ) -from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg from embodichain.lab.sim.atomic_actions.runtime_commands import ( JointPositionPayload, RuntimeCommandFrame, @@ -69,30 +68,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, ) @@ -102,7 +93,6 @@ AcceptedRuntimeCommandObserver, EnvironmentStepClock, GymPlanningObservationProvider, - RuntimeTransportActionEncoder, ) from .catalog import SimulationExpertProgramRegistration from .environment import ( @@ -725,14 +715,8 @@ 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 @@ -749,15 +733,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 +751,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 +777,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 +786,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 +811,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 +830,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 +849,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.""" @@ -974,7 +924,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 +988,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 +1026,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 +1095,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 +1104,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 +1114,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 +1125,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/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..380e35c60 100644 --- a/tests/gym/envs/expert_program/test_catalog.py +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -18,7 +18,9 @@ from __future__ import annotations -from dataclasses import dataclass +from concurrent.futures import ThreadPoolExecutor +from dataclasses import dataclass, replace +from threading import Event, Lock from typing import ClassVar import pytest @@ -37,6 +39,7 @@ from embodichain.lab.sim.skills import ( PLACE_ON_AFFORDANCE_CAPABILITY, BoundSemanticCall, + ControlPartEndpoint, HandOver, HandOverPoseProvider, HandOverPoseTargets, @@ -48,8 +51,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 +156,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 +217,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 +412,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 +442,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 +499,174 @@ 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, + skill_id=target.skill_id, + binding_contract=target.binding_contract, + 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, @@ -596,6 +924,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 bc9b55680..5dfcb35b0 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, @@ -79,6 +80,13 @@ 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 +94,7 @@ EndpointCommand, JointPositionPayload, RuntimeCommandFrame, + RuntimeCommandPayload, ) from embodichain.lab.sim.planners import MotionGenerator from embodichain.lab.sim.skills import ( @@ -119,6 +128,7 @@ EffectEvidenceSourceRef, HeldObjectRelation, HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, ) from embodichain.lab.sim.skills.evidence import ( BinaryEffectEvidenceQuery, @@ -719,12 +729,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 +743,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 +806,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 +970,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 +989,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,6 +1040,60 @@ 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.""" return SimulationRobotSkillProfileBinding( @@ -868,6 +1134,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( @@ -1007,6 +1293,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( @@ -1154,12 +1465,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"] @@ -1728,6 +2034,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() @@ -1800,21 +2344,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, @@ -1824,13 +2358,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( @@ -1842,11 +2371,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", From d5ca06082200c2cddc9de6b40e8ad8a91896b375 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 25 Aug 2026 02:41:50 +0000 Subject: [PATCH 27/28] Merge branch 'main' into feat/expert-program-registration-runtime-catalog --- docs/design/expert_program_rollout_report.md | 6 +++--- tests/data_pipeline/test_online_data.py | 5 ++++- tests/scripts/tools/test_expert_program_rollout_report.py | 4 ++-- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/docs/design/expert_program_rollout_report.md b/docs/design/expert_program_rollout_report.md index 34bbd6fcd..9bd33db4e 100644 --- a/docs/design/expert_program_rollout_report.md +++ b/docs/design/expert_program_rollout_report.md @@ -45,9 +45,9 @@ 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 | 166 | -432 (-72.2%) | 23912 | 5645 | -18267 (-76.4%) | `embodichain_tasks/embodichain_tasks/expert_program/repeated_pick_place.py`
`embodichain_tasks/configs/expert_program/repeated_pick_place.yaml` | -| Drawer | 245 | 348 | +103 (+42.0%) | 8833 | 12600 | +3767 (+42.6%) | `embodichain_tasks/embodichain_tasks/expert_program/open_drawer.py`
`embodichain_tasks/configs/expert_program/open_drawer.yaml` | -| Total | 843 | 514 | -329 (-39.0%) | 32745 | 18245 | -14500 (-44.3%) | the four files above | +| Cube | 598 | 197 | -401 (-67.1%) | 23912 | 6561 | -17351 (-72.6%) | `embodichain_tasks/embodichain_tasks/expert_program/repeated_pick_place.py`
`embodichain_tasks/configs/expert_program/repeated_pick_place.yaml` | +| Drawer | 245 | 371 | +126 (+51.4%) | 8833 | 13434 | +4601 (+52.1%) | `embodichain_tasks/embodichain_tasks/expert_program/open_drawer.py`
`embodichain_tasks/configs/expert_program/open_drawer.yaml` | +| Total | 843 | 568 | -275 (-32.6%) | 32745 | 19995 | -12750 (-38.9%) | the four files above | ## Demo Success Measurement diff --git a/tests/data_pipeline/test_online_data.py b/tests/data_pipeline/test_online_data.py index dd1641f8e..44cf41f5a 100644 --- a/tests/data_pipeline/test_online_data.py +++ b/tests/data_pipeline/test_online_data.py @@ -459,7 +459,10 @@ def test_real_consumer_process_can_sample(self, start_method: str) -> None: f"(exit code {process.exitcode})" ) - process.join(timeout=PROCESS_CLEANUP_TIMEOUT) + # A spawned consumer imports Torch and EmbodiChain from scratch. + # Under xdist load, interpreter cleanup can exceed the generic + # process-termination timeout even after the result is available. + process.join(timeout=CONSUMER_RESULT_TIMEOUT) if process.is_alive(): pytest.fail("consumer process did not exit after publishing a result") if process.exitcode != 0: diff --git a/tests/scripts/tools/test_expert_program_rollout_report.py b/tests/scripts/tools/test_expert_program_rollout_report.py index 916734d4a..a7e9a5e23 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": (166, 5_645), - "Drawer": (348, 12_600), + "Cube": (197, 6_561), + "Drawer": (371, 13_434), } EXPECTED_SOURCE_PATHS = { From 14f006beeb383f8a5144efec8b68777669b4cbb7 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 25 Aug 2026 06:23:49 +0000 Subject: [PATCH 28/28] wip --- .../topics/atomic-actions/atomic-actions.md | 4 + .../design/declarative_expert_program_plan.md | 2 +- docs/design/expert_program_rollout_report.md | 4 +- .../lab/gym/envs/expert_program/catalog.py | 63 +++----------- .../gym/envs/expert_program/environment.py | 4 +- .../lab/gym/envs/expert_program/extensions.py | 65 ++------------ .../configs/expert_program/open_drawer.yaml | 4 - .../expert_program/open_drawer.py | 84 ++++--------------- tests/gym/envs/expert_program/test_catalog.py | 9 +- .../test_simulation_environment.py | 2 +- .../test_task_vertical_slices.py | 68 +++++++++++++-- .../test_expert_program_rollout_report.py | 2 +- 12 files changed, 110 insertions(+), 201 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index e041c2d82..377da80d2 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -310,6 +310,10 @@ 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. +Their payloads carry task intent, while the selected `SkillPolicyPreset` is the +sole action-option source; a lowerer may read its owned option template for goal +grounding but must not mirror those options into the payload as a second policy +configuration. `SemanticSkillCompiler.analyze()` performs provider-free linking, resource and affordance validation, held-object flow analysis, and first-release look-ahead. diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index d10bcf087..219fab4c3 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -341,7 +341,7 @@ Drawer likewise has one canonical integration, `ExpertProgramOpenDrawer-v1`. | Environment ID | Declarative path | Atomic path | Application acceptance | |---|---|---|---| | `ExpertProgramRepeatedPickPlace-v1` | schema-v2 `Repeat(Segment(Sequence(Pick, Place)))` with a cyclic pose target | built-in `PickUp` and `Place` through the semantic compiler; the task installs no contact or constraint observer | standard `object_near_target` validator checks the measured cube position against the selected cyclic target; physical rollout remains unqualified without grasp evidence | -| `ExpertProgramOpenDrawer-v1` | registered `embodichain_tasks.open_drawer` call with a strict executable-free payload | a task-owned `RegisteredSemanticLowerer` produces the built-in `SlideGoal` and `SlideOptions` for the live drawer-handle link | standard `articulation_joint_position` validator checks the measured passive drawer joint against the configured threshold | +| `ExpertProgramOpenDrawer-v1` | registered `embodichain_tasks.open_drawer` call whose executable-free payload names only the drawer handle | a task-owned `RegisteredSemanticLowerer` produces the built-in `SlideGoal`; the selected policy preset is the sole owner of `SlideOptions` | standard `articulation_joint_position` validator checks the measured passive drawer joint against the configured threshold | Both configurations load their Expert Program through the top-level `expert_program_path`, bind the same UR5 parallel-gripper embodiment explicitly, diff --git a/docs/design/expert_program_rollout_report.md b/docs/design/expert_program_rollout_report.md index 9bd33db4e..eac68132f 100644 --- a/docs/design/expert_program_rollout_report.md +++ b/docs/design/expert_program_rollout_report.md @@ -46,8 +46,8 @@ 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 | 197 | -401 (-67.1%) | 23912 | 6561 | -17351 (-72.6%) | `embodichain_tasks/embodichain_tasks/expert_program/repeated_pick_place.py`
`embodichain_tasks/configs/expert_program/repeated_pick_place.yaml` | -| Drawer | 245 | 371 | +126 (+51.4%) | 8833 | 13434 | +4601 (+52.1%) | `embodichain_tasks/embodichain_tasks/expert_program/open_drawer.py`
`embodichain_tasks/configs/expert_program/open_drawer.yaml` | -| Total | 843 | 568 | -275 (-32.6%) | 32745 | 19995 | -12750 (-38.9%) | the four files above | +| Drawer | 245 | 313 | +68 (+27.8%) | 8833 | 11352 | +2519 (+28.5%) | `embodichain_tasks/embodichain_tasks/expert_program/open_drawer.py`
`embodichain_tasks/configs/expert_program/open_drawer.yaml` | +| Total | 843 | 510 | -333 (-39.5%) | 32745 | 17913 | -14832 (-45.3%) | the four files above | ## Demo Success Measurement diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index 0db8ff85f..ca242f2eb 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -36,7 +36,6 @@ 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 ( @@ -100,10 +99,7 @@ ) from .bridge import RuntimeTransportActionEncoder from .extensions import ( - EndpointAdapterDeclaration, ParallelCommandSafetyValidatorFactory, - ParallelSafetyDeclaration, - RuntimeTransportDeclaration, StandardExtensionDeclarations, build_standard_extension_declarations, validate_immutable_extension_declaration, @@ -425,16 +421,8 @@ class ExpertProgramIntegrationCatalog: call_catalog: SemanticCallCatalog relation_grounder_keys: frozenset[tuple[str, type[Affordance], str]] settle_preset_ids: frozenset[str] - endpoint_adapter_declarations: Mapping[ - type[ResourceEndpoint], EndpointAdapterDeclaration - ] - runtime_transport_declarations: tuple[RuntimeTransportDeclaration, ...] - parallel_safety_declaration: ParallelSafetyDeclaration | None + extensions: StandardExtensionDeclarations 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"): @@ -452,36 +440,18 @@ def __post_init__(self) -> None: "relation_grounder_keys", _snapshot_relation_grounder_keys(self.relation_grounder_keys), ) - extensions = StandardExtensionDeclarations( - endpoint_adapters=self.endpoint_adapter_declarations, - runtime_transports=self.runtime_transport_declarations, - parallel_safety=self.parallel_safety_declaration, - ) + if type(self.extensions) is not StandardExtensionDeclarations: + raise TypeError("extensions must be exactly StandardExtensionDeclarations.") 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): + if profile_endpoint_types != frozenset(self.extensions.endpoint_adapters): raise ValueError( - "endpoint_adapter_declarations must cover every exact robot " + "extensions.endpoint_adapters 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) @@ -496,11 +466,6 @@ def __post_init__(self) -> None: ) ): raise ValueError("fingerprint must be a lowercase SHA-256 digest.") - object.__setattr__( - self, - "_required_skills", - MappingProxyType(dict(self._required_skills)), - ) def validate_integration( self, @@ -658,7 +623,7 @@ def preflight(self, program: ExpertProgramCfg) -> CompiledProgram: for segment in compiled.iter_segments(): if ( segment.parallel_block is not None - and self.parallel_safety_declaration is None + and self.extensions.parallel_safety is None ): raise ExpertProgramValidationError( "parallel_safety_factory_not_registered", @@ -686,7 +651,10 @@ 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(): + for descriptor in self.call_catalog.descriptors.values(): + skill_id = descriptor.skill_id + expected = descriptor.target_descriptor + assert skill_id is not None and expected is not None actual = engine.skills.get(skill_id) if actual != expected: raise IntegrationFingerprintMismatch( @@ -714,7 +682,7 @@ def validate_bound_endpoint_extensions( transport_owner_by_target_type = { target_type: transport - for transport in self.runtime_transport_declarations + for transport in self.extensions.runtime_transports for target_type in transport.target_types } expected_resource_ids = frozenset(self.robot_profile.resources) @@ -757,7 +725,7 @@ def validate_bound_endpoint_extensions( "registered robot profile." ) endpoint_type = type(endpoint.endpoint) - declaration = self.endpoint_adapter_declarations.get(endpoint_type) + declaration = self.extensions.endpoint_adapters.get(endpoint_type) if declaration is None: raise IntegrationFingerprintMismatch( f"Bound endpoint {location!r} has undeclared exact type " @@ -1072,7 +1040,6 @@ def __post_init__(self) -> None: 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) @@ -1082,7 +1049,6 @@ def __post_init__(self) -> None: f"{descriptor.skill_id!r}, which is not installed by the " "standard simulation factory." ) - required_skills[descriptor.skill_id] = target fingerprint = _digest( _registration_payload( @@ -1112,11 +1078,8 @@ def __post_init__(self) -> None: call_catalog=self.call_catalog, relation_grounder_keys=relation_grounder_keys, settle_preset_ids=frozenset(settle_presets), - endpoint_adapter_declarations=extensions.endpoint_adapters, - runtime_transport_declarations=extensions.runtime_transports, - parallel_safety_declaration=extensions.parallel_safety, + extensions=extensions, fingerprint=fingerprint, - _required_skills=required_skills, ), ) object.__setattr__(self, "_parallel_safety_validator_history", []) diff --git a/embodichain/lab/gym/envs/expert_program/environment.py b/embodichain/lab/gym/envs/expert_program/environment.py index 74060b761..b71e7815b 100644 --- a/embodichain/lab/gym/envs/expert_program/environment.py +++ b/embodichain/lab/gym/envs/expert_program/environment.py @@ -683,9 +683,7 @@ def _assemble_execution_runtime( if self._registration is not None: expected_transport_ids = tuple( declaration.transport_id - for declaration in ( - self._registration.catalog.runtime_transport_declarations - ) + for declaration in self._registration.catalog.extensions.runtime_transports ) include_joint_position = ( JointPositionGymTransportEncoder.transport_id in expected_transport_ids diff --git a/embodichain/lab/gym/envs/expert_program/extensions.py b/embodichain/lab/gym/envs/expert_program/extensions.py index a0d549f02..508e9691b 100644 --- a/embodichain/lab/gym/envs/expert_program/extensions.py +++ b/embodichain/lab/gym/envs/expert_program/extensions.py @@ -788,8 +788,6 @@ def build_standard_extension_declarations( 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 @@ -801,16 +799,14 @@ def build_standard_extension_declarations( "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 - } + declared_transport_ids = set(transport_ids) 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 + missing_transports = required_transport_ids - declared_transport_ids + unused_transports = declared_transport_ids - required_transport_ids unused_transports.discard(JointPositionTarget.TRANSPORT_ID) if missing_transports or unused_transports: raise ValueError( @@ -820,57 +816,6 @@ def build_standard_extension_declarations( ) 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 @@ -878,7 +823,9 @@ def build_standard_extension_declarations( else declare_parallel_safety_factory(parallel_safety_factory) ) if parallel_safety is not None: - installed_transport_ids = frozenset(transport_by_id) + installed_transport_ids = frozenset( + declaration.transport_id for declaration in transport_declarations + ) if parallel_safety.supported_transport_ids != installed_transport_ids: raise ValueError( "parallel_safety_factory must support exactly the registered " diff --git a/embodichain_tasks/configs/expert_program/open_drawer.yaml b/embodichain_tasks/configs/expert_program/open_drawer.yaml index 5efca492b..c21d203ca 100644 --- a/embodichain_tasks/configs/expert_program/open_drawer.yaml +++ b/embodichain_tasks/configs/expert_program/open_drawer.yaml @@ -16,10 +16,6 @@ program: schema_version: 1 arguments: handle: drawer_handle - direction: pull - hand_interp_steps: 12 - approach_distance: 0.10 - translation_distance: 0.18 resources: primary: manipulator post: diff --git a/embodichain_tasks/embodichain_tasks/expert_program/open_drawer.py b/embodichain_tasks/embodichain_tasks/expert_program/open_drawer.py index 54a61cffa..c537262c7 100644 --- a/embodichain_tasks/embodichain_tasks/expert_program/open_drawer.py +++ b/embodichain_tasks/embodichain_tasks/expert_program/open_drawer.py @@ -79,24 +79,6 @@ _SLIDE_DESCRIPTOR = Slide.descriptor() -def _finite_float(value: object, *, field_name: str, positive: bool) -> float: - """Return one finite declarative number.""" - if type(value) not in (int, float): - raise TypeError(f"{field_name} must be an int or float.") - normalized = float(value) - if not math.isfinite(normalized) or (positive and normalized <= 0.0): - qualifier = "finite and positive" if positive else "finite" - raise ValueError(f"{field_name} must be {qualifier}.") - return normalized - - -def _positive_int(value: object, *, field_name: str) -> int: - """Return one exact positive integer.""" - if type(value) is not int or value < 1: - raise ValueError(f"{field_name} must be a positive integer.") - return value - - def _axis(value: object) -> tuple[float, float, float]: """Return one finite non-zero three-dimensional axis.""" if not isinstance(value, Sequence) or isinstance(value, (str, bytes)): @@ -137,63 +119,27 @@ def lower( del context, bound if type(option_template) is not SlideOptions: raise TypeError("Open Drawer requires an exact SlideOptions template.") - expected_keys = { - "handle", - "direction", - "hand_interp_steps", - "approach_distance", - "translation_distance", + arguments = dict(call.arguments) + task_arguments = {"handle": HANDLE_ENTITY_ID} + legacy_arguments = { + **task_arguments, + "direction": option_template.direction, + "hand_interp_steps": option_template.hand_interp_steps, + "approach_distance": option_template.approach_distance, + "translation_distance": option_template.translation_distance, } - if set(call.arguments) != expected_keys: - raise ValueError( - f"{OPEN_DRAWER_CALL_ID} arguments must be exactly " - f"{sorted(expected_keys)}." - ) - handle = call.arguments["handle"] - if type(handle) is not str or handle != HANDLE_ENTITY_ID: - raise ValueError( - f"handle must be exactly the canonical ID {HANDLE_ENTITY_ID!r}." - ) - direction = call.arguments["direction"] - if type(direction) is not str or direction not in ("pull", "push"): - raise ValueError("direction must be exactly 'pull' or 'push'.") - hand_interp_steps = _positive_int( - call.arguments["hand_interp_steps"], - field_name="hand_interp_steps", - ) - approach_distance = _finite_float( - call.arguments["approach_distance"], - field_name="approach_distance", - positive=False, - ) - if approach_distance < 0.0: - raise ValueError("approach_distance must be non-negative.") - translation_distance = _finite_float( - call.arguments["translation_distance"], - field_name="translation_distance", - positive=True, - ) - declared_values = ( - direction, - hand_interp_steps, - approach_distance, - translation_distance, - ) - configured_values = ( - option_template.direction, - option_template.hand_interp_steps, - option_template.approach_distance, - option_template.translation_distance, - ) - if declared_values != configured_values: + if arguments not in (task_arguments, legacy_arguments): raise ValueError( - "Open Drawer call arguments must match the registered policy " - "preset's SlideOptions template." + f"{OPEN_DRAWER_CALL_ID} arguments must name only the canonical " + "handle; legacy option fields, when present, must exactly match " + "the selected policy preset." ) + handle = arguments["handle"] + assert type(handle) is str return SemanticLowering( goal=SlideGoal( semantics=self._semantics, - target_pose=SceneEntityPose(HANDLE_ENTITY_ID), + target_pose=SceneEntityPose(handle), ) ) diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py index 92df4f0d4..60a7a538f 100644 --- a/tests/gym/envs/expert_program/test_catalog.py +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -377,11 +377,8 @@ def _place_relation_catalog( call_catalog=base.call_catalog, relation_grounder_keys=grounder_keys, 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, + extensions=base.extensions, fingerprint="0" * 64, - _required_skills={}, ) @@ -468,12 +465,12 @@ def test_catalog_declares_builtin_endpoint_and_ordered_transport_contracts() -> """The standard provider-free catalog contains its exact built-in wiring.""" catalog = _registration().catalog - adapter = catalog.endpoint_adapter_declarations[ControlPartEndpoint] + adapter = catalog.extensions.endpoint_adapters[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 + value.transport_id for value in catalog.extensions.runtime_transports ) == ("robot.joint_position",) diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 2e594370e..1ff6dfd8e 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -1637,7 +1637,7 @@ def test_standard_registration_owns_and_freezes_live_runtime_assembly() -> None: assert factory.expert_program_registration is registration assert assembly.command_encoder.transport_ids == tuple( declaration.transport_id - for declaration in registration.catalog.runtime_transport_declarations + for declaration in registration.catalog.extensions.runtime_transports ) assert assembly.command_encoder.is_frozen 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 8a3430443..8f651b413 100644 --- a/tests/gym/envs/expert_program/test_task_vertical_slices.py +++ b/tests/gym/envs/expert_program/test_task_vertical_slices.py @@ -36,7 +36,14 @@ EnvironmentStepClock, RuntimeCommandFrameEncoder, ) -from embodichain.lab.sim.atomic_actions import Affordance, EntityState, TaskState +from embodichain.lab.sim.atomic_actions import ( + Affordance, + EntityState, + ObjectSemantics, + SlideAffordance, + SlideOptions, + TaskState, +) from embodichain.lab.sim.skills.calls import Pick, Place, RegisteredSemanticCall from embodichain.lab.sim.skills.runtime import SkillResult, SkillStatus from embodichain.lab.sim.skills.scene import ( @@ -529,10 +536,6 @@ def test_open_drawer_program_compiles_to_registered_slide_call() -> None: assert call.call_id == "embodichain_tasks.open_drawer" assert dict(call.arguments) == { "handle": "drawer_handle", - "direction": "pull", - "hand_interp_steps": 12, - "approach_distance": 0.10, - "translation_distance": 0.18, } assert dict(call.resources) == {"primary": "manipulator"} validator = segments[0].validators[0].cfg @@ -541,6 +544,61 @@ def test_open_drawer_program_compiles_to_registered_slide_call() -> None: assert validator.minimum_position == 0.10 +def test_open_drawer_lowerer_preserves_legacy_payload_compatibility() -> None: + """Schema-v1 option fields remain accepted only as preset-matching input.""" + options = SlideOptions( + direction="pull", + hand_interp_steps=12, + approach_distance=0.10, + translation_distance=0.18, + ) + lowerer = drawer_task._OpenDrawerSlideLowerer( + ObjectSemantics( + label="drawer_handle", + entity_id=drawer_task.HANDLE_ENTITY_ID, + geometry={}, + affordance=SlideAffordance( + mesh_vertices=torch.tensor( + [[-0.1, 0.0, 0.0], [0.1, 0.0, 0.0], [0.0, 0.0, 0.0]] + ), + mesh_triangles=torch.tensor([[0, 1, 2]]), + translation_axis=torch.tensor([0.0, 1.0, 0.0]), + ), + ) + ) + minimal = {"handle": drawer_task.HANDLE_ENTITY_ID} + legacy = { + **minimal, + "direction": options.direction, + "hand_interp_steps": options.hand_interp_steps, + "approach_distance": options.approach_distance, + "translation_distance": options.translation_distance, + } + + for arguments in (minimal, legacy): + lowering = lowerer.lower( + RegisteredSemanticCall( + call_id=drawer_task.OPEN_DRAWER_CALL_ID, + arguments=arguments, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=options, + ) + assert lowering.goal.target_pose.entity_id == drawer_task.HANDLE_ENTITY_ID + + with pytest.raises(ValueError, match="legacy option fields"): + lowerer.lower( + RegisteredSemanticCall( + call_id=drawer_task.OPEN_DRAWER_CALL_ID, + arguments={**legacy, "direction": "push"}, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=options, + ) + + def test_task_classes_do_not_override_motion_or_demo_generation() -> None: """Both environments delegate planning and execution to the shared runtime.""" forbidden_overrides = { diff --git a/tests/scripts/tools/test_expert_program_rollout_report.py b/tests/scripts/tools/test_expert_program_rollout_report.py index a7e9a5e23..d20b49f4c 100644 --- a/tests/scripts/tools/test_expert_program_rollout_report.py +++ b/tests/scripts/tools/test_expert_program_rollout_report.py @@ -29,7 +29,7 @@ EXPECTED_CURRENT_COUNTS = { # Each tuple is (raw LF bytes, raw file bytes) for the explicit task pair. "Cube": (197, 6_561), - "Drawer": (371, 13_434), + "Drawer": (313, 11_352), } EXPECTED_SOURCE_PATHS = {