From 6fd571401108841615972b5c57b830e733c0053f Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 23:54:06 +0800 Subject: [PATCH] feat(skills): add declarative placement relations --- .../lab/gym/envs/expert_program/__init__.py | 4 + .../lab/gym/envs/expert_program/catalog.py | 24 +- .../lab/gym/envs/expert_program/simulation.py | 290 +++++++++++++++--- embodichain/lab/sim/skills/__init__.py | 10 + embodichain/lab/sim/skills/compiler.py | 53 ++++ embodichain/lab/sim/skills/scene.py | 58 ++++ tests/gym/envs/expert_program/test_catalog.py | 58 ++++ .../envs/expert_program/test_simulation.py | 82 +++++ tests/sim/skills/test_compiler.py | 47 +++ 9 files changed, 589 insertions(+), 37 deletions(-) diff --git a/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py index 43eec1892..e15b40619 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -118,6 +118,7 @@ AntipodalGraspAffordanceBinding, ArticulationOperationAffordanceBinding, ArticulationOperationTargetBinding, + ContainerAffordanceBinding, ControlPartCommandPreset, ControlPartEndpointBinding, ControlPartResourceBinding, @@ -129,6 +130,7 @@ SimulationRobotResourceBinding, SimulationRobotSkillProfileBinding, SimulationSceneBinding, + SupportSurfaceAffordanceBinding, ) from .catalog import ( ExpertProgramIntegrationCatalog, @@ -180,6 +182,7 @@ "CompiledProgramValidator", "CompiledRepeatFrame", "CompiledTargetSelection", + "ContainerAffordanceBinding", "ControlCommandStateEvidenceTracker", "ControlPartCommandPreset", "ControlPartEndpointBinding", @@ -261,6 +264,7 @@ "SimulationSceneBinding", "SimulationSegmentPolicyPort", "StandardExtensionDeclarations", + "SupportSurfaceAffordanceBinding", "SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS", "TargetCfg", "TargetRefCfg", diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index b4d2f17ee..8b468b1bb 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -56,6 +56,7 @@ PLACE_ON_AFFORDANCE_CAPABILITY, POSE_RELATION_EFFECT_CHANNEL, BoundRobotSkillProfile, + ContainerRelationTargetGrounder, HandOverPoseProvider, OperateArticulation, Place, @@ -74,6 +75,7 @@ SemanticIntegrationManifest, SemanticValidationError, SkillPolicyPreset, + SupportSurfaceRelationTargetGrounder, builtin_semantic_call_catalog, ) from embodichain.lab.sim.skills.effects import ( @@ -301,6 +303,18 @@ def _snapshot_relation_grounders( return tuple(values) +def _builtin_relation_grounders( + scene_binding: SimulationSceneBinding, +) -> tuple[RelationTargetGrounder, ...]: + """Install standard grounders for declared production relation bindings.""" + values: list[RelationTargetGrounder] = [] + if scene_binding.support_surfaces: + values.append(SupportSurfaceRelationTargetGrounder()) + if scene_binding.containers: + values.append(ContainerRelationTargetGrounder()) + return tuple(values) + + def _snapshot_relation_grounder_keys( values: frozenset[tuple[str, type[Affordance], str]], ) -> frozenset[tuple[str, type[Affordance], str]]: @@ -1262,7 +1276,15 @@ def __post_init__(self) -> None: _validate_standard_call_catalog(self.call_catalog) settle_presets = _snapshot_settle_presets(self.settle_presets) object.__setattr__(self, "settle_presets", settle_presets) - relation_grounders = _snapshot_relation_grounders(self.relation_grounders) + configured_relation_grounders = _snapshot_relation_grounders( + self.relation_grounders + ) + relation_grounders = _snapshot_relation_grounders( + ( + *_builtin_relation_grounders(self.scene_binding), + *configured_relation_grounders, + ) + ) object.__setattr__(self, "relation_grounders", relation_grounders) relation_grounder_keys = frozenset( _relation_grounder_key(grounder) for grounder in relation_grounders diff --git a/embodichain/lab/gym/envs/expert_program/simulation.py b/embodichain/lab/gym/envs/expert_program/simulation.py index b08235c32..8f8fd5f1e 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation.py +++ b/embodichain/lab/gym/envs/expert_program/simulation.py @@ -53,17 +53,23 @@ from embodichain.lab.sim.skills.integration import SceneEntityManifest, SceneManifest from embodichain.lab.sim.skills.scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, GRASP_AFFORDANCE_CAPABILITY, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneArticulationRef, SceneCollisionRole, SceneCollisionWorldMode, SceneDynamics, + SceneEntityRef, SceneEntityRegistration, SceneGeometryProvider, SceneLinkRef, SceneObjectRef, SceneRegistry, + SupportSurfaceAffordance, ) from embodichain.toolkits.graspkit.pg_grasp import GraspGeneratorCfg from embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker import ( @@ -326,6 +332,86 @@ def __post_init__(self) -> None: ) +def _validate_placement_binding(value: object) -> None: + """Validate fields shared by built-in placement-frame declarations.""" + for field_name in ("entity_id", "parent_id", "native_name"): + _identifier(getattr(value, field_name), field_name=field_name) + object.__setattr__( + value, + "aliases", + _identifier_tuple(getattr(value, "aliases"), field_name="aliases"), + ) + object.__setattr__( + value, + "object_target_pose", + _pose_tuple( + getattr(value, "object_target_pose"), + field_name="object_target_pose", + ), + ) + minimum_confidence = _finite( + getattr(value, "minimum_confidence"), + field_name="minimum_confidence", + ) + if not 0.0 <= minimum_confidence <= 1.0: + raise ValueError("minimum_confidence must be in [0, 1].") + object.__setattr__(value, "minimum_confidence", minimum_confidence) + if type(getattr(value, "is_default")) is not bool: + raise TypeError("is_default must be a bool.") + + +@dataclass(frozen=True, slots=True) +class SupportSurfaceAffordanceBinding: + """Declare one exact object target frame on a support parent. + + Args: + entity_id: Canonical ID of the placement affordance. + parent_id: Canonical object, articulation, or link parent ID. + native_name: Stable native name of this target frame. + aliases: Optional non-authoritative lookup aliases. + object_target_pose: Desired object pose relative to the parent. + minimum_confidence: Minimum parent/affordance observation confidence. + is_default: Whether this is the parent's default ``Place(on=...)`` frame. + """ + + entity_id: str + parent_id: str + native_name: str + aliases: tuple[str, ...] = () + object_target_pose: tuple[float, ...] = _IDENTITY_POSE + minimum_confidence: float = 0.0 + is_default: bool = False + + def __post_init__(self) -> None: + _validate_placement_binding(self) + + +@dataclass(frozen=True, slots=True) +class ContainerAffordanceBinding: + """Declare one exact object target frame inside a container parent. + + Args: + entity_id: Canonical ID of the placement affordance. + parent_id: Canonical object, articulation, or link parent ID. + native_name: Stable native name of this target frame. + aliases: Optional non-authoritative lookup aliases. + object_target_pose: Desired object pose relative to the parent. + minimum_confidence: Minimum parent/affordance observation confidence. + is_default: Whether this is the parent's default ``Place(inside=...)`` frame. + """ + + entity_id: str + parent_id: str + native_name: str + aliases: tuple[str, ...] = () + object_target_pose: tuple[float, ...] = _IDENTITY_POSE + minimum_confidence: float = 0.0 + is_default: bool = False + + def __post_init__(self) -> None: + _validate_placement_binding(self) + + @dataclass(frozen=True, slots=True) class ArticulationOperationTargetBinding: """Declarative named target for one articulation operation.""" @@ -554,6 +640,48 @@ def _antipodal_affordance( ) +def _placement_parent_ref( + parent_id: str, + *, + objects: Mapping[str, SimulationRigidObjectBinding], + articulations: Mapping[str, SimulationArticulationBinding], + links: Mapping[str, SimulationArticulationLinkBinding], +) -> SceneEntityRef: + """Resolve an explicitly declared placement parent to its exact ref type.""" + if parent_id in objects: + return SceneObjectRef(parent_id) + if parent_id in articulations: + return SceneArticulationRef(parent_id) + if parent_id in links: + return SceneLinkRef(parent_id) + raise KeyError(f"Placement affordance references unbound parent {parent_id!r}.") + + +def _placement_defaults( + support_surfaces: tuple[SupportSurfaceAffordanceBinding, ...], + containers: tuple[ContainerAffordanceBinding, ...], +) -> Mapping[str, Mapping[str, SceneAffordanceRef]]: + """Collect explicitly selected capability-scoped placement defaults.""" + defaults: dict[str, dict[str, SceneAffordanceRef]] = {} + for capability, bindings in ( + (PLACE_ON_AFFORDANCE_CAPABILITY, support_surfaces), + (PLACE_IN_AFFORDANCE_CAPABILITY, containers), + ): + for binding in bindings: + if not binding.is_default: + continue + parent_defaults = defaults.setdefault(binding.parent_id, {}) + previous = parent_defaults.get(capability) + if previous is not None: + raise ValueError( + f"Placement parent {binding.parent_id!r} has multiple default " + f"affordances for capability {capability!r}: " + f"{previous.entity_id!r} and {binding.entity_id!r}." + ) + parent_defaults[capability] = SceneAffordanceRef(binding.entity_id) + return defaults + + @dataclass(frozen=True, slots=True) class SimulationSceneBinding: """Build one authoritative registry from explicit simulation bindings.""" @@ -564,6 +692,8 @@ class SimulationSceneBinding: links: tuple[SimulationArticulationLinkBinding, ...] = () antipodal_grasps: tuple[AntipodalGraspAffordanceBinding, ...] = () articulation_operations: tuple[ArticulationOperationAffordanceBinding, ...] = () + support_surfaces: tuple[SupportSurfaceAffordanceBinding, ...] = () + containers: tuple[ContainerAffordanceBinding, ...] = () collision_world_mode: SceneCollisionWorldMode | None = None def __post_init__(self) -> None: @@ -574,6 +704,8 @@ def __post_init__(self) -> None: "links": SimulationArticulationLinkBinding, "antipodal_grasps": AntipodalGraspAffordanceBinding, "articulation_operations": ArticulationOperationAffordanceBinding, + "support_surfaces": SupportSurfaceAffordanceBinding, + "containers": ContainerAffordanceBinding, } all_ids: list[str] = [] for field_name, expected_type in expected_types.items(): @@ -596,6 +728,7 @@ def __post_init__(self) -> None: raise TypeError( "collision_world_mode must be SceneCollisionWorldMode or None." ) + _placement_defaults(self.support_surfaces, self.containers) def declare(self) -> SceneManifest: """Project the complete provider-free scene declaration. @@ -607,6 +740,10 @@ def declare(self) -> SceneManifest: objects = {item.entity_id: item for item in self.rigid_objects} articulations = {item.entity_id: item for item in self.articulations} links = {item.entity_id: item for item in self.links} + placement_defaults = _placement_defaults( + self.support_surfaces, + self.containers, + ) entries: list[SceneEntityManifest] = [] for binding in self.rigid_objects: @@ -615,15 +752,15 @@ def declare(self) -> SceneManifest: if binding.simulation_uid == binding.entity_id else (binding.simulation_uid,) ) - defaults = ( - {} - if binding.default_grasp_affordance is None - else { - GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( - binding.default_grasp_affordance - ) - } - ) + defaults = dict(placement_defaults.get(binding.entity_id, {})) + if binding.default_grasp_affordance is not None: + defaults.update( + { + GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( + binding.default_grasp_affordance + ) + } + ) entries.append( SceneEntityManifest( ref=SceneObjectRef(binding.entity_id), @@ -641,15 +778,15 @@ def declare(self) -> SceneManifest: if binding.simulation_uid == binding.entity_id else (binding.simulation_uid,) ) - defaults = ( - {} - if binding.default_operation_affordance is None - else { - ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( - SceneAffordanceRef(binding.default_operation_affordance) - ) - } - ) + defaults = dict(placement_defaults.get(binding.entity_id, {})) + if binding.default_operation_affordance is not None: + defaults.update( + { + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( + SceneAffordanceRef(binding.default_operation_affordance) + ) + } + ) entries.append( SceneEntityManifest( ref=SceneArticulationRef(binding.entity_id), @@ -675,6 +812,10 @@ def declare(self) -> SceneManifest: native_name=binding.native_link_name, dynamics=binding.dynamics, semantic_type=binding.semantic_type, + default_affordances=placement_defaults.get( + binding.entity_id, + {}, + ), ) ) @@ -728,6 +869,38 @@ def declare(self) -> SceneManifest: ) ) + for capability, payload_type, bindings in ( + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + self.support_surfaces, + ), + ( + PLACE_IN_AFFORDANCE_CAPABILITY, + ContainerAffordance, + self.containers, + ), + ): + for binding in bindings: + parent = _placement_parent_ref( + binding.parent_id, + objects=objects, + articulations=articulations, + links=links, + ) + entries.append( + SceneEntityManifest( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=parent, + native_name=binding.native_name, + affordance_capabilities=frozenset({capability}), + affordance_payload_type=payload_type, + affordance_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + relative_pose=binding.object_target_pose, + ) + ) + return SceneManifest(entries) def build(self, simulation: SimulationManager) -> SceneRegistry: @@ -741,6 +914,10 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: """ objects = {item.entity_id: item for item in self.rigid_objects} articulations = {item.entity_id: item for item in self.articulations} + placement_defaults = _placement_defaults( + self.support_surfaces, + self.containers, + ) geometry = { item.entity_id: item.geometry_provider for item in (*self.rigid_objects, *self.articulations) @@ -768,26 +945,26 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: entity_id = registration.ref.entity_id if isinstance(registration.ref, SceneObjectRef): binding = objects[entity_id] - defaults = ( - {} - if binding.default_grasp_affordance is None - else { - GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( - binding.default_grasp_affordance - ) - } - ) + defaults = dict(placement_defaults.get(entity_id, {})) + if binding.default_grasp_affordance is not None: + defaults.update( + { + GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( + binding.default_grasp_affordance + ) + } + ) else: binding = articulations[entity_id] - defaults = ( - {} - if binding.default_operation_affordance is None - else { - ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( - SceneAffordanceRef(binding.default_operation_affordance) - ) - } - ) + defaults = dict(placement_defaults.get(entity_id, {})) + if binding.default_operation_affordance is not None: + defaults.update( + { + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( + SceneAffordanceRef(binding.default_operation_affordance) + ) + } + ) registrations.append( replace( registration, @@ -841,6 +1018,10 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: native_name=binding.native_link_name, dynamics=binding.dynamics, semantic_type=binding.semantic_type, + default_affordances=placement_defaults.get( + binding.entity_id, + {}, + ), ) ) @@ -939,6 +1120,41 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: ) ) + for capability, payload_type, bindings in ( + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + self.support_surfaces, + ), + ( + PLACE_IN_AFFORDANCE_CAPABILITY, + ContainerAffordance, + self.containers, + ), + ): + for binding in bindings: + parent = _placement_parent_ref( + binding.parent_id, + objects=objects, + articulations=articulations, + links=links, + ) + payload = payload_type( + minimum_confidence=binding.minimum_confidence, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=parent, + native_name=binding.native_name, + affordance=payload, + affordance_capabilities=frozenset({capability}), + affordance_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + relative_pose=_pose_tensor(binding.object_target_pose), + ) + ) + return SceneRegistry( registrations, collision_world_mode=self.collision_world_mode, @@ -1466,6 +1682,7 @@ def declare(self) -> RobotSkillProfile: "AntipodalGraspAffordanceBinding", "ArticulationOperationAffordanceBinding", "ArticulationOperationTargetBinding", + "ContainerAffordanceBinding", "ControlPartCommandPreset", "ControlPartEndpointBinding", "ControlPartResourceBinding", @@ -1477,4 +1694,5 @@ def declare(self) -> RobotSkillProfile: "SimulationRobotResourceBinding", "SimulationRobotSkillProfileBinding", "SimulationSceneBinding", + "SupportSurfaceAffordanceBinding", ] diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 94a315f19..2760ce102 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -34,6 +34,7 @@ ) from .compiler import ( AnalyzedSemanticCall, + ContainerRelationTargetGrounder, GroundedHeldObjectGuard, GroundedPhaseEffectGate, GroundedSemanticCall, @@ -49,6 +50,7 @@ SemanticRelationTarget, SemanticSkillCompiler, SemanticWorkflow, + SupportSurfaceRelationTargetGrounder, ) from .effects import ( ArticulationJointStateExpectation, @@ -175,6 +177,8 @@ ) from .scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, + PLACEMENT_TARGET_AFFORDANCE_REVISION, SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, AmbiguousSceneAffordanceError, @@ -197,6 +201,7 @@ SceneLinkRef, SceneObjectRef, SceneRegistry, + SupportSurfaceAffordance, UnsupportedSceneAffordanceError, ) from .runtime import ( @@ -242,6 +247,8 @@ "ControlPartEvidenceAddress", "ControlPartRobotEvidenceSource", "ControlPartSimulationEvidenceProvider", + "ContainerAffordance", + "ContainerRelationTargetGrounder", "CoordinatedHeldObjectCleanupExpectation", "COMPOSITE_EFFECT_MONITOR_ID", "COMPOSITE_EFFECT_MONITOR_REVISION", @@ -293,6 +300,7 @@ "POSE_RELATION_EFFECT_CHANNEL", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", + "PLACEMENT_TARGET_AFFORDANCE_REVISION", "PathPart", "OperateArticulation", "ParallelBarrierUpdate", @@ -367,6 +375,8 @@ "SemanticEffectSpec", "SymbolicStateDomain", "SymbolicStateKey", + "SupportSurfaceAffordance", + "SupportSurfaceRelationTargetGrounder", "SemanticHandOverTarget", "SemanticIntegrationManifest", "SemanticLowering", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 904de30b5..c817de15c 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -94,6 +94,8 @@ ) from .scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, + PLACEMENT_TARGET_AFFORDANCE_REVISION, SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, ArticulationJointEvidenceAddress, @@ -101,6 +103,7 @@ PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneObjectRef, + SupportSurfaceAffordance, ) OptionT = TypeVar("OptionT", bound=ActionOptions) @@ -180,6 +183,54 @@ def ground( """ +class SupportSurfaceRelationTargetGrounder(RelationTargetGrounder): + """Ground a declared support target frame to a late-bound object pose.""" + + capability: ClassVar[str] = PLACE_ON_AFFORDANCE_CAPABILITY + affordance_type: ClassVar[type[Affordance]] = SupportSurfaceAffordance + affordance_revision: ClassVar[str] = PLACEMENT_TARGET_AFFORDANCE_REVISION + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> SceneEntityPose: + """Return the current support-relative target frame.""" + del context + if type(affordance) is not SupportSurfaceAffordance: + raise TypeError("affordance must be exactly SupportSurfaceAffordance.") + return SceneEntityPose( + relation.affordance.entity_id, + minimum_confidence=affordance.minimum_confidence, + ) + + +class ContainerRelationTargetGrounder(RelationTargetGrounder): + """Ground a declared container target frame to a late-bound object pose.""" + + capability: ClassVar[str] = PLACE_IN_AFFORDANCE_CAPABILITY + affordance_type: ClassVar[type[Affordance]] = ContainerAffordance + affordance_revision: ClassVar[str] = PLACEMENT_TARGET_AFFORDANCE_REVISION + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> SceneEntityPose: + """Return the current container-relative target frame.""" + del context + if type(affordance) is not ContainerAffordance: + raise TypeError("affordance must be exactly ContainerAffordance.") + return SceneEntityPose( + relation.affordance.entity_id, + minimum_confidence=affordance.minimum_confidence, + ) + + @dataclass(frozen=True, slots=True) class SemanticObjectTarget: """One object-space look-ahead target. @@ -2656,6 +2707,7 @@ def _broadcast_joint_position( __all__ = [ "AnalyzedSemanticCall", + "ContainerRelationTargetGrounder", "GroundedHeldObjectGuard", "GroundedPhaseEffectGate", "GroundedSemanticCall", @@ -2672,4 +2724,5 @@ def _broadcast_joint_position( "SemanticRelationTarget", "SemanticSkillCompiler", "SemanticWorkflow", + "SupportSurfaceRelationTargetGrounder", ] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py index cac2f0181..888ca5352 100644 --- a/embodichain/lab/sim/skills/scene.py +++ b/embodichain/lab/sim/skills/scene.py @@ -60,6 +60,9 @@ PLACE_IN_AFFORDANCE_CAPABILITY = "affordance.place.in" """Capability for an affordance that defines an ``inside`` placement relation.""" +PLACEMENT_TARGET_AFFORDANCE_REVISION = "1" +"""Schema revision for built-in support/container object-target frames.""" + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID = "builtin.scene_articulation" """Stable route for explicitly injected articulation-joint observations.""" @@ -92,6 +95,58 @@ class AmbiguousSceneAffordanceError(ValueError): """Raised when compatible affordances lack one explicitly scoped default.""" +@dataclass +class SupportSurfaceAffordance(Affordance): + """Typed target frame for placing an object's origin on a support surface. + + The registered affordance pose is the desired object pose, expressed + relative to its parent scene entity. The optional confidence threshold is + enforced whenever that late-bound target pose is resolved. + + Args: + minimum_confidence: Minimum confidence accepted while resolving the + late-bound target pose. + """ + + minimum_confidence: float = 0.0 + + def __post_init__(self) -> None: + if isinstance(self.minimum_confidence, bool) or not isinstance( + self.minimum_confidence, + (int, float), + ): + raise TypeError("minimum_confidence must be a number.") + self.minimum_confidence = float(self.minimum_confidence) + if not 0.0 <= self.minimum_confidence <= 1.0: + raise ValueError("minimum_confidence must be in [0, 1].") + + +@dataclass +class ContainerAffordance(Affordance): + """Typed target frame for placing an object's origin inside a container. + + The registered affordance pose is the desired object pose, expressed + relative to its parent scene entity. The optional confidence threshold is + enforced whenever that late-bound target pose is resolved. + + Args: + minimum_confidence: Minimum confidence accepted while resolving the + late-bound target pose. + """ + + minimum_confidence: float = 0.0 + + def __post_init__(self) -> None: + if isinstance(self.minimum_confidence, bool) or not isinstance( + self.minimum_confidence, + (int, float), + ): + raise TypeError("minimum_confidence must be a number.") + self.minimum_confidence = float(self.minimum_confidence) + if not 0.0 <= self.minimum_confidence <= 1.0: + raise ValueError("minimum_confidence must be in [0, 1].") + + def _validate_identifier(value: str, name: str) -> None: """Validate an exact, non-empty identifier without normalizing it.""" if type(value) is not str or not value or value != value.strip(): @@ -2106,7 +2161,9 @@ def _pose_change_mask( "ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY", "ArticulationJointEvidenceAddress", "AmbiguousSceneAffordanceError", + "ContainerAffordance", "GRASP_AFFORDANCE_CAPABILITY", + "PLACEMENT_TARGET_AFFORDANCE_REVISION", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", "RegistrySceneProvider", @@ -2126,5 +2183,6 @@ def _pose_change_mask( "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "SupportSurfaceAffordance", "UnsupportedSceneAffordanceError", ] diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py index 3886aa8a3..9374d7932 100644 --- a/tests/gym/envs/expert_program/test_catalog.py +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -31,12 +31,15 @@ IntegrationFingerprintMismatch, SimulationArticulationLinkBinding, SimulationExpertProgramRegistration, + SimulationRigidObjectBinding, SimulationSceneBinding, + SupportSurfaceAffordanceBinding, decode_expert_program, ) from embodichain.lab.gym.utils.registration import EnvSpec from embodichain.lab.sim.atomic_actions import Affordance, PlanningContext from embodichain.lab.sim.skills import ( + PLACEMENT_TARGET_AFFORDANCE_REVISION, PLACE_ON_AFFORDANCE_CAPABILITY, BoundSemanticCall, ControlPartEndpoint, @@ -47,6 +50,7 @@ RelationTargetGrounder, SemanticCallCatalog, SceneAffordanceRef, + SceneDynamics, SceneEntityManifest, SceneManifest, SceneObjectRef, @@ -54,6 +58,8 @@ RegisteredSemanticCall, SemanticCallDescriptor, SkillPolicyPreset, + SupportSurfaceAffordance, + SupportSurfaceRelationTargetGrounder, WorkflowRecoveryPolicy, builtin_semantic_call_catalog, ) @@ -743,6 +749,58 @@ def test_catalog_accepts_linked_place_relation_with_exact_grounder_key() -> None assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "place" +def test_standard_support_binding_installs_grounder_without_task_code() -> None: + """A task relation declaration supplies its production grounder implicitly.""" + base = create_cube_scene_binding(grasp_samples=32) + scene = replace( + base, + registry_id="relation_scene", + rigid_objects=( + *base.rigid_objects, + SimulationRigidObjectBinding( + entity_id="support", + simulation_uid="support", + dynamics=SceneDynamics.STATIC, + semantic_type="support_surface", + ), + ), + support_surfaces=( + SupportSurfaceAffordanceBinding( + entity_id="support_top", + parent_id="support", + native_name="top_object_target", + is_default=True, + ), + ), + ) + registration = SimulationExpertProgramRegistration( + scene_binding=scene, + robot_profile_binding=create_cube_robot_profile_binding(), + ) + + assert len(registration.relation_grounders) == 1 + assert type(registration.relation_grounders[0]) is ( + SupportSurfaceRelationTargetGrounder + ) + assert registration.catalog.relation_grounder_keys == frozenset( + { + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + ) + } + ) + + program = decode_expert_program( + _place_relation_payload(), + validation_context=registration.catalog, + ) + compiled = registration.catalog.preflight(program) + + assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "place" + + @pytest.mark.parametrize( ("overrides", "path"), ( diff --git a/tests/gym/envs/expert_program/test_simulation.py b/tests/gym/envs/expert_program/test_simulation.py index b3a6fc664..2cb1d27c4 100644 --- a/tests/gym/envs/expert_program/test_simulation.py +++ b/tests/gym/envs/expert_program/test_simulation.py @@ -27,6 +27,7 @@ AntipodalGraspAffordanceBinding, ArticulationOperationAffordanceBinding, ArticulationOperationTargetBinding, + ContainerAffordanceBinding, ControlPartCommandPreset, ControlPartEndpointBinding, ControlPartResourceBinding, @@ -38,6 +39,7 @@ SimulationRobotResourceBinding, SimulationRobotSkillProfileBinding, SimulationSceneBinding, + SupportSurfaceAffordanceBinding, ) from embodichain.lab.sim.atomic_actions import ( AntipodalAffordance, @@ -48,13 +50,17 @@ ) from embodichain.lab.sim.skills import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneArticulationRef, SceneDynamics, SceneLinkRef, SceneObjectRef, SkillPolicyPreset, + SupportSurfaceAffordance, ) from embodichain.lab.sim.skills.profiles import ResourceEndpoint @@ -212,6 +218,60 @@ def _scene_binding() -> SimulationSceneBinding: }, ), ), + support_surfaces=( + SupportSurfaceAffordanceBinding( + entity_id="cube_support_target", + parent_id="cube", + native_name="support_target", + object_target_pose=( + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.25, + 0.0, + 0.0, + 0.0, + 1.0, + ), + minimum_confidence=0.7, + is_default=True, + ), + ), + containers=( + ContainerAffordanceBinding( + entity_id="drawer_inside_target", + parent_id="drawer_handle_link", + native_name="inside_target", + object_target_pose=( + 1.0, + 0.0, + 0.0, + 0.1, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + ), + minimum_confidence=0.8, + is_default=True, + ), + ), ) @@ -293,6 +353,28 @@ def test_scene_binding_builds_existing_registry_contracts() -> None: snapshot.entities["drawer_handle_operation"].pose, simulation.articulation.link_pose, ) + support_ref = registry.resolve_affordance( + "cube", + capability=PLACE_ON_AFFORDANCE_CAPABILITY, + ) + support = registry.lookup(support_ref).affordance + assert type(support) is SupportSurfaceAffordance + assert support.minimum_confidence == pytest.approx(0.7) + assert torch.equal( + snapshot.entities[support_ref.entity_id].pose[:, 2, 3], + torch.full((_BATCH_SIZE,), 0.25), + ) + container_ref = registry.resolve_affordance( + "drawer_handle_link", + capability=PLACE_IN_AFFORDANCE_CAPABILITY, + ) + container = registry.lookup(container_ref).affordance + assert type(container) is ContainerAffordance + assert container.minimum_confidence == pytest.approx(0.8) + assert torch.allclose( + snapshot.entities[container_ref.entity_id].pose[:, 0, 3], + torch.tensor((0.4, 0.5)), + ) def test_scene_binding_fails_closed_on_missing_native_entity() -> None: diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index 6a3dc5a28..ac0133ca0 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -67,6 +67,7 @@ builtin_semantic_call_catalog, ) from embodichain.lab.sim.skills.compiler import ( + ContainerRelationTargetGrounder, GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, @@ -78,6 +79,7 @@ SemanticRelationTarget, SemanticSkillCompiler, SemanticWorkflow, + SupportSurfaceRelationTargetGrounder, ) from embodichain.lab.sim.skills.effects import ( BinaryEffectClause, @@ -111,7 +113,10 @@ SkillPolicyPreset, ) from embodichain.lab.sim.skills.scene import ( + ContainerAffordance, GRASP_AFFORDANCE_CAPABILITY, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, SceneCollisionRole, @@ -119,6 +124,7 @@ SceneEntityRegistration, SceneObjectRef, SceneRegistry, + SupportSurfaceAffordance, ) _MOTION_CAPABILITIES = frozenset( @@ -606,6 +612,47 @@ def _held_context( ) +@pytest.mark.parametrize( + ("grounder", "capability", "affordance_type"), + ( + ( + SupportSurfaceRelationTargetGrounder(), + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + ), + ( + ContainerRelationTargetGrounder(), + PLACE_IN_AFFORDANCE_CAPABILITY, + ContainerAffordance, + ), + ), +) +def test_builtin_relation_grounders_preserve_late_pose_and_confidence( + grounder: RelationTargetGrounder, + capability: str, + affordance_type: type[Affordance], +) -> None: + """Production relation grounders keep target frames live and typed.""" + registry, _ = _scene_registry() + relation = SemanticRelationTarget( + capability=capability, + affordance=SceneAffordanceRef("declared_target"), + payload_type=affordance_type, + payload_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + ) + + target = grounder.ground( + relation, + affordance=affordance_type(minimum_confidence=0.65), + context=_context(registry), + ) + + assert type(target) is SceneEntityPose + assert target.entity_id == "declared_target" + assert target.relative_pose is None + assert target.minimum_confidence == pytest.approx(0.65) + + def test_curated_analysis_selects_exact_preset_monitor_without_creating_it() -> None: registry, providers = _scene_registry() factory = _CountingRelationMonitorFactory()