From 01babb85775d512aa3d6f6940ac9b978170c02d5 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 20:28:33 +0800 Subject: [PATCH 1/9] 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 | 2 +- .../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(+), 5 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index a035d16f9..76fb32fe9 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 2161e529e..e64af32b7 100644 --- a/embodichain/lab/sim/atomic_actions/trajectory_ops.py +++ b/embodichain/lab/sim/atomic_actions/trajectory_ops.py @@ -181,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((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 e6713bd51..86b9a3cff 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( (num_envs,), success, dtype=torch.bool, device=resolved_device diff --git a/tests/sim/atomic_actions/test_trajectory_ops.py b/tests/sim/atomic_actions/test_trajectory_ops.py index ca4696d14..47e1b3cf6 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 79d2c27da7ec34bca524860e2e490c443040d4c0 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 20:28:40 +0800 Subject: [PATCH 2/9] 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 2c58d9899..3373c2b19 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 @@ -332,7 +354,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 @@ -463,13 +485,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 @@ -483,8 +506,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. @@ -502,23 +525,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 @@ -542,7 +569,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; @@ -559,7 +586,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 | @@ -641,20 +668,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 @@ -703,16 +739,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 @@ -740,7 +777,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. @@ -789,7 +826,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 @@ -802,7 +840,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; @@ -834,9 +873,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 875d3318d63aef873974e5462ec20c64abf87121 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 21:16:24 +0800 Subject: [PATCH 3/9] feat(atomic-actions): add stable snapshot identity --- .../lab/sim/atomic_actions/affordance.py | 40 ++- embodichain/lab/sim/atomic_actions/core.py | 52 +++- embodichain/lab/sim/atomic_actions/effects.py | 6 +- embodichain/lab/sim/atomic_actions/goals.py | 43 +++ embodichain/lab/sim/atomic_actions/state.py | 6 +- tests/sim/atomic_actions/test_affordance.py | 17 +- tests/sim/atomic_actions/test_core.py | 279 ++++++++++++++++++ 7 files changed, 427 insertions(+), 16 deletions(-) diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index 7c9b93652..34200f04f 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,43 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: Returns: Assemble-object target pose with shape ``(num_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 (num_envs, 4, 4)." + ) num_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(num_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 (num_envs, 4, 4)." + ) elif rel.shape[0] == 1: rel = rel.repeat(num_envs, 1, 1) + elif rel.shape[0] != num_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 947522ed0..5e9044201 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -71,9 +71,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.""" @@ -90,6 +96,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.") @@ -99,9 +108,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.""" @@ -432,6 +471,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], @@ -524,7 +570,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 3fddb0d6d..324260297 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -46,6 +46,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: @@ -63,7 +65,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 " @@ -74,7 +76,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 ), diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index 10d00e372..bb6502567 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, TYPE_CHECKING @@ -151,13 +152,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/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index f54b772f6..f168e9c06 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -33,9 +33,9 @@ def _same_physical_object( second: ObjectSemantics, ) -> bool: """Return whether two semantic records identify one physical object.""" - if first is second: - return True - return first.entity is not None and first.entity is second.entity + from .core import _same_object_identity + + return _same_object_identity(first, second) def _resolve_runtime_device(device: torch.device | str) -> torch.device: diff --git a/tests/sim/atomic_actions/test_affordance.py b/tests/sim/atomic_actions/test_affordance.py index 9bae52aba..097a1ffe8 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 == (num_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 66121e506..18c251784 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -27,15 +27,21 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionOptions, + ActionPlan, Affordance, + AtomicAction, DynamicCollisionMode, EndEffectorPoseGoal, EntityState, HeldObjectState, MotionPolicy, ObjectSemantics, + PlannerDiagnostics, PlanningContext, RecoveryPolicy, + ResolvedActionBinding, + ResolvedActionRequest, RobotObservation, SceneEntityPose, SceneSnapshot, @@ -44,6 +50,7 @@ TimedTrajectory, ) from embodichain.lab.sim.atomic_actions.goals import ( + _resolve_object_pose, collect_scene_dependencies, resolve_pose_goal, ) @@ -54,12 +61,14 @@ def _semantics( label: str = "object", *, entity: BatchEntity | None = None, + entity_id: str | None = None, ) -> ObjectSemantics: return ObjectSemantics( affordance=Affordance(), geometry={}, label=label, entity=entity, + entity_id=entity_id, ) @@ -88,6 +97,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"}, @@ -101,6 +146,22 @@ def test_action_binding_is_role_based_and_immutable() -> None: with pytest.raises(KeyError, match="destination"): binding.manipulator("destination") +@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) @@ -205,6 +266,134 @@ def test_task_state_reports_per_environment_exclusive_holds() -> None: assert state.held_object_mask("missing").tolist() == [False, False] +def test_task_state_treats_matching_entity_ids_as_shared() -> None: + state = TaskState( + batch_size=1, + device="cpu", + held_objects={ + "left_arm": _held( + batch_size=1, + semantics=_semantics(entity_id="tray"), + ), + "right_arm": _held( + batch_size=1, + semantics=_semantics(entity_id="tray"), + ), + }, + ) + + assert state.exclusive_held_object_mask("left_arm").tolist() == [False] + assert state.exclusive_held_object_mask("right_arm").tolist() == [False] + + +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_robot_observation_owns_input_tensors() -> None: qpos = torch.zeros(2, 4) observation = RobotObservation( @@ -255,6 +444,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 0c3a399e37b683f194c5f9d89e672433b0a02357 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 21:16:58 +0800 Subject: [PATCH 4/9] refactor(atomic-actions): ground object motion from snapshots --- .../primitives/coordinated_pickment.py | 34 +- .../atomic_actions/primitives/hand_over.py | 38 ++- .../primitives/move_held_object.py | 19 +- .../sim/atomic_actions/primitives/pick_up.py | 52 ++- .../sim/atomic_actions/primitives/place.py | 87 ++++- .../atomic_action/moving_target_recovery.py | 1 + tests/sim/atomic_actions/test_actions.py | 310 ++++++++++++++++-- 7 files changed, 453 insertions(+), 88 deletions(-) diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index dae604573..64a6ff5b0 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, ) @@ -62,7 +63,11 @@ class CoordinatedPickGoal(ObjectActionGoal): """Target pose for the shared object, shape ``(4, 4)`` or ``(num_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) @@ -338,6 +343,22 @@ class CoordinatedPickment( _resolve_dual_arm_start = _DualArmHelpers._resolve_dual_arm_start _resolve_pose = _DualArmHelpers._resolve_pose + 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], @@ -403,13 +424,12 @@ def _resolve_object_initial_pose( ), "object_initial_pose", ) - if target.semantics.entity is None: - raise ValueError( - "CoordinatedPickGoal requires object_initial_pose when " - "semantics.entity is not provided." - ) 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 0007fa968..78e146314 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 @@ -143,6 +143,14 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): end_effector_roles: ClassVar[tuple[str, ...]] = ("source", "destination") _repeat_qpos = staticmethod(repeat_qpos) + 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], @@ -235,6 +243,10 @@ def _plan( transfer_held_object.object_to_eef, "held_object.object_to_eef", ) + 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 middle_object_pose = self._resolve_matrix( @@ -250,10 +262,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. - if semantics.entity is None: - raise ValueError("HandOver requires the held object to have an entity.") - current_object_pose = 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] @@ -297,9 +316,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 ) @@ -547,11 +563,7 @@ def _validate_requested_object( held: ObjectSemantics, ) -> None: """Reject a request that names a different grounded object.""" - if ( - requested.entity is not None - and held.entity is not None - and requested.entity is not held.entity - ): + if not _same_object_identity(requested, held): raise ValueError( "HandOver goal semantics must identify the object held by the " "source control part." 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 11e73d5e3..a6ae5aa7a 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -23,7 +23,11 @@ import torch -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 @@ -130,18 +134,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.num_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.num_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 a6b8d703f..d8cd16d4b 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, ) @@ -153,6 +154,17 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) + 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, @@ -280,21 +292,29 @@ 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 ): raise ValueError( "PickUp requires an AntipodalAffordance when grasp_xpos is not set." ) - if sem.entity is None: - raise ValueError("PickUp requires an entity on the target semantics.") 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( @@ -304,7 +324,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.num_envs, dtype=torch.bool, device=self.device) grasp_success = normalize_success_mask( @@ -338,8 +360,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 ) @@ -355,18 +376,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, ) - num_envs = obj_poses.shape[0] + num_envs = object_pose.shape[0] n_max_pose = max(r[0].shape[0] for r in grasp_poses_result) grasp_xpos_padding = torch.zeros( (num_envs, n_max_pose, 4, 4), dtype=torch.float32, device=self.device @@ -390,10 +411,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, @@ -408,7 +428,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, @@ -423,7 +442,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() @@ -557,8 +578,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.""" @@ -573,8 +594,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 b1103d0db..023e76b52 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 @@ -30,7 +31,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 @@ -76,16 +82,29 @@ 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. """ 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): @@ -124,9 +143,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" @@ -138,6 +158,17 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) + 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], @@ -316,7 +347,7 @@ def _resolve_assemble_place_xpos( Place EEF poses with shape ``(num_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: @@ -325,14 +356,36 @@ def _resolve_assemble_place_xpos( f"part {control_part!r} (run PickUp first)." ) affordance = target.affordance - if affordance.base_object_entity is None: - raise ValueError( - "AssembleAffordance.base_object_entity must be set to assemble " - "onto a base object." + if target.base_pose is not None: + base_pose = resolve_object_target( + resolve_pose_goal( + target.base_pose, + state, + name="base_pose", + ), + num_envs=self.num_envs, + device=self.device, + name="base_pose", + ) + else: + if affordance.base_object_entity is None: + raise ValueError( + "AssembleGoal requires base_pose or " + "AssembleAffordance.base_object_entity." + ) + 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), + num_envs=self.num_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 5427fdb3e..06bafe4da 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(spec=BatchEntity) 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, ) @@ -367,12 +370,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), ) @@ -518,8 +525,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( @@ -609,15 +617,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_move_held_object_moves_only_exclusively_held_rows() -> None: @@ -832,32 +868,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", @@ -875,6 +920,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( @@ -883,7 +929,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, @@ -916,6 +968,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)) @@ -959,6 +1012,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, @@ -1003,16 +1057,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 @@ -1063,15 +1127,24 @@ def test_handover_does_not_mutate_cached_final_pose( ) 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)) @@ -1094,7 +1167,10 @@ def 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), ) @@ -1102,7 +1178,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", @@ -1135,7 +1222,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", @@ -1167,7 +1254,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] @@ -1179,6 +1267,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_handover_transfers_only_exclusively_held_rows() -> None: @@ -1250,8 +1370,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", @@ -1265,7 +1391,8 @@ 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] @@ -1276,6 +1403,8 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None assert isinstance(right_held, HeldObjectState) assert left_held.semantics is right_held.semantics assert left_held.semantics is not semantics + assert plan.scene_dependencies == () + request.goal.semantics.entity.get_local_pose.assert_not_called() assert [segment.name for segment in plan.segments] == [ "approach", "close", @@ -1285,6 +1414,131 @@ 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() + left_held = projected.get_held_object("left_arm") + right_held = projected.get_held_object("right_arm") + assert left_held is not None and right_held is not None + assert left_held.semantics is right_held.semantics + assert torch.allclose(left_held.object_to_eef, pose_inv(object_pose)) + assert torch.allclose(right_held.object_to_eef, pose_inv(object_pose)) + + +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 b7128611c10b5da090b439a1d4fe3a99ae2556f3 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 21:17:09 +0800 Subject: [PATCH 5/9] 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 | 105 ++++-- .../overview/sim/atomic_actions/index.md | 16 +- .../overview/sim/planners/curobo_planner.md | 6 + docs/source/tutorial/atomic_actions.rst | 18 +- 7 files changed, 463 insertions(+), 110 deletions(-) diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index b966ebf5b..5c66b2dfc 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 @@ -486,6 +498,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 76fb32fe9..ed82a12ce 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()`. Custom actions must be installed with `engine.register()` before using the same public entry points. +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: @@ -295,7 +365,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 @@ -305,13 +385,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 3373c2b19..8ec0009b1 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: @@ -273,18 +280,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: @@ -315,6 +366,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`. @@ -327,7 +386,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** @@ -337,11 +397,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 @@ -351,6 +418,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; @@ -532,12 +600,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 @@ -575,9 +655,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 @@ -601,7 +688,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. @@ -643,12 +733,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 | | +---------------+--------------+ @@ -666,7 +760,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: @@ -677,34 +771,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 @@ -727,9 +893,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 @@ -763,12 +932,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 @@ -819,9 +989,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; @@ -834,9 +1011,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 @@ -859,8 +1038,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 @@ -868,16 +1048,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. @@ -895,7 +1080,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 168a947bf..0205dcc8e 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, or the deprecated live `entity` fallback is available; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | | Effect | write `HeldObjectState` for the bound manipulator | | 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 | |---|---| @@ -402,27 +444,28 @@ 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 and that -attachment must be exclusive. 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, and +that attachment must be exclusive. `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. Planning declares +the same detach effect as a normal place. -**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)= @@ -461,7 +504,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 | write one `HeldObjectState` per bound manipulator; both entries share the same object semantics | | Verification | coordinated attachment must be externally verified | @@ -477,9 +520,13 @@ can inspect those per-manipulator entries directly; sharing the same release, and handover skills reject those shared rows rather than moving or detaching just one participant. -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: @@ -558,9 +605,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 dac4e8c52..683be6a31 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -367,6 +367,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 @@ -703,7 +708,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 bb65e7e0e..ecbeb106f 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -84,6 +84,11 @@ additional application execution entry point. Atomic-action authors implement the protected ``_plan()`` hook instead. Register custom action instances with ``engine.register()`` before using the same public planning entry points. +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 ----------------- @@ -360,10 +365,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 ------------------ @@ -396,7 +403,8 @@ Define an action-owned frozen goal dataclass. 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 b3de29aadd89402b3b2c10318f973147d2035666 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 22:27:12 +0800 Subject: [PATCH 6/9] 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 f168e9c06..a94c368ce 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 @@ -358,6 +359,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.""" @@ -422,7 +447,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 0b27f91c5..f26d411da 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 18c251784..954e063ed 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -553,6 +553,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 c6957718ee54570f143b2302ac66761df99f5265 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 22:30:41 +0800 Subject: [PATCH 7/9] 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 5c66b2dfc..d39db66f9 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 @@ -515,6 +553,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 ed82a12ce..bf2dcea77 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 8ec0009b1..3cd0b326b 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 @@ -288,26 +298,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. @@ -330,11 +375,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 @@ -736,11 +784,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 @@ -779,11 +828,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: @@ -823,17 +872,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: @@ -843,16 +893,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: @@ -989,8 +1058,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 683be6a31..ee889e568 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 | @@ -319,6 +319,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 @@ -720,6 +746,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 ecbeb106f..e26d7d5e5 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: @@ -257,7 +258,6 @@ must be resolved from the latest scene snapshot: from embodichain.lab.sim.atomic_actions import ( EndEffectorPoseGoal, RecoveryPolicy, - RigidObjectSceneProvider, SceneEntityPose, ) @@ -279,8 +279,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, @@ -323,13 +331,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 279735fca0cc3d9cde1e664ecc64faf64f0e1a2b Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 23:40:03 +0800 Subject: [PATCH 8/9] feat(sim): add declarative robot skill profiles --- agent_context/MAP.yaml | 39 + .../topics/atomic-actions/atomic-actions.md | 146 +- .../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, 4732 insertions(+), 66 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 d39db66f9..df7ac14aa 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 @@ -507,6 +512,38 @@ topics: - held_objects - HeldObjectState - 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 @@ -542,6 +579,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 @@ -554,6 +592,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 bf2dcea77..06bcf00c1 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 @@ -370,9 +490,12 @@ There is no `ActionCfg` or built-in `*Cfg` layer. 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. -Registration is engine-local; there is no independent process-wide action -catalog. Construct extensions explicitly and install them with +has compatible control parts, profiles, bindings, or task state. `engine.skills` +contains only agent-visible installed actions whose concrete classes explicitly +declare a `binding_contract`; when a robot profile is bound, +`engine.skill_profile.skills` further filters that catalog to valid resource +assignments. Registration is engine-local; there is no independent process-wide +action catalog. Construct extensions explicitly and install them with `engine.register()` so discovery and execution cannot observe disconnected registries. @@ -394,8 +517,9 @@ the same semantic object or live entity. Single-arm transport, release, and handover operations only succeed on exclusive rows; coordinated placement likewise requires two distinct, exclusively held objects. -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( @@ -414,7 +538,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 @@ -449,7 +577,9 @@ snapshot-grounded object example. 1. Define a frozen action-owned goal dataclass. 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 3cd0b326b..e8226f62b 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`, @@ -386,29 +391,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 @@ -766,10 +849,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 @@ -789,7 +873,7 @@ PR1 snapshot/identity bridge (complete) +-----------------------+ v v PR2A SceneRegistry PR2B RobotSkillProfile - (implemented) (next) + (implemented) (implemented) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -922,14 +1006,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 @@ -1108,6 +1209,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 b05db79b4..3c9703039 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 @@ -31,6 +31,18 @@ embodichain.lab.sim.atomic_actions ActionPlan CompiledTrajectory + .. rubric:: Semantic resource contracts + + .. autosummary:: + + SkillDescriptor + SkillBindingContract + SkillResourceSlot + SkillEndpointRequirement + ActionBindingRoute + DisjointSlotEndpoints + DisjointResourceSlots + .. rubric:: Execution contracts .. autosummary:: @@ -88,6 +100,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 ee889e568..20c451cf2 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 @@ -187,6 +188,14 @@ 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 @@ -221,11 +230,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 @@ -242,8 +252,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( @@ -324,10 +340,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 2b48fb5d0..99cf83e04 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -65,6 +65,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, @@ -130,6 +144,7 @@ __all__ = [ "ActionBinding", + "ActionBindingRoute", "ActionControlOverrides", "ActionInvocation", "ActionOptions", @@ -142,6 +157,8 @@ "AtomicAction", "AtomicActionEngine", "BUILTIN_ACTION_TYPES", + "BATCH_INVERSE_KINEMATICS_CAPABILITY", + "CARTESIAN_POSE_CAPABILITY", "CompiledTrajectory", "CommandAcknowledgement", "CommandAckStatus", @@ -157,6 +174,8 @@ "CoordinatedPlacementGoal", "CoordinatedPlacementOptions", "DynamicCollisionMode", + "DisjointResourceSlots", + "DisjointSlotEndpoints", "EndEffectorPoseGoal", "EntityState", "EffectVerificationRequest", @@ -170,15 +189,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", @@ -217,6 +240,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 a00d5a30f..614061969 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 5e9044201..d4b7906bb 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -47,6 +47,7 @@ normalize_success_mask, ) from .policies import DynamicCollisionMode +from .requirements import SkillBindingContract if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -151,6 +152,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: @@ -173,6 +176,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): @@ -201,6 +214,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) @@ -298,6 +319,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 e96b0f738..c52d724ea 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 @@ -45,6 +52,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. @@ -53,14 +64,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: @@ -92,6 +128,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. @@ -116,6 +208,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 64a6ff5b0..84d1d7f5b 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 HeldObjectState, PlanningContext from ..trajectory_ops import interpolate_joint_trajectory, translate_pose_world from ._helpers import ( @@ -332,6 +342,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 c287d6111..49e606b4d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -26,12 +26,22 @@ from embodichain.utils import logger 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 ..requirements import ( + ActionBindingRoute, + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( interpolate_hand_qpos, @@ -134,6 +144,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")),), + ) _repeat_qpos = staticmethod(repeat_qpos) def _resolve_resources( diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 78e146314..de1379076 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -27,11 +27,22 @@ 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 ..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 ( interpolate_hand_qpos, @@ -141,6 +152,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")),), + ) _repeat_qpos = staticmethod(repeat_qpos) def _scene_dependencies( 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 9a49b3c73..2c28d8041 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, @@ -56,6 +63,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 a6ae5aa7a..3b28b6fdf 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -30,11 +30,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 @@ -86,6 +96,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 _plan( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index 12db8e480..4cea2ab34 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, @@ -73,6 +80,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 _plan( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index d8cd16d4b..d2d71bf43 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, @@ -153,6 +164,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 _scene_dependencies( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index 023e76b52..86ae1f599 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -28,7 +28,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 ( @@ -39,6 +39,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, @@ -157,6 +167,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 _scene_dependencies( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index 888ff0718..d9d3ff920 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -24,11 +24,21 @@ import torch 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, @@ -69,6 +79,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 _plan( 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 83ff88847..e1d20f55b 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(num_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 c4eaf4e3f94574ccec28d2332b557edd93509519 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 18 Aug 2026 09:44:10 +0000 Subject: [PATCH 9/9] style(atomic-actions): format rebased changes --- embodichain/lab/sim/atomic_actions/affordance.py | 4 +--- tests/sim/atomic_actions/test_core.py | 1 + 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index 34200f04f..206aa33ac 100644 --- a/embodichain/lab/sim/atomic_actions/affordance.py +++ b/embodichain/lab/sim/atomic_actions/affordance.py @@ -290,9 +290,7 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: or base_pose.shape[0] == 0 or base_pose.shape[-2:] != (4, 4) ): - raise ValueError( - "base_pose must have shape (4, 4) or (num_envs, 4, 4)." - ) + raise ValueError("base_pose must have shape (4, 4) or (num_envs, 4, 4).") num_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.") diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 954e063ed..5d4a133dd 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -146,6 +146,7 @@ def test_action_binding_is_role_based_and_immutable() -> None: with pytest.raises(KeyError, match="destination"): binding.manipulator("destination") + @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"):