From a7b2be5dc15e821d89f2f37607bd9e24e9a3ef41 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 20:28:33 +0800 Subject: [PATCH 01/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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(