From 1c596fe419574ed092e7dc0193f0e0f87c82f652 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 22:27:12 +0800 Subject: [PATCH 1/2] feat(sim): add authoritative scene registry --- embodichain/lab/sim/atomic_actions/state.py | 29 +- embodichain/lab/sim/planners/base_planner.py | 16 + .../lab/sim/planners/curobo/curobo_planner.py | 120 +- .../lab/sim/planners/curobo/curobo_yaml.py | 46 +- .../lab/sim/planners/motion_generator.py | 121 +- embodichain/lab/sim/skills/__init__.py | 51 + embodichain/lab/sim/skills/scene.py | 1342 +++++++++++++++++ tests/sim/atomic_actions/test_core.py | 48 + tests/sim/planners/test_curobo_planner.py | 177 +++ .../planners/test_motion_generator_batched.py | 139 ++ tests/sim/skills/__init__.py | 19 + tests/sim/skills/test_scene.py | 868 +++++++++++ .../skills/test_scene_curobo_integration.py | 115 ++ 13 files changed, 3048 insertions(+), 43 deletions(-) create mode 100644 embodichain/lab/sim/skills/__init__.py create mode 100644 embodichain/lab/sim/skills/scene.py create mode 100644 tests/sim/skills/__init__.py create mode 100644 tests/sim/skills/test_scene.py create mode 100644 tests/sim/skills/test_scene_curobo_integration.py diff --git a/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index 985599ac3..69cf8d044 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -18,9 +18,10 @@ from __future__ import annotations +from collections.abc import Iterator, Mapping from dataclasses import dataclass, field from types import MappingProxyType -from typing import Mapping, TYPE_CHECKING +from typing import TYPE_CHECKING import torch @@ -423,6 +424,30 @@ def __post_init__(self) -> None: object.__setattr__(self, "pose", self.pose.clone()) +class _ImmutableEntityMapping(Mapping[str, EntityState]): + """Own entity states and return defensive copies on every public read.""" + + __slots__ = ("_states",) + + def __init__(self, states: Mapping[str, EntityState]) -> None: + self._states = MappingProxyType( + { + entity_id: EntityState(state.pose, confidence=state.confidence) + for entity_id, state in states.items() + } + ) + + def __getitem__(self, entity_id: str) -> EntityState: + state = self._states[entity_id] + return EntityState(state.pose, confidence=state.confidence) + + def __iter__(self) -> Iterator[str]: + return iter(self._states) + + def __len__(self) -> int: + return len(self._states) + + @dataclass(frozen=True, slots=True, eq=False) class SceneSnapshot: """Versioned scene state used to ground dynamic goals and obstacles.""" @@ -487,7 +512,7 @@ def __post_init__(self) -> None: "collision_entity_ids reference missing scene entities: " f"{sorted(missing)}." ) - object.__setattr__(self, "entities", MappingProxyType(normalized)) + object.__setattr__(self, "entities", _ImmutableEntityMapping(normalized)) object.__setattr__(self, "collision_entity_ids", collision_entity_ids) def collision_world_revisions(self, batch_size: int) -> tuple[int, ...]: diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index c6f0eecae..d58f7b019 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -22,6 +22,7 @@ from abc import ABC, abstractmethod from collections.abc import Mapping from dataclasses import MISSING +from typing import Literal from embodichain.utils import logger from embodichain.utils import configclass @@ -178,6 +179,21 @@ def __init__(self, cfg: BasePlannerCfg): supports_collision_world_updates: bool = False """Whether per-plan dynamic obstacle poses can update the collision world.""" + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical entity IDs accepted for dynamic pose updates.""" + return () + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every entity ID represented in the planner collision world.""" + return () + + @property + def collision_world_batch_mode(self) -> Literal["shared", "per_env"] | None: + """Return the planner collision world's batch-sharing mode, if any.""" + return None + def supports_move_type(self, move_type: MoveType) -> bool: """Return whether the planner accepts a movement target type directly. diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index 51766737f..6259134de 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -40,7 +40,7 @@ from dataclasses import dataclass from pathlib import Path from types import SimpleNamespace -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Literal import torch import yaml @@ -136,6 +136,25 @@ def __deepcopy__(self, memo: dict) -> "_RigidObjectRefList": # noqa: ARG002 return _RigidObjectRefList(self) +class _RigidObjectRefMapping(dict): + """Registry IDs mapped to live objects without deepcopying their handles.""" + + def __deepcopy__(self, memo: dict) -> "_RigidObjectRefMapping": # noqa: ARG002 + return _RigidObjectRefMapping(self) + + +def _named_rigid_objects( + rigid_objects: list[RigidObject] | Mapping[str, RigidObject] | None, +) -> list[tuple[str, RigidObject]]: + """Return canonical cuRobo obstacle names paired with their live objects.""" + if isinstance(rigid_objects, Mapping): + return list(rigid_objects.items()) + return [ + (getattr(obj, "uid", None) or f"obstacle_{index}", obj) + for index, obj in enumerate(rigid_objects or ()) + ] + + @configclass class CuroboWorldCfg: """Static collision-world configuration for the cuRobo backend. @@ -144,16 +163,19 @@ class CuroboWorldCfg: meshes (see :attr:`rigid_objects`); there is no external scene-YAML path. """ - rigid_objects: list[RigidObject] | None = None - """Live :class:`RigidObject` obstacles to bake into the auto-generated world YAML. + rigid_objects: list[RigidObject] | Mapping[str, RigidObject] | None = None + """Live :class:`RigidObject` obstacles to bake into the generated world YAML. The adapter reads each object's mesh (``get_vertices`` / ``get_triangles``) and world pose (``get_local_pose``) and writes a cuRobo V2 scene YAML (cached - on disk by content hash). Poses are written in the cuRobo world/base frame, - so this is exact when the robot base sits at the simulator world origin. For - obstacles that move or live in an offset base frame, also list their names in - :attr:`dynamic_obstacle_names` to update poses at plan time. ``None`` yields an - initially empty collision world. + on disk by content hash). A mapping is the registry-backed path: its keys are + authoritative obstacle IDs even when they differ from ``RigidObject.uid``. + The list form remains available for advanced callers and derives names from + ``uid`` (or ``obstacle_`` when absent). Poses are written in the cuRobo + world/base frame, so this is exact when the robot base sits at the simulator + world origin. For obstacles that move or live in an offset base frame, also + list their canonical names in :attr:`dynamic_obstacle_names` to update poses + at plan time. ``None`` yields an initially empty collision world. """ obstacle_representation: str = "sphere" @@ -178,7 +200,7 @@ class CuroboWorldCfg: """ dynamic_obstacle_names: list[str] = [] - """Registered rigid-object names whose poses may be updated between plans.""" + """Canonical obstacle IDs whose poses may be updated between plans.""" multi_env: bool = False """Whether cuRobo allocates one collision-world instance per environment. @@ -211,22 +233,45 @@ class CuroboWorldCfg: """ def __post_init__(self) -> None: - dynamic_names = list(self.dynamic_obstacle_names) - if len(set(dynamic_names)) != len(dynamic_names) or not all( - isinstance(name, str) and name for name in dynamic_names + if isinstance(self.dynamic_obstacle_names, (str, bytes)): + raise TypeError( + "dynamic_obstacle_names must be an iterable of obstacle IDs, " + "not a string." + ) + try: + dynamic_names = list(self.dynamic_obstacle_names) + except TypeError as exc: + raise TypeError( + "dynamic_obstacle_names must be an iterable of obstacle IDs." + ) from exc + if not all( + isinstance(name, str) and name and name == name.strip() + for name in dynamic_names ): raise ValueError( - "dynamic_obstacle_names must contain unique non-empty names." + "dynamic_obstacle_names must contain unique non-empty names " + "without outer whitespace." + ) + if len(set(dynamic_names)) != len(dynamic_names): + raise ValueError( + "dynamic_obstacle_names must contain unique non-empty names " + "without outer whitespace." ) - rigid_objects = list(self.rigid_objects or ()) - rigid_names = [ - getattr(obj, "uid", None) or f"obstacle_{index}" - for index, obj in enumerate(rigid_objects) - ] - if not all(isinstance(name, str) and name for name in rigid_names): + if self.rigid_objects is not None and not isinstance( + self.rigid_objects, + (list, Mapping), + ): + raise TypeError("rigid_objects must be a list, mapping, or None.") + named_rigid_objects = _named_rigid_objects(self.rigid_objects) + rigid_names = [name for name, _ in named_rigid_objects] + if not all( + isinstance(name, str) and name and name == name.strip() + for name in rigid_names + ): raise ValueError( - "CuroboWorldCfg.rigid_objects must have non-empty string names." + "CuroboWorldCfg.rigid_objects must have non-empty string obstacle " + "IDs without outer whitespace." ) if len(set(rigid_names)) != len(rigid_names): raise ValueError( @@ -243,7 +288,10 @@ def __post_init__(self) -> None: # Wrap live RigidObjects so the @configclass field-deepcopy (run right # after this by custom_post_init) shares references instead of trying to # pickle non-pickleable C++ dexsim handles held by each RigidObject. - if self.rigid_objects is not None and not isinstance( + if isinstance(self.rigid_objects, Mapping): + if not isinstance(self.rigid_objects, _RigidObjectRefMapping): + self.rigid_objects = _RigidObjectRefMapping(self.rigid_objects) + elif self.rigid_objects is not None and not isinstance( self.rigid_objects, _RigidObjectRefList ): self.rigid_objects = _RigidObjectRefList(self.rigid_objects) @@ -446,7 +494,7 @@ class CuroboPlanOptions(PlanOptions): """EmbodiChain control-part name to plan for.""" dynamic_obstacle_poses: dict[str, torch.Tensor] | None = None - """Per-obstacle world poses ``(B, 4, 4)`` keyed by configured name.""" + """World poses ``(B, 4, 4)`` keyed by canonical dynamic-obstacle ID.""" max_attempts: int | None = None """Per-plan override of ``CuroboPlannerCfg.max_attempts``.""" @@ -493,8 +541,8 @@ def _validate_dynamic_obstacles( """Validate dynamic-obstacle pose names and shapes. Args: - poses: Mapping of obstacle name -> pose tensor. ``None`` is a no-op. - allowed_names: Obstacle names declared in :class:`CuroboWorldCfg`. + poses: Mapping of canonical obstacle ID -> pose tensor. ``None`` is a no-op. + allowed_names: Canonical IDs declared in :class:`CuroboWorldCfg`. Raises: ValueError: If a name is not configured, or a pose is not ``(B, 4, 4)``. @@ -790,6 +838,23 @@ def preserve_plan_samples(self) -> bool: """ return self.cfg.preserve_plan_samples + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical registry IDs accepted for dynamic pose updates.""" + return tuple(self.cfg.world.dynamic_obstacle_names) + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every obstacle ID represented in the generated world.""" + return tuple( + name for name, _ in _named_rigid_objects(self.cfg.world.rigid_objects) + ) + + @property + def collision_world_batch_mode(self) -> Literal["shared", "per_env"]: + """Return the configured collision-world batching policy.""" + return "per_env" if self.cfg.world.multi_env else "shared" + def __init__(self, cfg: CuroboPlannerCfg) -> None: super().__init__(cfg) self.cfg: CuroboPlannerCfg = cfg @@ -1667,8 +1732,7 @@ def _world_yaml_cache_key(self, world_cfg: CuroboWorldCfg) -> str: hasher.update(str(auto.surface_radius).encode("utf-8")) hasher.update(str(auto.iterations).encode("utf-8")) hasher.update(str(auto.collision_sphere_buffer).encode("utf-8")) - for idx, obj in enumerate(world_cfg.rigid_objects or []): - name = getattr(obj, "uid", None) or f"obstacle_{idx}" + for name, obj in _named_rigid_objects(world_cfg.rigid_objects): hasher.update(name.encode("utf-8")) vertices = obj.get_vertices(env_ids=[0], scale=True)[0] faces = obj.get_triangles(env_ids=[0])[0] @@ -2248,8 +2312,8 @@ def update_dynamic_obstacles( """Update named dynamic obstacle poses on cached cuRobo collision worlds. Args: - poses: Mapping of obstacle name -> ``(B, 4, 4)`` world pose. ``None`` - is a no-op. + poses: Mapping of canonical obstacle ID -> ``(B, 4, 4)`` world pose. + ``None`` is a no-op. backend: Specific cached backend to update. If ``None``, updates all cached backends. sim_base_pose_inv: Precomputed inverse of the live sim base pose for diff --git a/embodichain/lab/sim/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/planners/curobo/curobo_yaml.py index 1b24eec68..f470f8e4b 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/planners/curobo/curobo_yaml.py @@ -28,7 +28,8 @@ from __future__ import annotations -from typing import TYPE_CHECKING, Sequence +from collections.abc import Mapping, Sequence +from typing import TYPE_CHECKING import torch @@ -539,7 +540,7 @@ def _mesh_to_obstacle_entry( def generate_curobo_world_yaml( - rigid_objects: Sequence[RigidObject], + rigid_objects: Sequence[RigidObject] | Mapping[str, RigidObject], output_path: str, *, representation: str = "cuboid", @@ -552,7 +553,7 @@ def generate_curobo_world_yaml( collision_sphere_buffer: float = 0.0, device: str = "cuda:0", ) -> str: - """Generate a cuRobo V2 scene (world) YAML from a sequence of ``RigidObject``. + """Generate a cuRobo V2 scene (world) YAML from live ``RigidObject`` handles. Each object's mesh (``get_vertices`` / ``get_triangles``) and world pose (``get_local_pose``) are converted into cuRobo obstacle entries under a single @@ -568,7 +569,9 @@ def generate_curobo_world_yaml( :meth:`~embodichain.lab.sim.planners.curobo.curobo_planner.CuroboPlanner.update_dynamic_obstacles`. Args: - rigid_objects: ``RigidObject`` instances to bake into the collision world. + rigid_objects: Objects to bake into the collision world. Mapping keys are + authoritative obstacle IDs. A sequence derives each name from the + object's ``uid`` (or ``obstacle_`` when absent). output_path: Destination YAML file path. representation: ``"cuboid"`` (default, AABB->OBB, no CUDA), ``"mesh"`` (exact triangle mesh, no CUDA), or ``"sphere"`` (cuRobo sphere fit, @@ -594,29 +597,48 @@ def generate_curobo_world_yaml( import yaml - rigid_objects = list(rigid_objects) - if not rigid_objects: + registry_backed = isinstance(rigid_objects, Mapping) + if registry_backed: + named_rigid_objects = list(rigid_objects.items()) + else: + named_rigid_objects = [ + (getattr(obj, "uid", None) or f"obstacle_{idx}", obj) + for idx, obj in enumerate(rigid_objects) + ] + if not named_rigid_objects: raise ValueError("rigid_objects must contain at least one RigidObject.") data: dict[str, dict[str, object]] = {} used_names: set[str] = set() - for idx, obj in enumerate(rigid_objects): - name = getattr(obj, "uid", None) or f"obstacle_{idx}" + for name, obj in named_rigid_objects: + if not isinstance(name, str) or not name or name != name.strip(): + raise ValueError( + "Obstacle IDs must be non-empty strings without outer whitespace." + ) if name in used_names: raise ValueError( - f"Duplicate obstacle name {name!r}; RigidObject uids must be unique." + f"Duplicate obstacle name {name!r}; obstacle IDs must be unique." ) used_names.add(name) vertices = obj.get_vertices(env_ids=[env_id], scale=True)[0] faces = obj.get_triangles(env_ids=[env_id])[0] - pose = obj.get_local_pose(to_matrix=False)[env_id] - - if vertices is None or faces is None or vertices.numel() == 0: + if ( + vertices is None + or faces is None + or vertices.numel() == 0 + or faces.numel() == 0 + ): + if registry_backed: + raise ValueError( + f"Registry-backed obstacle {name!r} has no mesh geometry; " + "the declared collision world cannot omit it." + ) logger.log_warning( f"RigidObject {name!r} has no mesh geometry; skipping collision export." ) continue + pose = obj.get_local_pose(to_matrix=False)[env_id] entries = _mesh_to_obstacle_entry( name, diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index e602dd6dc..0c3e9a4a4 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -168,6 +168,72 @@ def supports_dynamic_collision_world(self) -> bool: """ return getattr(self.planner, "supports_collision_world_updates", False) is True + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical dynamic-obstacle IDs declared by the planner.""" + entity_ids = getattr(self.planner, "dynamic_collision_entity_ids", ()) + return self._validate_collision_entity_ids( + entity_ids, + field_name="dynamic_collision_entity_ids", + ) + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every canonical entity ID in the planner collision world.""" + entity_ids = getattr(self.planner, "collision_world_entity_ids", ()) + return self._validate_collision_entity_ids( + entity_ids, + field_name="collision_world_entity_ids", + ) + + @staticmethod + def _validate_collision_entity_ids( + entity_ids: object, + *, + field_name: str, + ) -> tuple[str, ...]: + """Validate one planner-owned canonical collision-ID declaration.""" + if not isinstance(entity_ids, tuple) or not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in entity_ids + ): + raise TypeError( + f"Planner.{field_name} must be a tuple of " + "non-empty strings without outer whitespace." + ) + if len(set(entity_ids)) != len(entity_ids): + raise ValueError(f"Planner.{field_name} must contain unique IDs.") + return entity_ids + + @staticmethod + def _validate_collision_pose_keys( + poses: Mapping[object, object], + *, + field_name: str, + ) -> set[str]: + """Validate exact canonical IDs on one obstacle-pose mapping.""" + entity_ids = tuple(poses) + if not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in entity_ids + ): + raise TypeError( + f"{field_name} keys must be non-empty strings without outer " + "whitespace." + ) + return set(entity_ids) + + @property + def collision_world_batch_mode(self) -> Literal["shared", "per_env"] | None: + """Return the backend's dynamic collision-world batch-sharing mode.""" + mode = getattr(self.planner, "collision_world_batch_mode", None) + if mode not in (None, "shared", "per_env"): + raise ValueError( + "Planner.collision_world_batch_mode must be 'shared', 'per_env', " + "or None." + ) + return mode + def bind_collision_world( self, plan_opts: PlanOptions | None, @@ -192,15 +258,68 @@ def bind_collision_world( "collision-world updates.", ValueError, ) + configured_ids = self.dynamic_collision_entity_ids + received_ids = tuple(obstacle_poses) + if not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in received_ids + ): + raise TypeError( + "obstacle_poses keys must be non-empty strings without outer " + "whitespace." + ) + missing = sorted(set(configured_ids).difference(received_ids)) + extra = sorted(set(received_ids).difference(configured_ids)) + if missing or extra: + logger.log_error( + "Dynamic collision obstacle IDs do not match the planner " + f"configuration; missing={missing}, extra={extra}.", + ValueError, + ) options = ( deepcopy(plan_opts) if plan_opts is not None else self.planner.default_plan_options() ) - return self.planner.with_collision_world( + existing_poses = getattr(options, "dynamic_obstacle_poses", None) + if existing_poses is not None: + if not isinstance(existing_poses, Mapping): + raise TypeError( + "plan_opts.dynamic_obstacle_poses must be a mapping or None." + ) + existing_ids = self._validate_collision_pose_keys( + existing_poses, + field_name="plan_opts.dynamic_obstacle_poses", + ) + existing_extra = sorted(existing_ids.difference(configured_ids)) + if existing_extra: + raise ValueError( + "Caller planning options contain dynamic collision IDs that " + f"are not configured by the planner: {existing_extra}." + ) + bound = self.planner.with_collision_world( options, obstacle_poses=obstacle_poses, ) + if hasattr(bound, "dynamic_obstacle_poses"): + bound_poses = bound.dynamic_obstacle_poses + if bound_poses is None: + bound_ids: set[str] = set() + elif not isinstance(bound_poses, Mapping): + raise TypeError("Bound dynamic_obstacle_poses must be a mapping.") + else: + bound_ids = self._validate_collision_pose_keys( + bound_poses, + field_name="Bound dynamic_obstacle_poses", + ) + bound_missing = sorted(set(configured_ids).difference(bound_ids)) + bound_extra = sorted(bound_ids.difference(configured_ids)) + if bound_missing or bound_extra: + raise ValueError( + "Bound dynamic collision obstacle IDs do not match the planner " + f"configuration; missing={bound_missing}, extra={bound_extra}." + ) + return bound def resolve_plan_options( self, diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py new file mode 100644 index 000000000..f07a9222b --- /dev/null +++ b/embodichain/lab/sim/skills/__init__.py @@ -0,0 +1,51 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Semantic-skill integration contracts built on the atomic-action core.""" + +from __future__ import annotations + +from .scene import ( + RegistrySceneProvider, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneDynamics, + SceneEntityRef, + SceneEntityRegistration, + SceneEntityStateProvider, + SceneGeometryProvider, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + +__all__ = [ + "RegistrySceneProvider", + "SceneAffordanceRef", + "SceneArticulationRef", + "SceneCollisionRole", + "SceneCollisionWorldMode", + "SceneDynamics", + "SceneEntityRef", + "SceneEntityRegistration", + "SceneEntityStateProvider", + "SceneGeometryProvider", + "SceneLinkRef", + "SceneObjectRef", + "SceneRegistry", +] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py new file mode 100644 index 000000000..62d71ec61 --- /dev/null +++ b/embodichain/lab/sim/skills/scene.py @@ -0,0 +1,1342 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Authoritative scene identity and registration value contracts.""" + +from __future__ import annotations + +from collections.abc import Iterable, Iterator, Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass, replace +from enum import Enum +import math +from types import MappingProxyType +from typing import Any, Protocol, TYPE_CHECKING, TypeVar, runtime_checkable + +import torch + +from embodichain.lab.sim.common import BatchEntity +from embodichain.lab.sim.atomic_actions import ( + Affordance, + EntityState, + SceneProvider, + SceneSnapshot, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.planners import MotionGenerator + from embodichain.lab.sim.sim_manager import SimulationManager + + +RefT = TypeVar("RefT", bound="SceneEntityRef") + + +def _validate_identifier(value: str, name: str) -> None: + """Validate an exact, non-empty identifier without normalizing it.""" + if not isinstance(value, str) or not value or value != value.strip(): + raise ValueError(f"{name} must be a non-empty string without outer whitespace.") + + +@dataclass(frozen=True, slots=True) +class SceneEntityRef: + """Typed reference to one authoritative scene-registry entity. + + Args: + entity_id: Globally stable canonical registry identifier. + """ + + entity_id: str + """Globally stable authoritative registry identifier.""" + + def __post_init__(self) -> None: + _validate_identifier(self.entity_id, "entity_id") + + +@dataclass(frozen=True, slots=True) +class SceneObjectRef(SceneEntityRef): + """Reference to one object registered in the semantic scene.""" + + +@dataclass(frozen=True, slots=True) +class SceneArticulationRef(SceneEntityRef): + """Reference to one articulation registered in the semantic scene.""" + + +@dataclass(frozen=True, slots=True) +class SceneLinkRef(SceneEntityRef): + """Reference to one registered articulation link.""" + + +@dataclass(frozen=True, slots=True) +class SceneAffordanceRef(SceneEntityRef): + """Reference to one registered interaction affordance.""" + + +class SceneDynamics(str, Enum): + """Physical mobility classification owned by a scene registration.""" + + UNKNOWN = "unknown" + STATIC = "static" + KINEMATIC = "kinematic" + DYNAMIC = "dynamic" + + +class SceneCollisionRole(str, Enum): + """How an entity participates in the planner collision world.""" + + NONE = "none" + STATIC = "static" + DYNAMIC = "dynamic" + + +class SceneCollisionWorldMode(str, Enum): + """Batch-sharing policy for a dynamic planner collision world.""" + + SHARED = "shared" + PER_ENV = "per_env" + + +@runtime_checkable +class SceneEntityStateProvider(Protocol): + """Observe one registered entity for an ordered environment batch.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + """Return the entity state whose rows follow ``env_ids``. + + Args: + timestamp: Observation timestamp supplied by the integration. + env_ids: Stable ordered environment correlation IDs. + + Returns: + Current pose and confidence for the registered entity. + """ + + +@runtime_checkable +class SceneGeometryProvider(Protocol): + """Provide one entity's planner-facing collision geometry descriptor.""" + + def get_geometry(self) -> object: + """Return the planner-facing geometry descriptor. + + Returns: + Backend-consumable geometry or a live simulation entity. + """ + + +@dataclass(frozen=True, slots=True, eq=False) +class SceneEntityRegistration: + """Immutable integration metadata for one authoritative scene entity. + + Parent relationships, simulator-native names, pose sources, geometry, and + affordances belong to the registry registration rather than the lightweight + reference copied into semantic calls. + + Args: + ref: Canonical typed reference. + state_provider: Optional dynamic pose/confidence source. + aliases: External names normalized at the registry boundary. + parent: Canonical parent for a link or affordance. + native_name: Backend-local member name under ``parent``. + dynamics: Physical mobility classification. + geometry_provider: Planner-facing collision geometry source. + collision_role: Static, dynamic, or no planner collision role. + semantic_type: Optional application semantic type. + affordance: Affordance value for an affordance registration. + relative_pose: Optional parent-relative affordance transform. + """ + + ref: SceneEntityRef + """Canonical typed reference owned by the registry.""" + + state_provider: SceneEntityStateProvider | None = None + """Explicit dynamic pose/confidence source.""" + + aliases: tuple[str, ...] = () + """External or legacy names normalized once at the registry boundary.""" + + parent: SceneEntityRef | None = None + """Canonical parent reference for a link or affordance.""" + + native_name: str | None = None + """Backend-local link or affordance name under ``parent``.""" + + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + """Static, kinematic, dynamic, or unknown mobility classification.""" + + geometry_provider: SceneGeometryProvider | None = None + """Collision geometry source required for planner collision roles.""" + + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + """Static/dynamic planner-obstacle role, or ``none``.""" + + semantic_type: str | None = None + """Optional application semantic type such as ``container`` or ``tool``.""" + + affordance: Affordance | None = None + """Affordance value owned by a :class:`SceneAffordanceRef` registration.""" + + relative_pose: torch.Tensor | None = None + """Optional parent-relative pose when no explicit state provider exists.""" + + def __post_init__(self) -> None: + if not isinstance(self.ref, SceneEntityRef): + raise TypeError("ref must be a SceneEntityRef.") + if self.state_provider is not None and not isinstance( + self.state_provider, + SceneEntityStateProvider, + ): + raise TypeError("state_provider must implement SceneEntityStateProvider.") + + if isinstance(self.aliases, (str, bytes)): + raise TypeError("aliases must be an iterable of identifiers, not a string.") + try: + aliases = tuple(self.aliases) + except TypeError as exc: + raise TypeError("aliases must be an iterable of identifiers.") from exc + for alias in aliases: + _validate_identifier(alias, "alias") + aliases = tuple(alias for alias in aliases if alias != self.ref.entity_id) + if len(set(aliases)) != len(aliases): + raise ValueError("aliases must be unique.") + object.__setattr__(self, "aliases", aliases) + + if self.parent is not None and not isinstance(self.parent, SceneEntityRef): + raise TypeError("parent must be a SceneEntityRef or None.") + if self.native_name is not None: + _validate_identifier(self.native_name, "native_name") + if not isinstance(self.dynamics, SceneDynamics): + raise TypeError("dynamics must be a SceneDynamics value.") + if not isinstance(self.collision_role, SceneCollisionRole): + raise TypeError("collision_role must be a SceneCollisionRole value.") + if self.geometry_provider is not None and not isinstance( + self.geometry_provider, + SceneGeometryProvider, + ): + raise TypeError("geometry_provider must implement SceneGeometryProvider.") + if self.semantic_type is not None: + _validate_identifier(self.semantic_type, "semantic_type") + if self.affordance is not None and not isinstance(self.affordance, Affordance): + raise TypeError("affordance must be an Affordance or None.") + if self.relative_pose is not None: + if not isinstance(self.relative_pose, torch.Tensor): + raise TypeError("relative_pose must be a torch.Tensor or None.") + if self.relative_pose.shape != (4, 4): + raise ValueError("relative_pose must have shape (4, 4).") + object.__setattr__(self, "relative_pose", self.relative_pose.clone()) + if self.state_provider is not None and self.relative_pose is not None: + raise ValueError( + "state_provider and relative_pose are mutually exclusive pose sources." + ) + + self._validate_reference_contract() + if ( + self.collision_role is not SceneCollisionRole.NONE + and self.geometry_provider is None + ): + raise ValueError( + f"Collision entity {self.ref.entity_id!r} requires geometry_provider." + ) + + def _validate_reference_contract(self) -> None: + """Validate fields whose meaning follows from the typed ref.""" + if isinstance(self.ref, (SceneObjectRef, SceneArticulationRef)): + if self.parent is not None: + raise ValueError( + "Object and articulation registrations cannot have a parent." + ) + if self.native_name is not None: + raise ValueError( + "Object and articulation registrations cannot have native_name." + ) + if self.state_provider is None: + raise ValueError( + "Object and articulation registrations require state_provider." + ) + if self.relative_pose is not None: + raise ValueError( + "Object and articulation registrations cannot use relative_pose." + ) + if self.affordance is not None: + raise ValueError( + "Affordance values require a SceneAffordanceRef registration." + ) + return + + if isinstance(self.ref, SceneLinkRef): + if ( + not isinstance(self.parent, SceneArticulationRef) + or self.native_name is None + ): + raise ValueError("Link registrations require parent and native_name.") + if self.state_provider is None: + raise ValueError("Link registrations require state_provider.") + if self.relative_pose is not None: + raise ValueError("Link registrations cannot use relative_pose.") + if self.affordance is not None: + raise ValueError( + "Affordance values require a SceneAffordanceRef registration." + ) + return + + if isinstance(self.ref, SceneAffordanceRef): + if ( + not isinstance( + self.parent, + (SceneObjectRef, SceneArticulationRef, SceneLinkRef), + ) + or self.native_name is None + ): + raise ValueError( + "Affordance registrations require parent and native_name." + ) + if self.affordance is None: + raise ValueError("Affordance registrations require affordance.") + if self.state_provider is None and self.relative_pose is None: + raise ValueError( + "Affordance registrations require state_provider or relative_pose." + ) + return + + if self.parent is not None or self.native_name is not None: + raise ValueError("Generic entity registrations cannot declare a parent.") + if self.state_provider is None: + raise ValueError("Generic entity registrations require state_provider.") + + +def _copy_registration( + registration: SceneEntityRegistration, +) -> SceneEntityRegistration: + """Copy registry metadata without cloning live providers or entities.""" + relative_pose = registration.relative_pose + return replace( + registration, + affordance=_copy_affordance(registration.affordance), + relative_pose=relative_pose.clone() if relative_pose is not None else None, + ) + + +def _copy_affordance(affordance: Affordance | None) -> Affordance | None: + """Own mutable affordance metadata while preserving live entity handles.""" + if affordance is None: + return None + memo: dict[int, object] = {} + visited: set[int] = set() + + def visit(value: object) -> None: + value_id = id(value) + if value_id in visited: + return + visited.add(value_id) + if isinstance(value, BatchEntity): + memo[value_id] = value + return + if is_dataclass(value) and not isinstance(value, type): + for data_field in fields(value): + nested = getattr(value, data_field.name) + if data_field.name == "_generator" and nested is not None: + memo[id(nested)] = None + else: + visit(nested) + return + if isinstance(value, Mapping): + for key, nested in value.items(): + visit(key) + visit(nested) + return + if isinstance(value, (list, tuple, set, frozenset)): + for nested in value: + visit(nested) + + visit(affordance) + try: + return deepcopy(affordance, memo) + except Exception as exc: # noqa: BLE001 - normalize opaque metadata failures + raise TypeError( + f"Affordance {type(affordance).__name__} must contain copyable " + "registry metadata." + ) from exc + + +@dataclass(frozen=True, slots=True, eq=False, init=False) +class SceneRegistry: + """Immutable authoritative catalog of semantic scene entities. + + Canonical identifiers occupy one flat, globally unique namespace. Aliases + are accepted only at lookup and integration boundaries and always resolve + to a canonical typed reference before they leave the registry. + + Args: + registrations: Complete scene registrations. The iterable is copied and + cannot be extended after construction. + collision_world_mode: Explicit dynamic-collision batch policy. It may be + omitted for a single environment, which resolves to ``shared``. A + multi-environment dynamic world must select a mode explicitly. + """ + + _registrations: tuple[SceneEntityRegistration, ...] = field(repr=False) + _registrations_by_id: Mapping[str, SceneEntityRegistration] = field(repr=False) + _aliases: Mapping[str, str] = field(repr=False) + _collision_world_entity_ids: tuple[str, ...] = field(repr=False) + _dynamic_collision_entity_ids: tuple[str, ...] = field(repr=False) + _static_collision_entity_ids: tuple[str, ...] = field(repr=False) + collision_world_mode: SceneCollisionWorldMode | None + + def __init__( + self, + registrations: Iterable[SceneEntityRegistration] = (), + *, + collision_world_mode: SceneCollisionWorldMode | None = None, + ) -> None: + if collision_world_mode is not None and not isinstance( + collision_world_mode, + SceneCollisionWorldMode, + ): + raise TypeError( + "collision_world_mode must be a SceneCollisionWorldMode or None." + ) + try: + supplied = tuple(registrations) + except TypeError as exc: + raise TypeError("registrations must be an iterable.") from exc + if not all(isinstance(item, SceneEntityRegistration) for item in supplied): + raise TypeError( + "registrations must contain SceneEntityRegistration values." + ) + owned = tuple(_copy_registration(item) for item in supplied) + by_id: dict[str, SceneEntityRegistration] = {} + for registration in owned: + entity_id = registration.ref.entity_id + if entity_id in by_id: + raise ValueError(f"Duplicate canonical scene entity ID {entity_id!r}.") + by_id[entity_id] = registration + + aliases: dict[str, str] = {} + canonical_ids = set(by_id) + for registration in owned: + canonical_id = registration.ref.entity_id + for alias in registration.aliases: + if alias in canonical_ids: + raise ValueError( + f"Scene alias {alias!r} collides with canonical entity ID " + f"{alias!r}." + ) + previous = aliases.get(alias) + if previous is not None: + raise ValueError( + f"Scene alias {alias!r} is ambiguous between canonical " + f"IDs {previous!r} and {canonical_id!r}." + ) + aliases[alias] = canonical_id + + self._validate_relationships(owned, by_id) + object.__setattr__(self, "_registrations", owned) + object.__setattr__( + self, + "_registrations_by_id", + MappingProxyType(by_id), + ) + object.__setattr__(self, "_aliases", MappingProxyType(aliases)) + object.__setattr__( + self, + "_collision_world_entity_ids", + tuple( + item.ref.entity_id + for item in owned + if item.collision_role is not SceneCollisionRole.NONE + ), + ) + object.__setattr__( + self, + "_dynamic_collision_entity_ids", + tuple( + item.ref.entity_id + for item in owned + if item.collision_role is SceneCollisionRole.DYNAMIC + ), + ) + object.__setattr__( + self, + "_static_collision_entity_ids", + tuple( + item.ref.entity_id + for item in owned + if item.collision_role is SceneCollisionRole.STATIC + ), + ) + object.__setattr__(self, "collision_world_mode", collision_world_mode) + + @staticmethod + def _validate_relationships( + registrations: tuple[SceneEntityRegistration, ...], + by_id: Mapping[str, SceneEntityRegistration], + ) -> None: + """Require every parent to be a canonical, correctly typed ref.""" + native_members: dict[tuple[type[SceneEntityRef], str, str], str] = {} + for registration in registrations: + parent = registration.parent + if parent is None: + continue + if parent.entity_id == registration.ref.entity_id: + raise ValueError( + f"Scene entity {registration.ref.entity_id!r} cannot parent itself." + ) + parent_registration = by_id.get(parent.entity_id) + if parent_registration is None: + raise ValueError( + f"Scene entity {registration.ref.entity_id!r} references " + f"unregistered parent {parent.entity_id!r}." + ) + if type(parent_registration.ref) is not type(parent): + raise TypeError( + f"Parent {parent.entity_id!r} is registered as " + f"{type(parent_registration.ref).__name__}, not " + f"{type(parent).__name__}." + ) + if isinstance(registration.ref, (SceneLinkRef, SceneAffordanceRef)): + assert registration.native_name is not None + member_key = ( + type(registration.ref), + parent.entity_id, + registration.native_name, + ) + previous = native_members.get(member_key) + if previous is not None: + raise ValueError( + f"{type(registration.ref).__name__} parent " + f"{parent.entity_id!r} and native_name " + f"{registration.native_name!r} are already registered as " + f"canonical ID {previous!r}." + ) + native_members[member_key] = registration.ref.entity_id + + @property + def registrations(self) -> tuple[SceneEntityRegistration, ...]: + """Return structurally independent registration values.""" + return tuple(_copy_registration(item) for item in self._registrations) + + @property + def entity_refs(self) -> tuple[SceneEntityRef, ...]: + """Return canonical typed references in registration order.""" + return tuple(item.ref for item in self._registrations) + + @property + def aliases(self) -> Mapping[str, str]: + """Return the immutable alias-to-canonical-ID index.""" + return self._aliases + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every canonical ID represented in the planner world.""" + return self._collision_world_entity_ids + + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical IDs whose planner poses update dynamically.""" + return self._dynamic_collision_entity_ids + + @property + def static_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical IDs baked into the static planner world.""" + return self._static_collision_entity_ids + + def __len__(self) -> int: + return len(self._registrations) + + def __iter__(self) -> Iterator[SceneEntityRef]: + return iter(self.entity_refs) + + def __getitem__( + self, + identifier: str | SceneEntityRef, + ) -> SceneEntityRegistration: + return self.lookup(identifier) + + def resolve( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + ) -> RefT: + """Resolve a canonical ID or alias to a typed canonical reference. + + Args: + identifier: Canonical ID, alias, or already typed canonical ref. + expected_type: Required reference class for typed lookup. + + Returns: + Registry-owned canonical reference. + + Raises: + KeyError: If the canonical ID or alias is unknown. + TypeError: If the supplied or resolved reference has the wrong type. + """ + if not isinstance(expected_type, type) or not issubclass( + expected_type, + SceneEntityRef, + ): + raise TypeError("expected_type must be a SceneEntityRef subclass.") + supplied_ref: SceneEntityRef | None + if isinstance(identifier, SceneEntityRef): + canonical_id = identifier.entity_id + supplied_ref = identifier + elif isinstance(identifier, str): + _validate_identifier(identifier, "identifier") + canonical_id = self._aliases.get(identifier, identifier) + supplied_ref = None + else: + raise TypeError("identifier must be a string or SceneEntityRef.") + + registration = self._registrations_by_id.get(canonical_id) + if registration is None: + raise KeyError(f"Unknown scene entity {identifier!r}.") + canonical_ref = registration.ref + if supplied_ref is not None and type(supplied_ref) is not type(canonical_ref): + raise TypeError( + f"Scene entity {canonical_id!r} is registered as " + f"{type(canonical_ref).__name__}, not " + f"{type(supplied_ref).__name__}." + ) + if not isinstance(canonical_ref, expected_type): + raise TypeError( + f"Scene entity {canonical_id!r} is " + f"{type(canonical_ref).__name__}, not {expected_type.__name__}." + ) + return canonical_ref # type: ignore[return-value] + + def lookup( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + ) -> SceneEntityRegistration: + """Return an owned registration after canonical typed resolution. + + Args: + identifier: Canonical ID, alias, or typed canonical reference. + expected_type: Required reference class. + + Returns: + A structurally independent copy of the matching registration. + """ + ref = self.resolve(identifier, expected_type=expected_type) + return _copy_registration(self._registrations_by_id[ref.entity_id]) + + def make_scene_provider( + self, + *, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + batch_size: int | None = None, + ) -> RegistrySceneProvider: + """Create an independent provider without planner cross-validation. + + This factory is intended for perception and direct-core consumers. The + canonical planning path must use :meth:`make_planning_scene_provider` + so planner IDs, capabilities, and collision-world mode cannot drift. + + Args: + translation_threshold: Accumulated translation needed to publish a + material scene change. + rotation_threshold: Accumulated rotation needed to publish a + material scene change. + batch_size: Optional fixed integration batch size. Supplying it + validates the collision-world mode immediately and binds the + provider to that row count. + + Returns: + A new provider with independent revisions and published baselines. + """ + return RegistrySceneProvider( + self, + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + batch_size=batch_size, + ) + + def make_planning_scene_provider( + self, + motion_generator: MotionGenerator, + *, + batch_size: int, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + ) -> RegistrySceneProvider: + """Create a provider after complete planner/registry validation. + + Args: + motion_generator: Motion generator that will consume dynamic poses. + batch_size: Number of execution environments. + translation_threshold: Accumulated translation needed to publish a + material scene change. + rotation_threshold: Accumulated rotation needed to publish a + material scene change. + + Returns: + A new independently stateful, planner-validated scene provider. + """ + provider = self.make_scene_provider( + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + batch_size=batch_size, + ) + self.validate_collision_integration( + motion_generator, + batch_size=batch_size, + scene_provider=provider, + ) + return provider + + def collision_geometry_by_id( + self, + role: SceneCollisionRole | None = None, + ) -> Mapping[str, object]: + """Materialize planner geometry under canonical registry IDs. + + Args: + role: Optional exact collision-role filter. Without a filter, all + static and dynamic collision registrations are included. + Registrations whose role is :attr:`SceneCollisionRole.NONE` + never enter the planner collision world. + + Returns: + Fresh immutable canonical-ID-to-geometry mapping. + """ + if role is not None and not isinstance(role, SceneCollisionRole): + raise TypeError("role must be a SceneCollisionRole or None.") + geometry: dict[str, object] = {} + for registration in self._registrations: + provider = registration.geometry_provider + if provider is None: + continue + if role is None: + if registration.collision_role is SceneCollisionRole.NONE: + continue + elif registration.collision_role is not role: + continue + entity_id = registration.ref.entity_id + descriptor = provider.get_geometry() + if descriptor is None: + raise ValueError( + f"Collision geometry provider for scene entity " + f"{entity_id!r} returned None." + ) + geometry[entity_id] = descriptor + return MappingProxyType(geometry) + + def validate_collision_integration( + self, + motion_generator: MotionGenerator, + *, + batch_size: int, + scene_provider: SceneProvider | None = None, + ) -> SceneCollisionWorldMode | None: + """Validate registry/planner agreement before dynamic planning. + + Args: + motion_generator: Motion generator whose planner consumes obstacles. + batch_size: Number of execution environments. + scene_provider: Optional external perception or hardware provider. + Its concrete ``collision_entity_ids`` must agree exactly with + the registry and planner declarations. + + Returns: + Effective dynamic collision mode, or ``None`` without dynamic IDs. + """ + effective_mode = self.resolve_collision_world_mode(batch_size=batch_size) + try: + planner_dynamic_ids = motion_generator.dynamic_collision_entity_ids + planner_world_ids = motion_generator.collision_world_entity_ids + supports_updates = motion_generator.supports_dynamic_collision_world + planner_mode = motion_generator.collision_world_batch_mode + except AttributeError as exc: + raise TypeError( + "motion_generator must expose collision-world integration properties." + ) from exc + planner_dynamic_ids = self._validate_integration_ids( + planner_dynamic_ids, + field_name="motion_generator.dynamic_collision_entity_ids", + ) + planner_world_ids = self._validate_integration_ids( + planner_world_ids, + field_name="motion_generator.collision_world_entity_ids", + ) + registry_dynamic_ids = set(self.dynamic_collision_entity_ids) + planner_dynamic_id_set = set(planner_dynamic_ids) + if registry_dynamic_ids != planner_dynamic_id_set: + raise ValueError( + "Dynamic collision entity mismatch: registry missing from planner " + f"{sorted(registry_dynamic_ids - planner_dynamic_id_set)}, planner " + "missing from registry " + f"{sorted(planner_dynamic_id_set - registry_dynamic_ids)}. Planner IDs " + "must use authoritative registry IDs, not aliases." + ) + registry_world_ids = set(self.collision_world_entity_ids) + planner_world_id_set = set(planner_world_ids) + if registry_world_ids != planner_world_id_set: + raise ValueError( + "Collision world entity mismatch: registry missing from planner " + f"{sorted(registry_world_ids - planner_world_id_set)}, planner " + "missing from registry " + f"{sorted(planner_world_id_set - registry_world_ids)}. Planner IDs " + "must use authoritative registry IDs, not aliases." + ) + if scene_provider is not None: + if not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider.") + provider_ids = getattr(scene_provider, "collision_entity_ids", None) + provider_ids = self._validate_integration_ids( + provider_ids, + field_name="scene_provider.collision_entity_ids", + ) + provider_id_set = set(provider_ids) + if registry_dynamic_ids != provider_id_set: + raise ValueError( + "Dynamic collision entity mismatch: registry missing from " + "provider " + f"{sorted(registry_dynamic_ids - provider_id_set)}, provider " + "missing from registry " + f"{sorted(provider_id_set - registry_dynamic_ids)}. Provider IDs " + "must use authoritative registry IDs, not aliases." + ) + collision_geometry = self.collision_geometry_by_id() + if set(collision_geometry) != registry_world_ids: + raise ValueError( + "Collision geometry IDs do not match authoritative registry " + f"world IDs {sorted(registry_world_ids)}." + ) + if not registry_dynamic_ids: + return None + if supports_updates is not True: + raise ValueError( + "The selected motion generator does not support dynamic collision " + f"updates required by {sorted(registry_dynamic_ids)}." + ) + assert effective_mode is not None + if planner_mode != effective_mode.value: + raise ValueError( + "Dynamic collision world mode mismatch: registry requires " + f"{effective_mode.value!r}, planner declares {planner_mode!r}." + ) + return effective_mode + + @staticmethod + def _validate_integration_ids( + value: object, + *, + field_name: str, + ) -> tuple[str, ...]: + """Validate one canonical collision-ID declaration at a boundary.""" + if not isinstance(value, tuple) or not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in value + ): + raise TypeError( + f"{field_name} must be a tuple of non-empty canonical IDs " + "without outer whitespace." + ) + if len(set(value)) != len(value): + raise ValueError(f"{field_name} must contain unique IDs.") + return value + + def resolve_collision_world_mode( + self, + *, + batch_size: int, + ) -> SceneCollisionWorldMode | None: + """Resolve the configured collision mode for an execution batch. + + Args: + batch_size: Number of execution environments. + + Returns: + The effective mode, or ``None`` when no dynamic collision entity is + registered. + """ + return self._effective_collision_world_mode(batch_size) + + def _effective_collision_world_mode( + self, + batch_size: int, + ) -> SceneCollisionWorldMode | None: + """Resolve E without reading any live state or planner integration.""" + if isinstance(batch_size, bool) or not isinstance(batch_size, int): + raise TypeError("batch_size must be an integer.") + if batch_size <= 0: + raise ValueError("batch_size must be positive.") + if not self.dynamic_collision_entity_ids: + return None + if self.collision_world_mode is not None: + return self.collision_world_mode + if batch_size == 1: + return SceneCollisionWorldMode.SHARED + raise ValueError( + "Multi-environment dynamic collision requires an explicit " + "collision_world_mode of 'shared' or 'per_env'." + ) + + @classmethod + def from_simulation( + cls, + simulation: SimulationManager, + *, + rigid_objects: Mapping[str, str] | None = None, + articulations: Mapping[str, str] | None = None, + collision_roles: Mapping[str, SceneCollisionRole] | None = None, + geometry_providers: Mapping[str, SceneGeometryProvider] | None = None, + collision_world_mode: SceneCollisionWorldMode | None = None, + ) -> SceneRegistry: + """Opt explicitly selected simulation entities into a registry. + + ``rigid_objects`` and ``articulations`` map authoritative registry IDs + to simulation UIDs. UIDs become aliases automatically; unlisted + simulation entities are never imported. Collision participation + defaults to :attr:`SceneCollisionRole.NONE`. + + Args: + simulation: Simulation manager used only for explicit UID lookup. + rigid_objects: Canonical object IDs mapped to simulation UIDs. + articulations: Canonical articulation IDs mapped to simulation UIDs. + collision_roles: Optional collision roles keyed by canonical ID. + geometry_providers: Optional geometry overrides keyed by canonical + ID. Selected rigid objects otherwise expose their live handles. + collision_world_mode: Optional dynamic collision batch-sharing mode. + + Returns: + Immutable registry containing only the explicitly selected entities. + """ + object_ids = cls._normalize_simulation_mapping( + rigid_objects, + name="rigid_objects", + ) + articulation_ids = cls._normalize_simulation_mapping( + articulations, + name="articulations", + ) + duplicate_ids = set(object_ids).intersection(articulation_ids) + if duplicate_ids: + raise ValueError( + "Simulation registry IDs must be globally unique across entity " + f"types: {sorted(duplicate_ids)}." + ) + all_ids = set(object_ids).union(articulation_ids) + roles = dict(collision_roles or {}) + geometry = dict(geometry_providers or {}) + for mapping_name, values in ( + ("collision_roles", roles), + ("geometry_providers", geometry), + ): + unknown = set(values).difference(all_ids) + if unknown: + raise KeyError( + f"{mapping_name} reference unselected registry IDs: " + f"{sorted(unknown)}." + ) + + registrations: list[SceneEntityRegistration] = [] + for registry_id, uid in object_ids.items(): + entity = cls._get_simulation_entity( + simulation, + getter_name="get_rigid_object", + registry_id=registry_id, + uid=uid, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneObjectRef(registry_id), + state_provider=_SimulationEntityStateProvider(entity), + aliases=(uid,), + geometry_provider=geometry.get( + registry_id, + _SimulationEntityGeometryProvider(entity), + ), + collision_role=roles.get( + registry_id, + SceneCollisionRole.NONE, + ), + ) + ) + for registry_id, uid in articulation_ids.items(): + entity = cls._get_simulation_entity( + simulation, + getter_name="get_articulation", + registry_id=registry_id, + uid=uid, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneArticulationRef(registry_id), + state_provider=_SimulationEntityStateProvider(entity), + aliases=(uid,), + geometry_provider=geometry.get(registry_id), + collision_role=roles.get( + registry_id, + SceneCollisionRole.NONE, + ), + ) + ) + return cls( + registrations, + collision_world_mode=collision_world_mode, + ) + + @staticmethod + def _normalize_simulation_mapping( + mapping: Mapping[str, str] | None, + *, + name: str, + ) -> dict[str, str]: + if mapping is None: + return {} + if not isinstance(mapping, Mapping): + raise TypeError(f"{name} must be a mapping from registry ID to UID.") + normalized = dict(mapping) + for registry_id, uid in normalized.items(): + _validate_identifier(registry_id, f"{name} registry ID") + _validate_identifier(uid, f"{name} UID") + return normalized + + @staticmethod + def _get_simulation_entity( + simulation: SimulationManager, + *, + getter_name: str, + registry_id: str, + uid: str, + ) -> Any: + getter = getattr(simulation, getter_name, None) + if not callable(getter): + raise TypeError(f"simulation must provide {getter_name}().") + entity = getter(uid) + if entity is None: + raise KeyError( + f"Simulation UID {uid!r} selected for registry entity " + f"{registry_id!r} was not found." + ) + return entity + + +@dataclass(frozen=True, slots=True) +class _SimulationEntityStateProvider: + """Read poses from one explicitly selected simulation entity.""" + + entity: Any + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + pose = self.entity.get_local_pose(to_matrix=True) + if not isinstance(pose, torch.Tensor): + raise TypeError("Simulation entity get_local_pose() must return a tensor.") + return EntityState(pose) + + +@dataclass(frozen=True, slots=True) +class _SimulationEntityGeometryProvider: + """Expose a selected live rigid object as planner geometry input.""" + + entity: Any + + def get_geometry(self) -> object: + return self.entity + + +class RegistrySceneProvider(SceneProvider): + """Stateful scene provider derived from an immutable registry. + + Instances are created by :meth:`SceneRegistry.make_scene_provider`; each + instance owns its revision counters and material-pose baselines. + + Args: + registry: Immutable catalog that owns entity registrations. + translation_threshold: Accumulated translation needed to publish a + material scene change. + rotation_threshold: Accumulated rotation needed to publish a material + scene change. + batch_size: Optional fixed execution batch size. Factory-created + planning providers bind this value before their first observation. + """ + + def __init__( + self, + registry: SceneRegistry, + *, + translation_threshold: float, + rotation_threshold: float, + batch_size: int | None = None, + ) -> None: + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + for name, value in ( + ("translation_threshold", translation_threshold), + ("rotation_threshold", rotation_threshold), + ): + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(float(value)) + or value < 0.0 + ): + raise ValueError(f"{name} must be finite and non-negative.") + self.registry = registry + self.translation_threshold = float(translation_threshold) + self.rotation_threshold = float(rotation_threshold) + self.collision_entity_ids = registry.dynamic_collision_entity_ids + self._expected_batch_size = batch_size + self._last_timestamp: float | None = None + self._env_ids: torch.Tensor | None = None + self._published_poses: dict[str, torch.Tensor] = {} + self._published_confidences: dict[str, float] = {} + self._scene_version = 0 + self._collision_revisions: list[int] = [] + self._effective_collision_world_mode = ( + registry.resolve_collision_world_mode(batch_size=batch_size) + if batch_size is not None + else None + ) + + @property + def collision_world_mode(self) -> SceneCollisionWorldMode | None: + """Return the configured or first-snapshot-resolved collision mode.""" + return ( + self._effective_collision_world_mode + if self._effective_collision_world_mode is not None + else self.registry.collision_world_mode + ) + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + """Observe all canonical entities and advance material revisions. + + Args: + timestamp: Non-negative monotonic observation timestamp. + env_ids: Stable ordered correlation IDs for every environment row. + + Returns: + An immutable snapshot keyed only by canonical registry IDs. + """ + if ( + isinstance(timestamp, bool) + or not isinstance(timestamp, (int, float)) + or not math.isfinite(float(timestamp)) + or timestamp < 0.0 + ): + raise ValueError("timestamp must be finite and non-negative.") + if self._last_timestamp is not None and timestamp < self._last_timestamp: + raise ValueError("Scene provider timestamps must be monotonic.") + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError("env_ids must be a non-empty 1D int64 tensor.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + + batch_size = int(env_ids.numel()) + if ( + self._expected_batch_size is not None + and batch_size != self._expected_batch_size + ): + raise ValueError( + "Scene provider batch size must remain equal to its configured " + f"batch_size={self._expected_batch_size}; got {batch_size}." + ) + effective_mode = self.registry.resolve_collision_world_mode( + batch_size=batch_size + ) + stable_ids = env_ids.detach().to("cpu") + if self._env_ids is None: + self._env_ids = stable_ids.clone() + self._collision_revisions = [0] * batch_size + self._effective_collision_world_mode = effective_mode + elif not torch.equal(stable_ids, self._env_ids): + raise ValueError("Scene provider env_ids must remain stable and ordered.") + + states = self._observe_states( + timestamp=float(timestamp), + env_ids=env_ids, + ) + poses = {entity_id: state.pose for entity_id, state in states.items()} + confidences = { + entity_id: state.confidence for entity_id, state in states.items() + } + if self._published_poses: + changed_by_entity = { + entity_id: self._pose_change_mask( + self._published_poses[entity_id], + current_pose, + ) + for entity_id, current_pose in poses.items() + } + confidence_changed = any( + confidences[entity_id] != self._published_confidences[entity_id] + for entity_id in confidences + ) + if confidence_changed or any( + changed.any().item() for changed in changed_by_entity.values() + ): + self._scene_version += 1 + collision_changed = torch.zeros(batch_size, dtype=torch.bool) + for entity_id in self.collision_entity_ids: + collision_changed |= changed_by_entity[entity_id] + for row in collision_changed.nonzero(as_tuple=False).flatten().tolist(): + self._collision_revisions[row] += 1 + + for entity_id, changed in changed_by_entity.items(): + if changed.any(): + published_pose = self._published_poses[entity_id] + changed_on_published_device = changed.to(published_pose.device) + current_pose = poses[entity_id].to( + device=published_pose.device, + dtype=published_pose.dtype, + ) + published_pose[changed_on_published_device] = current_pose[ + changed_on_published_device + ] + self._published_confidences = confidences.copy() + else: + self._published_poses = { + entity_id: pose.clone() for entity_id, pose in poses.items() + } + self._published_confidences = confidences.copy() + + self._last_timestamp = float(timestamp) + return SceneSnapshot( + timestamp=float(timestamp), + version=self._scene_version, + entities=states, + collision_world_revision=tuple(self._collision_revisions), + collision_entity_ids=self.collision_entity_ids, + ) + + def _observe_states( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, EntityState]: + """Observe explicit sources before deriving relative affordance poses.""" + batch_size = int(env_ids.numel()) + states: dict[str, EntityState] = {} + relative_registrations: list[SceneEntityRegistration] = [] + for registration in self.registry._registrations: + entity_id = registration.ref.entity_id + if registration.state_provider is None: + relative_registrations.append(registration) + continue + state = registration.state_provider.observe( + timestamp=timestamp, + env_ids=env_ids.clone(), + ) + if not isinstance(state, EntityState): + raise TypeError( + f"State provider for {entity_id!r} must return EntityState." + ) + states[entity_id] = EntityState( + self._normalize_pose(state.pose, batch_size, entity_id), + confidence=state.confidence, + ) + + for registration in relative_registrations: + entity_id = registration.ref.entity_id + assert registration.parent is not None + assert registration.relative_pose is not None + parent_state = states[registration.parent.entity_id] + relative_pose = registration.relative_pose.to( + device=parent_state.pose.device, + dtype=parent_state.pose.dtype, + ) + pose = torch.matmul(parent_state.pose, relative_pose) + states[entity_id] = EntityState( + pose, + confidence=parent_state.confidence, + ) + return states + + @staticmethod + def _normalize_pose( + pose: torch.Tensor, + batch_size: int, + entity_id: str, + ) -> torch.Tensor: + if pose.shape == (4, 4): + return pose.unsqueeze(0).expand(batch_size, -1, -1).clone() + if pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Scene entity {entity_id!r} pose must have shape (4, 4) or " + f"({batch_size}, 4, 4)." + ) + return pose.clone() + + def _pose_change_mask( + self, + previous: torch.Tensor, + current: torch.Tensor, + ) -> torch.Tensor: + """Return CPU rows changed against the last material publication.""" + current = current.to(device=previous.device, dtype=previous.dtype) + translation = torch.linalg.vector_norm( + current[:, :3, 3] - previous[:, :3, 3], + dim=1, + ) + relative_rotation = torch.bmm( + previous[:, :3, :3].transpose(1, 2), + current[:, :3, :3], + ) + cosine = ( + (relative_rotation.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) / 2.0 + ).clamp(-1.0, 1.0) + rotation = torch.acos(cosine) + return ( + ( + (translation > self.translation_threshold) + | (rotation > self.rotation_threshold) + ) + .detach() + .to("cpu") + ) + + +__all__ = [ + "RegistrySceneProvider", + "SceneAffordanceRef", + "SceneArticulationRef", + "SceneCollisionRole", + "SceneCollisionWorldMode", + "SceneDynamics", + "SceneEntityRef", + "SceneEntityRegistration", + "SceneEntityStateProvider", + "SceneGeometryProvider", + "SceneLinkRef", + "SceneObjectRef", + "SceneRegistry", +] diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 2a4ae179c..618da4545 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -544,6 +544,54 @@ def test_scene_snapshot_expands_global_collision_world_revision() -> None: assert torch.equal(obstacle_poses["obstacle"], pose) +def test_scene_snapshot_owns_entity_state_storage() -> None: + pose = torch.eye(4) + state = EntityState(pose) + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"object": state}, + ) + + pose.fill_(2.0) + state.pose.fill_(3.0) + + assert torch.equal(snapshot.entities["object"].pose, torch.eye(4)) + + +def test_scene_snapshot_entity_reads_are_defensive() -> None: + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"object": EntityState(torch.eye(4))}, + ) + + first_read = snapshot.entities["object"] + first_read.pose.fill_(7.0) + + assert torch.equal(snapshot.entities["object"].pose, torch.eye(4)) + with pytest.raises(TypeError): + snapshot.entities["other"] = EntityState(torch.eye(4)) # type: ignore[index] + + +def test_scene_snapshot_collision_pose_reads_are_defensive() -> None: + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"obstacle": EntityState(torch.eye(4))}, + collision_entity_ids=("obstacle",), + ) + + obstacle_poses = snapshot.collision_obstacle_poses( + batch_size=1, + device=torch.device("cpu"), + dtype=torch.float32, + ) + obstacle_poses["obstacle"].fill_(5.0) + + assert torch.equal(snapshot.entities["obstacle"].pose, torch.eye(4)) + + def test_scene_snapshot_rejects_unknown_collision_entity() -> None: with pytest.raises(ValueError, match="missing scene entities"): SceneSnapshot( diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index ba86c7d4a..b1f5ec92e 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -27,6 +27,7 @@ import importlib import logging import math +from types import SimpleNamespace import pytest import torch @@ -229,6 +230,28 @@ def test_curobo_world_cfg_accepts_registered_dynamic_obstacle(): assert cfg.dynamic_obstacle_names == ["known"] +def test_curobo_world_cfg_mapping_uses_registry_id_for_dynamic_obstacle(): + obstacle = type("NamedObstacle", (), {"uid": "legacy_uid"})() + + cfg = CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names=["registry_cube"], + ) + + assert cfg.dynamic_obstacle_names == ["registry_cube"] + assert cfg.rigid_objects["registry_cube"] is obstacle + + +def test_curobo_world_cfg_mapping_does_not_accept_object_uid_as_alias(): + obstacle = type("NamedObstacle", (), {"uid": "legacy_uid"})() + + with pytest.raises(ValueError, match="not present in rigid_objects"): + CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names=["legacy_uid"], + ) + + def test_curobo_world_cfg_rejects_unregistered_dynamic_obstacle(): obstacle = type("NamedObstacle", (), {"uid": "known"})() @@ -249,6 +272,28 @@ def test_curobo_world_cfg_rejects_duplicate_dynamic_obstacle_names(): ) +def test_curobo_world_cfg_rejects_outer_whitespace_in_obstacle_ids(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(ValueError, match="without outer whitespace"): + CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names=[" registry_cube"], + ) + with pytest.raises(ValueError, match="without outer whitespace"): + CuroboWorldCfg(rigid_objects={" registry_cube": obstacle}) + + +def test_curobo_world_cfg_rejects_string_dynamic_obstacle_collection(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(TypeError, match="not a string"): + CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names="registry_cube", # type: ignore[arg-type] + ) + + def test_curobo_world_cfg_rejects_duplicate_rigid_object_names(): obstacle_type = type("NamedObstacle", (), {"uid": "duplicate"}) @@ -256,6 +301,26 @@ def test_curobo_world_cfg_rejects_duplicate_rigid_object_names(): CuroboWorldCfg(rigid_objects=[obstacle_type(), obstacle_type()]) +@pytest.mark.parametrize( + ("multi_env", "expected_mode"), + [(False, "shared"), (True, "per_env")], +) +def test_curobo_planner_exposes_collision_world_contract(multi_env, expected_mode): + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg( + rigid_objects={"registry_cube": object()}, + dynamic_obstacle_names=["registry_cube"], + multi_env=multi_env, + ), + ) + + assert planner.dynamic_collision_entity_ids == ("registry_cube",) + assert planner.collision_world_entity_ids == ("registry_cube",) + assert planner.collision_world_batch_mode == expected_mode + + def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): planner = object.__new__(CuroboPlanner) configured_pose = torch.eye(4).unsqueeze(0) @@ -551,6 +616,88 @@ def test_generate_cuboid_world_yaml_assembles_schema(tmp_path): assert data["cuboid"]["demo_block"]["pose"][:3] == pytest.approx([0.45, 0.0, 0.18]) +def test_generate_world_yaml_uses_mapping_key_instead_of_object_uid(tmp_path): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + output_path = tmp_path / "registry_world.yml" + + generate_curobo_world_yaml( + {"registry_cube": rigid_object}, + str(output_path), + representation="cuboid", + ) + data = yaml.safe_load(output_path.read_text(encoding="utf-8")) + + assert set(data["cuboid"]) == {"registry_cube"} + + +def test_world_yaml_cache_key_includes_registry_id(): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg(rigid_objects={"registry_cube": rigid_object}), + ) + registry_key = planner._world_yaml_cache_key(planner.cfg.world) + planner.cfg.world = CuroboWorldCfg( + rigid_objects={"renamed_registry_cube": rigid_object} + ) + + renamed_key = planner._world_yaml_cache_key(planner.cfg.world) + + assert registry_key != renamed_key + + +def test_dynamic_update_uses_registry_id_in_curobo_backend(): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg( + rigid_objects={"registry_cube": rigid_object}, + obstacle_representation="cuboid", + dynamic_obstacle_names=["registry_cube"], + ), + ) + planner._curobo_device = torch.device("cpu") + planner._bindings = SimpleNamespace(Pose=lambda **kwargs: kwargs) + updates = [] + collision_checker = SimpleNamespace( + update_obstacle_pose=lambda name, pose, env_idx: updates.append( + (name, pose, env_idx) + ) + ) + backend = SimpleNamespace( + batch_size=1, + profile=SimpleNamespace(sim_base_to_curobo_base=None), + sim_base_to_curobo_base_matrix=None, + planner=SimpleNamespace(scene_collision_checker=collision_checker), + ) + identity = torch.eye(4).unsqueeze(0) + + planner.update_dynamic_obstacles( + {"registry_cube": identity}, + backend=backend, + sim_base_pose_inv=identity, + ) + + assert [(name, env_idx) for name, _, env_idx in updates] == [("registry_cube", 0)] + + def test_generate_mesh_world_yaml_assembles_schema(tmp_path): rigid_object = _FakeRigidObject( "demo_block", @@ -604,6 +751,21 @@ def test_generate_world_yaml_rejects_empty_input(tmp_path): generate_curobo_world_yaml([], str(tmp_path / "world.yml")) +def test_registry_world_yaml_rejects_empty_geometry_instead_of_skipping(tmp_path): + rigid_object = _FakeRigidObject( + "legacy_uid", + torch.zeros((0, 3), dtype=torch.float32), + torch.zeros((0, 3), dtype=torch.int64), + _identity_pose(), + ) + + with pytest.raises(ValueError, match="Registry-backed obstacle.*no mesh"): + generate_curobo_world_yaml( + {"registry_cube": rigid_object}, + str(tmp_path / "world.yml"), + ) + + def test_generate_world_yaml_rejects_duplicate_names(tmp_path): pose = _identity_pose() first = _FakeRigidObject( @@ -626,6 +788,21 @@ def test_generate_world_yaml_rejects_duplicate_names(tmp_path): ) +def test_generate_world_yaml_rejects_outer_whitespace_in_mapping_id(tmp_path): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + + with pytest.raises(ValueError, match="without outer whitespace"): + generate_curobo_world_yaml( + {" registry_cube": rigid_object}, + str(tmp_path / "world.yml"), + ) + + def test_generated_cuboid_yaml_loads_in_curobo_scene_cfg(tmp_path): pytest.importorskip("curobo") from curobo._src.geom.types import SceneCfg diff --git a/tests/sim/planners/test_motion_generator_batched.py b/tests/sim/planners/test_motion_generator_batched.py index a2adbdb49..32d18fd9e 100644 --- a/tests/sim/planners/test_motion_generator_batched.py +++ b/tests/sim/planners/test_motion_generator_batched.py @@ -129,6 +129,7 @@ def test_direct_cartesian_planner_requires_joint_fallback_inputs(): def test_bind_collision_world_copies_caller_options() -> None: planner = Mock() planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("obstacle",) original = PlanOptions() obstacle_pose = torch.eye(4).unsqueeze(0) @@ -152,6 +153,143 @@ def bind(options, *, obstacle_poses): planner.with_collision_world.assert_called_once() +@pytest.mark.parametrize( + ("configured_ids", "obstacle_poses", "expected"), + [ + (("cube", "tray"), {"cube": torch.eye(4).unsqueeze(0)}, "missing"), + ( + ("cube",), + { + "cube": torch.eye(4).unsqueeze(0), + "tray": torch.eye(4).unsqueeze(0), + }, + "extra", + ), + ], +) +def test_bind_collision_world_requires_exact_planner_entity_ids( + configured_ids, obstacle_poses, expected +) -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = configured_ids + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(ValueError, match=expected): + generator.bind_collision_world(None, obstacle_poses=obstacle_poses) + + planner.with_collision_world.assert_not_called() + + +def test_bind_collision_world_rejects_extra_ids_in_caller_options() -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("cube",) + generator = object.__new__(MotionGenerator) + generator.planner = planner + options = PlanOptions() + options.dynamic_obstacle_poses = {"legacy_cube": torch.eye(4).unsqueeze(0)} + + with pytest.raises(ValueError, match="Caller planning options.*legacy_cube"): + generator.bind_collision_world( + options, + obstacle_poses={"cube": torch.eye(4).unsqueeze(0)}, + ) + + planner.with_collision_world.assert_not_called() + + +def test_bind_collision_world_rejects_ids_injected_by_backend() -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("cube",) + + def bind(options, *, obstacle_poses): + options.dynamic_obstacle_poses = { + **obstacle_poses, + "legacy_cube": torch.eye(4).unsqueeze(0), + } + return options + + planner.with_collision_world.side_effect = bind + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(ValueError, match="Bound dynamic collision.*legacy_cube"): + generator.bind_collision_world( + PlanOptions(), + obstacle_poses={"cube": torch.eye(4).unsqueeze(0)}, + ) + + +def test_bind_collision_world_allows_none_for_empty_configured_world() -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = () + planner.default_plan_options.return_value = PlanOptions() + + def bind(options, *, obstacle_poses): + assert obstacle_poses == {} + options.dynamic_obstacle_poses = None + return options + + planner.with_collision_world.side_effect = bind + generator = object.__new__(MotionGenerator) + generator.planner = planner + + bound = generator.bind_collision_world(None, obstacle_poses={}) + + assert bound.dynamic_obstacle_poses is None + + +def test_bind_collision_world_rejects_non_string_option_keys() -> None: + planner = Mock() + planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = () + generator = object.__new__(MotionGenerator) + generator.planner = planner + options = PlanOptions() + options.dynamic_obstacle_poses = {1: torch.eye(4).unsqueeze(0)} + + with pytest.raises(TypeError, match="keys must be non-empty strings"): + generator.bind_collision_world(options, obstacle_poses={}) + + planner.with_collision_world.assert_not_called() + + +def test_motion_generator_exposes_collision_integration_metadata() -> None: + planner = Mock() + planner.dynamic_collision_entity_ids = ("cube", "tray") + planner.collision_world_entity_ids = ("cube", "tray", "table") + planner.collision_world_batch_mode = "per_env" + generator = object.__new__(MotionGenerator) + generator.planner = planner + + assert generator.dynamic_collision_entity_ids == ("cube", "tray") + assert generator.collision_world_entity_ids == ("cube", "tray", "table") + assert generator.collision_world_batch_mode == "per_env" + + +@pytest.mark.parametrize( + ("entity_ids", "error_type", "match"), + [ + (("cube", "cube"), ValueError, "unique"), + ((" cube",), TypeError, "outer whitespace"), + ], +) +def test_motion_generator_rejects_invalid_collision_entity_metadata( + entity_ids, error_type, match +) -> None: + planner = Mock() + planner.dynamic_collision_entity_ids = entity_ids + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(error_type, match=match): + _ = generator.dynamic_collision_entity_ids + + def test_bind_collision_world_rejects_unsupported_planner() -> None: planner = Mock() planner.supports_collision_world_updates = False @@ -171,6 +309,7 @@ def test_bind_collision_world_rejects_unsupported_planner() -> None: def test_bind_collision_world_uses_backend_default_options() -> None: planner = Mock() planner.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("obstacle",) defaults = PlanOptions() planner.default_plan_options.return_value = defaults planner.with_collision_world.return_value = defaults diff --git a/tests/sim/skills/__init__.py b/tests/sim/skills/__init__.py new file mode 100644 index 000000000..8dc25c19d --- /dev/null +++ b/tests/sim/skills/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for semantic-skill integration contracts.""" + +from __future__ import annotations diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py new file mode 100644 index 000000000..89c7b34fc --- /dev/null +++ b/tests/sim/skills/test_scene.py @@ -0,0 +1,868 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for authoritative semantic-scene registrations.""" + +from __future__ import annotations + +from dataclasses import FrozenInstanceError + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import Affordance, EntityState, SceneSnapshot +from embodichain.lab.sim.skills import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneEntityRegistration, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + + +class _StateProvider: + """Return one fixed identity pose for registration validation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp + return EntityState(torch.eye(4).repeat(env_ids.numel(), 1, 1)) + + +class _GeometryProvider: + """Return one opaque collision-geometry descriptor.""" + + def get_geometry(self) -> object: + return {"kind": "box"} + + +class _EmptyGeometryProvider: + """Satisfy the geometry protocol but fail to materialize a descriptor.""" + + def get_geometry(self) -> object: + return None + + +class _MutableStateProvider: + """Expose a mutable pose while recording provider calls.""" + + def __init__(self, pose: torch.Tensor) -> None: + self.pose = pose + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + return EntityState(self.pose) + + +class _MotionGenerator: + """Minimal dynamic-collision integration surface.""" + + def __init__( + self, + *, + entity_ids: tuple[str, ...], + world_entity_ids: tuple[str, ...] | None = None, + supports_updates: bool = True, + batch_mode: str | None = "per_env", + ) -> None: + self.dynamic_collision_entity_ids = entity_ids + self.collision_world_entity_ids = ( + entity_ids if world_entity_ids is None else world_entity_ids + ) + self.supports_dynamic_collision_world = supports_updates + self.collision_world_batch_mode = batch_mode + + +class _ExternalSceneProvider: + """External provider with an explicit concrete collision declaration.""" + + def __init__(self, entity_ids: tuple[str, ...]) -> None: + self.collision_entity_ids = entity_ids + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + del timestamp, env_ids + raise NotImplementedError + + +class _SimulationEntity: + """Simulation entity pose source used by the opt-in adapter tests.""" + + def __init__(self, pose: torch.Tensor) -> None: + self.pose = pose + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix is True + return self.pose + + +class _Simulation: + """Minimal simulation lookup surface with selected and unselected assets.""" + + def __init__(self) -> None: + self.rigid_objects = { + "sim_cube": _SimulationEntity(torch.eye(4)), + "ignored": _SimulationEntity(torch.eye(4) * 2.0), + } + self.articulations = { + "sim_drawer": _SimulationEntity(torch.eye(4)), + } + + def get_rigid_object(self, uid: str) -> _SimulationEntity | None: + return self.rigid_objects.get(uid) + + def get_articulation(self, uid: str) -> _SimulationEntity | None: + return self.articulations.get(uid) + + +@pytest.mark.parametrize("entity_id", ["", " cube", "cube "]) +def test_scene_entity_ref_rejects_non_exact_identifier(entity_id: str) -> None: + with pytest.raises(ValueError, match="entity_id"): + SceneObjectRef(entity_id) + + +def test_scene_entity_refs_are_typed_and_immutable() -> None: + object_ref = SceneObjectRef("cube") + + assert object_ref != SceneArticulationRef("cube") + with pytest.raises(FrozenInstanceError): + object_ref.entity_id = "other" # type: ignore[misc] + + +def test_registration_normalizes_self_alias_without_rewriting_names() -> None: + registration = SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("cube", "sim_cube"), + ) + + assert registration.aliases == ("sim_cube",) + + +def test_registration_rejects_duplicate_aliases() -> None: + with pytest.raises(ValueError, match="aliases"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("sim_cube", "sim_cube"), + ) + + +def test_registration_rejects_string_as_alias_collection() -> None: + with pytest.raises(TypeError, match="aliases.*not a string"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases="sim_cube", # type: ignore[arg-type] + ) + + +def test_root_registration_requires_explicit_state_provider() -> None: + with pytest.raises(ValueError, match="state_provider"): + SceneEntityRegistration(ref=SceneObjectRef("cube")) + + +def test_link_registration_requires_parent_and_native_name() -> None: + with pytest.raises(ValueError, match="parent and native_name"): + SceneEntityRegistration( + ref=SceneLinkRef("drawer_handle_link"), + state_provider=_StateProvider(), + ) + + +def test_affordance_registration_owns_parent_relation_and_pose() -> None: + relative_pose = torch.eye(4) + registration = SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + parent=SceneLinkRef("drawer_handle_link"), + native_name="handle", + affordance=Affordance(), + relative_pose=relative_pose, + ) + relative_pose.fill_(4.0) + + assert registration.relative_pose is not None + assert torch.equal(registration.relative_pose, torch.eye(4)) + + +def test_affordance_registration_rejects_two_pose_sources() -> None: + with pytest.raises(ValueError, match="mutually exclusive"): + SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + state_provider=_StateProvider(), + parent=SceneLinkRef("drawer_handle_link"), + native_name="handle", + affordance=Affordance(), + relative_pose=torch.eye(4), + ) + + +def test_collision_registration_requires_geometry_provider() -> None: + with pytest.raises(ValueError, match="geometry_provider"): + SceneEntityRegistration( + ref=SceneObjectRef("obstacle"), + state_provider=_StateProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ) + + registration = SceneEntityRegistration( + ref=SceneObjectRef("obstacle"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ) + assert registration.geometry_provider is not None + + +def test_registry_resolves_aliases_to_typed_canonical_refs() -> None: + cube_ref = SceneObjectRef("cube") + drawer_ref = SceneArticulationRef("drawer") + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=cube_ref, + state_provider=_StateProvider(), + aliases=("sim_cube",), + ), + SceneEntityRegistration( + ref=drawer_ref, + state_provider=_StateProvider(), + aliases=("sim_drawer",), + ), + ) + ) + + assert registry.resolve("sim_cube", expected_type=SceneObjectRef) is cube_ref + assert registry.lookup("sim_drawer").ref is drawer_ref + assert registry.aliases == { + "sim_cube": "cube", + "sim_drawer": "drawer", + } + + with pytest.raises(TypeError, match="SceneObjectRef"): + registry.resolve("sim_cube", expected_type=SceneArticulationRef) + with pytest.raises(TypeError, match="SceneArticulationRef"): + registry.resolve(SceneArticulationRef("cube")) + + +def test_registry_enforces_one_flat_global_id_namespace() -> None: + registrations = ( + SceneEntityRegistration( + ref=SceneObjectRef("shared"), + state_provider=_StateProvider(), + ), + SceneEntityRegistration( + ref=SceneArticulationRef("shared"), + state_provider=_StateProvider(), + ), + ) + + with pytest.raises(ValueError, match="Duplicate canonical"): + SceneRegistry(registrations) + + +def test_registry_rejects_alias_collision_with_canonical_id() -> None: + with pytest.raises(ValueError, match="collides with canonical"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("drawer",), + ), + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=_StateProvider(), + ), + ) + ) + + +def test_registry_rejects_ambiguous_aliases_across_types() -> None: + with pytest.raises(ValueError, match="ambiguous"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("legacy",), + ), + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=_StateProvider(), + aliases=("legacy",), + ), + ) + ) + + +def test_registry_requires_registered_exact_typed_parent() -> None: + link_registration = SceneEntityRegistration( + ref=SceneLinkRef("drawer_link"), + parent=SceneArticulationRef("drawer"), + native_name="link", + state_provider=_StateProvider(), + ) + + with pytest.raises(ValueError, match="unregistered parent"): + SceneRegistry((link_registration,)) + with pytest.raises(TypeError, match="registered as SceneObjectRef"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("drawer"), + state_provider=_StateProvider(), + ), + link_registration, + ) + ) + + +@pytest.mark.parametrize( + "ref_type", + [SceneLinkRef, SceneAffordanceRef], +) +def test_registry_rejects_duplicate_parent_native_member(ref_type: type) -> None: + parent = SceneArticulationRef("drawer") + + def member_registration(entity_id: str) -> SceneEntityRegistration: + if ref_type is SceneLinkRef: + return SceneEntityRegistration( + ref=SceneLinkRef(entity_id), + parent=parent, + native_name="handle", + state_provider=_StateProvider(), + ) + return SceneEntityRegistration( + ref=SceneAffordanceRef(entity_id), + parent=parent, + native_name="handle", + affordance=Affordance(), + relative_pose=torch.eye(4), + ) + + with pytest.raises(ValueError, match="native_name.*already registered"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=parent, + state_provider=_StateProvider(), + ), + member_registration("first"), + member_registration("second"), + ) + ) + + +def test_registry_is_structurally_immutable_and_owns_relative_pose() -> None: + parent = SceneObjectRef("drawer") + relative_pose = torch.eye(4) + affordance_registration = SceneEntityRegistration( + ref=SceneAffordanceRef("handle"), + parent=parent, + native_name="handle", + affordance=Affordance(), + relative_pose=relative_pose, + ) + registrations = [ + SceneEntityRegistration( + ref=parent, + state_provider=_StateProvider(), + ), + affordance_registration, + ] + registry = SceneRegistry(registrations) + + registrations.clear() + relative_pose.fill_(3.0) + assert len(registry) == 2 + returned_pose = registry.lookup("handle").relative_pose + assert returned_pose is not None + assert torch.equal(returned_pose, torch.eye(4)) + returned_pose.fill_(5.0) + assert torch.equal(registry.lookup("handle").relative_pose, torch.eye(4)) + with pytest.raises(TypeError): + registry.aliases["new"] = "drawer" # type: ignore[index] + with pytest.raises(FrozenInstanceError): + registry.collision_world_mode = SceneCollisionWorldMode.SHARED # type: ignore[misc] + + +def test_registry_owns_and_defensively_copies_affordance_metadata() -> None: + parent = SceneObjectRef("drawer") + affordance = Affordance(custom_config={"limits": {"opening": 0.3}}) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=parent, + state_provider=_StateProvider(), + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("handle"), + parent=parent, + native_name="handle", + affordance=affordance, + relative_pose=torch.eye(4), + ), + ) + ) + + affordance.custom_config["limits"]["opening"] = 0.8 + public_affordance = registry.lookup("handle").affordance + assert public_affordance is not None + assert public_affordance.custom_config["limits"]["opening"] == 0.3 + + public_affordance.custom_config["limits"]["opening"] = 1.0 + second_read = registry.lookup("handle").affordance + assert second_read is not None + assert second_read.custom_config["limits"]["opening"] == 0.3 + + +def test_registry_provider_uses_canonical_ids_and_derives_relative_pose() -> None: + parent_pose = torch.eye(4).repeat(2, 1, 1) + parent_pose[:, 0, 3] = torch.tensor([1.0, 2.0]) + relative_pose = torch.eye(4) + relative_pose[1, 3] = 0.25 + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("drawer"), + state_provider=_MutableStateProvider(parent_pose), + aliases=("sim_drawer",), + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("handle"), + parent=SceneObjectRef("drawer"), + native_name="handle", + affordance=Affordance(), + relative_pose=relative_pose, + ), + ) + ) + + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor([10, 20], dtype=torch.long), + ) + + assert set(snapshot.entities) == {"drawer", "handle"} + assert "sim_drawer" not in snapshot.entities + assert torch.equal( + snapshot.entities["handle"].pose, + torch.matmul(parent_pose, relative_pose), + ) + + +def test_registry_providers_have_independent_revisions() -> None: + state_provider = _MutableStateProvider(torch.eye(4)) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=state_provider, + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + first_provider = registry.make_scene_provider() + second_provider = registry.make_scene_provider() + env_ids = torch.tensor([0, 1], dtype=torch.long) + first_provider.snapshot(timestamp=0.0, env_ids=env_ids) + moved = torch.eye(4).repeat(2, 1, 1) + moved[1, 0, 3] = 0.1 + state_provider.pose = moved + + changed = first_provider.snapshot(timestamp=1.0, env_ids=env_ids) + independent_initial = second_provider.snapshot(timestamp=1.0, env_ids=env_ids) + + assert changed.version == 1 + assert changed.collision_world_revisions(2) == (0, 1) + assert independent_initial.version == 0 + assert independent_initial.collision_world_revisions(2) == (0, 0) + + +def test_registry_provider_accumulates_subthreshold_motion_per_row() -> None: + pose = torch.eye(4).repeat(2, 1, 1) + state_provider = _MutableStateProvider(pose) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=state_provider, + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + provider = registry.make_scene_provider(translation_threshold=0.01) + env_ids = torch.tensor([4, 8], dtype=torch.long) + provider.snapshot(timestamp=0.0, env_ids=env_ids) + first_motion = pose.clone() + first_motion[1, 0, 3] = 0.006 + state_provider.pose = first_motion + + below_threshold = provider.snapshot(timestamp=1.0, env_ids=env_ids) + second_motion = first_motion.clone() + second_motion[1, 0, 3] = 0.012 + state_provider.pose = second_motion + accumulated_change = provider.snapshot(timestamp=2.0, env_ids=env_ids) + + assert below_threshold.version == 0 + assert below_threshold.collision_world_revisions(2) == (0, 0) + assert accumulated_change.version == 1 + assert accumulated_change.collision_world_revisions(2) == (0, 1) + + +def test_multi_env_dynamic_collision_requires_explicit_mode_before_observation() -> ( + None +): + state_provider = _MutableStateProvider(torch.eye(4)) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=state_provider, + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ) + ) + + with pytest.raises(ValueError, match="explicit collision_world_mode"): + registry.make_scene_provider(batch_size=2) + provider = registry.make_scene_provider() + with pytest.raises(ValueError, match="explicit collision_world_mode"): + provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0, 1], dtype=torch.long), + ) + assert state_provider.calls == 0 + + +def test_single_env_dynamic_collision_defaults_to_shared_mode() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ) + ) + provider = registry.make_scene_provider(batch_size=1) + + assert provider.collision_world_mode is SceneCollisionWorldMode.SHARED + + snapshot = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + assert provider.collision_entity_ids == ("cube",) + assert snapshot.collision_world_revisions(1) == (0,) + + +def test_collision_integration_requires_exact_canonical_ids_and_mode() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + aliases=("sim_cube",), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + assert ( + registry.validate_collision_integration( + _MotionGenerator(entity_ids=("cube",)), # type: ignore[arg-type] + batch_size=2, + scene_provider=_ExternalSceneProvider(("cube",)), + ) + is SceneCollisionWorldMode.PER_ENV + ) + with pytest.raises(ValueError, match="authoritative registry IDs"): + registry.validate_collision_integration( + _MotionGenerator(entity_ids=("sim_cube",)), # type: ignore[arg-type] + batch_size=2, + ) + with pytest.raises(ValueError, match="does not support"): + registry.validate_collision_integration( + _MotionGenerator( # type: ignore[arg-type] + entity_ids=("cube",), + supports_updates=False, + ), + batch_size=2, + ) + with pytest.raises(ValueError, match="mode mismatch"): + registry.validate_collision_integration( + _MotionGenerator( # type: ignore[arg-type] + entity_ids=("cube",), + batch_mode="shared", + ), + batch_size=2, + ) + + +def test_collision_integration_requires_exact_full_world_ids() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + SceneEntityRegistration( + ref=SceneObjectRef("table"), + aliases=("legacy_table",), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.STATIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + assert registry.collision_world_entity_ids == ("cube", "table") + assert ( + registry.validate_collision_integration( + _MotionGenerator( + entity_ids=("cube",), + world_entity_ids=("cube", "table"), + ), # type: ignore[arg-type] + batch_size=2, + ) + is SceneCollisionWorldMode.PER_ENV + ) + with pytest.raises(ValueError, match="Collision world.*authoritative registry IDs"): + registry.validate_collision_integration( + _MotionGenerator( + entity_ids=("cube",), + world_entity_ids=("cube", "legacy_table"), + ), # type: ignore[arg-type] + batch_size=2, + ) + + +def test_static_only_collision_world_does_not_require_dynamic_updates() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("table"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.STATIC, + ), + ) + ) + + assert ( + registry.validate_collision_integration( + _MotionGenerator( + entity_ids=(), + world_entity_ids=("table",), + supports_updates=False, + batch_mode=None, + ), # type: ignore[arg-type] + batch_size=2, + ) + is None + ) + + +def test_collision_integration_rejects_external_provider_id_drift() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + with pytest.raises(ValueError, match="provider.*authoritative registry IDs"): + registry.validate_collision_integration( + _MotionGenerator(entity_ids=("cube",)), # type: ignore[arg-type] + batch_size=2, + scene_provider=_ExternalSceneProvider(("legacy_cube",)), + ) + + +def test_planning_provider_factory_validates_before_returning_provider() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + provider = registry.make_planning_scene_provider( + _MotionGenerator(entity_ids=("cube",)), # type: ignore[arg-type] + batch_size=2, + ) + assert provider.collision_entity_ids == ("cube",) + + with pytest.raises(ValueError, match="entity mismatch"): + registry.make_planning_scene_provider( + _MotionGenerator(entity_ids=("other",)), # type: ignore[arg-type] + batch_size=2, + ) + + with pytest.raises(ValueError, match="configured batch_size=2"): + provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + +def test_collision_geometry_is_materialized_under_canonical_ids() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("dynamic_cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + SceneEntityRegistration( + ref=SceneObjectRef("static_table"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.STATIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.SHARED, + ) + + all_geometry = registry.collision_geometry_by_id() + dynamic_geometry = registry.collision_geometry_by_id(SceneCollisionRole.DYNAMIC) + + assert set(all_geometry) == {"dynamic_cube", "static_table"} + assert set(dynamic_geometry) == {"dynamic_cube"} + with pytest.raises(TypeError): + all_geometry["other"] = {} # type: ignore[index] + + +def test_collision_integration_rejects_empty_dynamic_geometry() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_EmptyGeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ) + ) + + with pytest.raises(ValueError, match="scene entity 'cube'.*None"): + registry.validate_collision_integration( + _MotionGenerator( # type: ignore[arg-type] + entity_ids=("cube",), + batch_mode="shared", + ), + batch_size=1, + ) + + +def test_from_simulation_is_explicit_and_uses_uid_only_as_alias() -> None: + simulation = _Simulation() + + registry = SceneRegistry.from_simulation( + simulation, # type: ignore[arg-type] + rigid_objects={"cube": "sim_cube"}, + ) + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + assert len(registry) == 1 + assert registry.resolve("sim_cube") == SceneObjectRef("cube") + assert registry.lookup("cube").collision_role is SceneCollisionRole.NONE + assert registry.dynamic_collision_entity_ids == () + assert registry.collision_geometry_by_id() == {} + assert set(snapshot.entities) == {"cube"} + assert "ignored" not in snapshot.entities + + +def test_from_simulation_derives_live_geometry_only_for_explicit_collision_role() -> ( + None +): + simulation = _Simulation() + registry = SceneRegistry.from_simulation( + simulation, # type: ignore[arg-type] + rigid_objects={"cube": "sim_cube"}, + collision_roles={"cube": SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.SHARED, + ) + + assert registry.dynamic_collision_entity_ids == ("cube",) + assert registry.collision_geometry_by_id() == { + "cube": simulation.rigid_objects["sim_cube"] + } + + +def test_from_simulation_allows_geometry_provider_override() -> None: + registry = SceneRegistry.from_simulation( + _Simulation(), # type: ignore[arg-type] + rigid_objects={"cube": "sim_cube"}, + collision_roles={"cube": SceneCollisionRole.STATIC}, + geometry_providers={"cube": _GeometryProvider()}, + ) + + assert registry.collision_geometry_by_id() == {"cube": {"kind": "box"}} + + +def test_from_simulation_requires_selected_uid_to_exist() -> None: + with pytest.raises(KeyError, match="missing"): + SceneRegistry.from_simulation( + _Simulation(), # type: ignore[arg-type] + articulations={"drawer": "missing"}, + ) diff --git a/tests/sim/skills/test_scene_curobo_integration.py b/tests/sim/skills/test_scene_curobo_integration.py new file mode 100644 index 000000000..13facf1f3 --- /dev/null +++ b/tests/sim/skills/test_scene_curobo_integration.py @@ -0,0 +1,115 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Cross-layer CPU tests for registry-backed cuRobo obstacle identity.""" + +from __future__ import annotations + +import torch + +from embodichain.lab.sim.planners import ( + CuroboPlanOptions, + CuroboPlanner, + CuroboPlannerCfg, + CuroboWorldCfg, + MotionGenerator, +) +from embodichain.lab.sim.skills import ( + SceneCollisionRole, + SceneCollisionWorldMode, + SceneRegistry, +) + + +class _RigidObject: + """Minimal live rigid-object geometry and pose surface.""" + + def __init__(self) -> None: + self.uid = "legacy_cube" + self.pose = torch.eye(4).repeat(2, 1, 1) + + def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: + assert to_matrix is True + return self.pose + + def get_vertices( + self, + env_ids: list[int], + *, + scale: bool, + ) -> list[torch.Tensor]: + assert env_ids == [0] + assert scale is True + return [torch.zeros(8, 3)] + + def get_triangles(self, env_ids: list[int]) -> list[torch.Tensor]: + assert env_ids == [0] + return [torch.zeros(12, 3, dtype=torch.long)] + + +class _Simulation: + """Resolve one rigid object through its simulation-native UID.""" + + def __init__(self, rigid_object: _RigidObject) -> None: + self.rigid_object = rigid_object + + def get_rigid_object(self, uid: str) -> _RigidObject | None: + return self.rigid_object if uid == self.rigid_object.uid else None + + +def test_registry_id_remains_authoritative_through_curobo_binding() -> None: + rigid_object = _RigidObject() + registry = SceneRegistry.from_simulation( + _Simulation(rigid_object), # type: ignore[arg-type] + rigid_objects={"cube": rigid_object.uid}, + collision_roles={"cube": SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + mode = registry.resolve_collision_world_mode(batch_size=2) + geometry = registry.collision_geometry_by_id() + world_cfg = CuroboWorldCfg( + rigid_objects=geometry, # type: ignore[arg-type] + obstacle_representation="cuboid", + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), + multi_env=mode is SceneCollisionWorldMode.PER_ENV, + ) + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg(robot_uid="unused", world=world_cfg) + motion_generator = object.__new__(MotionGenerator) + motion_generator.planner = planner + provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=2, + ) + + snapshot = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0, 1], dtype=torch.long), + ) + obstacle_poses = snapshot.collision_obstacle_poses( + batch_size=2, + device=torch.device("cpu"), + dtype=torch.float32, + ) + bound = motion_generator.bind_collision_world( + CuroboPlanOptions(), + obstacle_poses=obstacle_poses, + ) + + assert set(world_cfg.rigid_objects or {}) == {"cube"} + assert set(obstacle_poses) == {"cube"} + assert set(bound.dynamic_obstacle_poses or {}) == {"cube"} + assert rigid_object.uid not in obstacle_poses From 7879d7aae3fecbebeedfce4d88f929bc9ab0afba Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 10 Aug 2026 22:30:41 +0800 Subject: [PATCH 2/2] docs(sim): document scene registry integration --- agent_context/MAP.yaml | 40 +++ .../topics/atomic-actions/atomic-actions.md | 110 ++++++-- .../topics/motion-planning/motion-planning.md | 41 +++ .../design/declarative_expert_program_plan.md | 146 ++++++++--- .../embodichain/embodichain.lab.sim.rst | 14 +- .../embodichain.lab.sim.skills.rst | 72 ++++++ .../overview/sim/atomic_actions/index.md | 51 +++- docs/source/overview/sim/index.rst | 10 + .../overview/sim/planners/curobo_planner.md | 90 +++++-- docs/source/overview/sim/scene_registry.md | 244 ++++++++++++++++++ docs/source/tutorial/atomic_actions.rst | 39 ++- 11 files changed, 756 insertions(+), 101 deletions(-) create mode 100644 docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst create mode 100644 docs/source/overview/sim/scene_registry.md diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index b236d210c..3c26821ce 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -280,6 +280,8 @@ topics: - toppra - motion generator - trajectory planning + - curobo collision world + - dynamic collision integration - 运动生成 - 运动规划 - 轨迹规划 @@ -296,6 +298,16 @@ topics: - waypoint - velocity - acceleration + - canonical obstacle ID + - logical collision source ID + - physical YAML obstacle name + - sphere derived obstacle names + - empty collision mesh + - dynamic_collision_entity_ids + - collision_world_entity_ids + - collision_world_batch_mode + - collision_geometry_by_id + - make_planning_scene_provider paths: - topics/motion-planning/motion-planning.md source_of_truth: @@ -304,9 +316,11 @@ topics: - embodichain/lab/sim/planners/curobo/curobo_planner.py - embodichain/lab/sim/planners/curobo/curobo_yaml.py - embodichain/lab/sim/planners/motion_generator.py + - embodichain/lab/sim/skills/scene.py related_topics: - robot-system - ik-solvers + - atomic-actions status: active - id: rl-learning @@ -429,6 +443,8 @@ topics: - action primitive - object semantics - scene grounding + - scene registry + - semantic scene - AtomicAction - ActionInvocation - AtomicActionEngine @@ -453,6 +469,28 @@ topics: - SceneSnapshotSupplier - SceneProvider - RigidObjectSceneProvider + - SceneRegistry + - RegistrySceneProvider + - SceneEntityRef + - SceneObjectRef + - SceneArticulationRef + - SceneLinkRef + - SceneAffordanceRef + - SceneEntityRegistration + - SceneCollisionRole + - SceneCollisionWorldMode + - from_simulation + - validate_collision_integration + - make_planning_scene_provider + - collision_geometry_by_id + - collision_world_entity_ids + - canonical scene ID + - logical collision source ID + - sphere derived obstacle names + - empty collision mesh + - flat entity namespace + - native_name + - parent native source identity - ObjectSemantics - entity_id - frozen ObjectSemantics @@ -514,6 +552,8 @@ topics: - embodichain/lab/sim/atomic_actions/trajectory_ops.py - embodichain/lab/sim/atomic_actions/primitives/ - embodichain/lab/sim/atomic_actions/__init__.py + - embodichain/lab/sim/skills/scene.py + - embodichain/lab/sim/skills/__init__.py related_topics: - motion-planning - robot-system diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 4213c7477..80c21d65c 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -77,23 +77,81 @@ migrate an older custom action by renaming that implementation to `_plan()`. ## Object identity and pose grounding -`ObjectSemantics.entity_id` is the canonical pre-registry snapshot key. It is -optional for direct-core compatibility but, when supplied, must be a non-empty -string. Pose grounding with an explicit ID is strict: resolve it only from the -current `PlanningContext.scene`; a missing snapshot entry is an error and never -falls back to the live `entity`. Only when no ID is supplied may the core read -`ObjectSemantics.entity`; that path emits `DeprecationWarning`, reads live state, -and cannot declare a scene-motion dependency. +`ObjectSemantics.entity_id` is the typed core's canonical snapshot-key lowering +target. The registry-backed path obtains it from a resolved `SceneEntityRef`. +It remains optional for advanced direct-core compatibility but, when supplied, +must be a non-empty string. Pose grounding with an explicit ID is strict: +resolve it only from the current `PlanningContext.scene`; a missing snapshot +entry is an error and never falls back to the live `entity`. Only when no ID is +supplied may the core read `ObjectSemantics.entity`; that path emits +`DeprecationWarning`, reads live state, and cannot declare a scene-motion +dependency. `ObjectSemantics` is shallow-frozen. Top-level fields such as `entity_id`, `entity`, and `label` cannot be rebound after construction; create a new semantics value to change identity. Nested affordance and metadata objects may remain mutable, but they never establish identity. -`SceneSnapshot` owns copies of its input poses, but `EntityState.pose` tensors -are not deeply read-only. Callers must treat published snapshot values as -immutable and publish a newer scene version for changes. Enforced deep -immutability is deferred to the SceneRegistry/snapshot hardening phase. +`SceneSnapshot` owns copies of input entity states and returns a defensive +`EntityState`/pose copy on every public mapping lookup. Mutating an input tensor +or a previously returned pose cannot change the published snapshot. Publish a +new scene version for every material dynamic-state change. + +## Scene registry integration + +`embodichain.lab.sim.skills.SceneRegistry` is the canonical integration catalog. +It owns immutable registration metadata: typed identity, aliases, pose source, +parent relationships, backend-local names, dynamics, geometry, collision role, +semantic type, and affordance data. A `SceneSnapshot` does not duplicate that +catalog; it contains only versioned dynamic pose/confidence and collision +revision state. + +All object, articulation, link, and affordance IDs occupy one flat globally +unique namespace. Store link/affordance ancestry in +`SceneEntityRegistration.parent`, not by nesting or qualifying the ID. String +lookups may resolve aliases once to a canonical typed reference. An already +typed ref must contain a canonical ID and match the registered ref class. +Duplicate IDs, ambiguous aliases, alias/canonical collisions, missing parents, +and type mismatches fail at construction or lookup. Within one reference type, +the same `(parent, native_name)` physical source cannot be assigned multiple +canonical IDs; the same local name remains valid under different parents or +for different reference types. + +`SceneRegistry.from_simulation()` is explicit opt-in. Its `rigid_objects` and +`articulations` mappings are `registry_id -> simulation_uid`; selected UIDs are +installed as aliases, and unlisted simulation entities are never scanned. +Collision participation defaults to `NONE`, and every static/dynamic collision +registration requires a geometry provider. + +`registry.make_planning_scene_provider(motion_generator, batch_size=...)` +returns a fresh `RegistrySceneProvider` with independent baselines and revision +counters after eager registry/provider/planner validation. Snapshots expose +canonical IDs only. The provider requires stable ordered `env_ids` and +monotonic timestamps, derives relative affordance poses from the same +observation, compares movement against the last materially published pose, and +maintains per-row collision revisions. Plain `make_scene_provider()` is only +for perception and advanced direct-core consumers without planner agreement. + +For an external perception/hardware provider, call +`registry.validate_collision_integration(..., scene_provider=provider)` +directly. The registry's complete `STATIC ∪ DYNAMIC` ID set must exactly +match `MotionGenerator.collision_world_entity_ids`; separately, the registry, +provider, and planner dynamic ID sets must match exactly in the canonical +namespace. The planner must support live updates for a non-empty dynamic set, +and planner/registry batch mode must agree. With dynamic entities, one +environment may infer `SHARED`; multiple environments must explicitly select +`SceneCollisionWorldMode.SHARED` or `PER_ENV`. + +Construct a registry-backed cuRobo world with +`registry.collision_geometry_by_id()`. Its default mapping includes only +`STATIC` and `DYNAMIC` registrations and excludes `NONE`. Mapping keys are +canonical logical/source IDs for cache identity and full-world validation. With +`cuboid` or `mesh`, they are also the physical YAML and runtime-update keys. +Static `sphere` sources expand to backend names such as `id_0`; dynamic sphere +configuration is rejected, while cache/full-world identity stays on `id`. +Registry mappings fail fast when a source lacks geometry required by the chosen +representation. List-valued cuRobo worlds and `RigidObjectSceneProvider` remain +advanced direct-core paths. Stable object identity follows these exact rules: @@ -108,11 +166,10 @@ Stable object identity follows these exact rules: to the same live entity handle. `label` is descriptive and never establishes identity. -This is a snapshot/identity bridge, not alias resolution. A future -`SceneRegistry` owns uniqueness, aliases, normalization, and authoritative -registry IDs. Partial-batch `StateDelta` attachment merges use the same stable -identity rules, so equivalent semantic wrappers update one held object instead -of creating label-based duplicates. +The direct-core identity rules do not perform alias resolution; normalization +belongs only to `SceneRegistry`. Partial-batch `StateDelta` attachment merges +use the same stable identity rules, so equivalent semantic wrappers update one +held object instead of creating label-based duplicates. For both individual and coordinated attachments, a same-identity partial merge preserves scalar metadata: if any previously active environment row remains, @@ -251,11 +308,14 @@ acknowledgement timeout in their transport/controller layer. boundary used by execution adapters. `SceneSnapshot.collision_entity_ids` identifies obstacle poses consumed by a planner, while `collision_world_revision` is either global or per environment. A newer -revision invalidates only affected batch rows. `RigidObjectSceneProvider` -tracks live simulation objects, filters sub-threshold pose noise, advances the -general scene version, and maintains per-environment collision revisions. -Thresholds are measured from the last materially published pose per entity and -environment, so repeated sub-threshold motion eventually becomes observable. +revision invalidates only affected batch rows. `RegistrySceneProvider` is the +canonical provider and derives its entity/collision sets from one immutable +`SceneRegistry`. It filters sub-threshold pose noise, advances the general scene +version, and maintains per-environment collision revisions. Thresholds are +measured from the last materially published pose per entity and environment, so +repeated sub-threshold motion eventually becomes observable. +`RigidObjectSceneProvider` retains that lower-level revision behavior for +advanced direct-core integrations. For lightweight sources that do not need environment correlation IDs, `SimulationExecutionAdapter` also accepts a mutually exclusive `SceneSnapshotSupplier(timestamp)` callback. @@ -271,6 +331,14 @@ parameters to each skill. Add/remove/geometry mutations are not yet supported by this pose-update path; providers should revision only pose-updatable registered obstacles. +`BasePlanner.collision_world_entity_ids`, `dynamic_collision_entity_ids`, and +`collision_world_batch_mode` expose the backend's complete world, dynamic +subset, and batching contract. `MotionGenerator` validates and forwards those +properties for `SceneRegistry.make_planning_scene_provider()`. External +providers call `validate_collision_integration(..., scene_provider=...)`. +These construction checks are separate from per-plan +`bind_collision_world()`. + Runnable closed-loop examples live under `scripts/tutorials/atomic_action/`: `tracking_error_recovery.py`, `moving_target_recovery.py`, and `dynamic_obstacle_recovery.py`. Each injects one disturbance, reports the diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 6b10e5524..b5edd4a8b 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -97,6 +97,19 @@ Learning-based EEF waypoint planner. Franka Panda only. ### CuroboPlanner collision worlds +`CuroboWorldCfg.rigid_objects` accepts either a mapping or a sequence. Use +`Mapping[registry_id, RigidObject]` for a registry-backed integration. The +mapping key is the authoritative logical/source obstacle ID used by the +content-cache key, `collision_world_entity_ids`, and registry validation. For +`cuboid` and `mesh`, it is also the physical YAML obstacle name and dynamic +update key. For `sphere`, one static source expands to physical YAML names such +as `registry_id_0`; dynamic sphere configuration is rejected, while cache and +full-world identity remain keyed by `registry_id`. A registry mapping whose +source lacks mesh geometry required by the selected representation fails fast +instead of silently dropping the source. The sequence form is an advanced +direct-core path that derives names from each object's `uid` or an +`obstacle_` fallback. + `CuroboWorldCfg.multi_env` controls collision-world batching, not whether robot states or goals are batched: @@ -126,10 +139,33 @@ them into `CuroboPlanOptions.dynamic_obstacle_poses`. to the backend hook. Atomic actions use that facade from their framework-owned `plan()` template when a `SceneSnapshot` declares collision entities; individual skills must not construct backend obstacle options themselves. +`BasePlanner.collision_world_entity_ids`, `dynamic_collision_entity_ids`, and +`collision_world_batch_mode` expose the complete canonical world, its dynamic +subset, and the `"shared"` / `"per_env"` mode. `MotionGenerator` validates and +forwards those properties to the integration layer. For cuRobo, the complete +set is every mapping key (or inferred sequence name), while the dynamic set is +exactly `CuroboWorldCfg.dynamic_obstacle_names`. Sphere-expanded physical YAML +names are not part of either logical ID declaration. `CuroboWorldCfg` rejects duplicate obstacle names and requires every `dynamic_obstacle_name` to match an object registered in `rigid_objects`, so a planner-local mismatch fails before backend construction. +For the canonical path, pass `SceneRegistry.collision_geometry_by_id()` into +`CuroboWorldCfg.rigid_objects`, derive dynamic names from the registry, and call +`SceneRegistry.make_planning_scene_provider(motion_generator, batch_size=...)` +before execution. The geometry mapping excludes `NONE` registrations. The +factory first requires the registry's complete `STATIC ∪ DYNAMIC` set to +equal `MotionGenerator.collision_world_entity_ids`, then requires exact +registry/derived-provider/planner dynamic-subset agreement. It also checks +update capability for a non-empty dynamic set and the same collision-world +batch mode. An external perception/hardware provider instead uses +`validate_collision_integration(..., scene_provider=provider)`. + +One environment may infer `SHARED`; a multi-environment registry with dynamic +entities must explicitly choose `SHARED` or `PER_ENV`. Alias normalization +happens before planner construction, so planner IDs must never be simulator +UIDs unless that string is also the chosen canonical registry ID. + `MotionGenerator.resolve_plan_options()` is the corresponding option-ownership boundary. It copies caller-supplied typed options, otherwise obtains backend defaults; for TOPPRA it maps the requested sample count and generic @@ -271,3 +307,8 @@ The decorator checks that every `PlanState` in `target_states` shares the same l - **Fork safety with GPU sim** — `ToppraPlannerCfg.mp_context=None` defaults to `spawn` on GPU to avoid fork-after-CUDA-init hazards. Force `fork` only when the sim device is CPU or you have verified it is safe. - **cuRobo shared-world mismatch** — World-frame poses may differ solely because replicated arenas are offset. Compare poses after robot-base rebasing: keep `multi_env=False` if they match, and enable it only when robot-relative layouts differ. - **Dynamic obstacles silently stale** — A planner participates in atomic-action collision revision recovery only when it declares `supports_collision_world_updates=True`; its hook must bind every `collision_entity_id` pose into the current planning attempt. +- **Registry/planner identity drift** — Registry-backed cuRobo worlds must use a + canonical-ID mapping, not a list whose names are inferred from UIDs. Validate + exact full registry/planner collision-world agreement, dynamic + registry/provider/planner agreement, and batch-mode agreement through + `SceneRegistry` before starting execution. diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 21510e7b4..1bee119b8 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,6 +1,7 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: design plan +- Status: implementation in progress; Phase 0 and PR1 complete, PR2A implemented + on the feature branch - Baseline: `main@e445133c79c8b32019dab1c844b799b43a1658d6` - Last updated: 2026-08-10 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), @@ -90,8 +91,11 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is updated against committed `main@e445133c` after PR #475 rather -than uncommitted working-tree changes. +This plan is updated against committed `main@e445133c` after PR #475. The +implementation series is stacked from that baseline: PR1 is complete on +`refactor/atomic-actions-phase0`, and PR2A is implemented by the +`feat/atomic-action-pr2a-scene-registry` change. Neither status statement +implies that the stacked changes have landed on `main`. | Capability | Current main | Design consequence | |---|---|---| @@ -114,7 +118,7 @@ hard break: the project will not provide a compatibility adapter or deprecation window for that former extension contract. Custom actions must migrate to `_plan()` so framework-owned scene binding cannot be bypassed. -The remaining #474 prerequisites on this baseline are: +The remaining #474 prerequisites on `main` are: - scene pose, semantics, affordance, and collision registration still have multiple sources of truth; @@ -126,6 +130,12 @@ The remaining #474 prerequisites on this baseline are: - `MotionPolicy` still exposes implementation-level tuning that should be hidden behind semantic presets for ordinary users. +PR1 closes the snapshot-grounding and stable-identity bridge. PR2A closes the +first and third gaps for registry-backed integrations by introducing one +authoritative registration boundary, a registry-derived scene provider, and +construction-time collision-world validation. The semantic facade and named +presets remain later-phase work. + One #474 finding has changed since its review branch: the ambiguous `collision_check` switch has been replaced by `DynamicCollisionMode.OFF`, `AUTO`, and `REQUIRED` on current main. The semantic preset layer should build @@ -287,26 +297,61 @@ Rules: registry ID; they never replace the authoritative ID. Duplicate registry IDs, ambiguous aliases, or an alias colliding with another registry ID fail during registry construction. -3. Grounding reads pose and geometry from one immutable snapshot. It must not - mix a snapshot with a live simulation entity pose. + For links and affordances, one typed `(parent, native_name)` physical source + may have only one canonical ID; changing the canonical spelling does not + create a second entity. +3. Grounding reads dynamic pose/confidence from one immutable snapshot and + static geometry/affordance metadata from the immutable registry that + produced it. It must not mix a snapshot with a live simulation entity pose. 4. Automatic grasp selection declares a target dependency automatically. -5. Dynamic collision setup is derived from authoritative registry IDs. Registry - construction performs the complete provider/planner cross-validation: the - registry's dynamic-collision ID set, the provider's `collision_entity_ids`, - and the planner's dynamic-obstacle names must agree after alias normalization; - every ID must have the required geometry, and the selected planner must - support the declared update mode. The current +5. Collision setup is derived from authoritative registry IDs. + `collision_geometry_by_id()` derives planner geometry while excluding + non-collision registrations, and `make_planning_scene_provider()` performs + the complete provider/planner cross-validation. The registry's full + `STATIC ∪ DYNAMIC` collision ID set must exactly equal the planner's complete + collision-world ID set. Within it, the registry's dynamic subset, the + provider's `collision_entity_ids`, and the planner's dynamic-obstacle IDs must + also agree exactly. Every collision ID must have the required geometry, and + the selected planner must support the declared dynamic update mode. Aliases + are resolved before these contracts are constructed, never inside the + planner. The current planner-local name check remains a lower-level defensive validation, not the integration contract. 6. The `safe` preset requests `DynamicCollisionMode.REQUIRED` when the registry declares dynamic collision entities and fails early if the active planner cannot satisfy it. -7. Environment scene configuration should populate the registry automatically; - explicit providers are reserved for perception and hardware integration. - -Before PR2A introduces this registry, PR1 provides only a core migration bridge. -`ObjectSemantics.entity_id` is a caller-supplied `SceneSnapshot` key, not yet a -registry reference. `ObjectSemantics` is shallow-frozen so top-level fields, +7. Environment scene configuration opts into registry population explicitly; + it is not inferred by scanning the simulation. Explicit providers remain + available for perception and hardware integration. + +PR2A fixes three public identity and collision-world choices: + +- **Authoritative planner IDs:** registry-backed cuRobo configuration passes an + explicit `registry_id -> RigidObject` mapping derived by + `collision_geometry_by_id()`. Mapping keys are canonical logical/source IDs + for cache identity and the complete registry/planner collision-world + contract. Cuboid and mesh worlds also use them unchanged as physical YAML + obstacle names and runtime pose-update keys. A static sphere source expands + to derived physical names such as `registry_id_0`; dynamic sphere worlds are + rejected. A registry mapping with geometry missing for the selected + representation fails fast instead of silently omitting that source. The list + form remains an advanced direct-core path and continues to derive names from + simulator UIDs or fallback names. +- **Flat reference IDs:** object, articulation, link, and affordance IDs share + one globally unique flat namespace. Link and affordance ancestry is stored in + `SceneEntityRegistration.parent`; callers do not encode hierarchy into an ID. + Within one reference type, the same `(parent, native_name)` cannot be assigned + more than one canonical ID. +- **Explicit vectorized-world semantics:** one-environment dynamic collision + setup may infer a shared collision world. A multi-environment registry with + dynamic collision entities must explicitly select shared or per-environment + collision worlds, and integration validation requires the planner mode to + match that selection. + +PR1 provides the core migration bridge consumed by the registry. +`ObjectSemantics.entity_id` remains a string lowering target in the typed core; +the canonical semantic path obtains that value from a resolved +`SceneEntityRef`. `ObjectSemantics` is shallow-frozen so top-level fields, including `entity_id`, cannot be rebound after attachment state captures the semantics; identity changes require a new instance. Nested affordance and metadata objects remain mutable but never establish identity. @@ -329,11 +374,14 @@ The same boundary applies to `AssembleGoal.base_pose`: the snapshot reference is canonical, while an omitted reference permits the deprecated direct-core `AssembleAffordance.base_object_entity` path. -The current `SceneSnapshot` owns copies of input pose tensors, but exposed -`EntityState.pose` tensors are not deeply read-only. PR1 therefore requires -callers to treat snapshot values as immutable and uses the scene version for -publication/recovery semantics. Enforced deep immutability belongs to the PR2A -registry/snapshot hardening rather than this bridge. +PR2A hardens `SceneSnapshot` at the public boundary. Construction owns a copy of +every dynamic `EntityState`, and every public entity lookup returns a defensive +copy, so mutating an input state or a previously returned pose cannot mutate the +published snapshot. The registry continues to own static integration metadata, +including typed identity, aliases, parent relationships, geometry, collision +role, dynamics classification, semantic type, and affordances. A snapshot owns +only versioned dynamic pose/confidence plus collision revision metadata; it does +not duplicate the registration catalog. ### 7.2 Robot skill profiles @@ -735,11 +783,12 @@ tests. The dependency order is: Phase 0 correctness (complete) | v -PR1 snapshot/identity bridge +PR1 snapshot/identity bridge (complete) | +-----------------------+ v v PR2A SceneRegistry PR2B RobotSkillProfile + (implemented) (next) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -778,11 +827,11 @@ Landed on `main` through #475: core check. Complete provider/planner cross-validation is deliberately owned by the authoritative `SceneRegistry` integration in Phase 1. -Exit criteria are met on `main@e445133c`. Implementation may proceed to the -focused PR1 bridge without adding a legacy `plan()` adapter or a pre-registry -duplicate of the integration-level obstacle validator. +Exit criteria are met on `main@e445133c`; this remains the foundation for the +completed PR1 bridge. That bridge adds neither a legacy `plan()` adapter nor a +pre-registry duplicate of the integration-level obstacle validator. -### PR1: core snapshot and identity bridge +### PR1: core snapshot and identity bridge (complete) PR1 is deliberately smaller than Phase 1. It establishes the core seams that the later registry and profile integrations consume: @@ -822,17 +871,18 @@ cross-source uniqueness or collision validation, a `RobotSkillProfile`, or semantic presets. It does not require official task environments to migrate; they remain on the compatibility path until a later opt-in vertical slice. -Exit criteria: canonical object grounding never mixes snapshot and live poses; -explicit missing IDs fail; dependency metadata matches the poses actually -consumed; stable-identity merges are deterministic; and existing direct-core -callers remain usable only through the documented deprecated fallbacks. +Exit criteria are met on `refactor/atomic-actions-phase0`: canonical object +grounding never mixes snapshot and live poses; explicit missing IDs fail; +dependency metadata matches the poses actually consumed; stable-identity merges +are deterministic; and existing direct-core callers remain usable only through +the documented deprecated fallbacks. ### Phase 1: unified integration data Phase 1 is implemented as two focused follow-up PRs that join before the semantic facade/compiler work. -#### PR2A: SceneRegistry +#### PR2A: SceneRegistry (implemented on the feature branch) Deliverables: @@ -842,16 +892,35 @@ Deliverables: - immutable snapshots as the only grounding pose authority for the canonical semantic/compiler path; - opt-in environment-to-registry population and collision/provider derivation; -- complete construction-time agreement checks across registry collision IDs, - provider collision IDs, planner dynamic-obstacle names, geometry, and planner - capability; +- complete construction-time agreement checks between registry and planner + full collision-world IDs, plus registry/provider/planner dynamic subsets, + geometry, mode, and planner capability; - explicit catalog-discovery versus engine-installation terminology. +The implemented scope also records the A+C+E decisions from the PR2A API +review: + +- registry-backed cuRobo worlds use a canonical-ID mapping, while the list form + remains an advanced direct-core escape hatch; +- all reference IDs are globally unique and flat, with link/affordance parent + relations retained only by their registrations; +- one-environment dynamic worlds may default to shared, while vectorized + dynamic worlds require an explicit shared/per-environment choice. + `ObjectSemantics.entity_id` and `AssembleGoal.base_pose` already provide the lowering targets from PR1. PR2A replaces manually coordinated IDs/providers with one authoritative registration and performs alias normalization exactly once at the integration boundary. +PR2A exit criteria are met by the feature change: registry-ID and alias +collisions fail at construction, typed lookups cannot silently change entity +kind, one typed parent/native physical source cannot be registered twice, +registry-derived snapshots contain only canonical keys, independent providers +keep independent revision state, full registry/planner collision-world IDs and +dynamic registry/provider/planner subsets are validated before execution, the +collision-world batch mode agrees, and cuRobo uses canonical mapping keys end +to end as logical source IDs (and as physical keys for cuboid/mesh worlds). + #### PR2B: RobotSkillProfile Deliverables: @@ -988,8 +1057,9 @@ independent of adoption of the new path. - strict decoder, unknown fields, schema versioning, bounded repeats, and registry reference errors; -- authoritative registry-ID normalization, legacy-`uid` alias collisions, and - complete registry/provider/planner obstacle-set agreement; +- authoritative registry-ID normalization, legacy-`uid` alias collisions, + typed parent/native-source collisions, complete registry/planner collision- + world agreement, and registry/provider/planner dynamic-subset agreement; - cumulative scene movement and collision dependency revision behavior; - profile capability matching, deterministic binding, and ambiguity errors; - `AssembleGoal.base_pose` snapshot resolution and its automatic scene diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.rst index 716df56ef..0f1390158 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.rst @@ -10,9 +10,9 @@ The ``sim`` package is EmbodiChain's simulation core. It is organized around the :class:`SimulationManager` (the DexSim scene handle), the scene-object hierarchy (lights, rigid/soft/cloth bodies, articulations, robots, gizmos, constraints), the sensor suite (cameras, stereo cameras, contact sensors), IK -solvers and motion planners, the atomic-action motion-primitive layer, a -reusable workspace-analysis and sampling toolkit, and the shared configuration -types and utilities that wire all of these together. +solvers and motion planners, the semantic scene registry, the atomic-action +motion-primitive layer, a reusable workspace-analysis and sampling toolkit, and +the shared configuration types and utilities that wire all of these together. .. rubric:: Submodules @@ -132,6 +132,14 @@ Planners embodichain.lab.sim.planners +Semantic Scene Integration +-------------------------- + +.. toctree:: + :maxdepth: 1 + + embodichain.lab.sim.skills + Atomic Actions -------------- diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst new file mode 100644 index 000000000..44ac3c38c --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -0,0 +1,72 @@ +embodichain.lab.sim.skills +========================== + +.. automodule:: embodichain.lab.sim.skills + + .. rubric:: Scene integration contracts + + .. autosummary:: + + SceneRegistry + RegistrySceneProvider + SceneEntityRegistration + SceneEntityRef + SceneObjectRef + SceneArticulationRef + SceneLinkRef + SceneAffordanceRef + SceneEntityStateProvider + SceneGeometryProvider + SceneDynamics + SceneCollisionRole + SceneCollisionWorldMode + +.. currentmodule:: embodichain.lab.sim.skills + +Registry and provider +--------------------- + +.. autoclass:: SceneRegistry + :members: + +.. autoclass:: RegistrySceneProvider + :members: + +Registration contracts +---------------------- + +.. autoclass:: SceneEntityRegistration + :members: + +.. autoclass:: SceneEntityStateProvider + :members: + +.. autoclass:: SceneGeometryProvider + :members: + +References and enums +-------------------- + +.. autoclass:: SceneEntityRef + :members: + +.. autoclass:: SceneObjectRef + :members: + +.. autoclass:: SceneArticulationRef + :members: + +.. autoclass:: SceneLinkRef + :members: + +.. autoclass:: SceneAffordanceRef + :members: + +.. autoclass:: SceneDynamics + :members: + +.. autoclass:: SceneCollisionRole + :members: + +.. autoclass:: SceneCollisionWorldMode + :members: diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index c4b2f7ab2..331cff6b4 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -33,8 +33,8 @@ and whole-body control are not implemented by this module yet. +---------------+----------------+ +---------------+----------------+ | | v | - agent adapter: schema validation, | - scene grounding, capability binding | + semantic adapter: schema validation, | + SceneRegistry grounding, capability binding | | | +------------------+------------------+ | @@ -75,11 +75,11 @@ The boundary is deliberate: |---|---|---| | Task intent and sequencing | Action Agent, task graph, or user-authored application | Selects skills, goals, and execution order | | Invocation construction | Agent adapter or user-authored code/config loader | Produces the same typed `ActionInvocation`; the engine has no agent-only interface | -| Perception and grounding | Agent adapter or user application | Builds scene snapshots and resource bindings, or supplies already-grounded values directly | +| Perception and grounding | `SceneRegistry` on the canonical path; adapter or user application on the advanced path | Normalizes aliases to canonical typed references and publishes snapshots, or supplies already-grounded values directly | | Deterministic motion planning | Atomic action module | Produces an `ActionPlan` from an invocation and context | | Motion-generation resources | `AtomicActionEngine` | Owns one robot, motion generator, planner backend, device, trajectory builder, and control-part command profiles | | Recovery state | `ExecutionSession` | Consumes fresh contexts, emits at most one `JointCommand` per tick, and owns bounded recovery/revision state | -| Scene observation | `SceneProvider` | Captures ordered entities plus monotonic global or per-environment collision-world revisions | +| Scene observation | Registry-derived `SceneProvider` | Captures canonical ordered entities plus monotonic global or per-environment collision-world revisions | | Scheduling and controller lifecycle | `ExecutionRunner` | Observes only when due, dispatches timed commands, records acknowledgements, and performs safe stop | | Robot/simulator I/O | `ObservationProvider`, `CommandSink`, and `ExecutionClock` adapters | Isolates observation, command transport, and time/physics advancement from planning and session state | | Physical-effect verification | Application observer | Verifies grasp, release, handover, and other symbolic effects | @@ -317,6 +317,10 @@ entity poses from the current `SceneSnapshot` into copied backend options, then calls the skill-specific `_plan()` hook. Individual skills therefore do not own dynamic-obstacle parameters or mutate caller-owned motion policies. +For a canonical integration, construct the snapshot provider and collision +world from one {doc}`SceneRegistry <../scene_registry>`. Direct use of +`RigidObjectSceneProvider` remains an advanced-core path. + Registration means that an implementation is installed, not that every robot can execute it. Required roles, control parts, profiles, and task-state preconditions are validated while an invocation is resolved and planned. Agent @@ -512,7 +516,8 @@ while session.status is ExecutionStatus.RUNNING: ``` For most applications, use `ExecutionRunner` to keep scheduling and controller -acknowledgement handling outside the session: +acknowledgement handling outside the session. The following snippet shows the +advanced direct-core provider path: ```python scene_provider = RigidObjectSceneProvider({"moving_tray": moving_tray}) @@ -568,17 +573,38 @@ per-environment action scheduling belongs in a higher-level scheduler rather tha this atomic-action session. `SceneProvider.snapshot(timestamp=..., env_ids=...)` is the scene-observation -boundary. `SceneSnapshot.collision_entity_ids` identifies obstacle poses -consumed by a planner, while `collision_world_revision` can be global or -per-environment. `RigidObjectSceneProvider` tracks live simulation objects, -filters sub-threshold pose noise, and advances those revisions. Its threshold -baseline is the last materially published pose for each entity/environment, so -cumulative sub-threshold motion cannot remain hidden indefinitely. Backends opt -in through `supports_collision_world_updates` and `with_collision_world()`; +boundary. On the canonical planning path, +`SceneRegistry.make_planning_scene_provider()` derives an independent provider +and eagerly validates its collision contract against the motion generator. +Its snapshots expose canonical registry IDs only. +The registry owns static identity, aliases, geometry, affordances, hierarchy, +and collision roles; `SceneSnapshot` owns versioned dynamic pose/confidence and +collision revisions. Snapshot states are defensively copied on construction and +public read. + +`SceneSnapshot.collision_entity_ids` identifies obstacle poses consumed by a +planner, while `collision_world_revision` can be global or per-environment. +Registry-derived providers filter sub-threshold pose noise and advance those +revisions from the last materially published pose, so cumulative motion cannot +remain hidden indefinitely. Backends opt in through +`supports_collision_world_updates` and `with_collision_world()`; `MotionGenerator.bind_collision_world()` owns that backend boundary, and cuRobo maps the snapshot poses to `CuroboPlanOptions.dynamic_obstacle_poses`. A newer revision invalidates only affected rows before synchronized cohort replanning. +`make_planning_scene_provider()` requires two exact canonical-ID agreements: +the registry's complete `STATIC ∪ DYNAMIC` set must equal the planner's +complete collision-world set, and the registry, derived provider, and planner +dynamic subsets must equal one another. It also requires planner update support +for a non-empty dynamic subset and matching shared or per-environment world +semantics. A one-environment registry may infer `SHARED`; a multi-environment +dynamic registry must choose `SHARED` or `PER_ENV` explicitly. External +perception/hardware providers use +`validate_collision_integration(..., scene_provider=...)` directly. Plain +`make_scene_provider()` and `RigidObjectSceneProvider` are perception or +advanced direct-core paths without eager planner agreement. See +{doc}`../scene_registry` for setup. + `MotionPolicy.dynamic_collision_mode` controls this live-scene path. `AUTO` (the default) consumes collision entities when the selected motion strategy and planner support them, `OFF` ignores snapshot collision entities and their @@ -713,6 +739,7 @@ See {doc}`builtin_actions` for the shipped skill catalog and visual demos, and ## Further reading +- {doc}`../scene_registry` — canonical scene identity, snapshots, and collision integration - {doc}`../planners/motion_generator` — the motion generator owned by the engine - {doc}`../sim_robot` — robot control parts and kinematic configuration - {doc}`/tutorial/atomic_actions` — static, closed-loop, and recovery examples diff --git a/docs/source/overview/sim/index.rst b/docs/source/overview/sim/index.rst index 168fad8db..e7aa753b5 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -43,6 +43,8 @@ The simulation stack can be read from the bottom up: |-- planners | |-- joint-space and Cartesian trajectory generation | `-- time parameterization and sampling utilities + |-- scene registry + | `-- canonical semantic identity, snapshots, and collision integration `-- atomic actions `-- reusable manipulation primitives built from assets, solvers, and planners @@ -84,6 +86,11 @@ Submodule Relationships timing, and feasibility handling. - Use robot state and solver results to produce trajectories that can be replayed in the manager loop. + * - Scene registry + - Owns canonical typed entity IDs, aliases, pose sources, geometry, + affordances, hierarchy, and collision roles. + - Publishes registry-derived snapshots for atomic actions and validates + dynamic collision-world agreement with planners. * - Atomic actions - Package complete manipulation primitives such as move, pick, and place. - Compose semantic targets, solvers, planners, and robot control into @@ -120,6 +127,8 @@ Choosing Where to Start kinematics. - Use :doc:`planners/index` when a target pose or joint goal must become a time-ordered trajectory. +- Use :doc:`scene_registry` when semantic calls, snapshots, and planner + obstacles must share one authoritative entity namespace. - Use :doc:`atomic actions ` when building scripted manipulation from reusable motion primitives. @@ -145,4 +154,5 @@ See Also viser_visualization.md solvers/index planners/index + scene_registry.md atomic_actions/index diff --git a/docs/source/overview/sim/planners/curobo_planner.md b/docs/source/overview/sim/planners/curobo_planner.md index 24f3193c1..72a512290 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -59,6 +59,9 @@ locks both fingers at `0.04`, so use the same simulated finger state or include the fingers in the planned control part. A mismatch means cuRobo validates a different collision geometry from the one replayed in DexSim. +Assuming the scene has been registered as shown in +{doc}`../scene_registry`, construct the planner world from that catalog: + ~~~python from embodichain.lab.sim.planners import ( CuroboPlannerCfg, @@ -66,13 +69,27 @@ from embodichain.lab.sim.planners import ( MotionGenCfg, MotionGenerator, ) +from embodichain.lab.sim.skills import SceneCollisionWorldMode + +collision_mode = registry.resolve_collision_world_mode( + batch_size=robot.num_instances, +) planner_cfg = CuroboPlannerCfg( robot_uid="my_franka", planner_type="curobo", - world=CuroboWorldCfg(rigid_objects=[demo_block]), + world=CuroboWorldCfg( + rigid_objects=registry.collision_geometry_by_id(), + obstacle_representation="cuboid", + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), + multi_env=collision_mode is SceneCollisionWorldMode.PER_ENV, + ), ) motion_generator = MotionGenerator(MotionGenCfg(planner_cfg=planner_cfg)) +scene_provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, +) ~~~ cuRobo's Python logger defaults to error-only output. Set @@ -135,26 +152,55 @@ second one-time warmup and its graph-resident memory, but still no subprocess or second CUDA context. The collision world is always auto-generated from live `RigidObject` meshes via -`CuroboWorldCfg.rigid_objects`: the adapter reads each object's mesh -(`get_vertices` / `get_triangles`) and world pose (`get_local_pose`) and writes a -cached cuRobo scene YAML on the first plan, using +`CuroboWorldCfg.rigid_objects`. The canonical, registry-backed form is a mapping +from authoritative registry ID to live object; the adapter reads each object's +mesh (`get_vertices` / `get_triangles`) and world pose (`get_local_pose`) and +writes a cached cuRobo scene YAML on the first plan, using `CuroboWorldCfg.obstacle_representation` (`"sphere"` by default for fast collision queries; use `"cuboid"` for a local-frame AABB placed as an OBB via the object pose, or `"mesh"` for the exact triangle mesh). Generated poses are authored in the cuRobo base/world frame, so this is exact -when the robot base sits at the simulator world origin. For obstacles that move -or live in an offset base frame, also declare their names in +when the robot base sits at the simulator world origin. The mapping key, rather +than `RigidObject.uid`, is the canonical logical/source ID used by cache +identity and collision-world validation. For `"cuboid"` and `"mesh"`, that ID +is also used unchanged as the physical YAML obstacle name and runtime update +key. For obstacles that move or live in an offset base frame, also declare their +canonical IDs in `CuroboWorldCfg.dynamic_obstacle_names` and update poses at plan time through `CuroboPlanOptions.dynamic_obstacle_poses` (provision `CuroboWorldCfg.collision_cache` before planning). Dynamic updates require the `"cuboid"` or `"mesh"` representation because sphere fitting expands one object -into multiple independently named obstacles. +into physical YAML obstacles named `_0`, `_1`, and +so on; dynamic sphere configuration is rejected. These derived names are +backend details. The cache and registry/planner full-world contract continue to +use the unexpanded canonical source ID. + +Registry-backed mappings fail fast if a selected source has no mesh geometry +required by the chosen representation. This prevents a canonical collision ID +from being silently skipped during YAML generation. The advanced sequence form +retains its lower-level behavior independently of this registry contract. + +`CuroboPlanner.collision_world_entity_ids` reports every configured logical +source ID: each mapping key on the registry path, or each inferred name on the +advanced sequence path. It deliberately does not expose sphere-expanded +physical YAML names. `dynamic_collision_entity_ids` reports exactly the +configured dynamic subset. Static entries therefore participate in +construction-time identity validation even though they do not receive per-plan +pose updates. `CuroboWorldCfg` validates this planner-local registration at construction: -obstacle names must be unique, and every dynamic obstacle name must match the -`uid` (or generated fallback name) of an entry in `rigid_objects`. The later -`SceneRegistry` integration additionally cross-validates those names with the -scene provider rather than duplicating them in task code. +obstacle IDs must be unique, and every dynamic obstacle ID must match an entry +in `rigid_objects`. A sequence of objects is retained only as an advanced +direct-core path; it derives names from each `uid` or an `obstacle_` +fallback. Do not use that form for a registry-backed world. + +The {doc}`../scene_registry` integration performs two higher-level checks before +execution. First, all registry `STATIC ∪ DYNAMIC` IDs must exactly equal +`MotionGenerator.collision_world_entity_ids`. Second, registry, derived scene +provider, and planner dynamic-ID subsets must exactly agree. The planner must +also support pose updates and its shared/per-environment batch mode must agree +with the registry. Aliases are normalized at the registry boundary; cuRobo +never translates a canonical ID back to a simulator UID. ### Shared and per-environment collision worlds @@ -188,21 +234,25 @@ pose differs by environment must also: 3. Have its current `(B, 4, 4)` simulator-world poses passed through `CuroboPlanOptions.dynamic_obstacle_poses` when planning. -For example: +For a registry-backed world, derive both the geometry mapping and dynamic ID +list from the same catalog: ```python world_cfg = CuroboWorldCfg( - rigid_objects=[block], + rigid_objects=registry.collision_geometry_by_id(), obstacle_representation="cuboid", - dynamic_obstacle_names=["block"], + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), multi_env=True, ) +current_snapshot = scene_provider.snapshot(timestamp=now, env_ids=env_ids) plan_options = CuroboPlanOptions( control_part="arm", - dynamic_obstacle_poses={ - "block": block.get_local_pose(to_matrix=True), # (B, 4, 4) - }, + dynamic_obstacle_poses=current_snapshot.collision_obstacle_poses( + batch_size=robot.num_instances, + device=robot.device, + dtype=robot.get_qpos().dtype, + ), ) ``` @@ -213,6 +263,12 @@ does not insert new geometry at runtime. Independent worlds replicate scene data and collision caches across the batch, so retain the shared default when the rebased layouts are identical. +For a registry-backed integration, a single-environment dynamic world may infer +the registry's shared mode. A multi-environment registry with dynamic collision +entities must explicitly choose shared or per-environment semantics, then set +`multi_env=False` or `True` to match. The registry validator rejects a mismatch +before planning. + (curobo-auto-generated-robot-yaml)= ## Auto-generated robot YAML diff --git a/docs/source/overview/sim/scene_registry.md b/docs/source/overview/sim/scene_registry.md new file mode 100644 index 000000000..74f693c15 --- /dev/null +++ b/docs/source/overview/sim/scene_registry.md @@ -0,0 +1,244 @@ +(scene-registry)= + +# Scene registry + +```{currentmodule} embodichain.lab.sim.skills +``` + +`SceneRegistry` is the canonical integration boundary between semantic scene +identity, atomic-action snapshots, and planner collision worlds. Register an +entity once under an authoritative ID, resolve external names at that boundary, +then use only the canonical ID in semantic calls, snapshots, dependencies, and +dynamic-obstacle configuration. + +The registry is an immutable catalog. A {class}`RegistrySceneProvider` created +from it owns changing observation state, publication baselines, and revisions. +This separation lets multiple runtimes share one catalog without sharing their +revision counters. + +## What the registry owns + +Each {class}`SceneEntityRegistration` contains static integration metadata: + +- a typed canonical reference; +- aliases for simulator, perception, or hardware names; +- an explicit pose/confidence provider; +- optional parent and backend-local name; +- dynamics and planner collision role; +- optional geometry, semantic type, and affordance data. + +A `SceneSnapshot` contains only versioned dynamic pose/confidence values and +collision-world revisions. Snapshot construction copies every entity state, and +each public entity lookup returns a defensive copy. Mutating an original tensor +or a previously returned value therefore cannot change a published snapshot. + +References use one flat, globally unique namespace: + +```text +SceneEntityRef ++-- SceneObjectRef ++-- SceneArticulationRef ++-- SceneLinkRef +`-- SceneAffordanceRef +``` + +Do not encode hierarchy into link or affordance IDs. Store ancestry in +`SceneEntityRegistration.parent` and the backend-local member name in +`native_name`. A link parent must be an articulation; an affordance parent may +be an object, articulation, or link. The registry rejects duplicate canonical +IDs, ambiguous aliases, aliases that collide with another canonical ID, +unregistered parents, and typed-reference mismatches. Within one reference +type, a `(parent, native_name)` pair identifies one physical source and cannot +be registered under multiple canonical IDs. The same local name may still be +used under different parents or by different reference types. + +String lookups may use an alias and are normalized once: + +```python +cube = registry.resolve("sim_cube", expected_type=SceneObjectRef) +assert cube.entity_id == "cube" +``` + +An already typed reference is expected to contain a canonical ID. It cannot use +an alias or silently change entity kind. + +## Explicit simulation opt-in + +Use {meth}`SceneRegistry.from_simulation` to select simulator entities +explicitly. Mapping keys are authoritative registry IDs and values are existing +simulation UIDs. The UIDs are installed as legacy aliases; unlisted simulation +entities are not scanned or imported. + +```python +from embodichain.lab.sim.skills import SceneObjectRef, SceneRegistry + +registry = SceneRegistry.from_simulation( + sim, + rigid_objects={ + "cube": "sim_cube", + "tray": "task_tray_0", + }, + articulations={"drawer": "cabinet_articulation"}, +) + +cube = registry.resolve("cube", expected_type=SceneObjectRef) +assert registry.resolve("sim_cube", expected_type=SceneObjectRef) == cube +``` + +For perception or hardware, construct registrations with an implementation of +{class}`SceneEntityStateProvider` instead. Collision registrations also require +a {class}`SceneGeometryProvider`; the geometry belongs to the catalog even +though the snapshot contains only its current pose and confidence. + +## Publish canonical snapshots + +For an atomic-action planning runtime, create the provider through +{meth}`SceneRegistry.make_planning_scene_provider` and pass it to +`SimulationExecutionAdapter`: + +```python +provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, +) +adapter = SimulationExecutionAdapter( + sim, + robot, + scene_provider=provider, +) +``` + +This factory constructs a fresh provider and eagerly validates the complete +registry/provider/planner collision contract. Use +{meth}`SceneRegistry.make_scene_provider` only for perception and advanced +direct-core consumers that do not need planner agreement. Every factory call +returns an independent provider. Its snapshots contain canonical registry IDs +only; aliases never leak into `SceneSnapshot.entities` or +`collision_entity_ids`. + +The provider observes entities in the supplied `env_ids` order. Those IDs must +remain stable and ordered for the provider lifetime, and timestamps must be +monotonic. Translation and rotation thresholds are measured from the last +materially published pose per entity and environment, so repeated +sub-threshold motion eventually publishes a new scene version. Dynamic +collision entities additionally advance per-environment collision revisions. + +Parent-relative affordances are derived from the parent pose inside the same +observation. Their static relative transforms remain registry metadata. + +## Validate the complete collision world + +Collision setup has one canonical namespace. For a registry-backed +cuRobo world, derive both the explicit `registry_id -> RigidObject` mapping and +the dynamic-obstacle ID list from the registry: + +```python +from embodichain.lab.sim.planners import ( + CuroboPlannerCfg, + CuroboWorldCfg, + MotionGenCfg, + MotionGenerator, +) +from embodichain.lab.sim.skills import ( + SceneCollisionRole, + SceneCollisionWorldMode, + SceneRegistry, +) + +registry = SceneRegistry.from_simulation( + sim, + rigid_objects={"cube": "sim_cube"}, + collision_roles={"cube": SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.PER_ENV, +) + +world = CuroboWorldCfg( + rigid_objects=registry.collision_geometry_by_id(), + obstacle_representation="cuboid", + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), + multi_env=True, +) +motion_generator = MotionGenerator( + MotionGenCfg( + planner_cfg=CuroboPlannerCfg( + robot_uid=robot.uid, + planner_type="curobo", + world=world, + ) + ) +) + +provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, +) +``` + +`collision_geometry_by_id()` derives the cuRobo mapping from the catalog. By +default it includes only `STATIC` and `DYNAMIC` registrations and excludes +`NONE`; an optional exact role filter is available when a backend needs one +subset. `from_simulation()` automatically exposes a selected live rigid object +as its geometry source. Articulations and manually constructed collision +registrations still need an appropriate explicit geometry provider. + +The registry validator checks two nested identity contracts before execution: + +1. The registry's complete `STATIC ∪ DYNAMIC` collision ID set exactly equals + `MotionGenerator.collision_world_entity_ids`. This rejects a missing static + obstacle as well as planner geometry not owned by the registry. +2. The registry's `DYNAMIC` subset exactly equals both the provider's + `collision_entity_ids` and + `MotionGenerator.dynamic_collision_entity_ids`. +3. Every ID in that complete collision world has materialized registered + geometry. +4. The planner supports dynamic collision-world updates when that subset is + non-empty. +5. The planner's shared/per-environment mode equals the registry mode. + +Every collision registration has already proved that geometry exists. Planner +IDs are canonical logical/source IDs, not aliases. For cuRobo `cuboid` and +`mesh` worlds, each mapping key is also the physical YAML obstacle key and the +runtime pose-update key. A `sphere` world instead expands one canonical source +ID into physical YAML names such as `cube_0`, `cube_1`, and so on. Those derived +names are backend details: cache identity and the full-world contract remain +keyed by the canonical source ID, and dynamic sphere obstacles are rejected. + +A registry-backed mapping also fails fast when a selected collision source has +no mesh geometry required by its representation. It never silently omits that +canonical ID from generated planner geometry. + +When an external perception or hardware provider supplies snapshots, validate +that provider's dynamic subset explicitly instead of constructing a +registry-derived one. The complete registry/planner world check still applies: + +```python +registry.validate_collision_integration( + motion_generator, + batch_size=batch_size, + scene_provider=external_scene_provider, +) +``` + +{class}`SceneCollisionWorldMode` follows this rule: + +| Batch and collision setup | Required registry choice | cuRobo setting | +|---|---|---| +| No dynamic collision entities | No mode required | Planner-specific | +| One environment | Omitted mode resolves to `SHARED`; explicit mode also allowed | Match the effective mode | +| Multiple environments | Explicit `SHARED` or `PER_ENV` is required | `multi_env=False` or `True`, respectively | + +Choose `SHARED` only when obstacle poses are equal after rebasing every +environment into its robot-base frame. Choose `PER_ENV` for independently +randomized robot-relative layouts. + +## Advanced direct-core paths + +`RigidObjectSceneProvider` and a list-valued `CuroboWorldCfg.rigid_objects` +remain available to advanced callers that intentionally assemble the atomic +core by hand. The list form derives obstacle names from each object's `uid` (or +an `obstacle_` fallback). It is not the registry-backed path and does not +provide alias normalization or registry/provider/planner construction checks. + +See {doc}`atomic_actions/index` for snapshot grounding and recovery semantics, +and {doc}`planners/curobo_planner` for cuRobo world representation and frame +details. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 1bba99004..46f81a635 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -11,7 +11,8 @@ combines that snapshot with the latest For the complete architecture and ownership model, see :doc:`/overview/sim/atomic_actions/index`. For the capability matrix and visual demonstrations of every built-in skill, see -:doc:`/overview/sim/atomic_actions/builtin_actions`. +:doc:`/overview/sim/atomic_actions/builtin_actions`. Canonical scene identity and +snapshot/provider setup are documented in :doc:`/overview/sim/scene_registry`. The contracts deliberately separate six concerns: @@ -258,7 +259,6 @@ must be resolved from the latest scene snapshot: from embodichain.lab.sim.atomic_actions import ( EndEffectorPoseGoal, RecoveryPolicy, - RigidObjectSceneProvider, SceneEntityPose, ) @@ -280,8 +280,16 @@ must be resolved from the latest scene snapshot: SimulationExecutionAdapter, TaskState, ) + from embodichain.lab.sim.skills import SceneRegistry - scene_provider = RigidObjectSceneProvider({"moving_tray": moving_tray}) + registry = SceneRegistry.from_simulation( + sim, + rigid_objects={"moving_tray": moving_tray.uid}, + ) + scene_provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, + ) adapter = SimulationExecutionAdapter( sim, robot, @@ -324,13 +332,24 @@ for comparison: python scripts/tutorials/atomic_action/moving_target_recovery.py --headless --auto_play --device cpu -For collision-aware execution, list pose-updatable obstacles in -``RigidObjectSceneProvider.collision_entity_ids`` and configure matching -dynamic obstacle names on a supporting planner such as cuRobo. The provider -advances per-environment collision-world revisions when an obstacle moves; -the session invalidates affected rows and the framework binds the latest poses -before replanning. Pose thresholds use the last materially published pose as -their baseline, so cumulative sub-threshold motion is eventually reported: +For collision-aware execution, register each pose-updatable obstacle with +``SceneCollisionRole.DYNAMIC`` and configure the same canonical registry IDs as +the planner's dynamic obstacle names. Derive the cuRobo object mapping with +``registry.collision_geometry_by_id()`` and construct the runtime provider with +``registry.make_planning_scene_provider(motion_generator, batch_size=...)``. +That one factory call checks that the registry's complete ``STATIC ∪ +DYNAMIC`` set exactly matches the planner's complete collision world, then +checks that the registry, provider, and planner dynamic subsets exactly match. +It also checks planner capability and shared/per-environment world mode. One +environment may infer a shared world; a multi-environment dynamic registry must choose +``SceneCollisionWorldMode.SHARED`` or ``PER_ENV`` explicitly. See +:doc:`/overview/sim/scene_registry` for the complete cuRobo mapping example. + +The provider advances per-environment collision-world revisions when an +obstacle moves; the session invalidates affected rows and the framework binds +the latest poses before replanning. Pose thresholds use the last materially +published pose as their baseline, so cumulative sub-threshold motion is +eventually reported: .. code-block:: bash