diff --git a/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py index ca245f98b..6a39b35e3 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -130,6 +130,11 @@ SimulationRobotSkillProfileBinding, SimulationSceneBinding, ) +from .catalog import ( + ExpertProgramIntegrationCatalog, + IntegrationFingerprintMismatch, + SimulationExpertProgramRegistration, +) from .simulation_environment import ( ControlCommandStateEvidenceTracker, MotionGeneratorFactory, @@ -139,7 +144,10 @@ SimulationPlanningObservationProvider, create_simulation_expert_program_adapter, ) -from .simulation_policies import SimulationSegmentPolicyPort +from .simulation_policies import ( + SimulationSegmentPolicyPort, + default_simulation_settle_presets, +) __all__ = [ "AcceptedRuntimeCommandObserver", @@ -183,6 +191,7 @@ "ExpertProgramEnvironmentFactory", "ExpertProgramEnvironmentMixin", "ExpertProgramIntegrationCfg", + "ExpertProgramIntegrationCatalog", "ExpertProgramRuntimeAssembly", "ExpertProgramSceneResolver", "ExpertProgramValidationContext", @@ -190,6 +199,7 @@ "HandOverCfg", "GymPlanningObservationProvider", "InvokeCfg", + "IntegrationFingerprintMismatch", "MAX_DECLARATIVE_DEPTH", "MAX_DECLARATIVE_NODES", "MAX_EXPANDED_CALLS", @@ -228,6 +238,7 @@ "SimulationArticulationLinkBinding", "SimulationExpertProgramEnvironment", "SimulationExpertProgramFactory", + "SimulationExpertProgramRegistration", "SimulationPlanningObservationProvider", "SimulationRigidObjectBinding", "SimulationResourceEndpointBinding", @@ -242,6 +253,7 @@ "ValidatorCfg", "WaitStablePostCfg", "create_simulation_expert_program_adapter", + "default_simulation_settle_presets", "decode_expert_program", "load_expert_program", "loads_expert_program_json", diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py new file mode 100644 index 000000000..fe5e5d9ca --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -0,0 +1,1056 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Immutable task-registration catalog for declarative Expert Programs.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, field, fields, is_dataclass, replace +from enum import Enum +import hashlib +import json +import math +from types import MappingProxyType +import torch + +from embodichain.lab.gym.envs.settling import DynamicSettleMonitorCfg +from embodichain.lab.sim.atomic_actions import ( + Affordance, + ArticulationOperationAffordance, + AtomicActionEngine, + SkillDescriptor, +) +from embodichain.lab.sim.atomic_actions.primitives import BUILTIN_ACTION_TYPES +from embodichain.lab.sim.skills import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + HandOverPoseProvider, + OperateArticulation, + Place, + RelationTargetGrounder, + RobotSkillProfile, + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneManifest, + SceneObjectRef, + SceneRegistry, + SemanticCallCatalog, + SemanticIntegrationManifest, + SemanticValidationError, + SkillPolicyPreset, + builtin_semantic_call_catalog, +) + +from .cfg import ( + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + OperateArticulationCfg, + PostPolicyCfg, + RegisteredSemanticCallCfg, + SemanticCallCfg, + ValidatorCfg, +) +from .compiler import ( + CompiledProgram, + ExpertProgramCompileError, + ExpertProgramCompiler, + ExpertProgramSceneResolver, +) +from .decoder import ( + ConfigPath, + ExpertProgramValidationError, + SceneReferenceRole, +) +from .simulation import SimulationRobotSkillProfileBinding, SimulationSceneBinding +from .simulation_policies import default_simulation_settle_presets + +_CATALOG_FINGERPRINT_SCHEMA_VERSION = 1 +_POST_POLICY_KINDS = frozenset({"wait_stable"}) +_VALIDATOR_KINDS = frozenset({"object_near_target"}) + + +class IntegrationFingerprintMismatch(RuntimeError): + """Raised when a live integration no longer matches its registration.""" + + +def _qualified_name(value: type[object] | object) -> str: + """Return a stable fully-qualified type name.""" + value_type = value if isinstance(value, type) else type(value) + return f"{value_type.__module__}.{value_type.__qualname__}" + + +def _canonical_value(value: object) -> object: + """Convert provider-free declarations to deterministic JSON values.""" + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError("Fingerprint metadata cannot contain non-finite floats.") + return value + if isinstance(value, Enum): + return { + "type": _qualified_name(value), + "value": _canonical_value(value.value), + } + if isinstance(value, type): + return {"type": _qualified_name(value)} + if isinstance(value, torch.Tensor): + tensor = value.detach().cpu() + return { + "tensor_dtype": str(tensor.dtype), + "tensor_shape": list(tensor.shape), + "tensor_value": tensor.tolist(), + } + if isinstance(value, Mapping): + normalized: dict[str, object] = {} + for key, nested in value.items(): + if type(key) is not str: + raise TypeError("Fingerprint mapping keys must be exact strings.") + normalized[key] = _canonical_value(nested) + return {key: normalized[key] for key in sorted(normalized)} + if isinstance(value, (tuple, list)): + return [_canonical_value(nested) for nested in value] + if isinstance(value, (set, frozenset)): + normalized = [_canonical_value(nested) for nested in value] + return sorted( + normalized, + key=lambda item: json.dumps( + item, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=False, + ), + ) + if is_dataclass(value): + metadata = { + data_field.name: _canonical_value(getattr(value, data_field.name)) + for data_field in fields(value) + } + return {"type": _qualified_name(value), "fields": metadata} + raise TypeError( + "Registration fingerprint metadata contains unsupported value type " + f"{_qualified_name(value)!r}. Values must be complete declarative data; " + "live or opaque objects cannot be fingerprinted by type alone." + ) + + +def _provider_fingerprint_declaration(provider: object) -> object: + """Return the complete canonical declaration for one validated provider.""" + if is_dataclass(provider): + return provider + return {"provider_type": _qualified_name(provider)} + + +def _canonical_json(value: object) -> str: + """Encode one declaration using the versioned canonical JSON form.""" + return json.dumps( + _canonical_value(value), + sort_keys=True, + separators=(",", ":"), + ensure_ascii=False, + allow_nan=False, + ) + + +def _digest(payload: object) -> str: + """Return the SHA-256 digest for one canonical declaration payload.""" + return hashlib.sha256(_canonical_json(payload).encode("utf-8")).hexdigest() + + +def _snapshot_settle_presets( + values: Mapping[str, DynamicSettleMonitorCfg], +) -> Mapping[str, DynamicSettleMonitorCfg]: + """Own one strict named settle-preset table.""" + if not isinstance(values, Mapping) or not values: + raise ValueError("settle_presets must be a non-empty mapping.") + normalized: dict[str, DynamicSettleMonitorCfg] = {} + for preset_id, preset in values.items(): + if ( + type(preset_id) is not str + or not preset_id + or preset_id != preset_id.strip() + ): + raise ValueError( + "Settle preset IDs must be non-empty strings without outer " + "whitespace." + ) + if not isinstance(preset, DynamicSettleMonitorCfg): + raise TypeError( + "settle_presets values must be DynamicSettleMonitorCfg values." + ) + normalized[preset_id] = preset.snapshot() + return MappingProxyType(normalized) + + +def _exact_identifier(value: object, *, field_name: str) -> str: + """Validate one exact catalog identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _relation_grounder_key( + grounder: RelationTargetGrounder, +) -> tuple[str, type[Affordance], str]: + """Return the compiler-compatible exact key for one relation grounder.""" + grounder_type = type(grounder) + capability = _exact_identifier( + getattr(grounder_type, "capability", None), + field_name="RelationTargetGrounder.capability", + ) + affordance_type = getattr(grounder_type, "affordance_type", None) + if not isinstance(affordance_type, type) or not issubclass( + affordance_type, + Affordance, + ): + raise TypeError( + "RelationTargetGrounder.affordance_type must be an Affordance subclass." + ) + revision = _exact_identifier( + getattr(grounder_type, "affordance_revision", None), + field_name="RelationTargetGrounder.affordance_revision", + ) + return capability, affordance_type, revision + + +def _relation_grounder_order_key( + grounder: RelationTargetGrounder, +) -> tuple[str, str, str]: + """Return one totally ordered rendering of a relation-grounder key.""" + capability, affordance_type, revision = _relation_grounder_key(grounder) + return capability, _qualified_name(affordance_type), revision + + +def _validate_provider_declaration(provider: object, *, field_name: str) -> None: + """Accept only frozen dataclass declarations or stateless providers.""" + dataclass_declaration = is_dataclass(provider) + dataclass_field_names: set[str] = set() + if dataclass_declaration: + params = getattr(type(provider), "__dataclass_params__", None) + if params is None or not params.frozen: + raise TypeError( + f"{field_name} stateful declarations must be frozen dataclasses " + "so every configuration field enters the registration fingerprint." + ) + dataclass_field_names.update( + declaration_field.name for declaration_field in fields(provider) + ) + + state_names: set[str] = set() + instance_state = getattr(provider, "__dict__", None) + if isinstance(instance_state, Mapping): + state_names.update(instance_state) + for owner in type(provider).__mro__: + declared_slots = getattr(owner, "__slots__", ()) + slots = (declared_slots,) if isinstance(declared_slots, str) else declared_slots + for slot_name in slots: + if slot_name in {"__dict__", "__weakref__"}: + continue + storage_name = ( + f"_{owner.__name__.lstrip('_')}{slot_name}" + if slot_name.startswith("__") and not slot_name.endswith("__") + else slot_name + ) + if hasattr(provider, storage_name): + state_names.add(storage_name) + undeclared_state = ( + state_names.difference(dataclass_field_names) + if dataclass_declaration + else state_names + ) + if undeclared_state: + raise TypeError( + f"{field_name} providers contain unfingerprinted state " + f"{sorted(undeclared_state)}. Use a frozen dataclass declaration with " + "every state field declared; non-dataclass providers must be stateless." + ) + + +def _snapshot_relation_grounders( + values: tuple[RelationTargetGrounder, ...], +) -> tuple[RelationTargetGrounder, ...]: + """Validate and own one immutable relation-grounder tuple.""" + if type(values) is not tuple: + raise TypeError("relation_grounders must be an exact tuple.") + seen: set[tuple[str, type[Affordance], str]] = set() + for grounder in values: + if not isinstance(grounder, RelationTargetGrounder): + raise TypeError( + "relation_grounders must contain RelationTargetGrounder instances." + ) + _validate_provider_declaration( + grounder, + field_name="relation_grounders", + ) + key = _relation_grounder_key(grounder) + if key in seen: + raise ValueError(f"Duplicate relation grounder key {key!r}.") + seen.add(key) + return tuple(values) + + +def _snapshot_relation_grounder_keys( + values: frozenset[tuple[str, type[Affordance], str]], +) -> frozenset[tuple[str, type[Affordance], str]]: + """Validate immutable provider-free relation-grounder lookup keys.""" + if type(values) is not frozenset: + raise TypeError("relation_grounder_keys must be an exact frozenset.") + normalized: set[tuple[str, type[Affordance], str]] = set() + for key in values: + if type(key) is not tuple or len(key) != 3: + raise TypeError("relation_grounder_keys must contain exact 3-tuple values.") + capability, affordance_type, revision = key + _exact_identifier(capability, field_name="relation grounder capability") + if not isinstance(affordance_type, type) or not issubclass( + affordance_type, + Affordance, + ): + raise TypeError( + "relation grounder affordance types must be Affordance subclasses." + ) + _exact_identifier(revision, field_name="relation grounder revision") + normalized.add((capability, affordance_type, revision)) + return frozenset(normalized) + + +def _handover_pose_provider_id(provider: HandOverPoseProvider) -> str: + """Return the compiler-compatible class ID for one hand-over provider.""" + return _exact_identifier( + getattr(type(provider), "provider_id", None), + field_name="HandOverPoseProvider.provider_id", + ) + + +def _snapshot_handover_pose_providers( + values: tuple[HandOverPoseProvider, ...], +) -> tuple[HandOverPoseProvider, ...]: + """Validate and own one immutable hand-over-provider tuple.""" + if type(values) is not tuple: + raise TypeError("handover_pose_providers must be an exact tuple.") + seen: set[str] = set() + for provider in values: + if not isinstance(provider, HandOverPoseProvider): + raise TypeError( + "handover_pose_providers must contain HandOverPoseProvider instances." + ) + _validate_provider_declaration( + provider, + field_name="handover_pose_providers", + ) + provider_id = _handover_pose_provider_id(provider) + if provider_id in seen: + raise ValueError(f"Duplicate handover pose provider {provider_id!r}.") + seen.add(provider_id) + return tuple(values) + + +def _declared_articulation_operation_targets( + scene_binding: SimulationSceneBinding, +) -> dict[str, frozenset[str]]: + """Derive named operation-target IDs from the task-owned scene binding.""" + return { + binding.entity_id: frozenset(binding.semantic_targets) + for binding in scene_binding.articulation_operations + } + + +def _snapshot_articulation_operation_targets( + values: Mapping[str, frozenset[str]], + *, + scene: SceneManifest, +) -> Mapping[str, frozenset[str]]: + """Own and cross-check provider-free named articulation targets.""" + if not isinstance(values, Mapping): + raise TypeError("articulation_operation_targets must be a mapping.") + normalized: dict[str, frozenset[str]] = {} + for affordance_id, target_ids in values.items(): + _exact_identifier( + affordance_id, + field_name="articulation operation affordance IDs", + ) + if type(target_ids) is not frozenset: + raise TypeError( + "articulation_operation_targets values must be exact frozensets." + ) + for target_id in target_ids: + _exact_identifier( + target_id, + field_name="articulation operation target IDs", + ) + entry = scene.lookup( + affordance_id, + expected_type=SceneAffordanceRef, + path=("articulation_operation_targets", affordance_id), + ) + if entry.ref.entity_id != affordance_id: + raise ValueError( + "articulation_operation_targets keys must use canonical " + "affordance IDs." + ) + if ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + not in entry.affordance_capabilities + or entry.affordance_payload_type is not ArticulationOperationAffordance + ): + raise TypeError( + f"Scene affordance {affordance_id!r} is not an articulation " + "operation affordance." + ) + normalized[affordance_id] = frozenset(target_ids) + + declared_affordance_ids = { + entry.ref.entity_id + for entry in scene.entries + if type(entry.ref) is SceneAffordanceRef + and ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + in entry.affordance_capabilities + } + if set(normalized) != declared_affordance_ids: + raise ValueError( + "articulation_operation_targets must cover every declared operation " + f"affordance exactly; expected {sorted(declared_affordance_ids)}, got " + f"{sorted(normalized)}." + ) + return MappingProxyType(normalized) + + +class _SceneManifestProgramResolver: + """Resolve compiler references from an immutable :class:`SceneManifest`.""" + + def __init__(self, scene: SceneManifest) -> None: + if type(scene) is not SceneManifest: + raise TypeError("scene must be exactly SceneManifest.") + self._scene = scene + + def resolve( + self, + reference: str, + *, + expected_types: tuple[type[SceneEntityRef], ...], + path: ConfigPath, + ) -> SceneEntityRef: + """Resolve one reference without retaining a live registry.""" + if ( + type(expected_types) is not tuple + or not expected_types + or not all( + isinstance(expected_type, type) + and issubclass(expected_type, SceneEntityRef) + for expected_type in expected_types + ) + ): + raise TypeError( + "expected_types must be a non-empty tuple of scene-ref types." + ) + try: + resolved = self._scene.resolve(reference, path=path) + except (KeyError, TypeError, ValueError) as exc: + raise ExpertProgramCompileError( + "unknown_scene_reference", + path, + str(exc), + ) from exc + if type(resolved) not in expected_types: + raise ExpertProgramCompileError( + "scene_reference_type_mismatch", + path, + f"Scene reference {reference!r} resolves to " + f"{type(resolved).__name__}, expected one of " + f"{tuple(value.__name__ for value in expected_types)}.", + ) + return type(resolved)(resolved.entity_id) + + +@dataclass(frozen=True, slots=True) +class ExpertProgramIntegrationCatalog: + """Provider-free integration directory owned by one task registration.""" + + scene_registry_id: str + robot_profile_id: str + scene: SceneManifest + robot_profile: RobotSkillProfile + call_catalog: SemanticCallCatalog + relation_grounder_keys: frozenset[tuple[str, type[Affordance], str]] + articulation_operation_targets: Mapping[str, frozenset[str]] + settle_preset_ids: frozenset[str] + fingerprint: str + _required_skills: Mapping[str, SkillDescriptor] = field( + repr=False, + compare=False, + ) + + def __post_init__(self) -> None: + for field_name in ("scene_registry_id", "robot_profile_id"): + value = getattr(self, field_name) + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be an exact identifier.") + if type(self.scene) is not SceneManifest: + raise TypeError("scene must be exactly SceneManifest.") + if type(self.robot_profile) is not RobotSkillProfile: + raise TypeError("robot_profile must be exactly RobotSkillProfile.") + if type(self.call_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + object.__setattr__( + self, + "relation_grounder_keys", + _snapshot_relation_grounder_keys(self.relation_grounder_keys), + ) + object.__setattr__( + self, + "articulation_operation_targets", + _snapshot_articulation_operation_targets( + self.articulation_operation_targets, + scene=self.scene, + ), + ) + if self.robot_profile.profile_id != self.robot_profile_id: + raise ValueError("robot_profile_id must match robot_profile.profile_id.") + preset_ids = frozenset(self.settle_preset_ids) + if not preset_ids: + raise ValueError("settle_preset_ids must not be empty.") + object.__setattr__(self, "settle_preset_ids", preset_ids) + if ( + type(self.fingerprint) is not str + or len(self.fingerprint) != 64 + or any( + character not in "0123456789abcdef" for character in self.fingerprint + ) + ): + raise ValueError("fingerprint must be a lowercase SHA-256 digest.") + object.__setattr__( + self, + "_required_skills", + MappingProxyType(dict(self._required_skills)), + ) + + def validate_integration( + self, + integration: ExpertProgramIntegrationCfg, + *, + path: ConfigPath, + ) -> None: + """Validate exact scene, profile, and runtime-preset selection.""" + del path + if integration.scene_registry != self.scene_registry_id: + raise ValueError( + f"Expected scene_registry {self.scene_registry_id!r}, got " + f"{integration.scene_registry!r}." + ) + if integration.robot_profile != self.robot_profile_id: + raise ValueError( + f"Expected robot_profile {self.robot_profile_id!r}, got " + f"{integration.robot_profile!r}." + ) + if integration.runtime_preset not in self.robot_profile.presets: + raise KeyError( + f"Unknown runtime preset {integration.runtime_preset!r}; available " + f"presets are {sorted(self.robot_profile.presets)}." + ) + + def validate_semantic_call( + self, + call: SemanticCallCfg, + *, + path: ConfigPath, + ) -> None: + """Validate semantic-call catalog and payload revision references.""" + call_id = call.call_id if type(call) is RegisteredSemanticCallCfg else call.kind + descriptor = self.call_catalog.discover(call_id) + if type(call) is RegisteredSemanticCallCfg and ( + call.schema_version != descriptor.schema_version + ): + raise ValueError( + f"Semantic call {call_id!r} requires schema_version " + f"{descriptor.schema_version}, got {call.schema_version}." + ) + if type(call) is OperateArticulationCfg and call.target is not None: + self._validate_articulation_operation_target( + articulation=call.articulation, + handle=call.handle, + target=call.target, + path=path, + ) + + def _validate_articulation_operation_target( + self, + *, + articulation: str | SceneArticulationRef, + handle: str | SceneAffordanceRef | None, + target: str, + path: ConfigPath, + ) -> None: + """Resolve one operation affordance and validate its named target.""" + try: + articulation_ref = self.scene.resolve( + articulation, + expected_type=SceneArticulationRef, + path=(*path, "articulation"), + ) + except SemanticValidationError as exc: + raise ExpertProgramValidationError( + exc.diagnostic.code, + exc.diagnostic.path, + exc.diagnostic.message, + ) from exc + try: + affordance = self.scene.resolve_affordance( + articulation_ref, + capability=ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + explicit=handle, + path=(*path, "handle"), + ) + except SemanticValidationError as exc: + raise ExpertProgramValidationError( + exc.diagnostic.code, + exc.diagnostic.path, + exc.diagnostic.message, + ) from exc + target_ids = self.articulation_operation_targets.get(affordance.entity_id) + if target_ids is None: + raise ExpertProgramValidationError( + "missing_articulation_operation_targets", + (*path, "handle"), + f"Operation affordance {affordance.entity_id!r} has no static " + "named-target declaration.", + ) + if target not in target_ids: + raise ExpertProgramValidationError( + "unknown_articulation_operation_target", + (*path, "target"), + f"Unknown target {target!r} for operation affordance " + f"{affordance.entity_id!r}; available targets are " + f"{sorted(target_ids)}.", + ) + + def _validate_place_relation_grounder( + self, + call: Place, + *, + affordance: SceneAffordanceRef, + path: ConfigPath, + ) -> None: + """Require the exact linked relation-affordance grounder pre-sim.""" + if call.on is not None: + capability = PLACE_ON_AFFORDANCE_CAPABILITY + relation_field = "on" + elif call.inside is not None: + capability = PLACE_IN_AFFORDANCE_CAPABILITY + relation_field = "inside" + else: + return + entry = self.scene.lookup( + affordance, + expected_type=SceneAffordanceRef, + path=(*path, relation_field), + ) + payload_type = entry.affordance_payload_type + revision = entry.affordance_revision + if payload_type is None or revision is None: + raise ExpertProgramValidationError( + "incomplete_relation_affordance_declaration", + (*path, relation_field), + f"Relation affordance {affordance.entity_id!r} must declare an " + "exact payload type and revision.", + ) + key = (capability, payload_type, revision) + if key not in self.relation_grounder_keys: + rendered_key = ( + capability, + _qualified_name(payload_type), + revision, + ) + raise ExpertProgramValidationError( + "relation_grounder_not_registered", + (*path, relation_field), + f"No task-registration relation grounder matches linked " + f"affordance {affordance.entity_id!r} with key {rendered_key!r}.", + ) + + def validate_scene_reference( + self, + reference: str, + *, + role: SceneReferenceRole, + path: ConfigPath, + ) -> None: + """Validate one typed scene reference against its declared role.""" + expected: dict[str, tuple[type[SceneEntityRef], ...]] = { + "entity": (SceneEntityRef,), + "object": (SceneObjectRef,), + "articulation": (SceneArticulationRef,), + "affordance": (SceneAffordanceRef,), + "object_or_affordance": (SceneObjectRef, SceneAffordanceRef), + } + expected_types = expected.get(role) + if expected_types is None: + raise ValueError(f"Unsupported scene reference role {role!r}.") + resolved = self.scene.resolve(reference, path=path) + if not isinstance(resolved, expected_types): + raise TypeError( + f"Scene reference {reference!r} is {type(resolved).__name__}, " + f"not one of {tuple(value.__name__ for value in expected_types)}." + ) + + def validate_post_policy( + self, + policy: PostPolicyCfg, + *, + path: ConfigPath, + ) -> None: + """Validate one registered post-policy kind and named preset.""" + del path + if policy.kind not in _POST_POLICY_KINDS: + raise KeyError( + f"Unknown post-policy kind {policy.kind!r}; available kinds are " + f"{sorted(_POST_POLICY_KINDS)}." + ) + if policy.preset not in self.settle_preset_ids: + raise KeyError( + f"Unknown settle preset {policy.preset!r}; available presets are " + f"{sorted(self.settle_preset_ids)}." + ) + + def validate_validator( + self, + validator: ValidatorCfg, + *, + path: ConfigPath, + ) -> None: + """Validate one registered segment-validator kind.""" + del path + if validator.kind not in _VALIDATOR_KINDS: + raise KeyError( + f"Unknown validator kind {validator.kind!r}; available kinds are " + f"{sorted(_VALIDATOR_KINDS)}." + ) + + def preflight(self, program: ExpertProgramCfg) -> CompiledProgram: + """Compile and statically link every expanded semantic call.""" + self.validate_integration(program.integration, path=("integration",)) + resolver: ExpertProgramSceneResolver = _SceneManifestProgramResolver(self.scene) + compiled = ExpertProgramCompiler(resolver).compile(program) + manifest = SemanticIntegrationManifest( + scene=self.scene, + robot_profile=self.robot_profile, + call_catalog=self.call_catalog, + runtime_preset=program.integration.runtime_preset, + ) + for segment in compiled.iter_segments(): + for call in segment.calls: + if ( + type(call.call) is OperateArticulation + and call.call.target is not None + ): + self._validate_articulation_operation_target( + articulation=call.call.articulation, + handle=call.call.handle, + target=call.call.target, + path=call.source_path, + ) + linked = manifest.link_call(call.call, path=call.source_path) + if type(linked.call) is Place and linked.call.at is None: + destination = linked.affordances.get("destination") + if destination is None: + raise AssertionError( + "Linked relation Place call lacks a destination " + "affordance." + ) + self._validate_place_relation_grounder( + linked.call, + affordance=destination, + path=call.source_path, + ) + return compiled + + def validate_engine(self, engine: AtomicActionEngine) -> None: + """Require the live engine to expose every statically selected skill.""" + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + for skill_id, expected in self._required_skills.items(): + actual = engine.skills.get(skill_id) + if actual != expected: + raise IntegrationFingerprintMismatch( + f"Live skill {skill_id!r} differs from the registered " + "semantic target descriptor." + ) + + +def _profile_with_control_dt( + profile: RobotSkillProfile, + *, + control_dt: float, +) -> RobotSkillProfile: + """Return the registration profile aligned to one Gym control cadence.""" + return replace( + profile, + presets={ + preset_id: SkillPolicyPreset( + preset_id=preset.preset_id, + schema_version=preset.schema_version, + motion_policy=replace(preset.motion_policy, control_dt=control_dt), + tracking_policy=preset.tracking_policy, + recovery_policy=preset.recovery_policy, + runner_cfg=preset.runner_cfg, + effect_monitors=preset.effect_monitors, + ) + for preset_id, preset in profile.presets.items() + }, + ) + + +def _registration_payload( + *, + scene_binding: SimulationSceneBinding, + scene: SceneManifest, + articulation_operation_targets: Mapping[str, frozenset[str]], + robot_profile_binding: SimulationRobotSkillProfileBinding, + robot_profile: RobotSkillProfile, + call_catalog: SemanticCallCatalog, + settle_presets: Mapping[str, DynamicSettleMonitorCfg], + relation_grounder_keys: frozenset[tuple[str, type[Affordance], str]], + relation_grounders: tuple[RelationTargetGrounder, ...], + handover_pose_providers: tuple[HandOverPoseProvider, ...], +) -> dict[str, object]: + """Build the versioned canonical fingerprint payload.""" + return { + "schema_version": _CATALOG_FINGERPRINT_SCHEMA_VERSION, + "scene_binding": scene_binding, + "scene_manifest": scene.entries, + "articulation_operation_targets": articulation_operation_targets, + "robot_profile_binding": robot_profile_binding, + "robot_profile": robot_profile, + "call_descriptors": tuple( + sorted( + call_catalog.descriptors.values(), + key=lambda descriptor: descriptor.call_id, + ) + ), + "relation_grounder_keys": relation_grounder_keys, + "relation_grounders": tuple( + { + "key": _relation_grounder_key(grounder), + "provider": _provider_fingerprint_declaration(grounder), + } + for grounder in sorted( + relation_grounders, + key=_relation_grounder_order_key, + ) + ), + "handover_pose_providers": tuple( + { + "provider_id": _handover_pose_provider_id(provider), + "provider": _provider_fingerprint_declaration(provider), + } + for provider in sorted( + handover_pose_providers, + key=_handover_pose_provider_id, + ) + ), + "post_policy_kinds": _POST_POLICY_KINDS, + "settle_presets": settle_presets, + "validator_kinds": _VALIDATOR_KINDS, + } + + +@dataclass(frozen=True, slots=True) +class SimulationExpertProgramRegistration: + """Exact immutable task-owned simulation integration registration.""" + + scene_binding: SimulationSceneBinding + robot_profile_binding: SimulationRobotSkillProfileBinding + call_catalog: SemanticCallCatalog = field( + default_factory=builtin_semantic_call_catalog + ) + settle_presets: Mapping[str, DynamicSettleMonitorCfg] = field( + default_factory=default_simulation_settle_presets + ) + relation_grounders: tuple[RelationTargetGrounder, ...] = () + handover_pose_providers: tuple[HandOverPoseProvider, ...] = () + catalog: ExpertProgramIntegrationCatalog = field(init=False) + + def __post_init__(self) -> None: + if type(self.scene_binding) is not SimulationSceneBinding: + raise TypeError("scene_binding must be exactly SimulationSceneBinding.") + if type(self.robot_profile_binding) is not SimulationRobotSkillProfileBinding: + raise TypeError( + "robot_profile_binding must be exactly " + "SimulationRobotSkillProfileBinding." + ) + if type(self.call_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + settle_presets = _snapshot_settle_presets(self.settle_presets) + object.__setattr__(self, "settle_presets", settle_presets) + relation_grounders = _snapshot_relation_grounders(self.relation_grounders) + object.__setattr__(self, "relation_grounders", relation_grounders) + relation_grounder_keys = frozenset( + _relation_grounder_key(grounder) for grounder in relation_grounders + ) + handover_pose_providers = _snapshot_handover_pose_providers( + self.handover_pose_providers + ) + object.__setattr__( + self, + "handover_pose_providers", + handover_pose_providers, + ) + + scene = self.scene_binding.declare() + articulation_operation_targets = _declared_articulation_operation_targets( + self.scene_binding + ) + profile = self.robot_profile_binding.declare() + selected_handover_provider = profile.grounding_providers.get("hand_over") + registered_handover_provider_ids = { + _handover_pose_provider_id(provider) for provider in handover_pose_providers + } + if ( + selected_handover_provider is not None + and selected_handover_provider not in registered_handover_provider_ids + ): + raise ValueError( + "Robot profile selects handover pose provider " + f"{selected_handover_provider!r}, but the task registration did " + "not install it." + ) + builtin_skills = { + descriptor.skill_id: descriptor + for action_type in BUILTIN_ACTION_TYPES + if (descriptor := action_type.descriptor()).agent_visible + and descriptor.binding_contract is not None + } + required_skills: dict[str, SkillDescriptor] = {} + for descriptor in self.call_catalog.descriptors.values(): + target = descriptor.target_descriptor + installed = builtin_skills.get(descriptor.skill_id) + if target is None or installed != target: + raise ValueError( + f"Semantic call {descriptor.call_id!r} targets skill " + f"{descriptor.skill_id!r}, which is not installed by the " + "standard simulation factory." + ) + required_skills[descriptor.skill_id] = target + + fingerprint = _digest( + _registration_payload( + scene_binding=self.scene_binding, + scene=scene, + articulation_operation_targets=articulation_operation_targets, + robot_profile_binding=self.robot_profile_binding, + robot_profile=profile, + call_catalog=self.call_catalog, + settle_presets=settle_presets, + relation_grounder_keys=relation_grounder_keys, + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + ) + ) + object.__setattr__( + self, + "catalog", + ExpertProgramIntegrationCatalog( + scene_registry_id=self.scene_binding.registry_id, + robot_profile_id=self.robot_profile_binding.profile_id, + scene=scene, + robot_profile=profile, + call_catalog=self.call_catalog, + relation_grounder_keys=relation_grounder_keys, + articulation_operation_targets=articulation_operation_targets, + settle_preset_ids=frozenset(settle_presets), + fingerprint=fingerprint, + _required_skills=required_skills, + ), + ) + + @property + def fingerprint(self) -> str: + """Return the canonical registration fingerprint.""" + return self.catalog.fingerprint + + def assert_unchanged(self) -> None: + """Reject nested declaration drift before live component creation.""" + scene = self.scene_binding.declare() + articulation_operation_targets = _declared_articulation_operation_targets( + self.scene_binding + ) + profile = self.robot_profile_binding.declare() + try: + relation_grounders = _snapshot_relation_grounders(self.relation_grounders) + relation_grounder_keys = frozenset( + _relation_grounder_key(grounder) for grounder in relation_grounders + ) + handover_pose_providers = _snapshot_handover_pose_providers( + self.handover_pose_providers + ) + current = _digest( + _registration_payload( + scene_binding=self.scene_binding, + scene=scene, + articulation_operation_targets=(articulation_operation_targets), + robot_profile_binding=self.robot_profile_binding, + robot_profile=profile, + call_catalog=self.call_catalog, + settle_presets=self.settle_presets, + relation_grounder_keys=relation_grounder_keys, + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + ) + ) + except (TypeError, ValueError) as exc: + raise IntegrationFingerprintMismatch( + "Expert Program integration provider declaration changed after " + "task registration." + ) from exc + if current != self.fingerprint: + raise IntegrationFingerprintMismatch( + "Expert Program integration declaration changed after task " + "registration." + ) + + def validate_scene_registry(self, registry: SceneRegistry) -> None: + """Validate a live registry against the registered scene declaration.""" + self.assert_unchanged() + self.catalog.scene.validate_registry(registry) + + def validate_robot_profile( + self, + profile: RobotSkillProfile, + *, + step_dt: float, + ) -> None: + """Validate a cadence-aligned live profile against its declaration.""" + self.assert_unchanged() + if type(profile) is not RobotSkillProfile: + raise TypeError("profile must be exactly RobotSkillProfile.") + expected = _profile_with_control_dt( + self.catalog.robot_profile, + control_dt=step_dt, + ) + if _canonical_json(profile) != _canonical_json(expected): + raise IntegrationFingerprintMismatch( + "Live robot skill profile differs from the registered declaration." + ) + + +__all__ = [ + "ExpertProgramIntegrationCatalog", + "IntegrationFingerprintMismatch", + "SimulationExpertProgramRegistration", +] diff --git a/embodichain/lab/gym/envs/expert_program/environment.py b/embodichain/lab/gym/envs/expert_program/environment.py index 95b2294f3..2e75c934e 100644 --- a/embodichain/lab/gym/envs/expert_program/environment.py +++ b/embodichain/lab/gym/envs/expert_program/environment.py @@ -80,6 +80,7 @@ SegmentPostPolicyPort, SegmentValidatorPort, ) +from .catalog import ExpertProgramIntegrationCatalog from .cfg import ExpertProgramCfg, ExpertProgramIntegrationCfg from .compiler import ( CompiledProgram, @@ -268,6 +269,8 @@ class ExpertProgramEnvironmentAdapter: Args: factory: Environment-owned live-provider and engine factory. step_dt: Authoritative Gym control cadence in seconds. + integration_catalog: Optional immutable task-registration catalog used + for provider-free compilation. call_catalog: Optional immutable semantic call catalog. The built-in catalog is used when omitted. endpoint_adapters: Optional custom robot endpoint adapters. @@ -291,6 +294,7 @@ def __init__( factory: ExpertProgramEnvironmentFactory, *, step_dt: float, + integration_catalog: ExpertProgramIntegrationCatalog | None = None, call_catalog: SemanticCallCatalog | None = None, endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None @@ -319,7 +323,32 @@ def __init__( factory.robot_profile_id, field_name="factory.robot_profile_id", ) - selected_catalog = call_catalog or builtin_semantic_call_catalog() + if ( + integration_catalog is not None + and type(integration_catalog) is not ExpertProgramIntegrationCatalog + ): + raise TypeError( + "integration_catalog must be exactly " + "ExpertProgramIntegrationCatalog or None." + ) + if integration_catalog is not None: + if integration_catalog.scene_registry_id != scene_registry_id: + raise ValueError( + "integration_catalog scene_registry_id does not match factory." + ) + if integration_catalog.robot_profile_id != robot_profile_id: + raise ValueError( + "integration_catalog robot_profile_id does not match factory." + ) + if call_catalog is not None and ( + call_catalog is not integration_catalog.call_catalog + ): + raise ValueError( + "call_catalog cannot override the task registration catalog." + ) + selected_catalog = integration_catalog.call_catalog + else: + selected_catalog = call_catalog or builtin_semantic_call_catalog() if type(selected_catalog) is not SemanticCallCatalog: raise TypeError("call_catalog must be exactly SemanticCallCatalog or None.") if endpoint_adapters is not None and not isinstance(endpoint_adapters, Mapping): @@ -353,6 +382,7 @@ def __init__( self._scene_registry_id = scene_registry_id self._robot_profile_id = robot_profile_id self._step_dt = float(step_dt) + self._integration_catalog = integration_catalog self._call_catalog = selected_catalog self._endpoint_adapters = ( None if endpoint_adapters is None else dict(endpoint_adapters) @@ -406,6 +436,8 @@ def compile(self, program: ExpertProgramCfg) -> CompiledProgram: if type(program) is not ExpertProgramCfg: raise TypeError("program must be exactly ExpertProgramCfg.") self._validate_selection(program.integration) + if self._integration_catalog is not None: + return self._integration_catalog.preflight(program) registry = self._create_scene_registry() return ExpertProgramCompiler.from_scene_registry(registry).compile(program) diff --git a/embodichain/lab/gym/envs/expert_program/simulation.py b/embodichain/lab/gym/envs/expert_program/simulation.py index 5317dc091..b08235c32 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation.py +++ b/embodichain/lab/gym/envs/expert_program/simulation.py @@ -50,6 +50,7 @@ RobotSkillProfile, SkillPolicyPreset, ) +from embodichain.lab.sim.skills.integration import SceneEntityManifest, SceneManifest from embodichain.lab.sim.skills.scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, GRASP_AFFORDANCE_CAPABILITY, @@ -596,6 +597,139 @@ def __post_init__(self) -> None: "collision_world_mode must be SceneCollisionWorldMode or None." ) + def declare(self) -> SceneManifest: + """Project the complete provider-free scene declaration. + + Canonical topology errors are rejected here, before a simulation is + constructed. Native simulation UIDs, mesh data, link names, and joint + names remain live validation owned by :meth:`build`. + """ + 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} + entries: list[SceneEntityManifest] = [] + + for binding in self.rigid_objects: + native_aliases = ( + () + 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 + ) + } + ) + entries.append( + SceneEntityManifest( + ref=SceneObjectRef(binding.entity_id), + aliases=(*native_aliases, *binding.aliases), + dynamics=binding.dynamics, + collision_role=binding.collision_role, + semantic_type=binding.semantic_type, + default_affordances=defaults, + ) + ) + + for binding in self.articulations: + native_aliases = ( + () + 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) + ) + } + ) + entries.append( + SceneEntityManifest( + ref=SceneArticulationRef(binding.entity_id), + aliases=(*native_aliases, *binding.aliases), + dynamics=binding.dynamics, + collision_role=binding.collision_role, + semantic_type=binding.semantic_type, + default_affordances=defaults, + ) + ) + + for binding in self.links: + if binding.articulation_id not in articulations: + raise KeyError( + f"Link {binding.entity_id!r} references unbound articulation " + f"{binding.articulation_id!r}." + ) + entries.append( + SceneEntityManifest( + ref=SceneLinkRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=binding.native_link_name, + dynamics=binding.dynamics, + semantic_type=binding.semantic_type, + ) + ) + + for binding in self.antipodal_grasps: + if binding.object_id not in objects: + raise KeyError( + f"Grasp affordance {binding.entity_id!r} references unbound " + f"object {binding.object_id!r}." + ) + entries.append( + SceneEntityManifest( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneObjectRef(binding.object_id), + native_name=binding.native_name, + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_payload_type=AntipodalAffordance, + affordance_revision=binding.revision, + relative_pose=binding.relative_pose, + ) + ) + + for binding in self.articulation_operations: + if binding.articulation_id not in articulations: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound articulation {binding.articulation_id!r}." + ) + link = links.get(binding.link_id) + if link is None: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound link {binding.link_id!r}." + ) + if link.articulation_id != binding.articulation_id: + raise ValueError( + f"Operation affordance {binding.entity_id!r} and link " + f"{binding.link_id!r} select different articulations." + ) + entries.append( + SceneEntityManifest( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=link.native_link_name, + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_payload_type=ArticulationOperationAffordance, + affordance_revision=binding.revision, + ) + ) + + return SceneManifest(entries) + def build(self, simulation: SimulationManager) -> SceneRegistry: """Build the existing authoritative scene registry. @@ -854,6 +988,21 @@ def build(self, *, control_dof: int) -> ControlPartCommandProfile: } ) + def declare(self) -> ControlPartCommandProfile: + """Build a provider-free command profile from declared tuple widths.""" + widths = {len(positions) for positions in self.commands.values()} + if len(widths) > 1: + raise ValueError( + f"Command preset {self.preset_id!r} declares inconsistent command " + f"widths {sorted(widths)}." + ) + return ControlPartCommandProfile.joint_positions( + **{ + command_id: torch.tensor(positions, dtype=torch.float32) + for command_id, positions in self.commands.items() + } + ) + def _require_control_part_dof(robot: Robot, control_part: str) -> int: """Validate one native joint-backed control part and return its width.""" @@ -902,6 +1051,9 @@ def endpoint_id(self) -> str: def build(self, robot: Robot) -> ResourceEndpoint: """Build and validate one endpoint declaration for ``robot``.""" + def declare(self) -> ResourceEndpoint: + """Return the provider-free endpoint declaration.""" + @runtime_checkable class SimulationRobotResourceBinding(Protocol): @@ -918,6 +1070,9 @@ def members(self) -> tuple[str, ...]: def build(self, robot: Robot) -> RobotResource: """Build and validate one owned robot resource declaration.""" + def declare(self) -> RobotResource: + """Return the provider-free resource declaration.""" + @dataclass(frozen=True, slots=True) class RobotResourceBinding: @@ -951,6 +1106,14 @@ def build(self, robot: Robot) -> RobotResource: members=self.members, ) + def declare(self) -> RobotResource: + """Return an independently owned provider-free resource.""" + return RobotResource( + resource_id=self.resource_id, + endpoints=self.endpoints, + members=self.members, + ) + @dataclass(frozen=True, slots=True) class ControlPartEndpointBinding: @@ -981,6 +1144,14 @@ def build(self, robot: Robot) -> ResourceEndpoint: capabilities=self.capabilities, ) + def declare(self) -> ResourceEndpoint: + """Return the endpoint contract without reading a robot.""" + return ControlPartEndpoint( + control_part=self.control_part, + command_profile=self.command_preset, + capabilities=self.capabilities, + ) + @dataclass(frozen=True, slots=True) class ControlPartResourceBinding: @@ -1026,6 +1197,16 @@ def build(self, robot: Robot) -> RobotResource: members=self.members, ) + def declare(self) -> RobotResource: + """Return the resource graph without reading native control parts.""" + return RobotResource( + resource_id=self.resource_id, + endpoints={ + binding.endpoint_id: binding.declare() for binding in self.endpoints + }, + members=self.members, + ) + def _owned_nested_identifier_mapping( values: Mapping[str, Mapping[str, str]], @@ -1221,6 +1402,65 @@ def require_control_part(control_part: str) -> int: grounding_providers=self.grounding_providers, ) + def declare(self) -> RobotSkillProfile: + """Project the complete provider-free robot skill profile.""" + resources: dict[str, RobotResource] = {} + for binding in self.resources: + resource = binding.declare() + if type(resource) is not RobotResource: + raise TypeError( + f"Resource binding {binding.resource_id!r} must declare " + "exactly RobotResource." + ) + if resource.resource_id != binding.resource_id: + raise ValueError( + f"Resource binding {binding.resource_id!r} declared " + f"resource ID {resource.resource_id!r}." + ) + if resource.members != tuple(binding.members): + raise ValueError( + f"Resource binding {binding.resource_id!r} changed its " + "declared resource members." + ) + resources[resource.resource_id] = resource + + command_presets = {preset.preset_id: preset for preset in self.command_presets} + for resource in resources.values(): + for endpoint_id, endpoint in resource.endpoints.items(): + if not isinstance(endpoint, ControlPartEndpoint): + continue + preset_id = endpoint.command_profile + if preset_id is None: + continue + preset = command_presets.get(preset_id) + if preset is None: + raise KeyError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} " + f"references unknown command preset {preset_id!r}." + ) + if preset.control_part != endpoint.control_part: + raise ValueError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} uses " + f"control part {endpoint.control_part!r}, but command " + f"preset {preset_id!r} targets {preset.control_part!r}." + ) + + return RobotSkillProfile( + profile_id=self.profile_id, + resources=resources, + command_profiles={ + preset.preset_id: preset.declare() for preset in self.command_presets + }, + defaults={ + skill_id: ResourceBinding(resources=bindings) + for skill_id, bindings in self.defaults.items() + }, + presets={preset.preset_id: preset for preset in self.presets}, + default_preset=self.default_preset, + skill_presets=self.skill_presets, + grounding_providers=self.grounding_providers, + ) + __all__ = [ "AntipodalGraspAffordanceBinding", diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py index 557b8f707..eee4660eb 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -38,7 +38,6 @@ import torch -from embodichain.lab.gym.envs.settling import DynamicSettleMonitorCfg from embodichain.lab.sim.atomic_actions import ( AtomicActionEngine, EntityState, @@ -70,11 +69,8 @@ MotionGenerator, ToppraPlannerCfg, ) -from embodichain.lab.sim.skills.calls import SemanticCallCatalog from embodichain.lab.sim.skills.compiler import ( - HandOverPoseProvider, RegisteredSemanticLowerer, - RelationTargetGrounder, ) from embodichain.lab.sim.skills.effects import ( ControlPartEvidenceAddress, @@ -108,15 +104,12 @@ GymPlanningObservationProvider, RuntimeTransportActionEncoder, ) +from .catalog import SimulationExpertProgramRegistration from .environment import ( ExpertProgramEnvironmentAdapter, ExpertProgramEnvironmentFactory, PlanningObservationPort, ) -from .simulation import ( - SimulationRobotSkillProfileBinding, - SimulationSceneBinding, -) from .simulation_policies import SimulationSegmentPolicyPort if TYPE_CHECKING: @@ -725,8 +718,7 @@ class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): Args: simulation: Exact live simulation that owns ``robot`` and scene UIDs. robot: Exact robot selected for planning and evidence acquisition. - scene_binding: Canonical-to-native scene declaration. - robot_profile_binding: Typed robot resource and policy declaration. + registration: Exact task-owned static and live integration declaration. step_dt: Authoritative Gym control cadence. planner_cfg: Explicit planner configuration. ``None`` selects TOPPRA for ``robot.uid``. @@ -735,7 +727,6 @@ class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): planners and isolated tests. endpoint_adapters: Explicit adapters for non-built-in resource endpoint types. - settle_presets: Optional named segment settling policies. translation_threshold: Material scene translation threshold. rotation_threshold: Material scene rotation threshold. contact_observer: Optional raw contact evidence callback. @@ -753,8 +744,7 @@ def __init__( self, simulation: SimulationManager, robot: Robot, - scene_binding: SimulationSceneBinding, - robot_profile_binding: SimulationRobotSkillProfileBinding, + registration: SimulationExpertProgramRegistration, *, step_dt: float, planner_cfg: BasePlannerCfg | None = None, @@ -762,7 +752,6 @@ def __init__( endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None ) = None, - settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, translation_threshold: float = 1.0e-4, rotation_threshold: float = 1.0e-3, contact_observer: BinaryObservationCallback | None = None, @@ -770,13 +759,11 @@ def __init__( force_observer: ScalarObservationCallback | None = None, wrench_observer: ScalarObservationCallback | None = None, ) -> None: - if type(scene_binding) is not SimulationSceneBinding: - raise TypeError("scene_binding must be exactly SimulationSceneBinding.") - if type(robot_profile_binding) is not SimulationRobotSkillProfileBinding: + if type(registration) is not SimulationExpertProgramRegistration: raise TypeError( - "robot_profile_binding must be exactly " - "SimulationRobotSkillProfileBinding." + "registration must be exactly SimulationExpertProgramRegistration." ) + registration.assert_unchanged() if planner_cfg is not None and motion_generator_factory is not None: raise ValueError( "planner_cfg and motion_generator_factory are mutually exclusive." @@ -818,8 +805,9 @@ def __init__( self._simulation = simulation self._robot = robot - self._scene_binding = scene_binding - self._robot_profile_binding = robot_profile_binding + self._registration = registration + self._scene_binding = registration.scene_binding + self._robot_profile_binding = registration.robot_profile_binding self._step_dt = _positive_finite(step_dt, field_name="step_dt") self._planner_cfg = selected_planner_cfg self._motion_generator_factory = motion_generator_factory @@ -850,8 +838,8 @@ def __init__( self._segment_policy_port = SimulationSegmentPolicyPort( simulation, robot, - scene_binding, - settle_presets=settle_presets, + registration.scene_binding, + settle_presets=registration.settle_presets, env_ids=self._env_ids, ) @@ -860,14 +848,12 @@ def from_environment( cls, environment: SimulationExpertProgramEnvironment, *, - scene_binding: SimulationSceneBinding, - robot_profile_binding: SimulationRobotSkillProfileBinding, + registration: SimulationExpertProgramRegistration, planner_cfg: BasePlannerCfg | None = None, motion_generator_factory: MotionGeneratorFactory | None = None, endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None ) = None, - settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, translation_threshold: float = 1.0e-4, rotation_threshold: float = 1.0e-3, contact_observer: BinaryObservationCallback | None = None, @@ -887,13 +873,11 @@ def from_environment( return cls( simulation, robot, - scene_binding, - robot_profile_binding, + registration, step_dt=step_dt, planner_cfg=planner_cfg, motion_generator_factory=motion_generator_factory, endpoint_adapters=endpoint_adapters, - settle_presets=settle_presets, translation_threshold=translation_threshold, rotation_threshold=rotation_threshold, contact_observer=contact_observer, @@ -933,7 +917,9 @@ def endpoint_adapters( def create_scene_registry(self) -> SceneRegistry: """Build one fresh authoritative registry from explicit bindings.""" - return self._scene_binding.build(self._simulation) + registry = self._scene_binding.build(self._simulation) + self._registration.validate_scene_registry(registry) + return registry def create_robot_skill_profile(self) -> RobotSkillProfile: """Build a profile whose every motion policy uses the Gym cadence.""" @@ -946,6 +932,7 @@ def create_robot_skill_profile(self) -> RobotSkillProfile: preset.motion_policy, control_dt=self._step_dt, ), + tracking_policy=preset.tracking_policy, recovery_policy=preset.recovery_policy, runner_cfg=preset.runner_cfg, effect_monitors=preset.effect_monitors, @@ -958,6 +945,10 @@ def create_robot_skill_profile(self) -> RobotSkillProfile: for preset in aligned.presets.values() ): raise AssertionError("Profile motion policies were not cadence-aligned.") + self._registration.validate_robot_profile( + aligned, + step_dt=self._step_dt, + ) return aligned def create_atomic_action_engine( @@ -977,11 +968,13 @@ def create_atomic_action_engine( raise ValueError( "Motion generator must own the exact robot selected by the factory." ) - return AtomicActionEngine( + engine = AtomicActionEngine( motion_generator, skill_profile=profile, endpoint_adapters=self._endpoint_adapters, ) + self._registration.catalog.validate_engine(engine) + return engine def create_planning_observation_provider( self, @@ -1089,24 +1082,22 @@ def create_accepted_runtime_command_observer( def create_adapter( self, *, - call_catalog: SemanticCallCatalog | None = None, registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), - relation_grounders: Iterable[RelationTargetGrounder] = (), - handover_pose_providers: Iterable[HandOverPoseProvider] = (), effect_monitor_registry: EffectMonitorRegistry | None = None, runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), runner_cfg: ExecutionRunnerCfg | None = None, parallel_safety_validator: ParallelCommandSafetyValidator | None = None, ) -> ExpertProgramEnvironmentAdapter: """Create the exact Gym adapter with shared simulation policy ports.""" + self._registration.assert_unchanged() return ExpertProgramEnvironmentAdapter( self, step_dt=self._step_dt, - call_catalog=call_catalog, + integration_catalog=self._registration.catalog, endpoint_adapters=self._endpoint_adapters, registered_lowerers=registered_lowerers, - relation_grounders=relation_grounders, - handover_pose_providers=handover_pose_providers, + relation_grounders=self._registration.relation_grounders, + handover_pose_providers=self._registration.handover_pose_providers, effect_monitor_registry=effect_monitor_registry, runtime_transports=runtime_transports, runner_cfg=runner_cfg, @@ -1136,17 +1127,13 @@ def _create_motion_generator(self) -> MotionGenerator: def create_simulation_expert_program_adapter( environment: SimulationExpertProgramEnvironment, *, - scene_binding: SimulationSceneBinding, - robot_profile_binding: SimulationRobotSkillProfileBinding, + registration: SimulationExpertProgramRegistration, planner_cfg: BasePlannerCfg | None = None, motion_generator_factory: MotionGeneratorFactory | None = None, endpoint_adapters: ( Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None ) = None, - relation_grounders: Iterable[RelationTargetGrounder] = (), - handover_pose_providers: Iterable[HandOverPoseProvider] = (), runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), - settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, translation_threshold: float = 1.0e-4, rotation_threshold: float = 1.0e-3, contact_observer: BinaryObservationCallback | None = None, @@ -1158,26 +1145,23 @@ def create_simulation_expert_program_adapter( """Create a complete production adapter from one standard Gym environment. This is the intended task-side one-line integration. Relation-target - grounders and embodiment-owned handover pose providers are explicit and - default to empty collections, so calls that require an uninstalled provider - remain fail-closed during program preflight. Advanced callers can retain - :class:`SimulationExpertProgramFactory` and call ``create_adapter`` directly - to install registered semantic lowerers or custom monitors. Custom endpoint - adapters and their matching Gym runtime transports are accepted here so a - non-joint endpoint remains executable through the one-line path. + grounders and embodiment-owned handover pose providers come exclusively + from ``registration``, so the statically fingerprinted objects are the exact + objects consumed by the runtime compiler. Calls that require an unregistered + provider remain fail-closed during program preflight. Advanced callers can + retain :class:`SimulationExpertProgramFactory` and call ``create_adapter`` + directly to install registered semantic lowerers or custom monitors. Custom + endpoint adapters and their matching Gym runtime transports are accepted + here so a non-joint endpoint remains executable through the one-line path. Args: environment: Standard Gym simulation environment exposing ``sim``, ``robot``, and ``step_dt``. - scene_binding: Authoritative typed scene declaration. - robot_profile_binding: Typed robot resource and policy declaration. + registration: Exact task registration used during static config loading. planner_cfg: Optional planner configuration owned by the factory. motion_generator_factory: Optional factory for one fresh motion generator. endpoint_adapters: Optional exact-type custom endpoint adapters. - relation_grounders: Explicit typed relation-target grounders. - handover_pose_providers: Explicit embodiment-owned handover pose providers. runtime_transports: Additional runtime-command-to-Gym encoders. - settle_presets: Optional named dynamic-settling policies. translation_threshold: Scene translation revision threshold. rotation_threshold: Scene rotation revision threshold. contact_observer: Optional raw contact evidence callback. @@ -1191,12 +1175,10 @@ def create_simulation_expert_program_adapter( """ factory = SimulationExpertProgramFactory.from_environment( environment, - scene_binding=scene_binding, - robot_profile_binding=robot_profile_binding, + registration=registration, planner_cfg=planner_cfg, motion_generator_factory=motion_generator_factory, endpoint_adapters=endpoint_adapters, - settle_presets=settle_presets, translation_threshold=translation_threshold, rotation_threshold=rotation_threshold, contact_observer=contact_observer, @@ -1205,8 +1187,6 @@ def create_simulation_expert_program_adapter( wrench_observer=wrench_observer, ) return factory.create_adapter( - relation_grounders=relation_grounders, - handover_pose_providers=handover_pose_providers, runtime_transports=runtime_transports, parallel_safety_validator=parallel_safety_validator, ) diff --git a/embodichain/lab/gym/envs/expert_program/simulation_policies.py b/embodichain/lab/gym/envs/expert_program/simulation_policies.py index c18dace2c..408e7cac0 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_policies.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_policies.py @@ -62,7 +62,7 @@ class _SimulationSettleTarget: native_entity: Any -def _default_settle_presets() -> Mapping[str, DynamicSettleMonitorCfg]: +def default_simulation_settle_presets() -> Mapping[str, DynamicSettleMonitorCfg]: """Return independently owned built-in post-policy presets.""" return MappingProxyType( { @@ -133,7 +133,9 @@ def __init__( raise ValueError("env_ids must contain unique values.") selected_presets = ( - _default_settle_presets() if settle_presets is None else settle_presets + default_simulation_settle_presets() + if settle_presets is None + else settle_presets ) if not isinstance(selected_presets, Mapping) or not selected_presets: raise ValueError("settle_presets must be a non-empty mapping.") @@ -712,4 +714,4 @@ def _read_pose(self, entity: Any, *, entity_id: str) -> torch.Tensor: return pose.clone() -__all__ = ["SimulationSegmentPolicyPort"] +__all__ = ["SimulationSegmentPolicyPort", "default_simulation_settle_presets"] diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index e524b6765..c3df4bba8 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -399,6 +399,7 @@ def config_to_cfg( manager_modules: list | None = None, *, source_path: str | os.PathLike[str] | None = None, + expert_program_path_override: str | os.PathLike[str] | None = None, ) -> "EmbodiedEnvCfg": """Parser configuration file into cfgs for env initialization. @@ -410,6 +411,9 @@ def config_to_cfg( relative top-level ``expert_program_path`` is resolved from this file's directory. Without it, relative paths use the current working directory. + expert_program_path_override: Optional explicit program path. This is + selected instead of the Gym-config path and resolves from the + process working directory. Returns: EmbodiedEnvCfg: A configuration object for initializing the environment. @@ -456,13 +460,23 @@ class ComponentCfg: if key not in config: log_error(f"Missing required config key: {key}") - if "expert_program_path" in config: - expert_program_path = config["expert_program_path"] - if type(expert_program_path) is not str: - raise TypeError("expert_program_path must be an exact string.") - if ( - not expert_program_path - or expert_program_path != expert_program_path.strip() + configured_expert_program_path = config.get("expert_program_path") + if expert_program_path_override is not None or "expert_program_path" in config: + if expert_program_path_override is not None: + expert_program_path = expert_program_path_override + expert_program_base_dir = None + if not isinstance(expert_program_path, (str, os.PathLike)): + raise TypeError("expert_program_path must be a string or path.") + else: + expert_program_path = configured_expert_program_path + expert_program_base_dir = ( + None if source_path is None else Path(source_path).expanduser().parent + ) + if type(expert_program_path) is not str: + raise TypeError("expert_program_path must be an exact string.") + expert_program_path_text = os.fspath(expert_program_path) + if not expert_program_path_text or ( + expert_program_path_text != expert_program_path_text.strip() ): raise ValueError( "expert_program_path must be a non-empty string without outer " @@ -471,14 +485,23 @@ class ComponentCfg: from embodichain.lab.gym.envs.expert_program.loader import ( load_expert_program, ) + from embodichain.lab.gym.utils.registration import get_env_spec - expert_program_base_dir = ( - None if source_path is None else Path(source_path).expanduser().parent - ) - env_cfg.expert_program = load_expert_program( - expert_program_path, + env_spec = get_env_spec(config["id"]) + registration = env_spec.expert_program_registration + if registration is None: + raise ValueError( + f"Environment {config['id']!r} does not register an Expert " + "Program integration catalog." + ) + registration.assert_unchanged() + expert_program = load_expert_program( + expert_program_path_text, base_dir=expert_program_base_dir, + validation_context=registration.catalog, ) + registration.catalog.preflight(expert_program) + env_cfg.expert_program = expert_program env_cfg.max_episode_steps = config.get("max_episode_steps", 300) env_cfg.num_envs = config.get("num_envs", 1) @@ -1069,6 +1092,7 @@ def build_env_cfg_from_args( gym_config, manager_modules=get_manager_modules(), source_path=gym_config_source_path, + expert_program_path_override=getattr(args, "expert_program", None), ) cfg.filter_visual_rand = args.filter_visual_rand cfg.filter_dataset_saving = args.filter_dataset_saving diff --git a/embodichain/lab/gym/utils/registration.py b/embodichain/lab/gym/utils/registration.py index 2fce236aa..d571317a0 100644 --- a/embodichain/lab/gym/utils/registration.py +++ b/embodichain/lab/gym/utils/registration.py @@ -37,6 +37,9 @@ if TYPE_CHECKING: from embodichain.lab.gym.envs import BaseEnv, EmbodiedEnvCfg + from embodichain.lab.gym.envs.expert_program import ( + SimulationExpertProgramRegistration, + ) _logger = logging.getLogger(__name__) @@ -48,12 +51,27 @@ def __init__( cls: Type[BaseEnv], max_episode_steps=None, default_kwargs: dict = None, + expert_program_registration: SimulationExpertProgramRegistration | None = None, ): """A specification for a Embodied environment.""" + if expert_program_registration is not None: + from embodichain.lab.gym.envs.expert_program import ( + SimulationExpertProgramRegistration, + ) + + if ( + type(expert_program_registration) + is not SimulationExpertProgramRegistration + ): + raise TypeError( + "expert_program_registration must be exactly " + "SimulationExpertProgramRegistration or None." + ) self.uid = uid self.cls = cls self.max_episode_steps = max_episode_steps self.default_kwargs = {} if default_kwargs is None else default_kwargs + self.expert_program_registration = expert_program_registration def make(self, **kwargs): _kwargs = self.default_kwargs.copy() @@ -76,7 +94,11 @@ def gym_spec(self): def register( - name: str, cls: Type[BaseEnv], max_episode_steps=None, default_kwargs: dict = None + name: str, + cls: Type[BaseEnv], + max_episode_steps=None, + default_kwargs: dict = None, + expert_program_registration: SimulationExpertProgramRegistration | None = None, ): """Register a Embodied environment.""" @@ -88,7 +110,11 @@ def register( if not (issubclass(cls, BaseEnv) or issubclass(cls, BaseEnv)): raise TypeError(f"Env {name} must inherit from BaseEnv or BaseEnv") REGISTERED_ENVS[name] = EnvSpec( - name, cls, max_episode_steps=max_episode_steps, default_kwargs=default_kwargs + name, + cls, + max_episode_steps=max_episode_steps, + default_kwargs=default_kwargs, + expert_program_registration=expert_program_registration, ) @@ -146,6 +172,16 @@ def make(env_id, **kwargs): return env +def get_env_spec(env_id: str) -> EnvSpec: + """Return one registered environment specification or fail closed.""" + if type(env_id) is not str or not env_id or env_id != env_id.strip(): + raise ValueError("env_id must be a non-empty string without outer whitespace.") + try: + return REGISTERED_ENVS[env_id] + except KeyError as exc: + raise KeyError(f"Env {env_id!r} not found in registry.") from exc + + def build_env(env_id: str, base_env_cfg: EmbodiedEnvCfg): """Create an environment from a registered env id. @@ -172,7 +208,14 @@ def make_vec(env_id, **kwargs): return env -def register_env(uid: str, max_episode_steps=None, override=False, **kwargs): +def register_env( + uid: str, + max_episode_steps=None, + override=False, + *, + expert_program_registration: SimulationExpertProgramRegistration | None = None, + **kwargs, +): """A decorator to register Embodied environments. Args: @@ -193,13 +236,28 @@ def register_env(uid: str, max_episode_steps=None, override=False, **kwargs): ) def _register_env(cls): - cls = register_env_function(cls, uid, override, max_episode_steps, **kwargs) + cls = register_env_function( + cls, + uid, + override, + max_episode_steps, + expert_program_registration=expert_program_registration, + **kwargs, + ) return cls return _register_env -def register_env_function(cls, uid, override=False, max_episode_steps=None, **kwargs): +def register_env_function( + cls, + uid, + override=False, + max_episode_steps=None, + *, + expert_program_registration: SimulationExpertProgramRegistration | None = None, + **kwargs, +): if uid in REGISTERED_ENVS: if override: from gymnasium.envs.registration import registry @@ -216,6 +274,7 @@ def register_env_function(cls, uid, override=False, max_episode_steps=None, **kw cls, max_episode_steps=max_episode_steps, default_kwargs=deepcopy(kwargs), + expert_program_registration=expert_program_registration, ) # Register for gym diff --git a/embodichain/lab/scripts/run_env.py b/embodichain/lab/scripts/run_env.py index 8c99f098f..78cce4723 100644 --- a/embodichain/lab/scripts/run_env.py +++ b/embodichain/lab/scripts/run_env.py @@ -32,9 +32,6 @@ import tqdm from embodichain.lab.gym.envs.demo import DemoEpisodeResult, execute_demo_episode -from embodichain.lab.gym.envs.expert_program.loader import ( - load_expert_program as _load_expert_program, -) from embodichain.lab.gym.envs.wrapper import ReplayWrapper from embodichain.lab.gym.utils.gym_utils import ( add_env_launcher_args_to_parser, @@ -858,9 +855,6 @@ def cli(argv: Sequence[str] | None = None) -> None: execute_init_hooks() env_cfg, gym_config, action_config = build_env_cfg_from_args(args) - expert_program_path = getattr(args, "expert_program", None) - if expert_program_path is not None: - env_cfg.expert_program = _load_expert_program(expert_program_path) if args.replay and args.replay_mode == "control": log_info("Dataset saving disabled for control replay mode.", color="green") diff --git a/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json index 32cf15513..cbb4ba140 100644 --- a/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json +++ b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json @@ -50,10 +50,7 @@ } } }, - "extensions": { - "grasp_samples": 10000, - "force_reannotate": false - } + "extensions": {} }, "robot": { "class_type": "URRobot", diff --git a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py index 6965c6f95..1a048fd41 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -41,6 +41,7 @@ ExpertProgramEnvironmentAdapter, ExpertProgramEnvironmentMixin, SimulationRigidObjectBinding, + SimulationExpertProgramRegistration, SimulationRobotSkillProfileBinding, SimulationSceneBinding, create_simulation_expert_program_adapter, @@ -53,6 +54,7 @@ FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, RecoveryPolicy, + TrackingPolicy, ) from embodichain.lab.sim.cfg import ( LightCfg, @@ -72,6 +74,7 @@ __all__ = [ "MultiSegmentsCubePickPlaceEnv", + "CUBE_EXPERT_PROGRAM_REGISTRATION", "create_cube_robot_profile_binding", "create_cube_scene_binding", ] @@ -133,7 +136,12 @@ def _create_default_robot_cfg() -> URRobotCfg: def _load_default_expert_program() -> ExpertProgramCfg: """Decode the packaged semantic program for direct instantiation.""" - return load_expert_program(get_config_path(CUBE_EXPERT_PROGRAM_PATH)) + program = load_expert_program( + get_config_path(CUBE_EXPERT_PROGRAM_PATH), + validation_context=CUBE_EXPERT_PROGRAM_REGISTRATION.catalog, + ) + CUBE_EXPERT_PROGRAM_REGISTRATION.catalog.preflight(program) + return program def _create_default_env_cfg() -> EmbodiedEnvCfg: @@ -167,10 +175,7 @@ def _create_default_env_cfg() -> EmbodiedEnvCfg: init_pos=(-0.42, -0.08, 0.5 * CUBE_SIZE), ) ] - cfg.extensions = { - "grasp_samples": 10000, - "force_reannotate": False, - } + cfg.extensions = {} cfg.events = { "settle_cube_on_reset": EventCfg( func=wait_for_dynamic_objects_to_settle, @@ -289,14 +294,28 @@ def create_cube_robot_profile_binding() -> SimulationRobotSkillProfileBinding: presets=( SkillPolicyPreset( "safe", - recovery_policy=RecoveryPolicy(tracking_error_threshold=0.08), + recovery_policy=RecoveryPolicy(), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.08, + terminal_max_abs_error=0.08, + ), ), ), default_preset="safe", ) -@register_env("MultiSegmentsCubePickPlace-v1", max_episode_steps=1200) +CUBE_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(), + robot_profile_binding=create_cube_robot_profile_binding(), +) + + +@register_env( + "MultiSegmentsCubePickPlace-v1", + max_episode_steps=1200, + expert_program_registration=CUBE_EXPERT_PROGRAM_REGISTRATION, +) class MultiSegmentsCubePickPlaceEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): """Repeatedly pick and place a cube from a semantic config program.""" @@ -307,11 +326,7 @@ def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs: Any) -> None: super().__init__(cfg, **kwargs) self._expert_program_adapter = create_simulation_expert_program_adapter( self, - scene_binding=create_cube_scene_binding( - grasp_samples=getattr(self, "grasp_samples", 10000), - force_reannotate=getattr(self, "force_reannotate", False), - ), - robot_profile_binding=create_cube_robot_profile_binding(), + registration=CUBE_EXPERT_PROGRAM_REGISTRATION, ) @property diff --git a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py index ff1166c67..2661ac884 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py +++ b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py @@ -36,6 +36,7 @@ ExpertProgramEnvironmentMixin, SimulationArticulationBinding, SimulationArticulationLinkBinding, + SimulationExpertProgramRegistration, SimulationRobotSkillProfileBinding, SimulationSceneBinding, create_simulation_expert_program_adapter, @@ -51,6 +52,7 @@ __all__ = [ "OpenDrawerEnv", + "OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION", "create_open_drawer_robot_profile_binding", "create_open_drawer_scene_binding", ] @@ -204,7 +206,17 @@ def create_open_drawer_robot_profile_binding() -> SimulationRobotSkillProfileBin ) -@register_env("OpenDrawer-v1", max_episode_steps=300) +OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_open_drawer_scene_binding(), + robot_profile_binding=create_open_drawer_robot_profile_binding(), +) + + +@register_env( + "OpenDrawer-v1", + max_episode_steps=300, + expert_program_registration=OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, +) class OpenDrawerEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): """Open a drawer through a configured semantic Expert Program.""" @@ -213,8 +225,7 @@ def __init__(self, cfg: EmbodiedEnvCfg, **kwargs: Any) -> None: super().__init__(cfg, **kwargs) self._expert_program_adapter = create_simulation_expert_program_adapter( self, - scene_binding=create_open_drawer_scene_binding(), - robot_profile_binding=create_open_drawer_robot_profile_binding(), + registration=OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, ) @property diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py new file mode 100644 index 000000000..1d6c07fa7 --- /dev/null +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -0,0 +1,625 @@ +# ---------------------------------------------------------------------------- +# 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 task-registration-owned Expert Program integration catalogs.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar + +import pytest + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramIntegrationCatalog, + ExpertProgramValidationError, + IntegrationFingerprintMismatch, + SimulationArticulationLinkBinding, + SimulationExpertProgramRegistration, + SimulationSceneBinding, + 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 ( + PLACE_ON_AFFORDANCE_CAPABILITY, + BoundSemanticCall, + HandOver, + HandOverPoseProvider, + HandOverPoseTargets, + OperateArticulation, + RelationTargetGrounder, + SemanticCallCatalog, + SceneAffordanceRef, + SceneEntityManifest, + SceneManifest, + SceneObjectRef, + SemanticRelationTarget, + builtin_semantic_call_catalog, +) +from embodichain_tasks.multi_segments.cube_pick_place import ( + CUBE_ROBOT_PROFILE_ID, + CUBE_SCENE_REGISTRY_ID, + create_cube_robot_profile_binding, + create_cube_scene_binding, +) +from embodichain_tasks.tableware.open_drawer import ( + DRAWER_HANDLE_AFFORDANCE_ID, + DRAWER_ROBOT_PROFILE_ID, + DRAWER_SCENE_REGISTRY_ID, + DRAWER_UID, + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, +) + + +class _CatalogRelationGrounder(RelationTargetGrounder): + """Typed relation-grounder sentinel for registration validation.""" + + capability: ClassVar[str] = "test.catalog_relation" + affordance_type: ClassVar[type[Affordance]] = Affordance + affordance_revision: ClassVar[str] = "test-v1" + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> object: + """Remain unreachable in provider-free catalog tests.""" + del relation, affordance, context + raise AssertionError("Catalog tests must not execute live providers.") + + +class _CatalogPlaceAffordance(Affordance): + """Typed provider-free payload marker for relation-linking tests.""" + + +@dataclass(frozen=True, slots=True) +class _CatalogHandOverPoseProvider(HandOverPoseProvider): + """Frozen declaration used to prove malicious drift detection.""" + + provider_id: ClassVar[str] = "test.catalog_handover" + transfer_height: float + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable in provider-free catalog tests.""" + del call, context, bound + raise AssertionError("Catalog tests must not execute live providers.") + + +class _SecondCatalogRelationGrounder(_CatalogRelationGrounder): + """Second stateless grounder used for ordering regressions.""" + + capability: ClassVar[str] = "test.catalog_relation.second" + affordance_revision: ClassVar[str] = "test-v2" + + +class _SecondCatalogHandOverPoseProvider(HandOverPoseProvider): + """Second stateless hand-over provider used for ordering regressions.""" + + provider_id: ClassVar[str] = "test.catalog_handover.second" + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable in provider-free catalog tests.""" + del call, context, bound + raise AssertionError("Catalog tests must not execute live providers.") + + +class _StatefulCatalogRelationGrounder(_CatalogRelationGrounder): + """Invalid non-dataclass provider with public instance state.""" + + capability: ClassVar[str] = "test.catalog_relation.stateful" + + def __init__(self) -> None: + self.height = 0.5 + + +class _PrivateSlotHandOverPoseProvider(HandOverPoseProvider): + """Invalid provider whose state is hidden behind a mangled slot name.""" + + __slots__ = ("__height",) + + provider_id: ClassVar[str] = "test.catalog_handover.private_slot" + + def __init__(self) -> None: + self.__height = 0.5 + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable because registration rejects this provider.""" + del call, context, bound + raise AssertionError("Rejected providers must never execute.") + + +class _InheritedCachedHandOverPoseProvider(_CatalogHandOverPoseProvider): + """Invalid non-dataclass subclass adding state to a frozen declaration.""" + + __slots__ = ("cache",) + + provider_id: ClassVar[str] = "test.catalog_handover.inherited_cache" + + def __init__(self) -> None: + super().__init__(transfer_height=0.5) + object.__setattr__(self, "cache", {}) + + +@dataclass(frozen=True, slots=True) +class _OpaqueHandOverPoseProvider(HandOverPoseProvider): + """Provider declaration containing an unsupported opaque nested value.""" + + provider_id: ClassVar[str] = "test.catalog_handover.opaque" + opaque: object + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable because fingerprinting rejects this provider.""" + del call, context, bound + raise AssertionError("Opaque providers must never reach runtime.") + + +def _program_payload( + *, + scene_registry: str = CUBE_SCENE_REGISTRY_ID, + runtime_preset: str = "safe", + object_id: str = "cube", +) -> dict[str, object]: + """Return one minimal catalog-linked program payload.""" + return { + "schema_version": 1, + "program_id": "catalog_pick", + "integration": { + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": scene_registry, + "runtime_preset": runtime_preset, + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": {"kind": "pick", "object": object_id}, + }, + } + + +def _registration() -> SimulationExpertProgramRegistration: + """Build one isolated provider-free task registration.""" + return SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + ) + + +def _operate_articulation_payload( + *, + target: str, + handle: str | None = None, +) -> dict[str, object]: + """Return one named drawer-operation program with an optional handle.""" + call: dict[str, object] = { + "kind": "operate_articulation", + "articulation": DRAWER_UID, + "target": target, + } + if handle is not None: + call["handle"] = handle + return { + "schema_version": 1, + "program_id": "catalog_open_drawer", + "integration": { + "robot_profile": DRAWER_ROBOT_PROFILE_ID, + "scene_registry": DRAWER_SCENE_REGISTRY_ID, + "runtime_preset": "safe", + }, + "targets": {}, + "program": {"kind": "invoke", "call": call}, + } + + +def _place_relation_catalog( + *, + install_grounder_key: bool, +) -> ExpertProgramIntegrationCatalog: + """Build one provider-free placement catalog with an optional grounder key.""" + base = _registration().catalog + support_ref = SceneObjectRef("support") + affordance_ref = SceneAffordanceRef("support_top") + scene = SceneManifest( + ( + SceneEntityManifest(ref=SceneObjectRef("cube")), + SceneEntityManifest( + ref=support_ref, + default_affordances={ + PLACE_ON_AFFORDANCE_CAPABILITY: affordance_ref, + }, + ), + SceneEntityManifest( + ref=affordance_ref, + parent=support_ref, + native_name="support_top_surface", + affordance_capabilities=frozenset({PLACE_ON_AFFORDANCE_CAPABILITY}), + affordance_payload_type=_CatalogPlaceAffordance, + affordance_revision="test-v1", + ), + ) + ) + grounder_keys = ( + frozenset( + { + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + _CatalogPlaceAffordance, + "test-v1", + ) + } + ) + if install_grounder_key + else frozenset() + ) + return ExpertProgramIntegrationCatalog( + scene_registry_id="relation_scene", + robot_profile_id=base.robot_profile_id, + scene=scene, + robot_profile=base.robot_profile, + call_catalog=base.call_catalog, + relation_grounder_keys=grounder_keys, + articulation_operation_targets={}, + settle_preset_ids=base.settle_preset_ids, + fingerprint="0" * 64, + _required_skills={}, + ) + + +def _place_relation_payload() -> dict[str, object]: + """Return one Place(on=object) program requiring relation grounding.""" + return { + "schema_version": 1, + "program_id": "catalog_place_relation", + "integration": { + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": "relation_scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "on": "support", + }, + }, + } + + +def test_catalog_decodes_compiles_and_links_without_simulation() -> None: + """All external references are linked before a simulation is available.""" + registration = _registration() + + program = decode_expert_program( + _program_payload(), + validation_context=registration.catalog, + ) + compiled = registration.catalog.preflight(program) + + assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "pick" + + +@pytest.mark.parametrize("validation_stage", ("decode", "preflight")) +def test_catalog_rejects_unknown_named_articulation_target_at_exact_path( + validation_stage: str, +) -> None: + """Unknown provider-owned target IDs fail before simulation startup.""" + catalog = OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION.catalog + payload = _operate_articulation_payload(target="does_not_exist") + + with pytest.raises(ExpertProgramValidationError) as error: + if validation_stage == "decode": + decode_expert_program(payload, validation_context=catalog) + else: + catalog.preflight(decode_expert_program(payload)) + + assert error.value.code == "unknown_articulation_operation_target" + assert error.value.path == ("program", "call", "target") + + +@pytest.mark.parametrize("handle", (None, DRAWER_HANDLE_AFFORDANCE_ID)) +def test_catalog_accepts_named_target_through_default_or_explicit_affordance( + handle: str | None, +) -> None: + """Both handle-selection forms resolve the same registered target table.""" + catalog = OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION.catalog + program = decode_expert_program( + _operate_articulation_payload(target="open", handle=handle), + validation_context=catalog, + ) + + compiled = catalog.preflight(program) + + call = tuple(compiled.iter_segments())[0].calls[0].call + assert type(call) is OperateArticulation + assert call.target == "open" + + +def test_catalog_owns_immutable_articulation_operation_target_metadata() -> None: + """Named target IDs are a read-only task-registration catalog surface.""" + targets = ( + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION.catalog.articulation_operation_targets + ) + + assert targets == {DRAWER_HANDLE_AFFORDANCE_ID: frozenset({"open"})} + with pytest.raises(TypeError): + targets[DRAWER_HANDLE_AFFORDANCE_ID] = frozenset() # type: ignore[index] + + +def test_catalog_rejects_linked_place_relation_without_exact_grounder() -> None: + """A linked affordance cannot defer a missing typed grounder to runtime.""" + catalog = _place_relation_catalog(install_grounder_key=False) + program = decode_expert_program( + _place_relation_payload(), + validation_context=catalog, + ) + + with pytest.raises(ExpertProgramValidationError) as error: + catalog.preflight(program) + + assert error.value.code == "relation_grounder_not_registered" + assert error.value.path == ("program", "call", "on") + + +def test_catalog_accepts_linked_place_relation_with_exact_grounder_key() -> None: + """The capability, payload type, and revision must all match exactly.""" + catalog = _place_relation_catalog(install_grounder_key=True) + program = decode_expert_program( + _place_relation_payload(), + validation_context=catalog, + ) + + compiled = catalog.preflight(program) + + assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "place" + + +@pytest.mark.parametrize( + ("overrides", "path"), + ( + ({"scene_registry": "other_scene"}, ("integration",)), + ({"runtime_preset": "unknown"}, ("integration",)), + ({"object_id": "unknown_object"}, ("program", "call", "object")), + ), +) +def test_catalog_rejects_unknown_references_at_decode_time( + overrides: dict[str, str], + path: tuple[str, ...], +) -> None: + """Invalid task integration references retain exact config paths.""" + registration = _registration() + + with pytest.raises(ExpertProgramValidationError) as error: + decode_expert_program( + _program_payload(**overrides), + validation_context=registration.catalog, + ) + + assert error.value.path == path + + +def test_scene_declare_rejects_orphan_link_without_simulation() -> None: + """Canonical topology failures do not reach native entity lookup.""" + binding = SimulationSceneBinding( + registry_id="orphan_scene", + links=( + SimulationArticulationLinkBinding( + entity_id="handle", + articulation_id="missing_drawer", + native_link_name="handle_link", + ), + ), + ) + + with pytest.raises(KeyError, match="missing_drawer"): + binding.declare() + + +def test_fingerprint_is_stable_for_equivalent_declarations() -> None: + """Fresh equivalent registrations produce the same canonical digest.""" + left = _registration() + right = _registration() + + assert left.fingerprint == right.fingerprint + assert len(left.fingerprint) == 64 + + +def test_fingerprint_is_independent_of_catalog_and_provider_insertion_order() -> None: + """Semantically equivalent unordered registration inputs hash identically.""" + descriptors = tuple(builtin_semantic_call_catalog().descriptors.values()) + first_relation = _CatalogRelationGrounder() + second_relation = _SecondCatalogRelationGrounder() + first_handover = _CatalogHandOverPoseProvider(transfer_height=0.6) + second_handover = _SecondCatalogHandOverPoseProvider() + common = { + "scene_binding": create_cube_scene_binding(grasp_samples=32), + "robot_profile_binding": create_cube_robot_profile_binding(), + } + forward = SimulationExpertProgramRegistration( + **common, + call_catalog=SemanticCallCatalog(descriptors), + relation_grounders=(first_relation, second_relation), + handover_pose_providers=(first_handover, second_handover), + ) + reversed_registration = SimulationExpertProgramRegistration( + **common, + call_catalog=SemanticCallCatalog(tuple(reversed(descriptors))), + relation_grounders=(second_relation, first_relation), + handover_pose_providers=(second_handover, first_handover), + ) + + assert forward.fingerprint == reversed_registration.fingerprint + + +def test_fingerprint_owns_provider_ids_and_declarative_fields() -> None: + """Provider identity and dataclass configuration are registration data.""" + provider = _CatalogHandOverPoseProvider(transfer_height=0.6) + registration = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=(_CatalogRelationGrounder(),), + handover_pose_providers=(provider,), + ) + changed_value = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=(_CatalogRelationGrounder(),), + handover_pose_providers=(_CatalogHandOverPoseProvider(transfer_height=0.7),), + ) + + assert registration.handover_pose_providers == (provider,) + assert registration.fingerprint != changed_value.fingerprint + object.__setattr__(provider, "transfer_height", 0.8) + with pytest.raises(IntegrationFingerprintMismatch, match="changed"): + registration.assert_unchanged() + + +def test_fingerprint_rejects_opaque_nested_declaration_values() -> None: + """Unknown nested values cannot silently collapse to their Python type.""" + with pytest.raises(TypeError, match="unsupported value type"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + handover_pose_providers=(_OpaqueHandOverPoseProvider(opaque=object()),), + ) + + +def test_registration_rejects_duplicate_provider_keys_and_ids() -> None: + """Provider lookup tables remain unambiguous before simulation startup.""" + common = { + "scene_binding": create_cube_scene_binding(grasp_samples=32), + "robot_profile_binding": create_cube_robot_profile_binding(), + } + + with pytest.raises(ValueError, match="Duplicate relation grounder key"): + SimulationExpertProgramRegistration( + **common, + relation_grounders=( + _CatalogRelationGrounder(), + _CatalogRelationGrounder(), + ), + ) + with pytest.raises(ValueError, match="Duplicate handover pose provider"): + SimulationExpertProgramRegistration( + **common, + handover_pose_providers=( + _CatalogHandOverPoseProvider(transfer_height=0.6), + _CatalogHandOverPoseProvider(transfer_height=0.7), + ), + ) + + +def test_registration_requires_immutable_provider_tuples() -> None: + """Mutable provider containers cannot enter task registration metadata.""" + with pytest.raises(TypeError, match="relation_grounders must be an exact tuple"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=[_CatalogRelationGrounder()], # type: ignore[arg-type] + ) + with pytest.raises( + TypeError, + match="handover_pose_providers must be an exact tuple", + ): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + handover_pose_providers=[ # type: ignore[arg-type] + _CatalogHandOverPoseProvider(transfer_height=0.6) + ], + ) + + +@pytest.mark.parametrize( + ("field_name", "provider"), + ( + ("relation_grounders", _StatefulCatalogRelationGrounder()), + ("handover_pose_providers", _PrivateSlotHandOverPoseProvider()), + ( + "handover_pose_providers", + _InheritedCachedHandOverPoseProvider(), + ), + ), +) +def test_registration_rejects_stateful_non_dataclass_providers( + field_name: str, + provider: object, +) -> None: + """Public and name-mangled provider state cannot evade fingerprinting.""" + kwargs = {field_name: (provider,)} + + with pytest.raises(TypeError, match="Use a frozen dataclass"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + **kwargs, + ) + + +def test_nested_declaration_drift_is_detected_before_live_build() -> None: + """Mutable nested config cannot silently change a registered binding.""" + registration = _registration() + generator_cfg = registration.scene_binding.antipodal_grasps[0].generator_cfg + assert generator_cfg is not None + generator_cfg.antipodal_sampler_cfg.n_sample = 64 + + with pytest.raises(IntegrationFingerprintMismatch, match="changed"): + registration.assert_unchanged() + + +def test_env_spec_keeps_typed_registration_out_of_gym_kwargs() -> None: + """The integration catalog is metadata, not a duplicated Gym config source.""" + + class _Environment: + pass + + registration = _registration() + spec = EnvSpec( + "CatalogTest-v1", + _Environment, + default_kwargs={"physical_option": 3}, + expert_program_registration=registration, + ) + + assert spec.expert_program_registration is registration + assert spec.gym_spec.kwargs == {"physical_option": 3} diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 119ecca39..c52e18b88 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -48,6 +48,7 @@ InvokeCfg, RobotResourceBinding, SharedTickSceneProvider, + SimulationExpertProgramRegistration, SimulationExpertProgramFactory, SimulationPlanningObservationProvider, SimulationRigidObjectBinding, @@ -72,6 +73,7 @@ PlanningContext, StateDelta, TaskState, + TrackingPolicy, ) from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget @@ -102,7 +104,6 @@ SemanticObjectTarget, SemanticPose, SemanticRelationTarget, - SemanticValidationError, SkillPolicyPreset, ) from embodichain.lab.sim.skills.effects import ( @@ -685,9 +686,6 @@ class _ForwardedHandOverPoseProvider(HandOverPoseProvider): provider_id: ClassVar[str] = "test.handover_pose" - def __init__(self) -> None: - self.calls = 0 - def resolve( self, call: HandOver, @@ -697,7 +695,6 @@ def resolve( ) -> HandOverPoseTargets: """Return owned direct targets without embedding task-side motion code.""" del call, context, bound - self.calls += 1 pose = SemanticPose( position=(0.0, 0.0, 0.5), quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), @@ -848,6 +845,10 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: SkillPolicyPreset( "safe", motion_policy=MotionPolicy(control_dt=0.01), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.037, + terminal_max_abs_error=0.019, + ), ), ), default_preset="safe", @@ -977,8 +978,10 @@ def _factory() -> tuple[SimulationExpertProgramFactory, _Robot]: SimulationExpertProgramFactory( simulation, # type: ignore[arg-type] robot, # type: ignore[arg-type] - SimulationSceneBinding(registry_id="scene"), - _profile_binding(), + SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + ), step_dt=_STEP_DT, motion_generator_factory=lambda: _motion_generator(robot), ), @@ -1114,8 +1117,10 @@ def _evidence_adapter_runtime() -> tuple[ factory = SimulationExpertProgramFactory( simulation, # type: ignore[arg-type] robot, # type: ignore[arg-type] - scene_binding, - _evidence_profile_binding(), + SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=_evidence_profile_binding(), + ), step_dt=_STEP_DT, motion_generator_factory=lambda: _motion_generator(robot), ) @@ -1525,12 +1530,16 @@ def _assert_invocation_equivalent( def test_simulation_factory_aligns_every_motion_policy_to_gym_step() -> None: - """The environment cadence replaces unrelated preset fallback timing.""" + """Cadence alignment preserves the exact registered tracking contract.""" factory, _ = _factory() profile = factory.create_robot_skill_profile() assert profile.presets["safe"].motion_policy.control_dt == pytest.approx(_STEP_DT) + assert profile.presets["safe"].tracking_policy == TrackingPolicy.joint_position( + in_flight_max_abs_error=0.037, + terminal_max_abs_error=0.019, + ) def test_mllm_config_and_atomic_skills_share_invocations_and_verified_results( @@ -1689,8 +1698,8 @@ def test_simulation_factory_returns_exact_environment_adapter() -> None: assert factory.segment_policy_port is not None -def test_simulation_helper_forwards_semantic_grounding_extensions() -> None: - """Both explicit grounding seams reach the runtime compiler unchanged.""" +def test_simulation_helper_consumes_registered_semantic_grounding_extensions() -> None: + """Both registration-owned grounding seams reach the compiler unchanged.""" robot = _Robot() environment = SimpleNamespace( sim=_Simulation(robot), @@ -1701,11 +1710,13 @@ def test_simulation_helper_forwards_semantic_grounding_extensions() -> None: handover_provider = _ForwardedHandOverPoseProvider() adapter = create_simulation_expert_program_adapter( environment, # type: ignore[arg-type] - scene_binding=SimulationSceneBinding(registry_id="scene"), - robot_profile_binding=_profile_binding(), + registration=SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + relation_grounders=(relation_grounder,), + handover_pose_providers=(handover_provider,), + ), motion_generator_factory=lambda: _motion_generator(robot), - relation_grounders=(relation_grounder,), - handover_pose_providers=(handover_provider,), ) assembly = adapter.assemble_runtime( @@ -1722,41 +1733,35 @@ def test_simulation_helper_forwards_semantic_grounding_extensions() -> None: ) -def test_simulation_helper_handover_preflight_is_fail_closed_by_default() -> None: - """Selecting a provider ID does not infer or auto-install an implementation.""" - environment, scene_binding, profile_binding = _handover_helper_inputs() - robot = environment.robot - adapter = create_simulation_expert_program_adapter( - environment, # type: ignore[arg-type] - scene_binding=scene_binding, - robot_profile_binding=profile_binding, - motion_generator_factory=lambda: _motion_generator(robot), - ) - compiled = adapter.compile(_handover_program()) +def test_handover_registration_is_fail_closed_without_selected_provider() -> None: + """A profile-selected provider must be installed before simulation startup.""" + _, scene_binding, profile_binding = _handover_helper_inputs() - with pytest.raises(SemanticValidationError) as error: - adapter.create_bridge(compiled) - - assert error.value.diagnostic.code == "handover_grounding_provider_not_installed" + with pytest.raises(ValueError, match="selects handover pose provider"): + SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=profile_binding, + ) -def test_simulation_helper_forwards_handover_provider_to_preflight() -> None: - """An explicitly supplied embodiment provider satisfies standard preflight.""" +def test_simulation_helper_uses_registered_handover_provider_for_preflight() -> None: + """A registration-owned embodiment provider satisfies standard preflight.""" environment, scene_binding, profile_binding = _handover_helper_inputs() robot = environment.robot provider = _ForwardedHandOverPoseProvider() adapter = create_simulation_expert_program_adapter( environment, # type: ignore[arg-type] - scene_binding=scene_binding, - robot_profile_binding=profile_binding, + registration=SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=profile_binding, + handover_pose_providers=(provider,), + ), motion_generator_factory=lambda: _motion_generator(robot), - handover_pose_providers=(provider,), ) bridge = adapter.create_bridge(adapter.compile(_handover_program())) assert bridge is not None - assert provider.calls == 0 def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joints() -> ( @@ -1789,8 +1794,10 @@ def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joint adapter = create_simulation_expert_program_adapter( environment, # type: ignore[arg-type] - scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), - robot_profile_binding=profile_binding, + registration=SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=profile_binding, + ), motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] endpoint_adapters={_MobileEndpoint: _MobileEndpointAdapter()}, runtime_transports=(_MobileTransportEncoder(),), diff --git a/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py b/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py index a54203df9..6e6fe9495 100644 --- a/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py +++ b/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py @@ -40,6 +40,7 @@ from embodichain_tasks.multi_segments.cube_pick_place import ( # noqa: E402 CUBE_ROBOT_PROFILE_ID, CUBE_SCENE_REGISTRY_ID, + CUBE_EXPERT_PROGRAM_REGISTRATION, MultiSegmentsCubePickPlaceEnv, _create_default_env_cfg, create_cube_robot_profile_binding, @@ -70,6 +71,8 @@ def test_registered_task_uses_shared_expert_program_mixin() -> None: spec = REGISTERED_ENVS["MultiSegmentsCubePickPlace-v1"] assert spec.cls is MultiSegmentsCubePickPlaceEnv assert spec.max_episode_steps == 1200 + assert spec.expert_program_registration is CUBE_EXPERT_PROGRAM_REGISTRATION + assert "expert_program_registration" not in spec.default_kwargs assert issubclass(MultiSegmentsCubePickPlaceEnv, ExpertProgramEnvironmentMixin) assert issubclass(MultiSegmentsCubePickPlaceEnv, EmbodiedEnv) @@ -82,11 +85,7 @@ def test_gym_config_selects_packaged_expert_program() -> None: assert payload["expert_program_path"] == ( "../../expert_program/multi_segments/repeated_cube_pick_place.yaml" ) - extensions = payload["env"]["extensions"] - assert extensions == { - "grasp_samples": 10000, - "force_reannotate": False, - } + assert payload["env"]["extensions"] == {} settle = payload["env"]["events"]["settle_cube_on_reset"] assert settle["func"] == "wait_for_dynamic_objects_to_settle" assert settle["mode"] == "reset" @@ -130,7 +129,10 @@ def test_robot_profile_calibrates_physical_tracking_tolerance() -> None: binding = create_cube_robot_profile_binding() assert binding.presets[0].preset_id == "safe" - assert binding.presets[0].recovery_policy.tracking_error_threshold == 0.08 + tracking = binding.presets[0].tracking_policy + assert tracking.in_flight is not None + assert tracking.in_flight.metrics[0].tolerance == 0.08 + assert tracking.terminal.metrics[0].tolerance == 0.08 def test_task_initialization_delegates_to_shared_simulation_factory( @@ -162,14 +164,16 @@ def fake_create_adapter(environment, **kwargs): assert env.expert_program_adapter is adapter assert captured["environment"] is env + registration = captured["registration"] + assert registration is CUBE_EXPERT_PROGRAM_REGISTRATION assert ( - captured["scene_binding"] - .antipodal_grasps[0] - .generator_cfg.antipodal_sampler_cfg.n_sample - == 48 + registration.scene_binding.antipodal_grasps[ + 0 + ].generator_cfg.antipodal_sampler_cfg.n_sample + == 10000 ) - assert captured["scene_binding"].antipodal_grasps[0].force_reannotate is True - assert captured["robot_profile_binding"].profile_id == CUBE_ROBOT_PROFILE_ID + assert registration.scene_binding.antipodal_grasps[0].force_reannotate is False + assert registration.robot_profile_binding.profile_id == CUBE_ROBOT_PROFILE_ID def test_task_config_compiles_through_real_simulation_factory( diff --git a/tests/gym/envs/tasks/test_open_drawer.py b/tests/gym/envs/tasks/test_open_drawer.py index 81893c5a0..725d3c77d 100644 --- a/tests/gym/envs/tasks/test_open_drawer.py +++ b/tests/gym/envs/tasks/test_open_drawer.py @@ -43,6 +43,7 @@ DRAWER_OPEN_POSITION, DRAWER_ROBOT_PROFILE_ID, DRAWER_UID, + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, OpenDrawerEnv, create_open_drawer_scene_binding, ) @@ -68,6 +69,8 @@ def test_registered_drawer_task_uses_shared_expert_program_mixin() -> None: spec = REGISTERED_ENVS["OpenDrawer-v1"] assert spec.cls is OpenDrawerEnv + assert spec.expert_program_registration is OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION + assert "expert_program_registration" not in spec.default_kwargs assert issubclass(OpenDrawerEnv, ExpertProgramEnvironmentMixin) assert issubclass(OpenDrawerEnv, EmbodiedEnv) assert "create_demo_action_list" not in OpenDrawerEnv.__dict__ @@ -144,8 +147,10 @@ def fake_create_adapter(environment, **kwargs): assert env.expert_program_adapter is adapter assert captured["environment"] is env - assert captured["scene_binding"].links[0].native_link_name == "handle_xpos" - assert captured["robot_profile_binding"].profile_id == DRAWER_ROBOT_PROFILE_ID + registration = captured["registration"] + assert registration is OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION + assert registration.scene_binding.links[0].native_link_name == "handle_xpos" + assert registration.robot_profile_binding.profile_id == DRAWER_ROBOT_PROFILE_ID def test_task_config_compiles_through_real_simulation_factory( diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index db3119281..c0cd8ee2d 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -27,6 +27,7 @@ from tensordict import TensorDict +from embodichain.lab.gym.envs.expert_program import IntegrationFingerprintMismatch from embodichain.lab.gym.utils.gym_utils import ( add_env_launcher_args_to_parser, build_env_cfg_from_args, @@ -39,6 +40,11 @@ ) from embodichain.lab.sim.robots import URRobotCfg from embodichain.utils.utility import load_config, save_config +from embodichain_tasks.multi_segments.cube_pick_place import ( + CUBE_EXPERT_PROGRAM_REGISTRATION, + CUBE_ROBOT_PROFILE_ID, + CUBE_SCENE_REGISTRY_ID, +) class TestInitRolloutBufferFromConfig: @@ -513,7 +519,7 @@ class TestConfigToCfgFromFile: def _minimal_gym_config() -> dict[str, object]: """Return a minimal config that reaches the generic parser.""" return { - "id": "EmbodiedEnv-v1", + "id": "MultiSegmentsCubePickPlace-v1", "env": {}, "robot": { "class_type": "URRobot", @@ -529,9 +535,9 @@ def _expert_program_payload() -> dict[str, object]: "schema_version": 1, "program_id": "configured_pick", "integration": { - "robot_profile": "default_robot", - "scene_registry": "default_scene", - "runtime_preset": "default_runtime", + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": CUBE_SCENE_REGISTRY_ID, + "runtime_preset": "safe", }, "targets": {}, "program": { @@ -585,7 +591,7 @@ def test_expert_program_path_is_resolved_from_gym_config_source( ) assert cfg.expert_program.program_id == "configured_pick" - assert cfg.expert_program.integration.scene_registry == "default_scene" + assert cfg.expert_program.integration.scene_registry == CUBE_SCENE_REGISTRY_ID def test_build_env_cfg_loads_source_relative_expert_program( self, @@ -621,6 +627,81 @@ def test_build_env_cfg_loads_source_relative_expert_program( assert cfg.expert_program.program_id == "configured_pick" + def test_cli_program_override_is_selected_and_loaded_once( + self, + tmp_path, + monkeypatch, + ) -> None: + """The CLI override replaces the Gym path at the single loader boundary.""" + from embodichain.lab.gym.envs.expert_program import loader + + gym_path = tmp_path / "gym_config.json" + override_path = tmp_path / "override.yaml" + save_config(override_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = "must_not_be_loaded.yaml" + save_config(gym_path, config) + args = argparse.Namespace( + gym_config=str(gym_path), + expert_program=str(override_path), + num_envs=1, + device="cpu", + headless=True, + renderer=None, + gpu_id=0, + arena_space=2.0, + max_episodes=None, + filter_visual_rand=False, + filter_dataset_saving=False, + preview=False, + action_config=None, + ) + calls: list[str] = [] + original = loader.load_expert_program + + def load_once(path, **kwargs): + calls.append(str(path)) + return original(path, **kwargs) + + monkeypatch.setattr(loader, "load_expert_program", load_once) + + cfg, _, _ = build_env_cfg_from_args(args) + + assert cfg.expert_program.program_id == "configured_pick" + assert calls == [str(override_path)] + + def test_registration_drift_fails_before_program_loader( + self, + tmp_path, + monkeypatch, + ) -> None: + """The config boundary checks registration integrity before file loading.""" + from embodichain.lab.gym.envs.expert_program import loader + + program_path = tmp_path / "program.yaml" + save_config(program_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = str(program_path) + generator_cfg = CUBE_EXPERT_PROGRAM_REGISTRATION.scene_binding.antipodal_grasps[ + 0 + ].generator_cfg + assert generator_cfg is not None + sampler_cfg = generator_cfg.antipodal_sampler_cfg + monkeypatch.setattr(sampler_cfg, "n_sample", sampler_cfg.n_sample + 1) + loader_calls: list[str] = [] + + def unexpected_load(path, **kwargs): + del kwargs + loader_calls.append(str(path)) + raise AssertionError("Drift must fail before program loading.") + + monkeypatch.setattr(loader, "load_expert_program", unexpected_load) + + with pytest.raises(IntegrationFingerprintMismatch, match="changed"): + config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + assert loader_calls == [] + def test_config_to_cfg_uses_cwd_without_source_path( self, tmp_path, diff --git a/tests/lab/scripts/test_run_env.py b/tests/lab/scripts/test_run_env.py index 788a89f9b..1b0b1d1d8 100644 --- a/tests/lab/scripts/test_run_env.py +++ b/tests/lab/scripts/test_run_env.py @@ -24,11 +24,13 @@ import torch from embodichain.lab.gym.envs.demo import DemoEpisodeResult +from embodichain.lab.gym.envs.expert_program.loader import ( + load_expert_program as _load_expert_program, +) from embodichain.lab.gym.utils.gym_utils import merge_args_with_gym_config from embodichain.lab.scripts import run_env from embodichain.lab.scripts.run_env import ( _create_parser, - _load_expert_program, _run_replay_control_loop, generate_function, ) @@ -549,7 +551,7 @@ def test_cli_aborts_before_closing_environment_once(monkeypatch) -> None: assert env.events == [abort_event, abort_event, ("close", None)] -def test_cli_injects_decoded_expert_program_before_environment_creation( +def test_cli_uses_program_already_loaded_by_config_builder( monkeypatch, ) -> None: """The CLI attaches the strict program config to the environment config.""" @@ -569,16 +571,13 @@ def test_cli_injects_decoded_expert_program_before_environment_creation( monkeypatch.setattr(run_env, "_create_parser", lambda: parser) monkeypatch.setattr(run_env, "discover_task_packages", lambda: None) monkeypatch.setattr(run_env, "execute_init_hooks", lambda: None) - monkeypatch.setattr( - run_env, - "build_env_cfg_from_args", - lambda parsed_args: (env_cfg, {"id": GYM_ID}, {}), - ) - monkeypatch.setattr( - run_env, - "_load_expert_program", - MagicMock(return_value=decoded_program), - ) + + def build(parsed_args): + assert parsed_args is args + env_cfg.expert_program = decoded_program + return env_cfg, {"id": GYM_ID}, {} + + monkeypatch.setattr(run_env, "build_env_cfg_from_args", build) monkeypatch.setattr(run_env.gymnasium, "make", make) monkeypatch.setattr(run_env, "main", lambda *args, **kwargs: None) monkeypatch.setattr(