From 974368f92967ce7ebf7d148a1c67dfc47333092c Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:18:03 +0800 Subject: [PATCH] feat(sim): add semantic runtime effects and parallelism --- .../embodichain.lab.sim.skills.rst | 104 + .../atomic_actions/robot_skill_profiles.md | 49 + embodichain/lab/sim/skills/__init__.py | 216 +- embodichain/lab/sim/skills/calls.py | 148 +- embodichain/lab/sim/skills/compiler.py | 751 +++++- embodichain/lab/sim/skills/effects.py | 2250 ++++++++++++++++ embodichain/lab/sim/skills/evidence.py | 1467 +++++++++++ embodichain/lab/sim/skills/integration.py | 152 +- embodichain/lab/sim/skills/parallel.py | 354 +++ .../lab/sim/skills/parallel_runtime.py | 1487 +++++++++++ embodichain/lab/sim/skills/profiles.py | 154 +- embodichain/lab/sim/skills/runtime.py | 2294 +++++++++++++++++ embodichain/lab/sim/skills/scene.py | 265 +- .../sim/skills/test_articulation_semantics.py | 594 +++++ tests/sim/skills/test_calls.py | 20 + tests/sim/skills/test_compiler.py | 480 +++- ...o_semantic_runtime_dynamic_recovery_gpu.py | 375 +++ tests/sim/skills/test_effects.py | 863 +++++++ tests/sim/skills/test_evidence.py | 666 +++++ tests/sim/skills/test_integration.py | 363 ++- tests/sim/skills/test_parallel.py | 251 ++ tests/sim/skills/test_parallel_runtime.py | 1264 +++++++++ tests/sim/skills/test_profiles.py | 111 + tests/sim/skills/test_runtime.py | 910 +++++++ tests/sim/skills/test_scene.py | 111 +- 25 files changed, 15640 insertions(+), 59 deletions(-) create mode 100644 embodichain/lab/sim/skills/effects.py create mode 100644 embodichain/lab/sim/skills/evidence.py create mode 100644 embodichain/lab/sim/skills/parallel.py create mode 100644 embodichain/lab/sim/skills/parallel_runtime.py create mode 100644 embodichain/lab/sim/skills/runtime.py create mode 100644 tests/sim/skills/test_articulation_semantics.py create mode 100644 tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py create mode 100644 tests/sim/skills/test_effects.py create mode 100644 tests/sim/skills/test_evidence.py create mode 100644 tests/sim/skills/test_parallel.py create mode 100644 tests/sim/skills/test_parallel_runtime.py create mode 100644 tests/sim/skills/test_runtime.py diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst index 1b3022fe8..979bc9314 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -43,6 +43,38 @@ embodichain.lab.sim.skills UnsupportedSkillError AmbiguousSkillBindingError + .. rubric:: Semantic calls and runtime + + .. autosummary:: + + SemanticCallSpec + SemanticPose + Pick + Place + HandOver + OperateArticulation + RegisteredSemanticCall + SemanticCallCatalog + SemanticSkillCompiler + AtomicSkills + SkillRuntime + SkillResult + SkillCallTrace + SkillPlanAttemptTrace + SkillEffectTrace + + .. rubric:: Effects, evidence, and parallel execution + + .. autosummary:: + + SemanticEffectSpec + EffectMonitorRef + EffectMonitor + EffectEvidenceCollector + ParallelSkillRuntime + ParallelSkillResult + ParallelCommandSafetyValidator + .. currentmodule:: embodichain.lab.sim.skills Robot resources and profiles @@ -99,6 +131,78 @@ Profile errors .. autoclass:: AmbiguousSkillBindingError +Semantic calls and runtime +-------------------------- + +.. autoclass:: SemanticCallSpec + :members: + +.. autoclass:: SemanticPose + :members: + +.. autoclass:: Pick + :members: + +.. autoclass:: Place + :members: + +.. autoclass:: HandOver + :members: + +.. autoclass:: OperateArticulation + :members: + +.. autoclass:: RegisteredSemanticCall + :members: + +.. autoclass:: SemanticCallCatalog + :members: + +.. autoclass:: SemanticSkillCompiler + :members: + +.. autoclass:: AtomicSkills + :members: + +.. autoclass:: SkillRuntime + :members: + +.. autoclass:: SkillResult + :members: + +.. autoclass:: SkillCallTrace + :members: + +.. autoclass:: SkillPlanAttemptTrace + :members: + +.. autoclass:: SkillEffectTrace + :members: + +Effects, evidence, and parallel execution +----------------------------------------- + +.. autoclass:: SemanticEffectSpec + :members: + +.. autoclass:: EffectMonitorRef + :members: + +.. autoclass:: EffectMonitor + :members: + +.. autoclass:: EffectEvidenceCollector + :members: + +.. autoclass:: ParallelSkillRuntime + :members: + +.. autoclass:: ParallelSkillResult + :members: + +.. autoclass:: ParallelCommandSafetyValidator + :members: + Registry and provider --------------------- diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md index f03d453e2..66488542d 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -157,6 +157,55 @@ One-dimensional joint-position commands are broadcast across environments. Their last dimension must equal the resolved endpoint's degree of freedom. Use invocation-level command overrides for object- or environment-specific values. +## Safe preset and dynamic collision worlds + +When the authoritative scene registry declares dynamic collision entities and +`safe` is reachable through the integration-wide, per-skill, or +profile-default preset selection, semantic integration validates that path +conservatively during binding. The `safe` preset must use `motion_gen`, and the +active motion generator must explicitly support dynamic collision worlds; +otherwise binding fails before provider observation, planning, or command +emission. + +A linked call receives an effective immutable preset snapshot with +`DynamicCollisionMode.REQUIRED`; the source profile preset is not mutated. +Other presets, and scenes without dynamic collision entities, retain their +configured collision mode. + +## Select semantic effect monitors with the preset + +A {class}`SkillPolicyPreset` owns one coherent runtime choice: planning and +recovery policy, runner cadence, and the exact semantic-effect monitors used to +confirm physical postconditions. `effect_monitors` maps a semantic call ID to a +versioned {class}`EffectMonitorRef`. Its parameters are bounded declarative +values; executable objects, tensors, cyclic containers, and non-finite numbers +are rejected. + +When `effect_monitors` is omitted, the preset selects the built-in +pose-relation hysteresis monitor for `pick`, `place`, and `hand_over`. Passing an +explicit empty mapping disables that default; static analysis then reports +`missing_effect_monitor` if a curated effectful call selects that preset. A +manifest also rejects monitor entries whose semantic ID is absent from its call +catalog, and the compiler requires the exact monitor ID/revision and validates +its parameters before grounding. + +The semantic compiler creates a fresh monitor for every grounded call. Pick +expects one attached destination relation, place one detached source relation, +and handover both source-detached and destination-attached relations in the +same observation. The monitor compares fresh backend evidence with owned +object-to-endpoint baselines; it never treats the planned `StateDelta` or +current `TaskState` as proof that the physical effect occurred. Invalid or +missing per-environment evidence remains unresolved. Consecutive-sample state +survives request-mask shrinkage within one attempt and resets when recovery +installs a new attempt. + +```{note} +The monitor contract is backend-neutral. Simulation, hardware perception, or +controller feedback supplies typed pose-relation evidence. The semantic +runtime adapter that connects that evidence to `ExecutionRunner` is separate +from the profile and monitor configuration. +``` + ## Bind, discover, and resolve Pass the profile to diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index d3b7c2ea2..576d2f24a 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -21,6 +21,7 @@ from .calls import ( DeclarativeValue, HandOver, + OperateArticulation, Pick, Place, PlaceRelationTarget, @@ -39,7 +40,6 @@ RegisteredSemanticLowerer, RelationTargetGrounder, SemanticEffectDependency, - SemanticEffectKind, SemanticHandOverTarget, SemanticLowering, SemanticObjectTarget, @@ -47,6 +47,74 @@ SemanticSkillCompiler, SemanticWorkflow, ) +from .effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + CompositeEffectMonitor, + CompositeEffectMonitorCfg, + CompositeEffectMonitorFactory, + ControlPartEvidenceAddress, + CoordinatedHeldObjectCleanupExpectation, + EffectClause, + EffectEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectMonitor, + EffectMonitorDecision, + EffectMonitorFactory, + EffectMonitorParam, + EffectMonitorRef, + EffectMonitorRegistry, + EffectStateExpectation, + FORCE_EFFECT_CHANNEL, + HeldObjectRelation, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + JointStateEvidenceBatch, + POSE_RELATION_EFFECT_CHANNEL, + PoseRelationClause, + PoseRelationEvidenceBatch, + PoseRelationExpectation, + ScalarEffectClause, + ScalarEffectEvidenceBatch, + ScalarEvidenceKind, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, + SymbolicStateDomain, + SymbolicStateKey, +) +from .evidence import ( + ArticulationJointObservationCallback, + BinaryEffectEvidenceQuery, + BinaryEffectObservation, + BinaryObservationCallback, + ControlPartRobotEvidenceSource, + ControlPartSimulationEvidenceProvider, + EffectEvidenceCollectionContext, + EffectEvidenceCollector, + EffectEvidenceProvider, + EffectEvidenceProviderRegistry, + EffectEvidenceQuery, + EffectEvidenceQueryValue, + JointStateEvidenceQuery, + JointStateObservation, + PoseRelationEvidenceQuery, + ScalarEffectEvidenceQuery, + ScalarEffectObservation, + ScalarObservationCallback, + SceneArticulationEvidenceProvider, + build_effect_evidence_queries, +) from .integration import ( BoundSemanticCall, BoundSemanticIntegration, @@ -58,6 +126,29 @@ SemanticIntegrationManifest, SemanticValidationError, ) +from .parallel import ( + ParallelBarrierUpdate, + ParallelBranchPlan, + ParallelConflictError, + ParallelStateConflictError, + ParallelTimingError, + ParallelTimingPolicy, + align_parallel_commands, + merge_parallel_effects, + resolve_parallel_barrier, + validate_parallel_claims, +) +from .parallel_runtime import ( + ParallelBranchStaticAnalysis, + ParallelBranchRuntime, + ParallelCommandSafetyValidator, + ParallelLaneCommandSink, + ParallelRuntimeBranch, + ParallelSafetyError, + ParallelSkillResult, + ParallelSkillRuntime, + analyze_parallel_branches, +) from .profiles import ( AmbiguousSkillBindingError, BoundRobotSkillProfile, @@ -78,12 +169,17 @@ UnsupportedSkillError, ) from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, AmbiguousSceneAffordanceError, + ArticulationJointEvidenceAddress, GRASP_AFFORDANCE_CAPABILITY, PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, RegistrySceneProvider, SceneAffordanceRef, + SceneArticulationJointStateProvider, SceneArticulationRef, SceneCollisionRole, SceneCollisionWorldMode, @@ -98,30 +194,117 @@ SceneRegistry, UnsupportedSceneAffordanceError, ) +from .runtime import ( + AtomicSkills, + EffectEvidenceCollectorPort, + ResolvedCorePolicyTrace, + SkillCallTrace, + SkillEndpointBindingTrace, + SkillEffectTrace, + SkillFailure, + SkillPlanAttemptTrace, + SkillResult, + SkillRuntime, + SkillRuntimeProvider, + SkillScene, + SkillStatus, + task_state_to_metadata, +) __all__ = [ + "ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY", "AmbiguousSceneAffordanceError", "AmbiguousSkillBindingError", "AnalyzedSemanticCall", + "ArticulationJointEvidenceAddress", + "ArticulationJointObservationCallback", + "ArticulationJointStateExpectation", + "AtomicSkills", + "BinaryEffectClause", + "BinaryEffectEvidenceBatch", + "BinaryEffectEvidenceQuery", + "BinaryEffectObservation", + "BinaryEvidenceKind", + "BinaryObservationCallback", "BoundSemanticCall", "BoundSemanticIntegration", "BoundRobotSkillProfile", "ControlPartEndpoint", "ControlPartEndpointAdapter", + "ControlPartEvidenceAddress", + "ControlPartRobotEvidenceSource", + "ControlPartSimulationEvidenceProvider", + "CoordinatedHeldObjectCleanupExpectation", + "COMPOSITE_EFFECT_MONITOR_ID", + "COMPOSITE_EFFECT_MONITOR_REVISION", + "CONTACT_EFFECT_CHANNEL", + "CONSTRAINT_EFFECT_CHANNEL", + "CONTROL_PART_EVIDENCE_PROVIDER_ID", + "CONTROL_PART_EVIDENCE_PROVIDER_REVISION", + "CompositeEffectMonitor", + "CompositeEffectMonitorCfg", + "CompositeEffectMonitorFactory", "DeclarativeValue", "EndpointResolution", + "EffectClause", + "EffectEvidenceAddress", + "EffectEvidenceBatch", + "EffectEvidenceCollectionContext", + "EffectEvidenceCollector", + "EffectEvidenceCollectorPort", + "EffectEvidenceProvider", + "EffectEvidenceProviderRegistry", + "EffectEvidenceQuery", + "EffectEvidenceQueryValue", + "EffectEvidenceSourceRef", + "EffectMonitor", + "EffectMonitorDecision", + "EffectMonitorFactory", + "EffectMonitorParam", + "EffectMonitorRef", + "EffectMonitorRegistry", + "EffectStateExpectation", + "FORCE_EFFECT_CHANNEL", "GRASP_AFFORDANCE_CAPABILITY", "GroundedSemanticCall", + "HeldObjectRelation", + "HeldObjectStateExpectation", "HandOver", "HandOverPoseProvider", "HandOverPoseTargets", "LinkedSemanticCall", + "JOINT_STATE_EFFECT_CHANNEL", + "JointStateEffectClause", + "JointStateEvidenceBatch", + "JointStateEvidenceQuery", + "JointStateObservation", + "POSE_RELATION_EFFECT_CHANNEL", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", "PathPart", + "OperateArticulation", + "ParallelBarrierUpdate", + "ParallelBranchStaticAnalysis", + "ParallelBranchRuntime", + "ParallelCommandSafetyValidator", + "ParallelBranchPlan", + "ParallelConflictError", + "ParallelStateConflictError", + "ParallelTimingError", + "ParallelTimingPolicy", + "ParallelLaneCommandSink", + "ParallelRuntimeBranch", + "ParallelSafetyError", + "ParallelSkillResult", + "ParallelSkillRuntime", + "analyze_parallel_branches", "Pick", "Place", "PlaceRelationTarget", + "PoseRelationClause", + "PoseRelationEvidenceBatch", + "PoseRelationEvidenceQuery", + "PoseRelationExpectation", "ProfileValidationError", "RegistrySceneProvider", "ResolvedRobotResource", @@ -131,12 +314,15 @@ "ResourceClaim", "ResourceEndpoint", "ResourceEndpointAdapter", + "ResolvedCorePolicyTrace", "RegisteredSemanticCall", "RegisteredSemanticLowerer", "RelationTargetGrounder", "RobotResource", "RobotSkillProfile", "SceneAffordanceRef", + "SceneArticulationJointStateProvider", + "SceneArticulationEvidenceProvider", "SceneArticulationRef", "SceneCollisionRole", "SceneCollisionWorldMode", @@ -150,13 +336,25 @@ "SceneLinkRef", "SceneObjectRef", "SceneRegistry", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION", "SceneManifest", + "ScalarEffectClause", + "ScalarEffectEvidenceBatch", + "ScalarEffectEvidenceQuery", + "ScalarEffectObservation", + "ScalarEvidenceKind", + "ScalarExpectation", + "ScalarObservationCallback", "SemanticCallCatalog", "SemanticCallDescriptor", "SemanticCallSpec", "SemanticDiagnostic", "SemanticEffectDependency", "SemanticEffectKind", + "SemanticEffectSpec", + "SymbolicStateDomain", + "SymbolicStateKey", "SemanticHandOverTarget", "SemanticIntegrationManifest", "SemanticLowering", @@ -167,7 +365,23 @@ "SemanticValidationError", "SemanticWorkflow", "SkillPolicyPreset", + "SkillCallTrace", + "SkillEndpointBindingTrace", + "SkillEffectTrace", + "SkillFailure", + "SkillPlanAttemptTrace", + "SkillResult", + "SkillRuntime", + "SkillRuntimeProvider", + "SkillScene", + "SkillStatus", + "task_state_to_metadata", "UnsupportedSkillError", "UnsupportedSceneAffordanceError", + "build_effect_evidence_queries", + "align_parallel_commands", "builtin_semantic_call_catalog", + "merge_parallel_effects", + "resolve_parallel_barrier", + "validate_parallel_claims", ] diff --git a/embodichain/lab/sim/skills/calls.py b/embodichain/lab/sim/skills/calls.py index 71301fe71..7382c03d5 100644 --- a/embodichain/lab/sim/skills/calls.py +++ b/embodichain/lab/sim/skills/calls.py @@ -19,7 +19,7 @@ from __future__ import annotations from collections.abc import Iterable, Mapping -from dataclasses import dataclass, field +from dataclasses import dataclass, field, fields import math import re from types import MappingProxyType @@ -306,6 +306,36 @@ def to_matrix(self) -> torch.Tensor: output[:, 3, 3] = 1.0 return output[0] if was_unbatched else output + def to_metadata(self) -> dict[str, object]: + """Return the pose as deterministic JSON-safe semantic data.""" + return { + "position": self._position.detach().cpu().tolist(), + "quaternion_wxyz": self._quaternion_wxyz.detach().cpu().tolist(), + } + + +def _call_value_to_metadata(value: DeclarativeValue | object) -> object: + """Serialize one already validated semantic-call payload value.""" + if value is None or type(value) in (bool, int, float, str): + return value + if isinstance(value, SceneEntityRef): + return { + "entity_type": type(value).__name__, + "entity_id": value.entity_id, + } + if type(value) is SemanticPose: + return value.to_metadata() + if isinstance(value, Mapping): + return { + key: _call_value_to_metadata(nested) + for key, nested in sorted(value.items()) + } + if isinstance(value, tuple): + return [_call_value_to_metadata(nested) for nested in value] + raise TypeError( + f"Unsupported validated semantic-call metadata value {type(value).__name__}." + ) + @dataclass(frozen=True, slots=True, kw_only=True, eq=False) class SemanticCallSpec: @@ -327,6 +357,21 @@ def semantic_id(self) -> str: """Return the stable catalog identifier for this call.""" return self.call_kind + def to_metadata(self) -> dict[str, object]: + """Return this semantic call as deterministic JSON-safe data.""" + arguments = { + data_field.name: _call_value_to_metadata(getattr(self, data_field.name)) + for data_field in fields(self) + if data_field.name != "resources" + } + return { + "semantic_id": self.semantic_id, + "call_kind": self.call_kind, + "call_type": type(self).__name__, + "resources": _call_value_to_metadata(self.resources), + "arguments": arguments, + } + @dataclass(frozen=True, slots=True, eq=False) class Pick(SemanticCallSpec): @@ -447,6 +492,76 @@ def __post_init__(self) -> None: ) +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulation(SemanticCallSpec): + """Operate one registered articulation through a typed handle affordance. + + Select either a named affordance target or an explicit absolute joint + position plus handle-relative displacement. Grounding captures the current + live joint position as the source of that declared stroke. Recovery + replans then combine the latest handle pose and joint position to execute + only the remaining signed displacement. + + Args: + articulation: Authoritative articulation reference. + handle: Optional explicit operation affordance. Omission requests the + capability-scoped default registered on the articulation. + target: Optional target name registered by the affordance. + target_position: Explicit absolute desired joint position. + target_displacement: Explicit full signed operation displacement from + the joint position and handle pose captured during grounding. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "operate_articulation" + + articulation: SceneArticulationRef + handle: SceneAffordanceRef | None = None + target: str | None = None + target_position: float | None = None + target_displacement: float | None = None + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + if type(self.articulation) is not SceneArticulationRef: + raise TypeError( + "OperateArticulation.articulation must be a SceneArticulationRef." + ) + if self.handle is not None and type(self.handle) is not SceneAffordanceRef: + raise TypeError( + "OperateArticulation.handle must be a SceneAffordanceRef or None." + ) + named = self.target is not None + explicit_position = self.target_position is not None + explicit_displacement = self.target_displacement is not None + if named: + _validate_identifier( + self.target, + field_name="OperateArticulation.target", + ) + if explicit_position or explicit_displacement: + raise ValueError( + "OperateArticulation.target is mutually exclusive with " + "target_position and target_displacement." + ) + return + if not (explicit_position and explicit_displacement): + raise ValueError( + "OperateArticulation requires either target or the explicit " + "target_position and target_displacement pair." + ) + for field_name in ("target_position", "target_displacement"): + value = getattr(self, field_name) + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError( + f"OperateArticulation.{field_name} must be a finite scalar." + ) + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"OperateArticulation.{field_name} must be finite.") + object.__setattr__(self, field_name, normalized) + + DeclarativeValue: TypeAlias = ( None | bool @@ -608,11 +723,18 @@ def __post_init__(self) -> None: _validate_identifier( self.skill_id, field_name="SemanticCallDescriptor.skill_id" ) - if self.spec_type not in (Pick, Place, HandOver, RegisteredSemanticCall): + if self.spec_type not in ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ): raise TypeError( - "spec_type must be exactly Pick, Place, HandOver, or " - "RegisteredSemanticCall; extensions use the registered payload " - "contract rather than executable call subclasses." + "spec_type must be exactly Pick, Place, HandOver, " + "OperateArticulation, or RegisteredSemanticCall; extensions use " + "the registered payload contract rather than executable call " + "subclasses." ) _validate_static_binding_contract( self.binding_contract, @@ -674,6 +796,7 @@ def __post_init__(self) -> None: Pick.call_kind, Place.call_kind, HandOver.call_kind, + OperateArticulation.call_kind, RegisteredSemanticCall.call_kind, }: raise ValueError( @@ -738,7 +861,13 @@ def discover( if type(call) is str: call_id = _validate_identifier(call, field_name="semantic call ID") call_value = None - elif type(call) in (Pick, Place, HandOver, RegisteredSemanticCall): + elif type(call) in ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ): call_id = call.semantic_id call_value = call else: @@ -774,6 +903,9 @@ def _builtin_call_target( from embodichain.lab.sim.atomic_actions.primitives.hand_over import ( HandOver as HandOverAction, ) + from embodichain.lab.sim.atomic_actions.primitives.operate_articulation import ( + OperateArticulation as OperateArticulationAction, + ) from embodichain.lab.sim.atomic_actions.primitives.pick_up import PickUp from embodichain.lab.sim.atomic_actions.primitives.place import Place as PlaceAction @@ -781,6 +913,7 @@ def _builtin_call_target( Pick: PickUp.descriptor(), Place: PlaceAction.descriptor(), HandOver: HandOverAction.descriptor(), + OperateArticulation: OperateArticulationAction.descriptor(), } try: return targets[spec_type] @@ -802,7 +935,7 @@ def builtin_semantic_call_catalog() -> SemanticCallCatalog: skill_id=_builtin_call_target(spec_type).skill_id, binding_contract=_builtin_call_target(spec_type).binding_contract, ) - for spec_type in (Pick, Place, HandOver) + for spec_type in (Pick, Place, HandOver, OperateArticulation) ) return SemanticCallCatalog(descriptors) @@ -810,6 +943,7 @@ def builtin_semantic_call_catalog() -> SemanticCallCatalog: __all__ = [ "DeclarativeValue", "HandOver", + "OperateArticulation", "Pick", "Place", "PlaceRelationTarget", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 9d3914736..69cc0832b 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -21,7 +21,6 @@ from abc import ABC, abstractmethod from collections.abc import Iterable, Mapping from dataclasses import dataclass, field -from enum import Enum from types import MappingProxyType from typing import ClassVar from uuid import uuid4 @@ -33,26 +32,53 @@ ActionInvocation, ActionOptions, Affordance, + ArticulationOperationAffordance, GraspGoal, HandOverOptions, - JointPositionTarget, HeldObjectState, PickUpOptions, PlaceGoal, PlaceOptions, + OperateArticulationGoal, PlanningContext, PoseGoalValue, + SceneArticulationOperationGeometry, SceneEntityPose, SkillDescriptor, ) from .calls import ( HandOver, + OperateArticulation, Pick, Place, RegisteredSemanticCall, SemanticCallSpec, SemanticPose, ) +from .effects import ( + ArticulationJointStateExpectation, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + POSE_RELATION_EFFECT_CHANNEL, + BinaryEffectClause, + BinaryEvidenceKind, + CompositeEffectMonitorFactory, + CoordinatedHeldObjectCleanupExpectation, + EffectClause, + EffectMonitor, + EffectMonitorRef, + EffectMonitorRegistry, + EffectEvidenceSourceRef, + EffectStateExpectation, + HeldObjectRelation, + HeldObjectStateExpectation, + JointStateEffectClause, + PoseRelationClause, + PoseRelationExpectation, + SemanticEffectKind, + SemanticEffectSpec, + SymbolicStateKey, +) from .integration import ( BoundSemanticCall, BoundSemanticIntegration, @@ -61,6 +87,10 @@ SemanticValidationError, ) from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, @@ -87,15 +117,6 @@ def _diagnostic( return SemanticValidationError(SemanticDiagnostic(code, path, message, candidates)) -class SemanticEffectKind(str, Enum): - """Symbolic effect boundary inferred for a semantic call.""" - - ATTACH = "attach" - RELEASE = "release" - TRANSFER = "transfer" - REGISTERED = "registered" - - @dataclass(frozen=True, slots=True) class SemanticRelationTarget: """Statically selected relation affordance awaiting typed grounding.""" @@ -238,6 +259,9 @@ class AnalyzedSemanticCall: index: int bound: BoundSemanticCall effect_kind: SemanticEffectKind + symbolic_writes: frozenset[SymbolicStateKey] = frozenset() + opaque_symbolic_effect: bool = False + effect_monitor_ref: EffectMonitorRef | None = None downstream_object_targets: tuple[SemanticObjectTarget, ...] = () requires_verified_held_object: bool = False requires_fresh_observation: bool = True @@ -249,6 +273,29 @@ def __post_init__(self) -> None: raise TypeError("bound must be exactly BoundSemanticCall.") if not isinstance(self.effect_kind, SemanticEffectKind): raise TypeError("effect_kind must be a SemanticEffectKind.") + if type(self.symbolic_writes) is not frozenset or not all( + type(write) is SymbolicStateKey for write in self.symbolic_writes + ): + raise TypeError( + "symbolic_writes must be an exact frozenset of " + "SymbolicStateKey values." + ) + if type(self.opaque_symbolic_effect) is not bool: + raise TypeError("opaque_symbolic_effect must be a bool.") + if self.opaque_symbolic_effect and self.symbolic_writes: + raise ValueError( + "Opaque symbolic effects cannot also claim inferred exact keys." + ) + if self.effect_monitor_ref is not None: + if not isinstance(self.effect_monitor_ref, EffectMonitorRef): + raise TypeError( + "effect_monitor_ref must be an EffectMonitorRef or None." + ) + object.__setattr__( + self, + "effect_monitor_ref", + self.effect_monitor_ref.snapshot(), + ) targets = tuple(self.downstream_object_targets) if not all(type(target) is SemanticObjectTarget for target in targets): raise TypeError( @@ -423,6 +470,8 @@ class GroundedSemanticCall: analyzed: AnalyzedSemanticCall invocation: ActionInvocation + effect_spec: SemanticEffectSpec | None + effect_monitor: EffectMonitor | None = field(repr=False, compare=False) _eligible_mask: torch.Tensor = field(repr=False, compare=False) def __init__(self, *args: object, **kwargs: object) -> None: @@ -439,12 +488,16 @@ def _create( *, analyzed: AnalyzedSemanticCall, invocation: ActionInvocation, + effect_spec: SemanticEffectSpec | None, + effect_monitor: EffectMonitor | None, eligible_mask: torch.Tensor, ) -> GroundedSemanticCall: """Create one compiler-owned grounded result.""" instance = object.__new__(cls) object.__setattr__(instance, "analyzed", analyzed) object.__setattr__(instance, "invocation", invocation) + object.__setattr__(instance, "effect_spec", effect_spec) + object.__setattr__(instance, "effect_monitor", effect_monitor) object.__setattr__(instance, "_eligible_mask", eligible_mask.clone()) instance.__post_init__() return instance @@ -456,6 +509,20 @@ def __post_init__(self) -> None: raise TypeError("invocation must be exactly ActionInvocation.") if self.invocation.skill_id != self.analyzed.bound.linked.descriptor.skill_id: raise ValueError("invocation skill_id must match the analyzed call.") + if (self.effect_spec is None) != (self.effect_monitor is None): + raise ValueError( + "effect_spec and effect_monitor must either both be set or both be None." + ) + if self.effect_spec is not None: + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec or None.") + if not isinstance(self.effect_monitor, EffectMonitor): + raise TypeError("effect_monitor must be an EffectMonitor or None.") + if self.effect_spec.semantic_id != self.analyzed.call.semantic_id: + raise ValueError( + "effect_spec semantic_id must match the analyzed call." + ) + object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) if not isinstance(self._eligible_mask, torch.Tensor): raise TypeError("eligible_mask must be a torch.Tensor.") if self._eligible_mask.dtype != torch.bool or self._eligible_mask.dim() != 1: @@ -479,6 +546,7 @@ def __init__( registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), relation_grounders: Iterable[RelationTargetGrounder] = (), handover_pose_providers: Iterable[HandOverPoseProvider] = (), + effect_monitor_registry: EffectMonitorRegistry | None = None, ) -> None: """Install immutable semantic lowering and grounding registries. @@ -487,6 +555,7 @@ def __init__( registered_lowerers: Explicit implementations for registered calls. relation_grounders: Exact capability/payload/revision dispatch entries. handover_pose_providers: Named embodiment-owned handover providers. + effect_monitor_registry: Versioned semantic-effect monitor factories. """ if type(integration) is not BoundSemanticIntegration: raise TypeError("integration must be exactly BoundSemanticIntegration.") @@ -603,6 +672,16 @@ def __init__( self._registered_lowerers = MappingProxyType(lowerers) self._relation_grounders = MappingProxyType(normalized_grounders) self._handover_pose_providers = MappingProxyType(normalized_handover_providers) + selected_monitor_registry = ( + EffectMonitorRegistry((CompositeEffectMonitorFactory(),)) + if effect_monitor_registry is None + else effect_monitor_registry + ) + if not isinstance(selected_monitor_registry, EffectMonitorRegistry): + raise TypeError( + "effect_monitor_registry must be an EffectMonitorRegistry or None." + ) + self._effect_monitor_registry = selected_monitor_registry @property def integration(self) -> BoundSemanticIntegration: @@ -626,6 +705,11 @@ def handover_pose_providers(self) -> Mapping[str, HandOverPoseProvider]: """Return installed handover pose providers by stable provider ID.""" return self._handover_pose_providers + @property + def effect_monitor_registry(self) -> EffectMonitorRegistry: + """Return the immutable versioned effect-monitor factory registry.""" + return self._effect_monitor_registry + def analyze( self, calls: Iterable[SemanticCallSpec], @@ -659,7 +743,13 @@ def analyze( ) from exc if not supplied: raise ValueError("Semantic workflow requires at least one call.") - allowed_types = (Pick, Place, HandOver, RegisteredSemanticCall) + allowed_types = ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ) if not all(type(call) in allowed_types for call in supplied): raise TypeError("calls must contain exact supported semantic call values.") @@ -770,6 +860,8 @@ def analyze( index, bound.binding.resource_ids["destination"], ) + elif type(call) is OperateArticulation: + effect_kind = SemanticEffectKind.ARTICULATION else: effect_kind = SemanticEffectKind.REGISTERED # A registered extension has no declarative state-flow contract @@ -780,11 +872,23 @@ def analyze( if type(call) is Pick else () ) + effect_monitor_ref = self._effect_monitor_ref( + bound, + effect_kind, + path=(*path, index, "effect_monitor"), + ) + symbolic_writes, opaque_symbolic_effect = self._static_symbolic_writes( + bound, + path=(*path, index, "call"), + ) analyzed.append( AnalyzedSemanticCall( index=index, bound=bound, effect_kind=effect_kind, + symbolic_writes=symbolic_writes, + opaque_symbolic_effect=opaque_symbolic_effect, + effect_monitor_ref=effect_monitor_ref, downstream_object_targets=downstream_targets, requires_verified_held_object=requires_held, ) @@ -798,6 +902,124 @@ def analyze( compiler_id=self._compiler_id, ) + def _static_symbolic_writes( + self, + bound: BoundSemanticCall, + *, + path: tuple[PathPart, ...], + ) -> tuple[frozenset[SymbolicStateKey], bool]: + """Return exact provider-free ``TaskState`` keys for one linked call. + + Curated calls own these contracts. Registered calls remain an opaque + physical-effect boundary until their public descriptor grows an + explicit static-effect contract; lowering arguments are never guessed. + Conditional coordinated-held cleanup is likewise omitted because its + exact pair keys depend on the verified input ``TaskState``. + """ + call = bound.linked.call + if type(call) in (Pick, Place): + return ( + frozenset( + { + SymbolicStateKey.held_object( + self._participant_task_state_key( + bound, + slot_id="primary", + path=(*path, "resources", "primary"), + ) + ) + } + ), + False, + ) + if type(call) is HandOver: + return ( + frozenset( + SymbolicStateKey.held_object( + self._participant_task_state_key( + bound, + slot_id=slot_id, + path=(*path, "resources", slot_id), + ) + ) + for slot_id in ("source", "destination") + ), + False, + ) + if type(call) is OperateArticulation: + handle_ref = bound.linked.affordances.get("handle") + if handle_ref is None: + raise AssertionError( + "Linked articulation call lacks an operation affordance." + ) + registration = self._integration.scene_registry.lookup( + handle_ref, + expected_type=SceneAffordanceRef, + ) + affordance = registration.affordance + if ( + type(affordance) is not ArticulationOperationAffordance + or ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + not in registration.affordance_capabilities + ): + raise _diagnostic( + "invalid_articulation_affordance", + (*path, "handle"), + f"Handle {handle_ref.entity_id!r} must expose an exact " + "ArticulationOperationAffordance payload and the articulation " + "operation capability.", + ) + return ( + frozenset( + { + SymbolicStateKey.articulation_joint( + call.articulation.entity_id, + affordance.joint_id, + ) + } + ), + False, + ) + if type(call) is RegisteredSemanticCall: + return frozenset(), True + raise AssertionError(f"Unsupported linked call {type(call).__name__}.") + + @staticmethod + def _participant_task_state_key( + bound: BoundSemanticCall, + *, + slot_id: str, + path: tuple[PathPart, ...], + ) -> str: + """Resolve the exact held-object key shared by participant endpoints.""" + resource = bound.binding.resources.get(slot_id) + if resource is None: + raise _diagnostic( + "missing_effect_resource", + path, + f"Held-object effects require bound resource slot {slot_id!r}.", + tuple(bound.binding.resources), + ) + motion_endpoint = resource.endpoints.get("motion") + grasp_endpoint = resource.endpoints.get("grasp") + if motion_endpoint is None or grasp_endpoint is None: + raise _diagnostic( + "missing_effect_endpoint", + (*path, "endpoints"), + "Held-object effects require bound motion and grasp endpoints.", + tuple(resource.endpoints), + ) + task_state_key = motion_endpoint.task_state_key + assert isinstance(task_state_key, str) + if grasp_endpoint.task_state_key != task_state_key: + raise _diagnostic( + "effect_state_key_mismatch", + (*path, "task_state_key"), + "Motion and grasp endpoints for one participant must share one " + "logical task-state key.", + ) + return task_state_key + def ground( self, workflow: SemanticWorkflow, @@ -844,6 +1066,12 @@ def ground( lowering = self._lower_place(analyzed, context, eligible, path=path) elif type(call) is HandOver: lowering = self._lower_handover(analyzed, context, eligible, path=path) + elif type(call) is OperateArticulation: + lowering = self._lower_operate_articulation( + analyzed, + context, + path=path, + ) elif type(call) is RegisteredSemanticCall: lowering = self._lower_registered(analyzed, context, path=path) else: # pragma: no cover - exact workflow construction prevents this @@ -861,9 +1089,30 @@ def ground( invocation_id=f"{workflow.workflow_id}:{call_index}", revision=revision, ) + effect_spec = self._ground_effect_spec( + analyzed, + invocation, + context, + path=(*path, call_index, "effect"), + ) + effect_monitor: EffectMonitor | None = None + if effect_spec is not None and analyzed.effect_monitor_ref is not None: + try: + effect_monitor = self._effect_monitor_registry.create( + effect_spec, + analyzed.effect_monitor_ref, + ) + except (KeyError, TypeError, ValueError) as exc: + raise _diagnostic( + "effect_monitor_creation_failed", + (*path, call_index, "effect_monitor"), + f"Could not create the grounded effect monitor: {exc}", + ) from exc return GroundedSemanticCall._create( analyzed=analyzed, invocation=invocation, + effect_spec=effect_spec, + effect_monitor=effect_monitor, eligible_mask=eligible, ) @@ -956,6 +1205,62 @@ def _validate_context(self, context: PlanningContext) -> None: if context.robot.qpos.device != engine.device: raise ValueError("PlanningContext and compiler engine must share a device.") + def _effect_monitor_ref( + self, + bound: BoundSemanticCall, + effect_kind: SemanticEffectKind, + *, + path: tuple[PathPart, ...], + ) -> EffectMonitorRef | None: + """Resolve one preset-owned exact monitor reference without creating it.""" + semantic_id = bound.linked.call.semantic_id + monitor_ref = bound.preset.effect_monitors.get(semantic_id) + if monitor_ref is None: + if type(bound.linked.call) in ( + Pick, + Place, + HandOver, + OperateArticulation, + ): + raise _diagnostic( + "missing_effect_monitor", + path, + f"Semantic call {semantic_id!r} requires an effect monitor " + f"for its {effect_kind.value!r} postcondition.", + tuple(bound.preset.effect_monitors), + ) + return None + if type(bound.linked.call) is RegisteredSemanticCall: + raise _diagnostic( + "registered_effect_contract_not_installed", + path, + f"Registered semantic call {semantic_id!r} selects an effect " + "monitor but no declarative effect-contract grounder is " + "installed.", + ) + try: + self._effect_monitor_registry.validate_ref(monitor_ref) + except KeyError as exc: + available = tuple( + f"{monitor_id}@{revision}" + for monitor_id, revision in self._effect_monitor_registry.factories + ) + raise _diagnostic( + "effect_monitor_not_installed", + path, + f"Effect monitor {monitor_ref.monitor_id!r} revision " + f"{monitor_ref.revision!r} is not installed.", + available, + ) from exc + except (TypeError, ValueError) as exc: + raise _diagnostic( + "invalid_effect_monitor_config", + path, + f"Effect monitor {monitor_ref.monitor_id!r} revision " + f"{monitor_ref.revision!r} has invalid configuration: {exc}", + ) from exc + return monitor_ref.snapshot() + def _downstream_targets( self, pick_index: int, @@ -1038,14 +1343,14 @@ def _lower_place( """Convert an object-space place target using verified held state.""" call = analyzed.call assert type(call) is Place - control_part, held = self._require_held_object( + task_state_key, held = self._require_held_object( analyzed, context, eligible, slot_id="primary", path=(*path, analyzed.index, "call", "object"), ) - del control_part + del task_state_key if call.at is not None: object_target = self._broadcast_pose( call.at.to_matrix(), @@ -1113,6 +1418,136 @@ def _lower_handover( ), ) + def _lower_operate_articulation( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> SemanticLowering: + """Ground one handle operation from the latest scene snapshot.""" + call = analyzed.call + assert type(call) is OperateArticulation + handle_ref = analyzed.bound.linked.affordances.get("handle") + if handle_ref is None: + raise AssertionError( + "Linked articulation call lacks an operation affordance." + ) + registration = self._integration.scene_registry.lookup( + handle_ref, + expected_type=SceneAffordanceRef, + ) + affordance = registration.affordance + if ( + type(affordance) is not ArticulationOperationAffordance + or ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + not in registration.affordance_capabilities + ): + raise _diagnostic( + "invalid_articulation_affordance", + (*path, analyzed.index, "call", "handle"), + f"Handle {handle_ref.entity_id!r} must expose an exact " + "ArticulationOperationAffordance payload and the articulation " + "operation capability.", + ) + + if call.target is not None: + try: + resolved_target = affordance.resolve_target(call.target) + except KeyError as exc: + raise _diagnostic( + "unknown_articulation_target", + (*path, analyzed.index, "call", "target"), + f"Handle {handle_ref.entity_id!r} has no semantic target " + f"{call.target!r}.", + tuple(affordance.semantic_targets), + ) from exc + target_position = resolved_target.target_position + displacement = resolved_target.displacement + else: + assert call.target_position is not None + assert call.target_displacement is not None + target_position = call.target_position + displacement = call.target_displacement + + try: + handle_state = context.scene.entities[handle_ref.entity_id] + except KeyError as exc: + raise _diagnostic( + "missing_handle_observation", + (*path, analyzed.index, "call", "handle"), + f"The current planning snapshot has no pose for handle " + f"{handle_ref.entity_id!r}.", + ) from exc + try: + self._broadcast_pose( + handle_state.pose, + context, + name=f"handle {handle_ref.entity_id!r}", + ) + except (TypeError, ValueError) as exc: + raise _diagnostic( + "articulation_grounding_failed", + (*path, analyzed.index, "call", "handle"), + f"Could not ground articulation handle geometry: {exc}", + ) from exc + joint_address = call.articulation.entity_id, affordance.joint_id + observed_joint = context.scene.get_articulation_joint_state(*joint_address) + if observed_joint is None: + raise _diagnostic( + "missing_articulation_joint_observation", + (*path, analyzed.index, "call", "articulation"), + "Recovery-safe articulation grounding requires a live " + "ObservedArticulationJointState for " + f"{joint_address!r} in the current scene snapshot.", + ) + try: + source_position = self._broadcast_joint_position( + observed_joint.position, + context, + name=f"articulation joint {joint_address!r}", + ) + except (TypeError, ValueError) as exc: + raise _diagnostic( + "invalid_articulation_joint_observation", + (*path, analyzed.index, "call", "articulation"), + f"Could not use live articulation joint state: {exc}", + ) from exc + if observed_joint.valid_mask is not None: + valid = observed_joint.valid_mask.to(device=context.robot.qpos.device) + if bool((~valid).any()): + rows = (~valid).nonzero(as_tuple=False).flatten().tolist() + raise _diagnostic( + "invalid_articulation_joint_observation", + (*path, analyzed.index, "call", "articulation"), + "Live articulation joint state is unavailable for planning " + f"rows {rows}.", + ) + target = torch.full( + (context.batch_size, 1), + target_position, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + return SemanticLowering( + goal=OperateArticulationGoal( + articulation_id=call.articulation.entity_id, + joint_id=affordance.joint_id, + geometry=SceneArticulationOperationGeometry( + handle_pose=SceneEntityPose(handle_ref.entity_id), + approach_offset=affordance.approach_offset, + contact_offset=affordance.contact_offset, + operation_offset=affordance.operation_offset, + retract_offset=affordance.retract_offset, + operation_axis=affordance.operation_axis, + position_scale=affordance.position_scale, + ), + source_position=source_position, + target_position=target, + target_displacement=displacement, + ) + ) + def _lower_registered( self, analyzed: AnalyzedSemanticCall, @@ -1162,6 +1597,244 @@ def _lower_registered( ) return lowering + def _ground_effect_spec( + self, + analyzed: AnalyzedSemanticCall, + invocation: ActionInvocation, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> SemanticEffectSpec | None: + """Ground typed symbolic state and raw-evidence clauses.""" + if analyzed.effect_monitor_ref is None: + return None + call = analyzed.call + state_expectations: list[EffectStateExpectation] = [] + clauses: list[EffectClause] = [] + if type(call) is Pick: + expectation, grounded_clauses = self._ground_held_effect( + analyzed, + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + slot_id="primary", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "destination"), + ) + state_expectations.append(expectation) + clauses.extend(grounded_clauses) + state_expectations.extend( + self._coordinated_cleanup_expectations( + context, + task_state_keys=(expectation.task_state_key,), + ) + ) + elif type(call) is Place: + expectation, grounded_clauses = self._ground_held_effect( + analyzed, + expectation_id="source", + relation=HeldObjectRelation.DETACHED, + slot_id="primary", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "source"), + ) + state_expectations.append(expectation) + clauses.extend(grounded_clauses) + state_expectations.extend( + self._coordinated_cleanup_expectations( + context, + task_state_keys=(expectation.task_state_key,), + ) + ) + elif type(call) is HandOver: + source, source_clauses = self._ground_held_effect( + analyzed, + expectation_id="source", + relation=HeldObjectRelation.DETACHED, + slot_id="source", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "source"), + ) + destination, destination_clauses = self._ground_held_effect( + analyzed, + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + slot_id="destination", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "destination"), + ) + state_expectations.extend((source, destination)) + clauses.extend((*source_clauses, *destination_clauses)) + elif type(call) is OperateArticulation: + goal = invocation.goal + if type(goal) is not OperateArticulationGoal: + raise AssertionError( + "OperateArticulation lowering produced an incompatible goal." + ) + expectation = ArticulationJointStateExpectation( + expectation_id="joint", + articulation_id=goal.articulation_id, + joint_id=goal.joint_id, + target_position=goal.target_position, + ) + source = EffectEvidenceSourceRef( + provider_id=SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + revision=SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + address=ArticulationJointEvidenceAddress( + articulation_id=goal.articulation_id, + joint_id=goal.joint_id, + ), + ) + state_expectations.append(expectation) + clauses.append( + JointStateEffectClause( + clause_id="joint.position", + expectation_id=expectation.expectation_id, + source=source, + target_position=goal.target_position, + ) + ) + else: # pragma: no cover - exact workflow construction prevents this + raise AssertionError(f"Unsupported analyzed call {type(call).__name__}.") + return SemanticEffectSpec( + semantic_id=call.semantic_id, + effect_kind=analyzed.effect_kind, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + env_ids=context.env_ids, + state_expectations=tuple(state_expectations), + clauses=tuple(clauses), + ) + + @staticmethod + def _coordinated_cleanup_expectations( + context: PlanningContext, + *, + task_state_keys: tuple[str, ...], + ) -> tuple[CoordinatedHeldObjectCleanupExpectation, ...]: + """Declare the exact coordinated relations a primitive must remove.""" + related = set(task_state_keys) + return tuple( + CoordinatedHeldObjectCleanupExpectation( + expectation_id=f"cleanup:{resources[0]}:{resources[1]}", + task_state_keys=resources, + ) + for resources in context.task.coordinated_held_objects + if not set(resources).isdisjoint(related) + ) + + @staticmethod + def _effect_source( + sources: Mapping[str, EffectEvidenceSourceRef], + channel: str, + *, + path: tuple[PathPart, ...], + ) -> EffectEvidenceSourceRef: + """Resolve one exact endpoint-owned observation source.""" + source = sources.get(channel) + if source is None: + raise _diagnostic( + "missing_effect_source", + (*path, "effect_sources", channel), + f"The endpoint does not expose required effect channel {channel!r}.", + tuple(sources), + ) + return source.snapshot() + + def _ground_held_effect( + self, + analyzed: AnalyzedSemanticCall, + *, + expectation_id: str, + relation: HeldObjectRelation, + slot_id: str, + object_id: str, + context: PlanningContext, + path: tuple[PathPart, ...], + ) -> tuple[HeldObjectStateExpectation, tuple[EffectClause, ...]]: + """Bind one held-object state relation to generic endpoint sources.""" + resource = analyzed.bound.binding.resources[slot_id] + motion_endpoint = resource.endpoints.get("motion") + grasp_endpoint = resource.endpoints.get("grasp") + if motion_endpoint is None or grasp_endpoint is None: + raise _diagnostic( + "missing_effect_endpoint", + (*path, "endpoints"), + "Held-object effects require bound motion and grasp endpoints.", + tuple(resource.endpoints), + ) + task_state_key = motion_endpoint.task_state_key + assert isinstance(task_state_key, str) + if grasp_endpoint.task_state_key != task_state_key: + raise _diagnostic( + "effect_state_key_mismatch", + (*path, "task_state_key"), + "Motion and grasp endpoints for one participant must share one " + "logical task-state key.", + ) + baseline: torch.Tensor | None = None + if relation is HeldObjectRelation.DETACHED: + held = context.task.get_held_object(task_state_key) + if held is None or held.semantics.entity_id != object_id: + raise _diagnostic( + "verified_held_object_required", + (*path, "baseline"), + f"Detached relation requires verified object {object_id!r} " + f"held under logical state key {task_state_key!r}.", + ) + baseline = held.object_to_eef + state_expectation = HeldObjectStateExpectation( + expectation_id=expectation_id, + relation=relation, + object_id=object_id, + slot_id=slot_id, + resource_id=resource.resource_id, + task_state_key=task_state_key, + ) + pose_source = self._effect_source( + motion_endpoint.effect_sources, + POSE_RELATION_EFFECT_CHANNEL, + path=(*path, "motion"), + ) + binary_channel = ( + CONSTRAINT_EFFECT_CHANNEL + if CONSTRAINT_EFFECT_CHANNEL in grasp_endpoint.effect_sources + else CONTACT_EFFECT_CHANNEL + ) + binary_source = self._effect_source( + grasp_endpoint.effect_sources, + binary_channel, + path=(*path, "grasp"), + ) + pose_clause = PoseRelationClause( + clause_id=f"{expectation_id}.pose", + expectation_id=expectation_id, + source=pose_source, + expectation=( + PoseRelationExpectation.MATCHED + if relation is HeldObjectRelation.ATTACHED + else PoseRelationExpectation.SEPARATED + ), + baseline_object_to_endpoint=baseline, + ) + binary_kind = ( + BinaryEvidenceKind.CONSTRAINT + if binary_channel == CONSTRAINT_EFFECT_CHANNEL + else BinaryEvidenceKind.CONTACT + ) + binary_clause = BinaryEffectClause( + clause_id=f"{expectation_id}.{binary_kind.value}", + expectation_id=expectation_id, + source=binary_source, + evidence_kind=binary_kind, + expected=relation is HeldObjectRelation.ATTACHED, + ) + return state_expectation, (pose_clause, binary_clause) + def _relation_target( self, bound: BoundSemanticCall, @@ -1373,18 +2046,19 @@ def _require_held_object( slot_id: str, path: tuple[PathPart, ...], ) -> tuple[str, HeldObjectState]: - """Resolve the motion control part and verify its held-object identity.""" - endpoint = analyzed.bound.binding.action_binding.endpoint(slot_id, "motion") - try: - target = endpoint.require_target(JointPositionTarget) - except TypeError as exc: + """Resolve the logical participant key and verify held-object identity.""" + resource = analyzed.bound.binding.resources[slot_id] + endpoint = resource.endpoints.get("motion") + if endpoint is None: raise _diagnostic( - "unsupported_builtin_endpoint", + "missing_effect_endpoint", (*path, "resources", slot_id, "motion"), - "The current built-in semantic lowerer requires a joint-position " - "motion endpoint.", - ) from exc - held = context.task.get_held_object(target.control_part) + "The semantic lowerer requires a bound motion endpoint.", + tuple(resource.endpoints), + ) + task_state_key = endpoint.task_state_key + assert isinstance(task_state_key, str) + held = context.task.get_held_object(task_state_key) call_object = getattr(analyzed.call, "object", None) assert type(call_object) is SceneObjectRef if held is None or held.semantics.entity_id != call_object.entity_id: @@ -1392,7 +2066,7 @@ def _require_held_object( "verified_held_object_required", path, f"Call requires verified object {call_object.entity_id!r} held by " - f"{target.control_part!r}.", + f"logical state key {task_state_key!r}.", ) assert held.env_mask is not None missing = eligible & ~held.env_mask @@ -1408,7 +2082,7 @@ def _require_held_object( "every eligible environment.", missing_env_ids, ) - return target.control_part, held + return task_state_key, held @staticmethod def _broadcast_pose( @@ -1427,6 +2101,31 @@ def _broadcast_pose( ) return pose.clone() + @staticmethod + def _broadcast_joint_position( + position: torch.Tensor, + context: PlanningContext, + *, + name: str, + ) -> torch.Tensor: + """Move and broadcast one scalar articulation joint observation.""" + if not isinstance(position, torch.Tensor): + raise TypeError(f"{name} position must be a torch.Tensor.") + if not position.is_floating_point() or not torch.isfinite(position).all(): + raise ValueError(f"{name} position must be a finite floating tensor.") + position = position.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if position.shape == (1,): + return position.unsqueeze(0).expand(context.batch_size, -1).clone() + if position.shape != (context.batch_size, 1): + raise ValueError( + f"{name} position must have shape (1,) or " + f"({context.batch_size}, 1)." + ) + return position.clone() + __all__ = [ "AnalyzedSemanticCall", diff --git a/embodichain/lab/sim/skills/effects.py b/embodichain/lab/sim/skills/effects.py new file mode 100644 index 000000000..852cb99a0 --- /dev/null +++ b/embodichain/lab/sim/skills/effects.py @@ -0,0 +1,2250 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Backend-neutral semantic-effect contracts, evidence, and monitors.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from collections.abc import Hashable, Iterable, Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass +from enum import Enum +import math +from types import MappingProxyType +from typing import ClassVar, TypeAlias + +import torch + +from embodichain.lab.sim.atomic_actions.execution import EffectVerificationRequest +from embodichain.lab.sim.atomic_actions.state import ( + ArticulationJointState, + HeldObjectState, +) + +EffectMonitorParam: TypeAlias = ( + None + | bool + | int + | float + | str + | tuple["EffectMonitorParam", ...] + | Mapping[str, "EffectMonitorParam"] +) +"""Recursively immutable, non-executable monitor configuration value.""" + +COMPOSITE_EFFECT_MONITOR_ID = "builtin.composite_effect" +"""Stable ID of the built-in typed-clause monitor.""" + +COMPOSITE_EFFECT_MONITOR_REVISION = "1" +"""Exact behavior/configuration revision of the built-in monitor.""" + +CONTROL_PART_EVIDENCE_PROVIDER_ID = "builtin.control_part" +"""Stable provider ID used by generic control-part evidence addresses.""" + +CONTROL_PART_EVIDENCE_PROVIDER_REVISION = "1" +"""Exact contract revision of control-part evidence addresses.""" + +POSE_RELATION_EFFECT_CHANNEL = "pose_relation" +CONTACT_EFFECT_CHANNEL = "contact" +CONSTRAINT_EFFECT_CHANNEL = "constraint" +FORCE_EFFECT_CHANNEL = "force" +JOINT_STATE_EFFECT_CHANNEL = "joint_state" + +_EFFECT_CHANNELS = frozenset( + { + POSE_RELATION_EFFECT_CHANNEL, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } +) +_SE3_BASE_ATOL = 1.0e-5 +_SE3_EPS_MULTIPLIER = 10.0 + + +def _metadata_value(value: object) -> object: + """Convert one typed effect value to deterministic JSON-safe data.""" + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + return value if math.isfinite(value) else None + if isinstance(value, Enum): + return value.value + if isinstance(value, torch.Tensor): + return _metadata_value(value.detach().cpu().tolist()) + if isinstance(value, Mapping): + return { + str(key): _metadata_value(nested) + for key, nested in sorted(value.items(), key=lambda item: str(item[0])) + } + if isinstance(value, (tuple, list)): + return [_metadata_value(nested) for nested in value] + if is_dataclass(value) and not isinstance(value, type): + return { + "type": f"{type(value).__module__}.{type(value).__qualname__}", + **{ + data_field.name: _metadata_value(getattr(value, data_field.name)) + for data_field in fields(value) + }, + } + return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty 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 _snapshot_declarative_value( + value: object, + *, + path: str, + active: set[int] | None = None, + budget: list[int] | None = None, + depth: int = 0, +) -> EffectMonitorParam: + """Own one bounded, acyclic, non-executable declarative value.""" + if active is None: + active = set() + if budget is None: + budget = [4096] + if depth > 32: + raise ValueError(f"{path} exceeds the maximum declarative depth of 32.") + budget[0] -= 1 + if budget[0] < 0: + raise ValueError(f"{path} exceeds the maximum declarative node count.") + 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(f"{path} must be finite.") + return value + if type(value) in (dict, MappingProxyType): + container_id = id(value) + if container_id in active: + raise ValueError(f"{path} contains a cyclic mapping.") + active.add(container_id) + try: + snapshot: dict[str, EffectMonitorParam] = {} + for key, nested in value.items(): + _validate_identifier(key, field_name=f"{path} keys") + snapshot[key] = _snapshot_declarative_value( + nested, + path=f"{path}.{key}", + active=active, + budget=budget, + depth=depth + 1, + ) + return MappingProxyType(snapshot) + finally: + active.remove(container_id) + if type(value) in (tuple, list): + container_id = id(value) + if container_id in active: + raise ValueError(f"{path} contains a cyclic sequence.") + active.add(container_id) + try: + return tuple( + _snapshot_declarative_value( + nested, + path=f"{path}[{index}]", + active=active, + budget=budget, + depth=depth + 1, + ) + for index, nested in enumerate(value) + ) + finally: + active.remove(container_id) + raise TypeError( + f"{path} contains non-declarative {type(value).__name__}; callables, " + "classes, tensors, and live objects are not allowed." + ) + + +def _snapshot_monitor_params( + values: Mapping[str, EffectMonitorParam], +) -> Mapping[str, EffectMonitorParam]: + """Validate and own a monitor-parameter mapping.""" + if type(values) not in (dict, MappingProxyType): + raise TypeError( + "EffectMonitorRef.params must be an exact dict or mapping proxy." + ) + snapshot = _snapshot_declarative_value(values, path="EffectMonitorRef.params") + assert isinstance(snapshot, Mapping) + return snapshot + + +def _validate_pose_batch( + value: torch.Tensor, + *, + field_name: str, + valid_mask: torch.Tensor | None = None, +) -> torch.Tensor: + """Validate and own unbatched or batched proper SE(3) transforms.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.shape != (4, 4) and ( + value.dim() != 3 or value.shape[0] == 0 or value.shape[-2:] != (4, 4) + ): + raise ValueError(f"{field_name} must have shape (4, 4) or (B, 4, 4).") + if not value.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + poses = value.unsqueeze(0) if value.dim() == 2 else value + if valid_mask is not None: + if not isinstance(valid_mask, torch.Tensor): + raise TypeError("valid_mask must be a torch.Tensor.") + if valid_mask.dtype != torch.bool or valid_mask.shape != (poses.shape[0],): + raise ValueError("valid_mask must be a bool tensor with shape (B,).") + if valid_mask.device != poses.device: + raise ValueError("valid_mask and poses must share a device.") + poses = poses[valid_mask] + if poses.numel() == 0: + return value.clone() + if not torch.isfinite(poses).all(): + raise ValueError(f"{field_name} must contain only finite values.") + tolerance = max( + _SE3_BASE_ATOL, + _SE3_EPS_MULTIPLIER * float(torch.finfo(value.dtype).eps), + ) + checked = poses.to(dtype=torch.float64) + expected_bottom = checked.new_tensor((0.0, 0.0, 0.0, 1.0)) + if not torch.isclose( + checked[:, 3, :], + expected_bottom.expand(checked.shape[0], -1), + atol=tolerance, + rtol=0.0, + ).all(): + raise ValueError( + f"{field_name} must contain SE(3) transforms with homogeneous " + "bottom row [0, 0, 0, 1]." + ) + rotations = checked[:, :3, :3] + gram = rotations.transpose(-1, -2) @ rotations + identity = torch.eye(3, dtype=checked.dtype, device=checked.device).expand_as(gram) + if not torch.isclose(gram, identity, atol=tolerance, rtol=0.0).all(): + raise ValueError( + f"{field_name} must contain SE(3) transforms with orthonormal rotations." + ) + determinants = torch.linalg.det(rotations) + if not torch.isclose( + determinants, + torch.ones_like(determinants), + atol=tolerance, + rtol=0.0, + ).all(): + raise ValueError( + f"{field_name} must contain SE(3) transforms with rotation " + "determinant +1." + ) + return value.clone() + + +class SemanticEffectKind(str, Enum): + """Trace-level semantic effect category; clause types define behavior.""" + + ATTACH = "attach" + RELEASE = "release" + TRANSFER = "transfer" + ARTICULATION = "articulation" + REGISTERED = "registered" + + +class SymbolicStateDomain(str, Enum): + """Typed mapping domains owned by :class:`~atomic_actions.TaskState`.""" + + HELD_OBJECT = "held_object" + COORDINATED_HELD_OBJECT = "coordinated_held_object" + ARTICULATION_JOINT = "articulation_joint" + + +@dataclass(frozen=True, slots=True) +class SymbolicStateKey: + """Provider-free key for one exact symbolic ``TaskState`` write. + + The domain makes otherwise similar string and pair addresses impossible to + conflate during static parallel analysis. This contract intentionally + describes only exact keys; dynamic or opaque effects must not manufacture + a guessed key. + """ + + domain: SymbolicStateDomain + address: tuple[str, ...] + + def __post_init__(self) -> None: + if not isinstance(self.domain, SymbolicStateDomain): + raise TypeError("domain must be a SymbolicStateDomain.") + address = tuple(self.address) + expected_size = 1 if self.domain is SymbolicStateDomain.HELD_OBJECT else 2 + if len(address) != expected_size: + raise ValueError( + f"{self.domain.value} symbolic keys require exactly " + f"{expected_size} address component(s)." + ) + for component in address: + _validate_identifier( + component, + field_name=f"{self.domain.value} symbolic key components", + ) + object.__setattr__(self, "address", address) + + @classmethod + def held_object(cls, task_state_key: str) -> SymbolicStateKey: + """Build one held-object mapping key.""" + return cls(SymbolicStateDomain.HELD_OBJECT, (task_state_key,)) + + @classmethod + def coordinated_held_object( + cls, + first_task_state_key: str, + second_task_state_key: str, + ) -> SymbolicStateKey: + """Build one ordered coordinated-held-object mapping key.""" + return cls( + SymbolicStateDomain.COORDINATED_HELD_OBJECT, + (first_task_state_key, second_task_state_key), + ) + + @classmethod + def articulation_joint( + cls, + articulation_id: str, + joint_id: str, + ) -> SymbolicStateKey: + """Build one articulation-joint mapping key.""" + return cls( + SymbolicStateDomain.ARTICULATION_JOINT, + (articulation_id, joint_id), + ) + + @property + def rendered(self) -> str: + """Return a deterministic domain-qualified diagnostic form.""" + return f"{self.domain.value}[{', '.join(repr(item) for item in self.address)}]" + + +@dataclass(frozen=True, slots=True) +class EffectMonitorRef: + """Versioned, declarative reference to an effect-monitor factory.""" + + monitor_id: str + revision: str + params: Mapping[str, EffectMonitorParam] = field(default_factory=dict) + + def __post_init__(self) -> None: + _validate_identifier(self.monitor_id, field_name="EffectMonitorRef.monitor_id") + _validate_identifier(self.revision, field_name="EffectMonitorRef.revision") + object.__setattr__(self, "params", _snapshot_monitor_params(self.params)) + + def snapshot(self) -> EffectMonitorRef: + """Return an independently owned declarative reference.""" + return EffectMonitorRef(self.monitor_id, self.revision, self.params) + + def to_metadata(self) -> dict[str, object]: + """Return a deterministic JSON-safe monitor selection.""" + return { + "monitor_id": self.monitor_id, + "revision": self.revision, + "params": _metadata_value(self.params), + } + + +class EffectEvidenceAddress(ABC): + """Immutable observation address, deliberately separate from command targets.""" + + @property + @abstractmethod + def address_fingerprint(self) -> Hashable: + """Return a stable, hashable physical observation address.""" + + def snapshot(self) -> EffectEvidenceAddress: + """Return an independently owned address of the exact same type.""" + return deepcopy(self) + + +@dataclass(frozen=True, slots=True) +class ControlPartEvidenceAddress(EffectEvidenceAddress): + """Provider-neutral robot control-part observation address.""" + + control_part: str + channel: str + + def __post_init__(self) -> None: + _validate_identifier( + self.control_part, + field_name="ControlPartEvidenceAddress.control_part", + ) + _validate_identifier( + self.channel, field_name="ControlPartEvidenceAddress.channel" + ) + if self.channel not in _EFFECT_CHANNELS: + raise ValueError( + f"Unknown control-part effect channel {self.channel!r}; expected " + f"one of {sorted(_EFFECT_CHANNELS)}." + ) + + @property + def address_fingerprint(self) -> Hashable: + """Return the channel-scoped control-part observation address.""" + return type(self), self.control_part, self.channel + + +@dataclass(frozen=True, slots=True) +class EffectEvidenceSourceRef: + """Versioned provider route plus one immutable observation address.""" + + provider_id: str + revision: str + address: EffectEvidenceAddress + + def __post_init__(self) -> None: + _validate_identifier( + self.provider_id, + field_name="EffectEvidenceSourceRef.provider_id", + ) + _validate_identifier( + self.revision, + field_name="EffectEvidenceSourceRef.revision", + ) + if not isinstance(self.address, EffectEvidenceAddress): + raise TypeError( + "EffectEvidenceSourceRef.address must be an EffectEvidenceAddress." + ) + snapshot = self.address.snapshot() + if type(snapshot) is not type(self.address) or snapshot is self.address: + raise TypeError( + "EffectEvidenceAddress.snapshot() must return an independently " + "owned address of the same exact type." + ) + try: + source_fingerprint = self.address.address_fingerprint + snapshot_fingerprint = snapshot.address_fingerprint + hash(source_fingerprint) + hash(snapshot_fingerprint) + except TypeError as exc: + raise TypeError( + "EffectEvidenceAddress.address_fingerprint must be hashable." + ) from exc + if snapshot_fingerprint != source_fingerprint: + raise ValueError( + "EffectEvidenceAddress.snapshot() must preserve its fingerprint." + ) + object.__setattr__(self, "address", snapshot) + + @property + def source_fingerprint(self) -> Hashable: + """Return the provider-scoped source address fingerprint.""" + return ( + self.provider_id, + self.revision, + type(self.address), + self.address.address_fingerprint, + ) + + def snapshot(self) -> EffectEvidenceSourceRef: + """Return an independently owned source reference.""" + return EffectEvidenceSourceRef( + self.provider_id, + self.revision, + self.address, + ) + + def to_metadata(self) -> dict[str, object]: + """Return the versioned physical observation address as JSON-safe data.""" + return { + "provider_id": self.provider_id, + "revision": self.revision, + "address": _metadata_value(self.address), + } + + +class HeldObjectRelation(str, Enum): + """Expected symbolic held-object state at an effect boundary.""" + + ATTACHED = "attached" + DETACHED = "detached" + + +@dataclass(frozen=True, slots=True) +class HeldObjectStateExpectation: + """Typed individual held-object postcondition.""" + + expectation_id: str + relation: HeldObjectRelation + object_id: str + slot_id: str + resource_id: str + task_state_key: str + + def __post_init__(self) -> None: + for field_name in ( + "expectation_id", + "object_id", + "slot_id", + "resource_id", + "task_state_key", + ): + _validate_identifier( + getattr(self, field_name), + field_name=f"HeldObjectStateExpectation.{field_name}", + ) + if not isinstance(self.relation, HeldObjectRelation): + raise TypeError("relation must be a HeldObjectRelation.") + + def snapshot(self) -> HeldObjectStateExpectation: + """Return an independently constructed state expectation.""" + return HeldObjectStateExpectation( + self.expectation_id, + self.relation, + self.object_id, + self.slot_id, + self.resource_id, + self.task_state_key, + ) + + +@dataclass(frozen=True, slots=True) +class CoordinatedHeldObjectCleanupExpectation: + """Typed removal of one coordinated held-object relation.""" + + expectation_id: str + task_state_keys: tuple[str, str] + + def __post_init__(self) -> None: + _validate_identifier( + self.expectation_id, + field_name="CoordinatedHeldObjectCleanupExpectation.expectation_id", + ) + keys = tuple(self.task_state_keys) + if len(keys) != 2: + raise ValueError("task_state_keys must contain exactly two keys.") + for key in keys: + _validate_identifier(key, field_name="coordinated task-state keys") + if keys[0] == keys[1]: + raise ValueError("Coordinated task-state keys must be distinct.") + object.__setattr__(self, "task_state_keys", keys) + + def snapshot(self) -> CoordinatedHeldObjectCleanupExpectation: + """Return an independently constructed cleanup expectation.""" + return CoordinatedHeldObjectCleanupExpectation( + self.expectation_id, + self.task_state_keys, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class ArticulationJointStateExpectation: + """Future-compatible symbolic articulation-joint postcondition.""" + + expectation_id: str + articulation_id: str + joint_id: str + target_position: torch.Tensor + + def __post_init__(self) -> None: + for field_name in ("expectation_id", "articulation_id", "joint_id"): + _validate_identifier( + getattr(self, field_name), + field_name=f"ArticulationJointStateExpectation.{field_name}", + ) + target = self.target_position + if not isinstance(target, torch.Tensor) or target.dim() not in (1, 2): + raise ValueError("target_position must have shape (J,) or (B, J).") + if target.numel() == 0 or not target.is_floating_point(): + raise TypeError("target_position must be a non-empty floating tensor.") + if not torch.isfinite(target).all(): + raise ValueError("target_position must be finite.") + object.__setattr__(self, "target_position", target.clone()) + + def snapshot(self) -> ArticulationJointStateExpectation: + """Return an independently owned articulation expectation.""" + return ArticulationJointStateExpectation( + self.expectation_id, + self.articulation_id, + self.joint_id, + self.target_position, + ) + + +EffectStateExpectation: TypeAlias = ( + HeldObjectStateExpectation + | CoordinatedHeldObjectCleanupExpectation + | ArticulationJointStateExpectation +) + + +class PoseRelationExpectation(str, Enum): + """Expected relationship to a grounded pose baseline.""" + + MATCHED = "matched" + SEPARATED = "separated" + + +class BinaryEvidenceKind(str, Enum): + """Raw boolean evidence channel.""" + + CONTACT = "contact" + CONSTRAINT = "constraint" + + +class ScalarEvidenceKind(str, Enum): + """Raw scalar physical evidence channel.""" + + FORCE = "force" + WRENCH = "wrench" + + +class ScalarExpectation(str, Enum): + """Expected high/low magnitude band for scalar evidence.""" + + PRESENT = "present" + ABSENT = "absent" + + +def _validate_clause_identity( + clause_id: str, + expectation_id: str, + source: EffectEvidenceSourceRef, +) -> EffectEvidenceSourceRef: + """Validate common clause identity and own its source.""" + _validate_identifier(clause_id, field_name="effect clause_id") + _validate_identifier(expectation_id, field_name="effect expectation_id") + if not isinstance(source, EffectEvidenceSourceRef): + raise TypeError("effect clause source must be an EffectEvidenceSourceRef.") + return source.snapshot() + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseRelationClause: + """Object-to-endpoint pose condition with monitor-owned tolerances.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + expectation: PoseRelationExpectation + baseline_object_to_endpoint: torch.Tensor | None = None + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + if not isinstance(self.expectation, PoseRelationExpectation): + raise TypeError("expectation must be a PoseRelationExpectation.") + baseline = self.baseline_object_to_endpoint + if self.expectation is PoseRelationExpectation.SEPARATED: + if baseline is None: + raise ValueError("A separated pose clause requires a baseline.") + object.__setattr__( + self, + "baseline_object_to_endpoint", + _validate_pose_batch( + baseline, + field_name="PoseRelationClause.baseline_object_to_endpoint", + ), + ) + elif baseline is not None: + raise ValueError( + "A matched pose clause obtains its baseline from the expected " + "held-object StateDelta and must not embed one." + ) + + def snapshot(self) -> PoseRelationClause: + """Return an independently owned pose clause.""" + return PoseRelationClause( + self.clause_id, + self.expectation_id, + self.source, + self.expectation, + self.baseline_object_to_endpoint, + ) + + +@dataclass(frozen=True, slots=True) +class BinaryEffectClause: + """Raw contact or constraint-state condition.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + evidence_kind: BinaryEvidenceKind + expected: bool + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + if not isinstance(self.evidence_kind, BinaryEvidenceKind): + raise TypeError("evidence_kind must be a BinaryEvidenceKind.") + if type(self.expected) is not bool: + raise TypeError("expected must be a bool.") + + def snapshot(self) -> BinaryEffectClause: + """Return an independently owned binary clause.""" + return BinaryEffectClause( + self.clause_id, + self.expectation_id, + self.source, + self.evidence_kind, + self.expected, + ) + + +@dataclass(frozen=True, slots=True) +class ScalarEffectClause: + """Raw force/wrench magnitude condition with monitor-owned thresholds.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + evidence_kind: ScalarEvidenceKind + expectation: ScalarExpectation + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + if not isinstance(self.evidence_kind, ScalarEvidenceKind): + raise TypeError("evidence_kind must be a ScalarEvidenceKind.") + if not isinstance(self.expectation, ScalarExpectation): + raise TypeError("expectation must be a ScalarExpectation.") + + def snapshot(self) -> ScalarEffectClause: + """Return an independently owned scalar clause.""" + return ScalarEffectClause( + self.clause_id, + self.expectation_id, + self.source, + self.evidence_kind, + self.expectation, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateEffectClause: + """Raw articulation/robot joint-position target condition.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + target_position: torch.Tensor + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + target = self.target_position + if not isinstance(target, torch.Tensor) or target.dim() not in (1, 2): + raise ValueError("target_position must have shape (J,) or (B, J).") + if target.numel() == 0 or not target.is_floating_point(): + raise TypeError("target_position must be a non-empty floating tensor.") + if not torch.isfinite(target).all(): + raise ValueError("target_position must be finite.") + object.__setattr__(self, "target_position", target.clone()) + + def snapshot(self) -> JointStateEffectClause: + """Return an independently owned joint-state clause.""" + return JointStateEffectClause( + self.clause_id, + self.expectation_id, + self.source, + self.target_position, + ) + + +EffectClause: TypeAlias = ( + PoseRelationClause + | BinaryEffectClause + | ScalarEffectClause + | JointStateEffectClause +) +_STATE_EXPECTATION_TYPES = ( + HeldObjectStateExpectation, + CoordinatedHeldObjectCleanupExpectation, + ArticulationJointStateExpectation, +) +_CLAUSE_TYPES = ( + PoseRelationClause, + BinaryEffectClause, + ScalarEffectClause, + JointStateEffectClause, +) + + +@dataclass(frozen=True, slots=True, eq=False) +class SemanticEffectSpec: + """Grounded typed physical clauses and symbolic postconditions for one call.""" + + semantic_id: str + effect_kind: SemanticEffectKind + skill_id: str + invocation_id: str | None + invocation_revision: int + env_ids: torch.Tensor + state_expectations: tuple[EffectStateExpectation, ...] + clauses: tuple[EffectClause, ...] + + def __post_init__(self) -> None: + _validate_identifier( + self.semantic_id, + field_name="SemanticEffectSpec.semantic_id", + ) + _validate_identifier(self.skill_id, field_name="SemanticEffectSpec.skill_id") + if not isinstance(self.effect_kind, SemanticEffectKind): + raise TypeError("effect_kind must be a SemanticEffectKind.") + if self.invocation_id is not None: + _validate_identifier( + self.invocation_id, + field_name="SemanticEffectSpec.invocation_id", + ) + if type(self.invocation_revision) is not int or self.invocation_revision < 0: + raise ValueError("invocation_revision must be a non-negative integer.") + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if self.env_ids.dtype != torch.long or self.env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if self.env_ids.numel() == 0: + raise ValueError("env_ids must contain at least one environment ID.") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + object.__setattr__(self, "env_ids", self.env_ids.clone()) + + expectations = tuple(self.state_expectations) + if not expectations or not all( + type(value) in _STATE_EXPECTATION_TYPES for value in expectations + ): + raise TypeError( + "state_expectations must contain exact typed state expectations." + ) + expectation_ids = [value.expectation_id for value in expectations] + if len(set(expectation_ids)) != len(expectation_ids): + raise ValueError("State expectation IDs must be unique.") + held_keys = [ + value.task_state_key + for value in expectations + if type(value) is HeldObjectStateExpectation + ] + if len(set(held_keys)) != len(held_keys): + raise ValueError("Held-object task-state keys must be unique.") + cleanup_keys = [ + value.task_state_keys + for value in expectations + if type(value) is CoordinatedHeldObjectCleanupExpectation + ] + if len(set(cleanup_keys)) != len(cleanup_keys): + raise ValueError("Coordinated cleanup keys must be unique.") + + clauses = tuple(self.clauses) + if not clauses or not all(type(value) in _CLAUSE_TYPES for value in clauses): + raise TypeError("clauses must contain exact typed effect clauses.") + clause_ids = [value.clause_id for value in clauses] + if len(set(clause_ids)) != len(clause_ids): + raise ValueError("Effect clause IDs must be unique.") + unknown_expectations = {value.expectation_id for value in clauses}.difference( + expectation_ids + ) + if unknown_expectations: + raise ValueError( + "Effect clauses reference unknown state expectations: " + f"{sorted(unknown_expectations)}." + ) + uncovered = set(expectation_ids).difference( + value.expectation_id for value in clauses + ) + uncovered.difference_update( + value.expectation_id + for value in expectations + if type(value) is CoordinatedHeldObjectCleanupExpectation + ) + if uncovered: + raise ValueError( + "Every physical state expectation needs at least one clause; " + f"missing {sorted(uncovered)}." + ) + for value in expectations: + if ( + type(value) is ArticulationJointStateExpectation + and value.target_position.dim() == 2 + and value.target_position.shape[0] != self.env_ids.numel() + ): + raise ValueError( + "Batched articulation targets must match env_ids length." + ) + for value in clauses: + if type(value) is PoseRelationClause: + baseline = value.baseline_object_to_endpoint + if baseline is not None and baseline.dim() == 3: + if baseline.shape[0] != self.env_ids.numel(): + raise ValueError( + "Batched pose baselines must match env_ids length." + ) + if baseline.device != self.env_ids.device: + raise ValueError( + "Batched pose baselines and env_ids must share a device." + ) + elif ( + type(value) is JointStateEffectClause + and value.target_position.dim() == 2 + and value.target_position.shape[0] != self.env_ids.numel() + ): + raise ValueError("Batched joint targets must match env_ids length.") + + held_relations = { + value.relation + for value in expectations + if type(value) is HeldObjectStateExpectation + } + if self.effect_kind is SemanticEffectKind.ATTACH and held_relations != { + HeldObjectRelation.ATTACHED + }: + raise ValueError("An attach effect requires only attached state.") + if self.effect_kind is SemanticEffectKind.RELEASE and held_relations != { + HeldObjectRelation.DETACHED + }: + raise ValueError("A release effect requires only detached state.") + if self.effect_kind is SemanticEffectKind.TRANSFER and held_relations != { + HeldObjectRelation.ATTACHED, + HeldObjectRelation.DETACHED, + }: + raise ValueError("A transfer effect requires attached and detached state.") + if self.effect_kind is SemanticEffectKind.ARTICULATION and not any( + type(value) is ArticulationJointStateExpectation for value in expectations + ): + raise ValueError( + "An articulation effect requires an articulation-joint expectation." + ) + + object.__setattr__( + self, + "state_expectations", + tuple(value.snapshot() for value in expectations), + ) + object.__setattr__( + self, + "clauses", + tuple(value.snapshot() for value in clauses), + ) + + def snapshot(self) -> SemanticEffectSpec: + """Return an independently owned grounded effect contract.""" + return SemanticEffectSpec( + semantic_id=self.semantic_id, + effect_kind=self.effect_kind, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + env_ids=self.env_ids, + state_expectations=self.state_expectations, + clauses=self.clauses, + ) + + def to_metadata(self) -> dict[str, object]: + """Return this grounded effect contract as deterministic JSON-safe data.""" + return { + "semantic_id": self.semantic_id, + "effect_kind": self.effect_kind.value, + "skill_id": self.skill_id, + "invocation_id": self.invocation_id, + "invocation_revision": self.invocation_revision, + "env_ids": _metadata_value(self.env_ids), + "state_expectations": [ + _metadata_value(value) for value in self.state_expectations + ], + "clauses": [_metadata_value(value) for value in self.clauses], + } + + def state_expectation(self, expectation_id: str) -> EffectStateExpectation: + """Return an owned state expectation by effect-local ID.""" + for value in self.state_expectations: + if value.expectation_id == expectation_id: + return value.snapshot() + raise KeyError(f"Unknown effect state expectation {expectation_id!r}.") + + def validate_request(self, request: EffectVerificationRequest) -> None: + """Validate execution identity and typed symbolic postconditions.""" + if not isinstance(request, EffectVerificationRequest): + raise TypeError("request must be an EffectVerificationRequest.") + if request.skill_id != self.skill_id: + raise ValueError("Effect request skill_id does not match the spec.") + if request.invocation_id != self.invocation_id: + raise ValueError("Effect request invocation_id does not match the spec.") + if request.invocation_revision != self.invocation_revision: + raise ValueError( + "Effect request invocation_revision does not match the spec." + ) + if request.env_mask.shape != self.env_ids.shape: + raise ValueError("Effect request row count does not match spec env_ids.") + if request.env_mask.device != self.env_ids.device: + raise ValueError( + "Effect request mask and spec env_ids must share a device." + ) + + held_expectations = { + value.task_state_key: value + for value in self.state_expectations + if type(value) is HeldObjectStateExpectation + } + expected_held = request.expected_effects.held_object_updates + if set(expected_held) != set(held_expectations): + raise ValueError( + "Effect request held-object updates must exactly match typed " + "state expectation keys." + ) + for task_state_key, expectation in held_expectations.items(): + candidate = expected_held[task_state_key] + if expectation.relation is HeldObjectRelation.DETACHED: + if candidate is not None: + raise ValueError( + f"Detached expectation {expectation.expectation_id!r} must " + "remove its held-object state." + ) + continue + if not isinstance(candidate, HeldObjectState): + raise ValueError( + f"Attached expectation {expectation.expectation_id!r} requires " + "a HeldObjectState postcondition." + ) + if candidate.semantics.entity_id != expectation.object_id: + raise ValueError( + f"Attached expectation {expectation.expectation_id!r} targets " + "the wrong canonical object." + ) + if candidate.object_to_eef.device != request.env_mask.device: + raise ValueError( + "Attached postcondition poses and request rows must share a device." + ) + if ( + candidate.object_to_eef.dim() == 3 + and candidate.object_to_eef.shape[0] != self.env_ids.numel() + ): + raise ValueError( + "Batched attached postcondition poses must match spec env_ids." + ) + _validate_pose_batch( + candidate.object_to_eef, + field_name=( + f"Attached expectation {expectation.expectation_id!r} pose" + ), + ) + if candidate.env_mask is not None: + if ( + candidate.env_mask.shape != request.env_mask.shape + or candidate.env_mask.device != request.env_mask.device + ): + raise ValueError( + "Attached postcondition masks must match request rows and device." + ) + if (request.env_mask & ~candidate.env_mask).any(): + raise ValueError( + "Attached postconditions must cover every requested row." + ) + + cleanup_expectations = { + value.task_state_keys + for value in self.state_expectations + if type(value) is CoordinatedHeldObjectCleanupExpectation + } + expected_cleanup = request.expected_effects.coordinated_held_object_updates + if set(expected_cleanup) != cleanup_expectations: + raise ValueError( + "Effect request coordinated updates must exactly match typed " + "cleanup expectations." + ) + if any(value is not None for value in expected_cleanup.values()): + raise ValueError( + "Coordinated held-object cleanup expectations may only remove state." + ) + + articulation_expectations = { + (value.articulation_id, value.joint_id): value + for value in self.state_expectations + if type(value) is ArticulationJointStateExpectation + } + articulation_updates = request.expected_effects.articulation_joint_updates + if set(articulation_updates) != set(articulation_expectations): + raise ValueError( + "Articulation-joint updates must exactly match typed state " + "expectations." + ) + for key, expectation in articulation_expectations.items(): + candidate = articulation_updates[key] + if not isinstance(candidate, ArticulationJointState): + raise ValueError( + f"Articulation expectation {expectation.expectation_id!r} " + "requires an ArticulationJointState postcondition." + ) + if candidate.position.device != request.env_mask.device: + raise ValueError( + "Articulation postconditions and request rows must share a device." + ) + if candidate.position.dim() == 2: + if candidate.position.shape[0] != self.env_ids.numel(): + raise ValueError( + "Batched articulation postconditions must match spec env_ids." + ) + positions = candidate.position + else: + positions = candidate.position.unsqueeze(0).expand( + self.env_ids.numel(), -1 + ) + target = expectation.target_position + if target.device != positions.device or target.dtype != positions.dtype: + raise ValueError( + "Articulation postconditions must match target device and dtype." + ) + if target.dim() == 1: + target = target.unsqueeze(0).expand(self.env_ids.numel(), -1) + if positions.shape != target.shape or not torch.equal( + positions[request.env_mask], + target[request.env_mask], + ): + raise ValueError( + f"Articulation expectation {expectation.expectation_id!r} " + "postcondition does not match its target position." + ) + if candidate.env_mask is not None: + if ( + candidate.env_mask.shape != request.env_mask.shape + or candidate.env_mask.device != request.env_mask.device + ): + raise ValueError( + "Articulation postcondition masks must match request rows " + "and device." + ) + if (request.env_mask & ~candidate.env_mask).any(): + raise ValueError( + "Articulation postconditions must cover every requested row." + ) + + +def _validate_evidence_common( + *, + evidence_id: str, + valid: torch.Tensor, + acquisition_errors: tuple[str | None, ...], + timestamp: float, + env_ids: torch.Tensor, + observation_revision: int, + batch_size: int, + device: torch.device, +) -> tuple[torch.Tensor, tuple[str | None, ...], float, torch.Tensor]: + """Validate and own fields shared by every raw evidence batch.""" + _validate_identifier(evidence_id, field_name="effect evidence_id") + if not isinstance(valid, torch.Tensor): + raise TypeError("valid must be a torch.Tensor.") + if valid.dtype != torch.bool or valid.shape != (batch_size,): + raise ValueError("valid must be a bool tensor with shape (B,).") + if valid.device != device: + raise ValueError("valid and evidence payload must share a device.") + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.shape != (batch_size,): + raise ValueError("env_ids must be a torch.long tensor with shape (B,).") + if env_ids.device != device: + raise ValueError("env_ids and evidence payload must share a device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("Evidence env_ids must be unique.") + errors = tuple(acquisition_errors) + if len(errors) != batch_size: + raise ValueError("acquisition_errors must contain one entry per row.") + for row, (row_valid, error) in enumerate(zip(valid.tolist(), errors)): + if row_valid and error is not None: + raise ValueError(f"Valid evidence row {row} must not carry an error.") + if not row_valid and ( + type(error) is not str or not error or error != error.strip() + ): + raise ValueError(f"Invalid evidence row {row} requires a non-empty error.") + if not isinstance(timestamp, (int, float)) or isinstance(timestamp, bool): + raise TypeError("timestamp must be a number.") + normalized_timestamp = float(timestamp) + if not math.isfinite(normalized_timestamp) or normalized_timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if type(observation_revision) is not int or observation_revision < 0: + raise ValueError("observation_revision must be a non-negative integer.") + return valid.clone(), errors, normalized_timestamp, env_ids.clone() + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseRelationEvidenceBatch: + """Raw object-to-endpoint transform observations.""" + + evidence_id: str + object_to_endpoint: torch.Tensor + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + poses = self.object_to_endpoint + if not isinstance(poses, torch.Tensor) or ( + poses.dim() != 3 or poses.shape[0] == 0 or poses.shape[-2:] != (4, 4) + ): + raise ValueError("object_to_endpoint must have shape (B, 4, 4).") + if not poses.is_floating_point(): + raise TypeError("object_to_endpoint must use a floating-point dtype.") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=poses.shape[0], + device=poses.device, + ) + object.__setattr__( + self, + "object_to_endpoint", + _validate_pose_batch( + poses, + field_name="Valid pose-relation evidence", + valid_mask=valid, + ), + ) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> PoseRelationEvidenceBatch: + """Return an independently owned evidence batch.""" + return PoseRelationEvidenceBatch( + self.evidence_id, + self.object_to_endpoint, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw pose evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={"object_to_endpoint": self.object_to_endpoint}, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class BinaryEffectEvidenceBatch: + """Raw per-row contact or constraint-state observations.""" + + evidence_id: str + evidence_kind: BinaryEvidenceKind + values: torch.Tensor + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + if not isinstance(self.evidence_kind, BinaryEvidenceKind): + raise TypeError("evidence_kind must be a BinaryEvidenceKind.") + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if values.dtype != torch.bool or values.dim() != 1 or values.numel() == 0: + raise ValueError("binary evidence values must have bool shape (B,).") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=values.shape[0], + device=values.device, + ) + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> BinaryEffectEvidenceBatch: + """Return an independently owned evidence batch.""" + return BinaryEffectEvidenceBatch( + self.evidence_id, + self.evidence_kind, + self.values, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw binary evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={ + "evidence_kind": self.evidence_kind.value, + "values": self.values, + }, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class ScalarEffectEvidenceBatch: + """Raw per-row force or wrench-magnitude observations.""" + + evidence_id: str + evidence_kind: ScalarEvidenceKind + values: torch.Tensor + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + if not isinstance(self.evidence_kind, ScalarEvidenceKind): + raise TypeError("evidence_kind must be a ScalarEvidenceKind.") + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if values.dim() != 1 or values.numel() == 0 or not values.is_floating_point(): + raise ValueError("scalar evidence values must have floating shape (B,).") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=values.shape[0], + device=values.device, + ) + if not torch.isfinite(values[valid]).all(): + raise ValueError("Valid scalar evidence values must be finite.") + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> ScalarEffectEvidenceBatch: + """Return an independently owned evidence batch.""" + return ScalarEffectEvidenceBatch( + self.evidence_id, + self.evidence_kind, + self.values, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw scalar evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={ + "evidence_kind": self.evidence_kind.value, + "values": self.values, + }, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateEvidenceBatch: + """Raw per-row joint position/velocity observations.""" + + evidence_id: str + positions: torch.Tensor + velocities: torch.Tensor | None + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + positions = self.positions + if not isinstance(positions, torch.Tensor): + raise TypeError("positions must be a torch.Tensor.") + if ( + positions.dim() != 2 + or positions.shape[0] == 0 + or positions.shape[1] == 0 + or not positions.is_floating_point() + ): + raise ValueError("positions must have non-empty floating shape (B, J).") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=positions.shape[0], + device=positions.device, + ) + if not torch.isfinite(positions[valid]).all(): + raise ValueError("Valid joint positions must be finite.") + velocities = self.velocities + if velocities is not None: + if not isinstance(velocities, torch.Tensor): + raise TypeError("velocities must be a torch.Tensor or None.") + if ( + velocities.shape != positions.shape + or velocities.device != positions.device + ): + raise ValueError("velocities must match positions shape and device.") + if not velocities.is_floating_point(): + raise TypeError("velocities must use a floating-point dtype.") + if not torch.isfinite(velocities[valid]).all(): + raise ValueError("Valid joint velocities must be finite.") + object.__setattr__(self, "positions", positions.clone()) + object.__setattr__( + self, + "velocities", + None if velocities is None else velocities.clone(), + ) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> JointStateEvidenceBatch: + """Return an independently owned evidence batch.""" + return JointStateEvidenceBatch( + self.evidence_id, + self.positions, + self.velocities, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw joint-state evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={ + "positions": self.positions, + "velocities": self.velocities, + }, + ) + + +def _evidence_metadata( + batch: EffectEvidenceBatch, + *, + payload: Mapping[str, object], +) -> dict[str, object]: + """Serialize fields shared by all raw physical-evidence batches.""" + return { + "evidence_id": batch.evidence_id, + **{key: _metadata_value(value) for key, value in payload.items()}, + "valid_mask": _metadata_value(batch.valid), + "acquisition_errors": list(batch.acquisition_errors), + "timestamp": batch.timestamp, + "env_ids": _metadata_value(batch.env_ids), + "observation_revision": batch.observation_revision, + } + + +EffectEvidenceBatch: TypeAlias = ( + PoseRelationEvidenceBatch + | BinaryEffectEvidenceBatch + | ScalarEffectEvidenceBatch + | JointStateEvidenceBatch +) +_EVIDENCE_TYPES = ( + PoseRelationEvidenceBatch, + BinaryEffectEvidenceBatch, + ScalarEffectEvidenceBatch, + JointStateEvidenceBatch, +) + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectMonitorDecision: + """Uncorrelated per-row decision; runtime adds the verification ID.""" + + success_mask: torch.Tensor + failure_mask: torch.Tensor + + def __post_init__(self) -> None: + for field_name in ("success_mask", "failure_mask"): + value = getattr(self, field_name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{field_name} must be a one-dimensional bool tensor.") + if self.success_mask.shape != self.failure_mask.shape: + raise ValueError("Decision masks must have equal shapes.") + if self.success_mask.device != self.failure_mask.device: + raise ValueError("Decision masks must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("Decision masks must not overlap.") + object.__setattr__(self, "success_mask", self.success_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + + +class EffectMonitor(ABC): + """Stateful verifier owned by one grounded semantic call.""" + + @property + @abstractmethod + def spec(self) -> SemanticEffectSpec: + """Return an independently owned effect contract.""" + + @property + def resolved_params(self) -> Mapping[str, EffectMonitorParam]: + """Return resolved monitor thresholds for trace metadata. + + Custom monitors may override this property. The empty default keeps + third-party implementations source-compatible while built-ins expose + every effective threshold, including defaults omitted by configuration. + """ + return MappingProxyType({}) + + @abstractmethod + def observe( + self, + request: EffectVerificationRequest, + evidence: Mapping[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + """Consume one synchronized raw observation and decide requested rows.""" + + +class EffectMonitorFactory(ABC): + """Versioned constructor for independent semantic-effect monitors.""" + + monitor_id: ClassVar[str] + revision: ClassVar[str] + + @abstractmethod + def validate_ref(self, ref: EffectMonitorRef) -> None: + """Validate one reference without providers or state creation.""" + + @abstractmethod + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + """Create one independent monitor for ``spec`` and ``ref``.""" + + +def _same_tensor_value(left: torch.Tensor, right: torch.Tensor) -> bool: + """Return whether tensors have identical placement, type, shape, and value.""" + return ( + left.device == right.device + and left.dtype == right.dtype + and left.shape == right.shape + and torch.equal(left, right) + ) + + +def _same_source( + left: EffectEvidenceSourceRef, + right: EffectEvidenceSourceRef, +) -> bool: + return left.source_fingerprint == right.source_fingerprint + + +def _same_state_expectation( + left: EffectStateExpectation, + right: EffectStateExpectation, +) -> bool: + if type(left) is not type(right): + return False + if type(left) is HeldObjectStateExpectation: + assert type(right) is HeldObjectStateExpectation + return left == right + if type(left) is CoordinatedHeldObjectCleanupExpectation: + assert type(right) is CoordinatedHeldObjectCleanupExpectation + return left == right + assert type(left) is ArticulationJointStateExpectation + assert type(right) is ArticulationJointStateExpectation + return ( + left.expectation_id == right.expectation_id + and left.articulation_id == right.articulation_id + and left.joint_id == right.joint_id + and _same_tensor_value(left.target_position, right.target_position) + ) + + +def _same_clause(left: EffectClause, right: EffectClause) -> bool: + if type(left) is not type(right): + return False + if ( + left.clause_id != right.clause_id + or left.expectation_id != right.expectation_id + or not _same_source(left.source, right.source) + ): + return False + if type(left) is PoseRelationClause: + assert type(right) is PoseRelationClause + if left.expectation is not right.expectation: + return False + left_baseline = left.baseline_object_to_endpoint + right_baseline = right.baseline_object_to_endpoint + if left_baseline is None or right_baseline is None: + return left_baseline is None and right_baseline is None + return _same_tensor_value(left_baseline, right_baseline) + if type(left) is BinaryEffectClause: + assert type(right) is BinaryEffectClause + return ( + left.evidence_kind is right.evidence_kind + and left.expected is right.expected + ) + if type(left) is ScalarEffectClause: + assert type(right) is ScalarEffectClause + return ( + left.evidence_kind is right.evidence_kind + and left.expectation is right.expectation + ) + assert type(left) is JointStateEffectClause + assert type(right) is JointStateEffectClause + return _same_tensor_value(left.target_position, right.target_position) + + +def _same_effect_spec(left: SemanticEffectSpec, right: SemanticEffectSpec) -> bool: + """Return whether grounded typed effect specs are exactly equivalent.""" + return ( + left.semantic_id == right.semantic_id + and left.effect_kind is right.effect_kind + and left.skill_id == right.skill_id + and left.invocation_id == right.invocation_id + and left.invocation_revision == right.invocation_revision + and _same_tensor_value(left.env_ids, right.env_ids) + and len(left.state_expectations) == len(right.state_expectations) + and all( + _same_state_expectation(left_value, right_value) + for left_value, right_value in zip( + left.state_expectations, + right.state_expectations, + strict=True, + ) + ) + and len(left.clauses) == len(right.clauses) + and all( + _same_clause(left_value, right_value) + for left_value, right_value in zip( + left.clauses, + right.clauses, + strict=True, + ) + ) + ) + + +class EffectMonitorRegistry: + """Immutable exact-ID/revision registry of monitor factories.""" + + __slots__ = ("_factories",) + + def __init__(self, factories: Iterable[EffectMonitorFactory] = ()) -> None: + normalized: dict[tuple[str, str], EffectMonitorFactory] = {} + for factory in factories: + if not isinstance(factory, EffectMonitorFactory): + raise TypeError("factories must contain EffectMonitorFactory objects.") + monitor_id = _validate_identifier( + factory.monitor_id, + field_name="EffectMonitorFactory.monitor_id", + ) + revision = _validate_identifier( + factory.revision, + field_name="EffectMonitorFactory.revision", + ) + key = monitor_id, revision + if key in normalized: + raise ValueError(f"Duplicate effect-monitor factory {key!r}.") + normalized[key] = factory + self._factories = MappingProxyType(normalized) + + @property + def factories(self) -> Mapping[tuple[str, str], EffectMonitorFactory]: + """Return the immutable exact-key factory mapping.""" + return self._factories + + def resolve(self, ref: EffectMonitorRef) -> EffectMonitorFactory: + """Resolve the exact factory named by a declarative reference.""" + if not isinstance(ref, EffectMonitorRef): + raise TypeError("ref must be an EffectMonitorRef.") + key = ref.monitor_id, ref.revision + try: + return self._factories[key] + except KeyError as exc: + raise KeyError(f"Unknown effect-monitor factory {key!r}.") from exc + + def validate_ref(self, ref: EffectMonitorRef) -> None: + """Validate a reference provider-free through its exact factory.""" + self.resolve(ref).validate_ref(ref) + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + """Create one independent monitor through exact factory lookup.""" + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + factory = self.resolve(ref) + factory.validate_ref(ref) + monitor = factory.create(spec, ref) + if not isinstance(monitor, EffectMonitor): + raise TypeError( + "EffectMonitorFactory.create() must return an EffectMonitor." + ) + monitor_spec = monitor.spec + if not isinstance(monitor_spec, SemanticEffectSpec): + raise TypeError("EffectMonitor.spec must be a SemanticEffectSpec.") + if monitor_spec is spec: + raise TypeError( + "EffectMonitor.spec must return an independently owned contract." + ) + if not _same_effect_spec(monitor_spec, spec): + raise ValueError( + "EffectMonitorFactory created a monitor for a different effect spec." + ) + return monitor + + +@dataclass(frozen=True, slots=True) +class CompositeEffectMonitorCfg: + """Strict hysteresis policy for typed pose/binary/scalar/joint clauses.""" + + attached_translation_threshold: float = 0.02 + attached_rotation_threshold: float = 0.20 + detached_translation_threshold: float = 0.05 + detached_rotation_threshold: float = 0.50 + force_absent_threshold: float = 0.20 + force_present_threshold: float = 1.00 + joint_success_tolerance: float = 0.02 + joint_failure_tolerance: float = 0.10 + consecutive_samples: int = 2 + + def __post_init__(self) -> None: + for field_name in ( + "attached_translation_threshold", + "attached_rotation_threshold", + "detached_translation_threshold", + "detached_rotation_threshold", + "force_absent_threshold", + "force_present_threshold", + "joint_success_tolerance", + "joint_failure_tolerance", + ): + value = getattr(self, field_name) + if not isinstance(value, (int, float)) or isinstance(value, bool): + raise TypeError(f"{field_name} must be a number.") + if not math.isfinite(float(value)) or value < 0.0: + raise ValueError(f"{field_name} must be finite and non-negative.") + object.__setattr__(self, field_name, float(value)) + if self.attached_translation_threshold >= self.detached_translation_threshold: + raise ValueError( + "attached_translation_threshold must be less than " + "detached_translation_threshold." + ) + if self.attached_rotation_threshold >= self.detached_rotation_threshold: + raise ValueError( + "attached_rotation_threshold must be less than " + "detached_rotation_threshold." + ) + if self.detached_rotation_threshold > math.pi: + raise ValueError("detached_rotation_threshold must not exceed pi.") + if self.force_absent_threshold >= self.force_present_threshold: + raise ValueError( + "force_absent_threshold must be less than force_present_threshold." + ) + if self.joint_success_tolerance >= self.joint_failure_tolerance: + raise ValueError( + "joint_success_tolerance must be less than joint_failure_tolerance." + ) + if type(self.consecutive_samples) is not int or self.consecutive_samples <= 0: + raise ValueError("consecutive_samples must be a positive integer.") + + @classmethod + def from_params( + cls, + params: Mapping[str, EffectMonitorParam], + ) -> CompositeEffectMonitorCfg: + """Decode strict declarative factory parameters.""" + allowed = { + "attached_translation_threshold", + "attached_rotation_threshold", + "detached_translation_threshold", + "detached_rotation_threshold", + "force_absent_threshold", + "force_present_threshold", + "joint_success_tolerance", + "joint_failure_tolerance", + "consecutive_samples", + } + unknown = set(params).difference(allowed) + if unknown: + raise ValueError( + f"Unknown composite effect monitor parameters: {sorted(unknown)}." + ) + return cls(**dict(params)) # type: ignore[arg-type] + + def to_metadata(self) -> dict[str, object]: + """Return every resolved hysteresis threshold as JSON-safe data.""" + return { + "attached_translation_threshold": self.attached_translation_threshold, + "attached_rotation_threshold": self.attached_rotation_threshold, + "detached_translation_threshold": self.detached_translation_threshold, + "detached_rotation_threshold": self.detached_rotation_threshold, + "force_absent_threshold": self.force_absent_threshold, + "force_present_threshold": self.force_present_threshold, + "joint_success_tolerance": self.joint_success_tolerance, + "joint_failure_tolerance": self.joint_failure_tolerance, + "consecutive_samples": self.consecutive_samples, + } + + +def _pose_errors( + observed: torch.Tensor, + baseline: torch.Tensor, +) -> tuple[float, float]: + baseline = baseline.to(device=observed.device, dtype=observed.dtype) + translation = torch.linalg.vector_norm(observed[:3, 3] - baseline[:3, 3]) + relative_rotation = baseline[:3, :3].transpose(0, 1) @ observed[:3, :3] + cosine = torch.clamp((torch.trace(relative_rotation) - 1.0) * 0.5, -1.0, 1.0) + rotation = torch.acos(cosine) + return float(translation.item()), float(rotation.item()) + + +class CompositeEffectMonitor(EffectMonitor): + """Stateful conjunction monitor over typed physical evidence clauses.""" + + def __init__( + self, + spec: SemanticEffectSpec, + cfg: CompositeEffectMonitorCfg, + ) -> None: + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + if not isinstance(cfg, CompositeEffectMonitorCfg): + raise TypeError("cfg must be a CompositeEffectMonitorCfg.") + self._spec = spec.snapshot() + self._cfg = cfg + self._attempt_generation: int | None = None + self._active_env_ids: frozenset[int] = frozenset() + self._success_counts: dict[int, int] = {} + self._failure_counts: dict[int, int] = {} + self._last_observations: dict[int, tuple[float, int]] = {} + + @property + def spec(self) -> SemanticEffectSpec: + """Return an independently owned effect contract.""" + return self._spec.snapshot() + + @property + def resolved_params(self) -> Mapping[str, EffectMonitorParam]: + """Return all effective typed-clause thresholds, including defaults.""" + return MappingProxyType(self._cfg.to_metadata()) + + def _prepare_request(self, request: EffectVerificationRequest) -> None: + self._spec.validate_request(request) + active_env_ids = frozenset( + int(value) + for value in self._spec.env_ids[request.env_mask].detach().cpu().tolist() + ) + if self._attempt_generation != request.attempt_generation: + self._attempt_generation = request.attempt_generation + self._active_env_ids = active_env_ids + self._success_counts.clear() + self._failure_counts.clear() + self._last_observations.clear() + return + if not active_env_ids.issubset(self._active_env_ids): + raise ValueError( + "An effect-verification request may only shrink within one " + "attempt_generation." + ) + self._active_env_ids = active_env_ids + self._success_counts = { + env_id: count + for env_id, count in self._success_counts.items() + if env_id in active_env_ids + } + self._failure_counts = { + env_id: count + for env_id, count in self._failure_counts.items() + if env_id in active_env_ids + } + self._last_observations = { + env_id: observation + for env_id, observation in self._last_observations.items() + if env_id in active_env_ids + } + + @staticmethod + def _validate_evidence_type( + clause: EffectClause, + batch: EffectEvidenceBatch, + ) -> None: + if type(clause) is PoseRelationClause: + if type(batch) is not PoseRelationEvidenceBatch: + raise TypeError("PoseRelationClause requires pose evidence.") + return + if type(clause) is BinaryEffectClause: + if type(batch) is not BinaryEffectEvidenceBatch: + raise TypeError("BinaryEffectClause requires binary evidence.") + if batch.evidence_kind is not clause.evidence_kind: + raise ValueError("Binary evidence kind does not match its clause.") + return + if type(clause) is ScalarEffectClause: + if type(batch) is not ScalarEffectEvidenceBatch: + raise TypeError("ScalarEffectClause requires scalar evidence.") + if batch.evidence_kind is not clause.evidence_kind: + raise ValueError("Scalar evidence kind does not match its clause.") + return + if type(batch) is not JointStateEvidenceBatch: + raise TypeError("JointStateEffectClause requires joint-state evidence.") + + def _normalize_evidence( + self, + evidence: Mapping[str, EffectEvidenceBatch], + *, + requested_at: float, + deadline: float, + ) -> tuple[Mapping[str, EffectEvidenceBatch], tuple[int, ...]]: + if not isinstance(evidence, Mapping): + raise TypeError("evidence must be a mapping.") + clause_by_id = {value.clause_id: value for value in self._spec.clauses} + if set(evidence) != set(clause_by_id): + raise ValueError("Evidence keys must exactly match effect clause IDs.") + normalized: dict[str, EffectEvidenceBatch] = {} + first: EffectEvidenceBatch | None = None + for clause_id, batch in evidence.items(): + if type(batch) not in _EVIDENCE_TYPES: + raise TypeError( + "evidence values must be typed effect evidence batches." + ) + if batch.evidence_id != clause_id: + raise ValueError("Evidence keys must match batch evidence_id values.") + self._validate_evidence_type(clause_by_id[clause_id], batch) + if batch.timestamp < requested_at: + raise ValueError("Effect evidence must not predate the request.") + if batch.timestamp > deadline: + raise ValueError( + "Effect evidence must not exceed the request deadline." + ) + if first is None: + first = batch + elif ( + batch.timestamp != first.timestamp + or batch.observation_revision != first.observation_revision + or not torch.equal(batch.env_ids, first.env_ids) + ): + raise ValueError( + "All effect evidence must share timestamp, observation_revision, " + "and env_ids." + ) + normalized[clause_id] = batch.snapshot() + assert first is not None + known_env_ids = set(self._spec.env_ids.detach().cpu().tolist()) + observed_env_ids = tuple(int(value) for value in first.env_ids.cpu().tolist()) + if not set(observed_env_ids).issubset(known_env_ids): + raise ValueError("Evidence contains env_ids outside the effect spec.") + missing = self._active_env_ids.difference(observed_env_ids) + if missing: + for env_id in missing: + self._success_counts[env_id] = 0 + self._failure_counts[env_id] = 0 + raise ValueError( + "Evidence must cover every active request env_id exactly once; " + f"missing {sorted(missing)}. Acquisition failures must be explicit " + "valid=False rows." + ) + return MappingProxyType(normalized), observed_env_ids + + def _pose_baseline( + self, + clause: PoseRelationClause, + request: EffectVerificationRequest, + spec_row: int, + ) -> torch.Tensor: + baseline = clause.baseline_object_to_endpoint + if baseline is None: + expectation = self._spec.state_expectation(clause.expectation_id) + if type(expectation) is not HeldObjectStateExpectation: + raise ValueError( + "A request-derived pose baseline requires a held-object " + "state expectation." + ) + candidate = request.expected_effects.held_object_updates[ + expectation.task_state_key + ] + assert isinstance(candidate, HeldObjectState) + baseline = candidate.object_to_eef + return baseline if baseline.dim() == 2 else baseline[spec_row] + + def _classify_clause( + self, + clause: EffectClause, + batch: EffectEvidenceBatch, + *, + evidence_row: int, + spec_row: int, + request: EffectVerificationRequest, + ) -> int: + """Return 1 expected, -1 contradicted, or 0 unresolved.""" + if not bool(batch.valid[evidence_row].item()): + return 0 + if type(clause) is PoseRelationClause: + assert type(batch) is PoseRelationEvidenceBatch + observed = batch.object_to_endpoint[evidence_row] + baseline = self._pose_baseline(clause, request, spec_row) + translation_error, rotation_error = _pose_errors(observed, baseline) + matched = ( + translation_error <= self._cfg.attached_translation_threshold + and rotation_error <= self._cfg.attached_rotation_threshold + ) + separated = ( + translation_error >= self._cfg.detached_translation_threshold + or rotation_error >= self._cfg.detached_rotation_threshold + ) + if clause.expectation is PoseRelationExpectation.MATCHED: + return 1 if matched else (-1 if separated else 0) + return 1 if separated else (-1 if matched else 0) + if type(clause) is BinaryEffectClause: + assert type(batch) is BinaryEffectEvidenceBatch + return ( + 1 if bool(batch.values[evidence_row].item()) is clause.expected else -1 + ) + if type(clause) is ScalarEffectClause: + assert type(batch) is ScalarEffectEvidenceBatch + magnitude = abs(float(batch.values[evidence_row].item())) + present = magnitude >= self._cfg.force_present_threshold + absent = magnitude <= self._cfg.force_absent_threshold + if clause.expectation is ScalarExpectation.PRESENT: + return 1 if present else (-1 if absent else 0) + return 1 if absent else (-1 if present else 0) + assert type(clause) is JointStateEffectClause + assert type(batch) is JointStateEvidenceBatch + target = clause.target_position + if target.dim() == 2: + target = target[spec_row] + observed = batch.positions[evidence_row] + if target.shape != observed.shape: + raise ValueError("Joint evidence width does not match its clause target.") + error = float(torch.max(torch.abs(observed - target)).item()) + if error <= self._cfg.joint_success_tolerance: + return 1 + if error >= self._cfg.joint_failure_tolerance: + return -1 + return 0 + + def observe( + self, + request: EffectVerificationRequest, + evidence: Mapping[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + """Update typed-clause hysteresis and decide current request rows.""" + self._prepare_request(request) + batches, observed_env_ids = self._normalize_evidence( + evidence, + requested_at=request.requested_at, + deadline=request.deadline, + ) + success_mask = torch.zeros_like(request.env_mask) + failure_mask = torch.zeros_like(request.env_mask) + spec_rows = { + int(env_id): row + for row, env_id in enumerate(self._spec.env_ids.detach().cpu().tolist()) + } + request_rows = { + int(env_id): row + for row, env_id in enumerate(self._spec.env_ids.detach().cpu().tolist()) + if bool(request.env_mask[row].item()) + } + first_batch = next(iter(batches.values())) + observation_token = ( + first_batch.timestamp, + first_batch.observation_revision, + ) + for env_id in self._active_env_ids: + previous = self._last_observations.get(env_id) + if previous is None: + continue + if observation_token[0] < previous[0]: + raise ValueError( + "Evidence timestamps must be monotonic for every active env_id." + ) + if observation_token[1] < previous[1]: + raise ValueError( + "Evidence observation_revision values must be monotonic for " + "every active env_id." + ) + + clauses_by_expectation: dict[str, list[EffectClause]] = {} + for clause in self._spec.clauses: + clauses_by_expectation.setdefault(clause.expectation_id, []).append(clause) + physical_expectation_ids = set(clauses_by_expectation) + + for evidence_row, env_id in enumerate(observed_env_ids): + request_row = request_rows.get(env_id) + if request_row is None: + continue + if self._last_observations.get(env_id) == observation_token: + continue + self._last_observations[env_id] = observation_token + spec_row = spec_rows[env_id] + expected_groups = True + contradicted_group = False + for expectation_id in physical_expectation_ids: + classifications = [ + self._classify_clause( + clause, + batches[clause.clause_id], + evidence_row=evidence_row, + spec_row=spec_row, + request=request, + ) + for clause in clauses_by_expectation[expectation_id] + ] + group_expected = all(value == 1 for value in classifications) + group_contradicted = any(value == -1 for value in classifications) + expected_groups = expected_groups and group_expected + contradicted_group = contradicted_group or group_contradicted + if expected_groups: + self._success_counts[env_id] = self._success_counts.get(env_id, 0) + 1 + self._failure_counts[env_id] = 0 + elif contradicted_group: + self._failure_counts[env_id] = self._failure_counts.get(env_id, 0) + 1 + self._success_counts[env_id] = 0 + else: + self._success_counts[env_id] = 0 + self._failure_counts[env_id] = 0 + if self._success_counts.get(env_id, 0) >= self._cfg.consecutive_samples: + success_mask[request_row] = True + elif self._failure_counts.get(env_id, 0) >= self._cfg.consecutive_samples: + failure_mask[request_row] = True + success_mask &= request.env_mask + failure_mask &= request.env_mask + return EffectMonitorDecision(success_mask, failure_mask) + + +class CompositeEffectMonitorFactory(EffectMonitorFactory): + """Factory for the built-in typed-clause monitor.""" + + monitor_id = COMPOSITE_EFFECT_MONITOR_ID + revision = COMPOSITE_EFFECT_MONITOR_REVISION + + def validate_ref(self, ref: EffectMonitorRef) -> None: + """Validate exact built-in selection and typed thresholds.""" + if not isinstance(ref, EffectMonitorRef): + raise TypeError("ref must be an EffectMonitorRef.") + if (ref.monitor_id, ref.revision) != (self.monitor_id, self.revision): + raise ValueError("EffectMonitorRef does not select this exact factory.") + CompositeEffectMonitorCfg.from_params(ref.params) + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> CompositeEffectMonitor: + """Create one independently stateful typed-clause monitor.""" + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + self.validate_ref(ref) + return CompositeEffectMonitor( + spec.snapshot(), + CompositeEffectMonitorCfg.from_params(ref.params), + ) + + +__all__ = [ + "ArticulationJointStateExpectation", + "BinaryEffectClause", + "BinaryEffectEvidenceBatch", + "BinaryEvidenceKind", + "COMPOSITE_EFFECT_MONITOR_ID", + "COMPOSITE_EFFECT_MONITOR_REVISION", + "CONTACT_EFFECT_CHANNEL", + "CONSTRAINT_EFFECT_CHANNEL", + "CONTROL_PART_EVIDENCE_PROVIDER_ID", + "CONTROL_PART_EVIDENCE_PROVIDER_REVISION", + "CompositeEffectMonitor", + "CompositeEffectMonitorCfg", + "CompositeEffectMonitorFactory", + "ControlPartEvidenceAddress", + "CoordinatedHeldObjectCleanupExpectation", + "EffectClause", + "EffectEvidenceAddress", + "EffectEvidenceBatch", + "EffectEvidenceSourceRef", + "EffectMonitor", + "EffectMonitorDecision", + "EffectMonitorFactory", + "EffectMonitorParam", + "EffectMonitorRef", + "EffectMonitorRegistry", + "EffectStateExpectation", + "FORCE_EFFECT_CHANNEL", + "HeldObjectRelation", + "HeldObjectStateExpectation", + "JOINT_STATE_EFFECT_CHANNEL", + "JointStateEffectClause", + "JointStateEvidenceBatch", + "POSE_RELATION_EFFECT_CHANNEL", + "PoseRelationClause", + "PoseRelationEvidenceBatch", + "PoseRelationExpectation", + "ScalarEffectClause", + "ScalarEffectEvidenceBatch", + "ScalarEvidenceKind", + "ScalarExpectation", + "SemanticEffectKind", + "SemanticEffectSpec", + "SymbolicStateDomain", + "SymbolicStateKey", +] diff --git a/embodichain/lab/sim/skills/evidence.py b/embodichain/lab/sim/skills/evidence.py new file mode 100644 index 000000000..56fe7e94a --- /dev/null +++ b/embodichain/lab/sim/skills/evidence.py @@ -0,0 +1,1467 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Backend-neutral acquisition ports for typed semantic-effect evidence.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from collections.abc import Callable, Iterable, Mapping, Sequence +from dataclasses import dataclass +import math +from types import MappingProxyType +from typing import ClassVar, Protocol, TypeAlias, runtime_checkable + +import torch + +from embodichain.utils.math import pose_inv + +from ..atomic_actions import SceneProvider, SceneSnapshot +from .effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress, + CoordinatedHeldObjectCleanupExpectation, + EffectClause, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectStateExpectation, + FORCE_EFFECT_CHANNEL, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + JointStateEvidenceBatch, + POSE_RELATION_EFFECT_CHANNEL, + PoseRelationClause, + PoseRelationEvidenceBatch, + ScalarEffectClause, + ScalarEffectEvidenceBatch, + ScalarEvidenceKind, + SemanticEffectSpec, +) +from .scene import ( + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, +) + +_EFFECT_EXPECTATION_TYPES = ( + HeldObjectStateExpectation, + CoordinatedHeldObjectCleanupExpectation, + ArticulationJointStateExpectation, +) +_EFFECT_BATCH_TYPES = ( + PoseRelationEvidenceBatch, + BinaryEffectEvidenceBatch, + ScalarEffectEvidenceBatch, + JointStateEvidenceBatch, +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty 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 + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectEvidenceCollectionContext: + """One synchronized acquisition tick shared by all effect clauses. + + Args: + timestamp: Non-negative backend observation time. + observation_revision: Monotonic revision chosen by the runtime port. + env_ids: Ordered environment correlation IDs to observe. + """ + + timestamp: float + observation_revision: int + env_ids: torch.Tensor + + def __post_init__(self) -> None: + if isinstance(self.timestamp, bool) or not isinstance( + self.timestamp, (int, float) + ): + raise TypeError("timestamp must be a number.") + timestamp = float(self.timestamp) + if not math.isfinite(timestamp) or timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if type(self.observation_revision) is not int or self.observation_revision < 0: + raise ValueError("observation_revision must be a non-negative integer.") + env_ids = self.env_ids + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.dim() != 1 or env_ids.numel() == 0: + raise ValueError( + "env_ids must be a non-empty one-dimensional int64 tensor." + ) + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids.clone()) + + def snapshot(self) -> EffectEvidenceCollectionContext: + """Return an independently owned acquisition context.""" + return EffectEvidenceCollectionContext( + self.timestamp, + self.observation_revision, + self.env_ids, + ) + + +def _snapshot_expectation( + expectation: EffectStateExpectation, +) -> EffectStateExpectation: + """Validate and own one exact typed effect expectation.""" + if type(expectation) not in _EFFECT_EXPECTATION_TYPES: + raise TypeError("expectation must be an exact typed effect expectation.") + return expectation.snapshot() + + +class EffectEvidenceQuery(ABC): + """Typed request for the raw evidence of exactly one effect clause.""" + + @property + @abstractmethod + def evidence_id(self) -> str: + """Return the clause-local evidence identifier.""" + + @property + @abstractmethod + def source(self) -> EffectEvidenceSourceRef: + """Return an owned exact provider route and physical address.""" + + @property + @abstractmethod + def expectation(self) -> EffectStateExpectation: + """Return an owned symbolic expectation related to this query.""" + + @abstractmethod + def snapshot(self) -> EffectEvidenceQuery: + """Return an independently owned query of the exact same type.""" + + +def _validate_query( + clause: EffectClause, + expectation: EffectStateExpectation, +) -> EffectStateExpectation: + """Validate common clause/expectation correlation.""" + owned = _snapshot_expectation(expectation) + if clause.expectation_id != owned.expectation_id: + raise ValueError("Query clause and expectation IDs must match.") + return owned + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseRelationEvidenceQuery(EffectEvidenceQuery): + """Query for an object's pose relative to a resource endpoint.""" + + clause: PoseRelationClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not PoseRelationClause: + raise TypeError("clause must be a PoseRelationClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> PoseRelationEvidenceQuery: + """Return an independently owned pose query.""" + return PoseRelationEvidenceQuery(self.clause, self._expectation) + + +@dataclass(frozen=True, slots=True, eq=False) +class BinaryEffectEvidenceQuery(EffectEvidenceQuery): + """Query for one raw contact or constraint boolean.""" + + clause: BinaryEffectClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not BinaryEffectClause: + raise TypeError("clause must be a BinaryEffectClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> BinaryEffectEvidenceQuery: + """Return an independently owned binary query.""" + return BinaryEffectEvidenceQuery(self.clause, self._expectation) + + +@dataclass(frozen=True, slots=True, eq=False) +class ScalarEffectEvidenceQuery(EffectEvidenceQuery): + """Query for one raw force or wrench magnitude.""" + + clause: ScalarEffectClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not ScalarEffectClause: + raise TypeError("clause must be a ScalarEffectClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> ScalarEffectEvidenceQuery: + """Return an independently owned scalar query.""" + return ScalarEffectEvidenceQuery(self.clause, self._expectation) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateEvidenceQuery(EffectEvidenceQuery): + """Query for current joint positions and optional velocities.""" + + clause: JointStateEffectClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not JointStateEffectClause: + raise TypeError("clause must be a JointStateEffectClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> JointStateEvidenceQuery: + """Return an independently owned joint-state query.""" + return JointStateEvidenceQuery(self.clause, self._expectation) + + +EffectEvidenceQueryValue: TypeAlias = ( + PoseRelationEvidenceQuery + | BinaryEffectEvidenceQuery + | ScalarEffectEvidenceQuery + | JointStateEvidenceQuery +) +"""Closed set of typed clause queries accepted by evidence providers.""" + + +def build_effect_evidence_queries( + spec: SemanticEffectSpec, +) -> tuple[EffectEvidenceQueryValue, ...]: + """Build one independently owned typed query per effect clause. + + Args: + spec: Grounded semantic effect contract. + + Returns: + Queries in the contract's deterministic clause order. + """ + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + queries: list[EffectEvidenceQueryValue] = [] + for clause in spec.clauses: + expectation = spec.state_expectation(clause.expectation_id) + if type(clause) is PoseRelationClause: + queries.append(PoseRelationEvidenceQuery(clause, expectation)) + elif type(clause) is BinaryEffectClause: + queries.append(BinaryEffectEvidenceQuery(clause, expectation)) + elif type(clause) is ScalarEffectClause: + queries.append(ScalarEffectEvidenceQuery(clause, expectation)) + elif type(clause) is JointStateEffectClause: + queries.append(JointStateEvidenceQuery(clause, expectation)) + else: + raise TypeError(f"Unsupported effect clause type {type(clause).__name__}.") + return tuple(queries) + + +class EffectEvidenceProvider(ABC): + """Versioned backend port that acquires a group of exact-source queries.""" + + provider_id: ClassVar[str] + revision: ClassVar[str] + + @abstractmethod + def collect( + self, + queries: tuple[EffectEvidenceQueryValue, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire one synchronized batch for every supplied query.""" + + +class EffectEvidenceProviderRegistry: + """Immutable exact-ID/revision registry of live evidence providers.""" + + __slots__ = ("_providers",) + + def __init__(self, providers: Iterable[EffectEvidenceProvider] = ()) -> None: + normalized: dict[tuple[str, str], EffectEvidenceProvider] = {} + for provider in providers: + if not isinstance(provider, EffectEvidenceProvider): + raise TypeError( + "providers must contain EffectEvidenceProvider instances." + ) + provider_id = _validate_identifier( + provider.provider_id, + field_name="EffectEvidenceProvider.provider_id", + ) + revision = _validate_identifier( + provider.revision, + field_name="EffectEvidenceProvider.revision", + ) + key = provider_id, revision + if key in normalized: + raise ValueError(f"Duplicate effect-evidence provider {key!r}.") + normalized[key] = provider + self._providers = MappingProxyType(normalized) + + @property + def providers(self) -> Mapping[tuple[str, str], EffectEvidenceProvider]: + """Return the immutable exact-key provider mapping.""" + return self._providers + + def resolve(self, source: EffectEvidenceSourceRef) -> EffectEvidenceProvider: + """Resolve the exact provider selected by ``source``. + + Args: + source: Versioned evidence route from one effect clause. + + Returns: + Registered provider with the exact ID and revision. + + Raises: + KeyError: If no exact provider version is installed. + """ + if not isinstance(source, EffectEvidenceSourceRef): + raise TypeError("source must be an EffectEvidenceSourceRef.") + key = source.provider_id, source.revision + try: + return self._providers[key] + except KeyError as exc: + raise KeyError( + f"Unknown effect-evidence provider {key!r}; exact versions are " + "required." + ) from exc + + +def _expected_batch_type(query: EffectEvidenceQueryValue) -> type[EffectEvidenceBatch]: + """Return the exact evidence batch type required by one query.""" + if type(query) is PoseRelationEvidenceQuery: + return PoseRelationEvidenceBatch + if type(query) is BinaryEffectEvidenceQuery: + return BinaryEffectEvidenceBatch + if type(query) is ScalarEffectEvidenceQuery: + return ScalarEffectEvidenceBatch + if type(query) is JointStateEvidenceQuery: + return JointStateEvidenceBatch + raise TypeError(f"Unsupported effect evidence query {type(query).__name__}.") + + +class EffectEvidenceCollector: + """Dispatch and normalize a synchronized observation for one effect spec.""" + + __slots__ = ("_registry",) + + def __init__(self, registry: EffectEvidenceProviderRegistry) -> None: + if not isinstance(registry, EffectEvidenceProviderRegistry): + raise TypeError("registry must be an EffectEvidenceProviderRegistry.") + self._registry = registry + + @property + def registry(self) -> EffectEvidenceProviderRegistry: + """Return the immutable provider registry.""" + return self._registry + + def collect( + self, + spec: SemanticEffectSpec, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire and strictly synchronize evidence for every effect clause. + + Args: + spec: Grounded semantic effect contract. + timestamp: Backend observation time for this acquisition tick. + observation_revision: Runtime-owned observation revision. + env_ids: Optional ordered subset of ``spec.env_ids``. Acquisition + failures must remain present as rows with ``valid=False``. + + Returns: + Immutable mapping keyed exactly by effect clause ID. + """ + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + selected_env_ids = spec.env_ids if env_ids is None else env_ids + context = EffectEvidenceCollectionContext( + timestamp, + observation_revision, + selected_env_ids, + ) + known_ids = set(spec.env_ids.detach().cpu().tolist()) + selected_ids = set(context.env_ids.detach().cpu().tolist()) + if not selected_ids.issubset(known_ids): + raise ValueError("env_ids must be a subset of the effect spec env_ids.") + + queries = build_effect_evidence_queries(spec) + groups: dict[ + tuple[str, str], + list[EffectEvidenceQueryValue], + ] = {} + for query in queries: + source = query.source + self._registry.resolve(source) + groups.setdefault((source.provider_id, source.revision), []).append(query) + + batches: dict[str, EffectEvidenceBatch] = {} + for key, grouped_queries in groups.items(): + provider = self._registry.providers[key] + owned_queries = tuple(query.snapshot() for query in grouped_queries) + supplied = provider.collect(owned_queries, context.snapshot()) + if not isinstance(supplied, Mapping): + raise TypeError( + f"Effect-evidence provider {key!r} must return a mapping." + ) + expected_ids = {query.evidence_id for query in grouped_queries} + if set(supplied) != expected_ids: + raise ValueError( + f"Effect-evidence provider {key!r} must return exactly query " + f"IDs {sorted(expected_ids)}; got {sorted(supplied)}." + ) + for query in grouped_queries: + batch = supplied[query.evidence_id] + expected_type = _expected_batch_type(query) + if type(batch) is not expected_type: + raise TypeError( + f"Evidence {query.evidence_id!r} must be " + f"{expected_type.__name__}." + ) + if batch.evidence_id != query.evidence_id: + raise ValueError( + "Evidence mapping keys must match batch evidence_id values." + ) + if batch.timestamp != context.timestamp: + raise ValueError( + "Every evidence batch must use the collection timestamp." + ) + if batch.observation_revision != context.observation_revision: + raise ValueError( + "Every evidence batch must use the collection revision." + ) + if batch.env_ids.device != context.env_ids.device or not torch.equal( + batch.env_ids, context.env_ids + ): + raise ValueError( + "Every evidence batch must use the ordered collection env_ids." + ) + if type(query) is BinaryEffectEvidenceQuery: + assert type(batch) is BinaryEffectEvidenceBatch + if batch.evidence_kind is not query.clause.evidence_kind: + raise ValueError("Binary evidence kind must match its query.") + if type(query) is ScalarEffectEvidenceQuery: + assert type(batch) is ScalarEffectEvidenceBatch + if batch.evidence_kind is not query.clause.evidence_kind: + raise ValueError("Scalar evidence kind must match its query.") + batches[query.evidence_id] = batch.snapshot() + + expected_all = {query.evidence_id for query in queries} + if set(batches) != expected_all: + raise AssertionError("Evidence dispatch lost one or more effect clauses.") + return MappingProxyType(batches) + + +@dataclass(frozen=True, slots=True, eq=False) +class BinaryEffectObservation: + """Callback-owned raw binary values with explicit row validity.""" + + values: torch.Tensor + valid: torch.Tensor | None = None + acquisition_errors: tuple[str | None, ...] = () + + def __post_init__(self) -> None: + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if values.dtype != torch.bool or values.dim() != 1 or values.numel() == 0: + raise ValueError("values must have non-empty bool shape (B,).") + valid = torch.ones_like(values) if self.valid is None else self.valid + if ( + not isinstance(valid, torch.Tensor) + or valid.dtype != torch.bool + or valid.shape != values.shape + or valid.device != values.device + ): + raise ValueError("valid must match values shape, bool dtype, and device.") + errors = self.acquisition_errors or (None,) * values.shape[0] + _validate_observation_errors(valid, errors) + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid.clone()) + object.__setattr__(self, "acquisition_errors", tuple(errors)) + + +@dataclass(frozen=True, slots=True, eq=False) +class ScalarEffectObservation: + """Callback-owned raw scalar values with explicit row validity.""" + + values: torch.Tensor + valid: torch.Tensor | None = None + acquisition_errors: tuple[str | None, ...] = () + + def __post_init__(self) -> None: + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if not values.is_floating_point() or values.dim() != 1 or values.numel() == 0: + raise ValueError("values must have non-empty floating shape (B,).") + valid = ( + torch.ones_like(values, dtype=torch.bool) + if self.valid is None + else self.valid + ) + if ( + not isinstance(valid, torch.Tensor) + or valid.dtype != torch.bool + or valid.shape != values.shape + or valid.device != values.device + ): + raise ValueError("valid must match values shape, bool dtype, and device.") + if not torch.isfinite(values[valid]).all(): + raise ValueError("Valid scalar observations must be finite.") + errors = self.acquisition_errors or (None,) * values.shape[0] + _validate_observation_errors(valid, errors) + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid.clone()) + object.__setattr__(self, "acquisition_errors", tuple(errors)) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateObservation: + """Callback-owned raw joint state with explicit row validity.""" + + positions: torch.Tensor + velocities: torch.Tensor | None = None + valid: torch.Tensor | None = None + acquisition_errors: tuple[str | None, ...] = () + + def __post_init__(self) -> None: + positions = self.positions + if not isinstance(positions, torch.Tensor): + raise TypeError("positions must be a torch.Tensor.") + if ( + not positions.is_floating_point() + or positions.dim() != 2 + or positions.shape[0] == 0 + or positions.shape[1] == 0 + ): + raise ValueError("positions must have non-empty floating shape (B, J).") + valid = ( + torch.ones(positions.shape[0], dtype=torch.bool, device=positions.device) + if self.valid is None + else self.valid + ) + if ( + not isinstance(valid, torch.Tensor) + or valid.dtype != torch.bool + or valid.shape != (positions.shape[0],) + or valid.device != positions.device + ): + raise ValueError("valid must have bool shape (B,) on the positions device.") + if not torch.isfinite(positions[valid]).all(): + raise ValueError("Valid joint positions must be finite.") + velocities = self.velocities + if velocities is not None: + if not isinstance(velocities, torch.Tensor): + raise TypeError("velocities must be a torch.Tensor or None.") + if ( + velocities.shape != positions.shape + or velocities.device != positions.device + ): + raise ValueError("velocities must match positions shape and device.") + if not velocities.is_floating_point(): + raise TypeError("velocities must use a floating-point dtype.") + if not torch.isfinite(velocities[valid]).all(): + raise ValueError("Valid joint velocities must be finite.") + errors = self.acquisition_errors or (None,) * positions.shape[0] + _validate_observation_errors(valid, errors) + object.__setattr__(self, "positions", positions.clone()) + object.__setattr__( + self, + "velocities", + None if velocities is None else velocities.clone(), + ) + object.__setattr__(self, "valid", valid.clone()) + object.__setattr__(self, "acquisition_errors", tuple(errors)) + + +def _validate_observation_errors( + valid: torch.Tensor, + errors: Sequence[str | None], +) -> None: + """Validate explicit per-row acquisition errors.""" + if len(errors) != valid.shape[0]: + raise ValueError("acquisition_errors must contain one entry per row.") + for row, (row_valid, error) in enumerate(zip(valid.tolist(), errors)): + if row_valid and error is not None: + raise ValueError(f"Valid observation row {row} must not carry an error.") + if not row_valid and ( + type(error) is not str or not error or error != error.strip() + ): + raise ValueError( + f"Invalid observation row {row} requires a non-empty error." + ) + + +BinaryObservationCallback: TypeAlias = Callable[ + [BinaryEffectEvidenceQuery, EffectEvidenceCollectionContext], + BinaryEffectObservation, +] +ScalarObservationCallback: TypeAlias = Callable[ + [ScalarEffectEvidenceQuery, EffectEvidenceCollectionContext], + ScalarEffectObservation, +] +ArticulationJointObservationCallback: TypeAlias = Callable[ + [JointStateEvidenceQuery, EffectEvidenceCollectionContext], + JointStateObservation, +] + + +class SceneArticulationEvidenceProvider(EffectEvidenceProvider): + """Typed adapter for scene-articulation joint-state observations. + + Integrations inject either a direct observer or a :class:`SceneProvider` + whose snapshot contains ``ObservedArticulationJointState`` values. + The adapter never discovers live simulator objects from an environment. + Repeated clauses share one synchronized snapshot and one sample per exact + physical address. + """ + + provider_id = SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + revision = SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + + def __init__( + self, + observer: ArticulationJointObservationCallback | None = None, + *, + scene_provider: SceneProvider | None = None, + ) -> None: + if (observer is None) == (scene_provider is None): + raise ValueError( + "Exactly one of observer or scene_provider must be supplied." + ) + if observer is not None and not callable(observer): + raise TypeError("observer must be callable or None.") + if scene_provider is not None and not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider or be None.") + self._observer = observer + self._scene_provider = scene_provider + + def collect( + self, + queries: tuple[EffectEvidenceQueryValue, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + """Collect synchronized joint state for exact scene addresses.""" + if not isinstance(context, EffectEvidenceCollectionContext): + raise TypeError("context must be an EffectEvidenceCollectionContext.") + if not isinstance(queries, tuple) or not queries: + raise ValueError("queries must be a non-empty tuple.") + owned_queries = tuple(self._validate_query(query) for query in queries) + if len({query.evidence_id for query in owned_queries}) != len(owned_queries): + raise ValueError("queries must have unique evidence IDs.") + + observations: dict[object, JointStateObservation] = {} + batches: dict[str, JointStateEvidenceBatch] = {} + scene_snapshot: SceneSnapshot | None = None + if self._scene_provider is not None: + scene_snapshot = self._scene_provider.snapshot( + timestamp=context.timestamp, + env_ids=context.env_ids.clone(), + ) + if not isinstance(scene_snapshot, SceneSnapshot): + raise TypeError("scene_provider.snapshot() must return SceneSnapshot.") + if scene_snapshot.timestamp != context.timestamp: + raise ValueError( + "Scene snapshot timestamp must match the evidence tick." + ) + for query in owned_queries: + address = query.source.address + assert type(address) is ArticulationJointEvidenceAddress + fingerprint = address.address_fingerprint + observation = observations.get(fingerprint) + if observation is None: + supplied = ( + self._observe_scene_snapshot(query, context, scene_snapshot) + if scene_snapshot is not None + else self._observer(query.snapshot(), context.snapshot()) + ) + if not isinstance(supplied, JointStateObservation): + raise TypeError( + "Articulation observers must return JointStateObservation." + ) + observation = supplied + observations[fingerprint] = observation + self._validate_observation(query, observation, context) + assert observation.valid is not None + batches[query.evidence_id] = JointStateEvidenceBatch( + query.evidence_id, + observation.positions, + observation.velocities, + observation.valid, + observation.acquisition_errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + return MappingProxyType(batches) + + @staticmethod + def _observe_scene_snapshot( + query: JointStateEvidenceQuery, + context: EffectEvidenceCollectionContext, + snapshot: SceneSnapshot, + ) -> JointStateObservation: + """Adapt one typed live scene joint into raw effect evidence.""" + address = query.source.address + assert type(address) is ArticulationJointEvidenceAddress + state = snapshot.get_articulation_joint_state( + address.articulation_id, + address.joint_id, + ) + batch_size = int(context.env_ids.numel()) + if state is None: + width = int(query.clause.target_position.shape[-1]) + return JointStateObservation( + positions=torch.zeros( + (batch_size, width), + dtype=query.clause.target_position.dtype, + device=context.env_ids.device, + ), + valid=torch.zeros( + batch_size, + dtype=torch.bool, + device=context.env_ids.device, + ), + acquisition_errors=( + f"Scene snapshot has no live articulation joint " + f"{(address.articulation_id, address.joint_id)!r}.", + ) + * batch_size, + ) + positions = state.position + if positions.dim() == 1: + positions = positions.unsqueeze(0).expand(batch_size, -1) + if positions.shape[0] != batch_size: + raise ValueError( + "Scene articulation observation rows must match context env_ids." + ) + positions = positions.to(device=context.env_ids.device) + valid = state.valid_mask + if valid is None: + valid = torch.ones( + batch_size, + dtype=torch.bool, + device=context.env_ids.device, + ) + else: + valid = valid.to(device=context.env_ids.device) + errors = tuple( + None if bool(row_valid) else "Scene articulation joint row is invalid." + for row_valid in valid.tolist() + ) + return JointStateObservation( + positions=positions, + valid=valid, + acquisition_errors=errors, + ) + + def _validate_query( + self, + query: EffectEvidenceQueryValue, + ) -> JointStateEvidenceQuery: + """Require one exact joint query and matching canonical address.""" + if type(query) is not JointStateEvidenceQuery: + raise TypeError( + "SceneArticulationEvidenceProvider accepts only " + "JointStateEvidenceQuery values." + ) + source = query.source + if (source.provider_id, source.revision) != ( + self.provider_id, + self.revision, + ): + raise ValueError("Query does not select this exact provider version.") + if type(source.address) is not ArticulationJointEvidenceAddress: + raise TypeError( + "Scene articulation evidence requires " + "ArticulationJointEvidenceAddress." + ) + expectation = query.expectation + if type(expectation) is not ArticulationJointStateExpectation: + raise TypeError( + "Scene articulation evidence requires an " + "ArticulationJointStateExpectation." + ) + if ( + expectation.articulation_id != source.address.articulation_id + or expectation.joint_id != source.address.joint_id + ): + raise ValueError( + "Articulation evidence address must exactly match its typed " + "state expectation." + ) + return query.snapshot() + + @staticmethod + def _validate_observation( + query: JointStateEvidenceQuery, + observation: JointStateObservation, + context: EffectEvidenceCollectionContext, + ) -> None: + """Require callback rows/device/width to match the synchronized query.""" + if observation.positions.shape[0] != context.env_ids.numel(): + raise ValueError( + "Articulation observation rows must match context env_ids." + ) + if observation.positions.device != context.env_ids.device: + raise ValueError( + "Articulation observations and context env_ids must share a device." + ) + target_width = int(query.clause.target_position.shape[-1]) + if observation.positions.shape[1] != target_width: + raise ValueError( + f"Joint observation width {observation.positions.shape[1]} does " + f"not match query target width {target_width}." + ) + + +@runtime_checkable +class ControlPartRobotEvidenceSource(Protocol): + """Minimal simulation robot API used by the built-in provider.""" + + def get_qpos(self, name: str | None = None, target: bool = False) -> torch.Tensor: + """Return current robot or control-part joint positions.""" + + def get_qvel(self, name: str | None = None, target: bool = False) -> torch.Tensor: + """Return current robot or control-part joint velocities.""" + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: Sequence[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + """Return the selected endpoint pose for current joint positions.""" + + +class ControlPartSimulationEvidenceProvider(EffectEvidenceProvider): + """Built-in simulation acquisition for control-part evidence addresses. + + Pose evidence is computed as ``inverse(object_pose) @ endpoint_pose`` from + one scene snapshot and :meth:`Robot.compute_fk`. Joint evidence reads the + control part's measured positions and velocities. Contact, constraint, + force, and wrench signals are backend-specific, so callers inject raw + observation callbacks. An omitted callback yields explicit invalid rows; + the effect monitor can then retry until its normal deadline. + """ + + provider_id = CONTROL_PART_EVIDENCE_PROVIDER_ID + revision = CONTROL_PART_EVIDENCE_PROVIDER_REVISION + + def __init__( + self, + robot: ControlPartRobotEvidenceSource, + *, + scene_provider: SceneProvider | None = None, + contact_observer: BinaryObservationCallback | None = None, + constraint_observer: BinaryObservationCallback | None = None, + force_observer: ScalarObservationCallback | None = None, + wrench_observer: ScalarObservationCallback | None = None, + ) -> None: + if not isinstance(robot, ControlPartRobotEvidenceSource): + raise TypeError("robot must implement ControlPartRobotEvidenceSource.") + if scene_provider is not None and not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider or be None.") + for name, callback in ( + ("contact_observer", contact_observer), + ("constraint_observer", constraint_observer), + ("force_observer", force_observer), + ("wrench_observer", wrench_observer), + ): + if callback is not None and not callable(callback): + raise TypeError(f"{name} must be callable or None.") + self._robot = robot + self._scene_provider = scene_provider + self._binary_observers = { + BinaryEvidenceKind.CONTACT: contact_observer, + BinaryEvidenceKind.CONSTRAINT: constraint_observer, + } + self._scalar_observers = { + ScalarEvidenceKind.FORCE: force_observer, + ScalarEvidenceKind.WRENCH: wrench_observer, + } + + def collect( + self, + queries: tuple[EffectEvidenceQueryValue, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire all supplied control-part queries at one observation tick.""" + if not isinstance(context, EffectEvidenceCollectionContext): + raise TypeError("context must be an EffectEvidenceCollectionContext.") + if not isinstance(queries, tuple) or not queries: + raise ValueError("queries must be a non-empty tuple.") + owned_queries = tuple( + self._validate_and_snapshot_query(query) for query in queries + ) + if len({query.evidence_id for query in owned_queries}) != len(owned_queries): + raise ValueError("queries must have unique evidence IDs.") + + pose_queries = tuple( + query for query in owned_queries if type(query) is PoseRelationEvidenceQuery + ) + scene_snapshot = self._capture_scene(pose_queries, context) + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]] = {} + endpoint_cache: dict[str, torch.Tensor] = {} + results: dict[str, EffectEvidenceBatch] = {} + for query in owned_queries: + address = query.source.address + assert type(address) is ControlPartEvidenceAddress + if type(query) is PoseRelationEvidenceQuery: + results[query.evidence_id] = self._collect_pose( + query, + address, + context, + scene_snapshot, + joint_cache, + endpoint_cache, + ) + elif type(query) is BinaryEffectEvidenceQuery: + results[query.evidence_id] = self._collect_binary( + query, + address, + context, + ) + elif type(query) is ScalarEffectEvidenceQuery: + results[query.evidence_id] = self._collect_scalar( + query, + address, + context, + ) + elif type(query) is JointStateEvidenceQuery: + results[query.evidence_id] = self._collect_joint_state( + query, + address, + context, + joint_cache, + ) + else: + raise TypeError(f"Unsupported query type {type(query).__name__}.") + return MappingProxyType(results) + + def _validate_and_snapshot_query( + self, + query: EffectEvidenceQueryValue, + ) -> EffectEvidenceQueryValue: + """Require the exact built-in route and a control-part address.""" + if type(query) not in { + PoseRelationEvidenceQuery, + BinaryEffectEvidenceQuery, + ScalarEffectEvidenceQuery, + JointStateEvidenceQuery, + }: + raise TypeError("queries must contain exact typed evidence queries.") + source = query.source + if (source.provider_id, source.revision) != ( + self.provider_id, + self.revision, + ): + raise ValueError("Query does not select this exact provider version.") + if type(source.address) is not ControlPartEvidenceAddress: + raise TypeError( + "ControlPartSimulationEvidenceProvider requires " + "ControlPartEvidenceAddress values." + ) + return query.snapshot() + + def _capture_scene( + self, + queries: tuple[PoseRelationEvidenceQuery, ...], + context: EffectEvidenceCollectionContext, + ) -> SceneSnapshot | None: + """Capture one shared scene snapshot if pose queries need it.""" + if not queries or self._scene_provider is None: + return None + snapshot = self._scene_provider.snapshot( + timestamp=context.timestamp, + env_ids=context.env_ids.clone(), + ) + if not isinstance(snapshot, SceneSnapshot): + raise TypeError("scene_provider.snapshot() must return SceneSnapshot.") + if snapshot.timestamp != context.timestamp: + raise ValueError("Scene snapshot timestamp must match the evidence tick.") + return snapshot + + @staticmethod + def _require_channel( + address: ControlPartEvidenceAddress, + expected: str, + *, + evidence_id: str, + ) -> None: + """Reject clause/address channel mismatches before acquisition.""" + if address.channel != expected: + raise ValueError( + f"Evidence query {evidence_id!r} requires channel {expected!r}, " + f"not {address.channel!r}." + ) + + @staticmethod + def _select_rows( + value: torch.Tensor, context: EffectEvidenceCollectionContext + ) -> torch.Tensor: + """Select simulator rows addressed by the context's integer env IDs.""" + if not isinstance(value, torch.Tensor): + raise TypeError("Robot state accessors must return torch.Tensor values.") + if value.dim() != 2 or value.shape[0] == 0 or value.shape[1] == 0: + raise ValueError("Robot joint state must have non-empty shape (N, J).") + indices = context.env_ids.to(device=value.device) + if bool((indices < 0).any()) or int(indices.max().item()) >= value.shape[0]: + raise ValueError( + "The built-in simulation provider requires env_ids to address " + "valid simulator batch rows." + ) + selected = value.index_select(0, indices) + if selected.device != context.env_ids.device: + raise ValueError( + "Robot evidence and collection env_ids must share a device." + ) + return selected.clone() + + def _joint_state( + self, + control_part: str, + context: EffectEvidenceCollectionContext, + cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + ) -> tuple[torch.Tensor, torch.Tensor]: + """Read and cache measured positions and velocities for one part.""" + cached = cache.get(control_part) + if cached is not None: + return cached[0].clone(), cached[1].clone() + qpos = self._select_rows( + self._robot.get_qpos(name=control_part, target=False), + context, + ) + qvel = self._select_rows( + self._robot.get_qvel(name=control_part, target=False), + context, + ) + if qvel.shape != qpos.shape or qvel.device != qpos.device: + raise ValueError("Robot qvel must match qpos shape and device.") + if not qpos.is_floating_point() or not qvel.is_floating_point(): + raise TypeError("Robot qpos and qvel must use floating-point dtypes.") + cache[control_part] = qpos.clone(), qvel.clone() + return qpos, qvel + + def _endpoint_pose( + self, + control_part: str, + context: EffectEvidenceCollectionContext, + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + endpoint_cache: dict[str, torch.Tensor], + ) -> torch.Tensor: + """Compute and cache one control-part endpoint pose.""" + cached = endpoint_cache.get(control_part) + if cached is not None: + return cached.clone() + qpos, _ = self._joint_state(control_part, context, joint_cache) + pose = self._robot.compute_fk( + qpos=qpos, + name=control_part, + env_ids=context.env_ids.detach().cpu().tolist(), + to_matrix=True, + ) + if not isinstance(pose, torch.Tensor): + raise TypeError("robot.compute_fk() must return a torch.Tensor.") + if pose.shape != (context.env_ids.numel(), 4, 4): + raise ValueError("robot.compute_fk() must return shape (B, 4, 4).") + if pose.device != context.env_ids.device: + raise ValueError( + "Endpoint poses and collection env_ids must share a device." + ) + endpoint_cache[control_part] = pose.clone() + return pose + + @staticmethod + def _pose_entity_id(query: PoseRelationEvidenceQuery) -> str: + """Resolve the canonical scene entity observed by a pose relation.""" + expectation = query.expectation + if type(expectation) is HeldObjectStateExpectation: + return expectation.object_id + if type(expectation) is ArticulationJointStateExpectation: + return expectation.articulation_id + raise ValueError( + "Pose relation evidence requires an expectation with one canonical " + "scene entity." + ) + + def _collect_pose( + self, + query: PoseRelationEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + scene_snapshot: SceneSnapshot | None, + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + endpoint_cache: dict[str, torch.Tensor], + ) -> PoseRelationEvidenceBatch: + """Collect object-to-endpoint transforms from scene and FK state.""" + self._require_channel( + address, + POSE_RELATION_EFFECT_CHANNEL, + evidence_id=query.evidence_id, + ) + if scene_snapshot is None: + return self._invalid_pose( + query.evidence_id, + context, + "No scene provider is configured for pose-relation evidence.", + ) + entity_id = self._pose_entity_id(query) + try: + state = scene_snapshot.entities[entity_id] + except KeyError as exc: + raise KeyError( + f"Pose evidence references missing scene entity {entity_id!r}." + ) from exc + object_pose = state.pose + batch_size = int(context.env_ids.numel()) + if object_pose.shape == (4, 4): + object_pose = object_pose.unsqueeze(0).expand(batch_size, -1, -1) + if object_pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Scene entity {entity_id!r} pose must have shape (B, 4, 4)." + ) + endpoint_pose = self._endpoint_pose( + address.control_part, + context, + joint_cache, + endpoint_cache, + ) + object_pose = object_pose.to( + device=endpoint_pose.device, + dtype=endpoint_pose.dtype, + ) + relative = torch.bmm(pose_inv(object_pose), endpoint_pose) + valid = torch.full( + (batch_size,), + state.confidence > 0.0, + dtype=torch.bool, + device=relative.device, + ) + errors: tuple[str | None, ...] + if bool(valid.all()): + errors = (None,) * batch_size + else: + errors = ( + f"Scene entity {entity_id!r} has zero observation confidence.", + ) * batch_size + return PoseRelationEvidenceBatch( + query.evidence_id, + relative, + valid, + errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + def _collect_binary( + self, + query: BinaryEffectEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectEvidenceBatch: + """Collect callback-provided contact or constraint state.""" + expected_channel = ( + CONTACT_EFFECT_CHANNEL + if query.clause.evidence_kind is BinaryEvidenceKind.CONTACT + else CONSTRAINT_EFFECT_CHANNEL + ) + self._require_channel(address, expected_channel, evidence_id=query.evidence_id) + callback = self._binary_observers[query.clause.evidence_kind] + if callback is None: + return self._invalid_binary( + query, + context, + f"No {query.clause.evidence_kind.value} observation callback is configured.", + ) + observation = callback(query.snapshot(), context.snapshot()) + if not isinstance(observation, BinaryEffectObservation): + raise TypeError( + "Binary observation callbacks must return BinaryEffectObservation." + ) + self._validate_callback_rows(observation.values, context) + assert observation.valid is not None + return BinaryEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + observation.values, + observation.valid, + observation.acquisition_errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + def _collect_scalar( + self, + query: ScalarEffectEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + ) -> ScalarEffectEvidenceBatch: + """Collect callback-provided force or wrench magnitude.""" + self._require_channel( + address, FORCE_EFFECT_CHANNEL, evidence_id=query.evidence_id + ) + callback = self._scalar_observers[query.clause.evidence_kind] + if callback is None: + return self._invalid_scalar( + query, + context, + f"No {query.clause.evidence_kind.value} observation callback is configured.", + ) + observation = callback(query.snapshot(), context.snapshot()) + if not isinstance(observation, ScalarEffectObservation): + raise TypeError( + "Scalar observation callbacks must return ScalarEffectObservation." + ) + self._validate_callback_rows(observation.values, context) + assert observation.valid is not None + return ScalarEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + observation.values, + observation.valid, + observation.acquisition_errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + def _collect_joint_state( + self, + query: JointStateEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + ) -> JointStateEvidenceBatch: + """Collect measured control-part joint positions and velocities.""" + self._require_channel( + address, + JOINT_STATE_EFFECT_CHANNEL, + evidence_id=query.evidence_id, + ) + qpos, qvel = self._joint_state(address.control_part, context, joint_cache) + target_width = int(query.clause.target_position.shape[-1]) + if qpos.shape[1] != target_width: + raise ValueError( + f"Joint evidence width {qpos.shape[1]} does not match query target " + f"width {target_width}." + ) + batch_size = int(context.env_ids.numel()) + return JointStateEvidenceBatch( + query.evidence_id, + qpos, + qvel, + torch.ones(batch_size, dtype=torch.bool, device=qpos.device), + (None,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + @staticmethod + def _validate_callback_rows( + values: torch.Tensor, + context: EffectEvidenceCollectionContext, + ) -> None: + """Require callback values to follow the synchronized context rows.""" + if values.shape != context.env_ids.shape: + raise ValueError("Observation callback rows must match context env_ids.") + if values.device != context.env_ids.device: + raise ValueError( + "Observation callback values and context env_ids must share a device." + ) + + @staticmethod + def _invalid_pose( + evidence_id: str, + context: EffectEvidenceCollectionContext, + message: str, + ) -> PoseRelationEvidenceBatch: + """Create explicit invalid rows for unavailable pose acquisition.""" + batch_size = int(context.env_ids.numel()) + poses = torch.eye( + 4, + dtype=torch.float32, + device=context.env_ids.device, + ).expand(batch_size, -1, -1) + return PoseRelationEvidenceBatch( + evidence_id, + poses, + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + (message,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + @staticmethod + def _invalid_binary( + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + message: str, + ) -> BinaryEffectEvidenceBatch: + """Create explicit invalid rows for an unavailable binary channel.""" + batch_size = int(context.env_ids.numel()) + return BinaryEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + (message,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + @staticmethod + def _invalid_scalar( + query: ScalarEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + message: str, + ) -> ScalarEffectEvidenceBatch: + """Create explicit invalid rows for an unavailable scalar channel.""" + batch_size = int(context.env_ids.numel()) + return ScalarEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + torch.zeros(batch_size, dtype=torch.float32, device=context.env_ids.device), + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + (message,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + +__all__ = [ + "ArticulationJointObservationCallback", + "BinaryEffectEvidenceQuery", + "BinaryEffectObservation", + "BinaryObservationCallback", + "ControlPartRobotEvidenceSource", + "ControlPartSimulationEvidenceProvider", + "EffectEvidenceCollectionContext", + "EffectEvidenceCollector", + "EffectEvidenceProvider", + "EffectEvidenceProviderRegistry", + "EffectEvidenceQuery", + "EffectEvidenceQueryValue", + "JointStateEvidenceQuery", + "JointStateObservation", + "PoseRelationEvidenceQuery", + "ScalarEffectEvidenceQuery", + "ScalarEffectObservation", + "ScalarObservationCallback", + "SceneArticulationEvidenceProvider", + "build_effect_evidence_queries", +] diff --git a/embodichain/lab/sim/skills/integration.py b/embodichain/lab/sim/skills/integration.py index 8efc8e046..f98ab1fe4 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -26,6 +26,7 @@ from embodichain.lab.sim.atomic_actions import ( Affordance, AtomicActionEngine, + DynamicCollisionMode, DisjointResourceSlots, DisjointSlotEndpoints, SkillResourceSlot, @@ -33,6 +34,7 @@ from .calls import ( HandOver, + OperateArticulation, Pick, Place, RegisteredSemanticCall, @@ -51,6 +53,7 @@ SkillPolicyPreset, ) from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, GRASP_AFFORDANCE_CAPABILITY, PLACE_IN_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, @@ -577,7 +580,13 @@ class LinkedSemanticCall: affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) def __post_init__(self) -> None: - if type(self.call) not in (Pick, Place, HandOver, RegisteredSemanticCall): + if type(self.call) not in ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ): raise TypeError("call must be an exact supported semantic call value.") if type(self.descriptor) is not SemanticCallDescriptor: raise TypeError("descriptor must be exactly SemanticCallDescriptor.") @@ -685,6 +694,29 @@ def __post_init__(self) -> None: raise TypeError("robot_profile must be exactly RobotSkillProfile.") if type(self.call_catalog) is not SemanticCallCatalog: raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + known_semantic_ids = set(self.call_catalog.descriptors) + for preset_id, preset in self.robot_profile.presets.items(): + unknown_monitor_ids = sorted( + set(preset.effect_monitors).difference(known_semantic_ids) + ) + if unknown_monitor_ids: + semantic_id = unknown_monitor_ids[0] + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_effect_monitor_call", + ( + "integration", + "robot_profile", + "presets", + preset_id, + "effect_monitors", + semantic_id, + ), + f"Effect monitor configuration references unknown semantic " + f"call {semantic_id!r}.", + tuple(self.call_catalog.descriptors), + ) + ) if self.runtime_preset is not None: _validate_identifier( self.runtime_preset, @@ -781,6 +813,24 @@ def link_call( ) normalized_call = replace(call, object=object_ref) affordances["receiver_grasp"] = grasp + elif isinstance(call, OperateArticulation): + articulation_ref = self.scene.resolve( + call.articulation, + expected_type=SceneArticulationRef, + path=(*path, "articulation"), + ) + handle = self.scene.resolve_affordance( + articulation_ref, + capability=ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + explicit=call.handle, + path=(*path, "handle"), + ) + normalized_call = replace( + call, + articulation=articulation_ref, + handle=handle, + ) + affordances["handle"] = handle elif isinstance(call, RegisteredSemanticCall): normalized_call = replace( call, @@ -813,6 +863,34 @@ def link_call( affordances=affordances, ) + def _selects_preset(self, preset_id: str) -> bool: + """Return whether one preset is reachable through this integration. + + Args: + preset_id: Stable policy preset identifier. + + Returns: + ``True`` when the integration-wide override or at least one + catalogued target skill can resolve to ``preset_id`` through its + per-skill or profile-default selection. This is intentionally a + conservative integration-level check, not a concrete-program + reachability analysis. + """ + _validate_identifier(preset_id, field_name="preset_id") + if self.runtime_preset is not None: + return self.runtime_preset == preset_id + skill_ids = { + descriptor.skill_id for descriptor in self.call_catalog.descriptors.values() + } + return any( + self.robot_profile.skill_presets.get( + skill_id, + self.robot_profile.default_preset, + ) + == preset_id + for skill_id in skill_ids + ) + def _resolve_declared_preset( self, descriptor: SemanticCallDescriptor, @@ -1119,6 +1197,12 @@ def bind( ) -> BoundSemanticIntegration: """Validate live scene and robot bindings without observing or planning.""" self.scene.validate_registry(scene_registry) + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + self._validate_safe_dynamic_collision_policy( + scene_registry=scene_registry, + engine=engine, + ) try: bound_profile = engine.bind_skill_profile( self.robot_profile, @@ -1139,6 +1223,53 @@ def bind( engine=engine, ) + def _validate_safe_dynamic_collision_policy( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> None: + """Fail before observation when selected safe planning cannot be strict.""" + if not scene_registry.dynamic_collision_entity_ids or not self._selects_preset( + "safe" + ): + return + preset = self.robot_profile.presets["safe"] + policy_path: tuple[PathPart, ...] = ( + "integration", + "robot_profile", + "presets", + "safe", + "motion_policy", + ) + if preset.motion_policy.strategy != "motion_gen": + raise SemanticValidationError( + SemanticDiagnostic( + "safe_dynamic_collision_unsupported", + (*policy_path, "strategy"), + "The 'safe' preset requires strategy='motion_gen' when the " + "scene registry declares dynamic collision entities.", + ("motion_gen",), + ) + ) + if ( + getattr( + engine.motion_generator, + "supports_dynamic_collision_world", + False, + ) + is not True + ): + raise SemanticValidationError( + SemanticDiagnostic( + "safe_dynamic_collision_unsupported", + (*policy_path, "dynamic_collision_mode"), + "The 'safe' preset requires an active planner with dynamic " + "collision-world support for the registered dynamic entities " + f"{scene_registry.dynamic_collision_entity_ids!r}.", + ) + ) + class BoundSemanticIntegration: """Live-installed, still side-effect-free semantic integration link.""" @@ -1160,6 +1291,10 @@ def __init__( if not isinstance(engine, AtomicActionEngine): raise TypeError("engine must be an AtomicActionEngine.") manifest.scene.validate_registry(scene_registry) + manifest._validate_safe_dynamic_collision_policy( + scene_registry=scene_registry, + engine=engine, + ) if robot_profile.engine is not engine: raise ValueError("robot_profile belongs to a different engine.") if engine.skill_profile is not robot_profile: @@ -1240,6 +1375,21 @@ def link_call( str(exc), ) ) from exc + if ( + linked.preset_id == "safe" + and self._scene_registry.dynamic_collision_entity_ids + ): + preset = SkillPolicyPreset( + preset_id=preset.preset_id, + schema_version=preset.schema_version, + motion_policy=replace( + preset.motion_policy, + dynamic_collision_mode=DynamicCollisionMode.REQUIRED, + ), + recovery_policy=preset.recovery_policy, + runner_cfg=preset.runner_cfg, + effect_monitors=preset.effect_monitors, + ) return BoundSemanticCall._create( linked=linked, binding=binding, diff --git a/embodichain/lab/sim/skills/parallel.py b/embodichain/lab/sim/skills/parallel.py new file mode 100644 index 000000000..fe6d06d54 --- /dev/null +++ b/embodichain/lab/sim/skills/parallel.py @@ -0,0 +1,354 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Deterministic resource, timing, and state contracts for parallel skills.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from types import MappingProxyType +from typing import Mapping + +import torch + +from embodichain.lab.sim.atomic_actions import ( + RuntimeCommandFrame, + StateDelta, + TaskState, + TimedCommandSequence, +) + +from .profiles import ResourceClaim + + +def _validate_identifier(value: str, *, field_name: str) -> None: + """Validate one non-empty stable identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty stable identifier.") + + +@dataclass(frozen=True, slots=True) +class ParallelTimingPolicy: + """Strict environment-grid policy for one parallel barrier. + + Version 2 deliberately rejects fractional frame durations. Padding repeats + the last controller target, which is a deterministic position/tool hold; + no interpolation is hidden inside the scheduler. + """ + + step_dt: float + tolerance: float = 1.0e-6 + + def __post_init__(self) -> None: + for field_name in ("step_dt", "tolerance"): + value = getattr(self, field_name) + if not isinstance(value, (int, float)) or isinstance(value, bool): + raise TypeError(f"{field_name} must be a number.") + value = float(value) + if not math.isfinite(value) or value <= 0.0: + raise ValueError(f"{field_name} must be finite and positive.") + object.__setattr__(self, field_name, value) + + +@dataclass(frozen=True, slots=True, eq=False) +class ParallelBranchPlan: + """One independently planned lane entering a common barrier.""" + + branch_id: str + claim: ResourceClaim + commands: TimedCommandSequence + expected_effects: StateDelta = StateDelta() + + def __post_init__(self) -> None: + _validate_identifier(self.branch_id, field_name="branch_id") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + if not isinstance(self.commands, TimedCommandSequence): + raise TypeError("commands must be a TimedCommandSequence.") + if not isinstance(self.expected_effects, StateDelta): + raise TypeError("expected_effects must be a StateDelta.") + object.__setattr__(self, "commands", self.commands.snapshot()) + object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + + +class ParallelConflictError(ValueError): + """Raised before execution when parallel lanes claim overlapping resources.""" + + +class ParallelTimingError(ValueError): + """Raised when a command sequence cannot use the environment step grid.""" + + +class ParallelStateConflictError(ValueError): + """Raised when successful lanes update the same symbolic state row.""" + + +def validate_parallel_claims(branches: tuple[ParallelBranchPlan, ...]) -> None: + """Reject duplicate IDs and every pair of overlapping physical claims.""" + if not isinstance(branches, tuple) or len(branches) < 2: + raise ValueError("Parallel execution requires at least two branch plans.") + if not all(type(branch) is ParallelBranchPlan for branch in branches): + raise TypeError("branches must contain exact ParallelBranchPlan values.") + branch_ids = tuple(branch.branch_id for branch in branches) + if len(set(branch_ids)) != len(branch_ids): + raise ParallelConflictError("Parallel branch IDs must be unique.") + for index, left in enumerate(branches): + for right in branches[index + 1 :]: + if left.claim.conflicts_with(right.claim): + raise ParallelConflictError( + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} have overlapping physical claims." + ) + + +def _validate_grid_frame( + branch_id: str, + frame_index: int, + frame: RuntimeCommandFrame, + policy: ParallelTimingPolicy, +) -> None: + """Require one frame to occupy exactly one environment control step.""" + durations = frame.hold_duration + expected = torch.full_like(durations, policy.step_dt) + if not torch.allclose(durations, expected, atol=policy.tolerance, rtol=0.0): + values = sorted({float(value) for value in durations.detach().cpu().tolist()}) + raise ParallelTimingError( + f"Parallel branch {branch_id!r} frame {frame_index} has durations " + f"{values}; every emitted frame must equal step_dt={policy.step_dt}." + ) + + +def align_parallel_commands( + branches: tuple[ParallelBranchPlan, ...], + policy: ParallelTimingPolicy, +) -> TimedCommandSequence: + """Merge disjoint lanes on one grid and hold-pad shorter trajectories. + + Each merged frame is a single transport transaction. Runtime frame + validation independently rejects duplicate destinations or joint overlap, + defending against an incorrect custom ``ResourceClaim`` implementation. + """ + if not isinstance(policy, ParallelTimingPolicy): + raise TypeError("policy must be a ParallelTimingPolicy.") + validate_parallel_claims(branches) + first = branches[0].commands + if any( + branch.commands.device != first.device + or not torch.equal(branch.commands.env_ids, first.env_ids) + for branch in branches[1:] + ): + raise ParallelTimingError( + "Parallel command sequences must share ordered env_ids and device." + ) + if any(branch.commands.frame_count == 0 for branch in branches): + raise ParallelTimingError( + "Parallel branches must emit at least one command frame." + ) + for branch in branches: + for frame_index, frame in enumerate(branch.commands.frames): + _validate_grid_frame(branch.branch_id, frame_index, frame, policy) + + frame_count = max(branch.commands.frame_count for branch in branches) + merged: list[RuntimeCommandFrame] = [] + for frame_index in range(frame_count): + lane_frames = tuple( + branch.commands.frames[min(frame_index, branch.commands.frame_count - 1)] + for branch in branches + ) + reference_mask = lane_frames[0].active_mask + if any( + not torch.equal(frame.active_mask, reference_mask) + for frame in lane_frames[1:] + ): + raise ParallelTimingError( + "Parallel lanes cannot merge different per-environment active " + f"masks at frame {frame_index}; RuntimeCommandFrame owns one " + "mask for every command in the transaction." + ) + merged.append( + RuntimeCommandFrame( + commands=tuple( + command for frame in lane_frames for command in frame.commands + ), + active_mask=reference_mask, + env_ids=first.env_ids, + hold_duration=torch.full( + (first.batch_size,), + policy.step_dt, + dtype=lane_frames[0].hold_duration.dtype, + device=first.device, + ), + ) + ) + return TimedCommandSequence(frames=tuple(merged), env_ids=first.env_ids) + + +def _delta_keys(delta: StateDelta) -> frozenset[tuple[str, object]]: + """Return domain-qualified symbolic keys written by one delta.""" + return frozenset( + [("held", key) for key in delta.held_object_updates] + + [("coordinated", key) for key in delta.coordinated_held_object_updates] + + [("articulation", key) for key in delta.articulation_joint_updates] + ) + + +def merge_parallel_effects( + state: TaskState, + effects: Mapping[str, tuple[StateDelta, torch.Tensor]], +) -> TaskState: + """Apply disjoint branch effects with deterministic row-local conflict checks. + + Args: + state: Verified task state before the barrier. + effects: Branch ID to ``(delta, verified_success_mask)``. + + Returns: + New verified task state after all non-conflicting updates. + """ + if not isinstance(state, TaskState): + raise TypeError("state must be a TaskState.") + if not isinstance(effects, Mapping) or not effects: + raise ValueError("effects must be a non-empty branch mapping.") + normalized: dict[str, tuple[StateDelta, torch.Tensor]] = {} + for branch_id, value in effects.items(): + _validate_identifier(branch_id, field_name="effect branch IDs") + if not isinstance(value, tuple) or len(value) != 2: + raise TypeError("effect entries must be (StateDelta, success_mask) pairs.") + delta, mask = value + if not isinstance(delta, StateDelta): + raise TypeError("effect deltas must be StateDelta values.") + if ( + not isinstance(mask, torch.Tensor) + or mask.dtype != torch.bool + or mask.shape != (state.batch_size,) + or mask.device != state.device + ): + raise ValueError("effect masks must match TaskState batch and device.") + normalized[branch_id] = delta.snapshot(), mask.clone() + + entries = tuple(normalized.items()) + for index, (left_id, (left_delta, left_mask)) in enumerate(entries): + for right_id, (right_delta, right_mask) in entries[index + 1 :]: + overlapping_keys = _delta_keys(left_delta) & _delta_keys(right_delta) + overlapping_rows = left_mask & right_mask + if overlapping_keys and overlapping_rows.any(): + raise ParallelStateConflictError( + f"Parallel effects {left_id!r} and {right_id!r} write " + f"the same symbolic keys on rows " + f"{overlapping_rows.nonzero().flatten().tolist()}." + ) + result = state + for branch_id in sorted(normalized): + delta, mask = normalized[branch_id] + result = delta.apply(result, mask) + return result + + +@dataclass(frozen=True, slots=True, eq=False) +class ParallelBarrierUpdate: + """Per-row barrier status after one synchronized lane observation.""" + + completed_mask: torch.Tensor + failure_mask: torch.Tensor + cancellation_masks: Mapping[str, torch.Tensor] + + def __post_init__(self) -> None: + if ( + not isinstance(self.completed_mask, torch.Tensor) + or self.completed_mask.dtype != torch.bool + or self.completed_mask.dim() != 1 + ): + raise ValueError("completed_mask must be a one-dimensional bool tensor.") + if ( + not isinstance(self.failure_mask, torch.Tensor) + or self.failure_mask.dtype != torch.bool + or self.failure_mask.shape != self.completed_mask.shape + or self.failure_mask.device != self.completed_mask.device + ): + raise ValueError("failure_mask must match completed_mask.") + cancellations: dict[str, torch.Tensor] = {} + for branch_id, mask in self.cancellation_masks.items(): + _validate_identifier(branch_id, field_name="cancellation branch IDs") + if ( + not isinstance(mask, torch.Tensor) + or mask.dtype != torch.bool + or mask.shape != self.completed_mask.shape + or mask.device != self.completed_mask.device + ): + raise ValueError("cancellation masks must match completed_mask.") + cancellations[branch_id] = mask.clone() + object.__setattr__(self, "completed_mask", self.completed_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__( + self, + "cancellation_masks", + MappingProxyType(cancellations), + ) + + +def resolve_parallel_barrier( + *, + pending_masks: Mapping[str, torch.Tensor], + success_masks: Mapping[str, torch.Tensor], + failure_masks: Mapping[str, torch.Tensor], +) -> ParallelBarrierUpdate: + """Apply deterministic per-row fail-fast semantics at one barrier update.""" + branch_ids = tuple(pending_masks) + if ( + not branch_ids + or set(success_masks) != set(branch_ids) + or set(failure_masks) != set(branch_ids) + ): + raise ValueError( + "pending, success, and failure mappings must share branch IDs." + ) + reference = pending_masks[branch_ids[0]] + if not isinstance(reference, torch.Tensor): + raise TypeError("barrier masks must be torch.Tensor values.") + for mapping in (pending_masks, success_masks, failure_masks): + for mask in mapping.values(): + if ( + not isinstance(mask, torch.Tensor) + or mask.dtype != torch.bool + or mask.shape != reference.shape + or mask.device != reference.device + ): + raise ValueError("all barrier masks must share bool shape and device.") + failed = torch.stack(tuple(failure_masks.values()), dim=0).any(dim=0) + succeeded_all = torch.stack(tuple(success_masks.values()), dim=0).all(dim=0) + cancellations = { + branch_id: failed & pending_masks[branch_id] for branch_id in branch_ids + } + return ParallelBarrierUpdate( + completed_mask=succeeded_all | failed, + failure_mask=failed, + cancellation_masks=cancellations, + ) + + +__all__ = [ + "ParallelBarrierUpdate", + "ParallelBranchPlan", + "ParallelConflictError", + "ParallelStateConflictError", + "ParallelTimingError", + "ParallelTimingPolicy", + "align_parallel_commands", + "merge_parallel_effects", + "resolve_parallel_barrier", + "validate_parallel_claims", +] diff --git a/embodichain/lab/sim/skills/parallel_runtime.py b/embodichain/lab/sim/skills/parallel_runtime.py new file mode 100644 index 000000000..235bb7e18 --- /dev/null +++ b/embodichain/lab/sim/skills/parallel_runtime.py @@ -0,0 +1,1487 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Branch-local semantic execution joined by one deterministic barrier.""" + +from __future__ import annotations + +from collections.abc import Hashable, Mapping +from dataclasses import dataclass, field +import math +from types import MappingProxyType +from typing import Protocol, runtime_checkable + +import torch + +from embodichain.lab.sim.atomic_actions import ( + CommandAcknowledgement, + CommandSink, + ExecutionClock, + PlanningContext, + RuntimeCommandFrame, + RuntimeEndpointTarget, + StateDelta, + TaskState, + TimedCommandSequence, +) + +from .calls import SemanticCallSpec +from .compiler import SemanticSkillCompiler +from .effects import SymbolicStateKey +from .integration import ( + PathPart, + SemanticDiagnostic, + SemanticValidationError, +) +from .parallel import ( + ParallelBranchPlan, + ParallelTimingPolicy, + align_parallel_commands, + merge_parallel_effects, + resolve_parallel_barrier, +) +from .profiles import ResourceClaim +from .runtime import SkillResult, SkillRuntime, SkillStatus, task_state_to_metadata + + +def _validate_identifier(value: str, *, field_name: str) -> None: + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty stable identifier.") + + +def _snapshot_target(target: RuntimeEndpointTarget) -> RuntimeEndpointTarget: + snapshot = target.snapshot() + if type(snapshot) is not type(target) or snapshot is target: + raise TypeError("Runtime target snapshots must be independent exact values.") + return snapshot + + +def _target_fingerprint(target: RuntimeEndpointTarget) -> Hashable: + """Return one validated target address and safe-hold fingerprint.""" + fingerprint = target.address_fingerprint + try: + hash(fingerprint) + except TypeError as exc: + raise TypeError( + "RuntimeEndpointTarget.address_fingerprint must be hashable." + ) from exc + return fingerprint + + +@runtime_checkable +class ParallelBranchRuntime(Protocol): + """Minimal branch-local runtime surface required by the coordinator.""" + + @property + def result(self) -> SkillResult: + """Return the current immutable branch result.""" + + def start( + self, + *calls: SemanticCallSpec, + workflow_id: str, + eligible_mask: torch.Tensor | None = None, + ) -> SkillResult: + """Start one branch-local semantic workflow.""" + + def step(self) -> SkillResult: + """Advance the branch by one due runtime cycle.""" + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + """Remove peer-failed rows while other rows continue.""" + + def cancel(self, reason: str) -> SkillResult: + """Cancel the complete branch and apply its safe stop.""" + + +@runtime_checkable +class ParallelCommandSafetyValidator(Protocol): + """Fail-closed physical-safety boundary for one merged command tick. + + Resource claims prevent controller arbitration conflicts but cannot prove + that independently generated robot motions are collision-free when + executed together. Environment integrations must install a validator + backed by their authoritative robot/collision model before parallel + commands can leave the coordinator. + """ + + def validate( + self, + *, + branch_frames: Mapping[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + """Raise when the synchronized command is not physically safe.""" + + +class ParallelSafetyError(RuntimeError): + """Raised when physical parallel-command safety cannot be established.""" + + +class ParallelLaneCommandSink: + """Acknowledge one branch locally and expose its frame to a coordinator. + + The coordinator is the only object allowed to forward commands to the real + transport. A lane retains its last frame so shorter or temporarily waiting + branches use deterministic hold-last padding. + """ + + def __init__(self) -> None: + self._fresh_frame: RuntimeCommandFrame | None = None + self._last_frame: RuntimeCommandFrame | None = None + self._hold_requests: list[ + tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext] + ] = [] + self._cancel_targets: tuple[RuntimeEndpointTarget, ...] = () + + @property + def last_frame(self) -> RuntimeCommandFrame | None: + """Return an owned hold-last frame, if this lane has sent one.""" + return None if self._last_frame is None else self._last_frame.snapshot() + + @property + def hold_request( + self, + ) -> tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext | None]: + """Return all pending targets and their latest planning context.""" + targets: dict[Hashable, RuntimeEndpointTarget] = {} + context: PlanningContext | None = None + for requested, request_context in self._hold_requests: + for target in requested: + targets[_target_fingerprint(target)] = target + context = request_context + return ( + tuple(_snapshot_target(target) for target in targets.values()), + context, + ) + + @property + def cancel_targets(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return target snapshots from the most recent cancel request.""" + return tuple(_snapshot_target(target) for target in self._cancel_targets) + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Capture exactly one fresh frame for the current coordinator tick.""" + del timeout + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") + if self._fresh_frame is not None: + raise RuntimeError( + "A parallel lane emitted multiple command frames before drain." + ) + self._fresh_frame = command.snapshot() + self._last_frame = command.snapshot() + return CommandAcknowledgement.accepted_ack("buffered by parallel lane") + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Capture a target-scoped hold; hold-last remains the grid command.""" + del timeout + if not isinstance(context, PlanningContext): + raise TypeError("context must be a PlanningContext.") + self._hold_requests.append( + ( + tuple(_snapshot_target(target) for target in targets), + context, + ) + ) + return CommandAcknowledgement.accepted_ack("buffered parallel hold") + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Capture cancellation ownership for coordinator-level safe stop.""" + del timeout + self._fresh_frame = None + self._cancel_targets = tuple(_snapshot_target(target) for target in targets) + return CommandAcknowledgement.accepted_ack("buffered parallel cancel") + + def drain_frame(self) -> RuntimeCommandFrame | None: + """Consume the frame emitted since the previous coordinator step.""" + frame = self._fresh_frame + self._fresh_frame = None + return None if frame is None else frame.snapshot() + + def drain_hold_requests( + self, + ) -> tuple[tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext], ...]: + """Consume every completion/safe hold buffered since the last tick.""" + requests = tuple( + ( + tuple(_snapshot_target(target) for target in targets), + context, + ) + for targets, context in self._hold_requests + ) + self._hold_requests.clear() + return requests + + +@dataclass(frozen=True, slots=True) +class ParallelRuntimeBranch: + """One semantic-call lane and its exclusive resource claim.""" + + branch_id: str + calls: tuple[SemanticCallSpec, ...] + claim: ResourceClaim + runtime: ParallelBranchRuntime = field(repr=False, compare=False) + command_sink: ParallelLaneCommandSink = field(repr=False, compare=False) + + def __post_init__(self) -> None: + _validate_identifier(self.branch_id, field_name="branch_id") + calls = tuple(self.calls) + if not calls or not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError("calls must contain SemanticCallSpec values.") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + if not isinstance(self.runtime, ParallelBranchRuntime): + raise TypeError("runtime must implement ParallelBranchRuntime.") + if type(self.command_sink) is not ParallelLaneCommandSink: + raise TypeError("command_sink must be ParallelLaneCommandSink.") + object.__setattr__(self, "calls", calls) + + +@dataclass(frozen=True, slots=True) +class ParallelBranchStaticAnalysis: + """Provider-free physical and symbolic claims for one semantic lane.""" + + branch_id: str + calls: tuple[SemanticCallSpec, ...] + claim: ResourceClaim + symbolic_writes: frozenset[SymbolicStateKey] + opaque_symbolic_call_indices: tuple[int, ...] + source_path: tuple[PathPart, ...] + + def __post_init__(self) -> None: + _validate_identifier(self.branch_id, field_name="branch_id") + calls = tuple(self.calls) + if not calls or not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError("calls must contain SemanticCallSpec values.") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + if type(self.symbolic_writes) is not frozenset or not all( + type(write) is SymbolicStateKey for write in self.symbolic_writes + ): + raise TypeError( + "symbolic_writes must be an exact frozenset of " + "SymbolicStateKey values." + ) + opaque_indices = tuple(self.opaque_symbolic_call_indices) + if not all( + type(index) is int and 0 <= index < len(calls) for index in opaque_indices + ): + raise ValueError( + "opaque_symbolic_call_indices must select branch call indices." + ) + if len(set(opaque_indices)) != len(opaque_indices): + raise ValueError("opaque_symbolic_call_indices must be unique.") + source_path = tuple(self.source_path) + if not source_path or not all( + (type(part) is str and bool(part)) or type(part) is int + for part in source_path + ): + raise ValueError("source_path must contain valid diagnostic components.") + object.__setattr__(self, "calls", calls) + object.__setattr__(self, "opaque_symbolic_call_indices", opaque_indices) + object.__setattr__(self, "source_path", source_path) + + +def analyze_parallel_branches( + compiler: SemanticSkillCompiler, + branch_calls: Mapping[str, tuple[SemanticCallSpec, ...]], + *, + workflow_id: str = "parallel_static_analysis", + branch_paths: Mapping[str, tuple[PathPart, ...]] | None = None, +) -> tuple[ParallelBranchStaticAnalysis, ...]: + """Reject overlapping physical claims and exact symbolic write keys. + + This is the canonical provider-free parallel preflight shared by the core + runtime factory and higher-level declarative frontends. Dynamic command + collision safety remains the responsibility of + :class:`ParallelCommandSafetyValidator`. + + Args: + compiler: Canonical semantic compiler owning the current integration. + branch_calls: Ordered branch IDs and their complete semantic calls. + workflow_id: Stable diagnostic prefix for branch workflows. + branch_paths: Optional exact source path for every supplied branch. + + Returns: + Ordered owned branch analyses with combined resource claims. + + Raises: + ValueError: If fewer than two branches are supplied or claims overlap. + SemanticValidationError: If branches write one exact symbolic key. + """ + if not isinstance(compiler, SemanticSkillCompiler): + raise TypeError("compiler must be a SemanticSkillCompiler.") + if not isinstance(branch_calls, Mapping) or len(branch_calls) < 2: + raise ValueError("branch_calls must contain at least two branches.") + _validate_identifier(workflow_id, field_name="workflow_id") + if branch_paths is not None: + if not isinstance(branch_paths, Mapping): + raise TypeError("branch_paths must be a mapping or None.") + if set(branch_paths) != set(branch_calls): + raise ValueError("branch_paths keys must exactly match branch_calls.") + + analyses: list[ParallelBranchStaticAnalysis] = [] + for branch_index, (branch_id, supplied_calls) in enumerate(branch_calls.items()): + _validate_identifier(branch_id, field_name="parallel branch IDs") + calls = tuple(supplied_calls) + if not calls or not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError( + "parallel branch calls must contain SemanticCallSpec values." + ) + source_path = ( + ("parallel", "branches", branch_index) + if branch_paths is None + else tuple(branch_paths[branch_id]) + ) + workflow = compiler.analyze( + calls, + workflow_id=f"{workflow_id}:{branch_index}:{branch_id}", + path=source_path, + ) + analyses.append( + ParallelBranchStaticAnalysis( + branch_id=branch_id, + calls=calls, + claim=ResourceClaim.combine( + tuple(call.bound.binding.claim for call in workflow.calls) + ), + symbolic_writes=frozenset( + write + for analyzed_call in workflow.calls + for write in analyzed_call.symbolic_writes + ), + opaque_symbolic_call_indices=tuple( + analyzed_call.index + for analyzed_call in workflow.calls + if analyzed_call.opaque_symbolic_effect + ), + source_path=source_path, + ) + ) + for index, left in enumerate(analyses): + for right in analyses[index + 1 :]: + if left.claim.conflicts_with(right.claim): + raise SemanticValidationError( + SemanticDiagnostic( + "parallel_resource_conflict", + right.source_path, + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} have overlapping resource claims.", + (left.branch_id, right.branch_id), + ) + ) + shared_writes = left.symbolic_writes & right.symbolic_writes + if shared_writes: + conflict = min( + shared_writes, + key=lambda write: (write.domain.value, write.address), + ) + raise SemanticValidationError( + SemanticDiagnostic( + "parallel_symbolic_write_conflict", + right.source_path, + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} both write symbolic TaskState key " + f"{conflict.rendered}.", + (left.branch_id, right.branch_id), + ) + ) + return tuple(analyses) + + +@dataclass(frozen=True, slots=True, eq=False) +class ParallelSkillResult: + """Owned coordinator status at one explicit barrier.""" + + status: SkillStatus + env_ids: torch.Tensor + success_mask: torch.Tensor + failure_mask: torch.Tensor + cancelled_mask: torch.Tensor + pending_mask: torch.Tensor + task_state: TaskState + branch_results: Mapping[str, SkillResult] + elapsed_steps: int + command_count: int + wait_duration: float = 0.0 + message: str | None = None + + def __post_init__(self) -> None: + if not isinstance(self.status, SkillStatus): + raise TypeError("status must be a SkillStatus.") + if ( + not isinstance(self.env_ids, torch.Tensor) + or self.env_ids.dtype != torch.long + or self.env_ids.dim() != 1 + ): + raise ValueError("env_ids must be a one-dimensional int64 tensor.") + batch_size = int(self.env_ids.numel()) + for field_name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "pending_mask", + ): + value = getattr(self, field_name) + if ( + not isinstance(value, torch.Tensor) + or value.dtype != torch.bool + or value.shape != (batch_size,) + or value.device != self.env_ids.device + ): + raise ValueError(f"{field_name} must match env_ids.") + if ( + (self.success_mask & (self.failure_mask | self.cancelled_mask)).any() + or (self.failure_mask & self.cancelled_mask).any() + or ( + self.pending_mask + & (self.success_mask | self.failure_mask | self.cancelled_mask) + ).any() + ): + raise ValueError("parallel result masks must be disjoint.") + if not isinstance(self.task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if ( + self.task_state.batch_size != batch_size + or self.task_state.device != self.env_ids.device + ): + raise ValueError("task_state must match env_ids.") + if type(self.elapsed_steps) is not int or self.elapsed_steps < 0: + raise ValueError("elapsed_steps must be non-negative.") + if type(self.command_count) is not int or self.command_count < 0: + raise ValueError("command_count must be non-negative.") + if not math.isfinite(self.wait_duration) or self.wait_duration < 0.0: + raise ValueError("wait_duration must be finite and non-negative.") + if self.message is not None and type(self.message) is not str: + raise TypeError("message must be a string or None.") + branches: dict[str, SkillResult] = {} + for branch_id, result in self.branch_results.items(): + _validate_identifier(branch_id, field_name="branch result IDs") + if not isinstance(result, SkillResult): + raise TypeError("branch_results values must be SkillResult values.") + branches[branch_id] = result + object.__setattr__(self, "env_ids", self.env_ids.clone()) + for field_name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "pending_mask", + ): + object.__setattr__(self, field_name, getattr(self, field_name).clone()) + object.__setattr__( + self, + "task_state", + TaskState( + batch_size=self.task_state.batch_size, + device=self.task_state.device, + held_objects=self.task_state.held_objects, + coordinated_held_objects=self.task_state.coordinated_held_objects, + articulation_joints=self.task_state.articulation_joints, + ), + ) + object.__setattr__(self, "branch_results", MappingProxyType(branches)) + + @property + def terminal(self) -> bool: + """Whether every row has left the barrier.""" + return self.status in { + SkillStatus.COMPLETED, + SkillStatus.FAILED, + SkillStatus.CANCELLED, + } + + def to_metadata(self) -> dict[str, object]: + """Return a fresh deterministic JSON-safe parallel barrier result.""" + return { + "schema_version": 1, + "kind": "parallel_skill_result", + "status": self.status.value, + "env_ids": self.env_ids.detach().cpu().tolist(), + "masks": { + "success": self.success_mask.detach().cpu().tolist(), + "failure": self.failure_mask.detach().cpu().tolist(), + "cancelled": self.cancelled_mask.detach().cpu().tolist(), + "pending": self.pending_mask.detach().cpu().tolist(), + }, + "task_state": task_state_to_metadata(self.task_state), + "branches": { + branch_id: result.to_metadata() + for branch_id, result in sorted(self.branch_results.items()) + }, + "elapsed_steps": self.elapsed_steps, + "command_count": self.command_count, + "wait_duration": self.wait_duration, + "message": self.message, + } + + +def _optional_tensor_equal( + left: torch.Tensor | None, right: torch.Tensor | None +) -> bool: + return (left is None and right is None) or ( + left is not None and right is not None and torch.equal(left, right) + ) + + +def _state_value_equal(left: object, right: object) -> bool: + if type(left) is not type(right): + return False + if left is None or right is None: + return left is right + if hasattr(left, "position"): + return torch.equal(left.position, right.position) and _optional_tensor_equal( + left.env_mask, + right.env_mask, + ) + if hasattr(left, "left_object_to_eef"): + return ( + left.semantics.entity_id == right.semantics.entity_id + and torch.equal(left.left_object_to_eef, right.left_object_to_eef) + and torch.equal(left.right_object_to_eef, right.right_object_to_eef) + and torch.equal(left.left_grasp_xpos, right.left_grasp_xpos) + and torch.equal(left.right_grasp_xpos, right.right_grasp_xpos) + and _optional_tensor_equal(left.env_mask, right.env_mask) + ) + return ( + left.semantics.entity_id == right.semantics.entity_id + and torch.equal(left.object_to_eef, right.object_to_eef) + and torch.equal(left.grasp_xpos, right.grasp_xpos) + and _optional_tensor_equal(left.env_mask, right.env_mask) + ) + + +def _mapping_delta( + before: Mapping[object, object], after: Mapping[object, object] +) -> dict: + updates: dict[object, object | None] = {} + for key in set(before) | set(after): + if key not in after: + updates[key] = None + elif key not in before or not _state_value_equal(before[key], after[key]): + updates[key] = after[key] + return updates + + +def _task_state_delta(before: TaskState, after: TaskState) -> StateDelta: + if before.batch_size != after.batch_size or before.device != after.device: + raise ValueError("Parallel branch TaskState changed batch or device.") + return StateDelta( + held_object_updates=_mapping_delta( + before.held_objects, + after.held_objects, + ), + coordinated_held_object_updates=_mapping_delta( + before.coordinated_held_objects, + after.coordinated_held_objects, + ), + articulation_joint_updates=_mapping_delta( + before.articulation_joints, + after.articulation_joints, + ), + ) + + +class ParallelSkillRuntime: + """Run independent JIT semantic lanes on one synchronized command grid. + + Schema v2 deliberately uses conservative barrier ownership: branches are + not assigned disjoint environment-row partitions, so two branches that + write the same symbolic key conflict for the complete started batch even + when their observed value masks happen to be disjoint. A future schema + may add explicit row partitioning before relaxing this invariant. + + A lane completion hold is forwarded as an explicit grid action. Other + lanes therefore receive deterministic hold-padding for that environment + step; a merged frame generated in the same coordinator cycle is retained + and dispatched only after the clock advances. Branch runners are not + stepped while that retained frame is being dispatched. This keeps the + physical order ``observed hold -> next command`` and limits every normal + coordinator step to one action-producing transport operation. + """ + + def __init__( + self, + branches: tuple[ParallelRuntimeBranch, ...], + command_sink: CommandSink, + clock: ExecutionClock, + timing_policy: ParallelTimingPolicy, + safety_validator: ParallelCommandSafetyValidator, + *, + timeout_steps: int, + failure_policy: str = "fail_fast", + ) -> None: + if not isinstance(branches, tuple) or len(branches) < 2: + raise ValueError("ParallelSkillRuntime requires at least two branches.") + if not all(type(branch) is ParallelRuntimeBranch for branch in branches): + raise TypeError("branches must contain ParallelRuntimeBranch values.") + branch_ids = tuple(branch.branch_id for branch in branches) + if len(set(branch_ids)) != len(branch_ids): + raise ValueError("Parallel branch IDs must be unique.") + for index, left in enumerate(branches): + for right in branches[index + 1 :]: + if left.claim.conflicts_with(right.claim): + raise ValueError( + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} have overlapping resource claims." + ) + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + if not isinstance(clock, ExecutionClock): + raise TypeError("clock must implement ExecutionClock.") + if not isinstance(timing_policy, ParallelTimingPolicy): + raise TypeError("timing_policy must be ParallelTimingPolicy.") + if not isinstance(safety_validator, ParallelCommandSafetyValidator): + raise TypeError( + "safety_validator must implement ParallelCommandSafetyValidator; " + "resource claims alone do not establish collision safety." + ) + if type(timeout_steps) is not int or timeout_steps <= 0: + raise ValueError("timeout_steps must be positive.") + if failure_policy != "fail_fast": + raise ValueError("failure_policy must be exactly 'fail_fast'.") + initial = branches[0].runtime.result + for branch in branches[1:]: + result = branch.runtime.result + if ( + result.env_ids.device != initial.env_ids.device + or not torch.equal(result.env_ids, initial.env_ids) + or result.task_state.batch_size != initial.task_state.batch_size + or result.task_state.device != initial.task_state.device + ): + raise ValueError( + "Parallel branch runtimes must share env_ids, batch, and device." + ) + if not _task_state_delta(initial.task_state, result.task_state).is_empty: + raise ValueError( + "Parallel branch runtimes must start from the same verified " + "TaskState barrier snapshot." + ) + self._branches = branches + self._command_sink = command_sink + self._clock = clock + self._timing_policy = timing_policy + self._safety_validator = safety_validator + self._timeout_steps = timeout_steps + self._initial_state = initial.task_state + self._task_state = initial.task_state + self._env_ids = initial.env_ids + self._status = SkillStatus.IDLE + self._success = torch.zeros_like(initial.success_mask) + self._failure = torch.zeros_like(initial.failure_mask) + self._cancelled = torch.zeros_like(initial.cancelled_mask) + self._pending = torch.ones_like(initial.success_mask) + self._started_eligible = torch.zeros_like(initial.success_mask) + self._elapsed_steps = 0 + self._start_timestamp: float | None = None + self._command_count = 0 + self._wait_duration = 0.0 + self._message: str | None = None + self._force_mask_dispatch = False + self._terminal_stop_forwarded = False + self._held_target_fingerprints: set[Hashable] = set() + self._last_hold_context: PlanningContext | None = None + self._deferred_frame: RuntimeCommandFrame | None = None + self._deferred_lane_frames: dict[str, RuntimeCommandFrame] = {} + self._terminal_hold_pending = False + self._next_transport_at: float | None = None + + @classmethod + def from_template( + cls, + template_runtime: SkillRuntime, + branch_calls: Mapping[str, tuple[SemanticCallSpec, ...]], + command_sink: CommandSink, + timing_policy: ParallelTimingPolicy, + safety_validator: ParallelCommandSafetyValidator, + *, + timeout_steps: int, + failure_policy: str = "fail_fast", + workflow_id: str = "parallel_static_analysis", + branch_paths: Mapping[str, tuple[PathPart, ...]] | None = None, + ) -> ParallelSkillRuntime: + """Analyze claims and derive independent lanes from one runtime. + + This factory deliberately accepts semantic calls instead of compiled + Gym-program types. It keeps the simulation runtime independent of the + higher-level configuration package while giving every frontend one + canonical resource-conflict and lane-construction path. + + Args: + template_runtime: Idle runtime providing shared compiler and ports. + branch_calls: Ordered branch ID to semantic-call sequence mapping. + command_sink: The sole outbound merged command sink. + timing_policy: Exact shared environment grid. + safety_validator: Required physical/collision safety gate for each + synchronized outbound command. + timeout_steps: Maximum environment steps at the barrier. + failure_policy: Row-local barrier failure policy. + workflow_id: Stable prefix for provider-free claim analysis. + branch_paths: Optional exact source path for every branch. + + Returns: + A one-shot parallel runtime whose branches share no mutable runner + state. + """ + if not isinstance(template_runtime, SkillRuntime): + raise TypeError("template_runtime must be a SkillRuntime.") + if template_runtime.status is SkillStatus.RUNNING: + raise RuntimeError("template_runtime must not be running.") + branches: list[ParallelRuntimeBranch] = [] + for analysis in analyze_parallel_branches( + template_runtime.compiler, + branch_calls, + workflow_id=workflow_id, + branch_paths=branch_paths, + ): + lane_sink = ParallelLaneCommandSink() + lane_runtime = template_runtime.fork( + lane_sink, + task_state=template_runtime.task_state, + ) + branches.append( + ParallelRuntimeBranch( + branch_id=analysis.branch_id, + calls=analysis.calls, + claim=analysis.claim, + runtime=lane_runtime, + command_sink=lane_sink, + ) + ) + return cls( + tuple(branches), + command_sink, + template_runtime.clock, + timing_policy, + safety_validator, + timeout_steps=timeout_steps, + failure_policy=failure_policy, + ) + + @property + def result(self) -> ParallelSkillResult: + """Return an owned barrier snapshot.""" + return ParallelSkillResult( + status=self._status, + env_ids=self._env_ids, + success_mask=self._success, + failure_mask=self._failure, + cancelled_mask=self._cancelled, + pending_mask=self._pending, + task_state=self._task_state, + branch_results={ + branch.branch_id: branch.runtime.result for branch in self._branches + }, + elapsed_steps=self._elapsed_steps, + command_count=self._command_count, + wait_duration=self._wait_duration, + message=self._message, + ) + + @property + def clock(self) -> ExecutionClock: + """Return the exact clock shared by the coordinator and every lane.""" + return self._clock + + @property + def branch_claims(self) -> Mapping[str, ResourceClaim]: + """Return immutable statically analyzed claims in branch order.""" + return MappingProxyType( + {branch.branch_id: branch.claim for branch in self._branches} + ) + + def start( + self, + *, + workflow_id: str = "parallel_workflow", + eligible_mask: torch.Tensor | None = None, + ) -> ParallelSkillResult: + """Start all lanes from the same verified barrier state.""" + if self._status is not SkillStatus.IDLE: + raise RuntimeError("ParallelSkillRuntime instances are one-shot.") + _validate_identifier(workflow_id, field_name="workflow_id") + if eligible_mask is None: + eligible = torch.ones_like(self._pending) + else: + if ( + not isinstance(eligible_mask, torch.Tensor) + or eligible_mask.dtype != torch.bool + or eligible_mask.shape != self._pending.shape + or eligible_mask.device != self._pending.device + ): + raise ValueError("eligible_mask must match the parallel batch.") + eligible = eligible_mask.clone() + if not eligible.any(): + raise ValueError("eligible_mask must contain an active row.") + self._success.zero_() + self._failure.zero_() + self._cancelled.zero_() + self._pending = eligible.clone() + self._started_eligible = eligible.clone() + self._elapsed_steps = 0 + self._start_timestamp = self._read_clock() + self._command_count = 0 + self._wait_duration = 0.0 + self._message = None + self._force_mask_dispatch = False + self._terminal_stop_forwarded = False + self._held_target_fingerprints.clear() + self._last_hold_context = None + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + self._status = SkillStatus.RUNNING + started: list[ParallelRuntimeBranch] = [] + try: + for branch in self._branches: + branch.runtime.start( + *branch.calls, + workflow_id=f"{workflow_id}:{branch.branch_id}", + eligible_mask=eligible, + ) + started.append(branch) + except Exception as exc: + reason = "Parallel branch startup failed: " f"{type(exc).__name__}: {exc}" + for branch in started: + branch.runtime.cancel(reason) + self._failure = eligible.clone() + self._pending.zero_() + self._status = SkillStatus.FAILED + self._message = reason + return self.result + try: + self._sync_branch_identity() + self._update_barrier() + self._finish_if_complete() + except Exception as exc: + self._abort_coordinator("Parallel startup coordination failed", exc) + return self.result + + def step(self) -> ParallelSkillResult: + """Advance one deterministic coordinator state-machine transition.""" + if self._status is not SkillStatus.RUNNING: + return self.result + try: + self._update_elapsed_steps() + if self._elapsed_steps >= self._timeout_steps and ( + self._pending.any() or self._transport_flush_pending + ): + self._timeout_pending_rows() + self._finish_if_complete() + return self.result + transport_wait = self._remaining_transport_wait() + if transport_wait > 0.0: + self._wait_duration = transport_wait + return self.result + if self._deferred_frame is not None: + accepted = self._dispatch_deferred_frame() + if ( + accepted + and not self._pending.any() + and self._status is SkillStatus.RUNNING + ): + self._terminal_hold_pending = True + self._finish_if_complete() + return self.result + if self._terminal_hold_pending: + self._terminal_hold_pending = False + self._dispatch_requested_hold( + required=True, + include_last_targets=True, + ) + self._finish_if_complete() + return self.result + for branch in self._branches: + if not branch.runtime.result.terminal: + branch.runtime.step() + self._update_barrier() + self._dispatch_grid_frame() + self._finish_if_complete() + except Exception as exc: + self._abort_coordinator("Parallel coordinator step failed", exc) + return self.result + + def _timeout_pending_rows(self) -> None: + """Fail and safe-stop deadline-expired rows before another command.""" + timed_out = self._pending.clone() + if not timed_out.any() and self._transport_flush_pending: + timed_out = self._started_eligible.clone() + if not timed_out.any(): + return + self._failure |= timed_out + self._success &= ~timed_out + self._pending &= ~timed_out + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + self._message = f"Parallel barrier timed out after {self._timeout_steps} steps." + errors: list[str] = [] + for branch in self._branches: + if branch.runtime.result.terminal: + continue + try: + branch.runtime.cancel(self._message) + except Exception as exc: + errors.append(f"{branch.branch_id}: {type(exc).__name__}: {exc}") + stopped, stop_message = self._forward_safe_stop() + self._terminal_stop_forwarded = True + if not stopped and stop_message is not None: + errors.append(stop_message) + if errors: + self._message += " Safe stop errors: " + "; ".join(errors) + + def _read_clock(self) -> float: + """Read one finite non-negative timestamp from the shared clock.""" + now = float(self._clock.now()) + if not math.isfinite(now) or now < 0.0: + raise ValueError("ExecutionClock.now() must be finite and non-negative.") + return now + + def _update_elapsed_steps(self) -> None: + """Measure completed environment-grid intervals since start.""" + assert self._start_timestamp is not None + now = self._read_clock() + elapsed = now - self._start_timestamp + if elapsed < -self._timing_policy.tolerance: + raise RuntimeError("Parallel execution clock moved backwards.") + ratio = max(0.0, elapsed) / self._timing_policy.step_dt + tolerance = self._timing_policy.tolerance / self._timing_policy.step_dt + self._elapsed_steps = max( + self._elapsed_steps, + int(math.floor(ratio + tolerance)), + ) + + def _sync_branch_identity(self) -> None: + """Adopt and verify env IDs after every lane's first observation.""" + reference = self._branches[0].runtime.result + for branch in self._branches[1:]: + result = branch.runtime.result + if ( + result.env_ids.device != reference.env_ids.device + or not torch.equal(result.env_ids, reference.env_ids) + or result.task_state.batch_size != reference.task_state.batch_size + or result.task_state.device != reference.task_state.device + ): + raise ValueError( + "Parallel branch observations must share env_ids, batch, " + "and device." + ) + self._env_ids = reference.env_ids.clone() + + def cancel( + self, + reason: str = "Parallel workflow cancelled by caller.", + ) -> ParallelSkillResult: + """Cancel every lane and forward one target-scoped transport cancel.""" + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + if self._status is not SkillStatus.RUNNING: + return self.result + had_transport_flush = self._transport_flush_pending + cancelled = self._pending.clone() + if not cancelled.any() and had_transport_flush: + cancelled = self._started_eligible.clone() + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + errors: list[str] = [] + for branch in self._branches: + try: + branch.runtime.cancel(reason) + except Exception as exc: + errors.append(f"{branch.branch_id}: {type(exc).__name__}: {exc}") + stopped, stop_message = self._forward_safe_stop() + self._terminal_stop_forwarded = True + if stop_message is not None: + errors.append(stop_message) + self._pending &= ~cancelled + self._success &= ~cancelled + merge_succeeded = self._merge_verified_state() + if errors or not stopped or not merge_succeeded: + self._failure |= cancelled + self._cancelled &= ~cancelled + self._status = SkillStatus.FAILED + if errors or not stopped: + stop_detail = "; ".join(errors) or "unknown safe-stop failure" + self._message = reason + " Safe stop failed: " + stop_detail + elif self._message is None: + self._message = reason + " Verified-state merge failed." + else: + self._cancelled |= cancelled + self._status = SkillStatus.CANCELLED + self._message = reason + self._wait_duration = 0.0 + return self.result + + @property + def _transport_flush_pending(self) -> bool: + """Whether a retained command or mandatory final hold is outstanding.""" + return self._deferred_frame is not None or self._terminal_hold_pending + + def _remaining_transport_wait(self) -> float: + """Return time until another normal grid action may be forwarded.""" + ready_at = self._next_transport_at + if ready_at is None: + return 0.0 + remaining = ready_at - self._read_clock() + if remaining <= self._timing_policy.tolerance: + self._next_transport_at = None + return 0.0 + return remaining + + def _record_transport_action(self) -> None: + """Arm the next physical grid boundary after one accepted action.""" + self._next_transport_at = self._read_clock() + self._timing_policy.step_dt + self._wait_duration = self._timing_policy.step_dt + + def _update_barrier(self) -> None: + results = {branch.branch_id: branch.runtime.result for branch in self._branches} + pending = { + branch_id: ( + result.eligible_mask + & ~result.success_mask + & ~result.failure_mask + & ~result.cancelled_mask + ) + for branch_id, result in results.items() + } + update = resolve_parallel_barrier( + pending_masks=pending, + success_masks={ + branch_id: result.success_mask for branch_id, result in results.items() + }, + failure_masks={ + branch_id: result.failure_mask | result.cancelled_mask + for branch_id, result in results.items() + }, + ) + new_failure = update.failure_mask & ~self._failure + self._failure |= update.failure_mask + self._success |= update.completed_mask & ~update.failure_mask + self._pending &= ~update.completed_mask + if new_failure.any(): + self._force_mask_dispatch = True + reason = "A peer parallel branch failed for these environment rows." + for branch in self._branches: + mask = update.cancellation_masks[branch.branch_id] + if mask.any(): + branch.runtime.deactivate_rows(mask, reason=reason) + running = tuple(result for result in results.values() if not result.terminal) + if not running or any(result.wait_duration <= 0.0 for result in running): + self._wait_duration = 0.0 + else: + self._wait_duration = min(result.wait_duration for result in running) + + def _dispatch_grid_frame(self) -> None: + fresh: dict[str, RuntimeCommandFrame] = {} + for branch in self._branches: + frame = branch.command_sink.drain_frame() + if frame is not None: + if branch.runtime.result.terminal: + raise ParallelSafetyError( + f"Parallel branch {branch.branch_id!r} became terminal " + "while emitting a fresh command frame. A post-command " + "observation is required before a safe terminal hold." + ) + fresh[branch.branch_id] = frame + force_mask_dispatch = self._force_mask_dispatch + self._force_mask_dispatch = False + if not fresh and not force_mask_dispatch: + self._dispatch_requested_hold() + return + plans: list[ParallelBranchPlan] = [] + lane_frames: dict[str, RuntimeCommandFrame] = {} + requested_holds = { + _target_fingerprint(target) + for branch in self._branches + for target in branch.command_sink.hold_request[0] + } + for branch in self._branches: + frame = fresh.get(branch.branch_id) + is_fresh = frame is not None + if frame is None: + frame = branch.command_sink.last_frame + if frame is None: + continue + if not is_fresh: + commands = tuple( + command + for command in frame.commands + if _target_fingerprint(command.target) + not in self._held_target_fingerprints | requested_holds + ) + if not commands: + continue + frame = RuntimeCommandFrame( + commands=commands, + active_mask=frame.active_mask, + env_ids=frame.env_ids, + hold_duration=frame.hold_duration, + ) + frame = frame.with_active_mask(frame.active_mask & ~self._failure) + lane_frames[branch.branch_id] = frame.snapshot() + plans.append( + ParallelBranchPlan( + branch_id=branch.branch_id, + claim=branch.claim, + commands=TimedCommandSequence( + frames=(frame,), + env_ids=frame.env_ids, + ), + ) + ) + if not plans: + self._dispatch_requested_hold() + return + if len(plans) == 1: + frame = plans[0].commands.frames[0] + durations = frame.hold_duration + expected = torch.full_like( + durations, + self._timing_policy.step_dt, + ) + if not torch.allclose( + durations, + expected, + atol=self._timing_policy.tolerance, + rtol=0.0, + ): + raise ValueError( + "Parallel command frames must equal the environment step grid." + ) + merged = plans[0].commands + else: + merged = align_parallel_commands(tuple(plans), self._timing_policy) + frame = merged.frames[0] + if not frame.active_mask.any(): + self._dispatch_requested_hold(extra_targets=frame.targets) + return + + if self._has_unforwarded_hold_targets(): + self._deferred_frame = frame.snapshot() + self._deferred_lane_frames = { + branch_id: branch_frame.snapshot() + for branch_id, branch_frame in lane_frames.items() + } + if not self._dispatch_requested_hold(): + self._deferred_frame = None + self._deferred_lane_frames.clear() + return + + # Drain duplicate requests to refresh the latest synchronized context + # without producing another action, then send exactly one grid frame. + self._dispatch_requested_hold() + accepted = self._send_merged_frame(frame, lane_frames) + if accepted and not self._pending.any() and self._status is SkillStatus.RUNNING: + self._terminal_hold_pending = True + + def _dispatch_deferred_frame(self) -> bool: + """Send a frame retained behind one explicit hold-padding step.""" + frame = self._deferred_frame + if frame is None: + raise RuntimeError("No deferred parallel frame is available.") + lane_frames = { + branch_id: branch_frame.snapshot() + for branch_id, branch_frame in self._deferred_lane_frames.items() + } + self._deferred_frame = None + self._deferred_lane_frames.clear() + return self._send_merged_frame(frame, lane_frames) + + def _send_merged_frame( + self, + frame: RuntimeCommandFrame, + lane_frames: Mapping[str, RuntimeCommandFrame], + ) -> bool: + """Validate and forward one active synchronized command frame.""" + try: + safety_result = self._safety_validator.validate( + branch_frames=MappingProxyType(dict(lane_frames)), + merged_frame=frame.snapshot(), + ) + except ParallelSafetyError: + raise + except Exception as exc: + raise ParallelSafetyError( + "Parallel command safety validation failed: " + f"{type(exc).__name__}: {exc}" + ) from exc + if safety_result is not None: + raise ParallelSafetyError( + "ParallelCommandSafetyValidator.validate() must return None." + ) + acknowledgement = self._command_sink.send(frame, timeout=1.0) + if not isinstance(acknowledgement, CommandAcknowledgement): + raise TypeError("CommandSink.send() returned an invalid value.") + if not acknowledgement.accepted: + self._fail_transport(acknowledgement.message) + return False + self._command_count += 1 + self._record_transport_action() + self._held_target_fingerprints.difference_update( + _target_fingerprint(target) for target in frame.targets + ) + return True + + def _has_unforwarded_hold_targets(self) -> bool: + """Whether lane requests contain a target not already physically held.""" + for branch in self._branches: + targets, _ = branch.command_sink.hold_request + if any( + _target_fingerprint(target) not in self._held_target_fingerprints + for target in targets + ): + return True + return False + + def _dispatch_requested_hold( + self, + *, + extra_targets: tuple[RuntimeEndpointTarget, ...] = (), + include_last_targets: bool = False, + required: bool = False, + ) -> bool: + """Forward every lane hold without dropping earlier call targets.""" + targets: dict[Hashable, RuntimeEndpointTarget] = { + _target_fingerprint(target): target for target in extra_targets + } + context: PlanningContext | None = None + for branch in self._branches: + for ( + branch_targets, + branch_context, + ) in branch.command_sink.drain_hold_requests(): + for target in branch_targets: + targets[_target_fingerprint(target)] = target + context = branch_context + self._last_hold_context = branch_context + if include_last_targets: + last_frame = branch.command_sink.last_frame + if last_frame is not None: + for target in last_frame.targets: + targets[_target_fingerprint(target)] = target + targets = { + key: target + for key, target in targets.items() + if key not in self._held_target_fingerprints + } + if not targets: + return True + if context is None: + context = self._last_hold_context + if context is None: + message = "Parallel hold targets have no synchronized planning context." + if required or targets: + self._fail_transport(message) + return False + acknowledgement = self._command_sink.hold( + tuple(targets.values()), + context, + timeout=1.0, + ) + if not isinstance(acknowledgement, CommandAcknowledgement): + raise TypeError("CommandSink.hold() returned an invalid value.") + if not acknowledgement.accepted: + self._fail_transport(acknowledgement.message) + return False + self._held_target_fingerprints.update(targets) + self._last_hold_context = context + self._record_transport_action() + return True + + def _fail_transport(self, message: str) -> None: + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + failed = self._pending.clone() + if not failed.any(): + failed = self._started_eligible.clone() + self._failure |= failed + self._success &= ~failed + self._pending &= ~failed + self._message = "Parallel command transport rejected the merged operation." + if message: + self._message += f" {message}" + for branch in self._branches: + branch.runtime.cancel(self._message) + self._forward_safe_stop() + self._terminal_stop_forwarded = True + + def _forward_safe_stop(self) -> tuple[bool, str | None]: + """Forward lane-owned cancellation and hold once to the real sink.""" + targets: dict[Hashable, RuntimeEndpointTarget] = {} + context: PlanningContext | None = self._last_hold_context + for branch in self._branches: + for target in branch.command_sink.cancel_targets: + targets[_target_fingerprint(target)] = target + branch_targets, branch_context = branch.command_sink.hold_request + for target in branch_targets: + targets[_target_fingerprint(target)] = target + if branch_context is not None: + context = branch_context + last_frame = branch.command_sink.last_frame + if last_frame is not None: + for target in last_frame.targets: + targets[_target_fingerprint(target)] = target + if not targets: + return True, None + snapshots = tuple(targets.values()) + errors: list[str] = [] + try: + cancel_ack = self._command_sink.cancel(snapshots, timeout=1.0) + if not isinstance(cancel_ack, CommandAcknowledgement): + raise TypeError("CommandSink.cancel() returned an invalid value.") + if not cancel_ack.accepted: + errors.append(cancel_ack.message or "transport cancel was rejected") + except Exception as exc: + errors.append(f"cancel {type(exc).__name__}: {exc}") + if context is None: + errors.append("no planning context was available for final safe hold") + else: + try: + hold_ack = self._command_sink.hold( + snapshots, + context, + timeout=1.0, + ) + if not isinstance(hold_ack, CommandAcknowledgement): + raise TypeError("CommandSink.hold() returned an invalid value.") + if not hold_ack.accepted: + errors.append(hold_ack.message or "transport hold was rejected") + except Exception as exc: + errors.append(f"hold {type(exc).__name__}: {exc}") + if not errors: + self._held_target_fingerprints.update( + _target_fingerprint(target) for target in snapshots + ) + self._last_hold_context = context + return (not errors), (None if not errors else "; ".join(errors)) + + def _abort_coordinator(self, prefix: str, exc: Exception) -> None: + """Convert an internal tick exception into a safe terminal failure.""" + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + reason = f"{prefix}: {type(exc).__name__}: {exc}" + failed = self._pending.clone() + if not failed.any(): + failed = self._started_eligible.clone() + self._failure |= failed + self._success &= ~failed + self._pending &= ~failed + errors: list[str] = [] + for branch in self._branches: + if branch.runtime.result.terminal: + continue + try: + branch.runtime.cancel(reason) + except Exception as cancel_exc: + errors.append( + f"{branch.branch_id}: {type(cancel_exc).__name__}: {cancel_exc}" + ) + stopped, stop_message = self._forward_safe_stop() + self._terminal_stop_forwarded = True + if not stopped and stop_message is not None: + errors.append(stop_message) + self._message = reason + if errors: + self._message += " Safe stop errors: " + "; ".join(errors) + self._merge_verified_state() + self._status = SkillStatus.FAILED + self._wait_duration = 0.0 + + def _merge_verified_state(self) -> bool: + """Merge every branch-local verified patch at a terminal barrier.""" + effects = { + branch.branch_id: ( + _task_state_delta( + self._initial_state, + branch.runtime.result.task_state, + ), + self._started_eligible, + ) + for branch in self._branches + } + try: + self._task_state = merge_parallel_effects(self._initial_state, effects) + except Exception as exc: + self._failure |= self._started_eligible + self._success.zero_() + merge_message = ( + "Parallel verified-state merge failed: " f"{type(exc).__name__}: {exc}" + ) + self._message = ( + merge_message + if self._message is None + else f"{self._message} {merge_message}" + ) + return False + return True + + def _finish_if_complete(self) -> None: + if self._pending.any(): + return + if self._deferred_frame is not None or self._terminal_hold_pending: + return + self._merge_verified_state() + if self._status is SkillStatus.RUNNING and not self._terminal_stop_forwarded: + self._dispatch_requested_hold(required=True, include_last_targets=True) + self._wait_duration = 0.0 + if self._failure.any(): + self._status = SkillStatus.FAILED + elif self._cancelled.any(): + self._status = SkillStatus.CANCELLED + else: + self._status = SkillStatus.COMPLETED + + +__all__ = [ + "analyze_parallel_branches", + "ParallelBranchRuntime", + "ParallelBranchStaticAnalysis", + "ParallelCommandSafetyValidator", + "ParallelLaneCommandSink", + "ParallelRuntimeBranch", + "ParallelSkillResult", + "ParallelSkillRuntime", + "ParallelSafetyError", +] diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index a9fe70f14..9623e8a6d 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -43,11 +43,26 @@ DisjointResourceSlots, DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, INVERSE_KINEMATICS_CAPABILITY, SkillBindingContract, SkillResourceSlot, ) from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg +from .effects import ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + POSE_RELATION_EFFECT_CHANNEL, + ControlPartEvidenceAddress, + EffectEvidenceSourceRef, + EffectMonitorRef, +) if TYPE_CHECKING: from embodichain.lab.sim.atomic_actions.engine import AtomicActionEngine @@ -123,6 +138,39 @@ def _snapshot_endpoint_commands( return MappingProxyType(snapshots) +def _snapshot_effect_sources( + values: Mapping[str, EffectEvidenceSourceRef], + *, + field_name: str, +) -> Mapping[str, EffectEvidenceSourceRef]: + """Validate, own, and freeze endpoint observation sources by channel.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + snapshots: dict[str, EffectEvidenceSourceRef] = {} + for channel, source in values.items(): + _validate_identifier(channel, field_name=f"{field_name} channel names") + if not isinstance(source, EffectEvidenceSourceRef): + raise TypeError( + f"{field_name} values must be EffectEvidenceSourceRef instances." + ) + snapshot = source.snapshot() + if snapshot is source: + raise TypeError( + f"{field_name}[{channel!r}].snapshot() must return an independent " + "source reference." + ) + if ( + isinstance(snapshot.address, ControlPartEvidenceAddress) + and snapshot.address.channel != channel + ): + raise ValueError( + f"{field_name}[{channel!r}] disagrees with its control-part " + f"address channel {snapshot.address.channel!r}." + ) + snapshots[channel] = snapshot + return MappingProxyType(snapshots) + + @dataclass(frozen=True, slots=True, kw_only=True) class ResourceEndpoint(ABC): """Extensible execution endpoint in a robot resource graph. @@ -188,6 +236,12 @@ class EndpointResolution: runtime_target: RuntimeEndpointTarget """Typed immutable destination consumed by an endpoint command transport.""" + task_state_key: str | None = None + """Optional symbolic state key; profile binding defaults to its resource ID.""" + + effect_sources: Mapping[str, EffectEvidenceSourceRef] = field(default_factory=dict) + """Provider-routed raw observation sources keyed by open channel ID.""" + command_profile_key: str | None = None """Profile key that owns semantic commands for this endpoint, when any.""" @@ -226,6 +280,19 @@ def __post_init__(self) -> None: field_name="RuntimeEndpointTarget.target_id", ) object.__setattr__(self, "runtime_target", target) + if self.task_state_key is not None: + _validate_identifier( + self.task_state_key, + field_name="EndpointResolution.task_state_key", + ) + object.__setattr__( + self, + "effect_sources", + _snapshot_effect_sources( + self.effect_sources, + field_name="EndpointResolution.effect_sources", + ), + ) if self.command_profile_key is not None: _validate_identifier( self.command_profile_key, @@ -356,6 +423,18 @@ def resolve( f"Control part {endpoint.control_part!r} declares solver-backed " f"capabilities {sorted(declared)}, but has no configured solver." ) + effect_channels = { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } + if GRASP_CAPABILITY in endpoint.capabilities: + effect_channels.update( + { + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + } + ) return EndpointResolution( runtime_target=JointPositionTarget( control_part=endpoint.control_part, @@ -367,6 +446,14 @@ def resolve( else endpoint.command_profile ), requires_command_profile=endpoint.command_profile is not None, + effect_sources={ + channel: EffectEvidenceSourceRef( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress(endpoint.control_part, channel), + ) + for channel in sorted(effect_channels) + }, claim_tokens=frozenset({f"robot.control_part:{endpoint.control_part}"}), joint_ids=joint_ids, ) @@ -379,6 +466,8 @@ class ResolvedResourceEndpoint: endpoint: ResourceEndpoint adapter_id: str runtime_target: RuntimeEndpointTarget + task_state_key: str | None = None + effect_sources: Mapping[str, EffectEvidenceSourceRef] = field(default_factory=dict) command_profile_key: str | None = None requires_command_profile: bool = False commands: Mapping[str, ControlCommand] = field(default_factory=dict) @@ -405,6 +494,8 @@ def __post_init__(self) -> None: ) resolution = EndpointResolution( runtime_target=self.runtime_target, + task_state_key=self.task_state_key, + effect_sources=self.effect_sources, command_profile_key=self.command_profile_key, requires_command_profile=self.requires_command_profile, claim_tokens=self.claim_tokens, @@ -412,6 +503,13 @@ def __post_init__(self) -> None: exclusive=self.exclusive, ) object.__setattr__(self, "runtime_target", resolution.runtime_target) + resolved_state_key = ( + resolution.runtime_target.target_id + if resolution.task_state_key is None + else resolution.task_state_key + ) + object.__setattr__(self, "task_state_key", resolved_state_key) + object.__setattr__(self, "effect_sources", resolution.effect_sources) object.__setattr__( self, "command_profile_key", @@ -548,13 +646,14 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True, init=False) class SkillPolicyPreset: - """Versioned planning, recovery, and runner policy bundle.""" + """Versioned planning, recovery, runner, and effect-monitor bundle.""" preset_id: str schema_version: int _motion_policy: MotionPolicy _recovery_policy: RecoveryPolicy _runner_cfg: ExecutionRunnerCfg + _effect_monitors: Mapping[str, EffectMonitorRef] def __init__( self, @@ -563,6 +662,7 @@ def __init__( motion_policy: MotionPolicy | None = None, recovery_policy: RecoveryPolicy | None = None, runner_cfg: ExecutionRunnerCfg | None = None, + effect_monitors: Mapping[str, EffectMonitorRef] | None = None, ) -> None: """Own one policy bundle without exposing mutable nested configuration.""" _validate_identifier(preset_id, field_name="SkillPolicyPreset.preset_id") @@ -584,11 +684,45 @@ def __init__( raise TypeError("recovery_policy must be a RecoveryPolicy.") if not isinstance(selected_runner, ExecutionRunnerCfg): raise TypeError("runner_cfg must be an ExecutionRunnerCfg.") + selected_effect_monitors = ( + { + semantic_id: EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + for semantic_id in ( + "pick", + "place", + "hand_over", + "operate_articulation", + ) + } + if effect_monitors is None + else effect_monitors + ) + if not isinstance(selected_effect_monitors, Mapping): + raise TypeError("effect_monitors must be a mapping or None.") + normalized_effect_monitors: dict[str, EffectMonitorRef] = {} + for semantic_id, monitor_ref in selected_effect_monitors.items(): + _validate_identifier( + semantic_id, + field_name="SkillPolicyPreset effect semantic IDs", + ) + if not isinstance(monitor_ref, EffectMonitorRef): + raise TypeError( + "effect_monitors values must be EffectMonitorRef instances." + ) + normalized_effect_monitors[semantic_id] = monitor_ref.snapshot() object.__setattr__(self, "preset_id", preset_id) object.__setattr__(self, "schema_version", schema_version) object.__setattr__(self, "_motion_policy", deepcopy(selected_motion)) object.__setattr__(self, "_recovery_policy", deepcopy(selected_recovery)) object.__setattr__(self, "_runner_cfg", deepcopy(selected_runner)) + object.__setattr__( + self, + "_effect_monitors", + MappingProxyType(normalized_effect_monitors), + ) @property def motion_policy(self) -> MotionPolicy: @@ -605,6 +739,16 @@ def runner_cfg(self) -> ExecutionRunnerCfg: """Return an independently owned runner configuration.""" return deepcopy(self._runner_cfg) + @property + def effect_monitors(self) -> Mapping[str, EffectMonitorRef]: + """Return effect-monitor selections keyed by exact semantic call ID.""" + return MappingProxyType( + { + semantic_id: monitor_ref.snapshot() + for semantic_id, monitor_ref in self._effect_monitors.items() + } + ) + def snapshot(self) -> SkillPolicyPreset: """Return an independently owned preset value.""" return SkillPolicyPreset( @@ -613,6 +757,7 @@ def snapshot(self) -> SkillPolicyPreset: motion_policy=self.motion_policy, recovery_policy=self.recovery_policy, runner_cfg=self.runner_cfg, + effect_monitors=self.effect_monitors, ) @@ -1454,6 +1599,12 @@ def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: endpoint=endpoint, adapter_id=adapter.adapter_id, runtime_target=resolution.runtime_target, + task_state_key=( + resource_id + if resolution.task_state_key is None + else resolution.task_state_key + ), + effect_sources=resolution.effect_sources, command_profile_key=resolution.command_profile_key, requires_command_profile=resolution.requires_command_profile, commands=( @@ -1853,6 +2004,7 @@ def _lower_binding( resource_id=resource.resource_id, adapter_id=endpoint.adapter_id, target=endpoint.runtime_target, + task_state_key=endpoint.task_state_key, capabilities=endpoint.capabilities, commands=endpoint.commands, claim_tokens=endpoint.claim_tokens, diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py new file mode 100644 index 000000000..de98f5ed3 --- /dev/null +++ b/embodichain/lab/sim/skills/runtime.py @@ -0,0 +1,2294 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Canonical execution service and convenience facade for semantic skills.""" + +from __future__ import annotations + +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, replace +from enum import Enum +import math +from types import MappingProxyType +from typing import Protocol, runtime_checkable + +import torch + +from ..atomic_actions.bindings import EndpointBinding +from ..atomic_actions.engine import AtomicActionEngine +from ..atomic_actions.execution import ( + EffectVerificationRequest, + EffectVerificationResult, + ExecutionEvent, + ExecutionPlanAttempt, +) +from ..atomic_actions.plans import ExecutionFeedbackMode, TrajectorySegment +from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy +from ..atomic_actions.runner import ( + CommandSink, + ExecutionClock, + ExecutionRunner, + ExecutionRunnerCfg, + MonotonicExecutionClock, + ObservationProvider, + RunnerStatus, + RunnerStep, +) +from ..atomic_actions.state import PlanningContext, TaskState +from .calls import SemanticCallSpec +from .compiler import SemanticSkillCompiler +from .effects import ( + BinaryEffectEvidenceBatch, + EffectEvidenceBatch, + EffectMonitor, + EffectMonitorRef, + JointStateEvidenceBatch, + PoseRelationEvidenceBatch, + ScalarEffectEvidenceBatch, + SemanticEffectSpec, +) +from .scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + + +def _snapshot_task_state(state: TaskState) -> TaskState: + """Return a tensor-owning snapshot of verified symbolic state.""" + return TaskState( + batch_size=state.batch_size, + device=state.device, + held_objects=state.held_objects, + coordinated_held_objects=state.coordinated_held_objects, + articulation_joints=state.articulation_joints, + ) + + +def _snapshot_event(event: ExecutionEvent) -> ExecutionEvent: + """Return an independently owned execution event.""" + return ExecutionEvent( + kind=event.kind, + timestamp=event.timestamp, + skill_id=event.skill_id, + invocation_id=event.invocation_id, + invocation_revision=event.invocation_revision, + invocation_index=event.invocation_index, + env_mask=event.env_mask, + message=event.message, + ) + + +def _metadata_value(value: object, *, depth: int = 0) -> object: + """Convert supported runtime diagnostics to deterministic JSON-safe data.""" + if depth > 16: + return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + return value if math.isfinite(value) else None + if isinstance(value, Enum): + return value.value + if isinstance(value, torch.Tensor): + return _metadata_value(value.detach().cpu().tolist(), depth=depth + 1) + if isinstance(value, torch.device): + return str(value) + if isinstance(value, Mapping): + items = sorted(value.items(), key=lambda item: str(item[0])) + if all(type(key) is str and key and key == key.strip() for key, _ in items): + return { + key: _metadata_value(nested, depth=depth + 1) for key, nested in items + } + return { + "__entries__": [ + { + "key": _metadata_value(key, depth=depth + 1), + "value": _metadata_value(nested, depth=depth + 1), + } + for key, nested in items + ] + } + if isinstance(value, (tuple, list)): + return [_metadata_value(nested, depth=depth + 1) for nested in value] + if isinstance(value, (set, frozenset)): + return [ + _metadata_value(nested, depth=depth + 1) + for nested in sorted(value, key=str) + ] + return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} + + +def _snapshot_metadata_mapping(value: Mapping[str, object]) -> Mapping[str, object]: + """Own one JSON-safe string-keyed metadata mapping.""" + if not isinstance(value, Mapping): + raise TypeError("metadata must be a mapping.") + normalized = _metadata_value(value) + if not isinstance(normalized, dict): + raise TypeError("metadata normalization must produce a dict.") + return MappingProxyType(normalized) + + +def _event_to_metadata(event: ExecutionEvent) -> dict[str, object]: + """Serialize one execution/recovery event without exposing tensors.""" + return { + "kind": event.kind.value, + "timestamp": _metadata_value(event.timestamp), + "skill_id": event.skill_id, + "invocation_id": event.invocation_id, + "invocation_revision": event.invocation_revision, + "invocation_index": event.invocation_index, + "env_mask": _metadata_value(event.env_mask), + "message": event.message, + } + + +def task_state_to_metadata(state: TaskState) -> dict[str, object]: + """Return verified symbolic task state as deterministic JSON-safe data.""" + if not isinstance(state, TaskState): + raise TypeError("state must be a TaskState.") + held = [] + for resource_id, value in sorted(state.held_objects.items()): + held.append( + { + "resource_id": resource_id, + "object_id": value.semantics.entity_id, + "object_label": value.semantics.label, + "object_to_eef": _metadata_value(value.object_to_eef), + "grasp_xpos": _metadata_value(value.grasp_xpos), + "active_mask": _metadata_value(value.env_mask), + } + ) + coordinated = [] + for resource_ids, value in sorted(state.coordinated_held_objects.items()): + coordinated.append( + { + "resource_ids": list(resource_ids), + "object_id": value.semantics.entity_id, + "object_label": value.semantics.label, + "left_object_to_eef": _metadata_value(value.left_object_to_eef), + "right_object_to_eef": _metadata_value(value.right_object_to_eef), + "left_grasp_xpos": _metadata_value(value.left_grasp_xpos), + "right_grasp_xpos": _metadata_value(value.right_grasp_xpos), + "active_mask": _metadata_value(value.env_mask), + } + ) + articulations = [] + for (articulation_id, joint_id), value in sorted(state.articulation_joints.items()): + articulations.append( + { + "articulation_id": articulation_id, + "joint_id": joint_id, + "position": _metadata_value(value.position), + "active_mask": _metadata_value(value.env_mask), + } + ) + return { + "batch_size": state.batch_size, + "device": str(state.device), + "held_objects": held, + "coordinated_held_objects": coordinated, + "articulation_joints": articulations, + } + + +class SkillStatus(str, Enum): + """Lifecycle state of one semantic workflow run.""" + + IDLE = "idle" + RUNNING = "running" + COMPLETED = "completed" + FAILED = "failed" + CANCELLED = "cancelled" + + +@dataclass(frozen=True, slots=True) +class SkillEndpointBindingTrace: + """JSON-safe typed projection of one resolved execution endpoint.""" + + slot_id: str + endpoint_id: str + resource_id: str + adapter_id: str + transport_id: str + target_id: str + target_type: str + task_state_key: str + capabilities: tuple[str, ...] + command_ids: tuple[str, ...] + claim_tokens: tuple[str, ...] + joint_ids: tuple[int, ...] + + def __post_init__(self) -> None: + for name in ( + "slot_id", + "endpoint_id", + "resource_id", + "adapter_id", + "transport_id", + "target_id", + "target_type", + "task_state_key", + ): + value = getattr(self, name) + if type(value) is not str or not value: + raise ValueError(f"{name} must be a non-empty string.") + for name in ("capabilities", "command_ids", "claim_tokens"): + values = tuple(getattr(self, name)) + if tuple(sorted(set(values))) != values or not all( + type(value) is str and value for value in values + ): + raise ValueError(f"{name} must contain sorted unique identifiers.") + object.__setattr__(self, name, values) + joint_ids = tuple(self.joint_ids) + if len(set(joint_ids)) != len(joint_ids) or not all( + type(value) is int and value >= 0 for value in joint_ids + ): + raise ValueError("joint_ids must contain unique non-negative integers.") + object.__setattr__(self, "joint_ids", joint_ids) + + @classmethod + def from_binding(cls, binding: EndpointBinding) -> SkillEndpointBindingTrace: + """Project one owned endpoint binding without retaining its target.""" + if not isinstance(binding, EndpointBinding): + raise TypeError("binding must be an EndpointBinding.") + target = binding.target + return cls( + slot_id=binding.slot_id, + endpoint_id=binding.endpoint_id, + resource_id=binding.resource_id, + adapter_id=binding.adapter_id, + transport_id=target.transport_id, + target_id=target.target_id, + target_type=f"{type(target).__module__}.{type(target).__qualname__}", + task_state_key=binding.task_state_key, + capabilities=tuple(sorted(binding.capabilities)), + command_ids=tuple(sorted(binding.commands)), + claim_tokens=tuple(sorted(binding.claim_tokens)), + joint_ids=binding.joint_ids, + ) + + def to_metadata(self) -> dict[str, object]: + """Return stable endpoint, resource, adapter, and transport metadata.""" + return { + "slot_id": self.slot_id, + "endpoint_id": self.endpoint_id, + "resource_id": self.resource_id, + "adapter_id": self.adapter_id, + "transport_id": self.transport_id, + "target_id": self.target_id, + "target_type": self.target_type, + "task_state_key": self.task_state_key, + "capabilities": list(self.capabilities), + "command_ids": list(self.command_ids), + "claim_tokens": list(self.claim_tokens), + "joint_ids": list(self.joint_ids), + } + + +def _motion_policy_to_metadata(policy: MotionPolicy) -> dict[str, object]: + """Serialize one owned core motion policy without retaining planner objects.""" + plan_options = policy.plan_opts + options_metadata: object = None + if plan_options is not None: + values = ( + plan_options.to_dict() + if callable(getattr(plan_options, "to_dict", None)) + else None + ) + options_metadata = { + "type": f"{type(plan_options).__module__}.{type(plan_options).__qualname__}", + "values": _metadata_value(values), + } + return { + "planner": policy.planner, + "strategy": policy.strategy, + "sample_count": policy.sample_count, + "control_dt": _metadata_value(policy.control_dt), + "velocity_limit": _metadata_value(policy.velocity_limit), + "acceleration_limit": _metadata_value(policy.acceleration_limit), + "dynamic_collision_mode": policy.dynamic_collision_mode.value, + "plan_options": options_metadata, + } + + +def _recovery_policy_to_metadata(policy: RecoveryPolicy) -> dict[str, object]: + """Serialize all bounded-recovery settings.""" + return { + "max_replans": policy.max_replans, + "max_action_retries": policy.max_action_retries, + "tracking_error_threshold": _metadata_value(policy.tracking_error_threshold), + "goal_translation_threshold": _metadata_value( + policy.goal_translation_threshold + ), + "goal_rotation_threshold": _metadata_value(policy.goal_rotation_threshold), + "action_timeout": _metadata_value(policy.action_timeout), + } + + +@dataclass(frozen=True, slots=True) +class ResolvedCorePolicyTrace: + """Resolved preset, core policies, and execution binding for one plan.""" + + profile_id: str + preset_id: str + preset_schema_version: int + motion_policy: MotionPolicy + recovery_policy: RecoveryPolicy + endpoints: tuple[SkillEndpointBindingTrace, ...] + + def __post_init__(self) -> None: + for name in ("profile_id", "preset_id"): + value = getattr(self, name) + if type(value) is not str or not value: + raise ValueError(f"{name} must be a non-empty string.") + if ( + type(self.preset_schema_version) is not int + or self.preset_schema_version < 1 + ): + raise ValueError("preset_schema_version must be a positive integer.") + if not isinstance(self.motion_policy, MotionPolicy): + raise TypeError("motion_policy must be a MotionPolicy.") + if not isinstance(self.recovery_policy, RecoveryPolicy): + raise TypeError("recovery_policy must be a RecoveryPolicy.") + endpoints = tuple(self.endpoints) + if not all(type(value) is SkillEndpointBindingTrace for value in endpoints): + raise TypeError( + "endpoints must contain exact SkillEndpointBindingTrace values." + ) + keys = tuple((value.slot_id, value.endpoint_id) for value in endpoints) + if len(set(keys)) != len(keys): + raise ValueError("endpoints must use unique slot/endpoint keys.") + object.__setattr__(self, "motion_policy", replace(self.motion_policy)) + object.__setattr__(self, "recovery_policy", replace(self.recovery_policy)) + object.__setattr__(self, "endpoints", endpoints) + + @classmethod + def from_resolved_binding( + cls, + *, + profile_id: str, + preset_id: str, + preset_schema_version: int, + motion_policy: MotionPolicy, + recovery_policy: RecoveryPolicy, + endpoints: Iterable[EndpointBinding], + ) -> ResolvedCorePolicyTrace: + """Project one resolved preset and action binding to a trace.""" + return cls( + profile_id=profile_id, + preset_id=preset_id, + preset_schema_version=preset_schema_version, + motion_policy=motion_policy, + recovery_policy=recovery_policy, + endpoints=tuple( + SkillEndpointBindingTrace.from_binding(endpoint) + for endpoint in endpoints + ), + ) + + def snapshot(self) -> ResolvedCorePolicyTrace: + """Return an independently owned core-policy and binding trace.""" + return ResolvedCorePolicyTrace( + profile_id=self.profile_id, + preset_id=self.preset_id, + preset_schema_version=self.preset_schema_version, + motion_policy=self.motion_policy, + recovery_policy=self.recovery_policy, + endpoints=self.endpoints, + ) + + def to_metadata(self) -> dict[str, object]: + """Return deterministic policy and endpoint-binding metadata.""" + return { + "profile_id": self.profile_id, + "preset": { + "preset_id": self.preset_id, + "schema_version": self.preset_schema_version, + }, + "motion_policy": _motion_policy_to_metadata(self.motion_policy), + "recovery_policy": _recovery_policy_to_metadata(self.recovery_policy), + "endpoints": [endpoint.to_metadata() for endpoint in self.endpoints], + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillPlanAttemptTrace: + """Compact, typed trace of one installed action-plan generation. + + ``scene_dependency_monitor_until`` preserves the plan's per-entity exclusive + waypoint cutoff: an entity is monitored only while the current waypoint index + is smaller than its configured value. + """ + + attempt_generation: int + trigger: str + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + skill_id: str + invocation_id: str | None + invocation_revision: int + plan_success_mask: torch.Tensor + command_frame_count: int + trajectory_segments: tuple[TrajectorySegment, ...] + planned_scene_version: int + planned_collision_world_revision: tuple[int, ...] + scene_dependencies: tuple[str, ...] + scene_dependency_monitor_until: Mapping[str, int] + collision_world_sensitive: bool + replannable: bool + feedback_mode: ExecutionFeedbackMode + effect_verification_kind: str | None + resolved_core_policy: ResolvedCorePolicyTrace + planner_backend: str + planner_messages: tuple[str, ...] + planner_metadata: Mapping[str, object] + + def __post_init__(self) -> None: + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be non-negative.") + if type(self.trigger) is not str or not self.trigger: + raise ValueError("trigger must be a non-empty string.") + if not math.isfinite(self.planned_at) or self.planned_at < 0.0: + raise ValueError("planned_at must be finite and non-negative.") + if type(self.invocation_index) is not int or self.invocation_index < 0: + raise ValueError("invocation_index must be non-negative.") + for name in ("planned_mask", "plan_success_mask"): + value = getattr(self, name) + if ( + not isinstance(value, torch.Tensor) + or value.dtype != torch.bool + or value.dim() != 1 + ): + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if self.planned_mask.shape != self.plan_success_mask.shape: + raise ValueError("Plan-attempt masks must have equal shapes.") + if self.planned_mask.device != self.plan_success_mask.device: + raise ValueError("Plan-attempt masks must share a device.") + batch_size = int(self.planned_mask.numel()) + retries = tuple(self.action_retry_counts) + replans = tuple(self.replan_counts) + if len(retries) != batch_size or len(replans) != batch_size: + raise ValueError("Recovery counters must contain one value per row.") + if any(type(value) is not int or value < 0 for value in (*retries, *replans)): + raise ValueError("Recovery counters must be non-negative integers.") + if type(self.skill_id) is not str or not self.skill_id: + raise ValueError("skill_id must be a non-empty string.") + if self.invocation_id is not None and ( + type(self.invocation_id) is not str or not self.invocation_id + ): + raise ValueError("invocation_id must be a non-empty string or None.") + if type(self.invocation_revision) is not int or self.invocation_revision < 0: + raise ValueError("invocation_revision must be non-negative.") + if type(self.command_frame_count) is not int or self.command_frame_count < 0: + raise ValueError("command_frame_count must be non-negative.") + segments = tuple(self.trajectory_segments) + if not all(type(value) is TrajectorySegment for value in segments): + raise TypeError( + "trajectory_segments must contain TrajectorySegment values." + ) + if ( + type(self.planned_scene_version) is not int + or self.planned_scene_version < 0 + ): + raise ValueError("planned_scene_version must be non-negative.") + collision_revisions = tuple(self.planned_collision_world_revision) + if len(collision_revisions) != batch_size or any( + type(value) is not int or value < 0 for value in collision_revisions + ): + raise ValueError( + "planned_collision_world_revision must contain one non-negative " + "integer per row." + ) + dependencies = tuple(self.scene_dependencies) + if len(set(dependencies)) != len(dependencies) or not all( + type(value) is str and value for value in dependencies + ): + raise ValueError("scene_dependencies must contain unique identifiers.") + if not isinstance(self.scene_dependency_monitor_until, Mapping): + raise TypeError("scene_dependency_monitor_until must be a mapping.") + monitor_until = dict(self.scene_dependency_monitor_until) + if not set(monitor_until).issubset(dependencies): + raise ValueError( + "scene_dependency_monitor_until keys must be scene dependencies." + ) + for entity_id, waypoint_index in monitor_until.items(): + if ( + type(entity_id) is not str + or not entity_id + or type(waypoint_index) is not int + or not 0 <= waypoint_index <= self.command_frame_count + ): + raise ValueError( + "scene_dependency_monitor_until must map non-empty entity IDs " + "to waypoint indices within the command sequence." + ) + if type(self.collision_world_sensitive) is not bool: + raise TypeError("collision_world_sensitive must be a bool.") + if type(self.replannable) is not bool: + raise TypeError("replannable must be a bool.") + if not isinstance(self.feedback_mode, ExecutionFeedbackMode): + raise TypeError("feedback_mode must be an ExecutionFeedbackMode.") + if self.effect_verification_kind is not None and ( + type(self.effect_verification_kind) is not str + or not self.effect_verification_kind + ): + raise ValueError("effect_verification_kind must be non-empty or None.") + if type(self.resolved_core_policy) is not ResolvedCorePolicyTrace: + raise TypeError( + "resolved_core_policy must be exactly ResolvedCorePolicyTrace." + ) + if type(self.planner_backend) is not str or not self.planner_backend: + raise ValueError("planner_backend must be a non-empty string.") + messages = tuple(self.planner_messages) + if not all(type(value) is str for value in messages): + raise TypeError("planner_messages must contain strings.") + object.__setattr__(self, "planned_mask", self.planned_mask.clone()) + object.__setattr__(self, "plan_success_mask", self.plan_success_mask.clone()) + object.__setattr__(self, "action_retry_counts", retries) + object.__setattr__(self, "replan_counts", replans) + object.__setattr__(self, "trajectory_segments", segments) + object.__setattr__( + self, + "planned_collision_world_revision", + collision_revisions, + ) + object.__setattr__(self, "scene_dependencies", dependencies) + object.__setattr__( + self, + "scene_dependency_monitor_until", + MappingProxyType(monitor_until), + ) + object.__setattr__( + self, + "resolved_core_policy", + self.resolved_core_policy.snapshot(), + ) + object.__setattr__(self, "planner_messages", messages) + object.__setattr__( + self, + "planner_metadata", + _snapshot_metadata_mapping(self.planner_metadata), + ) + + @classmethod + def from_execution_attempt( + cls, + attempt: ExecutionPlanAttempt, + *, + profile_id: str, + preset_id: str, + preset_schema_version: int, + ) -> SkillPlanAttemptTrace: + """Project one session-owned plan attempt to compact trace metadata.""" + if not isinstance(attempt, ExecutionPlanAttempt): + raise TypeError("attempt must be an ExecutionPlanAttempt.") + plan = attempt.plan + request = attempt.request + return cls( + attempt_generation=attempt.attempt_generation, + trigger=attempt.event_kind.value, + planned_at=attempt.planned_at, + invocation_index=attempt.invocation_index, + planned_mask=attempt.planned_mask, + action_retry_counts=attempt.action_retry_counts, + replan_counts=attempt.replan_counts, + skill_id=plan.skill_id, + invocation_id=plan.invocation_id, + invocation_revision=plan.invocation_revision, + plan_success_mask=plan.plan_success, + command_frame_count=plan.commands.frame_count, + trajectory_segments=plan.segments, + planned_scene_version=plan.planned_scene_version, + planned_collision_world_revision=plan.planned_collision_world_revision, + scene_dependencies=plan.scene_dependencies, + scene_dependency_monitor_until=plan.scene_dependency_monitor_until, + collision_world_sensitive=plan.collision_world_sensitive, + replannable=plan.replannable, + feedback_mode=plan.feedback_mode, + effect_verification_kind=( + None + if plan.effect_verification is None + else plan.effect_verification.kind + ), + resolved_core_policy=ResolvedCorePolicyTrace.from_resolved_binding( + profile_id=profile_id, + preset_id=preset_id, + preset_schema_version=preset_schema_version, + motion_policy=request.motion_policy, + recovery_policy=request.recovery_policy, + endpoints=request.binding.endpoints, + ), + planner_backend=plan.diagnostics.backend, + planner_messages=plan.diagnostics.messages, + planner_metadata=plan.diagnostics.metadata, + ) + + def snapshot(self) -> SkillPlanAttemptTrace: + """Return an independently owned compact plan-attempt trace.""" + return SkillPlanAttemptTrace( + attempt_generation=self.attempt_generation, + trigger=self.trigger, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + plan_success_mask=self.plan_success_mask, + command_frame_count=self.command_frame_count, + trajectory_segments=self.trajectory_segments, + planned_scene_version=self.planned_scene_version, + planned_collision_world_revision=self.planned_collision_world_revision, + scene_dependencies=self.scene_dependencies, + scene_dependency_monitor_until=self.scene_dependency_monitor_until, + collision_world_sensitive=self.collision_world_sensitive, + replannable=self.replannable, + feedback_mode=self.feedback_mode, + effect_verification_kind=self.effect_verification_kind, + resolved_core_policy=self.resolved_core_policy, + planner_backend=self.planner_backend, + planner_messages=self.planner_messages, + planner_metadata=self.planner_metadata, + ) + + def to_metadata(self) -> dict[str, object]: + """Return one plan generation as deterministic JSON-safe data.""" + return { + "attempt_generation": self.attempt_generation, + "trigger": self.trigger, + "planned_at": self.planned_at, + "invocation_index": self.invocation_index, + "planned_mask": _metadata_value(self.planned_mask), + "recovery_counters": { + "action_retries": list(self.action_retry_counts), + "replans": list(self.replan_counts), + }, + "skill_id": self.skill_id, + "invocation_id": self.invocation_id, + "invocation_revision": self.invocation_revision, + "plan_success_mask": _metadata_value(self.plan_success_mask), + "command_frame_count": self.command_frame_count, + "trajectory_segments": [ + { + "name": segment.name, + "start": segment.start, + "stop": segment.stop, + "waypoint_count": segment.waypoint_count, + } + for segment in self.trajectory_segments + ], + "planned_scene_version": self.planned_scene_version, + "planned_collision_world_revision": list( + self.planned_collision_world_revision + ), + "scene_dependencies": list(self.scene_dependencies), + "scene_dependency_monitor_until": { + entity_id: self.scene_dependency_monitor_until[entity_id] + for entity_id in sorted(self.scene_dependency_monitor_until) + }, + "collision_world_sensitive": self.collision_world_sensitive, + "replannable": self.replannable, + "feedback_mode": self.feedback_mode.value, + "effect_verification_kind": self.effect_verification_kind, + "resolved_core_policy": self.resolved_core_policy.to_metadata(), + "planner_diagnostics": { + "backend": self.planner_backend, + "messages": list(self.planner_messages), + "metadata": _metadata_value(self.planner_metadata), + }, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillEffectTrace: + """One monitor decision correlated with an atomic verification boundary.""" + + call_index: int + verification_id: int + observation_revision: int + timestamp: float + success_mask: torch.Tensor + failure_mask: torch.Tensor + effect_spec: SemanticEffectSpec + monitor_id: str + monitor_revision: str | None + configured_monitor_params: Mapping[str, object] + resolved_monitor_params: Mapping[str, object] + evidence: Mapping[str, EffectEvidenceBatch] + + def __post_init__(self) -> None: + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") + if type(self.observation_revision) is not int or self.observation_revision < 0: + raise ValueError("observation_revision must be non-negative.") + if not math.isfinite(self.timestamp) or self.timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + for name in ("success_mask", "failure_mask"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if self.success_mask.shape != self.failure_mask.shape: + raise ValueError("Effect trace masks must have equal shapes.") + if self.success_mask.device != self.failure_mask.device: + raise ValueError("Effect trace masks must share a device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("Effect trace masks must not overlap.") + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec.") + if type(self.monitor_id) is not str or not self.monitor_id: + raise ValueError("monitor_id must be a non-empty string.") + if self.monitor_revision is not None and ( + type(self.monitor_revision) is not str or not self.monitor_revision + ): + raise ValueError("monitor_revision must be non-empty or None.") + evidence_types = ( + PoseRelationEvidenceBatch, + BinaryEffectEvidenceBatch, + ScalarEffectEvidenceBatch, + JointStateEvidenceBatch, + ) + evidence: dict[str, EffectEvidenceBatch] = {} + for evidence_id, batch in self.evidence.items(): + if type(evidence_id) is not str or not evidence_id: + raise ValueError("evidence keys must be non-empty strings.") + if type(batch) not in evidence_types: + raise TypeError("evidence values must be exact evidence batches.") + if batch.evidence_id != evidence_id: + raise ValueError("evidence keys must match batch evidence_id values.") + evidence[evidence_id] = batch.snapshot() + object.__setattr__(self, "success_mask", self.success_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) + object.__setattr__( + self, + "configured_monitor_params", + _snapshot_metadata_mapping(self.configured_monitor_params), + ) + object.__setattr__( + self, + "resolved_monitor_params", + _snapshot_metadata_mapping(self.resolved_monitor_params), + ) + object.__setattr__(self, "evidence", MappingProxyType(evidence)) + + def snapshot(self) -> SkillEffectTrace: + """Return an independently owned trace.""" + return SkillEffectTrace( + call_index=self.call_index, + verification_id=self.verification_id, + observation_revision=self.observation_revision, + timestamp=self.timestamp, + success_mask=self.success_mask, + failure_mask=self.failure_mask, + effect_spec=self.effect_spec, + monitor_id=self.monitor_id, + monitor_revision=self.monitor_revision, + configured_monitor_params=self.configured_monitor_params, + resolved_monitor_params=self.resolved_monitor_params, + evidence=self.evidence, + ) + + def to_metadata(self) -> dict[str, object]: + """Return monitor contract, evidence, thresholds, and decision metadata.""" + return { + "call_index": self.call_index, + "verification_id": self.verification_id, + "observation_revision": self.observation_revision, + "timestamp": self.timestamp, + "effect_spec": self.effect_spec.to_metadata(), + "monitor": { + "monitor_id": self.monitor_id, + "revision": self.monitor_revision, + "configured_params": _metadata_value(self.configured_monitor_params), + "resolved_params": _metadata_value(self.resolved_monitor_params), + }, + "evidence": { + evidence_id: batch.to_metadata() + for evidence_id, batch in sorted(self.evidence.items()) + }, + "decision": { + "success_mask": _metadata_value(self.success_mask), + "failure_mask": _metadata_value(self.failure_mask), + }, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillFailure: + """Per-environment semantic workflow failure.""" + + call_index: int + semantic_id: str + env_mask: torch.Tensor + message: str + + def __post_init__(self) -> None: + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + if type(self.semantic_id) is not str or not self.semantic_id: + raise ValueError("semantic_id must be a non-empty string.") + if not isinstance(self.env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") + if self.env_mask.dtype != torch.bool or self.env_mask.dim() != 1: + raise ValueError("env_mask must be a one-dimensional bool tensor.") + if type(self.message) is not str or not self.message: + raise ValueError("message must be a non-empty string.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + + def snapshot(self) -> SkillFailure: + """Return an independently owned failure.""" + return SkillFailure( + call_index=self.call_index, + semantic_id=self.semantic_id, + env_mask=self.env_mask, + message=self.message, + ) + + def to_metadata(self) -> dict[str, object]: + """Return one row-local failure as JSON-safe data.""" + return { + "call_index": self.call_index, + "semantic_id": self.semantic_id, + "env_mask": _metadata_value(self.env_mask), + "message": self.message, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillCallTrace: + """Terminal trace for exactly one semantic call and execution session.""" + + call_index: int + semantic_id: str + call_metadata: Mapping[str, object] + skill_id: str + invocation_id: str | None + invocation_revision: int + status: RunnerStatus + entered_mask: torch.Tensor + completed_mask: torch.Tensor + failed_mask: torch.Tensor + command_count: int + resolved_core_policy: ResolvedCorePolicyTrace + plan_attempts: tuple[SkillPlanAttemptTrace, ...] + events: tuple[ExecutionEvent, ...] = () + effects: tuple[SkillEffectTrace, ...] = () + + def __post_init__(self) -> None: + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + for name in ("semantic_id", "skill_id"): + value = getattr(self, name) + if type(value) is not str or not value: + raise ValueError(f"{name} must be a non-empty string.") + normalized_call = _snapshot_metadata_mapping(self.call_metadata) + if normalized_call.get("semantic_id") != self.semantic_id: + raise ValueError("call_metadata semantic_id must match semantic_id.") + if self.invocation_id is not None and ( + type(self.invocation_id) is not str or not self.invocation_id + ): + raise ValueError("invocation_id must be a non-empty string or None.") + if self.invocation_revision < 0: + raise ValueError("invocation_revision must be non-negative.") + if not isinstance(self.status, RunnerStatus): + raise TypeError("status must be a RunnerStatus.") + if self.status is RunnerStatus.RUNNING: + raise ValueError("A terminal call trace cannot have running status.") + for name in ("entered_mask", "completed_mask", "failed_mask"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if not ( + self.entered_mask.shape + == self.completed_mask.shape + == self.failed_mask.shape + ): + raise ValueError("Call trace masks must have equal shapes.") + if not ( + self.entered_mask.device + == self.completed_mask.device + == self.failed_mask.device + ): + raise ValueError("Call trace masks must share a device.") + if (self.completed_mask & ~self.entered_mask).any(): + raise ValueError("completed_mask must be a subset of entered_mask.") + if (self.failed_mask & ~self.entered_mask).any(): + raise ValueError("failed_mask must be a subset of entered_mask.") + if (self.completed_mask & self.failed_mask).any(): + raise ValueError("completed_mask and failed_mask must not overlap.") + if type(self.command_count) is not int or self.command_count < 0: + raise ValueError("command_count must be non-negative.") + if type(self.resolved_core_policy) is not ResolvedCorePolicyTrace: + raise TypeError( + "resolved_core_policy must be exactly ResolvedCorePolicyTrace." + ) + attempts = tuple(self.plan_attempts) + if not all(type(attempt) is SkillPlanAttemptTrace for attempt in attempts): + raise TypeError("plan_attempts must contain SkillPlanAttemptTrace values.") + if attempts: + generations = tuple(attempt.attempt_generation for attempt in attempts) + if generations != tuple( + range(generations[0], generations[0] + len(attempts)) + ): + raise ValueError( + "plan_attempts must use contiguous ordered generations." + ) + if attempts[-1].skill_id != self.skill_id: + raise ValueError("The active plan-attempt skill must match skill_id.") + elif self.status is not RunnerStatus.FAILED or self.command_count != 0: + raise ValueError( + "Only a preparation failure with no commands may omit plan_attempts." + ) + object.__setattr__(self, "entered_mask", self.entered_mask.clone()) + object.__setattr__(self, "completed_mask", self.completed_mask.clone()) + object.__setattr__(self, "failed_mask", self.failed_mask.clone()) + object.__setattr__(self, "call_metadata", normalized_call) + object.__setattr__( + self, + "resolved_core_policy", + self.resolved_core_policy.snapshot(), + ) + object.__setattr__( + self, + "plan_attempts", + tuple(attempt.snapshot() for attempt in attempts), + ) + object.__setattr__( + self, + "events", + tuple(_snapshot_event(event) for event in self.events), + ) + object.__setattr__( + self, + "effects", + tuple(effect.snapshot() for effect in self.effects), + ) + + def snapshot(self) -> SkillCallTrace: + """Return an independently owned call trace.""" + return SkillCallTrace( + call_index=self.call_index, + semantic_id=self.semantic_id, + call_metadata=self.call_metadata, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + status=self.status, + entered_mask=self.entered_mask, + completed_mask=self.completed_mask, + failed_mask=self.failed_mask, + command_count=self.command_count, + resolved_core_policy=self.resolved_core_policy, + plan_attempts=self.plan_attempts, + events=self.events, + effects=self.effects, + ) + + @property + def active_plan(self) -> SkillPlanAttemptTrace: + """Return the final installed plan generation as an owned trace.""" + if not self.plan_attempts: + raise RuntimeError("This call failed before an action plan was installed.") + return self.plan_attempts[-1].snapshot() + + def to_metadata(self) -> dict[str, object]: + """Return one semantic call, recovery history, and effects as JSON-safe data.""" + attempts = [attempt.to_metadata() for attempt in self.plan_attempts] + return { + "call_index": self.call_index, + "semantic_id": self.semantic_id, + "call": _metadata_value(self.call_metadata), + "skill_id": self.skill_id, + "invocation_id": self.invocation_id, + "invocation_revision": self.invocation_revision, + "status": self.status.value, + "masks": { + "entered": _metadata_value(self.entered_mask), + "completed": _metadata_value(self.completed_mask), + "failed": _metadata_value(self.failed_mask), + }, + "command_count": self.command_count, + "active_plan_attempt_generation": ( + None + if not self.plan_attempts + else self.plan_attempts[-1].attempt_generation + ), + "resolved_core_policy": self.resolved_core_policy.to_metadata(), + "plan_attempts": attempts, + "events": [_event_to_metadata(event) for event in self.events], + "effects": [effect.to_metadata() for effect in self.effects], + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillResult: + """Immutable workflow snapshot returned by sync and step-wise execution.""" + + status: SkillStatus + workflow_id: str | None + current_call_index: int | None + env_ids: torch.Tensor + success_mask: torch.Tensor + failure_mask: torch.Tensor + cancelled_mask: torch.Tensor + eligible_mask: torch.Tensor + task_state: TaskState + events: tuple[ExecutionEvent, ...] = () + calls: tuple[SkillCallTrace, ...] = () + effects: tuple[SkillEffectTrace, ...] = () + failures: tuple[SkillFailure, ...] = () + wait_duration: float = 0.0 + message: str | None = None + + def __post_init__(self) -> None: + if not isinstance(self.status, SkillStatus): + raise TypeError("status must be a SkillStatus.") + if self.workflow_id is not None and ( + type(self.workflow_id) is not str or not self.workflow_id + ): + raise ValueError("workflow_id must be a non-empty string or None.") + if self.current_call_index is not None and ( + type(self.current_call_index) is not int or self.current_call_index < 0 + ): + raise ValueError("current_call_index must be non-negative or None.") + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if self.env_ids.dtype != torch.long or self.env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if self.env_ids.numel() == 0: + raise ValueError("env_ids must contain at least one environment.") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + batch_size = int(self.env_ids.numel()) + for name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "eligible_mask", + ): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.shape != (batch_size,): + raise ValueError(f"{name} must be bool with shape ({batch_size},).") + if value.device != self.env_ids.device: + raise ValueError(f"{name} and env_ids must share a device.") + if (self.success_mask & (self.failure_mask | self.cancelled_mask)).any(): + raise ValueError("Successful rows cannot also fail or be cancelled.") + if (self.failure_mask & self.cancelled_mask).any(): + raise ValueError("Failed and cancelled masks must not overlap.") + if (self.eligible_mask & (self.failure_mask | self.cancelled_mask)).any(): + raise ValueError("Eligible rows cannot also fail or be cancelled.") + if (self.success_mask & ~self.eligible_mask).any(): + raise ValueError("success_mask must be a subset of eligible_mask.") + if not isinstance(self.task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if self.task_state.batch_size != batch_size: + raise ValueError("task_state batch size must match env_ids.") + if self.task_state.device != self.env_ids.device: + raise ValueError("task_state and env_ids must share a device.") + if not math.isfinite(self.wait_duration) or self.wait_duration < 0.0: + raise ValueError("wait_duration must be finite and non-negative.") + if self.message is not None and type(self.message) is not str: + raise TypeError("message must be a string or None.") + object.__setattr__(self, "env_ids", self.env_ids.clone()) + for name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "eligible_mask", + ): + object.__setattr__(self, name, getattr(self, name).clone()) + object.__setattr__(self, "task_state", _snapshot_task_state(self.task_state)) + object.__setattr__( + self, + "events", + tuple(_snapshot_event(event) for event in self.events), + ) + object.__setattr__( + self, + "calls", + tuple(call.snapshot() for call in self.calls), + ) + object.__setattr__( + self, + "effects", + tuple(effect.snapshot() for effect in self.effects), + ) + object.__setattr__( + self, + "failures", + tuple(failure.snapshot() for failure in self.failures), + ) + + @property + def terminal(self) -> bool: + """Whether the workflow no longer accepts execution steps.""" + return self.status in { + SkillStatus.COMPLETED, + SkillStatus.FAILED, + SkillStatus.CANCELLED, + } + + def to_metadata(self) -> dict[str, object]: + """Return a fresh deterministic JSON-safe workflow result. + + Recovery remains represented by the ordered :class:`ExecutionEvent` + stream and by each call's complete plan-attempt history. The returned + object owns only Python scalars, lists, and dictionaries and can be + serialized with ``json.dumps(..., allow_nan=False)``. + """ + return { + "schema_version": 1, + "kind": "skill_result", + "status": self.status.value, + "workflow_id": self.workflow_id, + "current_call_index": self.current_call_index, + "env_ids": _metadata_value(self.env_ids), + "masks": { + "success": _metadata_value(self.success_mask), + "failure": _metadata_value(self.failure_mask), + "cancelled": _metadata_value(self.cancelled_mask), + "eligible": _metadata_value(self.eligible_mask), + }, + "task_state": task_state_to_metadata(self.task_state), + "events": [_event_to_metadata(event) for event in self.events], + "calls": [call.to_metadata() for call in self.calls], + "effects": [effect.to_metadata() for effect in self.effects], + "failures": [failure.to_metadata() for failure in self.failures], + "wait_duration": self.wait_duration, + "message": self.message, + } + + +@runtime_checkable +class EffectEvidenceCollectorPort(Protocol): + """Minimal collector surface consumed by :class:`SkillRuntime`.""" + + def collect( + self, + spec: SemanticEffectSpec, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire synchronized raw evidence for one grounded effect.""" + + +@runtime_checkable +class SkillRuntimeProvider(Protocol): + """Explicit environment adapter installed for :meth:`AtomicSkills.from_env`.""" + + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + """Build a fully connected semantic runtime for this environment.""" + + +class _PrimedObservationProvider: + """Return a JIT-grounding observation once before delegating fresh reads.""" + + def __init__( + self, + context: PlanningContext, + delegate: ObservationProvider, + ) -> None: + self._context: PlanningContext | None = context + self._delegate = delegate + + def observe(self, task_state: TaskState) -> PlanningContext: + """Reuse the grounding snapshot for the session's first due cycle.""" + context = self._context + if context is None: + return self._delegate.observe(task_state) + self._context = None + return PlanningContext( + robot=context.robot, + task=task_state, + scene=context.scene, + env_ids=context.env_ids, + ) + + +class SkillRuntime: + """JIT-ground and execute semantic calls through one runner per call. + + Static workflow analysis occurs once in :meth:`start`. Each call then gets + a fresh observation, one grounded invocation, one execution session, and + one :class:`ExecutionRunner`. Verified task state and row eligibility cross + call barriers; execution sessions never do. + """ + + def __init__( + self, + compiler: SemanticSkillCompiler, + observation_provider: ObservationProvider, + command_sink: CommandSink, + evidence_collector: EffectEvidenceCollectorPort, + *, + task_state: TaskState | None = None, + clock: ExecutionClock | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> None: + if not isinstance(compiler, SemanticSkillCompiler): + raise TypeError("compiler must be a SemanticSkillCompiler.") + if not isinstance(observation_provider, ObservationProvider): + raise TypeError("observation_provider must implement ObservationProvider.") + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + if not isinstance(evidence_collector, EffectEvidenceCollectorPort): + raise TypeError( + "evidence_collector must implement EffectEvidenceCollectorPort." + ) + if clock is not None and not isinstance(clock, ExecutionClock): + raise TypeError("clock must implement ExecutionClock.") + if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") + integration = compiler.integration + engine = integration.engine + if not isinstance(engine, AtomicActionEngine): + raise TypeError( + "compiler.integration.engine must be an AtomicActionEngine." + ) + initial_task = ( + engine.initial_context().task if task_state is None else task_state + ) + if not isinstance(initial_task, TaskState): + raise TypeError("task_state must be a TaskState or None.") + if initial_task.device != engine.device: + raise ValueError("task_state and compiler engine must share a device.") + + self._compiler = compiler + self._engine = engine + self._observation_provider = observation_provider + self._command_sink = command_sink + self._evidence_collector = evidence_collector + self._clock = clock or MonotonicExecutionClock() + self._runner_cfg = runner_cfg or ExecutionRunnerCfg() + self._task_state = _snapshot_task_state(initial_task) + self._env_ids = torch.arange( + self._task_state.batch_size, + dtype=torch.long, + device=self._task_state.device, + ) + self._has_observed_env_ids = False + self._status = SkillStatus.IDLE + self._workflow: object | None = None + self._workflow_id: str | None = None + self._calls: tuple[SemanticCallSpec, ...] = () + self._execution_prefix_length = 0 + self._current_call_index: int | None = None + self._runner: ExecutionRunner | None = None + self._grounded: object | None = None + self._call_entered_mask = torch.zeros( + self._task_state.batch_size, + dtype=torch.bool, + device=self._task_state.device, + ) + self._eligible = torch.ones_like(self._call_entered_mask) + self._success = torch.zeros_like(self._eligible) + self._failed = torch.zeros_like(self._eligible) + self._cancelled = torch.zeros_like(self._eligible) + self._events: list[ExecutionEvent] = [] + self._call_traces: list[SkillCallTrace] = [] + self._effect_traces: list[SkillEffectTrace] = [] + self._failures: list[SkillFailure] = [] + self._call_event_offset = 0 + self._call_effect_offset = 0 + self._observation_revision = 0 + self._wait_duration = 0.0 + self._message: str | None = None + + @classmethod + def from_components( + cls, + compiler: SemanticSkillCompiler, + observation_provider: ObservationProvider, + command_sink: CommandSink, + evidence_collector: EffectEvidenceCollectorPort, + *, + task_state: TaskState | None = None, + clock: ExecutionClock | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> SkillRuntime: + """Construct the canonical runtime from explicit reusable ports.""" + return cls( + compiler, + observation_provider, + command_sink, + evidence_collector, + task_state=task_state, + clock=clock, + runner_cfg=runner_cfg, + ) + + @property + def compiler(self) -> SemanticSkillCompiler: + """Return the installed semantic compiler.""" + return self._compiler + + @property + def clock(self) -> ExecutionClock: + """Return the shared execution clock used by this runtime. + + Parallel coordinators use the same clock for every derived lane so a + branch cannot advance independently of the environment step grid. + """ + return self._clock + + @property + def scene_registry(self) -> SceneRegistry: + """Return the authoritative semantic scene registry.""" + return self._compiler.integration.scene_registry + + @property + def task_state(self) -> TaskState: + """Return an owned snapshot of persistent verified task state.""" + return _snapshot_task_state(self._task_state) + + def fork( + self, + command_sink: CommandSink, + *, + task_state: TaskState | None = None, + ) -> SkillRuntime: + """Create an independent execution lane from the same runtime ports. + + The derived runtime shares the immutable compiler integration, + observation/evidence providers, clock, and runner policy, but owns its + workflow, runner, masks, and verified task state. Its command sink is + supplied explicitly so a parallel coordinator can buffer commands + until all lanes have reached the same environment tick. + + Args: + command_sink: Lane-local command sink. + task_state: Optional verified barrier state. The current owned + task state is used when omitted. + + Returns: + A new idle semantic runtime for one independent lane. + """ + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + initial_state = self.task_state if task_state is None else task_state + if not isinstance(initial_state, TaskState): + raise TypeError("task_state must be a TaskState or None.") + return SkillRuntime( + self._compiler, + self._observation_provider, + command_sink, + self._evidence_collector, + task_state=initial_state, + clock=self._clock, + runner_cfg=self._runner_cfg, + ) + + @property + def status(self) -> SkillStatus: + """Return the current workflow status.""" + return self._status + + @property + def result(self) -> SkillResult: + """Return an immutable snapshot of the current workflow.""" + return SkillResult( + status=self._status, + workflow_id=self._workflow_id, + current_call_index=self._current_call_index, + env_ids=self._env_ids, + success_mask=self._success, + failure_mask=self._failed, + cancelled_mask=self._cancelled, + eligible_mask=self._eligible, + task_state=self._task_state, + events=tuple(self._events), + calls=tuple(self._call_traces), + effects=tuple(self._effect_traces), + failures=tuple(self._failures), + wait_duration=self._wait_duration, + message=self._message, + ) + + def start( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + """Analyze once and prepare the first call without blocking on motion. + + Args: + *calls: Complete ordered semantic analysis window. Calls after the + execution prefix participate in static look-ahead but are not + grounded or executed by this run. + workflow_id: Stable workflow identifier used in diagnostics. + eligible_mask: Optional row-local execution eligibility. + execution_prefix_length: Number of leading calls to execute. When + omitted, the complete analysis window is executed. + + Returns: + Immutable initial runtime result. + """ + if self._status is SkillStatus.RUNNING: + raise RuntimeError("A semantic workflow is already running.") + normalized = self._normalize_calls(calls) + if type(workflow_id) is not str or not workflow_id: + raise ValueError("workflow_id must be a non-empty string.") + prefix_length = self._normalize_execution_prefix_length( + execution_prefix_length, + call_count=len(normalized), + ) + workflow = self._compiler.analyze(normalized, workflow_id=workflow_id) + self._reset_workflow( + normalized, + workflow, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=prefix_length, + ) + try: + self._prepare_call(0) + except Exception as exc: # noqa: BLE001 - return one uniform result + self._fail_preparation(0, exc) + return self.result + + def step(self) -> SkillResult: + """Advance the current call by at most one due runner cycle.""" + if self._status is not SkillStatus.RUNNING: + return self.result + runner = self._require_runner() + grounded = self._require_grounded() + monitor = getattr(grounded, "effect_monitor", None) + verifier = self._effect_verifier if monitor is not None else None + runner_step = runner.step(effect_verifier=verifier) + self._consume_runner_step(runner_step) + if ( + runner_step.status is RunnerStatus.RUNNING + and runner_step.tick is not None + and runner_step.tick.pending_effect is not None + and monitor is None + ): + self._abort( + "The atomic plan requested effect verification, but the grounded " + "semantic call did not install an effect monitor." + ) + return self.result + if runner_step.status is RunnerStatus.RUNNING: + return self.result + self._finish_current_call(runner_step) + if runner_step.status is RunnerStatus.COMPLETED: + if self._eligible.any() and self._has_next_call: + assert self._current_call_index is not None + next_index = self._current_call_index + 1 + try: + self._prepare_call(next_index) + except Exception as exc: # noqa: BLE001 - preserve workflow trace + self._fail_preparation(next_index, exc) + elif self._eligible.any(): + self._success = self._eligible.clone() + self._status = SkillStatus.COMPLETED + self._current_call_index = None + self._wait_duration = 0.0 + else: + self._status = ( + SkillStatus.CANCELLED + if self._cancelled.any() and not self._failed.any() + else SkillStatus.FAILED + ) + self._current_call_index = None + self._wait_duration = 0.0 + elif runner_step.status is RunnerStatus.CANCELLED: + self._status = SkillStatus.CANCELLED + self._current_call_index = None + self._wait_duration = 0.0 + else: + self._status = SkillStatus.FAILED + self._current_call_index = None + self._wait_duration = 0.0 + return self.result + + def run( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + max_steps: int = 100_000, + ) -> SkillResult: + """Synchronously execute an analyzed semantic-call prefix.""" + if type(max_steps) is not int or max_steps <= 0: + raise ValueError("max_steps must be a positive integer.") + result = self.start( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + ) + for _ in range(max_steps): + if result.terminal: + return result + if result.wait_duration > 0.0: + self._clock.sleep(result.wait_duration) + result = self.step() + self._abort(f"Semantic runtime exceeded max_steps={max_steps}.") + return self.result + + def cancel( + self, reason: str = "Semantic workflow cancelled by caller." + ) -> SkillResult: + """Cancel the active runner and inherit its cancel-then-hold behavior.""" + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + if self._status is not SkillStatus.RUNNING: + return self.result + runner_step = self._require_runner().cancel(reason) + self._consume_runner_step(runner_step) + active = self._eligible & ~self._failed + self._message = runner_step.message or reason + self._finish_current_call(runner_step) + self._cancelled |= active + self._eligible &= ~active + self._status = ( + SkillStatus.CANCELLED + if runner_step.status is RunnerStatus.CANCELLED + else SkillStatus.FAILED + ) + if self._status is SkillStatus.FAILED: + self._failed |= active + self._cancelled &= ~active + self._current_call_index = None + self._wait_duration = 0.0 + return self.result + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + """Cancel selected rows while the remaining shared call keeps running. + + This is the row-local cancellation boundary used by a parallel + fail-fast coordinator. The active runner remains the sole owner of + controller neutralization and effect-request correlation. + + Args: + env_mask: Rows to remove permanently from this workflow. + reason: Human-readable cancellation reason. + + Returns: + Updated immutable workflow result. + """ + if self._status is not SkillStatus.RUNNING: + return self.result + if not isinstance(env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") + if ( + env_mask.dtype != torch.bool + or env_mask.shape != self._eligible.shape + or env_mask.device != self._eligible.device + ): + raise ValueError( + "env_mask must be bool and match the runtime batch/device." + ) + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + changed = self._require_runner().deactivate_rows( + env_mask & self._eligible, + reason=reason, + ) + self._cancelled |= changed + self._eligible &= ~changed + if not self._eligible.any(): + runner_step = self._require_runner().cancel(reason) + self._consume_runner_step(runner_step) + self._finish_current_call(runner_step) + self._status = ( + SkillStatus.CANCELLED + if runner_step.status is RunnerStatus.CANCELLED + else SkillStatus.FAILED + ) + if self._status is SkillStatus.FAILED: + failed = self._call_entered_mask & ~self._cancelled + self._failed |= failed + self._current_call_index = None + self._wait_duration = 0.0 + return self.result + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + """Install a verified state snapshot between independent workflows. + + Parallel coordinators use this explicit barrier operation after + deterministically merging branch-local effects. Running workflows + cannot replace their runner-owned state. + """ + if self._status is SkillStatus.RUNNING: + raise RuntimeError("Cannot replace task state while a workflow is running.") + if not isinstance(task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if ( + task_state.batch_size != self._task_state.batch_size + or task_state.device != self._task_state.device + ): + raise ValueError("task_state must match the runtime batch and device.") + self._task_state = _snapshot_task_state(task_state) + return self.result + + @property + def _has_next_call(self) -> bool: + assert self._current_call_index is not None + return self._current_call_index + 1 < self._execution_prefix_length + + @staticmethod + def _normalize_execution_prefix_length( + value: int | None, + *, + call_count: int, + ) -> int: + """Normalize a non-empty execution prefix inside one analysis window.""" + if value is None: + return call_count + if type(value) is not int: + raise TypeError("execution_prefix_length must be an integer or None.") + if not 1 <= value <= call_count: + raise ValueError( + "execution_prefix_length must be in " f"[1, {call_count}], got {value}." + ) + return value + + def _normalize_calls( + self, + supplied: tuple[SemanticCallSpec | Iterable[SemanticCallSpec], ...], + ) -> tuple[SemanticCallSpec, ...]: + """Normalize varargs and one explicit iterable to the same compiler path.""" + if len(supplied) == 1 and not isinstance(supplied[0], SemanticCallSpec): + candidate = supplied[0] + if isinstance(candidate, (str, bytes)): + raise TypeError("calls must contain SemanticCallSpec values.") + try: + calls = tuple(candidate) + except TypeError as exc: + raise TypeError( + "A single run argument must be a SemanticCallSpec or iterable." + ) from exc + else: + calls = tuple(supplied) + if not calls: + raise ValueError("A semantic workflow requires at least one call.") + if not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError("calls must contain SemanticCallSpec values.") + return calls + + def _reset_workflow( + self, + calls: tuple[SemanticCallSpec, ...], + workflow: object, + *, + workflow_id: str, + eligible_mask: torch.Tensor | None, + execution_prefix_length: int, + ) -> None: + """Reset per-run state while retaining verified symbolic state.""" + if eligible_mask is None: + eligible = torch.ones( + self._task_state.batch_size, + dtype=torch.bool, + device=self._task_state.device, + ) + else: + if not isinstance(eligible_mask, torch.Tensor): + raise TypeError("eligible_mask must be a torch.Tensor or None.") + if eligible_mask.dtype != torch.bool or eligible_mask.shape != ( + self._task_state.batch_size, + ): + raise ValueError( + "eligible_mask must be bool with shape " + f"({self._task_state.batch_size},)." + ) + eligible = eligible_mask.to(self._task_state.device).clone() + if not eligible.any(): + raise ValueError("eligible_mask must contain at least one active row.") + self._workflow = workflow + self._workflow_id = workflow_id + self._calls = calls + self._execution_prefix_length = execution_prefix_length + self._current_call_index = 0 + self._runner = None + self._grounded = None + self._eligible = eligible + self._success = torch.zeros_like(eligible) + self._failed = torch.zeros_like(eligible) + self._cancelled = torch.zeros_like(eligible) + self._events = [] + self._call_traces = [] + self._effect_traces = [] + self._failures = [] + self._call_event_offset = 0 + self._call_effect_offset = 0 + self._observation_revision = 0 + self._wait_duration = 0.0 + self._message = None + self._status = SkillStatus.RUNNING + + def _observe_for_grounding(self) -> PlanningContext: + """Capture and normalize one fresh context for JIT lowering.""" + context = self._observation_provider.observe(self._task_state) + if not isinstance(context, PlanningContext): + raise TypeError( + "ObservationProvider.observe() must return PlanningContext." + ) + normalized = PlanningContext( + robot=context.robot, + task=self._task_state, + scene=context.scene, + env_ids=context.env_ids, + ) + if normalized.batch_size != self._task_state.batch_size: + raise ValueError( + "Observation batch size changed during semantic execution." + ) + if normalized.robot.qpos.device != self._task_state.device: + raise ValueError("Observation and verified TaskState must share a device.") + if self._has_observed_env_ids: + if normalized.env_ids.device != self._env_ids.device or not torch.equal( + normalized.env_ids, + self._env_ids, + ): + raise ValueError( + "Observation env_ids must remain stable across call barriers." + ) + else: + self._env_ids = normalized.env_ids.clone() + self._has_observed_env_ids = True + return normalized + + def _prepare_call(self, call_index: int) -> None: + """Freshly ground and create exactly one session and runner.""" + assert self._workflow is not None + context = self._observe_for_grounding() + grounded = self._compiler.ground( + self._workflow, + call_index, + context, + eligible_mask=self._eligible, + ) + invocation = getattr(grounded, "invocation", None) + grounded_eligible = getattr(grounded, "eligible_mask", None) + effect_spec = getattr(grounded, "effect_spec", None) + effect_monitor = getattr(grounded, "effect_monitor", None) + if invocation is None: + raise TypeError("Semantic compiler ground() must return an invocation.") + if not isinstance(grounded_eligible, torch.Tensor) or not torch.equal( + grounded_eligible, + self._eligible, + ): + raise ValueError("Grounded call must preserve runtime eligibility.") + if (effect_spec is None) != (effect_monitor is None): + raise ValueError( + "Grounded effect_spec and effect_monitor must be set together." + ) + if effect_spec is not None: + if not isinstance(effect_spec, SemanticEffectSpec): + raise TypeError("Grounded effect_spec must be a SemanticEffectSpec.") + if not isinstance(effect_monitor, EffectMonitor): + raise TypeError("Grounded effect_monitor must be an EffectMonitor.") + if effect_spec.env_ids.device != context.env_ids.device or not torch.equal( + effect_spec.env_ids, + context.env_ids, + ): + raise ValueError("Grounded effect env_ids must match the call context.") + + self._grounded = grounded + session = self._engine.start( + (invocation,), + context, + eligible_mask=self._eligible, + ) + primed = _PrimedObservationProvider(context, self._observation_provider) + runner = ExecutionRunner( + session, + primed, + self._command_sink, + clock=self._clock, + cfg=self._runner_cfg, + ) + self._current_call_index = call_index + self._runner = runner + self._call_entered_mask = self._eligible.clone() + self._call_event_offset = len(self._events) + self._call_effect_offset = len(self._effect_traces) + self._wait_duration = 0.0 + + def _effect_verifier( + self, + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + """Collect raw evidence and feed the grounded call's monitor.""" + grounded = self._require_grounded() + spec = getattr(grounded, "effect_spec", None) + monitor = getattr(grounded, "effect_monitor", None) + if not isinstance(spec, SemanticEffectSpec) or not isinstance( + monitor, + EffectMonitor, + ): + raise RuntimeError( + "The active atomic plan requested effect verification, but its " + "semantic call has no grounded effect monitor." + ) + if request.skill_id != spec.skill_id: + raise ValueError("Effect request skill_id does not match the effect spec.") + if request.invocation_id != spec.invocation_id: + raise ValueError( + "Effect request invocation_id does not match the effect spec." + ) + if request.invocation_revision != spec.invocation_revision: + raise ValueError("Effect request revision does not match the effect spec.") + observation_revision = self._observation_revision + self._observation_revision += 1 + selected_env_ids = spec.env_ids[request.env_mask.to(spec.env_ids.device)] + evidence = self._evidence_collector.collect( + spec, + timestamp=context.robot.timestamp, + observation_revision=observation_revision, + env_ids=selected_env_ids, + ) + decision = monitor.observe(request, evidence) + analyzed = getattr(grounded, "analyzed", None) + monitor_ref = getattr(analyzed, "effect_monitor_ref", None) + if monitor_ref is not None and not isinstance(monitor_ref, EffectMonitorRef): + raise TypeError("Grounded effect monitor reference must be typed.") + if monitor_ref is None: + monitor_id = f"{type(monitor).__module__}.{type(monitor).__qualname__}" + monitor_revision = None + configured_monitor_params: Mapping[str, object] = {} + else: + monitor_id = monitor_ref.monitor_id + monitor_revision = monitor_ref.revision + configured_monitor_params = monitor_ref.params + resolved_monitor_params = monitor.resolved_params + if not isinstance(resolved_monitor_params, Mapping): + raise TypeError("EffectMonitor.resolved_params must return a mapping.") + trace = SkillEffectTrace( + call_index=self._require_call_index(), + verification_id=request.verification_id, + observation_revision=observation_revision, + timestamp=context.robot.timestamp, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + effect_spec=spec, + monitor_id=monitor_id, + monitor_revision=monitor_revision, + configured_monitor_params=configured_monitor_params, + resolved_monitor_params=resolved_monitor_params, + evidence=evidence, + ) + self._effect_traces.append(trace) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + ) + + def _consume_runner_step(self, runner_step: RunnerStep) -> None: + """Merge one runner update into workflow-level traces.""" + self._wait_duration = runner_step.wait_duration + if runner_step.tick is not None: + self._task_state = _snapshot_task_state(runner_step.tick.task_state) + self._events.extend( + _snapshot_event(event) for event in runner_step.tick.events + ) + if runner_step.message: + self._message = runner_step.message + + def _finish_current_call(self, runner_step: RunnerStep) -> None: + """Commit terminal row masks and append exactly one call trace.""" + runner = self._require_runner() + grounded = self._require_grounded() + call_index = self._require_call_index() + self._task_state = _snapshot_task_state(runner.session.task_state) + after = runner.session.eligible_mask + invocation = getattr(grounded, "invocation") + if runner_step.status is RunnerStatus.COMPLETED: + completed = self._call_entered_mask & after + failed = self._call_entered_mask & ~after & ~self._cancelled + elif runner_step.status is RunnerStatus.CANCELLED: + completed = torch.zeros_like(self._call_entered_mask) + failed = torch.zeros_like(self._call_entered_mask) + else: + completed = torch.zeros_like(self._call_entered_mask) + failed = self._call_entered_mask & ~self._cancelled + after = self._eligible & ~failed + + self._eligible = after.clone() + self._failed |= failed + if failed.any(): + message = runner_step.message or "Semantic call failed for these rows." + self._failures.append( + SkillFailure( + call_index=call_index, + semantic_id=self._calls[call_index].semantic_id, + env_mask=failed, + message=message, + ) + ) + plan_attempts = tuple( + SkillPlanAttemptTrace.from_execution_attempt( + attempt, + profile_id=grounded.analyzed.bound.robot_profile.profile_id, + preset_id=grounded.analyzed.bound.preset.preset_id, + preset_schema_version=grounded.analyzed.bound.preset.schema_version, + ) + for attempt in runner.session.plan_attempts + ) + self._call_traces.append( + SkillCallTrace( + call_index=call_index, + semantic_id=self._calls[call_index].semantic_id, + call_metadata=self._calls[call_index].to_metadata(), + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + status=runner_step.status, + entered_mask=self._call_entered_mask, + completed_mask=completed, + failed_mask=failed, + command_count=runner_step.command_count, + resolved_core_policy=plan_attempts[-1].resolved_core_policy, + plan_attempts=plan_attempts, + events=tuple(self._events[self._call_event_offset :]), + effects=tuple(self._effect_traces[self._call_effect_offset :]), + ) + ) + self._runner = None + self._grounded = None + + def _fail_preparation(self, call_index: int, exc: Exception) -> None: + """Convert a post-barrier grounding failure to a terminal result.""" + failed = self._eligible.clone() + self._failed |= failed + self._eligible &= ~failed + semantic_id = self._calls[call_index].semantic_id + message = ( + f"Could not prepare semantic call {call_index} ({semantic_id!r}): " + f"{type(exc).__name__}: {exc}" + ) + self._failures.append(SkillFailure(call_index, semantic_id, failed, message)) + self._append_preparation_failure_trace(call_index, failed) + self._message = message + self._status = SkillStatus.FAILED + self._current_call_index = None + self._runner = None + self._grounded = None + self._wait_duration = 0.0 + + def _append_preparation_failure_trace( + self, + call_index: int, + failed_mask: torch.Tensor, + ) -> None: + """Record statically resolved policy choices when planning never starts.""" + grounded = self._grounded + analyzed = getattr(grounded, "analyzed", None) + invocation = getattr(grounded, "invocation", None) + if analyzed is None: + workflow_calls = getattr(self._workflow, "calls", ()) + if call_index < len(workflow_calls): + analyzed = workflow_calls[call_index] + bound = getattr(analyzed, "bound", None) + if bound is None: + return + try: + profile = bound.robot_profile + preset = bound.preset + action_binding = ( + bound.binding.action_binding + if invocation is None + else invocation.binding + ) + resolved = ResolvedCorePolicyTrace.from_resolved_binding( + profile_id=profile.profile_id, + preset_id=preset.preset_id, + preset_schema_version=preset.schema_version, + motion_policy=( + preset.motion_policy + if invocation is None + else invocation.motion_policy + ), + recovery_policy=( + preset.recovery_policy + if invocation is None + else invocation.recovery_policy + ), + endpoints=action_binding.endpoints, + ) + skill_id = bound.linked.descriptor.skill_id + except (AttributeError, TypeError, ValueError): + return + self._call_traces.append( + SkillCallTrace( + call_index=call_index, + semantic_id=self._calls[call_index].semantic_id, + call_metadata=self._calls[call_index].to_metadata(), + skill_id=skill_id, + invocation_id=( + None if invocation is None else invocation.invocation_id + ), + invocation_revision=(0 if invocation is None else invocation.revision), + status=RunnerStatus.FAILED, + entered_mask=failed_mask, + completed_mask=torch.zeros_like(failed_mask), + failed_mask=failed_mask, + command_count=0, + resolved_core_policy=resolved, + plan_attempts=(), + ) + ) + + def _abort(self, reason: str) -> None: + """Safe-stop the active runner and mark remaining rows failed.""" + if self._runner is not None: + safe_stop_step = self._runner.cancel(reason) + runner_step = replace( + safe_stop_step, + status=RunnerStatus.FAILED, + message=reason, + ) + self._consume_runner_step(runner_step) + self._finish_current_call(runner_step) + failed = self._eligible.clone() + self._failed |= failed + self._eligible &= ~failed + if failed.any() and self._calls: + call_index = min( + self._current_call_index or 0, + len(self._calls) - 1, + ) + self._failures.append( + SkillFailure( + call_index, + self._calls[call_index].semantic_id, + failed, + reason, + ) + ) + self._message = reason + self._status = SkillStatus.FAILED + self._current_call_index = None + self._wait_duration = 0.0 + + def _require_runner(self) -> ExecutionRunner: + if self._runner is None: + raise RuntimeError("No semantic call runner is active.") + return self._runner + + def _require_grounded(self) -> object: + if self._grounded is None: + raise RuntimeError("No grounded semantic call is active.") + return self._grounded + + def _require_call_index(self) -> int: + if self._current_call_index is None: + raise RuntimeError("No semantic call is active.") + return self._current_call_index + + +class SkillScene: + """Typed convenience lookup surface backed by one immutable registry.""" + + def __init__(self, registry: SceneRegistry) -> None: + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + self._registry = registry + + @property + def registry(self) -> SceneRegistry: + """Return the authoritative scene registry.""" + return self._registry + + def entity(self, identifier: str | SceneEntityRef) -> SceneEntityRef: + """Resolve any registered semantic entity.""" + return self._registry.resolve(identifier) + + def object(self, identifier: str | SceneObjectRef) -> SceneObjectRef: + """Resolve a registered semantic object.""" + return self._registry.resolve(identifier, expected_type=SceneObjectRef) + + def articulation( + self, + identifier: str | SceneArticulationRef, + ) -> SceneArticulationRef: + """Resolve a registered articulation.""" + return self._registry.resolve(identifier, expected_type=SceneArticulationRef) + + def link(self, identifier: str | SceneLinkRef) -> SceneLinkRef: + """Resolve a registered articulation link.""" + return self._registry.resolve(identifier, expected_type=SceneLinkRef) + + def affordance( + self, + identifier: str | SceneAffordanceRef, + ) -> SceneAffordanceRef: + """Resolve a registered semantic affordance.""" + return self._registry.resolve(identifier, expected_type=SceneAffordanceRef) + + +class AtomicSkills: + """Small application-facing facade over :class:`SkillRuntime`.""" + + def __init__(self, runtime: SkillRuntime) -> None: + if not isinstance(runtime, SkillRuntime): + raise TypeError("runtime must be a SkillRuntime.") + self._runtime = runtime + self._scene = SkillScene(runtime.scene_registry) + + @classmethod + def from_components( + cls, + compiler: SemanticSkillCompiler, + observation_provider: ObservationProvider, + command_sink: CommandSink, + evidence_collector: EffectEvidenceCollectorPort, + *, + task_state: TaskState | None = None, + clock: ExecutionClock | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> AtomicSkills: + """Build a facade from explicit compiler and runtime ports.""" + return cls( + SkillRuntime.from_components( + compiler, + observation_provider, + command_sink, + evidence_collector, + task_state=task_state, + clock=clock, + runner_cfg=runner_cfg, + ) + ) + + @classmethod + def from_env(cls, env: object, *, preset: str = "safe") -> AtomicSkills: + """Build through an explicitly installed environment integration adapter. + + The method deliberately does not inspect generic environment attributes + for robots, scenes, controllers, or managers. An environment integration + must implement :class:`SkillRuntimeProvider` and own those decisions. + """ + if type(preset) is not str or not preset: + raise ValueError("preset must be a non-empty string.") + if not isinstance(env, SkillRuntimeProvider): + raise TypeError( + "Environment has no semantic-skill integration adapter. Install " + "SkillRuntimeProvider.create_skill_runtime(*, preset=...) or use " + "AtomicSkills.from_components(...) with explicit ports." + ) + runtime = env.create_skill_runtime(preset=preset) + if not isinstance(runtime, SkillRuntime): + raise TypeError( + "SkillRuntimeProvider.create_skill_runtime() must return " + "SkillRuntime." + ) + return cls(runtime) + + @property + def runtime(self) -> SkillRuntime: + """Return the canonical runtime for advanced step-wise use.""" + return self._runtime + + @property + def scene(self) -> SkillScene: + """Return typed semantic scene lookup helpers.""" + return self._scene + + @property + def result(self) -> SkillResult: + """Return the current immutable runtime result.""" + return self._runtime.result + + def start( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + """Start non-blocking semantic execution without exposing sessions.""" + return self._runtime.start( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + ) + + def step(self) -> SkillResult: + """Advance non-blocking execution by one due runner cycle.""" + return self._runtime.step() + + def run( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + max_steps: int = 100_000, + ) -> SkillResult: + """Synchronously execute calls without exposing core runtime objects.""" + return self._runtime.run( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + max_steps=max_steps, + ) + + def cancel( + self, reason: str = "Semantic workflow cancelled by caller." + ) -> SkillResult: + """Cancel and safe-stop the active semantic workflow.""" + return self._runtime.cancel(reason) + + +__all__ = [ + "AtomicSkills", + "EffectEvidenceCollectorPort", + "ResolvedCorePolicyTrace", + "SkillCallTrace", + "SkillEndpointBindingTrace", + "SkillEffectTrace", + "SkillFailure", + "SkillPlanAttemptTrace", + "SkillResult", + "SkillRuntime", + "SkillRuntimeProvider", + "SkillScene", + "SkillStatus", + "task_state_to_metadata", +] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py index 7d613da06..cac2f0181 100644 --- a/embodichain/lab/sim/skills/scene.py +++ b/embodichain/lab/sim/skills/scene.py @@ -18,7 +18,7 @@ from __future__ import annotations -from collections.abc import Iterable, Iterator, Mapping +from collections.abc import Hashable, Iterable, Iterator, Mapping from copy import deepcopy from dataclasses import dataclass, field, fields, is_dataclass, replace from enum import Enum @@ -32,11 +32,14 @@ from embodichain.lab.sim.atomic_actions import ( Affordance, AntipodalAffordance, + ArticulationOperationAffordance, EntityState, ObjectSemantics, + ObservedArticulationJointState, SceneProvider, SceneSnapshot, ) +from .effects import EffectEvidenceAddress if TYPE_CHECKING: from embodichain.lab.sim.planners import MotionGenerator @@ -48,12 +51,38 @@ GRASP_AFFORDANCE_CAPABILITY = "affordance.grasp" """Capability for an affordance usable by object pickup or handover.""" +ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY = "affordance.articulation.operation" +"""Capability for a typed handle-driven articulation operation.""" + PLACE_ON_AFFORDANCE_CAPABILITY = "affordance.place.on" """Capability for an affordance that defines an ``on`` placement relation.""" PLACE_IN_AFFORDANCE_CAPABILITY = "affordance.place.in" """Capability for an affordance that defines an ``inside`` placement relation.""" +SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID = "builtin.scene_articulation" +"""Stable route for explicitly injected articulation-joint observations.""" + +SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION = "1" +"""Exact contract revision for articulation-joint evidence addresses.""" + + +@dataclass(frozen=True, slots=True) +class ArticulationJointEvidenceAddress(EffectEvidenceAddress): + """Canonical scene articulation and joint observation address.""" + + articulation_id: str + joint_id: str + + def __post_init__(self) -> None: + _validate_identifier(self.articulation_id, "articulation_id") + _validate_identifier(self.joint_id, "joint_id") + + @property + def address_fingerprint(self) -> Hashable: + """Return the exact provider-independent joint address.""" + return type(self), self.articulation_id, self.joint_id + class UnsupportedSceneAffordanceError(ValueError): """Raised when a parent has no affordance for a required capability.""" @@ -322,6 +351,18 @@ def _validate_topology(self) -> None: f"{GRASP_AFFORDANCE_CAPABILITY!r} requires an " "AntipodalAffordance payload." ) + if ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + in self.affordance_capabilities + and not issubclass( + self.affordance_payload_type, + ArticulationOperationAffordance, + ) + ): + raise TypeError( + f"{ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY!r} requires " + "an ArticulationOperationAffordance payload." + ) return if self.parent is not None or self.native_name is not None: raise ValueError("Generic scene metadata cannot declare a parent.") @@ -390,6 +431,19 @@ def observe( """ +@runtime_checkable +class SceneArticulationJointStateProvider(Protocol): + """Observe canonical joints for one registered scene articulation.""" + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> Mapping[str, ObservedArticulationJointState]: + """Return live joint observations whose rows follow ``env_ids``.""" + + @runtime_checkable class SceneGeometryProvider(Protocol): """Provide one entity's planner-facing collision geometry descriptor.""" @@ -413,6 +467,7 @@ class SceneEntityRegistration: Args: ref: Canonical typed reference. state_provider: Optional dynamic pose/confidence source. + joint_state_provider: Optional live articulation-joint source. aliases: External names normalized at the registry boundary. parent: Canonical parent for a link or affordance. native_name: Backend-local member name under ``parent``. @@ -472,6 +527,9 @@ class SceneEntityRegistration: relative_pose: torch.Tensor | None = None """Optional parent-relative pose when no explicit state provider exists.""" + joint_state_provider: SceneArticulationJointStateProvider | None = None + """Explicit live joint source for an articulation registration.""" + def __post_init__(self) -> None: if type(self.ref) not in { SceneEntityRef, @@ -486,6 +544,14 @@ def __post_init__(self) -> None: SceneEntityStateProvider, ): raise TypeError("state_provider must implement SceneEntityStateProvider.") + if self.joint_state_provider is not None and not isinstance( + self.joint_state_provider, + SceneArticulationJointStateProvider, + ): + raise TypeError( + "joint_state_provider must implement " + "SceneArticulationJointStateProvider." + ) if isinstance(self.aliases, (str, bytes)): raise TypeError("aliases must be an iterable of identifiers, not a string.") @@ -584,9 +650,22 @@ def _validate_reference_contract(self) -> None: "affordance_capabilities require a SceneAffordanceRef " "registration." ) + if ( + isinstance(self.ref, SceneObjectRef) + and self.joint_state_provider is not None + ): + raise ValueError( + "joint_state_provider requires a SceneArticulationRef " + "registration." + ) return if isinstance(self.ref, SceneLinkRef): + if self.joint_state_provider is not None: + raise ValueError( + "joint_state_provider requires a SceneArticulationRef " + "registration." + ) if ( not isinstance(self.parent, SceneArticulationRef) or self.native_name is None @@ -608,6 +687,11 @@ def _validate_reference_contract(self) -> None: return if isinstance(self.ref, SceneAffordanceRef): + if self.joint_state_provider is not None: + raise ValueError( + "joint_state_provider requires a SceneArticulationRef " + "registration." + ) if ( not isinstance( self.parent, @@ -632,6 +716,10 @@ def _validate_reference_contract(self) -> None: if self.parent is not None or self.native_name is not None: raise ValueError("Generic entity registrations cannot declare a parent.") + if self.joint_state_provider is not None: + raise ValueError( + "joint_state_provider requires a SceneArticulationRef registration." + ) if self.state_provider is None: raise ValueError("Generic entity registrations require state_provider.") if self.affordance_capabilities: @@ -1490,7 +1578,7 @@ def from_simulation( SceneEntityRegistration( ref=SceneObjectRef(registry_id), state_provider=_SimulationEntityStateProvider(entity), - aliases=(uid,), + aliases=(() if uid == registry_id else (uid,)), geometry_provider=geometry.get( registry_id, _SimulationEntityGeometryProvider(entity), @@ -1512,7 +1600,10 @@ def from_simulation( SceneEntityRegistration( ref=SceneArticulationRef(registry_id), state_provider=_SimulationEntityStateProvider(entity), - aliases=(uid,), + joint_state_provider=( + _SimulationArticulationJointStateProvider(entity) + ), + aliases=(() if uid == registry_id else (uid,)), geometry_provider=geometry.get(registry_id), collision_role=roles.get( registry_id, @@ -1580,6 +1671,51 @@ def observe( return EntityState(pose) +@dataclass(frozen=True, slots=True) +class _SimulationArticulationJointStateProvider: + """Read named measured qpos from one selected simulation articulation.""" + + entity: Any + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> Mapping[str, ObservedArticulationJointState]: + del timestamp + qpos = self.entity.get_qpos(target=False) + if not isinstance(qpos, torch.Tensor): + raise TypeError("Simulation articulation get_qpos() must return a tensor.") + if qpos.dim() != 2 or qpos.shape[0] == 0 or qpos.shape[1] == 0: + raise ValueError( + "Simulation articulation qpos must have non-empty shape (N, J)." + ) + joint_names = tuple(self.entity.joint_names) + if len(joint_names) != qpos.shape[1]: + raise ValueError( + "Simulation articulation joint_names must match qpos width." + ) + for joint_name in joint_names: + _validate_identifier(joint_name, "simulation articulation joint name") + if len(set(joint_names)) != len(joint_names): + raise ValueError("Simulation articulation joint_names must be unique.") + indices = env_ids.to(device=qpos.device) + if bool((indices < 0).any()) or int(indices.max().item()) >= qpos.shape[0]: + raise ValueError( + "Simulation scene env_ids must address valid articulation rows." + ) + selected = qpos.index_select(0, indices) + return MappingProxyType( + { + joint_name: ObservedArticulationJointState( + selected[:, index : index + 1] + ) + for index, joint_name in enumerate(joint_names) + } + ) + + @dataclass(frozen=True, slots=True) class _SimulationEntityGeometryProvider: """Expose a selected live rigid object as planner geometry input.""" @@ -1636,6 +1772,8 @@ def __init__( self._env_ids: torch.Tensor | None = None self._published_poses: dict[str, torch.Tensor] = {} self._published_confidences: dict[str, float] = {} + self._published_joint_positions: dict[tuple[str, str], torch.Tensor] = {} + self._published_joint_validity: dict[tuple[str, str], torch.Tensor] = {} self._scene_version = 0 self._collision_revisions: list[int] = [] self._effective_collision_world_mode = ( @@ -1711,6 +1849,10 @@ def snapshot( timestamp=float(timestamp), env_ids=env_ids, ) + articulation_joints = self._observe_articulation_joints( + timestamp=float(timestamp), + env_ids=env_ids, + ) poses = {entity_id: state.pose for entity_id, state in states.items()} confidences = { entity_id: state.confidence for entity_id, state in states.items() @@ -1727,8 +1869,11 @@ def snapshot( confidences[entity_id] != self._published_confidences[entity_id] for entity_id in confidences ) - if confidence_changed or any( - changed.any().item() for changed in changed_by_entity.values() + joint_changed = self._joint_observations_changed(articulation_joints) + if ( + confidence_changed + or joint_changed + or any(changed.any().item() for changed in changed_by_entity.values()) ): self._scene_version += 1 collision_changed = torch.zeros(batch_size, dtype=torch.bool) @@ -1754,6 +1899,7 @@ def snapshot( entity_id: pose.clone() for entity_id, pose in poses.items() } self._published_confidences = confidences.copy() + self._store_joint_baseline(articulation_joints) self._last_timestamp = float(timestamp) return SceneSnapshot( @@ -1762,8 +1908,112 @@ def snapshot( entities=states, collision_world_revision=tuple(self._collision_revisions), collision_entity_ids=self.collision_entity_ids, + articulation_joints=articulation_joints, ) + def _observe_articulation_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[tuple[str, str], ObservedArticulationJointState]: + """Observe every explicitly registered articulation-joint provider.""" + batch_size = int(env_ids.numel()) + observed: dict[tuple[str, str], ObservedArticulationJointState] = {} + for registration in self.registry._registrations: + provider = registration.joint_state_provider + if provider is None: + continue + assert isinstance(registration.ref, SceneArticulationRef) + supplied = provider.observe_joints( + timestamp=timestamp, + env_ids=env_ids.clone(), + ) + if not isinstance(supplied, Mapping): + raise TypeError( + f"Joint provider for {registration.ref.entity_id!r} must " + "return a mapping." + ) + for joint_id, state in supplied.items(): + _validate_identifier(joint_id, "joint provider joint_id") + if not isinstance(state, ObservedArticulationJointState): + raise TypeError( + f"Joint provider for {registration.ref.entity_id!r} must " + "return ObservedArticulationJointState values." + ) + key = registration.ref.entity_id, joint_id + observed[key] = self._normalize_joint_observation( + state, + batch_size=batch_size, + address=key, + ) + return observed + + @staticmethod + def _normalize_joint_observation( + state: ObservedArticulationJointState, + *, + batch_size: int, + address: tuple[str, str], + ) -> ObservedArticulationJointState: + """Broadcast one live joint observation to the scene batch.""" + position = state.position + if position.dim() == 1: + position = position.unsqueeze(0).expand(batch_size, -1).clone() + elif position.shape[0] != batch_size: + raise ValueError( + f"Articulation joint {address!r} observation must have {batch_size} " + "rows." + ) + valid = state.valid_mask + if valid is None: + valid = torch.ones( + batch_size, + dtype=torch.bool, + device=position.device, + ) + return ObservedArticulationJointState(position, valid) + + def _joint_observations_changed( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> bool: + """Update live joint baselines and report any material value change.""" + changed = set(states) != set(self._published_joint_positions) + if not changed: + for key, state in states.items(): + previous_position = self._published_joint_positions[key] + previous_validity = self._published_joint_validity[key] + current_position = state.position.to( + device=previous_position.device, + dtype=previous_position.dtype, + ) + assert state.valid_mask is not None + current_validity = state.valid_mask.to(previous_validity.device) + if not torch.equal( + current_position, previous_position + ) or not torch.equal( + current_validity, + previous_validity, + ): + changed = True + break + self._store_joint_baseline(states) + return changed + + def _store_joint_baseline( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> None: + """Own the current live joint values used for scene revisioning.""" + self._published_joint_positions = { + key: state.position.clone() for key, state in states.items() + } + self._published_joint_validity = {} + for key, state in states.items(): + assert state.valid_mask is not None + self._published_joint_validity[key] = state.valid_mask.clone() + def _observe_states( self, *, @@ -1853,11 +2103,16 @@ def _pose_change_mask( __all__ = [ + "ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY", + "ArticulationJointEvidenceAddress", "AmbiguousSceneAffordanceError", "GRASP_AFFORDANCE_CAPABILITY", "PLACE_IN_AFFORDANCE_CAPABILITY", "PLACE_ON_AFFORDANCE_CAPABILITY", "RegistrySceneProvider", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION", + "SceneArticulationJointStateProvider", "SceneAffordanceRef", "SceneArticulationRef", "SceneCollisionRole", diff --git a/tests/sim/skills/test_articulation_semantics.py b/tests/sim/skills/test_articulation_semantics.py new file mode 100644 index 000000000..4cdcd1227 --- /dev/null +++ b/tests/sim/skills/test_articulation_semantics.py @@ -0,0 +1,594 @@ +# ---------------------------------------------------------------------------- +# 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 first-class semantic articulation operations.""" + +from __future__ import annotations + +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, + AtomicActionEngine, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + EntityState, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + ObservedArticulationJointState, + OperateArticulationGoal, + PlanningContext, + RobotObservation, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.skills.calls import ( + OperateArticulation, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + JointStateEffectClause, + SemanticEffectKind, + SymbolicStateKey, +) +from embodichain.lab.sim.skills.integration import ( + SceneManifest, + SemanticIntegrationManifest, + SemanticValidationError, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRegistration, + SceneRegistry, +) + +_BATCH_SIZE = 2 +_TARGET_POSITION = 0.42 +_TARGET_DISPLACEMENT = 0.4 +_POSITION_SCALE = 0.5 + + +class _MutablePoseProvider: + """Expose a mutable pose and count semantic observation calls.""" + + def __init__( + self, + pose: torch.Tensor, + *, + joint_position: torch.Tensor | None = None, + ) -> None: + self.pose = pose.clone() + self.joint_position = None if joint_position is None else joint_position.clone() + self.calls = 0 + self.joint_calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + return EntityState(self.pose) + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, ObservedArticulationJointState]: + del timestamp, env_ids + self.joint_calls += 1 + if self.joint_position is None: + raise RuntimeError("This provider has no articulation joint fixture.") + return {"drawer_slide": ObservedArticulationJointState(self.joint_position)} + + +def _translated_offset(x: float, y: float, z: float) -> torch.Tensor: + """Build one test-only proper local offset.""" + pose = torch.eye(4, dtype=torch.float32) + pose[:3, 3] = torch.tensor((x, y, z), dtype=torch.float32) + return pose + + +def _operation_affordance() -> ArticulationOperationAffordance: + """Build the canonical drawer-handle fixture.""" + return ArticulationOperationAffordance( + joint_id="drawer_slide", + approach_offset=_translated_offset(0.0, 0.0, -0.1), + contact_offset=torch.eye(4), + operation_offset=_translated_offset(0.0, 0.02, 0.0), + retract_offset=_translated_offset(0.0, 0.0, -0.1), + operation_axis=torch.tensor((1.0, 0.0, 0.0)), + position_scale=_POSITION_SCALE, + semantic_targets={ + "open": ArticulationOperationTarget( + target_position=_TARGET_POSITION, + displacement=_TARGET_DISPLACEMENT, + ) + }, + ) + + +def _registry() -> tuple[SceneRegistry, _MutablePoseProvider, _MutablePoseProvider]: + """Build an articulation plus one directly registered handle affordance.""" + articulation = SceneArticulationRef("drawer") + handle = SceneAffordanceRef("drawer_handle") + articulation_provider = _MutablePoseProvider( + torch.eye(4).repeat(_BATCH_SIZE, 1, 1), + joint_position=torch.zeros(_BATCH_SIZE, 1), + ) + handle_pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + handle_pose[:, 0, 3] = 0.3 + handle_provider = _MutablePoseProvider(handle_pose) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=articulation, + state_provider=articulation_provider, + joint_state_provider=articulation_provider, + semantic_type="drawer", + default_affordances={ + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: handle + }, + ), + SceneEntityRegistration( + ref=handle, + state_provider=handle_provider, + parent=articulation, + native_name="handle", + affordance=_operation_affordance(), + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision="drawer-handle-v1", + ), + ) + ) + return registry, articulation_provider, handle_provider + + +def _profile() -> RobotSkillProfile: + """Build one resource satisfying motion and interaction endpoints.""" + return RobotSkillProfile( + profile_id="articulation_test_robot", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + "interaction": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor((0.0,)), + grasp=torch.tensor((1.0,)), + ) + }, + presets={"safe": SkillPolicyPreset("safe")}, + default_preset="safe", + ) + + +def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: + """Construct a CPU-only engine with the minimum typed robot surface.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 2 + robot.control_parts = {"arm": object(), "hand": object()} + robot.get_qpos.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + robot.get_qvel.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + robot.get_joint_ids.side_effect = lambda name: {"arm": [0], "hand": [1]}[name] + robot.get_solver.return_value = object() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + return AtomicActionEngine(generator, skill_profile=profile) + + +def _compiler(registry: SceneRegistry) -> SemanticSkillCompiler: + """Bind the curated semantic catalog to the test scene and profile.""" + profile = _profile() + engine = _engine(profile) + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=builtin_semantic_call_catalog(), + ) + return SemanticSkillCompiler(manifest.bind(registry, engine)) + + +def _context(scene: SceneSnapshot, *, timestamp: float) -> PlanningContext: + """Build one immutable planning observation around a supplied scene.""" + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=torch.zeros(_BATCH_SIZE, 2), + qvel=torch.zeros(_BATCH_SIZE, 2), + ), + task=TaskState.empty(_BATCH_SIZE, "cpu"), + scene=scene, + env_ids=env_ids, + ) + + +def test_articulation_affordance_owns_configuration_and_grounds_geometry() -> None: + axis = torch.tensor((2.0, 0.0, 0.0)) + operation_offset = _translated_offset(0.0, 0.02, 0.0) + targets = { + "open": ArticulationOperationTarget( + _TARGET_POSITION, + _TARGET_DISPLACEMENT, + ) + } + affordance = ArticulationOperationAffordance( + joint_id="drawer_slide", + operation_axis=axis, + operation_offset=operation_offset, + position_scale=_POSITION_SCALE, + semantic_targets=targets, + ) + axis.zero_() + operation_offset.zero_() + targets.clear() + + handle = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + handle[:, 0, 3] = 0.3 + _, _, operation, _ = affordance.ground_poses( + handle, + displacement=_TARGET_DISPLACEMENT, + ) + + assert tuple(affordance.semantic_targets) == ("open",) + assert torch.allclose(affordance.operation_axis, torch.tensor((1.0, 0.0, 0.0))) + assert torch.allclose( + operation[:, :3, 3], + torch.tensor((0.5, 0.02, 0.0)).repeat(_BATCH_SIZE, 1), + ) + + +def test_registry_returns_owned_articulation_affordance_snapshots() -> None: + registry, _, _ = _registry() + + first = registry.lookup( + SceneAffordanceRef("drawer_handle"), + expected_type=SceneAffordanceRef, + ).affordance + second = registry.lookup( + SceneAffordanceRef("drawer_handle"), + expected_type=SceneAffordanceRef, + ).affordance + + assert type(first) is ArticulationOperationAffordance + assert type(second) is ArticulationOperationAffordance + assert first is not second + first.operation_offset[0, 3] = 99.0 + assert second.operation_offset[0, 3].item() == 0.0 + + +@pytest.mark.parametrize( + "kwargs", + ( + {}, + {"target_position": _TARGET_POSITION}, + {"target_displacement": _TARGET_DISPLACEMENT}, + { + "target": "open", + "target_position": _TARGET_POSITION, + "target_displacement": _TARGET_DISPLACEMENT, + }, + ), +) +def test_articulation_call_requires_exactly_one_complete_target(kwargs: dict) -> None: + with pytest.raises(ValueError): + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + **kwargs, + ) + + +def test_static_link_selects_default_without_observing_scene() -> None: + registry, articulation_provider, handle_provider = _registry() + compiler = _compiler(registry) + + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + + analyzed = workflow.calls[0] + assert analyzed.effect_kind is SemanticEffectKind.ARTICULATION + assert analyzed.bound.linked.affordances["handle"] == SceneAffordanceRef( + "drawer_handle" + ) + assert analyzed.bound.linked.descriptor.skill_id == "operate_articulation" + assert analyzed.symbolic_writes == frozenset( + {SymbolicStateKey.articulation_joint("drawer", "drawer_slide")} + ) + assert not analyzed.opaque_symbolic_effect + assert articulation_provider.calls == 0 + assert handle_provider.calls == 0 + + +def test_static_link_rejects_unknown_explicit_handle_with_path() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + handle=SceneAffordanceRef("missing_handle"), + target="open", + ), + ) + ) + + assert error.value.diagnostic.path == ("workflow", 0, "call", "handle") + + +def test_grounding_uses_fresh_handle_pose_and_lowers_typed_effect() -> None: + registry, articulation_provider, handle_provider = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + scene_provider = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ) + first_context = _context( + scene_provider.snapshot(timestamp=0.0, env_ids=env_ids), + timestamp=0.0, + ) + first = compiler.ground(workflow, 0, first_context) + handle_provider.pose[:, 0, 3] = 0.7 + assert articulation_provider.joint_position is not None + articulation_provider.joint_position[:, 0] = 0.1 + second_context = _context( + scene_provider.snapshot(timestamp=1.0, env_ids=env_ids), + timestamp=1.0, + ) + second = compiler.ground(workflow, 0, second_context, revision=1) + + first_goal = first.invocation.goal + second_goal = second.invocation.goal + assert type(first_goal) is OperateArticulationGoal + assert type(second_goal) is OperateArticulationGoal + first_poses = first_goal.geometry.resolve( + first_context, + displacement=torch.full((_BATCH_SIZE,), _TARGET_DISPLACEMENT), + ) + second_poses = second_goal.geometry.resolve( + second_context, + displacement=torch.full((_BATCH_SIZE,), _TARGET_DISPLACEMENT), + ) + assert torch.allclose(first_poses[0][:, 0, 3], torch.full((2,), 0.3)) + assert torch.allclose(second_poses[0][:, 0, 3], torch.full((2,), 0.7)) + assert torch.allclose(second_poses[2][:, 0, 3], torch.full((2,), 0.9)) + assert torch.equal( + first_goal.source_position, + torch.zeros(_BATCH_SIZE, 1), + ) + assert torch.equal( + second_goal.source_position, + torch.full((_BATCH_SIZE, 1), 0.1), + ) + assert second_goal.target_displacement == _TARGET_DISPLACEMENT + assert torch.allclose( + second_goal.target_position, + torch.full((_BATCH_SIZE, 1), _TARGET_POSITION), + ) + + effect = second.effect_spec + assert effect is not None + assert effect.effect_kind is SemanticEffectKind.ARTICULATION + expectation = effect.state_expectations[0] + clause = effect.clauses[0] + assert type(expectation) is ArticulationJointStateExpectation + assert expectation.articulation_id == "drawer" + assert expectation.joint_id == "drawer_slide" + assert type(clause) is JointStateEffectClause + assert torch.equal(clause.target_position, second_goal.target_position) + assert clause.source.provider_id == SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + assert clause.source.revision == SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + assert type(clause.source.address) is ArticulationJointEvidenceAddress + assert clause.source.address.articulation_id == "drawer" + assert clause.source.address.joint_id == "drawer_slide" + + +def test_explicit_target_pair_records_live_source_joint_state() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target_position=0.25, + target_displacement=-0.1, + ), + ) + ) + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + scene = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ).snapshot(timestamp=0.0, env_ids=env_ids) + + grounded = compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + goal = grounded.invocation.goal + assert type(goal) is OperateArticulationGoal + assert torch.allclose(goal.target_position, torch.full((_BATCH_SIZE, 1), 0.25)) + assert torch.equal(goal.source_position, torch.zeros(_BATCH_SIZE, 1)) + assert goal.target_displacement == -0.1 + operation = goal.geometry.resolve( + _context(scene, timestamp=0.0), + displacement=torch.full((_BATCH_SIZE,), -0.1), + )[2] + assert torch.allclose(operation[:, 0, 3], torch.full((2,), 0.25)) + + +def test_unknown_named_target_has_strict_grounding_diagnostic() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="closed", + ), + ) + ) + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + scene = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ).snapshot(timestamp=0.0, env_ids=env_ids) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_articulation_target" + assert diagnostic.path == ("workflow", 0, "call", "target") + assert diagnostic.candidates == ("open",) + + +def test_missing_handle_pose_has_strict_grounding_diagnostic() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer": EntityState(torch.eye(4).repeat(_BATCH_SIZE, 1, 1))}, + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "missing_handle_observation" + assert diagnostic.path == ("workflow", 0, "call", "handle") + + +def test_missing_live_joint_state_has_strict_grounding_diagnostic() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + handle = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer_handle": EntityState(handle)}, + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "missing_articulation_joint_observation" + assert diagnostic.path == ("workflow", 0, "call", "articulation") + + +def test_articulation_capability_rejects_untyped_affordance_payload() -> None: + articulation = SceneArticulationRef("drawer") + handle = SceneAffordanceRef("drawer_handle") + provider = _MutablePoseProvider(torch.eye(4).repeat(_BATCH_SIZE, 1, 1)) + + with pytest.raises(TypeError, match="ArticulationOperationAffordance"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=articulation, + state_provider=provider, + default_affordances={ + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: handle + }, + ), + SceneEntityRegistration( + ref=handle, + state_provider=provider, + parent=articulation, + native_name="handle", + affordance=Affordance(), + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision="bad-v1", + ), + ) + ) diff --git a/tests/sim/skills/test_calls.py b/tests/sim/skills/test_calls.py index 742d0058c..b6d7d8502 100644 --- a/tests/sim/skills/test_calls.py +++ b/tests/sim/skills/test_calls.py @@ -19,6 +19,7 @@ from __future__ import annotations from collections.abc import Callable, Mapping +import json import math import pytest @@ -113,6 +114,25 @@ def test_semantic_pose_converts_to_homogeneous_matrix() -> None: torch.testing.assert_close(pose.to_matrix(), expected, atol=1.0e-6, rtol=1.0e-6) +def test_semantic_call_metadata_is_deterministic_and_json_safe() -> None: + call = Place( + object=SceneObjectRef("cube"), + at=SemanticPose((1.0, 2.0, 3.0), (1.0, 0.0, 0.0, 0.0)), + resources={"primary": "left_arm"}, + ) + + metadata = call.to_metadata() + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["semantic_id"] == "place" + assert metadata["resources"] == {"primary": "left_arm"} + assert metadata["arguments"]["object"] == { + "entity_type": "SceneObjectRef", + "entity_id": "cube", + } + assert metadata["arguments"]["at"]["position"] == [1.0, 2.0, 3.0] + + @pytest.mark.parametrize( "factory", ( diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index f9a206e5f..5ad526016 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -32,12 +32,14 @@ BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, ControlPartCommandProfile, + DynamicCollisionMode, EntityState, FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, GraspGoal, HandOverOptions, HeldObjectState, + MotionPolicy, ObjectSemantics, PickUp, PickUpOptions, @@ -69,6 +71,24 @@ SemanticSkillCompiler, SemanticWorkflow, ) +from embodichain.lab.sim.skills.effects import ( + BinaryEffectClause, + BinaryEvidenceKind, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CompositeEffectMonitorFactory, + ControlPartEvidenceAddress, + EffectMonitor, + EffectMonitorRef, + EffectMonitorRegistry, + HeldObjectRelation, + HeldObjectStateExpectation, + PoseRelationClause, + PoseRelationExpectation, + SemanticEffectKind, + SemanticEffectSpec, + SymbolicStateKey, +) from embodichain.lab.sim.skills.integration import ( BoundSemanticCall, SceneManifest, @@ -86,6 +106,8 @@ GRASP_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, + SceneCollisionRole, + SceneCollisionWorldMode, SceneEntityRegistration, SceneObjectRef, SceneRegistry, @@ -119,6 +141,13 @@ def observe( return EntityState(self.pose) +class _GeometryProvider: + """Return one opaque planner-facing geometry descriptor.""" + + def get_geometry(self) -> object: + return object() + + class _FrameRelationGrounder(RelationTargetGrounder): """Explicit test contract: relation frame equals target object frame.""" @@ -234,7 +263,37 @@ def resolve( ) -def _scene_registry() -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider]]: +class _CountingRelationMonitorFactory(CompositeEffectMonitorFactory): + """Count monitor construction without changing built-in behavior.""" + + def __init__(self) -> None: + self.calls = 0 + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + self.calls += 1 + return super().create(spec, ref) + + +class _BadCreatingRelationMonitorFactory(CompositeEffectMonitorFactory): + """Return an invalid monitor value after successful static validation.""" + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + del spec, ref + return object() # type: ignore[return-value] + + +def _scene_registry( + *, + dynamic_collision: bool = False, +) -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider]]: cube_provider = _PoseProvider(torch.eye(4).repeat(2, 1, 1)) table_pose = torch.eye(4).repeat(2, 1, 1) table_pose[:, 0, 3] = 0.6 @@ -250,6 +309,12 @@ def _scene_registry() -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider state_provider=cube_provider, semantic_type="cube", default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp}, + geometry_provider=(_GeometryProvider() if dynamic_collision else None), + collision_role=( + SceneCollisionRole.DYNAMIC + if dynamic_collision + else SceneCollisionRole.NONE + ), ), SceneEntityRegistration( ref=grasp, @@ -275,12 +340,15 @@ def _scene_registry() -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider affordance_revision="relation-v1", relative_pose=torch.eye(4), ), - ) + ), + collision_world_mode=( + SceneCollisionWorldMode.PER_ENV if dynamic_collision else None + ), ) return registry, (cube_provider, table_provider) -def _profile() -> RobotSkillProfile: +def _profile(*, preset: SkillPolicyPreset | None = None) -> RobotSkillProfile: return RobotSkillProfile( profile_id="test_robot", resources={ @@ -304,7 +372,7 @@ def _profile() -> RobotSkillProfile: grasp=torch.tensor([1.0]), ) }, - presets={"safe": SkillPolicyPreset("safe")}, + presets={"safe": SkillPolicyPreset("safe") if preset is None else preset}, default_preset="safe", ) @@ -346,7 +414,11 @@ def _dual_profile(*, provider_id: str | None = "dual_center") -> RobotSkillProfi ) -def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: +def _engine( + profile: RobotSkillProfile, + *, + supports_dynamic_collision_world: bool = False, +) -> AtomicActionEngine: robot = Mock() robot.device = torch.device("cpu") control_parts = tuple( @@ -370,6 +442,7 @@ def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: generator.robot = robot generator.device = torch.device("cpu") generator.planner.cfg.planner_type = "stub_planner" + generator.supports_dynamic_collision_world = supports_dynamic_collision_world return AtomicActionEngine(generator, skill_profile=profile) @@ -377,8 +450,10 @@ def _integration( registry: SceneRegistry, *, registered: bool = False, + profile: RobotSkillProfile | None = None, + supports_dynamic_collision_world: bool = False, ) -> tuple[SemanticIntegrationManifest, AtomicActionEngine]: - profile = _profile() + selected_profile = _profile() if profile is None else profile catalog = builtin_semantic_call_catalog() if registered: assert _PICK_TARGET.binding_contract is not None @@ -393,10 +468,13 @@ def _integration( ) manifest = SemanticIntegrationManifest( scene=SceneManifest.from_registry(registry), - robot_profile=profile, + robot_profile=selected_profile, call_catalog=catalog, ) - return manifest, _engine(profile) + return manifest, _engine( + selected_profile, + supports_dynamic_collision_world=supports_dynamic_collision_world, + ) def _compiler( @@ -407,14 +485,25 @@ def _compiler( _FrameRelationGrounder(), ), registered_lowerers: tuple[RegisteredSemanticLowerer, ...] = (), + handover_pose_providers: tuple[HandOverPoseProvider, ...] = (), + profile: RobotSkillProfile | None = None, + effect_monitor_registry: EffectMonitorRegistry | None = None, + supports_dynamic_collision_world: bool = False, ) -> tuple[SemanticSkillCompiler, AtomicActionEngine]: - manifest, engine = _integration(registry, registered=registered) + manifest, engine = _integration( + registry, + registered=registered, + profile=profile, + supports_dynamic_collision_world=supports_dynamic_collision_world, + ) bound = manifest.bind(registry, engine) return ( SemanticSkillCompiler( bound, relation_grounders=relation_grounders, registered_lowerers=registered_lowerers, + handover_pose_providers=handover_pose_providers, + effect_monitor_registry=effect_monitor_registry, ), engine, ) @@ -450,7 +539,7 @@ def _held_context( object_to_eef: torch.Tensor, *, env_mask: torch.Tensor | None = None, - control_part: str = "arm", + task_state_key: str = "manipulator", robot_dof: int = 2, ) -> PlanningContext: held = HeldObjectState( @@ -464,12 +553,350 @@ def _held_context( task=TaskState( batch_size=2, device="cpu", - held_objects={control_part: held}, + held_objects={task_state_key: held}, ), robot_dof=robot_dof, ) +def test_curated_analysis_selects_exact_preset_monitor_without_creating_it() -> None: + registry, providers = _scene_registry() + factory = _CountingRelationMonitorFactory() + compiler, _ = _compiler( + registry, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + monitor_ref = workflow.calls[0].effect_monitor_ref + assert monitor_ref is not None + assert monitor_ref.monitor_id == COMPOSITE_EFFECT_MONITOR_ID + assert monitor_ref.revision == COMPOSITE_EFFECT_MONITOR_REVISION + assert workflow.calls[0].symbolic_writes == frozenset( + {SymbolicStateKey.held_object("manipulator")} + ) + assert not workflow.calls[0].opaque_symbolic_effect + assert factory.calls == 0 + assert [provider.calls for provider in providers] == [0, 0] + + +def test_curated_analysis_rejects_explicitly_missing_monitor() -> None: + registry, _ = _scene_registry() + profile = _profile( + preset=SkillPolicyPreset("safe", effect_monitors={}), + ) + compiler, _ = _compiler(registry, profile=profile) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + assert error.value.diagnostic.code == "missing_effect_monitor" + + +def test_uninstalled_effect_monitor_fails_analysis_without_factory_creation() -> None: + registry, providers = _scene_registry() + factory = _CountingRelationMonitorFactory() + profile = _profile( + preset=SkillPolicyPreset( + "safe", + effect_monitors={ + "pick": EffectMonitorRef("test.not_installed", "1"), + }, + ), + ) + compiler, _ = _compiler( + registry, + profile=profile, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + assert error.value.diagnostic.code == "effect_monitor_not_installed" + assert factory.calls == 0 + assert [provider.calls for provider in providers] == [0, 0] + + +def test_invalid_effect_monitor_config_fails_analysis_without_side_effects() -> None: + registry, providers = _scene_registry() + factory = _CountingRelationMonitorFactory() + profile = _profile( + preset=SkillPolicyPreset( + "safe", + effect_monitors={ + "pick": EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + { + "attached_translation_threshold": 0.10, + "detached_translation_threshold": 0.05, + }, + ), + }, + ), + ) + compiler, _ = _compiler( + registry, + profile=profile, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "invalid_effect_monitor_config" + assert diagnostic.path == ("workflow", 0, "effect_monitor") + assert factory.calls == 0 + assert [provider.calls for provider in providers] == [0, 0] + + +def test_pick_effect_spec_binds_destination_and_fresh_monitor_per_grounding() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + context = _context(registry) + + first = compiler.ground(workflow, 0, context) + repeated = compiler.ground(workflow, 0, context) + revised = compiler.ground(workflow, 0, context, revision=1) + + spec = first.effect_spec + assert spec is not None + assert spec.semantic_id == "pick" + assert spec.effect_kind is SemanticEffectKind.ATTACH + assert spec.skill_id == first.invocation.skill_id + assert spec.invocation_id == first.invocation.invocation_id + assert spec.invocation_revision == 0 + torch.testing.assert_close(spec.env_ids, context.env_ids) + assert len(spec.state_expectations) == 1 + relation = spec.state_expectations[0] + assert isinstance(relation, HeldObjectStateExpectation) + assert relation.expectation_id == "destination" + assert relation.relation is HeldObjectRelation.ATTACHED + assert relation.object_id == "cube" + assert relation.slot_id == "primary" + assert relation.resource_id == "manipulator" + assert relation.task_state_key == "manipulator" + pose, constraint = spec.clauses + assert isinstance(pose, PoseRelationClause) + assert pose.expectation is PoseRelationExpectation.MATCHED + assert pose.baseline_object_to_endpoint is None + assert pose.source.address == ControlPartEvidenceAddress("arm", "pose_relation") + assert isinstance(constraint, BinaryEffectClause) + assert constraint.evidence_kind is BinaryEvidenceKind.CONSTRAINT + assert constraint.expected is True + assert constraint.source.address == ControlPartEvidenceAddress("hand", "constraint") + assert ( + first.analyzed.bound.binding.action_binding.endpoint( + "primary", "motion" + ).task_state_key + == "manipulator" + ) + assert first.effect_monitor is not None + assert repeated.effect_monitor is not None + assert revised.effect_monitor is not None + assert repeated.effect_monitor is not first.effect_monitor + assert revised.effect_monitor is not first.effect_monitor + assert repeated.effect_spec is not None + assert repeated.effect_spec.invocation_revision == 0 + assert revised.effect_spec is not None + assert revised.effect_spec.invocation_revision == 1 + assert revised.effect_monitor.spec.invocation_revision == 1 + + +def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + pick = compiler.ground(pick_workflow, 0, _context(registry)) + semantics = pick.invocation.goal.semantics + object_to_eef = torch.eye(4).repeat(2, 1, 1) + object_to_eef[:, 2, 3] = 0.12 + context = _held_context(registry, semantics, object_to_eef) + workflow = compiler.analyze( + ( + Place( + object=SceneObjectRef("cube"), + at=SemanticPose( + (0.5, -0.2, 0.4), + (1.0, 0.0, 0.0, 0.0), + ), + ), + ) + ) + + assert workflow.calls[0].symbolic_writes == frozenset( + {SymbolicStateKey.held_object("manipulator")} + ) + grounded = compiler.ground(workflow, 0, context) + + spec = grounded.effect_spec + assert spec is not None + assert spec.semantic_id == "place" + assert spec.effect_kind is SemanticEffectKind.RELEASE + assert len(spec.state_expectations) == 1 + relation = spec.state_expectations[0] + assert isinstance(relation, HeldObjectStateExpectation) + assert relation.expectation_id == "source" + assert relation.relation is HeldObjectRelation.DETACHED + assert relation.object_id == "cube" + assert relation.slot_id == "primary" + assert relation.resource_id == "manipulator" + assert relation.task_state_key == "manipulator" + pose, constraint = spec.clauses + assert isinstance(pose, PoseRelationClause) + assert pose.expectation is PoseRelationExpectation.SEPARATED + assert pose.baseline_object_to_endpoint is not None + torch.testing.assert_close( + pose.baseline_object_to_endpoint, + object_to_eef, + ) + assert isinstance(constraint, BinaryEffectClause) + assert constraint.expected is False + + +def test_handover_effect_spec_binds_source_and_destination_relations() -> None: + registry, _ = _scene_registry() + provider = _DualCenterHandOverProvider() + compiler, _ = _compiler( + registry, + profile=_dual_profile(), + handover_pose_providers=(provider,), + ) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + pick = compiler.ground( + pick_workflow, + 0, + _context(registry, robot_dof=4), + ) + semantics = pick.invocation.goal.semantics + object_to_source = torch.eye(4).repeat(2, 1, 1) + object_to_source[:, 0, 3] = 0.08 + context = _held_context( + registry, + semantics, + object_to_source, + task_state_key="left", + robot_dof=4, + ) + workflow = compiler.analyze((HandOver(object=SceneObjectRef("cube")),)) + + assert workflow.calls[0].symbolic_writes == frozenset( + { + SymbolicStateKey.held_object("left"), + SymbolicStateKey.held_object("right"), + } + ) + grounded = compiler.ground(workflow, 0, context) + + spec = grounded.effect_spec + assert spec is not None + assert spec.semantic_id == "hand_over" + assert spec.effect_kind is SemanticEffectKind.TRANSFER + assert tuple(relation.expectation_id for relation in spec.state_expectations) == ( + "source", + "destination", + ) + source, destination = spec.state_expectations + assert isinstance(source, HeldObjectStateExpectation) + assert source.relation is HeldObjectRelation.DETACHED + assert source.object_id == "cube" + assert source.slot_id == "source" + assert source.resource_id == "left" + assert source.task_state_key == "left" + source_pose, source_constraint, destination_pose, destination_constraint = ( + spec.clauses + ) + assert isinstance(source_pose, PoseRelationClause) + assert source_pose.expectation is PoseRelationExpectation.SEPARATED + assert source_pose.baseline_object_to_endpoint is not None + torch.testing.assert_close( + source_pose.baseline_object_to_endpoint, + object_to_source, + ) + assert isinstance(source_constraint, BinaryEffectClause) + assert source_constraint.expected is False + assert isinstance(destination, HeldObjectStateExpectation) + assert destination.relation is HeldObjectRelation.ATTACHED + assert destination.object_id == "cube" + assert destination.slot_id == "destination" + assert destination.resource_id == "right" + assert destination.task_state_key == "right" + assert isinstance(destination_pose, PoseRelationClause) + assert destination_pose.expectation is PoseRelationExpectation.MATCHED + assert destination_pose.baseline_object_to_endpoint is None + assert isinstance(destination_constraint, BinaryEffectClause) + assert destination_constraint.expected is True + + +def test_registered_call_without_monitor_has_no_effect_contract() -> None: + registry, _ = _scene_registry() + factory = _CountingRelationMonitorFactory() + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_InspectLowerer(),), + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + + grounded = compiler.ground(workflow, 0, _context(registry)) + + assert workflow.calls[0].symbolic_writes == frozenset() + assert workflow.calls[0].opaque_symbolic_effect + assert workflow.calls[0].effect_monitor_ref is None + assert grounded.effect_spec is None + assert grounded.effect_monitor is None + assert factory.calls == 0 + + +def test_registered_monitor_without_effect_grounder_fails_during_analysis() -> None: + registry, _ = _scene_registry() + profile = _profile( + preset=SkillPolicyPreset( + "safe", + effect_monitors={ + "vendor.inspect": EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + }, + ) + ) + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_InspectLowerer(),), + profile=profile, + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + + assert error.value.diagnostic.code == "registered_effect_contract_not_installed" + assert error.value.diagnostic.path == ("workflow", 0, "effect_monitor") + + +def test_ground_wraps_effect_monitor_factory_contract_failure_with_path() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler( + registry, + effect_monitor_registry=EffectMonitorRegistry( + (_BadCreatingRelationMonitorFactory(),) + ), + ) + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(registry)) + + assert error.value.diagnostic.code == "effect_monitor_creation_failed" + assert error.value.diagnostic.path == ("workflow", 0, "effect_monitor") + + def test_analysis_is_provider_free_and_propagates_object_target() -> None: registry, providers = _scene_registry() compiler, engine = _compiler(registry) @@ -499,6 +926,33 @@ def test_analysis_is_provider_free_and_propagates_object_target() -> None: engine.resolve(grounded.invocation) +def test_grounded_safe_invocation_requires_registered_dynamic_collision() -> None: + registry, _ = _scene_registry(dynamic_collision=True) + profile = _profile( + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ) + ) + compiler, engine = _compiler( + registry, + profile=profile, + supports_dynamic_collision_world=True, + ) + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + grounded = compiler.ground(workflow, 0, _context(registry)) + + assert ( + grounded.invocation.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + assert ( + engine.resolve(grounded.invocation).motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + + def test_pick_relation_lookahead_stays_late_bound_scene_dependency() -> None: registry, _ = _scene_registry() compiler, engine = _compiler(registry) @@ -624,7 +1078,7 @@ def test_handover_uses_profile_selected_named_provider_and_stops_lookahead() -> registry, pick.invocation.goal.semantics, torch.eye(4).repeat(2, 1, 1), - control_part="left_arm", + task_state_key="left", robot_dof=4, ) handover = compiler.ground(workflow, 1, held_context) @@ -659,7 +1113,7 @@ def capture_middle(matrix: torch.Tensor, name: str) -> torch.Tensor: registry, pick.invocation.goal.semantics, torch.eye(4).repeat(2, 1, 1), - control_part="left_arm", + task_state_key="left", robot_dof=4, ) with pytest.raises(RuntimeError, match="captured target"): diff --git a/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py new file mode 100644 index 000000000..cfdb3ca3a --- /dev/null +++ b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py @@ -0,0 +1,375 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Real semantic-runtime recovery gate for a dynamic cuRobo collision world.""" + +from __future__ import annotations + +from typing import ClassVar + +import pytest +import torch + +# Module-level guards must precede cuRobo-only imports. +pytest.importorskip("curobo") +if not torch.cuda.is_available(): + pytest.skip("cuRobo V2 requires CUDA", allow_module_level=True) + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg # noqa: E402 +from embodichain.lab.sim.atomic_actions import ( # noqa: E402 + CARTESIAN_POSE_CAPABILITY, + AtomicActionEngine, + CommandAcknowledgement, + DynamicCollisionMode, + EndEffectorPoseGoal, + ExecutionEventKind, + ExecutionRunnerCfg, + MotionPolicy, + MoveEndEffector, + PlanningContext, + RecoveryPolicy, + RuntimeCommandFrame, + RuntimeEndpointTarget, + SimulationExecutionAdapter, + SkillDescriptor, +) +from embodichain.lab.sim.cfg import RigidBodyAttributesCfg # noqa: E402 +from embodichain.lab.sim.objects import RigidObjectCfg # noqa: E402 +from embodichain.lab.sim.planners import MotionGenCfg, MotionGenerator # noqa: E402 +from embodichain.lab.sim.planners.curobo.curobo_planner import ( # noqa: E402 + CuroboAutoGenCfg, + CuroboPlannerCfg, + CuroboWorldCfg, +) +from embodichain.lab.sim.robots import FrankaPandaCfg # noqa: E402 +from embodichain.lab.sim.shapes import CubeCfg # noqa: E402 +from embodichain.lab.sim.skills import ( # noqa: E402 + BoundSemanticCall, + ControlPartEndpoint, + EffectEvidenceCollector, + EffectEvidenceProviderRegistry, + RegisteredSemanticCall, + RegisteredSemanticLowerer, + RobotResource, + RobotSkillProfile, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneManifest, + SceneRegistry, + SemanticCallDescriptor, + SemanticIntegrationManifest, + SemanticLowering, + SemanticSkillCompiler, + SkillPolicyPreset, + SkillRuntime, + SkillStatus, + builtin_semantic_call_catalog, +) + +pytestmark = [ + pytest.mark.requires_sim, + pytest.mark.gpu, + pytest.mark.slow, +] + +ROBOT_UID = "semantic_dynamic_scene_franka" +OBSTACLE_UID = "semantic_dynamic_obstacle" +CONTROL_PART = "arm" +CALL_ID = "test.move_end_effector" +SAMPLE_COUNT = 80 +COMMAND_CYCLE_TIME = 0.1 +MOVE_AFTER_COMMAND = 12 +OBSTACLE_SIZE = [0.10, 0.10, 0.12] +OBSTACLE_START_POSITION = [0.59, -0.20, 0.455] +MAXIMUM_FINAL_EEF_ERROR = 0.04 + +_MOVE_TARGET = MoveEndEffector.descriptor() +assert _MOVE_TARGET.binding_contract is not None + + +class _MoveEndEffectorLowerer(RegisteredSemanticLowerer): + """Lower a declarative matrix into the built-in Cartesian motion goal.""" + + call_id: ClassVar[str] = CALL_ID + schema_version: ClassVar[int] = 1 + target_descriptor: ClassVar[SkillDescriptor] = _MOVE_TARGET + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> SemanticLowering: + del bound + values = call.arguments.get("xpos") + if type(values) is not tuple or len(values) != 16: + raise ValueError("xpos must contain one flattened 4x4 pose matrix.") + pose = torch.tensor( + values, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ).reshape(4, 4) + return SemanticLowering(goal=EndEffectorPoseGoal(xpos=pose)) + + +class _CountingCommandSink: + """Count accepted real-simulation command frames while delegating transport.""" + + def __init__(self, delegate: SimulationExecutionAdapter) -> None: + self.delegate = delegate + self.command_count = 0 + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + acknowledgement = self.delegate.send(command, timeout=timeout) + if acknowledgement.accepted: + self.command_count += 1 + return acknowledgement + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + return self.delegate.hold(targets, context, timeout=timeout) + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + return self.delegate.cancel(targets, timeout=timeout) + + +def _profile() -> RobotSkillProfile: + """Declare the exact robot resource and bounded safe recovery policy.""" + return RobotSkillProfile( + profile_id="semantic_dynamic_scene_franka", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part=CONTROL_PART, + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ) + }, + ) + }, + presets={ + "safe": SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=SAMPLE_COUNT, + control_dt=COMMAND_CYCLE_TIME, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + tracking_error_threshold=0.1, + action_timeout=30.0, + ), + runner_cfg=ExecutionRunnerCfg(minimum_cycle_time=COMMAND_CYCLE_TIME), + ) + }, + default_preset="safe", + ) + + +def _compiler( + registry: SceneRegistry, + engine: AtomicActionEngine, +) -> SemanticSkillCompiler: + """Bind the test semantic extension to the real engine and scene registry.""" + catalog = builtin_semantic_call_catalog().with_descriptor( + SemanticCallDescriptor( + call_id=CALL_ID, + spec_type=RegisteredSemanticCall, + skill_id=_MOVE_TARGET.skill_id, + binding_contract=_MOVE_TARGET.binding_contract, + target_descriptor=_MOVE_TARGET, + ) + ) + integration = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=_profile(), + call_catalog=catalog, + ).bind(registry, engine) + return SemanticSkillCompiler( + integration, + registered_lowerers=(_MoveEndEffectorLowerer(),), + ) + + +def test_semantic_runtime_replans_after_dynamic_curobo_world_change() -> None: + """Run semantic lowering, real cuRobo planning, world update, and recovery.""" + sim = SimulationManager( + SimulationManagerCfg(headless=True, sim_device="cuda", num_envs=1) + ) + planner = None + try: + robot = sim.add_robot( + cfg=FrankaPandaCfg.from_dict({"uid": ROBOT_UID, "robot_type": "panda"}) + ) + obstacle = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid=OBSTACLE_UID, + shape=CubeCfg(size=OBSTACLE_SIZE), + attrs=RigidBodyAttributesCfg(), + body_type="kinematic", + init_pos=OBSTACLE_START_POSITION, + init_rot=[0.0, 0.0, 0.0], + ) + ) + sim.update(step=10) + + motion_generator = MotionGenerator( + MotionGenCfg( + planner_cfg=CuroboPlannerCfg( + robot_uid=ROBOT_UID, + auto_gen=CuroboAutoGenCfg( + fit_type="morphit", + sphere_density=0.3, + collision_sphere_buffer=0.005, + ), + world=CuroboWorldCfg( + rigid_objects=[obstacle], + obstacle_representation="cuboid", + dynamic_obstacle_names=[OBSTACLE_UID], + multi_env=False, + ), + warmup_iterations=0, + ) + ) + ) + planner = motion_generator.planner + registry = SceneRegistry.from_simulation( + sim, + rigid_objects={OBSTACLE_UID: OBSTACLE_UID}, + collision_roles={OBSTACLE_UID: SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.SHARED, + ) + scene_provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=1, + ) + adapter = SimulationExecutionAdapter( + sim, + robot, + scene_provider=scene_provider, + ) + sink = _CountingCommandSink(adapter) + engine = AtomicActionEngine(motion_generator) + runtime = SkillRuntime.from_components( + _compiler(registry, engine), + adapter, + sink, + EffectEvidenceCollector(EffectEvidenceProviderRegistry()), + clock=adapter, + ) + + start_pose = robot.compute_fk( + qpos=robot.get_qpos(name=CONTROL_PART), + name=CONTROL_PART, + to_matrix=True, + ) + target_pose = start_pose.clone() + target_pose[:, :3, 3] += torch.tensor( + [0.22, 0.24, 0.12], + dtype=target_pose.dtype, + device=target_pose.device, + ) + call = RegisteredSemanticCall( + call_id=CALL_ID, + arguments={ + "xpos": tuple( + float(value) + for value in target_pose[0].detach().cpu().reshape(-1).tolist() + ) + }, + resources={"primary": "manipulator"}, + ) + + result = runtime.start(call, workflow_id="dynamic_curobo_recovery") + assert result.status is SkillStatus.RUNNING + obstacle_moved = False + for _ in range(2_000): + if result.terminal: + break + if result.wait_duration > 0.0: + adapter.sleep(result.wait_duration) + result = runtime.step() + if not obstacle_moved and sink.command_count >= MOVE_AFTER_COMMAND: + blocking_pose = obstacle.get_local_pose(to_matrix=True).clone() + blocking_pose[:, :3, 3] = 0.5 * ( + start_pose[:, :3, 3] + target_pose[:, :3, 3] + ) + obstacle.set_local_pose(blocking_pose) + adapter.sleep(adapter.physics_dt) + obstacle_moved = True + + assert obstacle_moved + assert result.status is SkillStatus.COMPLETED, result.message + assert result.success_mask.tolist() == [True] + assert len(result.calls) == 1 + trace = result.calls[0] + assert trace.semantic_id == CALL_ID + assert trace.skill_id == MoveEndEffector.skill_id + assert len(trace.plan_attempts) >= 2 + + event_kinds = tuple(event.kind for event in result.events) + assert ExecutionEventKind.COLLISION_WORLD_CHANGED in event_kinds + assert ExecutionEventKind.REPLANNED in event_kinds + + initial_attempt = trace.plan_attempts[0] + changed_attempts = tuple( + attempt + for attempt in trace.plan_attempts[1:] + if attempt.planned_collision_world_revision[0] + > initial_attempt.planned_collision_world_revision[0] + ) + assert changed_attempts + assert changed_attempts[0].trigger == ExecutionEventKind.REPLANNED.value + assert changed_attempts[0].planner_backend == "curobo" + assert initial_attempt.collision_world_sensitive + assert ( + initial_attempt.resolved_core_policy.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + + final_pose = robot.compute_fk( + qpos=robot.get_qpos(name=CONTROL_PART), + name=CONTROL_PART, + to_matrix=True, + ) + final_error = torch.linalg.vector_norm( + final_pose[:, :3, 3] - target_pose[:, :3, 3], + dim=1, + ) + assert bool((final_error < MAXIMUM_FINAL_EEF_ERROR).all().item()) + finally: + if planner is not None: + planner.close() + sim.destroy() + SimulationManager.flush_cleanup_queue() diff --git a/tests/sim/skills/test_effects.py b/tests/sim/skills/test_effects.py new file mode 100644 index 000000000..3794dad03 --- /dev/null +++ b/tests/sim/skills/test_effects.py @@ -0,0 +1,863 @@ +# ---------------------------------------------------------------------------- +# 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 typed semantic-effect contracts and raw evidence monitors.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, replace +import json +import math +from types import MappingProxyType + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + ArticulationJointState, + EffectVerificationRequest, + HeldObjectState, + ObjectSemantics, + StateDelta, +) +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CompositeEffectMonitor, + CompositeEffectMonitorCfg, + CompositeEffectMonitorFactory, + CoordinatedHeldObjectCleanupExpectation, + EffectEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectMonitor, + EffectMonitorDecision, + EffectMonitorFactory, + EffectMonitorRef, + EffectMonitorRegistry, + HeldObjectRelation, + HeldObjectStateExpectation, + JointStateEffectClause, + JointStateEvidenceBatch, + PoseRelationClause, + PoseRelationEvidenceBatch, + PoseRelationExpectation, + ScalarEffectClause, + ScalarEffectEvidenceBatch, + ScalarEvidenceKind, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, +) + +_ENV_IDS = torch.tensor([101, 205, 309], dtype=torch.long) +_OBJECT_ID = "scene/cube" +_STATE_KEY = "left_actor" +_SKILL_ID = "pick_up" +_INVOCATION_ID = "call-7" + + +@dataclass(frozen=True, slots=True) +class _EvidenceAddress(EffectEvidenceAddress): + """Minimal custom observation address used by contract tests.""" + + endpoint: str + channel: str + + @property + def address_fingerprint(self) -> tuple[type, str, str]: + return type(self), self.endpoint, self.channel + + +class _AliasingAddress(_EvidenceAddress): + """Address intentionally violating snapshot ownership.""" + + def snapshot(self) -> EffectEvidenceAddress: + return self + + +def _source(channel: str) -> EffectEvidenceSourceRef: + return EffectEvidenceSourceRef( + "test.raw_evidence", + "1", + _EvidenceAddress("left_actor", channel), + ) + + +def _poses(*x_offsets: float) -> torch.Tensor: + poses = torch.eye(4).repeat(len(x_offsets), 1, 1) + poses[:, 0, 3] = torch.tensor(x_offsets) + return poses + + +def _semantics(object_id: str = _OBJECT_ID) -> ObjectSemantics: + return ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="object", + entity_id=object_id, + ) + + +def _held( + *, + object_id: str = _OBJECT_ID, + baseline: torch.Tensor | None = None, + env_mask: torch.Tensor | None = None, +) -> HeldObjectState: + poses = _poses(0.0, 0.0, 0.0) if baseline is None else baseline + if env_mask is None: + env_mask = torch.ones(3, dtype=torch.bool) + return HeldObjectState( + semantics=_semantics(object_id), + object_to_eef=poses, + grasp_xpos=_poses(0.0, 0.0, 0.0), + env_mask=env_mask, + ) + + +def _expectation( + relation: HeldObjectRelation = HeldObjectRelation.ATTACHED, + *, + expectation_id: str = "destination", + state_key: str = _STATE_KEY, +) -> HeldObjectStateExpectation: + return HeldObjectStateExpectation( + expectation_id=expectation_id, + relation=relation, + object_id=_OBJECT_ID, + slot_id="primary", + resource_id="left_actor", + task_state_key=state_key, + ) + + +def _attach_spec() -> SemanticEffectSpec: + return SemanticEffectSpec( + semantic_id="pick", + effect_kind=SemanticEffectKind.ATTACH, + skill_id=_SKILL_ID, + invocation_id=_INVOCATION_ID, + invocation_revision=2, + env_ids=_ENV_IDS, + state_expectations=(_expectation(),), + clauses=( + PoseRelationClause( + "destination.pose", + "destination", + _source("pose_relation"), + PoseRelationExpectation.MATCHED, + ), + BinaryEffectClause( + "destination.constraint", + "destination", + _source("constraint"), + BinaryEvidenceKind.CONSTRAINT, + True, + ), + ), + ) + + +def _request( + *, + env_mask: torch.Tensor | None = None, + attempt_generation: int = 0, + verification_id: int = 1, + effects: StateDelta | None = None, +) -> EffectVerificationRequest: + if env_mask is None: + env_mask = torch.ones(3, dtype=torch.bool) + if effects is None: + effects = StateDelta(held_object_updates={_STATE_KEY: _held()}) + return EffectVerificationRequest( + verification_id=verification_id, + skill_id=_SKILL_ID, + invocation_id=_INVOCATION_ID, + invocation_revision=2, + invocation_index=0, + attempt_generation=attempt_generation, + terminal_segment="close", + requested_at=1.0, + deadline=10.0, + env_mask=env_mask, + expected_effects=effects, + ) + + +def _pose_evidence( + offsets: tuple[float, ...], + *, + timestamp: float, + env_ids: torch.Tensor = _ENV_IDS, + valid: torch.Tensor | None = None, + revision: int = 4, +) -> PoseRelationEvidenceBatch: + if valid is None: + valid = torch.ones(len(offsets), dtype=torch.bool) + return PoseRelationEvidenceBatch( + evidence_id="destination.pose", + object_to_endpoint=_poses(*offsets), + valid=valid, + acquisition_errors=tuple( + None if row_valid else "pose unavailable" for row_valid in valid + ), + timestamp=timestamp, + env_ids=env_ids, + observation_revision=revision, + ) + + +def _binary_evidence( + values: tuple[bool, ...], + *, + timestamp: float, + env_ids: torch.Tensor = _ENV_IDS, + valid: torch.Tensor | None = None, + revision: int = 4, +) -> BinaryEffectEvidenceBatch: + if valid is None: + valid = torch.ones(len(values), dtype=torch.bool) + return BinaryEffectEvidenceBatch( + evidence_id="destination.constraint", + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + values=torch.tensor(values, dtype=torch.bool), + valid=valid, + acquisition_errors=tuple( + None if row_valid else "constraint unavailable" for row_valid in valid + ), + timestamp=timestamp, + env_ids=env_ids, + observation_revision=revision, + ) + + +def _evidence( + offsets: tuple[float, ...], + constraints: tuple[bool, ...], + *, + timestamp: float, + env_ids: torch.Tensor = _ENV_IDS, + valid: torch.Tensor | None = None, + revision: int = 4, +) -> Mapping[str, object]: + return { + "destination.pose": _pose_evidence( + offsets, + timestamp=timestamp, + env_ids=env_ids, + valid=valid, + revision=revision, + ), + "destination.constraint": _binary_evidence( + constraints, + timestamp=timestamp, + env_ids=env_ids, + valid=valid, + revision=revision, + ), + } + + +def test_monitor_ref_owns_bounded_non_executable_params() -> None: + params = {"limits": [1, {"enabled": True}]} + ref = EffectMonitorRef("monitor", "v1", params) + params["limits"][1]["enabled"] = False # type: ignore[index] + + assert isinstance(ref.params, MappingProxyType) + assert ref.params["limits"] == (1, MappingProxyType({"enabled": True})) + assert ref.snapshot().params is not ref.params + + +@pytest.mark.parametrize("value", [torch.tensor(1.0), lambda: None, math.inf]) +def test_monitor_ref_rejects_live_or_nonfinite_params(value: object) -> None: + with pytest.raises((TypeError, ValueError)): + EffectMonitorRef("monitor", "v1", {"bad": value}) + + +def test_monitor_ref_rejects_cyclic_params() -> None: + params: dict[str, object] = {} + params["cycle"] = params + + with pytest.raises(ValueError, match="cyclic"): + EffectMonitorRef("monitor", "v1", params) + + +def test_evidence_source_is_independent_from_runtime_command_addresses() -> None: + address = _EvidenceAddress("left_actor", "pose_relation") + source = EffectEvidenceSourceRef("provider", "2", address) + + assert source.address is not address + assert source.source_fingerprint == ( + "provider", + "2", + _EvidenceAddress, + (_EvidenceAddress, "left_actor", "pose_relation"), + ) + assert not hasattr(source, "transport_id") + + +def test_evidence_source_enforces_snapshot_ownership() -> None: + with pytest.raises(TypeError, match="independently owned"): + EffectEvidenceSourceRef( + "provider", + "1", + _AliasingAddress("left_actor", "pose_relation"), + ) + + +def test_semantic_spec_owns_typed_state_and_heterogeneous_clauses() -> None: + env_ids = _ENV_IDS.clone() + spec = _attach_spec() + env_ids[0] = -1 + + assert torch.equal(spec.env_ids, _ENV_IDS) + assert type(spec.state_expectations[0]) is HeldObjectStateExpectation + assert tuple(type(clause) for clause in spec.clauses) == ( + PoseRelationClause, + BinaryEffectClause, + ) + assert spec.snapshot().clauses[0] is not spec.clauses[0] + + +def test_spec_rejects_clause_without_typed_state_expectation() -> None: + with pytest.raises(ValueError, match="unknown state expectations"): + replace( + _attach_spec(), + clauses=(replace(_attach_spec().clauses[0], expectation_id="missing"),), + ) + + +def test_articulation_and_joint_clause_are_first_class_typed_contracts() -> None: + target = torch.tensor([0.42]) + spec = SemanticEffectSpec( + semantic_id="operate_articulation", + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id="operate_articulation", + invocation_id="drawer-1", + invocation_revision=0, + env_ids=_ENV_IDS, + state_expectations=( + ArticulationJointStateExpectation( + "drawer_joint", + "drawer", + "slide", + target, + ), + ), + clauses=( + JointStateEffectClause( + "drawer_joint.position", + "drawer_joint", + _source("joint_state"), + target, + ), + ), + ) + target.fill_(9.0) + + expectation = spec.state_expectations[0] + clause = spec.clauses[0] + assert isinstance(expectation, ArticulationJointStateExpectation) + assert isinstance(clause, JointStateEffectClause) + torch.testing.assert_close(expectation.target_position, torch.tensor([0.42])) + torch.testing.assert_close(clause.target_position, torch.tensor([0.42])) + + request = EffectVerificationRequest( + verification_id=1, + skill_id="operate_articulation", + invocation_id="drawer-1", + invocation_revision=0, + invocation_index=0, + attempt_generation=0, + terminal_segment="operate", + requested_at=1.0, + deadline=10.0, + env_mask=torch.ones(3, dtype=torch.bool), + expected_effects=StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.42])) + } + ), + ) + spec.validate_request(request) + + wrong = replace( + request, + expected_effects=StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.7])) + } + ), + ) + with pytest.raises(ValueError, match="target position"): + spec.validate_request(wrong) + + +def test_request_validation_uses_logical_state_key() -> None: + _attach_spec().validate_request(_request()) + + wrong_key = _request( + effects=StateDelta(held_object_updates={"arm_control_part": _held()}) + ) + with pytest.raises(ValueError, match="exactly match"): + _attach_spec().validate_request(wrong_key) + + +def test_request_validation_declares_coordinated_cleanup_explicitly() -> None: + cleanup = CoordinatedHeldObjectCleanupExpectation( + "cleanup:left_actor:support", + (_STATE_KEY, "support"), + ) + spec = replace( + _attach_spec(), + state_expectations=(*_attach_spec().state_expectations, cleanup), + ) + request = _request( + effects=StateDelta( + held_object_updates={_STATE_KEY: _held()}, + coordinated_held_object_updates={(_STATE_KEY, "support"): None}, + ) + ) + + spec.validate_request(request) + + +def test_pose_evidence_owns_rows_and_allows_invalid_nonfinite_payload() -> None: + poses = _poses(0.0, 0.1) + poses[1].fill_(math.nan) + valid = torch.tensor([True, False]) + batch = PoseRelationEvidenceBatch( + "pose", + poses, + valid, + (None, "occluded"), + 2.0, + torch.tensor([101, 205]), + 3, + ) + poses.zero_() + valid.fill_(True) + + assert torch.isnan(batch.object_to_endpoint[1]).all() + assert batch.valid.tolist() == [True, False] + + +def test_effect_contract_evidence_and_resolved_thresholds_are_json_safe() -> None: + poses = _poses(0.0, 0.1) + poses[1].fill_(math.nan) + batch = PoseRelationEvidenceBatch( + "pose", + poses, + torch.tensor([True, False]), + (None, "occluded"), + 2.0, + torch.tensor([101, 205]), + 3, + ) + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=3), + ) + + metadata = { + "spec": _attach_spec().to_metadata(), + "evidence": batch.to_metadata(), + "thresholds": dict(monitor.resolved_params), + } + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["evidence"]["object_to_endpoint"][1][0][0] is None + assert metadata["thresholds"]["attached_translation_threshold"] == 0.02 + assert metadata["thresholds"]["consecutive_samples"] == 3 + + +def test_binary_scalar_and_joint_evidence_are_distinct_raw_batches() -> None: + valid = torch.tensor([True, True]) + env_ids = torch.tensor([101, 205]) + binary = BinaryEffectEvidenceBatch( + "contact", + BinaryEvidenceKind.CONTACT, + torch.tensor([True, False]), + valid, + (None, None), + 2.0, + env_ids, + 3, + ) + scalar = ScalarEffectEvidenceBatch( + "force", + ScalarEvidenceKind.FORCE, + torch.tensor([2.0, 0.0]), + valid, + (None, None), + 2.0, + env_ids, + 3, + ) + joint = JointStateEvidenceBatch( + "joint", + torch.tensor([[0.4], [0.5]]), + torch.zeros(2, 1), + valid, + (None, None), + 2.0, + env_ids, + 3, + ) + + assert binary.values.dtype == torch.bool + assert scalar.values.tolist() == [2.0, 0.0] + assert joint.positions.shape == (2, 1) + + +def test_valid_raw_evidence_rejects_nonfinite_payload() -> None: + with pytest.raises(ValueError, match="finite"): + ScalarEffectEvidenceBatch( + "force", + ScalarEvidenceKind.FORCE, + torch.tensor([math.nan]), + torch.tensor([True]), + (None,), + 2.0, + torch.tensor([101]), + 3, + ) + + +def test_monitor_requires_pose_and_binary_physical_evidence() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + request = _request() + pose_only = monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (False, False, False), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + + assert not pose_only.success_mask.any() + assert pose_only.failure_mask.all() + + +def test_monitor_reports_success_only_for_complete_consecutive_evidence() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + request = _request() + first = monitor.observe( + request, + _evidence( + (0.0, 0.01, 0.019), + (True, True, True), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + second = monitor.observe( + request, + _evidence( + (0.0, 0.01, 0.019), + (True, True, True), + timestamp=3.0, + revision=5, + ), # type: ignore[arg-type] + ) + + assert not first.success_mask.any() + assert second.success_mask.all() + assert not second.failure_mask.any() + + +def test_invalid_evidence_is_unresolved_and_resets_hysteresis() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + request = _request() + monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + invalid = monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=3.0, + valid=torch.tensor([False, True, True]), + revision=5, + ), # type: ignore[arg-type] + ) + after_reset = monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=4.0, + revision=6, + ), # type: ignore[arg-type] + ) + + assert invalid.success_mask.tolist() == [False, True, True] + assert after_reset.success_mask.tolist() == [False, True, True] + + +def test_request_shrink_preserves_counts_and_generation_change_resets() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + monitor.observe( + _request(), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + shrunk = _request( + env_mask=torch.tensor([False, True, True]), + verification_id=2, + ) + preserved = monitor.observe( + shrunk, + _evidence( + (0.0, 0.0), + (True, True), + timestamp=3.0, + env_ids=torch.tensor([205, 309]), + revision=5, + ), # type: ignore[arg-type] + ) + reset = monitor.observe( + _request(attempt_generation=1, verification_id=3), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=3.0, + revision=5, + ), # type: ignore[arg-type] + ) + + assert preserved.success_mask.tolist() == [False, True, True] + assert not reset.success_mask.any() + + +def test_monitor_rejects_expansion_duplicate_counting_and_late_evidence() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + shrunk = _request(env_mask=torch.tensor([False, True, True])) + sample = _evidence( + (0.0, 0.0), + (True, True), + timestamp=2.0, + env_ids=torch.tensor([205, 309]), + ) + monitor.observe(shrunk, sample) # type: ignore[arg-type] + repeated = monitor.observe(shrunk, sample) # type: ignore[arg-type] + + assert not repeated.success_mask.any() + with pytest.raises(ValueError, match="only shrink"): + monitor.observe( + _request(verification_id=2), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=3.0, + revision=5, + ), # type: ignore[arg-type] + ) + with pytest.raises(ValueError, match="deadline"): + CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ).observe( + _request(), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=10.01, + ), # type: ignore[arg-type] + ) + + +def test_scalar_and_joint_clauses_use_monitor_owned_policy() -> None: + spec = replace( + _attach_spec(), + clauses=( + ScalarEffectClause( + "destination.force", + "destination", + _source("force"), + ScalarEvidenceKind.FORCE, + ScalarExpectation.PRESENT, + ), + JointStateEffectClause( + "destination.joint", + "destination", + _source("joint_state"), + torch.tensor([0.5]), + ), + ), + ) + monitor = CompositeEffectMonitor( + spec, + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + valid = torch.ones(3, dtype=torch.bool) + errors = (None, None, None) + evidence = { + "destination.force": ScalarEffectEvidenceBatch( + "destination.force", + ScalarEvidenceKind.FORCE, + torch.tensor([2.0, 0.0, 0.5]), + valid, + errors, + 2.0, + _ENV_IDS, + 4, + ), + "destination.joint": JointStateEvidenceBatch( + "destination.joint", + torch.tensor([[0.5], [0.5], [0.7]]), + None, + valid, + errors, + 2.0, + _ENV_IDS, + 4, + ), + } + + decision = monitor.observe(_request(), evidence) + + assert decision.success_mask.tolist() == [True, False, False] + assert decision.failure_mask.tolist() == [False, True, True] + + +class _BoundMonitor(EffectMonitor): + def __init__(self, spec: SemanticEffectSpec, *, alias: bool = False) -> None: + self._spec = spec if alias else spec.snapshot() + self._alias = alias + + @property + def spec(self) -> SemanticEffectSpec: + return self._spec if self._alias else self._spec.snapshot() + + def observe( + self, + request: EffectVerificationRequest, + evidence: Mapping[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + del evidence + return EffectMonitorDecision( + torch.zeros_like(request.env_mask), + torch.zeros_like(request.env_mask), + ) + + +class _BoundFactory(EffectMonitorFactory): + monitor_id = "test.bound" + revision = "1" + + def __init__(self, spec: SemanticEffectSpec, *, alias: bool = False) -> None: + self._spec = spec if alias else spec.snapshot() + self._alias = alias + + def validate_ref(self, ref: EffectMonitorRef) -> None: + if (ref.monitor_id, ref.revision) != (self.monitor_id, self.revision): + raise ValueError("wrong key") + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + del spec, ref + return _BoundMonitor(self._spec, alias=self._alias) + + +def test_registry_is_exact_versioned_and_enforces_bound_spec() -> None: + factory = CompositeEffectMonitorFactory() + registry = EffectMonitorRegistry((factory,)) + ref = EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + {"consecutive_samples": 1}, + ) + + first = registry.create(_attach_spec(), ref) + second = registry.create(_attach_spec(), ref) + + assert isinstance(first, CompositeEffectMonitor) + assert first is not second + with pytest.raises(KeyError): + registry.resolve(EffectMonitorRef(factory.monitor_id, "unknown")) + with pytest.raises(ValueError, match="Duplicate"): + EffectMonitorRegistry((factory, CompositeEffectMonitorFactory())) + + +def test_registry_rejects_factory_spec_drift_or_aliasing() -> None: + requested = _attach_spec() + changed = replace(requested, semantic_id="other") + drift = _BoundFactory(changed) + with pytest.raises(ValueError, match="different effect spec"): + EffectMonitorRegistry((drift,)).create( + requested, + EffectMonitorRef(drift.monitor_id, drift.revision), + ) + + alias = _BoundFactory(requested, alias=True) + with pytest.raises(TypeError, match="independently owned"): + EffectMonitorRegistry((alias,)).create( + requested, + EffectMonitorRef(alias.monitor_id, alias.revision), + ) + + +def test_composite_config_requires_real_hysteresis_gaps() -> None: + with pytest.raises(ValueError, match="less than"): + CompositeEffectMonitorCfg( + attached_translation_threshold=0.05, + detached_translation_threshold=0.05, + ) + with pytest.raises(ValueError, match="positive integer"): + CompositeEffectMonitorCfg(consecutive_samples=True) + with pytest.raises(ValueError, match="Unknown"): + CompositeEffectMonitorFactory().validate_ref( + EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + {"typo": 1}, + ) + ) diff --git a/tests/sim/skills/test_evidence.py b/tests/sim/skills/test_evidence.py new file mode 100644 index 000000000..3fb097383 --- /dev/null +++ b/tests/sim/skills/test_evidence.py @@ -0,0 +1,666 @@ +# ---------------------------------------------------------------------------- +# 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 synchronized semantic-effect evidence acquisition.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + EntityState, + ObservedArticulationJointState, + SceneSnapshot, +) +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + FORCE_EFFECT_CHANNEL, + HeldObjectRelation, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + POSE_RELATION_EFFECT_CHANNEL, + PoseRelationClause, + PoseRelationExpectation, + ScalarEffectClause, + ScalarEvidenceKind, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, +) +from embodichain.lab.sim.skills.evidence import ( + BinaryEffectEvidenceQuery, + BinaryEffectObservation, + ControlPartSimulationEvidenceProvider, + EffectEvidenceCollectionContext, + EffectEvidenceCollector, + EffectEvidenceProvider, + EffectEvidenceProviderRegistry, + JointStateEvidenceQuery, + JointStateObservation, + PoseRelationEvidenceQuery, + ScalarEffectEvidenceQuery, + ScalarEffectObservation, + SceneArticulationEvidenceProvider, + build_effect_evidence_queries, +) +from embodichain.lab.sim.skills.scene import ( + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, +) + + +def _source(channel: str, *, provider_id: str | None = None) -> EffectEvidenceSourceRef: + return EffectEvidenceSourceRef( + provider_id or CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress("arm", channel), + ) + + +def _held_expectation() -> HeldObjectStateExpectation: + return HeldObjectStateExpectation( + "held", + HeldObjectRelation.ATTACHED, + "cube", + "actor", + "arm_resource", + "arm_resource", + ) + + +def _attach_spec( + *clauses: object, + env_ids: torch.Tensor | None = None, +) -> SemanticEffectSpec: + return SemanticEffectSpec( + semantic_id="pick:cube", + effect_kind=SemanticEffectKind.ATTACH, + skill_id="PickUp", + invocation_id="pick-1", + invocation_revision=0, + env_ids=torch.tensor([0, 1], dtype=torch.long) if env_ids is None else env_ids, + state_expectations=(_held_expectation(),), + clauses=clauses, + ) + + +def _pose_clause(clause_id: str = "pose") -> PoseRelationClause: + return PoseRelationClause( + clause_id, + "held", + _source(POSE_RELATION_EFFECT_CHANNEL), + PoseRelationExpectation.MATCHED, + ) + + +def _binary_clause( + clause_id: str = "contact", + *, + provider_id: str | None = None, +) -> BinaryEffectClause: + return BinaryEffectClause( + clause_id, + "held", + _source(CONTACT_EFFECT_CHANNEL, provider_id=provider_id), + BinaryEvidenceKind.CONTACT, + True, + ) + + +def _scalar_clause(clause_id: str = "force") -> ScalarEffectClause: + return ScalarEffectClause( + clause_id, + "held", + _source(FORCE_EFFECT_CHANNEL), + ScalarEvidenceKind.FORCE, + ScalarExpectation.PRESENT, + ) + + +class _FakeSceneProvider: + def __init__(self, poses: torch.Tensor, *, confidence: float = 1.0) -> None: + self.poses = poses + self.confidence = confidence + self.calls = 0 + self.received_env_ids: torch.Tensor | None = None + + def snapshot(self, *, timestamp: float, env_ids: torch.Tensor) -> SceneSnapshot: + self.calls += 1 + self.received_env_ids = env_ids.clone() + poses = self.poses.index_select(0, env_ids.to(device=self.poses.device)) + return SceneSnapshot( + timestamp=timestamp, + version=self.calls, + entities={"cube": EntityState(poses, confidence=self.confidence)}, + ) + + +class _FakeRobot: + def __init__(self, qpos: torch.Tensor, qvel: torch.Tensor | None = None) -> None: + self.qpos = qpos + self.qvel = torch.zeros_like(qpos) if qvel is None else qvel + self.fk_calls = 0 + self.qpos_calls = 0 + self.qvel_calls = 0 + + def get_qpos(self, name: str | None = None, target: bool = False) -> torch.Tensor: + assert name == "arm" + assert target is False + self.qpos_calls += 1 + return self.qpos + + def get_qvel(self, name: str | None = None, target: bool = False) -> torch.Tensor: + assert name == "arm" + assert target is False + self.qvel_calls += 1 + return self.qvel + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: Sequence[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + assert name == "arm" + assert env_ids is not None + assert to_matrix is True + self.fk_calls += 1 + poses = torch.eye(4, dtype=qpos.dtype, device=qpos.device).repeat( + qpos.shape[0], 1, 1 + ) + poses[:, 0, 3] = qpos[:, 0] + return poses + + +class _WrongTimestampProvider(EffectEvidenceProvider): + provider_id = "test.provider" + revision = "1" + + def collect( + self, + queries: tuple[object, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + query = queries[0] + assert isinstance(query, BinaryEffectEvidenceQuery) + batch_size = int(context.env_ids.numel()) + return { + query.evidence_id: BinaryEffectEvidenceBatch( + query.evidence_id, + BinaryEvidenceKind.CONTACT, + torch.ones(batch_size, dtype=torch.bool), + torch.ones(batch_size, dtype=torch.bool), + (None,) * batch_size, + context.timestamp + 1.0, + context.env_ids, + context.observation_revision, + ) + } + + +class _SecondRevisionProvider(_WrongTimestampProvider): + revision = "2" + + +def test_collection_context_validates_and_owns_env_ids() -> None: + env_ids = torch.tensor([3, 1], dtype=torch.long) + context = EffectEvidenceCollectionContext(1.25, 7, env_ids) + env_ids[0] = 99 + + assert context.timestamp == 1.25 + assert context.observation_revision == 7 + assert context.env_ids.tolist() == [3, 1] + assert context.snapshot().env_ids.data_ptr() != context.env_ids.data_ptr() + + with pytest.raises(ValueError, match="unique"): + EffectEvidenceCollectionContext(0.0, 0, torch.tensor([1, 1])) + with pytest.raises(ValueError, match="non-negative"): + EffectEvidenceCollectionContext(-0.1, 0, torch.tensor([0])) + + +def test_build_queries_preserves_clause_order_and_exact_types() -> None: + spec = _attach_spec(_pose_clause(), _binary_clause(), _scalar_clause()) + + queries = build_effect_evidence_queries(spec) + + assert tuple(type(query) for query in queries) == ( + PoseRelationEvidenceQuery, + BinaryEffectEvidenceQuery, + ScalarEffectEvidenceQuery, + ) + assert tuple(query.evidence_id for query in queries) == ( + "pose", + "contact", + "force", + ) + assert all(query.expectation.expectation_id == "held" for query in queries) + + +def test_provider_registry_requires_exact_unique_versions() -> None: + first = _WrongTimestampProvider() + second = _SecondRevisionProvider() + registry = EffectEvidenceProviderRegistry((first, second)) + + source_v1 = _source(CONTACT_EFFECT_CHANNEL, provider_id="test.provider") + assert registry.resolve(source_v1) is first + assert registry.providers[("test.provider", "2")] is second + + with pytest.raises(ValueError, match="Duplicate"): + EffectEvidenceProviderRegistry((first, _WrongTimestampProvider())) + with pytest.raises(KeyError, match="exact versions"): + registry.resolve( + EffectEvidenceSourceRef( + "test.provider", + "missing", + ControlPartEvidenceAddress("arm", CONTACT_EFFECT_CHANNEL), + ) + ) + + +def test_collector_rejects_provider_metadata_drift() -> None: + spec = _attach_spec(_binary_clause(provider_id="test.provider")) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((_WrongTimestampProvider(),)) + ) + + with pytest.raises(ValueError, match="collection timestamp"): + collector.collect(spec, timestamp=2.0, observation_revision=4) + + +def test_control_part_provider_collects_pose_and_joint_state_once() -> None: + object_poses = torch.eye(4).repeat(2, 1, 1) + object_poses[:, 0, 3] = torch.tensor([0.25, 0.5]) + robot = _FakeRobot(torch.tensor([[0.75, 1.0], [1.5, 2.0]])) + scene = _FakeSceneProvider(object_poses) + joint_clause = JointStateEffectClause( + "joints", + "held", + _source(JOINT_STATE_EFFECT_CHANNEL), + torch.tensor([0.0, 0.0]), + ) + spec = _attach_spec(_pose_clause(), joint_clause) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (ControlPartSimulationEvidenceProvider(robot, scene_provider=scene),) + ) + ) + + evidence = collector.collect(spec, timestamp=3.5, observation_revision=11) + + assert list(evidence) == ["pose", "joints"] + assert evidence["pose"].timestamp == 3.5 + assert evidence["joints"].observation_revision == 11 + assert torch.allclose( + evidence["pose"].object_to_endpoint[:, 0, 3], + torch.tensor([0.5, 1.0]), + ) + assert torch.equal(evidence["joints"].positions, robot.qpos) + assert torch.equal(evidence["joints"].velocities, robot.qvel) + assert scene.calls == 1 + assert robot.qpos_calls == 1 + assert robot.qvel_calls == 1 + assert robot.fk_calls == 1 + + +def test_control_part_provider_selects_requested_simulator_rows() -> None: + env_ids = torch.tensor([2, 0], dtype=torch.long) + object_poses = torch.eye(4).repeat(3, 1, 1) + robot = _FakeRobot(torch.tensor([[1.0], [2.0], [3.0]])) + scene = _FakeSceneProvider(object_poses) + spec = _attach_spec(_pose_clause(), env_ids=env_ids) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (ControlPartSimulationEvidenceProvider(robot, scene_provider=scene),) + ) + ) + + evidence = collector.collect(spec, timestamp=0.0, observation_revision=0) + + assert evidence["pose"].env_ids.tolist() == [2, 0] + assert evidence["pose"].object_to_endpoint[:, 0, 3].tolist() == [3.0, 1.0] + assert scene.received_env_ids is not None + assert scene.received_env_ids.tolist() == [2, 0] + + +def test_pose_queries_share_one_scene_and_fk_snapshot() -> None: + robot = _FakeRobot(torch.tensor([[0.0], [0.0]])) + scene = _FakeSceneProvider(torch.eye(4).repeat(2, 1, 1)) + spec = _attach_spec(_pose_clause("first"), _pose_clause("second")) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (ControlPartSimulationEvidenceProvider(robot, scene_provider=scene),) + ) + ) + + evidence = collector.collect(spec, timestamp=1.0, observation_revision=1) + + assert set(evidence) == {"first", "second"} + assert scene.calls == 1 + assert robot.fk_calls == 1 + assert robot.qpos_calls == 1 + + +def test_missing_backend_specific_callbacks_return_explicit_invalid_rows() -> None: + robot = _FakeRobot(torch.zeros((2, 1))) + spec = _attach_spec(_binary_clause(), _scalar_clause()) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((ControlPartSimulationEvidenceProvider(robot),)) + ) + + evidence = collector.collect(spec, timestamp=1.0, observation_revision=2) + + assert not evidence["contact"].valid.any() + assert not evidence["force"].valid.any() + assert all("callback" in error for error in evidence["contact"].acquisition_errors) + assert all("callback" in error for error in evidence["force"].acquisition_errors) + + +def test_callbacks_receive_owned_queries_and_propagate_row_validity() -> None: + robot = _FakeRobot(torch.zeros((2, 1))) + binary_values = torch.tensor([True, False]) + scalar_values = torch.tensor([3.0, 0.0]) + received_query: BinaryEffectEvidenceQuery | None = None + + def observe_contact( + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectObservation: + nonlocal received_query + received_query = query + assert context.env_ids.tolist() == [0, 1] + return BinaryEffectObservation( + binary_values, + torch.tensor([True, False]), + (None, "contact sensor unavailable"), + ) + + def observe_force( + query: ScalarEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> ScalarEffectObservation: + del query, context + return ScalarEffectObservation(scalar_values) + + provider = ControlPartSimulationEvidenceProvider( + robot, + contact_observer=observe_contact, + force_observer=observe_force, + ) + collector = EffectEvidenceCollector(EffectEvidenceProviderRegistry((provider,))) + + evidence = collector.collect( + _attach_spec(_binary_clause(), _scalar_clause()), + timestamp=1.0, + observation_revision=2, + ) + binary_values[:] = False + scalar_values[:] = 99.0 + + assert received_query is not None + assert received_query.evidence_id == "contact" + assert evidence["contact"].values.tolist() == [True, False] + assert evidence["contact"].valid.tolist() == [True, False] + assert evidence["force"].values.tolist() == [3.0, 0.0] + + +def test_pose_without_scene_provider_is_invalid_not_fabricated() -> None: + robot = _FakeRobot(torch.zeros((2, 1))) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((ControlPartSimulationEvidenceProvider(robot),)) + ) + + evidence = collector.collect( + _attach_spec(_pose_clause()), + timestamp=0.0, + observation_revision=0, + ) + + assert not evidence["pose"].valid.any() + assert all( + "scene provider" in error for error in evidence["pose"].acquisition_errors + ) + assert robot.fk_calls == 0 + + +def test_channel_mismatch_fails_before_callback() -> None: + wrong_clause = BinaryEffectClause( + "contact", + "held", + _source(CONSTRAINT_EFFECT_CHANNEL), + BinaryEvidenceKind.CONTACT, + True, + ) + robot = _FakeRobot(torch.zeros((2, 1))) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((ControlPartSimulationEvidenceProvider(robot),)) + ) + + with pytest.raises(ValueError, match="requires channel"): + collector.collect( + _attach_spec(wrong_clause), + timestamp=0.0, + observation_revision=0, + ) + + +def test_joint_query_type_is_built_for_articulation_expectation() -> None: + expectation = ArticulationJointStateExpectation( + "drawer_joint", + "drawer", + "slide", + torch.tensor([0.4]), + ) + clause = JointStateEffectClause( + "joint", + "drawer_joint", + _source(JOINT_STATE_EFFECT_CHANNEL), + torch.tensor([0.4]), + ) + spec = SemanticEffectSpec( + semantic_id="open:drawer", + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id="OperateArticulation", + invocation_id="open-1", + invocation_revision=0, + env_ids=torch.tensor([0]), + state_expectations=(expectation,), + clauses=(clause,), + ) + + query = build_effect_evidence_queries(spec)[0] + + assert isinstance(query, JointStateEvidenceQuery) + assert query.expectation.articulation_id == "drawer" + + +def _articulation_spec( + *clauses: JointStateEffectClause, + expectation_joint: str = "slide", +) -> SemanticEffectSpec: + expectation = ArticulationJointStateExpectation( + "drawer_joint", + "drawer", + expectation_joint, + torch.tensor([[0.4], [0.4]]), + ) + return SemanticEffectSpec( + semantic_id="open:drawer", + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id="OperateArticulation", + invocation_id="open-1", + invocation_revision=0, + env_ids=torch.tensor([0, 1]), + state_expectations=(expectation,), + clauses=clauses, + ) + + +def _articulation_clause(clause_id: str = "joint") -> JointStateEffectClause: + return JointStateEffectClause( + clause_id, + "drawer_joint", + EffectEvidenceSourceRef( + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress("drawer", "slide"), + ), + torch.tensor([[0.4], [0.4]]), + ) + + +def test_scene_articulation_provider_uses_explicit_typed_observer() -> None: + calls = 0 + + def observe_joint( + query: JointStateEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> JointStateObservation: + nonlocal calls + calls += 1 + address = query.source.address + assert isinstance(address, ArticulationJointEvidenceAddress) + assert (address.articulation_id, address.joint_id) == ("drawer", "slide") + assert context.observation_revision == 8 + return JointStateObservation( + positions=torch.tensor([[0.4], [0.3]]), + velocities=torch.tensor([[0.0], [0.1]]), + valid=torch.tensor([True, False]), + acquisition_errors=(None, "joint sensor unavailable"), + ) + + provider = SceneArticulationEvidenceProvider(observe_joint) + collector = EffectEvidenceCollector(EffectEvidenceProviderRegistry((provider,))) + + evidence = collector.collect( + _articulation_spec(_articulation_clause()), + timestamp=4.0, + observation_revision=8, + ) + + assert calls == 1 + assert torch.allclose( + evidence["joint"].positions, + torch.tensor([[0.4], [0.3]]), + ) + assert evidence["joint"].valid.tolist() == [True, False] + assert evidence["joint"].acquisition_errors == ( + None, + "joint sensor unavailable", + ) + + +def test_scene_articulation_provider_reads_typed_scene_snapshot_once() -> None: + class _JointSceneProvider: + calls = 0 + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + self.calls += 1 + return SceneSnapshot( + timestamp=timestamp, + version=self.calls, + articulation_joints={ + ("drawer", "slide"): ObservedArticulationJointState( + torch.tensor([[0.4], [0.3]]), + torch.tensor([True, False]), + ) + }, + ) + + scene_provider = _JointSceneProvider() + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (SceneArticulationEvidenceProvider(scene_provider=scene_provider),) + ) + ) + + evidence = collector.collect( + _articulation_spec( + _articulation_clause("position"), + _articulation_clause("settled_position"), + ), + timestamp=2.0, + observation_revision=5, + ) + + assert scene_provider.calls == 1 + assert torch.equal(evidence["position"].positions, torch.tensor([[0.4], [0.3]])) + assert evidence["settled_position"].valid.tolist() == [True, False] + + +def test_scene_articulation_provider_samples_same_address_once() -> None: + calls = 0 + + def observe_joint( + query: JointStateEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> JointStateObservation: + nonlocal calls + del query, context + calls += 1 + return JointStateObservation(torch.tensor([[0.4], [0.4]])) + + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (SceneArticulationEvidenceProvider(observe_joint),) + ) + ) + spec = _articulation_spec( + _articulation_clause("position"), + _articulation_clause("settled_position"), + ) + + evidence = collector.collect(spec, timestamp=1.0, observation_revision=1) + + assert set(evidence) == {"position", "settled_position"} + assert calls == 1 + + +def test_scene_articulation_provider_rejects_address_expectation_drift() -> None: + provider = SceneArticulationEvidenceProvider( + lambda query, context: JointStateObservation(torch.tensor([[0.4], [0.4]])) + ) + collector = EffectEvidenceCollector(EffectEvidenceProviderRegistry((provider,))) + + with pytest.raises(ValueError, match="exactly match"): + collector.collect( + _articulation_spec( + _articulation_clause(), + expectation_joint="other_joint", + ), + timestamp=1.0, + observation_revision=1, + ) diff --git a/tests/sim/skills/test_integration.py b/tests/sim/skills/test_integration.py index ffa790bce..8a32e3bfd 100644 --- a/tests/sim/skills/test_integration.py +++ b/tests/sim/skills/test_integration.py @@ -30,9 +30,11 @@ BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, ControlPartCommandProfile, + DynamicCollisionMode, EntityState, FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, + MotionPolicy, ) from embodichain.lab.sim.skills.calls import ( Pick, @@ -41,8 +43,10 @@ SemanticCallDescriptor, builtin_semantic_call_catalog, ) +from embodichain.lab.sim.skills.effects import EffectMonitorRef from embodichain.lab.sim.skills.integration import ( BoundSemanticCall, + BoundSemanticIntegration, SceneEntityManifest, SceneManifest, SemanticIntegrationManifest, @@ -59,6 +63,8 @@ GRASP_AFFORDANCE_CAPABILITY, PLACE_ON_AFFORDANCE_CAPABILITY, SceneAffordanceRef, + SceneCollisionRole, + SceneCollisionWorldMode, SceneEntityRegistration, SceneObjectRef, SceneRegistry, @@ -102,9 +108,17 @@ def __deepcopy__(self, memo: dict[int, object]) -> _CopyTrackedAffordance: return _CopyTrackedAffordance() +class _GeometryProvider: + """Return one opaque planner-facing geometry descriptor.""" + + def get_geometry(self) -> object: + return object() + + def _scene_registry( *, with_default: bool, + dynamic_collision: bool = False, ) -> tuple[SceneRegistry, _NeverObservedStateProvider]: provider = _NeverObservedStateProvider() object_ref = SceneObjectRef("cube") @@ -118,6 +132,12 @@ def _scene_registry( state_provider=provider, aliases=("sim_cube",), default_affordances=defaults, + geometry_provider=(_GeometryProvider() if dynamic_collision else None), + collision_role=( + SceneCollisionRole.DYNAMIC + if dynamic_collision + else SceneCollisionRole.NONE + ), ), SceneEntityRegistration( ref=side_grasp, @@ -142,14 +162,31 @@ def _scene_registry( affordance_revision="grasp-v1", relative_pose=torch.eye(4), ), - ) + ), + collision_world_mode=( + SceneCollisionWorldMode.PER_ENV if dynamic_collision else None + ), ) return registry, provider def _semantic_integration( registry: SceneRegistry, + *, + preset: SkillPolicyPreset | None = None, + additional_presets: tuple[SkillPolicyPreset, ...] = (), + default_preset: str | None = None, + skill_presets: dict[str, str] | None = None, + runtime_preset: str | None = None, ) -> SemanticIntegrationManifest: + selected_preset = SkillPolicyPreset("safe") if preset is None else preset + presets = {selected_preset.preset_id: selected_preset} + presets.update( + { + additional_preset.preset_id: additional_preset + for additional_preset in additional_presets + } + ) robot_profile = RobotSkillProfile( profile_id="test_robot", resources={ @@ -173,18 +210,24 @@ def _semantic_integration( grasp=torch.tensor([1.0]), ) }, - presets={"safe": SkillPolicyPreset("safe")}, - default_preset="safe", + presets=presets, + default_preset=( + selected_preset.preset_id if default_preset is None else default_preset + ), + skill_presets={} if skill_presets is None else skill_presets, ) return SemanticIntegrationManifest( scene=SceneManifest.from_registry(registry), robot_profile=robot_profile, call_catalog=builtin_semantic_call_catalog(), + runtime_preset=runtime_preset, ) def _engine_for_integration( integration: SemanticIntegrationManifest, + *, + supports_dynamic_collision_world: bool = False, ) -> AtomicActionEngine: """Build a minimal live engine whose resource graph matches the manifest.""" robot = Mock() @@ -204,6 +247,7 @@ def _engine_for_integration( generator.robot = robot generator.device = torch.device("cpu") generator.planner.cfg.planner_type = "stub_planner" + generator.supports_dynamic_collision_world = supports_dynamic_collision_world return AtomicActionEngine( generator, skill_profile=integration.robot_profile, @@ -378,6 +422,36 @@ class LiveCatalog(SemanticCallCatalog): ) +def test_semantic_integration_rejects_monitor_for_unknown_call_with_path() -> None: + registry, _ = _scene_registry(with_default=True) + unknown_semantic_id = "not_catalogued" + + with pytest.raises(SemanticValidationError) as error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + effect_monitors={ + unknown_semantic_id: EffectMonitorRef("test.monitor", "1") + }, + ), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_effect_monitor_call" + assert diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "effect_monitors", + unknown_semantic_id, + ) + assert diagnostic.rendered_path == ( + "integration.robot_profile.presets.safe.effect_monitors.not_catalogued" + ) + + def test_scene_manifest_reports_structured_pathful_diagnostic() -> None: manifest = SceneManifest((SceneEntityManifest(ref=SceneObjectRef("cube")),)) @@ -506,6 +580,289 @@ def test_bound_semantic_call_is_factory_owned_by_installed_profile() -> None: BoundSemanticCall() +@pytest.mark.parametrize( + "source_mode", + [ + DynamicCollisionMode.AUTO, + DynamicCollisionMode.OFF, + DynamicCollisionMode.REQUIRED, + ], +) +def test_safe_preset_requires_dynamic_collision_for_dynamic_scene( + source_mode: DynamicCollisionMode, +) -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy( + strategy="motion_gen", + dynamic_collision_mode=source_mode, + ), + ), + ) + engine = _engine_for_integration( + integration, + supports_dynamic_collision_world=True, + ) + + bound = integration.bind(registry, engine).link_call( + Pick(object=SceneObjectRef("cube")) + ) + + assert ( + bound.preset.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + assert ( + integration.robot_profile.presets["safe"].motion_policy.dynamic_collision_mode + is source_mode + ) + assert provider.calls == 0 + + +def test_safe_preset_rejects_unsupported_dynamic_planner_before_observation() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ) + engine = _engine_for_integration(integration) + bind_skill_profile = Mock(wraps=engine.bind_skill_profile) + engine.bind_skill_profile = bind_skill_profile # type: ignore[method-assign] + + with pytest.raises(SemanticValidationError) as error: + integration.bind(registry, engine) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "safe_dynamic_collision_unsupported" + assert diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "motion_policy", + "dynamic_collision_mode", + ) + assert diagnostic.candidates == () + assert "('cube',)" in diagnostic.message + bind_skill_profile.assert_not_called() + assert provider.calls == 0 + + +def test_per_skill_safe_preset_is_conservatively_preflighted() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + pick_skill_id = builtin_semantic_call_catalog().descriptors["pick"].skill_id + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset("fast"), + additional_presets=( + SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ), + skill_presets={pick_skill_id: "safe"}, + ) + engine = _engine_for_integration(integration) + + with pytest.raises(SemanticValidationError) as error: + integration.bind(registry, engine) + + assert error.value.diagnostic.code == "safe_dynamic_collision_unsupported" + assert provider.calls == 0 + + +def test_fully_overridden_safe_default_is_not_reachable() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + catalog = builtin_semantic_call_catalog() + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + additional_presets=(SkillPolicyPreset("fast"),), + skill_presets={ + descriptor.skill_id: "fast" for descriptor in catalog.descriptors.values() + }, + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine) + + assert ( + bound.link_call(Pick(object=SceneObjectRef("cube"))).preset.preset_id == "fast" + ) + assert provider.calls == 0 + + +def test_runtime_non_safe_override_makes_safe_default_unreachable() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + additional_presets=(SkillPolicyPreset("fast"),), + runtime_preset="fast", + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine) + + assert ( + bound.link_call(Pick(object=SceneObjectRef("cube"))).preset.preset_id == "fast" + ) + assert provider.calls == 0 + + +def test_bound_integration_cannot_bypass_safe_dynamic_planner_preflight() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ) + engine = _engine_for_integration(integration) + bound_profile = engine.bind_skill_profile(integration.robot_profile) + + with pytest.raises(SemanticValidationError) as error: + BoundSemanticIntegration( + manifest=integration, + scene_registry=registry, + robot_profile=bound_profile, + engine=engine, + ) + + assert error.value.diagnostic.code == "safe_dynamic_collision_unsupported" + assert error.value.diagnostic.path[-1] == "dynamic_collision_mode" + assert provider.calls == 0 + + +def test_bind_rejects_invalid_engine_before_safe_capability_lookup() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ) + + with pytest.raises(TypeError, match="engine must be an AtomicActionEngine"): + integration.bind(registry, object()) # type: ignore[arg-type] + + assert provider.calls == 0 + + +def test_safe_preset_rejects_non_motion_generator_strategy_for_dynamic_scene() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(strategy="ik_interp"), + ), + ) + engine = _engine_for_integration( + integration, + supports_dynamic_collision_world=True, + ) + + with pytest.raises(SemanticValidationError) as error: + integration.bind(registry, engine) + + assert error.value.diagnostic.code == "safe_dynamic_collision_unsupported" + assert error.value.diagnostic.path[-1] == "strategy" + assert provider.calls == 0 + + +@pytest.mark.parametrize( + "source_mode", + [DynamicCollisionMode.AUTO, DynamicCollisionMode.OFF], +) +def test_non_safe_preset_preserves_dynamic_collision_policy( + source_mode: DynamicCollisionMode, +) -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "fast", + motion_policy=MotionPolicy(dynamic_collision_mode=source_mode), + ), + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine).link_call( + Pick(object=SceneObjectRef("cube")) + ) + + assert bound.preset.preset_id == "fast" + assert bound.preset.motion_policy.dynamic_collision_mode is source_mode + assert provider.calls == 0 + + +@pytest.mark.parametrize( + "source_mode", + [DynamicCollisionMode.AUTO, DynamicCollisionMode.OFF], +) +def test_safe_preset_preserves_policy_without_dynamic_collision( + source_mode: DynamicCollisionMode, +) -> None: + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + motion_policy=MotionPolicy(dynamic_collision_mode=source_mode), + ), + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine).link_call( + Pick(object=SceneObjectRef("cube")) + ) + + assert bound.preset.motion_policy.dynamic_collision_mode is source_mode + assert provider.calls == 0 + + def test_bound_semantic_integration_rejects_engine_profile_rebind() -> None: registry, _ = _scene_registry(with_default=True) integration = _semantic_integration(registry) diff --git a/tests/sim/skills/test_parallel.py b/tests/sim/skills/test_parallel.py new file mode 100644 index 000000000..9c6044288 --- /dev/null +++ b/tests/sim/skills/test_parallel.py @@ -0,0 +1,251 @@ +# ---------------------------------------------------------------------------- +# 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 deterministic parallel-skill contracts.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + EndpointCommand, + JointPositionPayload, + JointPositionTarget, + RuntimeCommandFrame, + StateDelta, + TaskState, + TimedCommandSequence, +) +from embodichain.lab.sim.skills.parallel import ( + ParallelBranchPlan, + ParallelConflictError, + ParallelStateConflictError, + ParallelTimingError, + ParallelTimingPolicy, + align_parallel_commands, + merge_parallel_effects, + resolve_parallel_barrier, +) +from embodichain.lab.sim.skills.profiles import ResourceClaim + +ENV_IDS = torch.tensor([3, 7], dtype=torch.long) + + +def _sequence( + control_part: str, + joint_id: int, + frame_count: int, + *, + duration: float = 0.1, +) -> TimedCommandSequence: + target = JointPositionTarget(control_part, (joint_id,)) + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target, + JointPositionPayload(torch.full((2, 1), float(index + joint_id))), + ), + ), + active_mask=torch.tensor([True, True]), + env_ids=ENV_IDS, + hold_duration=torch.full((2,), duration), + ) + for index in range(frame_count) + ) + return TimedCommandSequence(frames, ENV_IDS) + + +def _branch( + branch_id: str, + control_part: str, + joint_id: int, + frame_count: int, + *, + duration: float = 0.1, +) -> ParallelBranchPlan: + return ParallelBranchPlan( + branch_id=branch_id, + claim=ResourceClaim(frozenset({control_part}), (joint_id,)), + commands=_sequence( + control_part, + joint_id, + frame_count, + duration=duration, + ), + ) + + +def test_parallel_alignment_hold_pads_shorter_disjoint_branch() -> None: + merged = align_parallel_commands( + ( + _branch("left", "left_arm", 0, 2), + _branch("right", "right_arm", 1, 3), + ), + ParallelTimingPolicy(step_dt=0.1), + ) + + assert merged.frame_count == 3 + assert all(len(frame.commands) == 2 for frame in merged.frames) + left_final = merged.frames[-1].commands[0].payload + assert isinstance(left_final, JointPositionPayload) + assert torch.equal(left_final.positions, torch.full((2, 1), 1.0)) + assert torch.equal(merged.frames[-1].active_mask, torch.tensor([True, True])) + + +def test_parallel_alignment_rejects_claim_and_grid_conflicts() -> None: + with pytest.raises(ParallelConflictError, match="overlapping"): + align_parallel_commands( + ( + _branch("one", "arm", 0, 2), + _branch("two", "arm", 1, 2), + ), + ParallelTimingPolicy(0.1), + ) + + +def test_parallel_alignment_rejects_different_lane_active_masks() -> None: + left = _branch("left", "left", 0, 1) + right = _branch("right", "right", 1, 1) + right_frame = right.commands.frames[0].with_active_mask(torch.tensor([False, True])) + right = ParallelBranchPlan( + branch_id=right.branch_id, + claim=right.claim, + commands=TimedCommandSequence((right_frame,), ENV_IDS), + ) + + with pytest.raises(ParallelTimingError, match="active masks"): + align_parallel_commands( + (left, right), + ParallelTimingPolicy(0.1), + ) + + +def test_parallel_alignment_validates_inactive_row_durations_on_same_grid() -> None: + left = _branch("left", "left", 0, 1) + left_frame = RuntimeCommandFrame( + commands=left.commands.frames[0].commands, + active_mask=torch.tensor([False, True]), + env_ids=ENV_IDS, + hold_duration=torch.tensor([0.2, 0.1]), + ) + left = ParallelBranchPlan( + branch_id=left.branch_id, + claim=left.claim, + commands=TimedCommandSequence((left_frame,), ENV_IDS), + ) + right = _branch("right", "right", 1, 1) + right_frame = RuntimeCommandFrame( + commands=right.commands.frames[0].commands, + active_mask=torch.tensor([False, True]), + env_ids=ENV_IDS, + hold_duration=torch.tensor([0.1, 0.1]), + ) + right = ParallelBranchPlan( + branch_id=right.branch_id, + claim=right.claim, + commands=TimedCommandSequence((right_frame,), ENV_IDS), + ) + + with pytest.raises(ParallelTimingError, match="step_dt"): + align_parallel_commands((left, right), ParallelTimingPolicy(0.1)) + + +def test_parallel_alignment_rejects_off_grid_duration() -> None: + with pytest.raises(ParallelTimingError, match="step_dt"): + align_parallel_commands( + ( + _branch("left", "left", 0, 2, duration=0.05), + _branch("right", "right", 1, 2), + ), + ParallelTimingPolicy(0.1), + ) + + +def test_parallel_effects_merge_disjoint_keys_by_verified_row() -> None: + state = TaskState.empty(batch_size=2, device="cpu") + merged = merge_parallel_effects( + state, + { + "drawer": ( + StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + } + ), + torch.tensor([True, False]), + ), + "door": ( + StateDelta( + articulation_joint_updates={ + ("door", "hinge"): ArticulationJointState(torch.tensor([1.0])) + } + ), + torch.tensor([False, True]), + ), + }, + ) + + drawer = merged.get_articulation_joint_state("drawer", "slide") + door = merged.get_articulation_joint_state("door", "hinge") + assert drawer is not None and door is not None + assert torch.equal(drawer.env_mask, torch.tensor([True, False])) + assert torch.equal(door.env_mask, torch.tensor([False, True])) + + +def test_parallel_effects_reject_same_key_on_same_row() -> None: + delta = StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + } + ) + with pytest.raises(ParallelStateConflictError, match="same symbolic keys"): + merge_parallel_effects( + TaskState.empty(2, "cpu"), + { + "one": (delta, torch.tensor([True, False])), + "two": (delta, torch.tensor([True, True])), + }, + ) + + +def test_parallel_barrier_cancels_pending_siblings_per_failed_row() -> None: + update = resolve_parallel_barrier( + pending_masks={ + "left": torch.tensor([False, True, True]), + "right": torch.tensor([True, False, True]), + }, + success_masks={ + "left": torch.tensor([True, False, False]), + "right": torch.tensor([False, True, False]), + }, + failure_masks={ + "left": torch.tensor([False, False, True]), + "right": torch.tensor([False, False, False]), + }, + ) + + assert torch.equal(update.failure_mask, torch.tensor([False, False, True])) + assert torch.equal(update.completed_mask, torch.tensor([False, False, True])) + assert torch.equal( + update.cancellation_masks["right"], + torch.tensor([False, False, True]), + ) + + +__all__: list[str] = [] diff --git a/tests/sim/skills/test_parallel_runtime.py b/tests/sim/skills/test_parallel_runtime.py new file mode 100644 index 000000000..6d53cb1bf --- /dev/null +++ b/tests/sim/skills/test_parallel_runtime.py @@ -0,0 +1,1264 @@ +# ---------------------------------------------------------------------------- +# 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 branch-local semantic execution at a parallel barrier.""" + +from __future__ import annotations + +from dataclasses import dataclass +import json + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + CommandAcknowledgement, + EndpointCommand, + JointPositionPayload, + JointPositionTarget, + PlanningContext, + RobotObservation, + RuntimeCommandFrame, + SceneSnapshot, + StateDelta, + TaskState, +) +from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelLaneCommandSink, + ParallelRuntimeBranch, + ParallelSkillRuntime, +) +from embodichain.lab.sim.skills.profiles import ResourceClaim +from embodichain.lab.sim.skills.runtime import SkillResult, SkillStatus + +ENV_IDS = torch.tensor([4, 9], dtype=torch.long) + + +class _Clock: + """Deterministic environment-grid clock.""" + + def __init__(self) -> None: + self.time = 0.0 + + def now(self) -> float: + return self.time + + def sleep(self, duration: float) -> None: + self.time += duration + + +class _OutboundSink: + """Record the coordinator's one merged transport transaction.""" + + def __init__( + self, + *, + reject: bool = False, + reject_cancel: bool = False, + reject_hold: bool = False, + raise_send: bool = False, + ) -> None: + self.reject = reject + self.reject_cancel = reject_cancel + self.reject_hold = reject_hold + self.raise_send = raise_send + self.frames: list[RuntimeCommandFrame] = [] + self.hold_targets: list[tuple[str, ...]] = [] + self.hold_fingerprints: list[tuple[object, ...]] = [] + self.operations: list[str] = [] + self.holds = 0 + self.cancels = 0 + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + del timeout + if self.raise_send: + raise RuntimeError("send exploded") + self.operations.append("send") + self.frames.append(command.snapshot()) + if self.reject: + return CommandAcknowledgement.rejected_ack("test rejection") + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[object, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + del context, timeout + self.operations.append("hold") + self.holds += 1 + self.hold_targets.append( + tuple(getattr(target, "target_id") for target in targets) + ) + self.hold_fingerprints.append( + tuple(getattr(target, "address_fingerprint") for target in targets) + ) + if self.reject_hold: + return CommandAcknowledgement.rejected_ack("hold rejected") + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[object, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, timeout + self.operations.append("cancel") + self.cancels += 1 + if self.reject_cancel: + return CommandAcknowledgement.rejected_ack("cancel rejected") + return CommandAcknowledgement.accepted_ack() + + +class _AcceptSafety: + """Accept fake joint commands while recording validation calls.""" + + def __init__(self) -> None: + self.calls = 0 + + def validate( + self, + *, + branch_frames: dict[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + assert branch_frames + assert merged_frame.commands + self.calls += 1 + + +class _RejectSafety: + """Reject every synchronized motion as physically unsafe.""" + + def validate( + self, + *, + branch_frames: dict[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + del branch_frames, merged_frame + raise RuntimeError("predicted self collision") + + +@dataclass(frozen=True, slots=True) +class _ScriptStep: + """One fake lane cycle.""" + + status: SkillStatus + eligible: torch.Tensor + success: torch.Tensor + failure: torch.Tensor + cancelled: torch.Tensor + frame: RuntimeCommandFrame | None = None + task_state: TaskState | None = None + wait_duration: float = 0.0 + emit_hold: bool = False + hold_targets: tuple[JointPositionTarget, ...] = () + + +class _BranchRuntime: + """Small deterministic implementation of the parallel runtime protocol.""" + + def __init__( + self, + script: tuple[_ScriptStep, ...], + sink: ParallelLaneCommandSink, + *, + initial_state: TaskState | None = None, + emit_terminal_hold: bool = True, + ) -> None: + self._script = script + self._sink = sink + self._index = 0 + self._state = initial_state or TaskState.empty(2, "cpu") + self._emit_terminal_hold = emit_terminal_hold + self._result = self._make_result( + SkillStatus.IDLE, + eligible=torch.ones(2, dtype=torch.bool), + ) + + @property + def result(self) -> SkillResult: + return self._result + + @property + def step_count(self) -> int: + return self._index + + def start( + self, + *calls: RegisteredSemanticCall, + workflow_id: str, + eligible_mask: torch.Tensor | None = None, + ) -> SkillResult: + del calls + eligible = ( + torch.ones(2, dtype=torch.bool) + if eligible_mask is None + else eligible_mask.clone() + ) + self._result = self._make_result( + SkillStatus.RUNNING, + workflow_id=workflow_id, + eligible=eligible, + ) + return self._result + + def step(self) -> SkillResult: + scripted = self._script[min(self._index, len(self._script) - 1)] + self._index += 1 + if scripted.frame is not None: + self._sink.send(scripted.frame, timeout=1.0) + self._state = scripted.task_state or self._state + if scripted.hold_targets: + self._sink.hold( + scripted.hold_targets, + _context(self._state), + timeout=1.0, + ) + elif scripted.emit_hold or ( + scripted.status is not SkillStatus.RUNNING and self._emit_terminal_hold + ): + last_frame = scripted.frame or self._sink.last_frame + targets = () if last_frame is None else last_frame.targets + self._sink.hold(targets, _context(self._state), timeout=1.0) + self._result = self._make_result( + scripted.status, + workflow_id=self._result.workflow_id, + eligible=scripted.eligible & ~self._result.cancelled_mask, + success=scripted.success & ~self._result.cancelled_mask, + failure=scripted.failure, + cancelled=self._result.cancelled_mask | scripted.cancelled, + wait_duration=scripted.wait_duration, + ) + return self._result + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + del reason + changed = env_mask & self._result.eligible_mask + self._result = self._make_result( + self._result.status, + workflow_id=self._result.workflow_id, + eligible=self._result.eligible_mask & ~changed, + success=self._result.success_mask & ~changed, + failure=self._result.failure_mask, + cancelled=self._result.cancelled_mask | changed, + wait_duration=self._result.wait_duration, + ) + return self._result + + def cancel(self, reason: str) -> SkillResult: + del reason + active = self._result.eligible_mask & ~self._result.failure_mask + last_frame = self._sink.last_frame + targets = () if last_frame is None else last_frame.targets + self._sink.cancel(targets, timeout=1.0) + self._sink.hold(targets, _context(self._state), timeout=1.0) + self._result = self._make_result( + SkillStatus.CANCELLED, + workflow_id=self._result.workflow_id, + eligible=self._result.eligible_mask & ~active, + failure=self._result.failure_mask, + cancelled=self._result.cancelled_mask | active, + ) + return self._result + + def _make_result( + self, + status: SkillStatus, + *, + workflow_id: str | None = None, + eligible: torch.Tensor | None = None, + success: torch.Tensor | None = None, + failure: torch.Tensor | None = None, + cancelled: torch.Tensor | None = None, + wait_duration: float = 0.0, + ) -> SkillResult: + zeros = torch.zeros(2, dtype=torch.bool) + return SkillResult( + status=status, + workflow_id=workflow_id, + current_call_index=0 if status is SkillStatus.RUNNING else None, + env_ids=ENV_IDS, + success_mask=zeros if success is None else success, + failure_mask=zeros if failure is None else failure, + cancelled_mask=zeros if cancelled is None else cancelled, + eligible_mask=( + torch.ones(2, dtype=torch.bool) if eligible is None else eligible + ), + task_state=self._state, + wait_duration=wait_duration, + ) + + +def _mask(first: bool, second: bool) -> torch.Tensor: + return torch.tensor([first, second], dtype=torch.bool) + + +def _context(task_state: TaskState) -> PlanningContext: + return PlanningContext( + robot=RobotObservation( + timestamp=1.0, + qpos=torch.zeros(2, 3), + qvel=torch.zeros(2, 3), + ), + task=task_state, + scene=SceneSnapshot.empty(), + env_ids=ENV_IDS, + ) + + +def _frame(joint_id: int, values: tuple[float, float]) -> RuntimeCommandFrame: + target = JointPositionTarget(f"resource_{joint_id}", (joint_id,)) + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target, + JointPositionPayload(torch.tensor(values).reshape(2, 1)), + ), + ), + active_mask=_mask(True, True), + env_ids=ENV_IDS, + hold_duration=torch.full((2,), 0.1), + ) + + +def _branch( + branch_id: str, + joint_id: int, + script: tuple[_ScriptStep, ...], + *, + initial_state: TaskState | None = None, + emit_terminal_hold: bool = True, +) -> ParallelRuntimeBranch: + sink = ParallelLaneCommandSink() + return ParallelRuntimeBranch( + branch_id=branch_id, + calls=(RegisteredSemanticCall(f"test.{branch_id}"),), + claim=ResourceClaim(frozenset({f"resource_{joint_id}"}), (joint_id,)), + runtime=_BranchRuntime( + script, + sink, + initial_state=initial_state, + emit_terminal_hold=emit_terminal_hold, + ), + command_sink=sink, + ) + + +def _running_step( + *, + frame: RuntimeCommandFrame | None = None, + eligible: torch.Tensor | None = None, + failure: torch.Tensor | None = None, + task_state: TaskState | None = None, + wait_duration: float = 0.0, + emit_hold: bool = False, + hold_targets: tuple[JointPositionTarget, ...] = (), +) -> _ScriptStep: + return _ScriptStep( + SkillStatus.RUNNING, + _mask(True, True) if eligible is None else eligible, + _mask(False, False), + _mask(False, False) if failure is None else failure, + _mask(False, False), + frame, + task_state, + wait_duration=wait_duration, + emit_hold=emit_hold, + hold_targets=hold_targets, + ) + + +def _completed_step( + *, + frame: RuntimeCommandFrame | None = None, + success: torch.Tensor | None = None, + failure: torch.Tensor | None = None, + task_state: TaskState | None = None, +) -> _ScriptStep: + succeeded = _mask(True, True) if success is None else success + failed = _mask(False, False) if failure is None else failure + return _ScriptStep( + SkillStatus.COMPLETED, + succeeded, + succeeded, + failed, + _mask(False, False), + frame, + task_state, + ) + + +def test_parallel_runtime_merges_one_frame_and_hold_pads_short_lane() -> None: + left_state = TaskState.empty(2, "cpu") + left_state = StateDelta( + articulation_joint_updates={ + ("left_fixture", "joint"): ArticulationJointState(torch.full((2, 1), 0.5)) + } + ).apply(left_state, _mask(True, True)) + right_state = TaskState.empty(2, "cpu") + right_state = StateDelta( + articulation_joint_updates={ + ("right_fixture", "joint"): ArticulationJointState(torch.full((2, 1), 1.0)) + } + ).apply(right_state, _mask(True, True)) + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(task_state=left_state), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _running_step(frame=_frame(1, (3.0, 3.0))), + _completed_step(task_state=right_state), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=8, + ) + + result = runtime.start() + assert result.status is SkillStatus.RUNNING + result = runtime.step() + assert len(outbound.frames) == 1 + assert len(outbound.frames[0].commands) == 2 + assert outbound.operations == ["send"] + assert isinstance(left.runtime, _BranchRuntime) + assert isinstance(right.runtime, _BranchRuntime) + first_lane_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["send"] + assert (left.runtime.step_count, right.runtime.step_count) == first_lane_steps + + clock.time = 0.1 + result = runtime.step() + assert result.status is SkillStatus.RUNNING + assert outbound.operations == ["send", "hold"] + assert len(outbound.frames) == 1 + branch_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["send", "hold"] + assert (left.runtime.step_count, right.runtime.step_count) == branch_steps + + clock.time = 0.2 + result = runtime.step() + assert result.status is SkillStatus.RUNNING + assert outbound.operations == ["send", "hold", "send"] + assert len(outbound.frames[1].commands) == 1 + assert outbound.frames[1].commands[0].target.target_id == "resource_1" + assert (left.runtime.step_count, right.runtime.step_count) == branch_steps + + clock.time = 0.3 + result = runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert result.command_count == 2 + assert outbound.holds == 2 + assert outbound.operations == ["send", "hold", "send", "hold"] + assert ( + result.task_state.get_articulation_joint_state("left_fixture", "joint") + is not None + ) + assert ( + result.task_state.get_articulation_joint_state("right_fixture", "joint") + is not None + ) + metadata = result.to_metadata() + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["kind"] == "parallel_skill_result" + assert list(metadata["branches"]) == ["left", "right"] + assert metadata["elapsed_steps"] == 3 + + +def test_deferred_command_waits_for_clock_after_padding_hold() -> None: + left = _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ) + right = _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + padded = runtime.step() + lane_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + + assert padded.status is SkillStatus.RUNNING + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["hold"] + assert not outbound.frames + assert (left.runtime.step_count, right.runtime.step_count) == lane_steps + + clock.time = 0.1 + runtime.step() + + assert outbound.operations == ["hold", "send"] + assert len(outbound.frames) == 1 + assert (left.runtime.step_count, right.runtime.step_count) == lane_steps + + +def test_completion_hold_waits_for_clock_after_accepted_command() -> None: + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _completed_step(), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + lane_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + + assert same_tick.status is SkillStatus.RUNNING + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["send"] + assert (left.runtime.step_count, right.runtime.step_count) == lane_steps + + clock.time = 0.1 + completed = runtime.step() + + assert completed.status is SkillStatus.COMPLETED + assert outbound.operations == ["send", "hold"] + + +def test_parallel_runtime_fail_fast_is_row_local() -> None: + left = _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + eligible=_mask(False, True), + failure=_mask(True, False), + ), + _completed_step( + success=_mask(False, True), + failure=_mask(True, False), + ), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (3.0, 3.0))), + _completed_step( + success=_mask(False, True), + ), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + first = runtime.step() + + assert torch.equal(first.failure_mask, _mask(True, False)) + assert torch.equal( + first.branch_results["right"].cancelled_mask, + _mask(True, False), + ) + assert torch.equal(outbound.frames[0].active_mask, _mask(False, True)) + + clock.time = 0.1 + result = runtime.step() + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, False)) + assert torch.equal(result.success_mask, _mask(False, True)) + + +def test_parallel_failure_without_fresh_peer_frame_forces_masked_dispatch() -> None: + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _running_step( + eligible=_mask(False, True), + failure=_mask(True, False), + ), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _running_step(wait_duration=0.1), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + assert torch.equal(outbound.frames[-1].active_mask, _mask(True, True)) + + # The failure update has no fresh frame from either lane. The coordinator + # still replays the last transaction with row 0 inactive. + clock.time = 0.1 + runtime.step() + assert len(outbound.frames) == 2 + assert torch.equal(outbound.frames[-1].active_mask, _mask(False, True)) + + +def test_parallel_timeout_counts_completed_environment_steps() -> None: + left = _branch("left", 0, (_running_step(wait_duration=0.1),)) + right = _branch("right", 1, (_running_step(wait_duration=0.1),)) + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + _OutboundSink(), + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=1, + ) + + runtime.start() + before_step = runtime.step() + assert before_step.status is SkillStatus.RUNNING + assert before_step.elapsed_steps == 0 + + clock.time = 0.1 + timed_out = runtime.step() + assert timed_out.status is SkillStatus.FAILED + assert timed_out.elapsed_steps == 1 + assert torch.equal(timed_out.failure_mask, _mask(True, True)) + + +def test_parallel_timeout_does_not_execute_deadline_tick() -> None: + left_runtime_steps = ( + _running_step(frame=_frame(0, (1.0, 1.0)), wait_duration=0.1), + _running_step(frame=_frame(0, (2.0, 2.0))), + ) + right_runtime_steps = ( + _running_step(frame=_frame(1, (3.0, 3.0)), wait_duration=0.1), + _running_step(frame=_frame(1, (4.0, 4.0))), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, left_runtime_steps), + _branch("right", 1, right_runtime_steps), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=1, + ) + + runtime.start() + runtime.step() + assert len(outbound.frames) == 1 + clock.time = 0.1 + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert len(outbound.frames) == 1 + assert outbound.cancels == 1 + assert outbound.holds == 1 + + +def test_parallel_timeout_discards_frame_deferred_behind_completion_hold() -> None: + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ), + _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=1, + ) + + runtime.start() + padded = runtime.step() + assert padded.status is SkillStatus.RUNNING + assert outbound.operations == ["hold"] + assert not outbound.frames + + clock.time = 0.1 + timed_out = runtime.step() + + assert timed_out.status is SkillStatus.FAILED + assert torch.equal(timed_out.failure_mask, _mask(True, True)) + assert not timed_out.success_mask.any() + assert not outbound.frames + assert outbound.operations == ["hold", "cancel", "hold"] + + +def test_parallel_cancel_discards_deferred_frame_and_covers_started_rows() -> None: + outbound = _OutboundSink() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ), + _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + cancelled = runtime.cancel("operator stop during padding") + + assert cancelled.status is SkillStatus.CANCELLED + assert torch.equal(cancelled.cancelled_mask, _mask(True, True)) + assert not cancelled.success_mask.any() + assert not cancelled.failure_mask.any() + assert not outbound.frames + assert outbound.operations == ["hold", "cancel", "hold"] + + +def test_deferred_frame_validation_failure_does_not_advance_lanes() -> None: + outbound = _OutboundSink() + clock = _Clock() + left = _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ) + right = _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ) + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _RejectSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + assert isinstance(left.runtime, _BranchRuntime) + assert isinstance(right.runtime, _BranchRuntime) + steps_before_dispatch = (left.runtime.step_count, right.runtime.step_count) + + clock.time = 0.1 + failed = runtime.step() + + assert failed.status is SkillStatus.FAILED + assert (left.runtime.step_count, right.runtime.step_count) == steps_before_dispatch + assert not outbound.frames + assert outbound.operations == ["hold", "cancel", "hold"] + + +def test_terminal_fresh_frames_fail_closed_without_post_command_observation() -> None: + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + (_completed_step(frame=_frame(0, (1.0, 1.0))),), + ), + _branch( + "right", + 1, + (_completed_step(frame=_frame(1, (2.0, 2.0))),), + ), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.success_mask.any() + assert not outbound.frames + assert outbound.operations == ["cancel", "hold"] + assert "post-command observation" in (result.message or "") + + +def test_hold_aggregation_preserves_same_destination_distinct_fingerprints() -> None: + target_a = JointPositionTarget("shared_arm", (0,)) + target_b = JointPositionTarget("shared_arm", (1,)) + lane_sink = ParallelLaneCommandSink() + lane_sink.hold( + (target_a, target_b), + _context(TaskState.empty(2, "cpu")), + timeout=1.0, + ) + pending_targets, _ = lane_sink.hold_request + assert tuple(target.address_fingerprint for target in pending_targets) == ( + target_a.address_fingerprint, + target_b.address_fingerprint, + ) + + outbound = _OutboundSink() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + (_running_step(hold_targets=(target_a, target_b)),), + ), + _branch("right", 2, (_running_step(wait_duration=0.1),)), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + + assert outbound.hold_targets == [("shared_arm", "shared_arm")] + assert outbound.hold_fingerprints == [ + (target_a.address_fingerprint, target_b.address_fingerprint) + ] + + +def test_parallel_lane_does_not_drop_prior_call_completion_hold() -> None: + left_sink = ParallelLaneCommandSink() + left = ParallelRuntimeBranch( + branch_id="left", + calls=( + RegisteredSemanticCall("test.left_first"), + RegisteredSemanticCall("test.left_second"), + ), + claim=ResourceClaim(frozenset({"left"}), (0, 2)), + runtime=_BranchRuntime( + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + _running_step(frame=_frame(2, (2.0, 2.0))), + _completed_step(), + ), + left_sink, + ), + command_sink=left_sink, + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (3.0, 3.0))), + _running_step(frame=_frame(1, (4.0, 4.0))), + _completed_step(), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + result = runtime.result + for step_index in range(1, 8): + if result.terminal: + break + clock.time = step_index * 0.1 + result = runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert any("resource_0" in targets for targets in outbound.hold_targets) + assert any("resource_2" in targets for targets in outbound.hold_targets) + + +def test_parallel_runtime_rejects_overlapping_claims_before_start() -> None: + script = (_running_step(),) + left = _branch("left", 0, script) + right_sink = ParallelLaneCommandSink() + right = ParallelRuntimeBranch( + branch_id="right", + calls=(RegisteredSemanticCall("test.right"),), + claim=ResourceClaim(frozenset({"different_name"}), (0,)), + runtime=_BranchRuntime(script, right_sink), + command_sink=right_sink, + ) + + with pytest.raises(ValueError, match="overlapping resource claims"): + ParallelSkillRuntime( + (left, right), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + +def test_parallel_runtime_requires_equal_branch_barrier_state() -> None: + changed = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(TaskState.empty(2, "cpu"), _mask(True, True)) + + with pytest.raises(ValueError, match="same verified TaskState"): + ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(),)), + _branch( + "right", + 1, + (_running_step(),), + initial_state=changed, + ), + ), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + +def test_terminal_targets_without_hold_context_fail_closed() -> None: + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ), + _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ), + ), + _OutboundSink(), + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + result = runtime.step() + assert result.status is SkillStatus.RUNNING + clock.time = 0.1 + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert "no synchronized planning context" in (result.message or "") + + +@pytest.mark.parametrize( + ("sink_kwargs", "expected_status"), + [ + ({}, SkillStatus.CANCELLED), + ({"reject_cancel": True}, SkillStatus.FAILED), + ({"reject_hold": True}, SkillStatus.FAILED), + ], +) +def test_parallel_caller_cancel_checks_cancel_and_hold_acknowledgements( + sink_kwargs: dict[str, bool], + expected_status: SkillStatus, +) -> None: + outbound = _OutboundSink(**sink_kwargs) + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + runtime.start() + runtime.step() + + result = runtime.cancel("operator stop") + + assert result.status is expected_status + assert outbound.cancels == 1 + assert outbound.holds >= 1 + if expected_status is SkillStatus.CANCELLED: + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert not result.failure_mask.any() + else: + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.cancelled_mask.any() + + +@pytest.mark.parametrize( + ("safety", "sink"), + [ + (_RejectSafety(), _OutboundSink()), + (_AcceptSafety(), _OutboundSink(raise_send=True)), + ], +) +def test_parallel_tick_exception_safe_stops_with_disjoint_failure_masks( + safety: object, + sink: _OutboundSink, +) -> None: + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + sink, + _Clock(), + ParallelTimingPolicy(0.1), + safety, + timeout_steps=5, + ) + runtime.start() + + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.success_mask.any() + assert not result.cancelled_mask.any() + assert sink.cancels == 1 + assert sink.holds == 1 + + +def test_cancel_preserves_verified_state_from_an_earlier_branch_call() -> None: + changed = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(TaskState.empty(2, "cpu"), _mask(True, True)) + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + task_state=changed, + ), + ), + ), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + runtime.start() + runtime.step() + + result = runtime.cancel() + + assert result.status is SkillStatus.CANCELLED + assert ( + result.task_state.get_articulation_joint_state("fixture", "joint") is not None + ) + + +def test_disjoint_intrinsic_rows_still_conflict_on_same_unpartitioned_key() -> None: + initial = TaskState.empty(2, "cpu") + left_state = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(initial, _mask(True, False)) + right_state = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.full((2, 1), 2.0)) + } + ).apply(initial, _mask(False, True)) + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + (_running_step(frame=_frame(0, (1.0, 1.0)), task_state=left_state),), + ), + _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0)), task_state=right_state),), + ), + ), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + runtime.start() + runtime.step() + + result = runtime.cancel() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.cancelled_mask.any() + + +__all__: list[str] = [] diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index ac31f550e..f50666d3b 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -57,9 +57,19 @@ from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.skills import ( AmbiguousSkillBindingError, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CONSTRAINT_EFFECT_CHANNEL, + CONTACT_EFFECT_CHANNEL, ControlPartEndpoint, ControlPartEndpointAdapter, + ControlPartEvidenceAddress, + EffectEvidenceSourceRef, + EffectMonitorRef, EndpointResolution, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + POSE_RELATION_EFFECT_CHANNEL, ProfileValidationError, ResourceBinding, ResourceEndpoint, @@ -477,6 +487,32 @@ def test_endpoint_resolution_owns_runtime_target_snapshot() -> None: assert resolution.runtime_target.aliases == ["base"] +def test_endpoint_resolution_owns_and_freezes_effect_sources() -> None: + source = EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("left_arm", POSE_RELATION_EFFECT_CHANNEL), + ) + sources = {POSE_RELATION_EFFECT_CHANNEL: source} + + resolution = EndpointResolution( + runtime_target=_BaseVelocityTarget("base_controller"), + task_state_key="mobile_actor", + effect_sources=sources, + exclusive=False, + ) + sources.clear() + + assert resolution.task_state_key == "mobile_actor" + assert tuple(resolution.effect_sources) == (POSE_RELATION_EFFECT_CHANNEL,) + assert resolution.effect_sources[POSE_RELATION_EFFECT_CHANNEL] is not source + assert resolution.effect_sources[POSE_RELATION_EFFECT_CHANNEL].address == ( + ControlPartEvidenceAddress("left_arm", POSE_RELATION_EFFECT_CHANNEL) + ) + with pytest.raises(TypeError): + resolution.effect_sources["new"] = source # type: ignore[index] + + @pytest.mark.parametrize("returns_self", [False, True]) def test_endpoint_resolution_rejects_invalid_target_snapshot( returns_self: bool, @@ -1112,6 +1148,28 @@ def test_unique_capability_binding_lowers_to_exact_action_binding() -> None: ) assert motion.require_target(JointPositionTarget).control_part == "left_arm" assert grasp.require_target(JointPositionTarget).control_part == "left_hand" + assert motion.task_state_key == "left_actor" + assert grasp.task_state_key == "left_actor" + resource = resolved.resources["primary"] + motion_sources = resource.endpoints["motion"].effect_sources + grasp_sources = resource.endpoints["grasp"].effect_sources + assert set(motion_sources) == { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } + assert set(grasp_sources) == { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + } + assert motion_sources[POSE_RELATION_EFFECT_CHANNEL].address == ( + ControlPartEvidenceAddress("left_arm", POSE_RELATION_EFFECT_CHANNEL) + ) + assert grasp_sources[CONSTRAINT_EFFECT_CHANNEL].address == ( + ControlPartEvidenceAddress("left_hand", CONSTRAINT_EFFECT_CHANNEL) + ) assert resolved.claim.leaf_resource_ids == frozenset({"left_arm", "left_hand"}) assert resolved.claim.joint_ids == (0, 1, 2) @@ -1346,6 +1404,59 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: incompatible.bind(_engine(control_profiles=_command_profiles())) +def test_policy_preset_defaults_exact_builtin_effect_monitor_refs() -> None: + preset = SkillPolicyPreset("safe") + + assert set(preset.effect_monitors) == { + "pick", + "place", + "hand_over", + "operate_articulation", + } + for monitor_ref in preset.effect_monitors.values(): + assert monitor_ref.monitor_id == COMPOSITE_EFFECT_MONITOR_ID + assert monitor_ref.revision == COMPOSITE_EFFECT_MONITOR_REVISION + assert dict(monitor_ref.params) == {} + + +def test_policy_preset_distinguishes_explicit_empty_effect_monitor_mapping() -> None: + preset = SkillPolicyPreset("unmonitored", effect_monitors={}) + + assert dict(preset.effect_monitors) == {} + assert dict(preset.snapshot().effect_monitors) == {} + + +def test_policy_preset_owns_and_snapshots_effect_monitor_refs() -> None: + source_params = { + "consecutive_samples": 3, + "metadata": ["strict", {"source": "profile"}], + } + source_ref = EffectMonitorRef("test.monitor", "2", source_params) + source_mapping = {"pick": source_ref} + preset = SkillPolicyPreset("custom", effect_monitors=source_mapping) + + source_params["consecutive_samples"] = 99 + source_params["metadata"][1]["source"] = "mutated" # type: ignore[index] + source_mapping["pick"] = EffectMonitorRef("replacement", "1") + first = preset.effect_monitors + snapshot = preset.snapshot() + second = snapshot.effect_monitors + + assert first["pick"] is not source_ref + assert first["pick"].monitor_id == "test.monitor" + assert first["pick"].params["consecutive_samples"] == 3 + assert first["pick"].params["metadata"] == ( + "strict", + {"source": "profile"}, + ) + assert second["pick"] is not first["pick"] + assert second["pick"].params == first["pick"].params + with pytest.raises(TypeError): + first["place"] = source_ref # type: ignore[index] + with pytest.raises(TypeError): + first["pick"].params["consecutive_samples"] = 4 # type: ignore[index] + + def test_profile_owns_named_grounding_provider_selections() -> None: selections = {"hand_over": "dual_center"} profile = RobotSkillProfile( diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py new file mode 100644 index 000000000..098f0d7cf --- /dev/null +++ b/tests/sim/skills/test_runtime.py @@ -0,0 +1,910 @@ +# ---------------------------------------------------------------------------- +# 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 canonical semantic-skill execution and its public facade.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import json +from types import MethodType, SimpleNamespace +from typing import ClassVar +from unittest.mock import Mock + +import pytest +import torch + +import embodichain.lab.sim.skills.runtime as runtime_module +from embodichain.lab.sim.atomic_actions import ( + ActionInvocation, + ActionOptions, + ActionPlan, + ArticulationJointState, + AtomicAction, + AtomicActionEngine, + CommandAcknowledgement, + EffectVerificationRequirement, + EffectVerificationRequest, + EndpointBinding, + JointPositionTarget, + MotionPolicy, + PlanningContext, + RecoveryPolicy, + ResolvedActionRequest, + RobotObservation, + SceneSnapshot, + SkillBindingContract, + StateDelta, + TaskState, + TimedCommandSequence, +) +from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + ControlPartEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectMonitor, + EffectMonitorDecision, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + SemanticEffectKind, + SemanticEffectSpec, +) +from embodichain.lab.sim.skills.runtime import ( + AtomicSkills, + SkillEndpointBindingTrace, + SkillRuntime, + SkillStatus, +) +from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy +from embodichain.lab.sim.skills.parallel_runtime import ParallelSkillRuntime +from embodichain.lab.sim.skills.profiles import ResourceClaim +from embodichain.lab.sim.skills.scene import SceneRegistry + +BATCH_SIZE = 2 + + +class _Clock: + """Deterministic execution clock.""" + + def __init__(self) -> None: + self.time = 0.0 + self.sleeps: list[float] = [] + + def now(self) -> float: + return self.time + + def sleep(self, duration: float) -> None: + self.sleeps.append(duration) + self.time += duration + + +class _ObservationProvider: + """Return a new timestamped context on every external observation.""" + + def __init__(self) -> None: + self.calls = 0 + self.task_states: list[TaskState] = [] + + def observe(self, task_state: TaskState) -> PlanningContext: + self.calls += 1 + self.task_states.append(task_state) + timestamp = float(self.calls) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=torch.zeros(BATCH_SIZE, 1), + qvel=torch.zeros(BATCH_SIZE, 1), + ), + task=task_state, + scene=SceneSnapshot(timestamp=timestamp, version=self.calls), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + ) + + +class _CommandSink: + """Accept every command while recording safe-stop operations.""" + + def __init__(self) -> None: + self.sent = 0 + self.held = 0 + self.cancelled = 0 + + def send(self, command: object, *, timeout: float) -> CommandAcknowledgement: + del command, timeout + self.sent += 1 + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[object, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, context, timeout + self.held += 1 + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[object, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, timeout + self.cancelled += 1 + return CommandAcknowledgement.accepted_ack() + + +class _Collector: + """Fake acquisition boundary; the test monitor owns decisions.""" + + def __init__(self) -> None: + self.calls: list[tuple[int, float, torch.Tensor]] = [] + + def collect( + self, + spec: SemanticEffectSpec, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> dict[str, EffectEvidenceBatch]: + assert env_ids is not None + self.calls.append((observation_revision, timestamp, env_ids.clone())) + del spec + return {} + + +class _DecisionMonitor(EffectMonitor): + """Return one deterministic row-local physical-effect decision.""" + + def __init__(self, spec: SemanticEffectSpec, decision: EffectMonitorDecision): + self._spec = spec + self._decision = decision + self.calls = 0 + self.requests: list[EffectVerificationRequest] = [] + + @property + def spec(self) -> SemanticEffectSpec: + return self._spec.snapshot() + + def observe( + self, + request: EffectVerificationRequest, + evidence: dict[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + del evidence + self.calls += 1 + self.requests.append(request.snapshot()) + return EffectMonitorDecision( + self._decision.success_mask, + self._decision.failure_mask, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _EffectGoal: + """Test-only goal carrying plan success and symbolic target value.""" + + goal_kind: ClassVar[str] = "runtime_test_effect" + + plan_success: torch.Tensor + target_position: float + + +class _EffectAction(AtomicAction[_EffectGoal, ActionOptions]): + """Zero-frame action with an explicit verified articulation effect.""" + + skill_id: ClassVar[str] = "runtime_test_effect" + GoalType: ClassVar[type] = _EffectGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract() + + def __init__(self) -> None: + super().__init__() + self.plan_count = 0 + + def _scene_dependencies( + self, + request: ResolvedActionRequest[_EffectGoal, ActionOptions], + ) -> tuple[str, ...]: + del request + return ("fixture",) + + def _plan( + self, + request: ResolvedActionRequest[_EffectGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + self.plan_count += 1 + position = torch.full( + (context.batch_size, 1), + goal.target_position, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + return self.build_command_plan( + request, + context, + success=goal.plan_success, + commands=TimedCommandSequence((), context.env_ids), + expected_effects=StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(position) + } + ), + effect_verification=EffectVerificationRequirement("semantic_effect"), + replannable=False, + scene_dependency_monitor_until={"fixture": 0}, + ) + + +@dataclass(frozen=True, slots=True) +class _Workflow: + workflow_id: str + calls: tuple[RegisteredSemanticCall, ...] + + +@dataclass(frozen=True, slots=True) +class _Grounded: + analyzed: object + invocation: ActionInvocation + effect_spec: SemanticEffectSpec + effect_monitor: EffectMonitor + eligible_mask: torch.Tensor + + +@dataclass(frozen=True, slots=True) +class _Integration: + engine: AtomicActionEngine + scene_registry: SceneRegistry + + +class _Compiler(SemanticSkillCompiler): + """Semantic compiler test double retaining the production call boundaries.""" + + def __init__( + self, + engine: AtomicActionEngine, + decisions: tuple[EffectMonitorDecision, ...], + plan_success: tuple[torch.Tensor, ...], + ) -> None: + self._test_integration = _Integration(engine, SceneRegistry()) + self._decisions = decisions + self._plan_success = plan_success + self.analyze_count = 0 + self.ground_count = 0 + self.ground_timestamps: list[float] = [] + self.ground_task_masks: list[torch.Tensor | None] = [] + self.invocations: list[ActionInvocation] = [] + self.monitors: list[_DecisionMonitor] = [] + + @property + def integration(self) -> _Integration: + return self._test_integration + + def analyze( + self, + calls: tuple[RegisteredSemanticCall, ...], + *, + workflow_id: str = "semantic_workflow", + path: tuple[object, ...] = ("workflow",), + ) -> _Workflow: + del path + self.analyze_count += 1 + return _Workflow(workflow_id, tuple(calls)) + + def ground( + self, + workflow: _Workflow, + call_index: int, + context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, + revision: int = 0, + path: tuple[object, ...] = ("workflow",), + ) -> _Grounded: + del path + assert eligible_mask is not None + self.ground_count += 1 + self.ground_timestamps.append(context.robot.timestamp) + state = context.task.get_articulation_joint_state("fixture", "joint") + self.ground_task_masks.append(None if state is None else state.env_mask.clone()) + call = workflow.calls[call_index] + invocation = ActionInvocation( + skill_id=_EffectAction.skill_id, + goal=_EffectGoal( + self._plan_success[call_index].clone(), + float(call_index + 1), + ), + binding=self.integration.engine.bind_control_parts( + _EffectAction.skill_id, + {}, + ), + motion_policy=MotionPolicy( + planner="runtime_test", + sample_count=7, + control_dt=0.02, + velocity_limit=0.4, + acceleration_limit=0.8, + ), + recovery_policy=RecoveryPolicy( + max_replans=0, + max_action_retries=0, + action_timeout=100.0, + ), + invocation_id=f"{workflow.workflow_id}:{call_index}", + revision=revision, + ) + target = torch.full((BATCH_SIZE, 1), float(call_index + 1)) + expectation = ArticulationJointStateExpectation( + "joint_target", + "fixture", + "joint", + target, + ) + source = EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("virtual", JOINT_STATE_EFFECT_CHANNEL), + ) + spec = SemanticEffectSpec( + semantic_id=call.semantic_id, + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + env_ids=context.env_ids, + state_expectations=(expectation,), + clauses=( + JointStateEffectClause( + "joint_position", + expectation.expectation_id, + source, + target, + ), + ), + ) + monitor = _DecisionMonitor(spec, self._decisions[call_index]) + self.invocations.append(invocation) + self.monitors.append(monitor) + analyzed = SimpleNamespace( + bound=SimpleNamespace( + robot_profile=SimpleNamespace(profile_id="runtime_test_profile"), + binding=SimpleNamespace(action_binding=invocation.binding), + linked=SimpleNamespace( + descriptor=SimpleNamespace(skill_id=invocation.skill_id) + ), + preset=SimpleNamespace( + preset_id="runtime_test_preset", + schema_version=1, + motion_policy=invocation.motion_policy, + recovery_policy=invocation.recovery_policy, + ), + ) + ) + return _Grounded( + analyzed, + invocation, + spec, + monitor, + eligible_mask.clone(), + ) + + +@dataclass(slots=True) +class _System: + runtime: SkillRuntime + compiler: _Compiler + engine: AtomicActionEngine + action: _EffectAction + observation: _ObservationProvider + sink: _CommandSink + collector: _Collector + clock: _Clock + + +def _mask(*values: bool) -> torch.Tensor: + return torch.tensor(values, dtype=torch.bool) + + +def _call(name: str) -> RegisteredSemanticCall: + return RegisteredSemanticCall(call_id=f"test.{name}") + + +def _system( + decisions: tuple[EffectMonitorDecision, ...], + *, + plan_success: tuple[torch.Tensor, ...] | None = None, +) -> _System: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 1 + robot.control_parts = {} + robot.get_qpos.return_value = torch.zeros(BATCH_SIZE, 1) + robot.get_qvel.return_value = torch.zeros(BATCH_SIZE, 1) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "runtime_test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _EffectAction() + engine.register(action) + selected_plan_success = plan_success or tuple(_mask(True, True) for _ in decisions) + compiler = _Compiler(engine, decisions, selected_plan_success) + observation = _ObservationProvider() + sink = _CommandSink() + collector = _Collector() + clock = _Clock() + runtime = SkillRuntime.from_components( + compiler, + observation, + sink, + collector, + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + clock=clock, + ) + return _System( + runtime, + compiler, + engine, + action, + observation, + sink, + collector, + clock, + ) + + +def test_runtime_analyzes_once_and_uses_one_fresh_session_per_call( + monkeypatch: pytest.MonkeyPatch, +) -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + ) + session_calls = 0 + runner_calls = 0 + original_start = system.engine.start + original_runner = runtime_module.ExecutionRunner + + def counted_start(self: AtomicActionEngine, *args: object, **kwargs: object): + nonlocal session_calls + del self + session_calls += 1 + return original_start(*args, **kwargs) + + system.engine.start = MethodType(counted_start, system.engine) + + def counted_runner(*args: object, **kwargs: object): + nonlocal runner_calls + runner_calls += 1 + return original_runner(*args, **kwargs) + + monkeypatch.setattr(runtime_module, "ExecutionRunner", counted_runner) + result = system.runtime.run((_call("first"), _call("second"))) + + assert result.status is SkillStatus.COMPLETED + assert system.compiler.analyze_count == 1 + assert system.compiler.ground_count == 2 + assert session_calls == 2 + assert runner_calls == 2 + assert system.action.plan_count == 2 + assert len(result.calls) == 2 + assert len(system.collector.calls) == 2 + assert system.compiler.ground_timestamps[1] > system.compiler.ground_timestamps[0] + assert system.observation.calls == 4 + + +def test_runtime_analyzes_downstream_calls_but_executes_only_requested_prefix() -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + ) + calls = (_call("current_segment"), _call("downstream_segment")) + + result = system.runtime.run(calls, execution_prefix_length=1) + + assert result.status is SkillStatus.COMPLETED + assert system.compiler.analyze_count == 1 + assert system.compiler.ground_count == 1 + assert len(system.compiler.invocations) == 1 + assert len(result.calls) == 1 + assert result.calls[0].semantic_id == "test.current_segment" + + +@pytest.mark.parametrize("prefix_length", (0, 3, True, 1.5)) +def test_runtime_rejects_invalid_execution_prefix_before_analysis( + prefix_length: object, +) -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + ) + + with pytest.raises((TypeError, ValueError), match="execution_prefix_length"): + system.runtime.start( + (_call("first"), _call("second")), + execution_prefix_length=prefix_length, # type: ignore[arg-type] + ) + + assert system.compiler.analyze_count == 0 + assert system.observation.calls == 0 + + +def test_runtime_keeps_partial_rows_at_the_shared_call_barrier() -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, False), _mask(False, True)), + EffectMonitorDecision(_mask(True, False), _mask(False, False)), + ) + ) + result = system.runtime.run(_call("first"), _call("second")) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) + assert torch.equal(result.calls[0].completed_mask, _mask(True, False)) + assert torch.equal(result.calls[0].failed_mask, _mask(False, True)) + assert torch.equal(result.calls[1].entered_mask, _mask(True, False)) + assert torch.equal(system.compiler.ground_task_masks[1], _mask(True, False)) + joint = result.task_state.get_articulation_joint_state("fixture", "joint") + assert joint is not None + assert torch.equal(joint.env_mask, _mask(True, False)) + assert torch.allclose(joint.position[0], torch.tensor([2.0])) + assert len(result.failures) == 1 + + +def test_nonblocking_step_routes_effect_feedback_through_collector() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.start(_call("stepwise")) + + assert result.status is SkillStatus.RUNNING + while not result.terminal: + if result.wait_duration: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert len(result.effects) == 1 + assert len(result.calls[0].effects) == 1 + assert system.collector.calls[0][0] == 0 + assert torch.equal(system.collector.calls[0][2], torch.tensor([0, 1])) + assert system.compiler.monitors[0].requests[0].verification_id == 0 + + +def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + + result = system.runtime.run(_call("metadata")) + metadata = result.to_metadata() + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["schema_version"] == 1 + assert metadata["kind"] == "skill_result" + call = metadata["calls"][0] + assert call["semantic_id"] == "test.metadata" + assert call["call"]["arguments"]["call_id"] == "test.metadata" + assert call["active_plan_attempt_generation"] == 0 + attempt = call["plan_attempts"][0] + assert attempt["trigger"] == "action_planned" + assert attempt["planned_scene_version"] == 1 + assert attempt["planned_collision_world_revision"] == [0, 0] + assert attempt["scene_dependencies"] == ["fixture"] + assert attempt["scene_dependency_monitor_until"] == {"fixture": 0} + typed_attempt = result.calls[0].plan_attempts[0] + assert typed_attempt.scene_dependency_monitor_until == {"fixture": 0} + assert typed_attempt.snapshot().scene_dependency_monitor_until == {"fixture": 0} + resolved = call["resolved_core_policy"] + assert resolved["profile_id"] == "runtime_test_profile" + assert resolved["preset"] == { + "preset_id": "runtime_test_preset", + "schema_version": 1, + } + assert resolved["motion_policy"]["strategy"] == "ik_interp" + assert resolved["motion_policy"]["planner"] == "runtime_test" + assert resolved["motion_policy"]["sample_count"] == 7 + assert resolved["recovery_policy"]["max_replans"] == 0 + assert resolved["endpoints"] == [] + assert attempt["resolved_core_policy"] == resolved + assert result.calls[0].resolved_core_policy.preset_id == "runtime_test_preset" + effect = call["effects"][0] + assert effect["effect_spec"]["semantic_id"] == "test.metadata" + assert effect["monitor"]["monitor_id"].endswith("._DecisionMonitor") + assert effect["evidence"] == {} + + metadata["masks"]["success"][0] = False + assert system.runtime.result.to_metadata()["masks"]["success"] == [True, True] + + +@pytest.mark.parametrize("waypoint_index", (-1, 1, True, 1.5)) +def test_plan_attempt_trace_rejects_invalid_scene_dependency_monitor_cutoff( + waypoint_index: object, +) -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.run(_call("trace_cutoff")) + attempt = result.calls[0].plan_attempts[0] + + with pytest.raises(ValueError, match="waypoint indices"): + replace( + attempt, + scene_dependency_monitor_until={ + "fixture": waypoint_index # type: ignore[dict-item] + }, + ) + + +def test_plan_attempt_trace_rejects_monitor_cutoff_for_non_dependency() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.run(_call("trace_dependency")) + attempt = result.calls[0].plan_attempts[0] + + with pytest.raises(ValueError, match="keys must be scene dependencies"): + replace( + attempt, + scene_dependency_monitor_until={"other": 0}, + ) + + +def test_endpoint_binding_trace_records_only_stable_binding_choices() -> None: + binding = EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="left_arm", + adapter_id="control_part", + target=JointPositionTarget("left_arm_control", (3, 1)), + task_state_key="left_arm_state", + capabilities=frozenset({"cartesian_pose", "joint_position"}), + claim_tokens=frozenset({"arm_workspace", "left_side"}), + joint_ids=(3, 1), + ) + + trace = SkillEndpointBindingTrace.from_binding(binding) + metadata = trace.to_metadata() + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["resource_id"] == "left_arm" + assert metadata["adapter_id"] == "control_part" + assert metadata["transport_id"] == "robot.joint_position" + assert metadata["target_id"] == "left_arm_control" + assert metadata["capabilities"] == ["cartesian_pose", "joint_position"] + assert metadata["claim_tokens"] == ["arm_workspace", "left_side"] + assert metadata["joint_ids"] == [3, 1] + assert "target" not in metadata + + +def test_preparation_failure_keeps_resolved_policy_without_plan_attempt( + monkeypatch: pytest.MonkeyPatch, +) -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + monkeypatch.setattr( + system.engine, + "start", + Mock(side_effect=RuntimeError("planner unavailable")), + ) + + result = system.runtime.start(_call("planning_failure")) + metadata = result.to_metadata() + + assert result.status is SkillStatus.FAILED + assert len(result.calls) == 1 + assert result.calls[0].plan_attempts == () + assert result.calls[0].resolved_core_policy.preset_id == "runtime_test_preset" + assert metadata["calls"][0]["active_plan_attempt_generation"] is None + assert ( + metadata["calls"][0]["resolved_core_policy"]["motion_policy"]["planner"] + == "runtime_test" + ) + + +def test_cancel_inherits_runner_cancel_then_hold_safe_stop() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + system.runtime.start(_call("cancel")) + + result = system.runtime.cancel("operator stop") + + assert result.status is SkillStatus.CANCELLED + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert not result.eligible_mask.any() + assert system.sink.cancelled == 1 + assert system.sink.held == 1 + assert result.calls[0].status.value == "cancelled" + + +def test_facade_varargs_and_programmatic_iterable_share_runtime_path() -> None: + decisions = ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + iterable_system = _system(decisions) + facade_system = _system(decisions) + calls = (_call("first"), _call("second")) + + iterable_result = iterable_system.runtime.run(calls) + facade_result = AtomicSkills(facade_system.runtime).run(*calls) + + assert iterable_result.status is facade_result.status + assert torch.equal(iterable_result.success_mask, facade_result.success_mask) + assert [trace.skill_id for trace in iterable_result.calls] == [ + trace.skill_id for trace in facade_result.calls + ] + assert iterable_system.compiler.analyze_count == 1 + assert facade_system.compiler.analyze_count == 1 + assert [item.skill_id for item in iterable_system.compiler.invocations] == [ + item.skill_id for item in facade_system.compiler.invocations + ] + + +def test_from_env_requires_an_explicit_runtime_provider() -> None: + class AttributeBag: + compiler = object() + robot = object() + scene = object() + + with pytest.raises(TypeError, match="no semantic-skill integration adapter"): + AtomicSkills.from_env(AttributeBag()) + + +def test_from_env_delegates_preset_to_installed_provider() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + + class Provider: + def __init__(self) -> None: + self.presets: list[str] = [] + + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + self.presets.append(preset) + return system.runtime + + provider = Provider() + skills = AtomicSkills.from_env(provider, preset="precise") + + assert skills.runtime is system.runtime + assert provider.presets == ["precise"] + + +def test_result_snapshots_do_not_expose_runtime_masks() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.run(_call("owned")) + + result.success_mask.zero_() + result.calls[0].completed_mask.zero_() + fresh = system.runtime.result + + assert torch.equal(fresh.success_mask, _mask(True, True)) + assert torch.equal(fresh.calls[0].completed_mask, _mask(True, True)) + + +def test_fork_creates_an_independent_lane_on_the_shared_clock() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + lane_sink = _CommandSink() + + lane = system.runtime.fork(lane_sink) + + assert lane is not system.runtime + assert lane.compiler is system.runtime.compiler + assert lane.clock is system.runtime.clock + assert lane.status is SkillStatus.IDLE + assert lane_sink.sent == 0 + + +def test_runner_failure_does_not_relabel_peer_cancelled_rows() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + system.runtime.start(_call("row_failure")) + system.runtime.deactivate_rows(_mask(True, False), reason="peer branch failed") + + def fail_observation(task_state: TaskState) -> PlanningContext: + del task_state + raise RuntimeError("observation unavailable") + + system.observation.observe = fail_observation + result = system.runtime.step() + if result.wait_duration > 0.0: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.cancelled_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) + + +def test_deactivate_all_rows_safe_stops_immediately_before_due_time() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + system.runtime.start(_call("deactivate_all")) + + result = system.runtime.deactivate_rows( + _mask(True, True), + reason="parallel peer failed", + ) + + assert result.status is SkillStatus.CANCELLED + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert system.sink.cancelled == 1 + assert system.sink.held == 1 + + +def test_parallel_factory_analyzes_claims_and_forks_owned_shared_clock_lanes() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + + def analyze_claims( + self: _Compiler, + calls: tuple[RegisteredSemanticCall, ...], + *, + workflow_id: str, + path: tuple[object, ...] = ("workflow",), + ) -> object: + del self, workflow_id, path + analyzed = [] + for call_index, call in enumerate(calls): + joint_id = 0 if call.call_id.endswith("left") else 1 + analyzed.append( + SimpleNamespace( + index=call_index, + symbolic_writes=frozenset(), + opaque_symbolic_effect=False, + bound=SimpleNamespace( + binding=SimpleNamespace( + claim=ResourceClaim( + frozenset({f"resource_{joint_id}"}), + (joint_id,), + ) + ) + ), + ) + ) + return SimpleNamespace(calls=tuple(analyzed)) + + class AcceptSafety: + def validate(self, *, branch_frames: object, merged_frame: object) -> None: + del branch_frames, merged_frame + + system.compiler.analyze = MethodType(analyze_claims, system.compiler) + parallel = ParallelSkillRuntime.from_template( + system.runtime, + { + "left": (_call("left"),), + "right": (_call("right"),), + }, + system.sink, + ParallelTimingPolicy(0.1), + AcceptSafety(), + timeout_steps=5, + ) + + assert parallel.clock is system.runtime.clock + assert parallel.branch_claims["left"].joint_ids == (0,) + assert parallel.branch_claims["right"].joint_ids == (1,) + + changed = StateDelta( + articulation_joint_updates={ + ("template", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(system.runtime.task_state, _mask(True, True)) + system.runtime.adopt_verified_task_state(changed) + assert all( + result.task_state.get_articulation_joint_state("template", "joint") is None + for result in parallel.result.branch_results.values() + ) diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py index bf364f56f..6b9a97d9f 100644 --- a/tests/sim/skills/test_scene.py +++ b/tests/sim/skills/test_scene.py @@ -27,6 +27,7 @@ Affordance, AntipodalAffordance, EntityState, + ObservedArticulationJointState, SceneSnapshot, ) from embodichain.lab.sim.skills import ( @@ -89,6 +90,24 @@ def observe( return EntityState(self.pose) +class _MutableJointProvider: + """Expose one mutable canonical articulation joint observation.""" + + def __init__(self, position: torch.Tensor) -> None: + self.position = position + self.calls = 0 + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, ObservedArticulationJointState]: + del timestamp, env_ids + self.calls += 1 + return {"slide": ObservedArticulationJointState(self.position)} + + class _MotionGenerator: """Minimal dynamic-collision integration surface.""" @@ -127,13 +146,27 @@ def snapshot( class _SimulationEntity: """Simulation entity pose source used by the opt-in adapter tests.""" - def __init__(self, pose: torch.Tensor) -> None: + def __init__( + self, + pose: torch.Tensor, + *, + qpos: torch.Tensor | None = None, + joint_names: tuple[str, ...] = (), + ) -> None: self.pose = pose + self.qpos = qpos + self.joint_names = joint_names def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: assert to_matrix is True return self.pose + def get_qpos(self, *, target: bool) -> torch.Tensor: + assert target is False + if self.qpos is None: + raise RuntimeError("This simulation fixture has no articulation qpos.") + return self.qpos + class _Simulation: """Minimal simulation lookup surface with selected and unselected assets.""" @@ -144,7 +177,11 @@ def __init__(self) -> None: "ignored": _SimulationEntity(torch.eye(4) * 2.0), } self.articulations = { - "sim_drawer": _SimulationEntity(torch.eye(4)), + "sim_drawer": _SimulationEntity( + torch.eye(4), + qpos=torch.tensor([[0.25]]), + joint_names=("slide",), + ), } def get_rigid_object(self, uid: str) -> _SimulationEntity | None: @@ -220,6 +257,47 @@ def test_root_registration_requires_explicit_state_provider() -> None: SceneEntityRegistration(ref=SceneObjectRef("cube")) +def test_joint_state_provider_is_owned_by_articulation_registration() -> None: + joint_provider = _MutableJointProvider(torch.tensor([[0.1], [0.2]])) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=_StateProvider(), + joint_state_provider=joint_provider, + ), + ) + ) + provider = registry.make_scene_provider() + env_ids = torch.tensor([0, 1], dtype=torch.long) + + first = provider.snapshot(timestamp=0.0, env_ids=env_ids) + returned = first.articulation_joints[("drawer", "slide")] + returned.position.zero_() + assert torch.equal( + first.articulation_joints[("drawer", "slide")].position, + torch.tensor([[0.1], [0.2]]), + ) + + joint_provider.position[:, 0] = torch.tensor([0.3, 0.4]) + second = provider.snapshot(timestamp=1.0, env_ids=env_ids) + assert second.version == first.version + 1 + assert torch.equal( + second.articulation_joints[("drawer", "slide")].position, + torch.tensor([[0.3], [0.4]]), + ) + assert joint_provider.calls == 2 + + +def test_joint_state_provider_rejects_non_articulation_registration() -> None: + with pytest.raises(ValueError, match="SceneArticulationRef"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + joint_state_provider=_MutableJointProvider(torch.tensor([0.0])), + ) + + def test_link_registration_requires_parent_and_native_name() -> None: with pytest.raises(ValueError, match="parent and native_name"): SceneEntityRegistration( @@ -1025,6 +1103,35 @@ def test_from_simulation_is_explicit_and_uses_uid_only_as_alias() -> None: assert "ignored" not in snapshot.entities +def test_from_simulation_does_not_register_canonical_uid_as_alias() -> None: + simulation = _Simulation() + simulation.rigid_objects["cube"] = simulation.rigid_objects["sim_cube"] + + registry = SceneRegistry.from_simulation( + simulation, # type: ignore[arg-type] + rigid_objects={"cube": "cube"}, + ) + + assert registry.resolve("cube") == SceneObjectRef("cube") + assert registry.aliases == {} + + +def test_from_simulation_publishes_named_articulation_qpos() -> None: + registry = SceneRegistry.from_simulation( + _Simulation(), # type: ignore[arg-type] + articulations={"drawer": "sim_drawer"}, + ) + + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + state = snapshot.articulation_joints[("drawer", "slide")] + assert torch.equal(state.position, torch.tensor([[0.25]])) + assert state.valid_mask is not None and state.valid_mask.tolist() == [True] + + def test_from_simulation_derives_live_geometry_only_for_explicit_collision_role() -> ( None ):