From baaa55bd917deaa0642999ef1a1d03bf921dbaa0 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 02:24:26 +0800 Subject: [PATCH] feat(sim): add declarative scene affordances --- embodichain/lab/sim/skills/__init__.py | 12 + embodichain/lab/sim/skills/scene.py | 489 ++++++++++++++++++++++++- tests/sim/skills/test_scene.py | 127 ++++++- 3 files changed, 624 insertions(+), 4 deletions(-) diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 7a990fb28..c3019445b 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -38,6 +38,10 @@ UnsupportedSkillError, ) from .scene import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, RegistrySceneProvider, SceneAffordanceRef, SceneArticulationRef, @@ -45,20 +49,26 @@ SceneCollisionWorldMode, SceneDynamics, SceneEntityRef, + SceneEntityMetadata, SceneEntityRegistration, SceneEntityStateProvider, SceneGeometryProvider, SceneLinkRef, SceneObjectRef, SceneRegistry, + UnsupportedSceneAffordanceError, ) __all__ = [ + "AmbiguousSceneAffordanceError", "AmbiguousSkillBindingError", "BoundRobotSkillProfile", "ControlPartEndpoint", "ControlPartEndpointAdapter", "EndpointResolution", + "GRASP_AFFORDANCE_CAPABILITY", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", "ProfileValidationError", "RegistrySceneProvider", "ResolvedRobotResource", @@ -76,6 +86,7 @@ "SceneCollisionWorldMode", "SceneDynamics", "SceneEntityRef", + "SceneEntityMetadata", "SceneEntityRegistration", "SceneEntityStateProvider", "SceneGeometryProvider", @@ -84,4 +95,5 @@ "SceneRegistry", "SkillPolicyPreset", "UnsupportedSkillError", + "UnsupportedSceneAffordanceError", ] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py index 62d71ec61..00fa219b0 100644 --- a/embodichain/lab/sim/skills/scene.py +++ b/embodichain/lab/sim/skills/scene.py @@ -31,6 +31,7 @@ from embodichain.lab.sim.common import BatchEntity from embodichain.lab.sim.atomic_actions import ( Affordance, + AntipodalAffordance, EntityState, SceneProvider, SceneSnapshot, @@ -43,13 +44,66 @@ RefT = TypeVar("RefT", bound="SceneEntityRef") +GRASP_AFFORDANCE_CAPABILITY = "affordance.grasp" +"""Capability for an affordance usable by object pickup or handover.""" + +PLACE_ON_AFFORDANCE_CAPABILITY = "affordance.place.on" +"""Capability for an affordance that defines an ``on`` placement relation.""" + +PLACE_IN_AFFORDANCE_CAPABILITY = "affordance.place.in" +"""Capability for an affordance that defines an ``inside`` placement relation.""" + + +class UnsupportedSceneAffordanceError(ValueError): + """Raised when a parent has no affordance for a required capability.""" + + +class AmbiguousSceneAffordanceError(ValueError): + """Raised when compatible affordances lack one explicitly scoped default.""" + def _validate_identifier(value: str, name: str) -> None: """Validate an exact, non-empty identifier without normalizing it.""" - if not isinstance(value, str) or not value or value != value.strip(): + if type(value) is not str or not value or value != value.strip(): raise ValueError(f"{name} must be a non-empty string without outer whitespace.") +def _normalize_affordance_capabilities( + values: Iterable[str], +) -> frozenset[str]: + """Validate one open set of namespaced affordance capabilities.""" + if isinstance(values, (str, bytes)): + raise TypeError( + "affordance_capabilities must be an iterable of strings, not a string." + ) + try: + capabilities = frozenset(values) + except TypeError as exc: + raise TypeError( + "affordance_capabilities must be an iterable of strings." + ) from exc + for capability in capabilities: + _validate_identifier(capability, "affordance capability") + return capabilities + + +def _normalize_default_affordances( + values: Mapping[str, SceneAffordanceRef], +) -> Mapping[str, SceneAffordanceRef]: + """Validate and own a capability-scoped default-affordance mapping.""" + if not isinstance(values, Mapping): + raise TypeError("default_affordances must be a mapping.") + defaults: dict[str, SceneAffordanceRef] = {} + for capability, affordance_ref in values.items(): + _validate_identifier(capability, "default affordance capability") + if type(affordance_ref) is not SceneAffordanceRef: + raise TypeError( + "default_affordances values must be SceneAffordanceRef instances." + ) + defaults[capability] = affordance_ref + return MappingProxyType(defaults) + + @dataclass(frozen=True, slots=True) class SceneEntityRef: """Typed reference to one authoritative scene-registry entity. @@ -109,6 +163,211 @@ class SceneCollisionWorldMode(str, Enum): PER_ENV = "per_env" +@dataclass(frozen=True, slots=True) +class SceneEntityMetadata: + """Provider-free semantic metadata projected from one registration. + + Args: + ref: Canonical typed entity reference. + aliases: Boundary aliases, compared as an order-independent set. + parent: Canonical parent for links and affordances. + native_name: Backend-local child name. + dynamics: Physical mobility classification. + collision_role: Planner collision classification. + semantic_type: Optional application semantic type. + affordance_capabilities: Open capabilities of an affordance. + default_affordances: Capability-scoped direct-child defaults. + affordance_payload_type: Exact registered affordance value type. + affordance_revision: Integrator-owned payload revision or fingerprint. + relative_pose: Flattened parent-relative 4x4 pose, when declared. + """ + + ref: SceneEntityRef + aliases: tuple[str, ...] = () + parent: SceneEntityRef | None = None + native_name: str | None = None + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + semantic_type: str | None = None + affordance_capabilities: frozenset[str] = frozenset() + default_affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) + affordance_payload_type: type[Affordance] | None = None + affordance_revision: str | None = None + relative_pose: tuple[float, ...] | None = None + + def __post_init__(self) -> None: + allowed_ref_types = { + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + } + if type(self.ref) not in allowed_ref_types: + raise TypeError("SceneEntityMetadata.ref must be a SceneEntityRef.") + if isinstance(self.aliases, (str, bytes)): + raise TypeError("SceneEntityMetadata.aliases must be an iterable.") + aliases = tuple(sorted(set(self.aliases))) + for alias in aliases: + _validate_identifier(alias, "scene alias") + object.__setattr__(self, "aliases", aliases) + if self.parent is not None and type(self.parent) not in allowed_ref_types: + raise TypeError("SceneEntityMetadata.parent must be a SceneEntityRef.") + if self.native_name is not None: + _validate_identifier(self.native_name, "native_name") + if not isinstance(self.dynamics, SceneDynamics): + raise TypeError("SceneEntityMetadata.dynamics must be SceneDynamics.") + if not isinstance(self.collision_role, SceneCollisionRole): + raise TypeError( + "SceneEntityMetadata.collision_role must be SceneCollisionRole." + ) + if self.semantic_type is not None: + _validate_identifier(self.semantic_type, "semantic_type") + object.__setattr__( + self, + "affordance_capabilities", + _normalize_affordance_capabilities(self.affordance_capabilities), + ) + object.__setattr__( + self, + "default_affordances", + _normalize_default_affordances(self.default_affordances), + ) + if self.affordance_payload_type is not None and ( + not isinstance(self.affordance_payload_type, type) + or not issubclass(self.affordance_payload_type, Affordance) + ): + raise TypeError( + "affordance_payload_type must be an Affordance subclass or None." + ) + if self.affordance_revision is not None: + _validate_identifier(self.affordance_revision, "affordance_revision") + if self.relative_pose is not None: + relative_pose = tuple(float(value) for value in self.relative_pose) + if len(relative_pose) != 16 or not all( + math.isfinite(value) for value in relative_pose + ): + raise ValueError( + "SceneEntityMetadata.relative_pose must contain 16 finite values." + ) + object.__setattr__(self, "relative_pose", relative_pose) + self._validate_topology() + + def _validate_topology(self) -> None: + """Apply the typed topology contract without requiring live providers.""" + if isinstance(self.ref, (SceneObjectRef, SceneArticulationRef)): + if self.parent is not None or self.native_name is not None: + raise ValueError( + "Object and articulation metadata cannot declare a parent " + "or native_name." + ) + if self.affordance_capabilities or self.affordance_payload_type is not None: + raise ValueError( + "Object and articulation metadata cannot declare affordance " + "payload capabilities." + ) + if self.affordance_revision is not None or self.relative_pose is not None: + raise ValueError( + "Object and articulation metadata cannot declare affordance " + "revision or relative_pose." + ) + return + if isinstance(self.ref, SceneLinkRef): + if not isinstance(self.parent, SceneArticulationRef) or ( + self.native_name is None + ): + raise ValueError( + "Link metadata requires an articulation parent and native_name." + ) + if self.affordance_capabilities or self.affordance_payload_type is not None: + raise ValueError( + "Link metadata cannot declare affordance payload capabilities." + ) + if self.affordance_revision is not None or self.relative_pose is not None: + raise ValueError( + "Link metadata cannot declare affordance revision or relative_pose." + ) + return + if isinstance(self.ref, SceneAffordanceRef): + if ( + not isinstance( + self.parent, + (SceneObjectRef, SceneArticulationRef, SceneLinkRef), + ) + or self.native_name is None + ): + raise ValueError( + "Affordance metadata requires an object, articulation, or link " + "parent and native_name." + ) + if self.affordance_payload_type is None: + raise ValueError( + "Affordance metadata requires affordance_payload_type." + ) + if self.default_affordances: + raise ValueError( + "Affordance metadata cannot declare default_affordances." + ) + if self.affordance_capabilities and self.affordance_revision is None: + raise ValueError( + "Capability-bearing affordance metadata requires an explicit " + "affordance_revision." + ) + if ( + GRASP_AFFORDANCE_CAPABILITY in self.affordance_capabilities + and not issubclass(self.affordance_payload_type, AntipodalAffordance) + ): + raise TypeError( + f"{GRASP_AFFORDANCE_CAPABILITY!r} requires an " + "AntipodalAffordance payload." + ) + return + if self.parent is not None or self.native_name is not None: + raise ValueError("Generic scene metadata cannot declare a parent.") + if self.affordance_capabilities or self.affordance_payload_type is not None: + raise ValueError( + "Generic scene metadata cannot declare affordance capabilities." + ) + if self.default_affordances: + raise ValueError( + "Generic scene metadata cannot declare default_affordances." + ) + if self.affordance_revision is not None or self.relative_pose is not None: + raise ValueError( + "Generic scene metadata cannot declare affordance revision or pose." + ) + + @classmethod + def from_registration( + cls, + registration: SceneEntityRegistration, + ) -> SceneEntityMetadata: + """Project semantic metadata without copying a live payload/provider.""" + relative_pose = registration.relative_pose + return cls( + ref=registration.ref, + aliases=registration.aliases, + parent=registration.parent, + native_name=registration.native_name, + dynamics=registration.dynamics, + collision_role=registration.collision_role, + semantic_type=registration.semantic_type, + affordance_capabilities=registration.affordance_capabilities, + default_affordances=registration.default_affordances, + affordance_payload_type=( + None + if registration.affordance is None + else type(registration.affordance) + ), + affordance_revision=registration.affordance_revision, + relative_pose=( + None + if relative_pose is None + else tuple(relative_pose.detach().cpu().reshape(-1).tolist()) + ), + ) + + @runtime_checkable class SceneEntityStateProvider(Protocol): """Observe one registered entity for an ordered environment batch.""" @@ -161,6 +420,12 @@ class SceneEntityRegistration: collision_role: Static, dynamic, or no planner collision role. semantic_type: Optional application semantic type. affordance: Affordance value for an affordance registration. + affordance_capabilities: Open semantic operations supported by an + affordance registration. + default_affordances: Capability-to-child mapping owned by a parent + object, articulation, or link registration. + affordance_revision: Stable integrator-owned revision or fingerprint for + capability-bearing affordance payload data. relative_pose: Optional parent-relative affordance transform. """ @@ -194,11 +459,26 @@ class SceneEntityRegistration: affordance: Affordance | None = None """Affordance value owned by a :class:`SceneAffordanceRef` registration.""" + affordance_capabilities: frozenset[str] = frozenset() + """Open semantic capabilities declared by an affordance registration.""" + + default_affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) + """Capability-scoped child affordances selected when multiple are valid.""" + + affordance_revision: str | None = None + """Stable payload revision required by capability-bearing affordances.""" + relative_pose: torch.Tensor | None = None """Optional parent-relative pose when no explicit state provider exists.""" def __post_init__(self) -> None: - if not isinstance(self.ref, SceneEntityRef): + if type(self.ref) not in { + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + }: raise TypeError("ref must be a SceneEntityRef.") if self.state_provider is not None and not isinstance( self.state_provider, @@ -219,7 +499,13 @@ def __post_init__(self) -> None: raise ValueError("aliases must be unique.") object.__setattr__(self, "aliases", aliases) - if self.parent is not None and not isinstance(self.parent, SceneEntityRef): + if self.parent is not None and type(self.parent) not in { + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + }: raise TypeError("parent must be a SceneEntityRef or None.") if self.native_name is not None: _validate_identifier(self.native_name, "native_name") @@ -236,6 +522,18 @@ def __post_init__(self) -> None: _validate_identifier(self.semantic_type, "semantic_type") if self.affordance is not None and not isinstance(self.affordance, Affordance): raise TypeError("affordance must be an Affordance or None.") + object.__setattr__( + self, + "affordance_capabilities", + _normalize_affordance_capabilities(self.affordance_capabilities), + ) + object.__setattr__( + self, + "default_affordances", + _normalize_default_affordances(self.default_affordances), + ) + if self.affordance_revision is not None: + _validate_identifier(self.affordance_revision, "affordance_revision") if self.relative_pose is not None: if not isinstance(self.relative_pose, torch.Tensor): raise TypeError("relative_pose must be a torch.Tensor or None.") @@ -248,6 +546,7 @@ def __post_init__(self) -> None: ) self._validate_reference_contract() + SceneEntityMetadata.from_registration(self) if ( self.collision_role is not SceneCollisionRole.NONE and self.geometry_provider is None @@ -279,6 +578,11 @@ def _validate_reference_contract(self) -> None: raise ValueError( "Affordance values require a SceneAffordanceRef registration." ) + if self.affordance_capabilities: + raise ValueError( + "affordance_capabilities require a SceneAffordanceRef " + "registration." + ) return if isinstance(self.ref, SceneLinkRef): @@ -295,6 +599,11 @@ def _validate_reference_contract(self) -> None: raise ValueError( "Affordance values require a SceneAffordanceRef registration." ) + if self.affordance_capabilities: + raise ValueError( + "affordance_capabilities require a SceneAffordanceRef " + "registration." + ) return if isinstance(self.ref, SceneAffordanceRef): @@ -314,12 +623,25 @@ def _validate_reference_contract(self) -> None: raise ValueError( "Affordance registrations require state_provider or relative_pose." ) + if self.default_affordances: + raise ValueError( + "An affordance registration cannot declare default_affordances." + ) return if self.parent is not None or self.native_name is not None: raise ValueError("Generic entity registrations cannot declare a parent.") if self.state_provider is None: raise ValueError("Generic entity registrations require state_provider.") + if self.affordance_capabilities: + raise ValueError( + "affordance_capabilities require a SceneAffordanceRef registration." + ) + if self.default_affordances: + raise ValueError( + "Only object, articulation, or link registrations may declare " + "default_affordances." + ) def _copy_registration( @@ -398,6 +720,10 @@ class SceneRegistry: _collision_world_entity_ids: tuple[str, ...] = field(repr=False) _dynamic_collision_entity_ids: tuple[str, ...] = field(repr=False) _static_collision_entity_ids: tuple[str, ...] = field(repr=False) + _entity_metadata: tuple[SceneEntityMetadata, ...] = field(repr=False) + _affordances_by_parent_capability: Mapping[ + tuple[str, str], tuple[SceneAffordanceRef, ...] + ] = field(repr=False) collision_world_mode: SceneCollisionWorldMode | None def __init__( @@ -448,6 +774,10 @@ def __init__( aliases[alias] = canonical_id self._validate_relationships(owned, by_id) + affordances_by_parent_capability = self._index_affordances(owned, by_id) + entity_metadata = tuple( + SceneEntityMetadata.from_registration(item) for item in owned + ) object.__setattr__(self, "_registrations", owned) object.__setattr__( self, @@ -482,6 +812,12 @@ def __init__( if item.collision_role is SceneCollisionRole.STATIC ), ) + object.__setattr__( + self, + "_affordances_by_parent_capability", + MappingProxyType(affordances_by_parent_capability), + ) + object.__setattr__(self, "_entity_metadata", entity_metadata) object.__setattr__(self, "collision_world_mode", collision_world_mode) @staticmethod @@ -528,11 +864,65 @@ def _validate_relationships( ) native_members[member_key] = registration.ref.entity_id + for registration in registrations: + for capability, default_ref in registration.default_affordances.items(): + default_registration = by_id.get(default_ref.entity_id) + if default_registration is None: + raise ValueError( + f"Scene entity {registration.ref.entity_id!r} declares " + f"unknown default affordance {default_ref.entity_id!r} " + f"for capability {capability!r}." + ) + if not isinstance(default_registration.ref, SceneAffordanceRef): + raise TypeError( + f"Default affordance {default_ref.entity_id!r} is registered " + f"as {type(default_registration.ref).__name__}, not " + "SceneAffordanceRef." + ) + if default_registration.parent != registration.ref: + actual_parent = default_registration.parent + raise ValueError( + f"Default affordance {default_ref.entity_id!r} is not a " + f"direct child of {registration.ref.entity_id!r}; its parent " + f"is {None if actual_parent is None else actual_parent.entity_id!r}." + ) + if capability not in default_registration.affordance_capabilities: + raise ValueError( + f"Default affordance {default_ref.entity_id!r} does not " + f"declare capability {capability!r}." + ) + + @staticmethod + def _index_affordances( + registrations: tuple[SceneEntityRegistration, ...], + by_id: Mapping[str, SceneEntityRegistration], + ) -> dict[tuple[str, str], tuple[SceneAffordanceRef, ...]]: + """Build deterministic parent/capability reverse lookup entries.""" + del by_id + mutable: dict[tuple[str, str], list[SceneAffordanceRef]] = {} + for registration in registrations: + if not isinstance(registration.ref, SceneAffordanceRef): + continue + assert registration.parent is not None + for capability in registration.affordance_capabilities: + mutable.setdefault( + (registration.parent.entity_id, capability), [] + ).append(registration.ref) + return { + key: tuple(sorted(refs, key=lambda ref: ref.entity_id)) + for key, refs in mutable.items() + } + @property def registrations(self) -> tuple[SceneEntityRegistration, ...]: """Return structurally independent registration values.""" return tuple(_copy_registration(item) for item in self._registrations) + @property + def entity_metadata(self) -> tuple[SceneEntityMetadata, ...]: + """Return provider-free metadata without copying affordance payloads.""" + return self._entity_metadata + @property def entity_refs(self) -> tuple[SceneEntityRef, ...]: """Return canonical typed references in registration order.""" @@ -640,6 +1030,93 @@ def lookup( ref = self.resolve(identifier, expected_type=expected_type) return _copy_registration(self._registrations_by_id[ref.entity_id]) + def affordances( + self, + parent: str | SceneEntityRef, + *, + capability: str, + ) -> tuple[SceneAffordanceRef, ...]: + """Return compatible direct-child affordances without selecting one. + + Args: + parent: Canonical ID, alias, or typed parent reference. + capability: Required open affordance capability. + + Returns: + Compatible canonical references sorted by canonical ID. + """ + parent_ref = self.resolve(parent) + _validate_identifier(capability, "affordance capability") + return self._affordances_by_parent_capability.get( + (parent_ref.entity_id, capability), + (), + ) + + def resolve_affordance( + self, + parent: str | SceneEntityRef, + *, + capability: str, + explicit: str | SceneAffordanceRef | None = None, + ) -> SceneAffordanceRef: + """Select one compatible affordance with strict scoped-default rules. + + Args: + parent: Entity that directly owns the affordance. + capability: Required semantic affordance capability. + explicit: Optional explicit affordance ID or typed reference. + + Returns: + One canonical compatible affordance reference. + + Raises: + UnsupportedSceneAffordanceError: If no compatible affordance exists + or an explicit affordance has the wrong parent/capability. + AmbiguousSceneAffordanceError: If multiple candidates exist without + a scoped default. + """ + parent_ref = self.resolve(parent) + _validate_identifier(capability, "affordance capability") + candidates = self.affordances(parent_ref, capability=capability) + if explicit is not None: + try: + selected = self.resolve(explicit, expected_type=SceneAffordanceRef) + except (KeyError, TypeError, ValueError) as exc: + raise UnsupportedSceneAffordanceError( + f"Explicit affordance {explicit!r} is not a registered " + "SceneAffordanceRef." + ) from exc + registration = self._registrations_by_id[selected.entity_id] + if registration.parent != parent_ref: + raise UnsupportedSceneAffordanceError( + f"Affordance {selected.entity_id!r} is not a direct child of " + f"{parent_ref.entity_id!r}." + ) + if capability not in registration.affordance_capabilities: + raise UnsupportedSceneAffordanceError( + f"Affordance {selected.entity_id!r} does not support " + f"capability {capability!r}." + ) + return selected + if not candidates: + raise UnsupportedSceneAffordanceError( + f"Scene entity {parent_ref.entity_id!r} has no affordance for " + f"capability {capability!r}." + ) + if len(candidates) == 1: + return candidates[0] + parent_registration = self._registrations_by_id[parent_ref.entity_id] + default = parent_registration.default_affordances.get(capability) + if default is not None: + return self.resolve(default, expected_type=SceneAffordanceRef) + raise AmbiguousSceneAffordanceError( + f"Scene entity {parent_ref.entity_id!r} has multiple affordances for " + f"capability {capability!r}: " + f"{[candidate.entity_id for candidate in candidates]}. Configure " + "default_affordances for this parent and capability or select one " + "explicitly." + ) + def make_scene_provider( self, *, @@ -1326,6 +1803,10 @@ def _pose_change_mask( __all__ = [ + "AmbiguousSceneAffordanceError", + "GRASP_AFFORDANCE_CAPABILITY", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", "RegistrySceneProvider", "SceneAffordanceRef", "SceneArticulationRef", @@ -1333,10 +1814,12 @@ def _pose_change_mask( "SceneCollisionWorldMode", "SceneDynamics", "SceneEntityRef", + "SceneEntityMetadata", "SceneEntityRegistration", "SceneEntityStateProvider", "SceneGeometryProvider", "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "UnsupportedSceneAffordanceError", ] diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py index 89c7b34fc..935dda770 100644 --- a/tests/sim/skills/test_scene.py +++ b/tests/sim/skills/test_scene.py @@ -23,8 +23,15 @@ import pytest import torch -from embodichain.lab.sim.atomic_actions import Affordance, EntityState, SceneSnapshot +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AntipodalAffordance, + EntityState, + SceneSnapshot, +) from embodichain.lab.sim.skills import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneArticulationRef, SceneCollisionRole, @@ -33,6 +40,7 @@ SceneLinkRef, SceneObjectRef, SceneRegistry, + UnsupportedSceneAffordanceError, ) @@ -146,6 +154,17 @@ def get_articulation(self, uid: str) -> _SimulationEntity | None: return self.articulations.get(uid) +class _CopyTrackedAffordance(AntipodalAffordance): + """Count payload copies so metadata projection can prove it performs none.""" + + copies = 0 + + def __deepcopy__(self, memo: dict[int, object]) -> _CopyTrackedAffordance: + del memo + type(self).copies += 1 + return _CopyTrackedAffordance() + + @pytest.mark.parametrize("entity_id", ["", " cube", "cube "]) def test_scene_entity_ref_rejects_non_exact_identifier(entity_id: str) -> None: with pytest.raises(ValueError, match="entity_id"): @@ -228,6 +247,112 @@ def test_affordance_registration_rejects_two_pose_sources() -> None: ) +def test_grasp_capability_requires_typed_versioned_payload() -> None: + object_ref = SceneObjectRef("cube") + common = { + "ref": SceneAffordanceRef("cube_grasp"), + "parent": object_ref, + "native_name": "grasp", + "relative_pose": torch.eye(4), + "affordance_capabilities": frozenset({GRASP_AFFORDANCE_CAPABILITY}), + } + + with pytest.raises(TypeError, match="AntipodalAffordance"): + SceneEntityRegistration( + **common, + affordance=Affordance(), + affordance_revision="v1", + ) + with pytest.raises(ValueError, match="affordance_revision"): + SceneEntityRegistration( + **common, + affordance=AntipodalAffordance(), + ) + + +def test_registry_selects_only_explicit_scoped_affordance_default() -> None: + object_ref = SceneObjectRef("cube") + first = SceneAffordanceRef("first_grasp") + second = SceneAffordanceRef("second_grasp") + + def registrations(*, with_default: bool) -> tuple[SceneEntityRegistration, ...]: + return ( + SceneEntityRegistration( + ref=object_ref, + state_provider=_StateProvider(), + default_affordances=( + {GRASP_AFFORDANCE_CAPABILITY: second} if with_default else {} + ), + ), + *tuple( + SceneEntityRegistration( + ref=ref, + parent=object_ref, + native_name=ref.entity_id, + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="v1", + relative_pose=torch.eye(4), + ) + for ref in (first, second) + ), + ) + + ambiguous = SceneRegistry(registrations(with_default=False)) + with pytest.raises(AmbiguousSceneAffordanceError, match="multiple"): + ambiguous.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + with pytest.raises(UnsupportedSceneAffordanceError, match="no affordance"): + ambiguous.resolve_affordance( + object_ref, + capability="affordance.unknown", + ) + + registry = SceneRegistry(registrations(with_default=True)) + assert ( + registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + == second + ) + assert ( + registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + explicit=first, + ) + == first + ) + + +def test_registry_metadata_projection_does_not_copy_affordance_payload() -> None: + object_ref = SceneObjectRef("cube") + _CopyTrackedAffordance.copies = 0 + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=object_ref, state_provider=_StateProvider()), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube_grasp"), + parent=object_ref, + native_name="grasp", + affordance=_CopyTrackedAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="v1", + relative_pose=torch.eye(4), + ), + ) + ) + _CopyTrackedAffordance.copies = 0 + + metadata = registry.entity_metadata + + assert metadata[1].affordance_payload_type is _CopyTrackedAffordance + assert _CopyTrackedAffordance.copies == 0 + + def test_collision_registration_requires_geometry_provider() -> None: with pytest.raises(ValueError, match="geometry_provider"): SceneEntityRegistration(