diff --git a/docs/source/api_reference/embodichain/embodichain.gen_sim.action_engine.rst b/docs/source/api_reference/embodichain/embodichain.gen_sim.action_engine.rst new file mode 100644 index 000000000..a7ccfedde --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.gen_sim.action_engine.rst @@ -0,0 +1,172 @@ +embodichain.gen_sim.action_engine +================================== + +Action Engine turns validated task semantics into coordinate-free action +graphs and reproducible execution bundles. This page documents the planning +and generation surface; live grounding and execution are added in the +dependent runtime layers. + +Core protocol and capabilities +------------------------------ + +.. automodule:: embodichain.gen_sim.action_engine + :members: + +.. automodule:: embodichain.gen_sim.action_engine.protocol + :members: + +.. automodule:: embodichain.gen_sim.action_engine.capabilities + :members: + +.. automodule:: embodichain.gen_sim.action_engine.capabilities.atomic + :members: + +.. automodule:: embodichain.gen_sim.action_engine.capabilities.builtins + :members: + +.. automodule:: embodichain.gen_sim.action_engine.capabilities.held_hand_over + :members: + +.. automodule:: embodichain.gen_sim.action_engine.capabilities.registry + :members: + +Domain contracts +---------------- + +.. automodule:: embodichain.gen_sim.action_engine.domain + :members: + +.. automodule:: embodichain.gen_sim.action_engine.domain.motion + :members: + +.. automodule:: embodichain.gen_sim.action_engine.domain.programs + :members: + +.. automodule:: embodichain.gen_sim.action_engine.domain.task_contracts + :members: + +.. automodule:: embodichain.gen_sim.action_engine.domain.v2 + :members: + +.. automodule:: embodichain.gen_sim.action_engine.domain.visual_contracts + :members: + +Compilation and planning +------------------------ + +.. automodule:: embodichain.gen_sim.action_engine.compiler + :members: + +.. automodule:: embodichain.gen_sim.action_engine.compiler.core + :members: + +.. automodule:: embodichain.gen_sim.action_engine.compiler.v2 + :members: + +.. automodule:: embodichain.gen_sim.action_engine.planning + :members: + +.. automodule:: embodichain.gen_sim.action_engine.planning.dual + :members: + +.. automodule:: embodichain.gen_sim.action_engine.planning.linker + :members: + +.. automodule:: embodichain.gen_sim.action_engine.planning.online + :members: + +.. automodule:: embodichain.gen_sim.action_engine.planning.planner + :members: + +.. automodule:: embodichain.gen_sim.action_engine.planning.selection + :members: + +.. automodule:: embodichain.gen_sim.action_engine.planning.task_planner_prompt + :members: + +.. automodule:: embodichain.gen_sim.action_engine.planning.vision + :members: + +Task assembly +------------- + +.. automodule:: embodichain.gen_sim.action_engine.tasks + :members: + +.. automodule:: embodichain.gen_sim.action_engine.tasks.assembly + :members: + +.. automodule:: embodichain.gen_sim.action_engine.tasks.grounding + :members: + +.. automodule:: embodichain.gen_sim.action_engine.tasks.interpretation + :members: + +.. automodule:: embodichain.gen_sim.action_engine.tasks.recipes + :members: + +.. automodule:: embodichain.gen_sim.action_engine.tasks.scene + :members: + +Bundle generation +----------------- + +.. automodule:: embodichain.gen_sim.action_engine.config + :members: + +.. automodule:: embodichain.gen_sim.action_engine.config.runtime_policy + :members: + +.. automodule:: embodichain.gen_sim.action_engine.generation + :members: + +.. automodule:: embodichain.gen_sim.action_engine.generation.artifacts + :members: + +.. automodule:: embodichain.gen_sim.action_engine.generation.assets + :members: + +.. automodule:: embodichain.gen_sim.action_engine.generation.config_builder + :members: + +.. automodule:: embodichain.gen_sim.action_engine.generation.generator + :members: + +.. automodule:: embodichain.gen_sim.action_engine.generation.models + :members: + +.. automodule:: embodichain.gen_sim.action_engine.generation.source_scene + :members: + +.. automodule:: embodichain.gen_sim.action_engine.cli.generate_action_agent_config + :members: + +Supporting planning utilities +----------------------------- + +.. automodule:: embodichain.gen_sim.action_engine.graph_visualization + :members: + +.. automodule:: embodichain.gen_sim.action_engine.gripper_profiles + :members: + +.. automodule:: embodichain.gen_sim.action_engine.orientation + :members: + +.. automodule:: embodichain.gen_sim.action_engine.solver_profiles + :members: + +.. automodule:: embodichain.gen_sim.action_engine.runtime + :members: + +.. automodule:: embodichain.gen_sim.action_engine.runtime.loader + :members: + +.. automodule:: embodichain.gen_sim.action_engine.runtime.models + :members: + +.. automodule:: embodichain.gen_sim.action_engine.runtime.motion_policy + :members: + +.. automodule:: embodichain.gen_sim.action_engine.runtime.state + :members: diff --git a/docs/source/api_reference/index.rst b/docs/source/api_reference/index.rst index be454024d..92c3b6f56 100644 --- a/docs/source/api_reference/index.rst +++ b/docs/source/api_reference/index.rst @@ -80,4 +80,5 @@ documentation. CI runs this same checker after style checks and before tests. :maxdepth: 1 public_api + embodichain/embodichain.gen_sim.action_engine embodichain/embodichain.gen_sim.task_engine diff --git a/embodichain/gen_sim/action_engine/__init__.py b/embodichain/gen_sim/action_engine/__init__.py new file mode 100644 index 000000000..3a7bd1927 --- /dev/null +++ b/embodichain/gen_sim/action_engine/__init__.py @@ -0,0 +1,33 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Capability-driven planning for generated simulations.""" + +from __future__ import annotations + +from .protocol import ( + ACTION_ENGINE_CONFIG_SCHEMA, + ACTION_ENGINE_ENV_ID, + EXECUTION_PROGRAM_SCHEMA, + TASK_AGENT_SCHEMA, +) + +__all__ = [ + "ACTION_ENGINE_CONFIG_SCHEMA", + "ACTION_ENGINE_ENV_ID", + "EXECUTION_PROGRAM_SCHEMA", + "TASK_AGENT_SCHEMA", +] diff --git a/embodichain/gen_sim/action_engine/capabilities/__init__.py b/embodichain/gen_sim/action_engine/capabilities/__init__.py new file mode 100644 index 000000000..b23dc8867 --- /dev/null +++ b/embodichain/gen_sim/action_engine/capabilities/__init__.py @@ -0,0 +1,60 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Semantic operator and atomic-action capability registry.""" + +from __future__ import annotations + +from .atomic import ( + ACTION_CONTRACT_VERSION, + AtomicCapability, + AtomicCapabilityRegistry, + ResolvedActionContract, + ResourceClaim, + StateAtom, + StateEffect, + build_atomic_capability_registry, + capability_precondition, +) +from .builtins import build_default_registry +from .held_hand_over import HeldObjectHandOver, HeldObjectHandOverOptions +from .registry import ( + ActionCapability, + ActionTemplate, + CapabilityRegistry, + OperatorCapability, + PhaseTemplate, +) + +__all__ = [ + "ACTION_CONTRACT_VERSION", + "ActionCapability", + "ActionTemplate", + "AtomicCapability", + "AtomicCapabilityRegistry", + "CapabilityRegistry", + "HeldObjectHandOver", + "HeldObjectHandOverOptions", + "OperatorCapability", + "PhaseTemplate", + "ResolvedActionContract", + "ResourceClaim", + "StateAtom", + "StateEffect", + "build_atomic_capability_registry", + "build_default_registry", + "capability_precondition", +] diff --git a/embodichain/gen_sim/action_engine/capabilities/atomic.py b/embodichain/gen_sim/action_engine/capabilities/atomic.py new file mode 100644 index 000000000..07b2f6d51 --- /dev/null +++ b/embodichain/gen_sim/action_engine/capabilities/atomic.py @@ -0,0 +1,1293 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Single-source AtomicAction capability descriptors.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from dataclasses import dataclass +import hashlib +import json +from typing import Any + +import torch + +from embodichain.gen_sim.action_engine.gripper_profiles import get_gripper_profile + +__all__ = [ + "ACTION_CONTRACT_VERSION", + "AtomicCapability", + "AtomicCapabilityRegistry", + "ResolvedActionContract", + "ResourceClaim", + "StateAtom", + "StateEffect", + "build_atomic_capability_registry", + "capability_precondition", +] + +_RETRY_MODES = frozenset({"direct", "recover_then_retry", "non_retryable"}) +ACTION_CONTRACT_VERSION = "action_contract_v2" +_PREDICATES = frozenset( + { + "arm_free", + "object_free", + "object_held", + "object_coordinated_held", + "handover_complete", + "arm_clear", + "arm_home", + } +) +_EFFECT_OPERATIONS = frozenset({"add", "delete"}) +_RESOURCE_ACCESS = frozenset({"shared_read", "exclusive"}) +_RESOURCE_LIFETIMES = frozenset({"action", "until_release"}) +_COMPLETION_MODES = frozenset({"ordinary", "cleanup", "terminal_barrier"}) +_FAILURE_POLICIES = frozenset({"task_required", "safety_required", "best_effort"}) + + +@dataclass(frozen=True) +class StateAtom: + """One symbolic state fact used by an Action Contract.""" + + predicate: str + object_uid: str | None = None + arm: str | None = None + + def __post_init__(self) -> None: + if self.predicate not in _PREDICATES: + raise ValueError(f"Unknown Action Contract predicate {self.predicate!r}.") + if self.object_uid is not None and not self.object_uid: + raise ValueError("StateAtom.object_uid must not be empty.") + if self.arm is not None and not self.arm: + raise ValueError("StateAtom.arm must not be empty.") + + def as_mapping(self) -> dict[str, str]: + """Return the stable JSON representation of this fact.""" + result = {"predicate": self.predicate} + if self.object_uid is not None: + result["object_uid"] = self.object_uid + if self.arm is not None: + result["arm"] = self.arm + return result + + +@dataclass(frozen=True) +class StateEffect: + """Add or delete one symbolic state fact.""" + + op: str + atom: StateAtom + + def __post_init__(self) -> None: + if self.op not in _EFFECT_OPERATIONS: + raise ValueError(f"Unknown Action Contract effect operation {self.op!r}.") + + def as_mapping(self) -> dict[str, Any]: + """Return the stable JSON representation of this effect.""" + return {"op": self.op, "atom": self.atom.as_mapping()} + + +@dataclass(frozen=True) +class ResourceClaim: + """One resource access claim made by an AtomicAction.""" + + resource: str + access: str = "exclusive" + lifetime: str = "action" + + def __post_init__(self) -> None: + if not self.resource: + raise ValueError("ResourceClaim.resource must not be empty.") + if self.access not in _RESOURCE_ACCESS: + raise ValueError(f"Unknown resource access mode {self.access!r}.") + if self.lifetime not in _RESOURCE_LIFETIMES: + raise ValueError(f"Unknown resource lifetime {self.lifetime!r}.") + + def as_mapping(self) -> dict[str, str]: + """Return the stable JSON representation of this claim.""" + return { + "resource": self.resource, + "access": self.access, + "lifetime": self.lifetime, + } + + +@dataclass(frozen=True) +class ResolvedActionContract: + """Fully resolved, serializable contract for one action node.""" + + requires: tuple[StateAtom, ...] = () + effects: tuple[StateEffect, ...] = () + claims: tuple[ResourceClaim, ...] = () + completion: str = "ordinary" + failure_policy: str = "task_required" + version: str = ACTION_CONTRACT_VERSION + + def __post_init__(self) -> None: + if self.version != ACTION_CONTRACT_VERSION: + raise ValueError( + f"Unsupported Action Contract version {self.version!r}; " + f"expected {ACTION_CONTRACT_VERSION!r}." + ) + if self.completion not in _COMPLETION_MODES: + raise ValueError(f"Unknown Action Contract completion {self.completion!r}.") + if self.failure_policy not in _FAILURE_POLICIES: + raise ValueError( + f"Unknown Action Contract failure policy {self.failure_policy!r}." + ) + + def as_mapping(self) -> dict[str, Any]: + """Return the stable JSON representation persisted in SeedGraph v3.""" + return { + "version": self.version, + "requires": [atom.as_mapping() for atom in self.requires], + "effects": [effect.as_mapping() for effect in self.effects], + "claims": [claim.as_mapping() for claim in self.claims], + "completion": self.completion, + "failure_policy": self.failure_policy, + } + + +@dataclass(frozen=True) +class AtomicCapability: + """Describe planning, grounding, execution, and recovery for one skill.""" + + name: str + action_type: type | None + config_type: type | None + binding_kinds: frozenset[str] + controls: frozenset[str] + resource_mode: str + state_effect: str + target_materializer: str + motion_base: str | None = None + config_materializer: str = "single_arm" + verifier: str = "postcondition" + failure_classifier: str = "default" + retry_mode: str = "direct" + runtime_available: bool = True + unavailable_reason: str | None = None + target_materializer_hook: Callable[..., Any] | None = None + config_materializer_hook: Callable[..., Any] | None = None + verifier_hook: Callable[..., Any] | None = None + failure_classifier_hook: Callable[..., str] | None = None + contract_resolver_hook: ( + Callable[[Mapping[str, Any]], ResolvedActionContract] | None + ) = None + allows_target_contact: bool = False + """Whether motion planning may temporarily exclude the action target.""" + + def __post_init__(self) -> None: + if not self.name: + raise ValueError("AtomicCapability.name must not be empty.") + if self.motion_base is not None and not self.motion_base: + raise ValueError( + f"AtomicCapability {self.name!r} motion_base must not be empty." + ) + if not self.binding_kinds or not self.controls: + raise ValueError( + f"AtomicCapability {self.name!r} requires bindings and controls." + ) + if self.retry_mode not in _RETRY_MODES: + raise ValueError( + f"AtomicCapability {self.name!r} has invalid retry_mode {self.retry_mode!r}." + ) + if not isinstance(self.allows_target_contact, bool): + raise TypeError("allows_target_contact must be a boolean.") + if self.runtime_available: + if self.action_type is None or self.config_type is None: + raise ValueError( + f"Executable AtomicCapability {self.name!r} requires action/config types." + ) + if self.unavailable_reason is not None: + raise ValueError( + f"Executable AtomicCapability {self.name!r} cannot have an unavailable reason." + ) + elif not self.unavailable_reason: + raise ValueError( + f"Planning-only AtomicCapability {self.name!r} requires unavailable_reason." + ) + for field_name in ( + "target_materializer_hook", + "config_materializer_hook", + "verifier_hook", + "failure_classifier_hook", + "contract_resolver_hook", + ): + value = getattr(self, field_name) + if value is not None and not callable(value): + raise TypeError( + f"AtomicCapability {self.name!r} {field_name} must be callable." + ) + + def resolve_contract(self, node: Mapping[str, Any]) -> ResolvedActionContract: + """Resolve the deterministic Action Contract for one bound node.""" + if self.contract_resolver_hook is not None: + contract = self.contract_resolver_hook(node) + if not isinstance(contract, ResolvedActionContract): + raise TypeError( + f"AtomicCapability {self.name!r} contract resolver must return " + "ResolvedActionContract." + ) + return contract + return _resolve_default_contract(self, node) + + def as_catalog_entry(self) -> dict[str, Any]: + """Return the stable, JSON-safe planning view of this capability.""" + return { + "name": self.name, + "binding_kinds": sorted(self.binding_kinds), + "controls": sorted(self.controls), + "resource_mode": self.resource_mode, + "state_effect": self.state_effect, + "target_materializer": self.target_materializer, + "motion_base": self.motion_base or self.name, + "config_materializer": self.config_materializer, + "verifier": self.verifier, + "failure_classifier": self.failure_classifier, + "retry_mode": self.retry_mode, + "runtime_available": self.runtime_available, + "unavailable_reason": self.unavailable_reason, + "allows_target_contact": self.allows_target_contact, + "custom_target_materializer": _callable_name(self.target_materializer_hook), + "custom_config_materializer": _callable_name(self.config_materializer_hook), + "custom_verifier": _callable_name(self.verifier_hook), + "custom_failure_classifier": _callable_name(self.failure_classifier_hook), + "contract_version": ACTION_CONTRACT_VERSION, + "contract_resolver": _callable_name(self.contract_resolver_hook) + or f"{__name__}._resolve_default_contract", + } + + +class AtomicCapabilityRegistry: + """Strict registry shared by planners, validators, grounders, and runtime.""" + + def __init__(self) -> None: + self._capabilities: dict[str, AtomicCapability] = {} + + def register(self, capability: AtomicCapability) -> None: + if capability.name in self._capabilities: + raise ValueError( + f"AtomicCapability {capability.name!r} is already registered." + ) + self._capabilities[capability.name] = capability + + def get(self, name: str) -> AtomicCapability: + try: + return self._capabilities[name] + except KeyError as exc: + raise ValueError( + f"Unknown AtomicAction {name!r}; available actions are {list(self.names())}." + ) from exc + + def require_executable(self, name: str) -> AtomicCapability: + capability = self.get(name) + if not capability.runtime_available: + raise ValueError( + f"AtomicAction {name!r} is planning-only and cannot be executed: " + f"{capability.unavailable_reason}" + ) + return capability + + def validate_binding(self, action: Mapping[str, Any]) -> None: + name = str(action.get("atomic_action", action.get("atomic_action_class", ""))) + capability = self.get(name) + binding = action.get("target_binding") + if not isinstance(binding, Mapping): + raise ValueError( + f"AtomicAction {name!r} requires a target_binding mapping." + ) + kind = str(binding.get("kind", "")) + if kind not in capability.binding_kinds: + raise ValueError( + f"AtomicAction {name!r} does not accept binding kind {kind!r}; " + f"expected one of {sorted(capability.binding_kinds)}." + ) + control = str(action.get("control", "arm")) + if control not in capability.controls: + raise ValueError( + f"AtomicAction {name!r} does not support control {control!r}; " + f"expected one of {sorted(capability.controls)}." + ) + + def names(self) -> tuple[str, ...]: + return tuple(sorted(self._capabilities)) + + def executable_names(self) -> tuple[str, ...]: + return tuple( + name for name in self.names() if self._capabilities[name].runtime_available + ) + + def catalog(self) -> dict[str, dict[str, Any]]: + return { + name: self._capabilities[name].as_catalog_entry() for name in self.names() + } + + def catalog_hash(self) -> str: + payload = json.dumps( + self.catalog(), sort_keys=True, separators=(",", ":"), allow_nan=False + ).encode("utf-8") + return hashlib.sha256(payload).hexdigest() + + +def build_atomic_capability_registry() -> AtomicCapabilityRegistry: + """Build the default catalog, including explicit planning-only skills.""" + from embodichain.lab.sim.atomic_actions import ( + AxisAlign, + CoordinatedPickment, + CoordinatedPickmentOptions, + CoordinatedPlacement, + CoordinatedPlacementOptions, + AxisAlignOptions, + MoveEndEffector, + MoveEndEffectorOptions, + MoveHeldObject, + MoveHeldObjectOptions, + MoveJoints, + MoveJointsOptions, + PickUp, + PickUpOptions, + Place, + PlaceOptions, + Pour, + PourOptions, + Press, + PressOptions, + Slide, + SlideOptions, + Twist, + TwistOptions, + ) + from .held_hand_over import HeldObjectHandOver, HeldObjectHandOverOptions + + registry = AtomicCapabilityRegistry() + definitions = ( + AtomicCapability( + "AxisAlign", + AxisAlign, + AxisAlignOptions, + frozenset({"object"}), + frozenset({"arm"}), + "single_arm_object", + "hold", + "axis_align", + motion_base="AxisAlign", + verifier="postcondition", + failure_classifier="grasp", + contract_resolver_hook=_resolve_axis_align_contract, + allows_target_contact=True, + ), + AtomicCapability( + "PickUp", + PickUp, + PickUpOptions, + frozenset({"object"}), + frozenset({"arm"}), + "single_arm_object", + "hold", + "object_grasp", + verifier="held_object", + failure_classifier="grasp", + allows_target_contact=True, + ), + AtomicCapability( + "MoveHeldObject", + MoveHeldObject, + MoveHeldObjectOptions, + frozenset({"semantic_goal", "visual_constraint", "handover_staging"}), + frozenset({"arm"}), + "single_arm_object", + "preserve_hold", + "semantic_held_object", + ), + AtomicCapability( + "MoveEndEffector", + MoveEndEffector, + MoveEndEffectorOptions, + frozenset({"policy_pose", "visual_constraint"}), + frozenset({"arm"}), + "single_arm", + "preserve", + "eef_pose", + verifier_hook=_verify_arm_clearance, + contract_resolver_hook=_resolve_end_effector_contract, + ), + AtomicCapability( + "MoveJoints", + MoveJoints, + MoveJointsOptions, + frozenset({"joint_state"}), + frozenset({"arm", "hand"}), + "control_part", + "preserve", + "joint_state", + verifier_hook=_verify_move_joints, + contract_resolver_hook=_resolve_joints_contract, + ), + AtomicCapability( + "Place", + Place, + PlaceOptions, + frozenset({"current_held_pose"}), + frozenset({"arm"}), + "single_arm_object", + "release", + "current_held_pose", + ), + AtomicCapability( + "Pour", + Pour, + PourOptions, + frozenset({"pour_goal"}), + frozenset({"arm"}), + "single_arm_object", + "preserve_hold", + "pour", + motion_base="MoveHeldObject", + verifier="postcondition", + retry_mode="non_retryable", + contract_resolver_hook=_resolve_pour_contract, + ), + AtomicCapability( + "PullArticulatedPart", + Slide, + SlideOptions, + frozenset({"articulation_goal"}), + frozenset({"arm"}), + "single_arm_object", + "articulation_change", + "slide", + motion_base="Press", + verifier="postcondition", + retry_mode="non_retryable", + contract_resolver_hook=_resolve_articulation_contract, + allows_target_contact=True, + ), + AtomicCapability( + "PushArticulatedPart", + Slide, + SlideOptions, + frozenset({"articulation_goal"}), + frozenset({"arm"}), + "single_arm_object", + "articulation_change", + "slide", + motion_base="Press", + verifier="postcondition", + retry_mode="non_retryable", + contract_resolver_hook=_resolve_articulation_contract, + allows_target_contact=True, + ), + AtomicCapability( + "TurnKnob", + Twist, + TwistOptions, + frozenset({"articulation_goal"}), + frozenset({"arm"}), + "single_arm_object", + "articulation_change", + "twist", + motion_base="Press", + verifier="postcondition", + retry_mode="non_retryable", + contract_resolver_hook=_resolve_articulation_contract, + allows_target_contact=True, + ), + AtomicCapability( + "Press", + Press, + PressOptions, + frozenset({"object", "semantic_goal"}), + frozenset({"arm"}), + "single_arm_object", + "preserve", + "press", + verifier="pressed", + allows_target_contact=True, + ), + AtomicCapability( + "CoordinatedPickment", + CoordinatedPickment, + CoordinatedPickmentOptions, + frozenset({"object", "coordinated_goal"}), + frozenset({"coordinated"}), + "coordinated_object", + "coordinated_hold", + "coordinated_pickment", + config_materializer="coordinated_pickment", + verifier="coordinated_hold", + failure_classifier="grasp", + ), + AtomicCapability( + "CoordinatedPlacement", + CoordinatedPlacement, + CoordinatedPlacementOptions, + frozenset({"coordinated_placement_goal"}), + frozenset({"coordinated"}), + "coordinated_object", + "coordinated_release", + "coordinated_placement", + config_materializer="coordinated_placement", + ), + AtomicCapability( + "HandOver", + HeldObjectHandOver, + HeldObjectHandOverOptions, + frozenset({"handover_goal"}), + frozenset({"coordinated"}), + "coordinated_object", + "transfer_hold", + "handover", + config_materializer="handover", + verifier="receiver_holds", + failure_classifier="handover", + retry_mode="recover_then_retry", + ), + ) + for capability in definitions: + registry.register(capability) + + return registry + + +def capability_precondition( + capability: AtomicCapability, + *, + object_uid: str, + actor: Mapping[str, Any], + target_binding: Mapping[str, Any], +) -> dict[str, Any]: + """Build the generic live precondition used to authorize a retry.""" + if target_binding.get("single_release", False): + # Opening a gripper is idempotent. A retry remains safe when the first + # attempt physically released the object but failed terminal tracking. + return {} + if target_binding.get("coordinated_release_role") is not None: + # Opening a gripper is idempotent. A retry must remain legal when one + # hand opened on the first attempt and the physical dual-hold predicate + # therefore no longer holds. + return {} + if capability.state_effect == "coordinated_release": + return {"type": "held_by_both_grippers", "object": object_uid} + if capability.state_effect in {"preserve_hold", "release", "transfer_hold"}: + result = {"type": "object_held", "object": object_uid} + arm = target_binding.get("transfer_arm") + if arm is None and actor.get("mode") in {"required", "preferred"}: + arm = actor.get("arm") + if isinstance(arm, str) and arm: + result["arm"] = arm + return result + return {} + + +def _resolve_default_contract( + capability: AtomicCapability, node: Mapping[str, Any] +) -> ResolvedActionContract: + object_uid = _required_string(node.get("object_uid"), "node.object_uid") + actor = node.get("actor", {}) + if not isinstance(actor, Mapping): + raise ValueError("Action Contract resolution requires a mapping actor.") + binding = node.get("target_binding", {}) + if not isinstance(binding, Mapping): + raise ValueError("Action Contract resolution requires a target_binding.") + arms = _actor_arms(actor) + arm = arms[0] if len(arms) == 1 else None + arm_claims = tuple(ResourceClaim(f"arm:{item}") for item in arms) + object_claim = ResourceClaim(f"object:{object_uid}") + payload_claims = _payload_resource_claims(binding, object_uid) + + if capability.name == "PickUp": + required_arm = _required_arm(arm, capability.name) + return ResolvedActionContract( + requires=( + StateAtom("arm_free", arm=required_arm), + StateAtom("object_free", object_uid=object_uid), + ), + effects=( + StateEffect("delete", StateAtom("arm_free", arm=required_arm)), + StateEffect("delete", StateAtom("object_free", object_uid=object_uid)), + StateEffect( + "add", + StateAtom("object_held", object_uid=object_uid, arm=required_arm), + ), + ), + claims=( + ResourceClaim(f"arm:{required_arm}", lifetime="until_release"), + ResourceClaim(f"object:{object_uid}", lifetime="until_release"), + ) + + payload_claims, + ) + if capability.name == "MoveHeldObject": + required_arm = _required_arm(arm, capability.name) + terminal_hold = binding.get("terminal_hold", False) + if not isinstance(terminal_hold, bool): + raise TypeError("MoveHeldObject terminal_hold must be a boolean.") + return ResolvedActionContract( + requires=( + StateAtom("object_held", object_uid=object_uid, arm=required_arm), + ), + claims=( + ResourceClaim(f"arm:{required_arm}", lifetime="until_release"), + ResourceClaim(f"object:{object_uid}", lifetime="until_release"), + ) + + payload_claims, + completion="terminal_barrier" if terminal_hold else "ordinary", + ) + if capability.name == "Place": + required_arm = _required_arm(arm, capability.name) + return ResolvedActionContract( + requires=( + StateAtom("object_held", object_uid=object_uid, arm=required_arm), + ), + effects=( + StateEffect( + "delete", + StateAtom("object_held", object_uid=object_uid, arm=required_arm), + ), + StateEffect("add", StateAtom("arm_free", arm=required_arm)), + StateEffect("add", StateAtom("object_free", object_uid=object_uid)), + ), + claims=arm_claims + (object_claim,) + payload_claims, + ) + if capability.name == "HandOver": + transfer = _required_string( + binding.get("transfer_arm"), "target_binding.transfer_arm" + ) + receive = _required_string( + binding.get("receive_arm"), "target_binding.receive_arm" + ) + if transfer == receive: + raise ValueError("HandOver requires distinct transfer and receive arms.") + return ResolvedActionContract( + requires=( + StateAtom("object_held", object_uid=object_uid, arm=transfer), + StateAtom("arm_free", arm=receive), + ), + effects=( + StateEffect( + "delete", + StateAtom("object_held", object_uid=object_uid, arm=transfer), + ), + StateEffect("delete", StateAtom("arm_free", arm=receive)), + StateEffect("add", StateAtom("arm_free", arm=transfer)), + StateEffect( + "add", + StateAtom("object_held", object_uid=object_uid, arm=receive), + ), + StateEffect( + "add", StateAtom("handover_complete", object_uid=object_uid) + ), + ), + claims=( + ResourceClaim(f"arm:{transfer}"), + ResourceClaim(f"arm:{receive}", lifetime="until_release"), + ResourceClaim(f"object:{object_uid}", lifetime="until_release"), + ), + ) + if capability.name == "CoordinatedPickment": + coordinated_arms = _coordinated_arms(arms, capability.name) + requires = tuple(StateAtom("arm_free", arm=item) for item in coordinated_arms) + effects = tuple( + StateEffect("delete", StateAtom("arm_free", arm=item)) + for item in coordinated_arms + ) + ( + StateEffect("delete", StateAtom("object_free", object_uid=object_uid)), + StateEffect( + "add", StateAtom("object_coordinated_held", object_uid=object_uid) + ), + ) + claims = tuple( + ResourceClaim(f"arm:{item}", lifetime="until_release") + for item in coordinated_arms + ) + ( + ResourceClaim(f"object:{object_uid}", lifetime="until_release"), + ) + return ResolvedActionContract( + requires=requires + (StateAtom("object_free", object_uid=object_uid),), + effects=effects, + claims=claims + payload_claims, + ) + if capability.name == "CoordinatedPlacement": + coordinated_arms = _coordinated_arms(arms, capability.name) + effects = ( + StateEffect( + "delete", StateAtom("object_coordinated_held", object_uid=object_uid) + ), + StateEffect("add", StateAtom("object_free", object_uid=object_uid)), + ) + tuple( + StateEffect("add", StateAtom("arm_free", arm=item)) + for item in coordinated_arms + ) + claims = tuple(ResourceClaim(f"arm:{item}") for item in coordinated_arms) + ( + object_claim, + ) + return ResolvedActionContract( + requires=(StateAtom("object_coordinated_held", object_uid=object_uid),), + effects=effects, + claims=claims + payload_claims, + ) + + requirements: tuple[StateAtom, ...] = () + if capability.state_effect == "coordinated_hold": + requirements = (StateAtom("object_free", object_uid=object_uid),) + elif capability.state_effect == "coordinated_release": + requirements = (StateAtom("object_coordinated_held", object_uid=object_uid),) + elif capability.resource_mode in {"single_arm", "single_arm_object"}: + required_arm = _required_arm(arm, capability.name) + requirements = (StateAtom("arm_free", arm=required_arm),) + claims = arm_claims + if "object" in capability.resource_mode: + claims += (object_claim,) + return ResolvedActionContract( + requires=requirements, + claims=claims + payload_claims, + ) + + +def _payload_resource_claims( + binding: Mapping[str, Any], object_uid: str +) -> tuple[ResourceClaim, ...]: + """Resolve exclusive claims for objects physically carried by a carrier.""" + raw_payloads = binding.get("payloads", ()) + if not isinstance(raw_payloads, Sequence) or isinstance( + raw_payloads, (str, bytes, bytearray) + ): + raise ValueError("target_binding.payloads must be a list.") + payload_uids: list[str] = [] + for index, raw_payload in enumerate(raw_payloads): + value = ( + raw_payload.get("object") + if isinstance(raw_payload, Mapping) + else raw_payload + ) + if not isinstance(value, str) or not value: + raise ValueError( + f"target_binding.payloads[{index}] requires an object UID." + ) + if value == object_uid: + raise ValueError("An AtomicAction carrier cannot be its own payload.") + payload_uids.append(value) + if len(payload_uids) != len(set(payload_uids)): + raise ValueError("target_binding payload objects must be unique.") + return tuple(ResourceClaim(f"object:{uid}") for uid in payload_uids) + + +def _resolve_end_effector_contract( + node: Mapping[str, Any], +) -> ResolvedActionContract: + object_uid = _required_string(node.get("object_uid"), "node.object_uid") + actor = node.get("actor", {}) + binding = node.get("target_binding", {}) + if not isinstance(actor, Mapping) or not isinstance(binding, Mapping): + raise ValueError("MoveEndEffector contract requires actor and target_binding.") + arm = _required_arm(_actor_arms(actor)[0], "MoveEndEffector") + if binding.get("operation") == "retreat" or node.get("role") == "cleanup": + requires = [ + StateAtom( + ( + "arm_clear" + if binding.get("requires_arm_clear", False) + or binding.get("operation") + in {"reorient_tool_down", "retreat_after_lift"} + else "arm_free" + ), + arm=arm, + ) + ] + if binding.get("source") == "handover": + requires.append(StateAtom("handover_complete", object_uid=object_uid)) + return ResolvedActionContract( + requires=tuple(requires), + effects=(StateEffect("add", StateAtom("arm_clear", arm=arm)),), + claims=(ResourceClaim(f"arm:{arm}"),), + completion="cleanup", + failure_policy="safety_required", + ) + return ResolvedActionContract( + requires=(StateAtom("arm_free", arm=arm),), + claims=(ResourceClaim(f"arm:{arm}"),), + ) + + +def _resolve_axis_align_contract(node: Mapping[str, Any]) -> ResolvedActionContract: + """Acquire one object and retain it until an explicit release action.""" + object_uid = _required_string(node.get("object_uid"), "node.object_uid") + actor = node.get("actor", {}) + if not isinstance(actor, Mapping): + raise ValueError("AxisAlign contract requires an actor mapping.") + arm = _required_arm(_actor_arms(actor)[0], "AxisAlign") + return ResolvedActionContract( + requires=( + StateAtom("arm_free", arm=arm), + StateAtom("object_free", object_uid=object_uid), + ), + effects=( + StateEffect("delete", StateAtom("arm_free", arm=arm)), + StateEffect("delete", StateAtom("object_free", object_uid=object_uid)), + StateEffect( + "add", + StateAtom("object_held", object_uid=object_uid, arm=arm), + ), + ), + claims=( + ResourceClaim(f"arm:{arm}", lifetime="until_release"), + ResourceClaim(f"object:{object_uid}", lifetime="until_release"), + ), + failure_policy="task_required", + ) + + +def _resolve_pour_contract(node: Mapping[str, Any]) -> ResolvedActionContract: + """Retain one verified holder until the E3 action chain completes.""" + object_uid = _required_string(node.get("object_uid"), "node.object_uid") + actor = node.get("actor", {}) + binding = node.get("target_binding", {}) + if not isinstance(actor, Mapping) or not isinstance(binding, Mapping): + raise ValueError("Pour contract requires actor and target_binding mappings.") + arm = _required_arm(_actor_arms(actor)[0], "Pour") + return ResolvedActionContract( + requires=(StateAtom("object_held", object_uid=object_uid, arm=arm),), + claims=( + ResourceClaim(f"arm:{arm}", lifetime="until_release"), + ResourceClaim(f"object:{object_uid}", lifetime="until_release"), + ) + + _payload_resource_claims(binding, object_uid), + completion="terminal_barrier", + failure_policy="task_required", + ) + + +def _resolve_articulation_contract( + node: Mapping[str, Any], +) -> ResolvedActionContract: + """Require one free arm and verify the observed articulation terminal state.""" + object_uid = _required_string(node.get("object_uid"), "node.object_uid") + actor = node.get("actor", {}) + if not isinstance(actor, Mapping): + raise ValueError("Articulation action contract requires an actor mapping.") + arm = _required_arm(_actor_arms(actor)[0], "articulation action") + return ResolvedActionContract( + requires=(StateAtom("arm_free", arm=arm),), + claims=( + ResourceClaim(f"arm:{arm}"), + ResourceClaim(f"object:{object_uid}"), + ), + completion="terminal_barrier", + failure_policy="task_required", + ) + + +def _resolve_joints_contract(node: Mapping[str, Any]) -> ResolvedActionContract: + object_uid = _required_string(node.get("object_uid"), "node.object_uid") + actor = node.get("actor", {}) + if not isinstance(actor, Mapping): + raise ValueError("MoveJoints contract requires an actor mapping.") + arm = _required_arm(_actor_arms(actor)[0], "MoveJoints") + binding = node.get("target_binding", {}) + if not isinstance(binding, Mapping): + raise ValueError("MoveJoints contract requires a target_binding mapping.") + single_release = binding.get("single_release", False) + if not isinstance(single_release, bool): + raise TypeError("joint_state single_release must be a boolean.") + if single_release: + if ( + node.get("control") != "hand" + or binding.get("source") != "gripper_open" + or binding.get("coordinated_release_role") is not None + ): + raise ValueError( + "Single-arm MoveJoints release requires a hand action targeting " + "gripper_open without a coordinated release role." + ) + return ResolvedActionContract( + requires=(StateAtom("object_held", object_uid=object_uid, arm=arm),), + effects=( + StateEffect( + "delete", + StateAtom("object_held", object_uid=object_uid, arm=arm), + ), + StateEffect("add", StateAtom("arm_free", arm=arm)), + StateEffect("add", StateAtom("object_free", object_uid=object_uid)), + ), + claims=( + ResourceClaim(f"arm:{arm}", lifetime="until_release"), + ResourceClaim(f"object:{object_uid}", lifetime="until_release"), + ), + failure_policy="task_required", + ) + release_role = binding.get("coordinated_release_role") + if release_role is not None: + if ( + node.get("control") != "hand" + or binding.get("source") != "gripper_open" + or not node.get("sync_group") + ): + raise ValueError( + "Coordinated MoveJoints release requires a synchronized " + "hand action targeting gripper_open." + ) + if release_role not in {"participant", "commit"}: + raise ValueError( + "coordinated_release_role must be 'participant' or 'commit'." + ) + claims = ( + ResourceClaim(f"arm:{arm}", lifetime="until_release"), + ResourceClaim(f"object:{object_uid}", lifetime="until_release"), + ) + if release_role == "participant": + return ResolvedActionContract( + requires=(StateAtom("object_coordinated_held", object_uid=object_uid),), + claims=claims, + ) + return ResolvedActionContract( + requires=(StateAtom("object_coordinated_held", object_uid=object_uid),), + effects=( + StateEffect( + "delete", + StateAtom("object_coordinated_held", object_uid=object_uid), + ), + StateEffect("add", StateAtom("object_free", object_uid=object_uid)), + StateEffect("add", StateAtom("arm_free", arm="left_arm")), + StateEffect("add", StateAtom("arm_free", arm="right_arm")), + ), + claims=claims, + ) + if node.get("control") == "hand": + return ResolvedActionContract( + requires=(StateAtom("object_held", object_uid=object_uid, arm=arm),), + claims=( + ResourceClaim(f"arm:{arm}", lifetime="until_release"), + ResourceClaim(f"object:{object_uid}", lifetime="until_release"), + ), + ) + if node.get("role") == "cleanup": + required_home = binding.get("required_home", False) + if not isinstance(required_home, bool): + raise TypeError("joint_state required_home must be a boolean.") + return ResolvedActionContract( + requires=(StateAtom("arm_clear", arm=arm),), + effects=( + StateEffect("add", StateAtom("arm_home", arm=arm)), + StateEffect("add", StateAtom("arm_free", arm=arm)), + ), + claims=(ResourceClaim(f"arm:{arm}"),), + completion="terminal_barrier", + failure_policy="safety_required" if required_home else "best_effort", + ) + return ResolvedActionContract( + requires=(StateAtom("arm_free", arm=arm),), + claims=(ResourceClaim(f"arm:{arm}"),), + ) + + +def _verify_arm_clearance( + *, + executor: Any, + step: Any, + arm: str, + outcome: Any, + attempted: torch.Tensor, +) -> torch.Tensor: + """Verify a released TCP is clear, plus the transfer side for handover.""" + policy = outcome.grounded.motion_policy + object_uid = policy.get("clearance_object_uid") + if not isinstance(object_uid, str) or not object_uid: + return attempted + transfer_arm = str(policy.get("transfer_arm", arm)) + if transfer_arm not in {"left_arm", "right_arm"}: + return torch.zeros_like(attempted) + entity = executor.env.sim.get_rigid_object(object_uid) + getter = getattr(executor.env, "get_current_xpos_agent", None) + if entity is None or not callable(getter): + return torch.zeros_like(attempted) + left, right = getter() + eef = torch.as_tensor( + left if transfer_arm == "left_arm" else right, + dtype=torch.float32, + device=executor.env.device, + ) + if eef.ndim == 2: + eef = eef.unsqueeze(0).repeat(int(executor.env.num_envs), 1, 1) + object_pose = torch.as_tensor( + entity.get_local_pose(to_matrix=True), + dtype=torch.float32, + device=executor.env.device, + ) + if object_pose.ndim == 2: + object_pose = object_pose.unsqueeze(0).repeat(int(executor.env.num_envs), 1, 1) + offset = eef[:, :3, 3] - object_pose[:, :3, 3] + distance = torch.linalg.vector_norm(offset, dim=1) + minimum_clearance = policy.get( + "minimum_clearance", + policy.get("minimum_transfer_clearance", 0.10), + ) + clear = distance >= float(minimum_clearance) + role_axis = policy.get("transfer_role_axis") + if role_axis is not None: + role_axis = torch.as_tensor( + role_axis, + dtype=offset.dtype, + device=offset.device, + ) + if role_axis.ndim == 1: + role_axis = role_axis.unsqueeze(0).repeat(int(executor.env.num_envs), 1) + lateral = torch.sum(offset * role_axis, dim=1) + clear &= lateral >= float( + policy.get("minimum_transfer_lateral_clearance", 0.06) + ) + if bool(policy.get("verify_lift_clear", False)): + target = getattr(outcome.grounded.target, "xpos", None) + if not isinstance(target, torch.Tensor): + return torch.zeros_like(attempted) + if target.ndim == 4: + target = target[:, -1] + if target.shape != eef.shape: + return torch.zeros_like(attempted) + target = target.to(dtype=eef.dtype, device=eef.device) + tolerance = float( + policy.get( + "postcondition_tolerance", + executor.runtime_policy.predicate_fallbacks["position_tolerance"], + ) + ) + clear &= ( + torch.linalg.vector_norm( + eef[:, :3, 3] - target[:, :3, 3], + dim=1, + ) + <= tolerance + ) + return attempted & clear + + +def _verify_move_joints( + *, + executor: Any, + step: Any, + arm: str, + outcome: Any, + attempted: torch.Tensor, +) -> torch.Tensor: + """Route joint effects to their dedicated physical verifier.""" + policy = outcome.grounded.motion_policy + if bool(policy.get("single_release", False)): + return _verify_single_release( + executor=executor, + step=step, + arm=arm, + outcome=outcome, + attempted=attempted, + ) + return _verify_required_home( + executor=executor, + arm=arm, + outcome=outcome, + attempted=attempted, + ) + + +def _verify_single_release( + *, + executor: Any, + step: Any, + arm: str, + outcome: Any, + attempted: torch.Tensor, +) -> torch.Tensor: + """Verify normalized hand opening plus stable object support.""" + env = executor.env + stable_support = attempted.clone() + support_reference = getattr(executor, "_support_reference_uid", None) + support_stable_for = getattr(executor, "_support_stable_for", None) + if callable(support_reference) and callable(support_stable_for): + support_uid = support_reference(step) + if not isinstance(support_uid, str) or not support_uid: + stable_support &= False + else: + stable_support &= torch.as_tensor( + support_stable_for(step, support_uid, attempted), + dtype=torch.bool, + device=env.device, + ).reshape(-1) + + upright = attempted.clone() + entity_pose = getattr(executor, "_entity_pose", None) + orientation_satisfied = getattr( + executor, + "_placement_orientation_satisfied", + None, + ) + if callable(entity_pose) and callable(orientation_satisfied): + upright &= torch.as_tensor( + orientation_satisfied(step, entity_pose(step.object_uid)), + dtype=torch.bool, + device=env.device, + ).reshape(-1) + + getter = getattr(env, "get_current_gripper_state_agent", None) + if not callable(getter) or arm not in {"left_arm", "right_arm"}: + return torch.zeros_like(attempted) + values = getter() + index = 0 if arm == "left_arm" else 1 + if not isinstance(values, (tuple, list)) or len(values) <= index: + return torch.zeros_like(attempted) + current = torch.as_tensor( + values[index], + dtype=torch.float32, + device=env.device, + ) + if current.ndim == 1: + current = current.unsqueeze(0).repeat(int(env.num_envs), 1) + expected_open = torch.as_tensor( + env.open_state, + dtype=current.dtype, + device=current.device, + ).flatten() + expected_close = torch.as_tensor( + env.close_state, + dtype=current.dtype, + device=current.device, + ).flatten() + configured = getattr(env, "agent_gripper_state_joint_indices", {}) + side = "left" if arm == "left_arm" else "right" + indices = configured.get(side) if isinstance(configured, Mapping) else None + if indices is not None: + indices = list(indices) + current = current[:, indices] + expected_open = expected_open[indices] + expected_close = expected_close[indices] + else: + repeats = ( + current.shape[-1] + expected_open.numel() - 1 + ) // expected_open.numel() + expected_open = expected_open.repeat(repeats)[: current.shape[-1]] + expected_close = expected_close.repeat(repeats)[: current.shape[-1]] + stroke = torch.linalg.vector_norm(expected_close - expected_open) + if not torch.isfinite(stroke) or stroke <= 1.0e-6: + return torch.zeros_like(attempted) + open_error_fraction = ( + torch.linalg.vector_norm( + current - expected_open.unsqueeze(0), + dim=1, + ) + / stroke + ) + gripper_profile = get_gripper_profile(getattr(env, "agent_gripper_model", "pgi")) + tolerance = float( + outcome.grounded.motion_policy.get( + "release_open_fraction_tolerance", + gripper_profile.release_open_fraction_tolerance, + ) + ) + opened = open_error_fraction <= tolerance + accepted = attempted & opened & stable_support + planner_trace = getattr(outcome, "planner_trace", None) + if isinstance(planner_trace, dict): + planner_trace["release_verification"] = { + "state_joint_indices": None if indices is None else indices, + "current_state": current.detach().cpu().tolist(), + "expected_open_state": expected_open.detach().cpu().tolist(), + "open_error_fraction": open_error_fraction.detach().cpu().tolist(), + "open_fraction_tolerance": tolerance, + "gripper_open": opened.detach().cpu().tolist(), + "support_stable": stable_support.detach().cpu().tolist(), + "upright": upright.detach().cpu().tolist(), + "accepted": accepted.detach().cpu().tolist(), + } + return accepted + + +def _verify_required_home( + *, + executor: Any, + arm: str, + outcome: Any, + attempted: torch.Tensor, +) -> torch.Tensor: + """Verify an explicit required-home effect against live arm joints.""" + policy = outcome.grounded.motion_policy + if not bool(policy.get("verify_required_home", False)): + return attempted + env = executor.env + get_part = getattr(env, "get_agent_arm_control_part", None) + if not callable(get_part): + return torch.zeros_like(attempted) + control_part = get_part(arm == "left_arm") + if not isinstance(control_part, str) or not control_part: + return torch.zeros_like(attempted) + target = getattr(outcome.grounded.target, "target", None) + if not isinstance(target, torch.Tensor): + return torch.zeros_like(attempted) + joint_ids = env.robot.get_joint_ids(name=control_part) + current = env.robot.get_qpos()[:, joint_ids] + target = target.to(dtype=current.dtype, device=current.device) + if target.ndim == 1: + target = target.unsqueeze(0).repeat(int(env.num_envs), 1) + if target.shape != current.shape: + return torch.zeros_like(attempted) + tolerance = float( + policy.get( + "postcondition_tolerance", + executor.runtime_policy.predicate_fallbacks["arm_initial_qpos_tolerance"], + ) + ) + reached = torch.all(torch.abs(current - target) <= tolerance, dim=1) + return attempted & reached + + +def _actor_arms(actor: Mapping[str, Any]) -> tuple[str, ...]: + mode = str(actor.get("mode", "auto")) + if mode == "coordinated": + arms = actor.get("arms", ()) + if not isinstance(arms, (list, tuple)): + raise ValueError("Coordinated actor arms must be a sequence.") + result = tuple(str(item) for item in arms) + if len(result) < 2 or any(not item for item in result): + raise ValueError("Coordinated actor requires at least two named arms.") + return result + if mode in {"required", "preferred"}: + return (_required_string(actor.get("arm"), "actor.arm"),) + return ("auto",) + + +def _coordinated_arms(arms: tuple[str, ...], action: str) -> tuple[str, ...]: + if len(arms) < 2: + raise ValueError(f"{action} requires a coordinated actor.") + return arms + + +def _required_arm(arm: str | None, action: str) -> str: + if arm is None: + raise ValueError(f"{action} requires exactly one arm.") + return arm + + +def _required_string(value: Any, context: str) -> str: + if not isinstance(value, str) or not value: + raise ValueError(f"{context} must be a non-empty string.") + return value + + +def _callable_name(value: Callable[..., Any] | None) -> str | None: + if value is None: + return None + module = getattr(value, "__module__", "") + name = getattr( + value, "__qualname__", getattr(value, "__name__", type(value).__name__) + ) + return f"{module}.{name}" if module else str(name) diff --git a/embodichain/gen_sim/action_engine/capabilities/builtins.py b/embodichain/gen_sim/action_engine/capabilities/builtins.py new file mode 100644 index 000000000..4011083c8 --- /dev/null +++ b/embodichain/gen_sim/action_engine/capabilities/builtins.py @@ -0,0 +1,1051 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Built-in semantic operators lowered to public atomic-action contracts.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +from typing import Any + +from embodichain.gen_sim.action_engine.domain.motion import ( + motion_policy as build_motion_policy, +) +from embodichain.gen_sim.action_engine.domain.task_contracts import ( + PLACEMENT_RELATIONS, + TERMINAL_BEHAVIORS, + TRANSPORT_DIRECTIONS, + normalize_placement_relation, +) +from embodichain.gen_sim.action_engine.orientation import ( + compile_orientation_constraint, +) + +from .registry import ( + ActionCapability, + ActionTemplate, + CapabilityRegistry, + OperatorCapability, + PhaseTemplate, +) + +__all__ = ["build_default_registry"] + +_SINGLE_ARM_PHASE_OPERATORS = frozenset( + {"build_stack", "hold_hover", "orient_object", "place_relative"} +) + + +def build_default_registry() -> CapabilityRegistry: + """Build a fresh registry containing all Action Engine v1 capabilities.""" + registry = CapabilityRegistry() + from .atomic import build_atomic_capability_registry + + for capability in build_atomic_capability_registry().catalog().values(): + registry.register_action( + ActionCapability( + str(capability["name"]), + frozenset(capability["binding_kinds"]), + frozenset(capability["controls"]), + ) + ) + + definitions = ( + OperatorCapability( + "arrange_line", + "Arrange two or more movable objects into one live-grounded line.", + _expand_arrange_line, + _build_arrange_line_phases, + expansion_topology="parallel_children", + ), + OperatorCapability( + "build_stack", + "Build one ordered vertical or nested stack.", + _expand_build_stack, + _build_single_arm_phases, + ), + OperatorCapability( + "place_relative", + "Place one object at a symbolic relation to another object.", + _expand_place_relative, + _build_single_arm_phases, + ), + OperatorCapability( + "orient_object", + "Reorient one object in place and release it in a stable pose.", + _expand_orient_object, + _build_orient_object_phases, + ), + OperatorCapability( + "coordinated_transport", + "Use both arms to pick and transport one shared object.", + _expand_coordinated_transport, + _build_coordinated_transport_phases, + ), + # These internal operators preserve runtime characterization coverage + # for public Atomic Actions. They are intentionally absent from the + # planner catalog during the five-skill first phase. + OperatorCapability( + "hold_hover", + "Internal terminal-hold compatibility operator.", + _expand_hold_hover, + _build_single_arm_phases, + lifecycle="terminal_hold", + planner_visible=False, + ), + OperatorCapability( + "press", + "Internal press compatibility operator.", + _expand_press, + _build_press_phases, + planner_visible=False, + ), + OperatorCapability( + "coordinated_place", + "Internal coordinated-placement compatibility operator.", + _expand_coordinated_place, + _build_coordinated_place_phases, + planner_visible=False, + ), + ) + for definition in definitions: + registry.register_operator(definition) + return registry + + +def _expand_arrange_line(step: Mapping[str, Any]) -> list[dict[str, Any]]: + objects = _collective_objects(step, "arrange_line", minimum=2) + goal = _goal( + step, + allowed={ + "anchor", + "axis", + "order_by", + "order_constraint", + "order_direction", + "orientation_axis", + "orientation_constraint", + "orientation_directed", + "orientation_goal", + "participation", + }, + ) + orientation_goal, orientation_axis = _orientation(goal, "arrange_line") + axis = str(goal.get("axis", "world_y")) + if axis not in {"world_x", "world_y", "table_long_axis"}: + raise ValueError("arrange_line goal.axis must be a symbolic table axis.") + anchor = str(goal.get("anchor", "table_center")) + if anchor != "table_center": + raise ValueError("arrange_line currently requires anchor='table_center'.") + order_constraint = str(goal.get("order_constraint", "free")) + if order_constraint not in {"free", "ordered"}: + raise ValueError( + "arrange_line goal.order_constraint must be 'free' or 'ordered'." + ) + order_by = str(goal.get("order_by", "explicit")) + if order_by not in {"explicit", "size", "color"}: + raise ValueError("arrange_line order_by must be explicit, size, or color.") + order_direction = str(goal.get("order_direction", "given")) + if order_direction not in {"given", "ascending", "descending"}: + raise ValueError( + "arrange_line order_direction must be given, ascending, or descending." + ) + participation = str(goal.get("participation", "auto")) + if participation not in {"auto", "both_arms"}: + raise ValueError("arrange_line participation must be auto or both_arms.") + + common_goal = { + "layout": "line", + "objects": objects, + "axis": axis, + "anchor": anchor, + "order_by": order_by, + "order_direction": order_direction, + "order_constraint": order_constraint, + "participation": participation, + "orientation_goal": orientation_goal, + "orientation_axis": orientation_axis, + **_orientation_extensions(goal), + } + actor = _single_arm_actor(step) + expanded: list[dict[str, Any]] = [] + for slot_index, object_uid in enumerate(objects): + child_goal = { + **deepcopy(common_goal), + "nominal_slot_index": slot_index, + "slot_constraint": ( + "required" if order_constraint == "ordered" else "free_reassignable" + ), + } + expanded.append( + _execution_step( + step, + child_id=f"{step['id']}__{slot_index + 1:02d}", + object_uid=object_uid, + actor=( + { + **actor, + "allocation_group": f"{step['id']}_both_arms", + } + if participation == "both_arms" and slot_index < 2 + else actor + ), + goal=child_goal, + postcondition={ + "type": "line_member_placed", + "nominal_slot_index": slot_index, + "slot_constraint": child_goal["slot_constraint"], + "order_constraint": order_constraint, + }, + ) + ) + return expanded + + +def _expand_build_stack(step: Mapping[str, Any]) -> list[dict[str, Any]]: + objects = _collective_objects(step, "build_stack", minimum=1) + goal = _goal( + step, + allowed={ + "anchor", + "orientation_axis", + "orientation_constraint", + "orientation_directed", + "orientation_goal", + "stack_mode", + }, + ) + orientation_goal, orientation_axis = _orientation(goal, "build_stack") + stack_mode = str(goal.get("stack_mode", "on_top")) + if stack_mode not in {"on_top", "nested"}: + raise ValueError("build_stack goal.stack_mode must be 'on_top' or 'nested'.") + anchor = goal.get("anchor", "table_center") + if not isinstance(anchor, str) or not anchor: + raise ValueError("build_stack goal.anchor must be an object or table_center.") + + actor = _single_arm_actor(step) + expanded: list[dict[str, Any]] = [] + for layer_index, object_uid in enumerate(objects): + reference = objects[layer_index - 1] if layer_index else anchor + support_reference = "table" if reference == "table_center" else reference + child_goal: dict[str, Any] = { + "relation": ( + "inside" if stack_mode == "nested" and layer_index > 0 else "on" + ), + "reference_object": support_reference, + "reference_state": "live", + "layer_index": layer_index, + "stack_mode": stack_mode, + "orientation_goal": orientation_goal, + "orientation_axis": orientation_axis, + **_orientation_extensions(goal), + } + expanded.append( + _execution_step( + step, + child_id=f"{step['id']}__{layer_index + 1:02d}", + object_uid=object_uid, + actor=actor, + goal=child_goal, + postcondition={ + "type": "stack_layer_supported", + "layer_index": layer_index, + "reference_object": support_reference, + }, + ) + ) + return expanded + + +def _expand_place_relative(step: Mapping[str, Any]) -> list[dict[str, Any]]: + object_uid = _single_object(step, "place_relative") + goal = _goal( + step, + allowed={ + "orientation_axis", + "orientation_constraint", + "orientation_directed", + "orientation_goal", + "orientation_reference_object", + "payloads", + "reference_object", + "reference_state", + "relation", + "slot", + }, + ) + orientation_goal, orientation_axis = _orientation(goal, "place_relative") + reference = _required_string(goal, "reference_object", "place_relative") + relation = normalize_placement_relation(goal.get("relation", "on")) + normalized_goal = { + "reference_object": reference, + "reference_state": str(goal.get("reference_state", "live")), + "relation": relation, + "orientation_goal": orientation_goal, + "orientation_axis": orientation_axis, + **_orientation_extensions(goal), + "slot": str(goal.get("slot", "auto")), + } + if normalized_goal["reference_state"] not in {"initial", "live"}: + raise ValueError("place_relative reference_state must be 'initial' or 'live'.") + if normalized_goal["slot"] not in {"auto", "left", "center", "right"}: + raise ValueError("place_relative slot must be left, center, right, or auto.") + if "orientation_reference_object" in goal: + normalized_goal["orientation_reference_object"] = goal[ + "orientation_reference_object" + ] + payloads = _normalize_payloads( + goal.get("payloads", []), + object_uid, + "place_relative", + ) + if payloads: + normalized_goal["payloads"] = payloads + return [ + _execution_step( + step, + object_uid=object_uid, + actor=_single_arm_actor(step), + goal=normalized_goal, + postcondition={ + "type": "semantic_goal", + "relation": relation, + "reference_object": reference, + }, + ) + ] + + +def _expand_hold_hover(step: Mapping[str, Any]) -> list[dict[str, Any]]: + object_uid = _single_object(step, "hold_hover") + goal = _goal( + step, + allowed={ + "orientation_axis", + "orientation_constraint", + "orientation_directed", + "orientation_goal", + "reference_object", + "reference_state", + }, + ) + orientation_goal, orientation_axis = _orientation( + goal, + "hold_hover", + ) + reference = str(goal.get("reference_object", object_uid)) + return [ + _execution_step( + step, + object_uid=object_uid, + actor=_single_arm_actor(step), + goal={ + "relation": "held_above_initial", + "reference_object": reference, + "reference_state": str(goal.get("reference_state", "initial")), + "orientation_goal": orientation_goal, + "orientation_axis": orientation_axis, + **_orientation_extensions(goal), + }, + postcondition={"type": "object_held", "object": object_uid}, + ) + ] + + +def _expand_orient_object(step: Mapping[str, Any]) -> list[dict[str, Any]]: + """Normalize an in-place orientation request into one executable step. + + Keeping the target position symbolic is important: runtime observes the + object's live position immediately before grounding, so prior independent + operations and simulator settling cannot make this plan stale. + """ + object_uid = _single_object(step, "orient_object") + goal = _goal( + step, + allowed={ + "orientation_axis", + "orientation_constraint", + "orientation_directed", + "orientation_goal", + "position_anchor", + "support_object", + "upright_local_axis", + }, + ) + orientation_goal, orientation_axis = _orientation(goal, "orient_object") + if orientation_goal not in {"upright", "lay_flat", "axis_align"}: + raise ValueError( + "orient_object requires upright, lay_flat, or axis_align orientation." + ) + position_anchor = str(goal.get("position_anchor", "initial_xy")) + if position_anchor not in {"initial_xy", "live_xy"}: + raise ValueError( + "orient_object position_anchor must be 'initial_xy' or 'live_xy'." + ) + upright_local_axis = str(goal.get("upright_local_axis", "auto")) + if upright_local_axis not in {"auto", "long_axis", "x", "y", "z"}: + raise ValueError( + "orient_object upright_local_axis must be auto, long_axis, x, y, or z." + ) + support_object = str(goal.get("support_object", "table")) + if not support_object: + raise ValueError("orient_object support_object must be a non-empty string.") + return [ + _execution_step( + step, + object_uid=object_uid, + actor=_single_arm_actor(step), + goal={ + "relation": "none", + "reference_state": "live", + "orientation_goal": orientation_goal, + "orientation_axis": orientation_axis, + **_orientation_extensions(goal), + "position_anchor": position_anchor, + "support_object": support_object, + "upright_local_axis": upright_local_axis, + }, + postcondition={ + "type": "semantic_goal", + "relation": "none", + "orientation_goal": orientation_goal, + }, + ) + ] + + +def _expand_coordinated_transport( + step: Mapping[str, Any], +) -> list[dict[str, Any]]: + object_uid = _single_object(step, "coordinated_transport") + goal = _goal( + step, + allowed={ + "direction", + "orientation_axis", + "orientation_constraint", + "orientation_directed", + "orientation_goal", + "payloads", + "reference_object", + "relation", + "terminal_behavior", + }, + ) + orientation_goal, orientation_axis = _orientation( + goal, + "coordinated_transport", + ) + terminal_behavior = str(goal.get("terminal_behavior", "hold")) + if terminal_behavior not in TERMINAL_BEHAVIORS - {"none"}: + raise ValueError( + "coordinated_transport terminal_behavior must be 'hold' or 'place'." + ) + direction = str(goal.get("direction", "none")) + if direction not in TRANSPORT_DIRECTIONS: + raise ValueError( + f"coordinated_transport direction {direction!r} is unsupported." + ) + relation = goal.get("relation") + if relation is not None and str(relation) not in PLACEMENT_RELATIONS: + raise ValueError( + f"coordinated_transport relation {str(relation)!r} is unsupported." + ) + normalized_goal = { + "direction": direction, + "terminal_behavior": terminal_behavior, + "orientation_goal": orientation_goal, + "orientation_axis": orientation_axis, + **_orientation_extensions(goal), + } + normalized_payloads = _normalize_payloads( + goal.get("payloads", []), + object_uid, + "coordinated_transport", + ) + if normalized_payloads: + normalized_goal["payloads"] = normalized_payloads + for key in ("reference_object", "relation"): + if key in goal: + normalized_goal[key] = goal[key] + postcondition = ( + {"type": "semantic_goal", "relation": normalized_goal.get("relation", "at")} + if terminal_behavior == "place" + else {"type": "held_by_both_grippers", "object": object_uid} + ) + return [ + _execution_step( + step, + object_uid=object_uid, + actor={"mode": "coordinated", "arms": ["left_arm", "right_arm"]}, + goal=normalized_goal, + postcondition=postcondition, + ) + ] + + +def _expand_press(step: Mapping[str, Any]) -> list[dict[str, Any]]: + object_uid = _single_object(step, "press") + goal = _goal( + step, + allowed={"interaction", "reference_object", "terminal_state"}, + ) + return [ + _execution_step( + step, + object_uid=object_uid, + actor=_single_arm_actor(step), + goal={ + "interaction": str(goal.get("interaction", "press")), + "terminal_state": str(goal.get("terminal_state", "activated")), + **( + {"reference_object": goal["reference_object"]} + if "reference_object" in goal + else {} + ), + }, + postcondition={ + "type": "pressed", + "object": object_uid, + "terminal_state": str(goal.get("terminal_state", "activated")), + }, + ) + ] + + +def _expand_coordinated_place(step: Mapping[str, Any]) -> list[dict[str, Any]]: + object_uid = _single_object(step, "coordinated_place") + goal = _goal( + step, + allowed={"relation", "release", "support_object"}, + ) + support_object = _required_string(goal, "support_object", "coordinated_place") + if support_object == object_uid: + raise ValueError("coordinated_place requires two distinct objects.") + relation = str(goal.get("relation", "on")) + if relation not in {"on", "inside"}: + raise ValueError("coordinated_place relation must be 'on' or 'inside'.") + release = goal.get("release", True) + if not isinstance(release, bool): + raise ValueError("coordinated_place goal.release must be boolean.") + return [ + _execution_step( + step, + object_uid=object_uid, + actor={"mode": "coordinated", "arms": ["left_arm", "right_arm"]}, + goal={ + "support_object": support_object, + "relation": relation, + "release": release, + }, + postcondition={ + "type": "coordinated_placed", + "object": object_uid, + "support_object": support_object, + "relation": relation, + }, + ) + ] + + +def _build_arrange_line_phases( + step: Mapping[str, Any], +) -> tuple[PhaseTemplate, ...]: + return ( + _pickup_phase(step), + _move_phase(step, "staging"), + _move_phase(step, "final"), + *_release_retreat_home(step), + ) + + +def _build_single_arm_phases( + step: Mapping[str, Any], +) -> tuple[PhaseTemplate, ...]: + if step["operator"] not in _SINGLE_ARM_PHASE_OPERATORS: + raise ValueError(f"Unexpected single-arm operator {step['operator']!r}.") + phases: tuple[PhaseTemplate, ...] = ( + _pickup_phase(step), + _move_phase(step), + ) + if step["operator"] == "hold_hover": + return phases + ( + PhaseTemplate( + name="keep_holding", + state_semantic=f"`{step['object']}` remains held", + actions=( + ActionTemplate( + "MoveJoints", + {"kind": "joint_state", "source": "gripper_closed"}, + build_motion_policy(), + control="hand", + ), + ), + ), + ) + return phases + _release_retreat_home(step) + + +def _build_orient_object_phases( + step: Mapping[str, Any], +) -> tuple[PhaseTemplate, ...]: + """Rotate at a clearance waypoint before descending to the support.""" + upright = build_motion_policy(("orientation", "upright")) + return ( + _pickup_phase(step, motion_policy=upright), + _move_phase( + step, + "staging", + motion_policy=upright, + ), + _move_phase( + step, + "final", + motion_policy=upright, + ), + *_release_retreat_home( + step, + release_policy=upright, + retreat_policy=upright, + ), + ) + + +def _build_coordinated_transport_phases( + step: Mapping[str, Any], +) -> tuple[PhaseTemplate, ...]: + phases: tuple[PhaseTemplate, ...] = ( + PhaseTemplate( + name="coordinated_transport", + state_semantic=f"`{step['object']}` reaches its coordinated goal", + actions=( + ActionTemplate( + "CoordinatedPickment", + { + "kind": "coordinated_goal", + "semantic_step": step["id"], + "object": step["object"], + "payloads": deepcopy(step["goal"].get("payloads", [])), + }, + build_motion_policy(), + control="coordinated", + ), + ), + ), + ) + if step["goal"]["terminal_behavior"] != "place": + return phases + release = ( + PhaseTemplate( + name="dual_release", + state_semantic="Both grippers release the transported object", + actions=tuple( + ActionTemplate( + "MoveJoints", + { + "kind": "joint_state", + "source": "gripper_open", + "coordinated_release_role": release_role, + }, + build_motion_policy(), + control="hand", + actor={"mode": "required", "arm": arm}, + ) + for arm, release_role in ( + ("left_arm", "participant"), + ("right_arm", "commit"), + ) + ), + ), + ) + lifts = tuple( + PhaseTemplate( + name=f"{side}_lift_clear", + state_semantic=f"The {side} end effector lifts clear of the object", + actions=( + ActionTemplate( + "MoveEndEffector", + { + "kind": "policy_pose", + "source": "release", + "operation": "lift_clear", + "verify_lift_clear": True, + }, + build_motion_policy(), + actor={"mode": "required", "arm": f"{side}_arm"}, + ), + ), + ) + for side in ("left", "right") + ) + homes = tuple( + PhaseTemplate( + name=f"{side}_home", + state_semantic=f"The {side} arm returns to its initial state", + actions=( + ActionTemplate( + "MoveJoints", + { + "kind": "joint_state", + "source": "initial", + "operation": "e5_home", + "required_home": True, + }, + build_motion_policy(), + actor={"mode": "required", "arm": f"{side}_arm"}, + ), + ), + ) + for side in ("left", "right") + ) + return phases + release + lifts + homes + + +def _build_press_phases(step: Mapping[str, Any]) -> tuple[PhaseTemplate, ...]: + return ( + PhaseTemplate( + name="press", + state_semantic=f"`{step['object']}` has been pressed", + actions=( + ActionTemplate( + "Press", + { + "kind": "semantic_goal", + "semantic_step": step["id"], + "object": step["object"], + "interaction": "press", + }, + build_motion_policy(), + ), + ), + ), + ) + + +def _build_coordinated_place_phases( + step: Mapping[str, Any], +) -> tuple[PhaseTemplate, ...]: + return ( + PhaseTemplate( + name="dual_pick_up", + state_semantic=( + f"`{step['object']}` is held by the left arm and " + f"`{step['goal']['support_object']}` is held by the right arm" + ), + actions=( + ActionTemplate( + "PickUp", + { + "kind": "object", + "object": step["object"], + "affordance": "antipodal", + }, + build_motion_policy(), + actor={"mode": "required", "arm": "left_arm"}, + ), + ActionTemplate( + "PickUp", + { + "kind": "object", + "object": step["goal"]["support_object"], + "affordance": "antipodal", + }, + build_motion_policy(), + actor={"mode": "required", "arm": "right_arm"}, + ), + ), + ), + PhaseTemplate( + name="coordinated_place", + state_semantic=( + f"`{step['object']}` is coordinated with " + f"`{step['goal']['support_object']}`" + ), + actions=( + ActionTemplate( + "CoordinatedPlacement", + { + "kind": "coordinated_placement_goal", + "semantic_step": step["id"], + "placing_object": step["object"], + "support_object": step["goal"]["support_object"], + }, + build_motion_policy(), + control="coordinated", + ), + ), + ), + ) + + +def _pickup_phase( + step: Mapping[str, Any], + *, + motion_policy: Mapping[str, Any] | None = None, +) -> PhaseTemplate: + payloads = deepcopy(step["goal"].get("payloads", [])) + return PhaseTemplate( + name="pick_up", + state_semantic=f"Holding `{step['object']}`", + actions=( + ActionTemplate( + "PickUp", + { + "kind": "object", + "object": step["object"], + "affordance": "antipodal", + **({"payloads": payloads} if payloads else {}), + }, + motion_policy or build_motion_policy(), + ), + ), + ) + + +def _move_phase( + step: Mapping[str, Any], + phase: str | None = None, + *, + motion_policy: Mapping[str, Any] | None = None, +) -> PhaseTemplate: + target_binding = { + "kind": "semantic_goal", + "semantic_step": step["id"], + } + if phase is not None: + target_binding["phase"] = phase + payloads = deepcopy(step["goal"].get("payloads", [])) + if payloads: + target_binding["payloads"] = payloads + return PhaseTemplate( + name=f"move_to_{phase or 'semantic_goal'}", + state_semantic=f"`{step['object']}` is held at {phase or 'its semantic goal'}", + actions=( + ActionTemplate( + "MoveHeldObject", + target_binding, + motion_policy or build_motion_policy(), + ), + ), + ) + + +def _release_retreat_home( + step: Mapping[str, Any], + *, + release_policy: Mapping[str, Any] | None = None, + retreat_policy: Mapping[str, Any] | None = None, +) -> tuple[PhaseTemplate, ...]: + payloads = deepcopy(step["goal"].get("payloads", [])) + return ( + PhaseTemplate( + name="release", + state_semantic=f"`{step['object']}` is released at its semantic goal", + actions=( + ActionTemplate( + "Place", + { + "kind": "current_held_pose", + **({"payloads": payloads} if payloads else {}), + }, + release_policy or build_motion_policy(), + ), + ), + ), + PhaseTemplate( + name="retreat", + state_semantic=f"The end effector retreats from `{step['object']}`", + actions=( + ActionTemplate( + "MoveEndEffector", + { + "kind": "policy_pose", + "source": "release", + "operation": "retreat", + }, + retreat_policy or build_motion_policy(), + ), + ), + ), + PhaseTemplate( + name="home", + state_semantic="The selected arm returns to its initial state", + actions=( + ActionTemplate( + "MoveJoints", + {"kind": "joint_state", "source": "initial"}, + build_motion_policy(), + ), + ), + ), + ) + + +def _dual_arm_phase( + name: str, + state_semantic: str, + action_class: str, + target_binding: Mapping[str, Any], + motion_policy: Mapping[str, Any], + *, + control: str = "arm", +) -> PhaseTemplate: + return PhaseTemplate( + name=name, + state_semantic=state_semantic, + actions=tuple( + ActionTemplate( + action_class, + target_binding, + motion_policy, + control=control, + actor={"mode": "required", "arm": arm}, + ) + for arm in ("left_arm", "right_arm") + ), + ) + + +def _execution_step( + parent: Mapping[str, Any], + *, + object_uid: str, + actor: Mapping[str, Any], + goal: Mapping[str, Any], + postcondition: Mapping[str, Any], + child_id: str | None = None, +) -> dict[str, Any]: + return { + "id": child_id or parent["id"], + "parent_step_id": parent["id"], + "operator": parent["operator"], + "object": object_uid, + "actor": deepcopy(dict(actor)), + "goal": deepcopy(dict(goal)), + "depends_on": [], + "postcondition": deepcopy(dict(postcondition)), + "edge_ids": [], + } + + +def _single_object(step: Mapping[str, Any], operator: str) -> str: + if "object" not in step: + raise ValueError(f"{operator} requires one 'object', not 'objects'.") + return str(step["object"]) + + +def _collective_objects( + step: Mapping[str, Any], + operator: str, + *, + minimum: int, +) -> list[str]: + if "objects" not in step: + raise ValueError(f"{operator} requires an 'objects' list.") + objects = [str(value) for value in step["objects"]] + if len(objects) < minimum: + raise ValueError(f"{operator} requires at least {minimum} object(s).") + return objects + + +def _single_arm_actor(step: Mapping[str, Any]) -> dict[str, Any]: + actor = deepcopy(dict(step["actor"])) + if actor["mode"] == "coordinated": + raise ValueError(f"{step['operator']} requires one arm, not coordinated arms.") + if actor["mode"] == "required": + arm = str(actor["arm"]) + if arm in {"left", "right"}: + actor["arm"] = f"{arm}_arm" + return actor + + +def _goal(step: Mapping[str, Any], *, allowed: set[str]) -> dict[str, Any]: + goal = deepcopy(dict(step["goal"])) + unknown = sorted(set(goal) - allowed) + if unknown: + raise ValueError( + f"{step['operator']} goal contains unsupported fields: {unknown}." + ) + return goal + + +def _normalize_payloads( + value: Any, + carrier_uid: str, + operator: str, +) -> list[dict[str, str]]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes, bytearray)): + raise ValueError(f"{operator} payloads must be a list.") + if len(value) > 4: + raise ValueError(f"{operator} supports at most four payloads.") + result = [] + for index, payload in enumerate(value): + item = {"object": payload} if isinstance(payload, str) else dict(payload) + uid = item.get("object") + slot = str(item.get("slot", "auto")) + if not isinstance(uid, str) or not uid: + raise ValueError(f"payloads[{index}] requires an object UID.") + if uid == carrier_uid: + raise ValueError(f"A {operator} carrier cannot be its own payload.") + if slot not in {"left", "right", "center", "auto"}: + raise ValueError(f"Unsupported payload slot {slot!r}.") + result.append({"object": uid, "slot": slot}) + payload_uids = [item["object"] for item in result] + if len(payload_uids) != len(set(payload_uids)): + raise ValueError(f"{operator} payload objects must be unique.") + return result + + +def _required_string( + value: Mapping[str, Any], + key: str, + operator: str, +) -> str: + result = value.get(key) + if not isinstance(result, str) or not result.strip(): + raise ValueError(f"{operator} goal.{key} must be a non-empty string.") + return result + + +def _orientation( + goal: Mapping[str, Any], + operator: str, +) -> tuple[str, str]: + orientation_goal = str(goal.get("orientation_goal", "none")) + orientation_axis = str(goal.get("orientation_axis", "none")) + allowed_goals = {"none", "preserve", "upright", "lay_flat", "axis_align"} + if orientation_goal not in allowed_goals: + raise ValueError( + f"{operator} orientation_goal {orientation_goal!r} is unsupported." + ) + if orientation_axis not in {"none", "x", "y", "long_axis", "short_axis"}: + raise ValueError( + f"{operator} orientation_axis {orientation_axis!r} is unsupported." + ) + if orientation_goal == "axis_align" and orientation_axis == "none": + raise ValueError(f"{operator} axis_align requires an orientation_axis.") + compile_orientation_constraint(goal) + return orientation_goal, orientation_axis + + +def _orientation_extensions(goal: Mapping[str, Any]) -> dict[str, Any]: + """Copy optional composable fields after operator-level validation.""" + return { + key: deepcopy(goal[key]) + for key in ("orientation_constraint", "orientation_directed") + if key in goal + } diff --git a/embodichain/gen_sim/action_engine/capabilities/held_hand_over.py b/embodichain/gen_sim/action_engine/capabilities/held_hand_over.py new file mode 100644 index 000000000..d269dcb4f --- /dev/null +++ b/embodichain/gen_sim/action_engine/capabilities/held_hand_over.py @@ -0,0 +1,660 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""GenSim-local handover for transferring an already-held object.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar + +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionOptions, + ActionPlan, + AntipodalAffordance, + AtomicAction, + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_COMMAND, + GraspGoal, + HeldObjectState, + JointPositionCommand, + JointPositionTarget, + ObjectSemantics, + OPEN_COMMAND, + PlanningContext, + ResolvedActionRequest, + SkillBindingContract, + StateDelta, + TimedTrajectory, +) +from embodichain.lab.sim.atomic_actions.goals import ( + PoseGoalValue, + collect_scene_dependencies, + resolve_pose_goal, + validate_pose_goal, +) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) +from embodichain.lab.sim.atomic_actions.primitives._helpers import ( + assemble_full_robot_trajectory, + plan_named_arm_trajectory, + repeat_qpos, + require_shared_task_state_key, + resolve_batched_pose, +) +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + interpolate_hand_qpos, + translate_pose_world, +) +from embodichain.lab.sim.planners.utils import normalize_success_mask +from embodichain.utils.math import pose_inv + + +@dataclass(frozen=True, slots=True, eq=False) +class HeldObjectHandOverOptions(ActionOptions): + """Per-invocation behavior for transferring an already-held object.""" + + receive_pick_object_part: str = "bottom" + middle_object_pose: PoseGoalValue | None = None + final_object_pose: PoseGoalValue | None = None + receive_approach_direction: torch.Tensor = torch.tensor([0.0, 0.0, -1.0]) + pre_grasp_distance: float = 0.10 + lift_height: float = 0.08 + hand_interp_steps: int = 10 + hold_steps: int = 4 + retreat_steps: int = 24 + + def __post_init__(self) -> None: + if self.receive_pick_object_part not in frozenset({"center", "top", "bottom"}): + raise ValueError( + "receive_pick_object_part must be 'center', 'top', or 'bottom'." + ) + direction = self.receive_approach_direction + if ( + not isinstance(direction, torch.Tensor) + or direction.shape != (3,) + or not torch.isfinite(direction).all() + or torch.linalg.vector_norm(direction) <= 1.0e-6 + ): + raise ValueError( + "receive_approach_direction must be a finite non-zero (3,) tensor." + ) + if self.pre_grasp_distance < 0.0 or self.lift_height < 0.0: + raise ValueError("Handover distances must be non-negative.") + for name in ("hand_interp_steps", "hold_steps", "retreat_steps"): + value = getattr(self, name) + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a non-negative integer.") + + object.__setattr__(self, "receive_approach_direction", direction.clone()) + for name in ("middle_object_pose", "final_object_pose"): + value = getattr(self, name) + if value is None: + continue + validate_pose_goal(value, name, allow_waypoints=False) + object.__setattr__( + self, + name, + value.clone() if isinstance(value, torch.Tensor) else value.snapshot(), + ) + + +@dataclass(frozen=True, slots=True) +class _HandoverResources: + source_state_key: str + destination_state_key: str + source_arm: JointPositionTarget + destination_arm: JointPositionTarget + source_hand: JointPositionTarget + destination_hand: JointPositionTarget + source_hand_open_qpos: torch.Tensor + source_hand_grasp_qpos: torch.Tensor + destination_hand_open_qpos: torch.Tensor + destination_hand_grasp_qpos: torch.Tensor + + +class HeldObjectHandOver(AtomicAction[GraspGoal, HeldObjectHandOverOptions]): + """Transfer an existing attachment while leaving the receiver holding it.""" + + skill_id: ClassVar[str] = "hand_over" + GoalType: ClassVar[type] = GraspGoal + OptionsType: ClassVar[type] = HeldObjectHandOverOptions + manipulator_roles: ClassVar[tuple[str, ...]] = ("source", "destination") + end_effector_roles: ClassVar[tuple[str, ...]] = ("source", "destination") + open_loop: ClassVar[bool] = True + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "source", + motion_capabilities=frozenset( + {CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY} + ), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + make_manipulation_slot( + "destination", + motion_capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointResourceSlots(("source", "destination")),), + ) + + def __init__( + self, default_options: HeldObjectHandOverOptions | None = None + ) -> None: + super().__init__(default_options) + + def _scene_dependencies( + self, + request: ResolvedActionRequest[GraspGoal, HeldObjectHandOverOptions], + ) -> tuple[str, ...]: + """Return scene entities whose poses materially affect this plan.""" + return collect_scene_dependencies( + tuple( + value + for value in ( + request.skill_options.middle_object_pose, + request.skill_options.final_object_pose, + ) + if value is not None + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[GraspGoal, HeldObjectHandOverOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + options = request.skill_options + self._require_exchange_pose(options) + resources = self._resolve_resources(request) + + if ( + request.motion_policy.strategy == "motion_gen" + and self.motion_generator.planner.cfg.planner_type == "curobo" + ): + raise ValueError( + "Coordinated dual-arm planning is not supported by cuRobo." + ) + + held = context.get_held_object(resources.source_state_key) + if held is None: + raise ValueError( + "HeldObjectHandOver requires the source participant to hold an object." + ) + self._require_same_object(goal.semantics, held.semantics) + eligible = context.task.exclusive_held_object_mask(resources.source_state_key) + if not eligible.any(): + return self.failed_plan( + request, + context, + message="Source object must be held exclusively.", + ) + + source_start, destination_start = self._start_qpos(context, resources) + object_to_source = self._pose(held.object_to_eef, "held.object_to_eef") + source_eef = self.robot.compute_fk( + qpos=source_start, + name=resources.source_arm.control_part, + to_matrix=True, + ) + current_object_pose = torch.bmm(source_eef, pose_inv(object_to_source)) + assert options.middle_object_pose is not None + assert options.final_object_pose is not None + middle_object_pose = self._pose( + resolve_pose_goal( + options.middle_object_pose, + context, + name="middle_object_pose", + ), + "middle_object_pose", + ) + final_object_pose = self._pose( + resolve_pose_goal( + options.final_object_pose, + context, + name="final_object_pose", + ), + "final_object_pose", + ) + middle_object_pose[:, :3, :3] = current_object_pose[:, :3, :3] + final_object_pose[:, :3, :3] = current_object_pose[:, :3, :3] + if not torch.allclose( + middle_object_pose, + final_object_pose, + atol=1.0e-5, + rtol=1.0e-5, + ): + raise ValueError( + "HeldObjectHandOver requires final_object_pose to match the exchange " + "pose so the receiver remains stationary." + ) + + source_middle_eef = torch.bmm(middle_object_pose, object_to_source) + destination_grasp, grasp_success = self._destination_grasp( + held.semantics, + middle_object_pose, + resources.destination_hand.control_part, + options, + ) + success = normalize_success_mask( + grasp_success, + num_envs=self.num_envs, + device=self.device, + name="Receiving-grasp success", + ) + success &= eligible + if not success.any(): + return self.failed_plan( + request, + context, + message="No receiving grasp was available.", + ) + + object_to_destination = torch.bmm( + pose_inv(middle_object_pose), destination_grasp + ) + destination_pre_grasp = translate_pose_world( + destination_grasp, + -destination_grasp[:, :3, 2] * options.pre_grasp_distance, + ) + source_retreat_eef = translate_pose_world( + source_middle_eef, + source_middle_eef.new_tensor([0.0, 0.0, options.lift_height]), + ) + lengths = self._segment_lengths(request.motion_policy.sample_count, options) + + segment_success, source_transfer = plan_named_arm_trajectory( + self.motion_generator, + resources.source_arm.control_part, + source_start, + source_middle_eef.unsqueeze(1), + lengths["transfer"], + request.motion_policy, + context.control_dt, + ) + success &= self._success(segment_success, "Source transfer") + segment_success, destination_approach = plan_named_arm_trajectory( + self.motion_generator, + resources.destination_arm.control_part, + destination_start, + torch.stack((destination_pre_grasp, destination_grasp), dim=1), + lengths["approach"], + request.motion_policy, + context.control_dt, + ) + success &= self._success(segment_success, "Destination approach") + source_hold = source_transfer[:, -1] + destination_hold = destination_approach[:, -1] + segment_success, source_retreat = plan_named_arm_trajectory( + self.motion_generator, + resources.source_arm.control_part, + source_hold, + source_retreat_eef.unsqueeze(1), + lengths["retreat"], + request.motion_policy, + context.control_dt, + ) + success &= self._success(segment_success, "Source retreat") + if not success.any(): + return self.failed_plan( + request, + context, + message="Handover arm planning failed.", + ) + + segments = [ + ( + "transfer", + self._segment( + context, + resources, + source_transfer, + repeat_qpos(destination_start, lengths["transfer"]), + repeat_qpos(resources.source_hand_grasp_qpos, lengths["transfer"]), + repeat_qpos( + resources.destination_hand_open_qpos, lengths["transfer"] + ), + ), + ), + ( + "approach", + self._segment( + context, + resources, + repeat_qpos(source_hold, lengths["approach"]), + destination_approach, + repeat_qpos(resources.source_hand_grasp_qpos, lengths["approach"]), + repeat_qpos( + resources.destination_hand_open_qpos, lengths["approach"] + ), + ), + ), + ( + "close", + self._segment( + context, + resources, + repeat_qpos(source_hold, lengths["close"]), + repeat_qpos(destination_hold, lengths["close"]), + repeat_qpos(resources.source_hand_grasp_qpos, lengths["close"]), + interpolate_hand_qpos( + resources.destination_hand_open_qpos, + resources.destination_hand_grasp_qpos, + n_waypoints=lengths["close"], + ), + ), + ), + ] + if lengths["hold"]: + segments.append( + ( + "hold", + self._segment( + context, + resources, + repeat_qpos(source_hold, lengths["hold"]), + repeat_qpos(destination_hold, lengths["hold"]), + repeat_qpos(resources.source_hand_grasp_qpos, lengths["hold"]), + repeat_qpos( + resources.destination_hand_grasp_qpos, lengths["hold"] + ), + ), + ) + ) + segments.extend( + ( + ( + "release", + self._segment( + context, + resources, + repeat_qpos(source_hold, lengths["release"]), + repeat_qpos(destination_hold, lengths["release"]), + interpolate_hand_qpos( + resources.source_hand_grasp_qpos, + resources.source_hand_open_qpos, + n_waypoints=lengths["release"], + ), + repeat_qpos( + resources.destination_hand_grasp_qpos, + lengths["release"], + ), + ), + ), + ( + "retreat", + self._segment( + context, + resources, + source_retreat, + repeat_qpos(destination_hold, lengths["retreat"]), + repeat_qpos( + resources.source_hand_open_qpos, lengths["retreat"] + ), + repeat_qpos( + resources.destination_hand_grasp_qpos, + lengths["retreat"], + ), + ), + ), + ) + ) + + trajectory = torch.cat([value for _, value in segments], dim=1) + received = HeldObjectState( + semantics=held.semantics, + object_to_eef=object_to_destination, + grasp_xpos=destination_grasp, + ) + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + trajectory, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + expected_effects=StateDelta( + held_object_updates={ + resources.source_state_key: None, + resources.destination_state_key: received, + } + ), + segment_lengths={name: value.shape[1] for name, value in segments}, + ) + + def _resolve_resources( + self, + request: ResolvedActionRequest[GraspGoal, HeldObjectHandOverOptions], + ) -> _HandoverResources: + binding = request.binding + source_motion = binding.endpoint("source", "motion") + source_grasp = binding.endpoint("source", "grasp") + destination_motion = binding.endpoint("destination", "motion") + destination_grasp = binding.endpoint("destination", "grasp") + source_arm = source_motion.require_target(JointPositionTarget) + source_hand = source_grasp.require_target(JointPositionTarget) + destination_arm = destination_motion.require_target(JointPositionTarget) + destination_hand = destination_grasp.require_target(JointPositionTarget) + source_key = require_shared_task_state_key( + source_motion, + source_grasp, + participant="HeldObjectHandOver source", + ) + destination_key = require_shared_task_state_key( + destination_motion, + destination_grasp, + participant="HeldObjectHandOver destination", + ) + if source_key == destination_key: + raise ValueError("Handover participants require different state keys.") + return _HandoverResources( + source_state_key=source_key, + destination_state_key=destination_key, + source_arm=source_arm, + destination_arm=destination_arm, + source_hand=source_hand, + destination_hand=destination_hand, + source_hand_open_qpos=source_grasp.joint_positions( + OPEN_COMMAND, + num_envs=self.num_envs, + device=self.device, + dtype=torch.float32, + ), + source_hand_grasp_qpos=source_grasp.joint_positions( + GRASP_COMMAND, + num_envs=self.num_envs, + device=self.device, + dtype=torch.float32, + ), + destination_hand_open_qpos=destination_grasp.joint_positions( + OPEN_COMMAND, + num_envs=self.num_envs, + device=self.device, + dtype=torch.float32, + ), + destination_hand_grasp_qpos=destination_grasp.joint_positions( + GRASP_COMMAND, + num_envs=self.num_envs, + device=self.device, + dtype=torch.float32, + ), + ) + + def _destination_grasp( + self, + semantics: ObjectSemantics, + object_pose: torch.Tensor, + grasp_target_id: str, + options: HeldObjectHandOverOptions, + ) -> tuple[torch.Tensor, torch.Tensor]: + affordance = semantics.affordance + if not isinstance(affordance, AntipodalAffordance): + raise ValueError("HeldObjectHandOver requires AntipodalAffordance.") + direction = options.receive_approach_direction.to( + device=self.device, dtype=torch.float32 + ) + direction = direction / torch.linalg.vector_norm(direction) + direction = direction.expand(self.num_envs, -1) + axis = None + positive: bool | torch.Tensor = True + if options.receive_pick_object_part != "center": + local_axis = object_pose.new_tensor([0.0, 0.0, 1.0]) + axis = torch.matmul(object_pose[:, :3, :3], local_axis) + positive = torch.full( + (self.num_envs,), + options.receive_pick_object_part == "top", + dtype=torch.bool, + device=self.device, + ) + + generator = self.planning_services.grasp_pose_generator(grasp_target_id) + sampled = generator.get_valid_grasp_poses( + mesh_vertices=affordance.mesh_vertices, + mesh_triangles=affordance.mesh_triangles, + obj_poses=object_pose, + approach_direction=direction, + obj_longest_axis=axis, + is_positive_part=positive, + ) + poses = torch.eye(4, dtype=torch.float32, device=self.device).repeat( + self.num_envs, 1, 1 + ) + success = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + for env_index, (candidates, costs) in enumerate(sampled): + candidates = candidates.to(device=self.device, dtype=torch.float32) + costs = costs.to(device=self.device, dtype=torch.float32) + finite = torch.isfinite(costs) + if candidates.shape[0] == 0 or not finite.any(): + continue + ranked = torch.where(finite, costs, torch.inf) + poses[env_index] = candidates[torch.argmin(ranked)] + success[env_index] = True + return poses, success + + @staticmethod + def _segment( + context: PlanningContext, + resources: _HandoverResources, + source_arm: torch.Tensor, + destination_arm: torch.Tensor, + source_hand: torch.Tensor, + destination_hand: torch.Tensor, + ) -> torch.Tensor: + return assemble_full_robot_trajectory( + context.robot.qpos, + ( + (resources.source_arm.joint_ids, source_arm), + (resources.destination_arm.joint_ids, destination_arm), + (resources.source_hand.joint_ids, source_hand), + (resources.destination_hand.joint_ids, destination_hand), + ), + ) + + def _success(self, value: torch.Tensor, name: str) -> torch.Tensor: + return normalize_success_mask( + value, + num_envs=self.num_envs, + device=self.device, + name=name, + ) + + def _pose(self, value: torch.Tensor, name: str) -> torch.Tensor: + return resolve_batched_pose( + value, + num_envs=self.num_envs, + device=self.device, + name=name, + ) + + @staticmethod + def _require_exchange_pose(options: HeldObjectHandOverOptions) -> None: + if options.middle_object_pose is None or options.final_object_pose is None: + raise ValueError( + "middle_object_pose and final_object_pose are required for " + "HeldObjectHandOver." + ) + + @staticmethod + def _require_same_object( + requested: ObjectSemantics, + held: ObjectSemantics, + ) -> None: + if requested.entity_id is not None and held.entity_id is not None: + matches = requested.entity_id == held.entity_id + elif requested.entity is not None and held.entity is not None: + matches = requested.entity is held.entity + else: + matches = bool(requested.label) and requested.label == held.label + if not matches: + raise ValueError( + "Handover goal must identify the object held by the source." + ) + + @staticmethod + def _start_qpos( + context: PlanningContext, + resources: _HandoverResources, + ) -> tuple[torch.Tensor, torch.Tensor]: + qpos = context.robot.qpos.to(dtype=torch.float32) + return ( + qpos[:, list(resources.source_arm.joint_ids)], + qpos[:, list(resources.destination_arm.joint_ids)], + ) + + @staticmethod + def _segment_lengths( + sample_count: int, + options: HeldObjectHandOverOptions, + ) -> dict[str, int]: + close = max(2, options.hand_interp_steps) + release = max(2, options.hand_interp_steps) + retreat = max(2, options.retreat_steps) + hold = options.hold_steps + reserved = close + release + retreat + hold + transfer = max(2, (sample_count - reserved) // 2) + approach = sample_count - reserved - transfer + if approach < 2: + raise ValueError( + "Not enough handover waypoints; increase sample_count or reduce " + "handover segment lengths." + ) + return { + "transfer": transfer, + "approach": approach, + "close": close, + "hold": hold, + "release": release, + "retreat": retreat, + } + + +__all__ = ["HeldObjectHandOver", "HeldObjectHandOverOptions"] diff --git a/embodichain/gen_sim/action_engine/capabilities/registry.py b/embodichain/gen_sim/action_engine/capabilities/registry.py new file mode 100644 index 000000000..83ade2d3a --- /dev/null +++ b/embodichain/gen_sim/action_engine/capabilities/registry.py @@ -0,0 +1,183 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Capability registry shared by planning metadata and compilation.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from dataclasses import dataclass +from types import MappingProxyType +from typing import Any + +from embodichain.gen_sim.action_engine.domain.motion import validate_motion_policy + +__all__ = [ + "ActionCapability", + "ActionTemplate", + "CapabilityRegistry", + "OperatorCapability", + "PhaseTemplate", +] + + +@dataclass(frozen=True) +class ActionCapability: + """Describe one public AtomicAction class exposed to the compiler.""" + + class_name: str + target_binding_kinds: frozenset[str] + controls: frozenset[str] + + +@dataclass(frozen=True) +class ActionTemplate: + """Describe one symbolic atomic action before actor materialization.""" + + atomic_action_class: str + target_binding: Mapping[str, Any] + motion_policy: Mapping[str, Any] + control: str = "arm" + actor: Mapping[str, Any] | None = None + + def __post_init__(self) -> None: + object.__setattr__( + self, + "target_binding", + MappingProxyType(dict(self.target_binding)), + ) + object.__setattr__( + self, + "motion_policy", + MappingProxyType(validate_motion_policy(self.motion_policy)), + ) + if self.actor is not None: + object.__setattr__(self, "actor", MappingProxyType(dict(self.actor))) + + +@dataclass(frozen=True) +class PhaseTemplate: + """Group atomic actions that execute on one graph edge.""" + + name: str + state_semantic: str + actions: tuple[ActionTemplate, ...] + + +ExpandOperator = Callable[[Mapping[str, Any]], list[dict[str, Any]]] +BuildPhases = Callable[[Mapping[str, Any]], Sequence[PhaseTemplate]] + + +@dataclass(frozen=True) +class OperatorCapability: + """Bind a semantic operator to deterministic expansion and lowering.""" + + name: str + description: str + expand: ExpandOperator + build_phases: BuildPhases + expansion_topology: str = "serial" + lifecycle: str = "release" + planner_visible: bool = True + + def __post_init__(self) -> None: + if self.expansion_topology not in {"serial", "parallel_children"}: + raise ValueError( + "Operator expansion_topology must be 'serial' or " + "'parallel_children'." + ) + if self.lifecycle not in {"release", "terminal_hold"}: + raise ValueError("Operator lifecycle must be 'release' or 'terminal_hold'.") + + +class CapabilityRegistry: + """Store explicit operator and atomic-action capabilities. + + Registration is intentionally strict. Replacing a capability by accident + would silently change compilation semantics, so callers must construct a + new registry when they need a different definition. + """ + + def __init__(self) -> None: + self._operators: dict[str, OperatorCapability] = {} + self._actions: dict[str, ActionCapability] = {} + + def register_operator(self, capability: OperatorCapability) -> None: + """Register one semantic operator.""" + if capability.name in self._operators: + raise ValueError(f"Operator {capability.name!r} is already registered.") + self._operators[capability.name] = capability + + def register_action(self, capability: ActionCapability) -> None: + """Register one public AtomicAction contract.""" + if capability.class_name in self._actions: + raise ValueError( + f"Atomic action {capability.class_name!r} is already registered." + ) + self._actions[capability.class_name] = capability + + def operator(self, name: str) -> OperatorCapability: + """Return an operator or raise a capability-focused error.""" + try: + return self._operators[name] + except KeyError as exc: + raise ValueError( + f"Unknown semantic operator {name!r}; available operators are " + f"{sorted(self._operators)}." + ) from exc + + def action(self, class_name: str) -> ActionCapability: + """Return an atomic-action contract or raise a focused error.""" + try: + return self._actions[class_name] + except KeyError as exc: + raise ValueError( + f"Unknown atomic action {class_name!r}; available actions are " + f"{sorted(self._actions)}." + ) from exc + + def operator_names(self) -> tuple[str, ...]: + """Return only the semantic skills exposed to the LLM planner.""" + return tuple( + sorted( + name + for name, capability in self._operators.items() + if capability.planner_visible + ) + ) + + def operator_descriptions(self) -> dict[str, str]: + """Return JSON-safe operator descriptions.""" + return { + name: self._operators[name].description for name in self.operator_names() + } + + def validate_action_template(self, template: ActionTemplate) -> None: + """Validate one compiler-produced action against its registered API.""" + capability = self.action(template.atomic_action_class) + kind = template.target_binding.get("kind") + if kind not in capability.target_binding_kinds: + raise ValueError( + f"{template.atomic_action_class} does not accept target binding " + f"kind {kind!r}; expected one of " + f"{sorted(capability.target_binding_kinds)}." + ) + if template.control not in capability.controls: + raise ValueError( + f"{template.atomic_action_class} does not support control " + f"{template.control!r}; expected one of " + f"{sorted(capability.controls)}." + ) diff --git a/embodichain/gen_sim/action_engine/cli/__init__.py b/embodichain/gen_sim/action_engine/cli/__init__.py new file mode 100644 index 000000000..3710e5404 --- /dev/null +++ b/embodichain/gen_sim/action_engine/cli/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Command-line entry points for Action Engine generation.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/embodichain/gen_sim/action_engine/cli/generate_action_agent_config.py b/embodichain/gen_sim/action_engine/cli/generate_action_agent_config.py new file mode 100644 index 000000000..0660b7f2f --- /dev/null +++ b/embodichain/gen_sim/action_engine/cli/generate_action_agent_config.py @@ -0,0 +1,302 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""CLI for generating Action Engine configs from a Prompt2Scene gym export.""" + +from __future__ import annotations + +import argparse +from collections.abc import Mapping +from pathlib import Path + +import yaml + +from embodichain.gen_sim.action_engine.config import generation_defaults +from embodichain.gen_sim.action_engine.config.runtime_policy import ( + _PLANNER_MODES, + _planner_policy_with_mode, + _resolve_planner_policy, +) +from embodichain.gen_sim.action_engine.generation import ( + generate_action_engine_config, +) + +__all__ = ["build_parser", "cli"] + +_GENERATION_DEFAULTS = generation_defaults() +_TASK_DEFAULTS = _GENERATION_DEFAULTS["task"] +_SCENE_DEFAULTS = _GENERATION_DEFAULTS["scene"] + +_ROBOT_PROFILE_CHOICES = ( + "ur5", + "ur10", + "dual_ur5", + "dual_ur10", + "franka", + "dual_franka", +) + + +def build_parser() -> argparse.ArgumentParser: + """Build the standalone config-generation argument parser.""" + parser = argparse.ArgumentParser( + description=( + "Plan and compile an Action Engine task from an exported tabletop " + "gym project." + ) + ) + parser.add_argument( + "--gym_project", + "--gym-project", + required=True, + help=( + "Prompt2Scene task/export directory or gym_config.json/" + "scene_config.json path." + ), + ) + parser.add_argument( + "--output_dir", + "--output-dir", + required=True, + help="Directory receiving canonical JSON artifacts and the Seed PNG.", + ) + parser.add_argument( + "--task_name", + "--task-name", + required=True, + help="Stable task identifier stored in both programs.", + ) + parser.add_argument( + "--task_description", + "--task-description", + help="Natural-language goal passed to structured LLM interpretation.", + ) + parser.add_argument( + "--task_file", + "--task-file", + help="Optional UTF-8 file containing the natural-language goal.", + ) + parser.add_argument( + "--task-spec", + "--task_spec", + dest="task_spec", + help=( + "Optional existing Action Engine v2 TaskSpec JSON; bypasses text " + "LLM interpretation and uses its role_bindings hand-off." + ), + ) + parser.add_argument( + "--robot-profile", + "--robot_profile", + choices=_ROBOT_PROFILE_CHOICES, + default=str(_TASK_DEFAULTS["default_robot_profile"]), + help="Robot template used in fast_gym_config.json.", + ) + parser.add_argument( + "--gripper-model", + "--gripper_model", + choices=("pgi", "robotiq"), + default=str(_TASK_DEFAULTS["default_gripper_model"]), + help="Gripper asset, control, TCP, and grasp profile used by both arms.", + ) + parser.add_argument( + "--ik-solver", + choices=("auto", "ur", "pytorch"), + default=str(_TASK_DEFAULTS["default_ik_solver"]), + help="Generation-time IK solver used by both arms.", + ) + parser.add_argument( + "--llm_model", + "--llm-model", + default=None, + help="Optional planner model override.", + ) + parser.add_argument( + "--vlm_model", + "--vlm-model", + default=None, + help="Optional online visual/planner model override stored for A/B runs.", + ) + parser.add_argument( + "--planning-mode", + "--planning_mode", + choices=("offline", "ab"), + default="offline", + help="Generate one offline bundle or an offline/online A/B bundle.", + ) + parser.add_argument( + "--planner-config", + default=None, + help=( + "Optional YAML file containing a planner mapping. The canonical " + "policy and runtime_policy_hash are regenerated into agent_config.json." + ), + ) + parser.add_argument( + "--planner-mode", + choices=_PLANNER_MODES, + default=None, + help=( + "Explicit planner mode override. When omitted, planner YAML/defaults " + "remain authoritative." + ), + ) + parser.add_argument( + "--source_scene_z_rotation_degrees", + "--source-scene-z-rotation-degrees", + type=float, + default=None, + help=( + "World-frame scene rotation. Prompt2Scene exports default to -90 " + "degrees; other inputs default to zero." + ), + ) + parser.add_argument( + "--body-scale-policy", + choices=("preserve", "multiply", "absolute"), + default=str(_SCENE_DEFAULTS["body_scale_policy"]), + help="How the requested xyz scale combines with source body_scale.", + ) + parser.add_argument( + "--body-scale", + type=float, + nargs=3, + default=tuple(float(value) for value in _SCENE_DEFAULTS["body_scale"]), + metavar=("X", "Y", "Z"), + help="Positive xyz scale used by multiply or absolute policy.", + ) + parser.add_argument( + "--max_episodes", + "--max-episodes", + type=int, + default=int(_TASK_DEFAULTS["max_episodes"]), + help="Episode count written to fast_gym_config.json.", + ) + parser.add_argument( + "--max_episode_steps", + "--max-episode-steps", + type=int, + default=int(_TASK_DEFAULTS["max_episode_steps"]), + help="Per-episode step limit written to fast_gym_config.json.", + ) + parser.add_argument( + "--overwrite", + action="store_true", + help="Replace existing canonical artifacts in the output directory.", + ) + parser.add_argument( + "--randomize-scene", + action="store_true", + help="Randomize rigid-object poses and table height on every reset.", + ) + parser.add_argument( + "--randomize-table-material", + action="store_true", + help="Randomize the table material independently on every reset.", + ) + return parser + + +def cli() -> None: + """Generate and report the canonical Action Engine artifact bundle.""" + args = build_parser().parse_args() + task_description = _resolve_task_description(args) + planner_policy = _load_planner_config(args.planner_config) + if args.planner_mode is not None: + planner_policy = _planner_policy_with_mode( + planner_policy, + args.planner_mode, + ) + paths = generate_action_engine_config( + args.gym_project, + args.output_dir, + task_name=args.task_name, + task_description=task_description, + task_spec=args.task_spec, + robot_profile=args.robot_profile, + gripper_model=args.gripper_model, + ik_solver=args.ik_solver, + llm_model=args.llm_model, + source_scene_z_rotation_degrees=args.source_scene_z_rotation_degrees, + body_scale_policy=args.body_scale_policy, + body_scale=args.body_scale, + overwrite=args.overwrite, + max_episodes=args.max_episodes, + max_episode_steps=args.max_episode_steps, + randomize_scene=args.randomize_scene, + randomize_table_material=args.randomize_table_material, + planning_mode=args.planning_mode, + vlm_model=args.vlm_model, + planner_policy=planner_policy, + ) + + print(f"Generated gym config: {paths.gym_config}") + print(f"Generated agent config: {paths.agent_config}") + print(f"Generated TaskSpec: {paths.task_spec}") + print(f"Generated SceneRequirements: {paths.scene_requirements}") + print(f"Generated SeedGraph: {paths.seed_task_graph}") + print(f"Generated Seed graph PNG: {paths.seed_task_graph_png}") + print( + "Run with:\n" + "python -m embodichain.gen_sim.action_engine.cli.run_agent " + f"--task_name {args.task_name} " + f'--gym_config "{paths.gym_config}" ' + f'--agent_config "{paths.agent_config}" ' + "--regenerate" + ) + + +def _load_planner_config(path: str | None) -> dict[str, object] | None: + if path is None: + return None + config_path = Path(path).expanduser().resolve() + raw = yaml.safe_load(config_path.read_text(encoding="utf-8")) + if not isinstance(raw, Mapping): + raise TypeError("Planner YAML must contain a mapping.") + planner = raw.get("planner") if set(raw) == {"planner"} else raw + if not isinstance(planner, Mapping): + raise TypeError("Planner YAML 'planner' must be a mapping.") + resolved = dict(planner) + _resolve_planner_policy(resolved) + return resolved + + +def _resolve_task_description(args: argparse.Namespace) -> str: + task_spec = getattr(args, "task_spec", None) + if task_spec: + if args.task_description or args.task_file: + raise ValueError( + "--task-spec cannot be combined with --task_description or " + "--task_file." + ) + return "" + if args.task_description and args.task_file: + raise ValueError("Use either --task_description or --task_file, not both.") + if args.task_file: + description = ( + Path(args.task_file).expanduser().read_text(encoding="utf-8").strip() + ) + else: + description = str(args.task_description or "").strip() + if not description: + raise ValueError( + "--task_description (or --task_file) must provide a non-empty goal." + ) + return description + + +if __name__ == "__main__": + cli() diff --git a/embodichain/gen_sim/action_engine/compiler/__init__.py b/embodichain/gen_sim/action_engine/compiler/__init__.py new file mode 100644 index 000000000..fbd36a858 --- /dev/null +++ b/embodichain/gen_sim/action_engine/compiler/__init__.py @@ -0,0 +1,33 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Stable deterministic compiler API.""" + +from __future__ import annotations + +from .core import compile_task_agent +from .v2 import ( + compile_task_agent_v2, + execution_program_to_seed_graph, + seed_graph_to_execution_program, +) + +__all__ = [ + "compile_task_agent", + "compile_task_agent_v2", + "execution_program_to_seed_graph", + "seed_graph_to_execution_program", +] diff --git a/embodichain/gen_sim/action_engine/compiler/core.py b/embodichain/gen_sim/action_engine/compiler/core.py new file mode 100644 index 000000000..aa1a36617 --- /dev/null +++ b/embodichain/gen_sim/action_engine/compiler/core.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. +# ---------------------------------------------------------------------------- + +"""Deterministically lower a route-free TaskAgent into an action DAG.""" + +from __future__ import annotations + +import re +from collections import deque +from collections.abc import Collection, Mapping, Sequence +from copy import deepcopy +from typing import Any + +from embodichain.gen_sim.action_engine.capabilities import ( + ActionTemplate, + CapabilityRegistry, + PhaseTemplate, + build_default_registry, +) +from embodichain.gen_sim.action_engine.domain import ( + EXECUTION_PROGRAM_SCHEMA, + MOTION_POLICY_VERSION, + validate_execution_program, + validate_task_agent, +) + +__all__ = ["compile_task_agent"] + +_UNSAFE_ID_RE = re.compile(r"[^0-9a-z]+") + + +def compile_task_agent( + program: Mapping[str, Any], + *, + registry: CapabilityRegistry | None = None, + known_objects: Collection[str] | None = None, +) -> dict[str, Any]: + """Compile semantic steps into a complete coordinate-free action DAG. + + Compilation never reads simulator state and never calls an LLM. Collective + operators such as ``arrange_line`` and ``build_stack`` expand into one + execution semantic step per object, while dependencies are rewritten to + point at the terminal expanded step of each parent operation. + + Args: + program: Valid or validation-ready TaskAgent mapping. + registry: Optional capability registry for controlled extensions. + known_objects: Optional runtime scene UIDs used for pre-simulator + object-reference validation. + + Returns: + A validated ``action_engine_execution_graph_v1`` mapping. + """ + task_agent = validate_task_agent(program, known_objects=known_objects) + capabilities = registry or build_default_registry() + ordered_task_steps = _stable_topological_steps(task_agent["semantic_steps"]) + + expanded_by_parent: dict[str, list[dict[str, Any]]] = {} + all_expanded_ids: set[str] = set() + for task_step in ordered_task_steps: + definition = capabilities.operator(task_step["operator"]) + expanded = definition.expand(task_step) + if not expanded: + raise ValueError( + f"Operator {task_step['operator']!r} produced no execution steps." + ) + for child in expanded: + child_id = str(child.get("id", "")) + if not child_id or child_id in all_expanded_ids: + raise ValueError( + f"Operator {task_step['operator']!r} produced duplicate or " + f"empty execution step ID {child_id!r}." + ) + all_expanded_ids.add(child_id) + expanded_by_parent[task_step["id"]] = expanded + + # Operator expansion validates each step's shape first, so held-state + # diagnostics never mask a more direct capability-contract error. + _validate_held_state_contract(ordered_task_steps) + + terminal_children: dict[str, list[str]] = {} + for task_step in ordered_task_steps: + definition = capabilities.operator(task_step["operator"]) + children = expanded_by_parent[task_step["id"]] + terminal_children[task_step["id"]] = ( + [child["id"] for child in children] + if definition.expansion_topology == "parallel_children" + else [children[-1]["id"]] + ) + expanded_steps: list[dict[str, Any]] = [] + for task_step in ordered_task_steps: + definition = capabilities.operator(task_step["operator"]) + children = expanded_by_parent[task_step["id"]] + parent_dependencies = [ + child_id + for parent_id in task_step["depends_on"] + for child_id in terminal_children[parent_id] + ] + for index, child in enumerate(children): + child["depends_on"] = ( + parent_dependencies + if index == 0 or definition.expansion_topology == "parallel_children" + else [children[index - 1]["id"]] + ) + expanded_steps.append(child) + + phases_by_step: dict[str, tuple[PhaseTemplate, ...]] = {} + for step in expanded_steps: + definition = capabilities.operator(step["operator"]) + phases = tuple(definition.build_phases(step)) + if not phases or any(not phase.actions for phase in phases): + raise ValueError( + f"Operator {step['operator']!r} produced an empty action phase." + ) + for phase in phases: + for action in phase.actions: + capabilities.validate_action_template(action) + phases_by_step[step["id"]] = phases + + graph = _build_graph( + task=task_agent["task"], + goal_description=task_agent["goal"], + semantic_steps=expanded_steps, + phases_by_step=phases_by_step, + ) + graph["allocation_groups"] = _merge_allocation_groups( + _compile_task_allocation_groups( + task_agent["allocation_groups"], + expanded_by_parent, + ), + _derive_allocation_groups( + expanded_steps, + phases_by_step, + ), + ) + return validate_execution_program(graph) + + +def _compile_task_allocation_groups( + groups: Sequence[Mapping[str, Any]], + expanded_by_parent: Mapping[str, Sequence[Mapping[str, Any]]], +) -> list[dict[str, Any]]: + result: list[dict[str, Any]] = [] + for group in groups: + members = [ + expanded_by_parent[parent_id][0]["id"] + for parent_id in group["semantic_step_ids"] + ] + result.append( + { + "id": group["id"], + "semantic_step_ids": members, + "arm_constraint": "distinct_arms", + "execution_policy": "parallel_if_feasible", + "parallel_action_classes": ["PickUp"], + "workspace_policy": "shared_target_serial", + } + ) + return result + + +def _merge_allocation_groups( + explicit: Sequence[Mapping[str, Any]], + derived: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + result = [deepcopy(dict(group)) for group in explicit] + assigned = {step_id for group in result for step_id in group["semantic_step_ids"]} + used_ids = {group["id"] for group in result} + for group in derived: + if set(group["semantic_step_ids"]) & assigned: + continue + candidate = deepcopy(dict(group)) + base_id = candidate["id"] + suffix = 2 + while candidate["id"] in used_ids: + candidate["id"] = f"{base_id}_{suffix}" + suffix += 1 + result.append(candidate) + used_ids.add(candidate["id"]) + assigned.update(candidate["semantic_step_ids"]) + return result + + +def _build_graph( + *, + task: str, + goal_description: str, + semantic_steps: list[dict[str, Any]], + phases_by_step: Mapping[str, tuple[PhaseTemplate, ...]], +) -> dict[str, Any]: + start_id = "v0_start" + goal_id = "v_goal" + dependents: dict[str, list[str]] = {step["id"]: [] for step in semantic_steps} + for step in semantic_steps: + for dependency in step["depends_on"]: + dependents[dependency].append(step["id"]) + + terminal_node = { + step["id"]: ( + f"v_{_slug(step['id'])}_done" if dependents[step["id"]] else goal_id + ) + for step in semantic_steps + } + nodes: list[dict[str, str]] = [ + { + "id": start_id, + "semantic": "Initial state before executing the semantic action DAG", + } + ] + node_ids = {start_id} + edges: list[dict[str, Any]] = [] + final_edge_by_step: dict[str, str] = {} + + def add_node(node_id: str, semantic: str) -> None: + if node_id in node_ids or node_id == goal_id: + return + node_ids.add(node_id) + nodes.append({"id": node_id, "semantic": semantic}) + + for step in semantic_steps: + phases = phases_by_step[step["id"]] + if step["depends_on"]: + source_id = terminal_node[step["depends_on"][0]] + else: + source_id = start_id + add_node( + source_id, + f"Dependencies for semantic step `{step['id']}` are complete", + ) + + step_edge_ids: list[str] = [] + previous_edge_id: str | None = None + for phase_index, phase in enumerate(phases, start=1): + is_last = phase_index == len(phases) + target_id = ( + terminal_node[step["id"]] + if is_last + else f"v_{_slug(step['id'])}_{phase_index:02d}_{_slug(phase.name)}" + ) + add_node(target_id, phase.state_semantic) + edge_id = f"e{len(edges) + 1:03d}_{_slug(step['id'])}_{_slug(phase.name)}" + edge_dependencies = ( + [final_edge_by_step[item] for item in step["depends_on"]] + if previous_edge_id is None + else [previous_edge_id] + ) + actions = [ + _materialize_action(action, default_actor=step["actor"]) + for action in phase.actions + ] + edges.append( + { + "id": edge_id, + "source": source_id, + "target": target_id, + "semantic_step_id": step["id"], + "actions": actions, + "depends_on": edge_dependencies, + "resources": _edge_resources(step, actions), + } + ) + step_edge_ids.append(edge_id) + previous_edge_id = edge_id + source_id = target_id + step["edge_ids"] = step_edge_ids + final_edge_by_step[step["id"]] = step_edge_ids[-1] + + nodes.append( + { + "id": goal_id, + "semantic": "All required semantic steps have reached their postconditions", + } + ) + return { + "schema_version": EXECUTION_PROGRAM_SCHEMA, + "task": task, + "goal_description": goal_description, + "start": start_id, + "goal": goal_id, + "nodes": nodes, + "edges": edges, + "semantic_steps": semantic_steps, + "allocation_groups": [], + "motion_policy_version": MOTION_POLICY_VERSION, + } + + +def _materialize_action( + template: ActionTemplate, + *, + default_actor: Mapping[str, Any], +) -> dict[str, Any]: + actor = template.actor if template.actor is not None else default_actor + return { + "atomic_action_class": template.atomic_action_class, + "actor": deepcopy(dict(actor)), + "control": template.control, + "target_binding": deepcopy(dict(template.target_binding)), + "motion_policy": deepcopy(dict(template.motion_policy)), + } + + +def _edge_resources( + step: Mapping[str, Any], + actions: Sequence[Mapping[str, Any]], +) -> list[str]: + resources = {f"object:{step['object']}"} + reference = step["goal"].get("reference_object") + support = step["goal"].get("support_object") + + for action in actions: + actor = action["actor"] + if actor["mode"] == "auto": + resources.add("arm:auto") + elif actor["mode"] == "required": + resources.add(f"arm:{actor['arm']}") + else: + resources.update(f"arm:{arm}" for arm in actor["arms"]) + + binding = action["target_binding"] + for key in ("object", "placing_object", "support_object"): + object_uid = binding.get(key) + if isinstance(object_uid, str) and object_uid: + resources.add(f"object:{object_uid}") + for payload in binding.get("payloads", []): + object_uid = ( + payload.get("object") if isinstance(payload, Mapping) else payload + ) + if isinstance(object_uid, str) and object_uid: + resources.add(f"object:{object_uid}") + + action_classes = {action["atomic_action_class"] for action in actions} + uses_goal_workspace = bool( + action_classes + & { + "MoveHeldObject", + "MoveEndEffector", + "Place", + "CoordinatedPickment", + "CoordinatedPlacement", + "Press", + } + ) + if isinstance(reference, str) and reference and uses_goal_workspace: + resources.add(f"workspace:{reference}") + elif isinstance(support, str) and support: + # Passive supports such as a table may be shared by independent + # pickups. Only a coordinated placement manipulates and owns its + # support object throughout the semantic step. + if step["operator"] == "coordinated_place": + resources.add(f"object:{support}") + if uses_goal_workspace: + resources.add(f"workspace:{support}") + + if action_classes & { + "MoveHeldObject", + "Place", + "CoordinatedPickment", + "CoordinatedPlacement", + "Press", + }: + if step["operator"] == "arrange_line": + resources.add("workspace:table") + elif reference is None and support is None: + resources.add("workspace:world") + return sorted(resources) + + +def _derive_allocation_groups( + semantic_steps: Sequence[Mapping[str, Any]], + phases_by_step: Mapping[str, tuple[PhaseTemplate, ...]], +) -> list[dict[str, Any]]: + """Declare only explicit, independent distinct-arm pickup pairs.""" + groups: list[dict[str, Any]] = [] + ancestor_ids = _ancestor_sets(semantic_steps) + used_steps: set[str] = set() + for index, first in enumerate(semantic_steps): + if first["id"] in used_steps or not _starts_with_pickup( + phases_by_step[first["id"]] + ): + continue + for second in semantic_steps[index + 1 :]: + if second["id"] in used_steps or not _starts_with_pickup( + phases_by_step[second["id"]] + ): + continue + if not _actors_request_distinct_arms( + first["actor"], + second["actor"], + ): + continue + if ( + second["id"] in ancestor_ids[first["id"]] + or first["id"] in ancestor_ids[second["id"]] + ): + continue + if first["object"] == second["object"]: + continue + groups.append( + { + "id": f"g{len(groups) + 1:02d}_distinct_arms", + "semantic_step_ids": [first["id"], second["id"]], + "arm_constraint": "distinct_arms", + "execution_policy": "parallel_if_feasible", + "parallel_action_classes": ["PickUp"], + "workspace_policy": "shared_target_serial", + } + ) + used_steps.update({first["id"], second["id"]}) + break + return groups + + +def _validate_held_state_contract( + semantic_steps: Sequence[Mapping[str, Any]], +) -> None: + """Validate persistent object ownership and required-arm reservations. + + ``hold_hover`` is terminal behavior for its object and reserves the + selected arm through task completion. Unrelated downstream work remains + legal because runtime can assign it to another free arm. Action Engine v1 + does not expose a "continue with currently held object" operator, however, + so any second step that references the held object would imply an unsafe + pickup, handover, or use of a moving reference. Planner-produced + hold/place pairs are fused before this boundary. + """ + ancestors = _ancestor_sets(semantic_steps) + for hold in semantic_steps: + terminal_coordinated = ( + hold["operator"] == "coordinated_transport" + and hold["goal"].get("terminal_behavior", "hold") == "hold" + ) + if hold["operator"] != "hold_hover" and not terminal_coordinated: + continue + hold_id = hold["id"] + held_object = hold["object"] + for other in semantic_steps: + other_id = other["id"] + if other_id == hold_id or other_id in ancestors[hold_id]: + continue + if held_object in _step_object_references(other): + raise ValueError( + f"hold_hover step {hold_id!r} reserves object " + f"{held_object!r} through task completion, but step " + f"{other_id!r} also references it." + ) + hold_actor = hold["actor"] + other_actor = other["actor"] + if terminal_coordinated: + raise ValueError( + f"Terminal coordinated step {hold_id!r} reserves both arms, " + f"but step {other_id!r} is not an ancestor." + ) + if hold_actor["mode"] != "required": + continue + reserved_arm = _canonical_arm(hold_actor["arm"]) + conflicts = other_actor["mode"] == "coordinated" or ( + other_actor["mode"] == "required" + and _canonical_arm(other_actor["arm"]) == reserved_arm + ) + if conflicts: + raise ValueError( + f"hold_hover step {hold_id!r} reserves arm " + f"{reserved_arm!r}, but non-ancestor step {other_id!r} " + "also requires it." + ) + + +def _step_object_references(step: Mapping[str, Any]) -> set[str]: + """Return object UIDs whose ownership or workspace a step may require.""" + result = {step["object"]} if "object" in step else set(step.get("objects", ())) + goal = step["goal"] + for key in ( + "anchor", + "orientation_reference_object", + "reference_object", + "support_object", + ): + value = goal.get(key) + if isinstance(value, str): + result.add(value) + for payload in goal.get("payloads", []): + value = payload.get("object") if isinstance(payload, Mapping) else payload + if isinstance(value, str): + result.add(value) + for content in goal.get("contents", []): + value = content.get("object") if isinstance(content, Mapping) else content + if isinstance(value, str): + result.add(value) + return result + + +def _ancestor_sets( + semantic_steps: Sequence[Mapping[str, Any]], +) -> dict[str, set[str]]: + direct = {step["id"]: set(step["depends_on"]) for step in semantic_steps} + ancestors: dict[str, set[str]] = {} + for step in semantic_steps: + pending = list(direct[step["id"]]) + result: set[str] = set() + while pending: + dependency = pending.pop() + if dependency in result: + continue + result.add(dependency) + pending.extend(direct[dependency]) + ancestors[step["id"]] = result + return ancestors + + +def _starts_with_pickup(phases: Sequence[PhaseTemplate]) -> bool: + return bool( + phases + and phases[0].actions + and phases[0].actions[0].atomic_action_class == "PickUp" + ) + + +def _actors_request_distinct_arms( + first: Mapping[str, Any], + second: Mapping[str, Any], +) -> bool: + """Return whether actors explicitly request a distinct-arm assignment.""" + first_group = first.get("allocation_group") + same_group = first_group is not None and first_group == second.get( + "allocation_group" + ) + required_opposite = ( + first["mode"] == "required" + and second["mode"] == "required" + and _canonical_arm(first["arm"]) != _canonical_arm(second["arm"]) + ) + if same_group and not required_opposite: + both_required = first["mode"] == second["mode"] == "required" + if both_required: + raise ValueError( + f"Allocation group {first_group!r} requires distinct arms, " + "but both steps require the same arm." + ) + return same_group or required_opposite + + +def _canonical_arm(value: Any) -> str: + arm = str(value) + return f"{arm}_arm" if arm in {"left", "right"} else arm + + +def _stable_topological_steps( + semantic_steps: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + original = [deepcopy(dict(step)) for step in semantic_steps] + order = {step["id"]: index for index, step in enumerate(original)} + by_id = {step["id"]: step for step in original} + indegree = {step["id"]: len(step["depends_on"]) for step in original} + dependents: dict[str, list[str]] = {step["id"]: [] for step in original} + for step in original: + for dependency in step["depends_on"]: + dependents[dependency].append(step["id"]) + + ready = deque( + sorted( + (step_id for step_id, degree in indegree.items() if degree == 0), + key=order.__getitem__, + ) + ) + result: list[dict[str, Any]] = [] + while ready: + step_id = ready.popleft() + result.append(by_id[step_id]) + newly_ready: list[str] = [] + for dependent in dependents[step_id]: + indegree[dependent] -= 1 + if indegree[dependent] == 0: + newly_ready.append(dependent) + ready.extend(sorted(newly_ready, key=order.__getitem__)) + return result + + +def _slug(value: Any) -> str: + slug = _UNSAFE_ID_RE.sub("_", str(value).lower()).strip("_") + return slug[:64].rstrip("_") or "step" diff --git a/embodichain/gen_sim/action_engine/compiler/v2.py b/embodichain/gen_sim/action_engine/compiler/v2.py new file mode 100644 index 000000000..9677ce780 --- /dev/null +++ b/embodichain/gen_sim/action_engine/compiler/v2.py @@ -0,0 +1,429 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Bridge mature v1 task recipes to the direct AtomicAction SeedGraph v3.""" + +from __future__ import annotations + +from collections import deque +from collections.abc import Collection, Mapping, Sequence +from copy import deepcopy +import re +from typing import Any + +from embodichain.gen_sim.action_engine.capabilities import ( + AtomicCapabilityRegistry, + build_atomic_capability_registry, + capability_precondition, +) +from embodichain.gen_sim.action_engine.domain import ( + EXECUTION_PROGRAM_SCHEMA, + MOTION_POLICY_VERSION, + validate_execution_program, + validate_seed_graph, +) +from embodichain.gen_sim.action_engine.protocol import SEED_GRAPH_SCHEMA +from embodichain.gen_sim.action_engine.planning.linker import ( + link_seed_graph, + validate_persisted_contracts, +) + +__all__ = [ + "compile_task_agent_v2", + "execution_program_to_seed_graph", + "seed_graph_to_execution_program", +] + +_UNSAFE_ID_RE = re.compile(r"[^0-9a-z]+") +_OPERATOR_TASK_TYPES = { + "arrange_line": "E1", + "build_stack": "E1", + "coordinated_place": "E5", + "coordinated_transport": "E5", + "hold_hover": "E1", + "orient_object": "E2", + "place_in_line": "E1", + "place_relative": "E1", + "press": "E9", +} + + +def compile_task_agent_v2( + program: Mapping[str, Any], + *, + known_objects: Collection[str] | None = None, + registry: AtomicCapabilityRegistry | None = None, +) -> dict[str, Any]: + """Compile a mature semantic recipe directly to the persisted v3 graph.""" + from .core import compile_task_agent + + legacy = compile_task_agent(program, known_objects=known_objects) + return execution_program_to_seed_graph( + legacy, + known_objects=known_objects, + registry=registry, + ) + + +def execution_program_to_seed_graph( + program: Mapping[str, Any], + *, + planner_route: str = "offline", + known_objects: Collection[str] | None = None, + registry: AtomicCapabilityRegistry | None = None, +) -> dict[str, Any]: + """Convert a mature v1 result without changing its AtomicAction topology.""" + legacy = validate_execution_program(program) + capabilities = registry or build_atomic_capability_registry() + steps = {str(step["id"]): step for step in legacy["semantic_steps"]} + node_ids_by_edge: dict[str, list[str]] = {} + nodes: list[dict[str, Any]] = [] + + for edge in legacy["edges"]: + edge_id = str(edge["id"]) + step = steps[str(edge["semantic_step_id"])] + task_type = _task_type(str(step["operator"])) + dependencies = [ + node_id + for dependency in edge.get("depends_on", []) + for node_id in node_ids_by_edge[str(dependency)] + ] + edge_nodes: list[str] = [] + actions = list(edge["actions"]) + for action_index, action in enumerate(actions): + action_name = str(action["atomic_action_class"]) + descriptor_view = { + "atomic_action": action_name, + "control": action.get("control", "arm"), + "target_binding": action["target_binding"], + } + capabilities.validate_binding(descriptor_view) + capability = capabilities.get(action_name) + node_id = _node_id(edge_id, action_name, action_index, len(actions)) + postcondition = ( + deepcopy(step["postcondition"]) + if edge_id == step["edge_ids"][-1] + else {} + ) + node = { + "id": node_id, + "atomic_action": action_name, + "object_uid": str(step["object"]), + "actor": _v2_actor(action["actor"]), + "control": str(action.get("control", "arm")), + "target_binding": deepcopy(dict(action["target_binding"])), + "depends_on": list(dict.fromkeys(dependencies)), + "task_instance_id": str(step["id"]), + "task_type": task_type, + "role": _node_role(action_name, action["target_binding"]), + "precondition": capability_precondition( + capability, + object_uid=str(step["object"]), + actor=_v2_actor(action["actor"]), + target_binding=action["target_binding"], + ), + "postcondition": postcondition, + "motion_policy": deepcopy(dict(action["motion_policy"])), + } + if len(actions) > 1: + node["sync_group"] = edge_id + nodes.append(node) + edge_nodes.append(node_id) + node_ids_by_edge[edge_id] = edge_nodes + + groups = [] + for step in legacy["semantic_steps"]: + group_node_ids = [ + node_id + for edge_id in step["edge_ids"] + for node_id in node_ids_by_edge[str(edge_id)] + ] + groups.append( + { + "id": str(step["id"]), + "task_type": _task_type(str(step["operator"])), + "role": "primary", + "operator": str(step["operator"]), + "object_uid": str(step["object"]), + "actor": _v2_actor(step["actor"]), + "goal": deepcopy(dict(step.get("goal", {}))), + "depends_on": list(step.get("depends_on", [])), + "parent_task_instance_id": str(step.get("parent_step_id", step["id"])), + "node_ids": group_node_ids, + "success": deepcopy(dict(step["postcondition"])), + } + ) + + level = _level(groups) + graph = { + "schema_version": SEED_GRAPH_SCHEMA, + "task_id": str(legacy["task"]), + "instruction": str(legacy["goal_description"]), + "level": level, + "reasoning_type": "none", + "planner_route": planner_route, + "nodes": nodes, + "task_groups": groups, + "success": { + "op": "all", + "terms": [deepcopy(group["success"]) for group in groups], + }, + "capability_catalog_hash": capabilities.catalog_hash(), + "metadata": { + "source_schema": EXECUTION_PROGRAM_SCHEMA, + "legacy_allocation_groups": deepcopy(legacy.get("allocation_groups", [])), + "planning_latency_seconds": 0.0, + "vlm_call_count": 0, + }, + } + return link_seed_graph( + graph, + registry=capabilities, + task_order=[str(step["id"]) for step in legacy["semantic_steps"]], + known_objects=known_objects, + ) + + +def seed_graph_to_execution_program( + graph: Mapping[str, Any], + *, + known_objects: Collection[str] | None = None, + registry: AtomicCapabilityRegistry | None = None, + require_executable: bool = True, +) -> dict[str, Any]: + """Materialize the v3 DAG as the existing in-memory runtime view.""" + capabilities = registry or build_atomic_capability_registry() + seed = validate_seed_graph( + graph, + known_objects=known_objects, + known_actions=capabilities.names(), + executable_actions=capabilities.executable_names(), + require_executable=require_executable, + ) + if seed["capability_catalog_hash"] != capabilities.catalog_hash(): + raise ValueError( + "SeedGraph capability_catalog_hash does not match the runtime catalog." + ) + validate_persisted_contracts(seed, capabilities) + for node in seed["nodes"]: + capabilities.validate_binding(node) + + node_by_id = {str(node["id"]): node for node in seed["nodes"]} + unit_by_node, units = _execution_units(seed["nodes"]) + ordered_units = _topological_units(units) + edge_id_by_unit = {unit_id: f"edge_{_slug(unit_id)}" for unit_id in ordered_units} + target_by_unit = { + unit_id: f"state_{index + 1:04d}_{_slug(unit_id)}" + for index, unit_id in enumerate(ordered_units) + } + start = "state_start" + edges = [] + graph_nodes = [{"id": start, "semantic": "Initial live simulator state"}] + for unit_id in ordered_units: + unit = units[unit_id] + dependencies = sorted(unit["depends_on"]) + source = start if not dependencies else target_by_unit[dependencies[0]] + target = target_by_unit[unit_id] + graph_nodes.append( + { + "id": target, + "semantic": f"Completed AtomicAction unit {unit_id}", + } + ) + unit_nodes = [node_by_id[node_id] for node_id in unit["node_ids"]] + edges.append( + { + "id": edge_id_by_unit[unit_id], + "source": source, + "target": target, + "semantic_step_id": str(unit_nodes[0]["task_instance_id"]), + "actions": [ + { + "atomic_action_class": node["atomic_action"], + "actor": deepcopy(node["actor"]), + "control": node["control"], + "target_binding": deepcopy(node["target_binding"]), + "motion_policy": node["motion_policy"], + "seed_node_id": node["id"], + "failure_policy": node["contract"]["failure_policy"], + } + for node in unit_nodes + ], + "depends_on": [edge_id_by_unit[item] for item in dependencies], + "resources": sorted( + { + str(claim["resource"]) + for node in unit_nodes + for claim in node["contract"]["claims"] + } + ), + } + ) + + group_by_id = {str(group["id"]): group for group in seed["task_groups"]} + semantic_steps = [] + for group_id in _topological_groups(seed["task_groups"]): + group = group_by_id[group_id] + group_units = [ + unit_id + for unit_id in ordered_units + if any( + node_by_id[node_id]["task_instance_id"] == group_id + for node_id in units[unit_id]["node_ids"] + ) + ] + semantic_steps.append( + { + "id": group_id, + "parent_step_id": str(group.get("parent_task_instance_id", group_id)), + "operator": str(group["operator"]), + "object": str(group["object_uid"]), + "actor": deepcopy(group["actor"]), + "goal": deepcopy(group["goal"]), + "depends_on": list(group["depends_on"]), + "postcondition": deepcopy(group["success"]), + "edge_ids": [edge_id_by_unit[unit_id] for unit_id in group_units], + } + ) + + metadata = seed.get("metadata", {}) + allocation_groups = ( + deepcopy(metadata.get("legacy_allocation_groups", [])) + if isinstance(metadata, Mapping) + else [] + ) + program = { + "schema_version": EXECUTION_PROGRAM_SCHEMA, + "task": seed["task_id"], + "goal_description": seed["instruction"], + "start": start, + "goal": target_by_unit[ordered_units[-1]], + "nodes": graph_nodes, + "edges": edges, + "semantic_steps": semantic_steps, + "allocation_groups": allocation_groups, + "motion_policy_version": MOTION_POLICY_VERSION, + } + return validate_execution_program(program) + + +def _execution_units( + nodes: Sequence[Mapping[str, Any]], +) -> tuple[dict[str, str], dict[str, dict[str, Any]]]: + unit_by_node = { + str(node["id"]): str(node.get("sync_group", node["id"])) for node in nodes + } + units: dict[str, dict[str, Any]] = {} + for node in nodes: + node_id = str(node["id"]) + unit_id = unit_by_node[node_id] + unit = units.setdefault(unit_id, {"node_ids": [], "depends_on": set()}) + unit["node_ids"].append(node_id) + for dependency in node["depends_on"]: + dependency_unit = unit_by_node[str(dependency)] + if dependency_unit == unit_id: + raise ValueError( + f"Synchronized unit {unit_id!r} has an internal dependency." + ) + unit["depends_on"].add(dependency_unit) + for unit_id, unit in units.items(): + groups = { + str( + next(node for node in nodes if node["id"] == node_id)[ + "task_instance_id" + ] + ) + for node_id in unit["node_ids"] + } + if len(groups) != 1: + raise ValueError(f"Synchronized unit {unit_id!r} crosses task groups.") + return unit_by_node, units + + +def _topological_units(units: Mapping[str, Mapping[str, Any]]) -> list[str]: + return _topological_ids( + {unit_id: list(unit["depends_on"]) for unit_id, unit in units.items()} + ) + + +def _topological_groups(groups: Sequence[Mapping[str, Any]]) -> list[str]: + return _topological_ids( + {str(group["id"]): list(group["depends_on"]) for group in groups} + ) + + +def _topological_ids(dependencies: Mapping[str, Sequence[str]]) -> list[str]: + outgoing = {item_id: [] for item_id in dependencies} + indegree = {item_id: 0 for item_id in dependencies} + for item_id, parents in dependencies.items(): + for parent in parents: + outgoing[parent].append(item_id) + indegree[item_id] += 1 + ready = deque( + sorted(item_id for item_id, degree in indegree.items() if degree == 0) + ) + ordered = [] + while ready: + item_id = ready.popleft() + ordered.append(item_id) + for child in sorted(outgoing[item_id]): + indegree[child] -= 1 + if indegree[child] == 0: + ready.append(child) + if len(ordered) != len(dependencies): + raise ValueError("Graph contains a dependency cycle.") + return ordered + + +def _v2_actor(value: Mapping[str, Any]) -> dict[str, Any]: + actor = deepcopy(dict(value)) + actor.pop("allocation_group", None) + if actor.get("mode") == "required" and actor.get("arm") in {"left", "right"}: + actor["arm"] = f"{actor['arm']}_arm" + return actor + + +def _task_type(operator: str) -> str: + try: + return _OPERATOR_TASK_TYPES[operator] + except KeyError as exc: + raise ValueError( + f"Semantic operator {operator!r} has no registered task contract." + ) from exc + + +def _level(groups: Sequence[Mapping[str, Any]]) -> str: + types = {str(group["task_type"]) for group in groups} + if len(groups) == 1: + return "L1" + return "L2" if len(types) == 1 else "L3" + + +def _node_role(action_name: str, binding: Mapping[str, Any]) -> str: + if ( + action_name == "MoveJoints" and binding.get("source") == "initial" + ) or binding.get("kind") == "policy_pose": + return "cleanup" + return "primary" + + +def _node_id(edge_id: str, action: str, index: int, count: int) -> str: + base = f"{_slug(edge_id)}_{_slug(action)}" + return base if count == 1 else f"{base}_{index + 1}" + + +def _slug(value: str) -> str: + return _UNSAFE_ID_RE.sub("_", value.lower()).strip("_") or "node" diff --git a/embodichain/gen_sim/action_engine/config/__init__.py b/embodichain/gen_sim/action_engine/config/__init__.py new file mode 100644 index 000000000..d9759d702 --- /dev/null +++ b/embodichain/gen_sim/action_engine/config/__init__.py @@ -0,0 +1,41 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Validated package policy for Action Engine generation and runtime.""" + +from __future__ import annotations + +from .runtime_policy import ( + ACTION_ENGINE_DEFAULTS_SCHEMA, + RUNTIME_POLICY_SCHEMA, + ArmSelectionPolicyCfg, + RuntimePolicyCfg, + default_runtime_policy, + generation_defaults, + resolve_agent_runtime_policy, + runtime_policy_hash, +) + +__all__ = [ + "ACTION_ENGINE_DEFAULTS_SCHEMA", + "RUNTIME_POLICY_SCHEMA", + "ArmSelectionPolicyCfg", + "RuntimePolicyCfg", + "default_runtime_policy", + "generation_defaults", + "resolve_agent_runtime_policy", + "runtime_policy_hash", +] diff --git a/embodichain/gen_sim/action_engine/config/defaults.yaml b/embodichain/gen_sim/action_engine/config/defaults.yaml new file mode 100644 index 000000000..e7c1ff81c --- /dev/null +++ b/embodichain/gen_sim/action_engine/config/defaults.yaml @@ -0,0 +1,360 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +schema_version: action_engine_defaults_v1 + +# Generation policy is materialized into fast_gym_config.json. It is not part +# of the coordinate-free Execution Program or the runtime-policy hash. +generation: + task: + default_robot_profile: ur10 + default_gripper_model: pgi + default_ik_solver: auto + max_episodes: 1 + max_episode_steps: 2000 + environment: + viewer_camera_uid: cam_high + ignore_terminations_during_agent: true + recording: + enabled: true + resolution: [640, 360] + interval_step: 5 + arm_aim_yaw_offset: + left: 0.0 + right: 0.0 + scene: + prompt2scene_z_rotation_degrees: -90.0 + default_tabletop_z: 0.7 + body_scale_policy: preserve + body_scale: [1.0, 1.0, 1.0] + object_length_sample_points: 5000 + physics: + background: + mass: 10.0 + static_friction: 0.95 + dynamic_friction: 0.9 + restitution: 0.01 + max_convex_hull_num: 1 + rigid_object: + mass: 0.1 + static_friction: 0.95 + dynamic_friction: 0.9 + linear_damping: 0.9 + angular_damping: 0.9 + contact_offset: 0.003 + rest_offset: 0.001 + restitution: 0.05 + max_depenetration_velocity: 0.8 + max_linear_velocity: 5.0 + max_angular_velocity: 5.0 + min_position_iters: 32 + min_velocity_iters: 8 + max_convex_hull_num: 16 + acd_method: vhacd + randomization: + rigid_object_position_range: [[-0.04, -0.04, 0.0], [0.04, 0.04, 0.0]] + rigid_object_rotation_range: [[0.0, 0.0, -30.0], [0.0, 0.0, 30.0]] + table_height_delta_range: [[-0.05], [0.05]] + table_material: + random_texture_prob: 0.0 + base_color_range: [[0.55, 0.55, 0.55], [0.95, 0.95, 0.95]] + metallic_range: [0.0, 0.15] + roughness_range: [0.45, 0.95] + dataset: + control_frequency: 25 + save_failed_episodes: true + use_videos: true + +# Runtime policy is resolved per robot profile, snapshotted in agent_config, +# hash-verified at startup, and recorded with every execution. +runtime: + common: + execution: + max_transitions: 1000 + semantic_step_settle_steps: 10 + max_retries_per_action: 2 + max_graph_revisions: 8 + max_recovery_actions: 12 + support_stability_samples: 3 + support_stability_interval_steps: 5 + support_linear_velocity_tolerance: 0.02 + support_angular_velocity_tolerance: 0.20 + + planner: + backend: curobo + single_arm_strategy: motion_gen + coordinated_strategy: ik_interp + fallback_strategy: ik_interp + allow_fallback: true + dynamic_collision: false + static_obstacle_uids: [] + dynamic_obstacle_uids: [] + curobo: + log_level: error + obstacle_representation: cuboid + multi_env: false + use_cuda_graph: true + preserve_plan_samples: false + max_attempts: 5 + collision_activation_distance: 0.01 + + # Crossing is measured along the live right-to-left arm-base axis so the + # same-side constraint follows translated and rotated robot workspaces. + arm_selection: + crossing_deadband_ratio: 0.08 + allow_cross_side_fallback: false + pickup_crossing_weight: 1.0 + placement_crossing_weight: 1.5 + motion_cost_scale: 3.141592653589793 + fallback_workspace_half_width: 0.5 + orient_object_preferred_arm_deadband: 0.02 + + grounding: + semantic_defaults: + surface_clearance: 0.003 + transport_clearance: 0.10 + staging_lift_height: 0.12 + relation_distance: 0.16 + hover_height: 0.10 + press_depth: 0.004 + retreat_height: 0.10 + maximum_eef_height: 0.80 + arrangement: + slot_margin: 0.08 + minimum_spacing: 0.07 + layout_clearance: 0.025 + row_search_step: 0.025 + row_search_radius: 0.25 + placement: + clearance: 0.012 + candidate_count: 5 + candidate_offset_fraction: 0.50 + support_margin: 0.002 + recovery_attempts: 2 + coordinated_grasp: + inset_fraction: 0.15 + minimum_inset: 0.01 + handover: + retreat_height: 0.10 + retreat_distance: 0.10 + maximum_eef_height: 1.50 + minimum_transfer_clearance: 0.10 + minimum_transfer_lateral_clearance: 0.06 + joint_state: + hand_close_sample_interval: 10 + hand_open_sample_interval: 15 + + grasp: + antipodal_n_sample: 10000 + antipodal_max_angle: 0.2617993877991494 + point_sample_dense: 0.012 + max_deviation_angle: 0.3490658503988659 + n_deviated_approach_directions: 4 + viser_port: 11801 + max_decomposition_hulls: 16 + force_grasp_reannotate: false + + motion_defaults: + AxisAlign: + sample_interval: 180 + pre_grasp_distance: 0.15 + lift_height: 0.16 + lower_distance: 0.16 + hand_interp_steps: 12 + PickUp: + pre_grasp_distance: 0.15 + lift_height: 0.16 + sample_interval: 120 + hand_interp_steps: 12 + MoveHeldObject: + sample_interval: 120 + relation_distance: 0.18 + robot_relative_distance: 0.10 + relation_clearance: 0.02 + exchange_clearance: 0.06 + exchange_candidate_offset: 0.16 + exchange_obstacle_clearance: 0.04 + exchange_gripper_horizontal_envelope: 0.035 + exchange_wrist_horizontal_envelope: 0.055 + exchange_gripper_vertical_envelope: 0.025 + exchange_wrist_vertical_envelope: 0.04 + exchange_minimum_reach: 0.10 + exchange_maximum_reach: 1.00 + exchange_candidate_count: 4 + hover_height: 0.10 + line_spacing: 0.14 + transport_clearance: 0.10 + staging_lift_height: 0.30 + surface_clearance: 0.005 + postcondition_tolerance: 0.08 + line_axis_tolerance: 0.06 + line_perpendicular_tolerance: 0.06 + preserve_orientation_tolerance: 0.2617993877991494 + Place: + sample_interval: 120 + lift_height: 0.14 + post_hold_steps: 60 + cartesian_waypoint_count: 2 + hand_interp_steps: 12 + MoveEndEffector: + sample_interval: 20 + retreat_height: 0.30 + minimum_retreat_height: 0.05 + maximum_eef_height: 1.10 + postcondition_tolerance: 0.05 + MoveJoints: + sample_interval: 30 + postcondition_tolerance: 0.05 + Press: + sample_interval: 80 + press_depth: 0.004 + postcondition_tolerance: 0.03 + CoordinatedPickment: + sample_interval: 120 + object_motion_keyframes: 6 + pre_grasp_distance: 0.10 + lift_height: 0.08 + middle_empty_ratio: 0.4 + grasp_opening_margin: 0.02 + grasp_seed: 17393 + grasp_pair_candidate_count: 3 + minimum_grasp_separation: 0.08 + minimum_grasp_lateral_gap: 0.05 + maximum_joint_step: 0.25 + maximum_wrist_orientation_error: 0.20 + inter_arm_capsule_radius: 0.04 + minimum_inter_arm_clearance: 0.01 + is_filter_ground_collision: false + release_sample_interval: 60 + release_gripper_tolerance: 0.08 + postcondition_tolerance: 0.06 + HandOver: + sample_interval: 140 + pre_grasp_distance: 0.08 + lift_height: 0.08 + receiver_hold_joint_tolerance: 0.002 + receive_pick_object_part: bottom + exchange_clearance: 0.06 + exchange_candidate_offset: 0.16 + exchange_obstacle_clearance: 0.04 + exchange_gripper_horizontal_envelope: 0.035 + exchange_wrist_horizontal_envelope: 0.055 + exchange_gripper_vertical_envelope: 0.025 + exchange_wrist_vertical_envelope: 0.04 + exchange_minimum_reach: 0.10 + exchange_maximum_reach: 1.00 + exchange_candidate_count: 4 + held_position_tolerance: 0.03 + hand_interp_steps: 10 + hold_steps: 4 + retreat_steps: 28 + postcondition_tolerance: 0.06 + CoordinatedPlacement: + sample_interval: 100 + hand_interp_steps: 10 + hold_steps: 4 + retreat_steps: 16 + postcondition_tolerance: 0.06 + + motion_modifiers: + orientation: + upright: + PickUp: + rotate_upright: 0.7853981633974483 + MoveHeldObject: + staging_lift_height: 0.25 + surface_clearance: 0.05 + upright_xy_tolerance: 0.05 + upright_max_tilt: 0.2617993877991494 + Place: + sample_interval: 120 + post_hold_steps: 60 + hand_interp_steps: 12 + MoveEndEffector: + sample_interval: 30 + retreat_height: 0.30 + retreat_distance: 0.10 + maximum_eef_height: 1.50 + handover_role: + transfer: + PickUp: + sample_interval: 80 + hand_interp_steps: 5 + pick_object_part: top + + predicate_fallbacks: + held_position_tolerance: 0.06 + held_gripper_tolerance: 0.01 + position_tolerance: 0.05 + xy_tolerance: 0.05 + container_xy_radius: 0.20 + container_min_z_offset: -0.05 + container_max_z_offset: 0.35 + support_xy_radius: 0.08 + support_com_margin: 0.002 + support_max_vertical_gap: 0.03 + support_max_penetration: 0.01 + support_min_overlap_ratio: 0.25 + not_fallen_max_tilt: 0.7853981633974483 + upright_max_tilt: 0.2617993877991494 + axis_tolerance: 0.03 + collinearity_tolerance: 0.03 + ordering_tolerance: 0.02 + minimum_lift_height: 0.08 + arm_initial_qpos_tolerance: 0.05 + gripper_state_tolerance: 0.001 + gripper_clear_min_distance: 0.08 + line_axis_tolerance: 0.06 + line_perpendicular_tolerance: 0.06 + preserve_orientation_tolerance: 0.2617993877991494 + payload_minimum_upright_cosine: 0.94 + payload_position_tolerance: 0.08 + payload_support_margin: 0.015 + + profiles: + dual_franka: + motion_defaults: + MoveHeldObject: + exchange_maximum_reach: 0.85 + MoveEndEffector: + retreat_height: 0.10 + HandOver: + exchange_maximum_reach: 0.85 + dual_ur3: + motion_defaults: + MoveHeldObject: + exchange_maximum_reach: 0.55 + HandOver: + exchange_maximum_reach: 0.55 + dual_ur5: + motion_defaults: + MoveHeldObject: + exchange_maximum_reach: 0.85 + HandOver: + exchange_maximum_reach: 0.85 + motion_modifiers: + orientation: + upright: + PickUp: + lift_height: 0.12 + MoveHeldObject: + staging_lift_height: 0.12 + dual_ur10: + motion_defaults: + MoveHeldObject: + exchange_maximum_reach: 1.25 + HandOver: + exchange_maximum_reach: 1.25 diff --git a/embodichain/gen_sim/action_engine/config/planner_modes/curobo.yaml b/embodichain/gen_sim/action_engine/config/planner_modes/curobo.yaml new file mode 100644 index 000000000..b322cd707 --- /dev/null +++ b/embodichain/gen_sim/action_engine/config/planner_modes/curobo.yaml @@ -0,0 +1,18 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +planner: + mode: curobo diff --git a/embodichain/gen_sim/action_engine/config/planner_modes/ik_interp.yaml b/embodichain/gen_sim/action_engine/config/planner_modes/ik_interp.yaml new file mode 100644 index 000000000..2acd3e2b3 --- /dev/null +++ b/embodichain/gen_sim/action_engine/config/planner_modes/ik_interp.yaml @@ -0,0 +1,18 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +planner: + mode: ik_interp diff --git a/embodichain/gen_sim/action_engine/config/planner_modes/toppra.yaml b/embodichain/gen_sim/action_engine/config/planner_modes/toppra.yaml new file mode 100644 index 000000000..20d9b7433 --- /dev/null +++ b/embodichain/gen_sim/action_engine/config/planner_modes/toppra.yaml @@ -0,0 +1,18 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +planner: + mode: toppra diff --git a/embodichain/gen_sim/action_engine/config/runtime_policy.py b/embodichain/gen_sim/action_engine/config/runtime_policy.py new file mode 100644 index 000000000..ee725f08b --- /dev/null +++ b/embodichain/gen_sim/action_engine/config/runtime_policy.py @@ -0,0 +1,898 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Load, resolve, snapshot, and hash package-owned Action Engine defaults.""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +import hashlib +import json +import math +from pathlib import Path +from typing import Any, Final + +from embodichain.gen_sim.action_engine.domain.motion import MOTION_MODIFIER_MODES +from embodichain.gen_sim.action_engine.gripper_profiles import get_gripper_profile +from embodichain.gen_sim.action_engine.solver_profiles import resolve_ik_solver_mode +from embodichain.utils import configclass +from embodichain.utils.utility import load_config + +__all__ = [ + "ACTION_ENGINE_DEFAULTS_SCHEMA", + "RUNTIME_POLICY_SCHEMA", + "ArmSelectionPolicyCfg", + "RuntimePolicyCfg", + "default_runtime_policy", + "generation_defaults", + "resolve_agent_runtime_policy", + "runtime_policy_hash", +] + +ACTION_ENGINE_DEFAULTS_SCHEMA: Final = "action_engine_defaults_v1" +RUNTIME_POLICY_SCHEMA: Final = "action_engine_runtime_policy_v8" +_PRE_GRIPPER_PROFILE_RUNTIME_POLICY_SCHEMA: Final = "action_engine_runtime_policy_v7" +_PRE_AXIS_RUNTIME_POLICY_SCHEMA: Final = "action_engine_runtime_policy_v6" +_PREVIOUS_RUNTIME_POLICY_SCHEMA: Final = "action_engine_runtime_policy_v5" +_PRE_GRASP_RUNTIME_POLICY_SCHEMA: Final = "action_engine_runtime_policy_v4" +_PRE_PLANNER_RUNTIME_POLICY_SCHEMA: Final = "action_engine_runtime_policy_v3" +_LEGACY_RUNTIME_POLICY_SCHEMA: Final = "action_engine_runtime_policy_v1" +_DEFAULTS_PATH = Path(__file__).with_name("defaults.yaml") +_ARM_SELECTION_KEYS = ( + "crossing_deadband_ratio", + "pickup_crossing_weight", + "placement_crossing_weight", + "motion_cost_scale", + "fallback_workspace_half_width", + "orient_object_preferred_arm_deadband", +) +_ARM_SELECTION_OPTIONAL_KEYS = {"allow_cross_side_fallback"} +_GROUNDING_KEYS = { + "semantic_defaults": { + "surface_clearance", + "transport_clearance", + "staging_lift_height", + "relation_distance", + "hover_height", + "press_depth", + "retreat_height", + "maximum_eef_height", + }, + "arrangement": { + "slot_margin", + "minimum_spacing", + "layout_clearance", + "row_search_step", + "row_search_radius", + }, + "placement": { + "clearance", + "candidate_count", + "candidate_offset_fraction", + "support_margin", + "recovery_attempts", + }, + "coordinated_grasp": {"inset_fraction", "minimum_inset"}, + "handover": { + "retreat_height", + "retreat_distance", + "maximum_eef_height", + "minimum_transfer_clearance", + "minimum_transfer_lateral_clearance", + }, + "joint_state": { + "hand_close_sample_interval", + "hand_open_sample_interval", + }, +} +_GRASP_KEYS = { + "antipodal_n_sample", + "antipodal_max_angle", + "point_sample_dense", + "max_deviation_angle", + "n_deviated_approach_directions", + "viser_port", + "max_decomposition_hulls", + "force_grasp_reannotate", +} +_PLANNER_KEYS = { + "backend", + "single_arm_strategy", + "coordinated_strategy", + "fallback_strategy", + "allow_fallback", + "dynamic_collision", + "static_obstacle_uids", + "dynamic_obstacle_uids", + "curobo", +} +_CUROBO_KEYS = { + "log_level", + "obstacle_representation", + "multi_env", + "use_cuda_graph", + "preserve_plan_samples", + "max_attempts", + "collision_activation_distance", +} +_PLANNER_MODES = ("curobo", "toppra", "ik_interp") +_PLANNER_MODE_FIELDS = frozenset( + { + "backend", + "single_arm_strategy", + "coordinated_strategy", + "dynamic_collision", + } +) +_PLANNER_MODE_PATCHES: dict[str, dict[str, Any]] = { + "curobo": { + "backend": "curobo", + "single_arm_strategy": "motion_gen", + "coordinated_strategy": "ik_interp", + }, + "toppra": { + "backend": "toppra", + "single_arm_strategy": "motion_gen", + "coordinated_strategy": "ik_interp", + }, + "ik_interp": { + "backend": "toppra", + "single_arm_strategy": "ik_interp", + "coordinated_strategy": "ik_interp", + }, +} +_MOTION_DEFAULT_ACTIONS = { + "AxisAlign", + "CoordinatedPickment", + "CoordinatedPlacement", + "HandOver", + "MoveEndEffector", + "MoveHeldObject", + "MoveJoints", + "PickUp", + "Place", + "Press", +} +_PREDICATE_KEYS = { + "held_position_tolerance", + "held_gripper_tolerance", + "position_tolerance", + "xy_tolerance", + "container_xy_radius", + "container_min_z_offset", + "container_max_z_offset", + "support_xy_radius", + "support_com_margin", + "support_max_vertical_gap", + "support_max_penetration", + "support_min_overlap_ratio", + "not_fallen_max_tilt", + "upright_max_tilt", + "axis_tolerance", + "collinearity_tolerance", + "ordering_tolerance", + "minimum_lift_height", + "arm_initial_qpos_tolerance", + "gripper_state_tolerance", + "gripper_clear_min_distance", + "line_axis_tolerance", + "line_perpendicular_tolerance", + "preserve_orientation_tolerance", + "payload_minimum_upright_cosine", + "payload_position_tolerance", + "payload_support_margin", +} +_DEPRECATED_PREDICATE_KEYS = { + "support_min_z_offset", + "support_max_z_offset", +} + + +@configclass +class ArmSelectionPolicyCfg: + """Arm-allocation constraints and costs resolved for one robot profile.""" + + crossing_deadband_ratio: float = 0.08 + allow_cross_side_fallback: bool = False + pickup_crossing_weight: float = 1.0 + placement_crossing_weight: float = 1.5 + motion_cost_scale: float = math.pi + fallback_workspace_half_width: float = 0.5 + orient_object_preferred_arm_deadband: float = 0.02 + + def __post_init__(self) -> None: + if not isinstance(self.allow_cross_side_fallback, bool): + raise TypeError("allow_cross_side_fallback must be a bool.") + for name in _ARM_SELECTION_KEYS: + value = float(getattr(self, name)) + if not math.isfinite(value): + raise ValueError(f"{name} must be finite.") + if not 0.0 <= float(self.crossing_deadband_ratio) < 1.0: + raise ValueError("crossing_deadband_ratio must be in [0, 1).") + for name in _ARM_SELECTION_KEYS[1:3]: + if float(getattr(self, name)) < 0.0: + raise ValueError(f"{name} must be non-negative.") + for name in _ARM_SELECTION_KEYS[3:]: + if float(getattr(self, name)) <= 0.0: + raise ValueError(f"{name} must be positive.") + + @classmethod + def from_mapping(cls, value: Mapping[str, Any]) -> ArmSelectionPolicyCfg: + """Build a strict policy from a JSON/YAML mapping.""" + keys = frozenset(value) + if keys not in { + frozenset(_ARM_SELECTION_KEYS), + frozenset((*_ARM_SELECTION_KEYS, *_ARM_SELECTION_OPTIONAL_KEYS)), + }: + raise ValueError("arm_selection fields do not match the policy schema.") + fields: dict[str, Any] = {key: float(value[key]) for key in _ARM_SELECTION_KEYS} + if "allow_cross_side_fallback" in value: + fields["allow_cross_side_fallback"] = value["allow_cross_side_fallback"] + return cls(**fields) + + def as_mapping(self) -> dict[str, float | bool]: + """Return a stable JSON-compatible representation.""" + return { + "crossing_deadband_ratio": float(self.crossing_deadband_ratio), + "allow_cross_side_fallback": bool(self.allow_cross_side_fallback), + "pickup_crossing_weight": float(self.pickup_crossing_weight), + "placement_crossing_weight": float(self.placement_crossing_weight), + "motion_cost_scale": float(self.motion_cost_scale), + "fallback_workspace_half_width": float(self.fallback_workspace_half_width), + "orient_object_preferred_arm_deadband": float( + self.orient_object_preferred_arm_deadband + ), + } + + +@configclass +class RuntimePolicyCfg: + """Effective runtime policy persisted in generated agent artifacts.""" + + schema_version: str = RUNTIME_POLICY_SCHEMA + arm_selection: ArmSelectionPolicyCfg = ArmSelectionPolicyCfg() + execution: dict[str, Any] = {} + planner: dict[str, Any] = {} + grounding: dict[str, Any] = {} + grasp: dict[str, Any] = {} + motion_defaults: dict[str, dict[str, Any]] = {} + motion_modifiers: dict[str, dict[str, dict[str, dict[str, Any]]]] = {} + predicate_fallbacks: dict[str, Any] = {} + + def __post_init__(self) -> None: + if self.schema_version != RUNTIME_POLICY_SCHEMA: + raise ValueError( + f"Unsupported runtime policy schema {self.schema_version!r}." + ) + if not isinstance(self.arm_selection, ArmSelectionPolicyCfg): + raise TypeError("arm_selection must be an ArmSelectionPolicyCfg.") + for name in ( + "execution", + "planner", + "grounding", + "grasp", + "motion_defaults", + "motion_modifiers", + "predicate_fallbacks", + ): + if not isinstance(getattr(self, name), dict): + raise TypeError(f"{name} must be a mapping.") + _validate_finite_numbers(getattr(self, name), name) + if int(self.execution.get("max_transitions", 0)) <= 0: + raise ValueError("execution.max_transitions must be positive.") + if int(self.execution.get("semantic_step_settle_steps", -1)) < 0: + raise ValueError( + "execution.semantic_step_settle_steps must be non-negative." + ) + for name in ( + "max_retries_per_action", + "max_graph_revisions", + "max_recovery_actions", + "support_stability_interval_steps", + ): + if int(self.execution.get(name, -1)) < 0: + raise ValueError(f"execution.{name} must be non-negative.") + _require_keys( + self.execution, + { + "max_transitions", + "semantic_step_settle_steps", + "max_retries_per_action", + "max_graph_revisions", + "max_recovery_actions", + "support_stability_samples", + "support_stability_interval_steps", + "support_linear_velocity_tolerance", + "support_angular_velocity_tolerance", + }, + "execution", + ) + if int(self.execution["support_stability_samples"]) <= 0: + raise ValueError("execution.support_stability_samples must be positive.") + for name in ( + "support_linear_velocity_tolerance", + "support_angular_velocity_tolerance", + ): + if float(self.execution[name]) < 0.0: + raise ValueError(f"execution.{name} must be non-negative.") + _validate_planner(self.planner) + _require_keys(self.grounding, set(_GROUNDING_KEYS), "grounding") + for name, keys in _GROUNDING_KEYS.items(): + section = self.grounding.get(name) + if not isinstance(section, Mapping): + raise ValueError(f"grounding.{name} must be a mapping.") + _require_keys(section, keys, f"grounding.{name}") + placement = self.grounding["placement"] + if not 1 <= int(placement["candidate_count"]) <= 9: + raise ValueError("grounding.placement.candidate_count must be in [1, 9].") + if int(placement["recovery_attempts"]) < 0: + raise ValueError( + "grounding.placement.recovery_attempts must be non-negative." + ) + if not 0.0 <= float(placement["candidate_offset_fraction"]) <= 1.0: + raise ValueError( + "grounding.placement.candidate_offset_fraction must be in [0, 1]." + ) + if float(placement["support_margin"]) < 0.0: + raise ValueError("grounding.placement.support_margin must be non-negative.") + _require_keys(self.grasp, _GRASP_KEYS, "grasp") + _require_keys( + self.motion_defaults, + _MOTION_DEFAULT_ACTIONS, + "motion_defaults", + ) + if not all( + isinstance(policy, Mapping) and policy + for policy in self.motion_defaults.values() + ): + raise ValueError("Every motion default must be a non-empty mapping.") + _validate_motion_modifiers(self.motion_modifiers) + _require_keys( + self.predicate_fallbacks, + _PREDICATE_KEYS, + "predicate_fallbacks", + ) + direction_count = self.grasp.get("n_deviated_approach_directions") + if ( + isinstance(direction_count, bool) + or not isinstance(direction_count, int) + or not 1 <= direction_count <= 16 + ): + raise ValueError("grasp.n_deviated_approach_directions must be in [1, 16].") + + @classmethod + def from_mapping(cls, value: Mapping[str, Any]) -> RuntimePolicyCfg: + """Parse one fully resolved policy snapshot.""" + fields = { + "schema_version", + "execution", + "planner", + "arm_selection", + "grounding", + "grasp", + "motion_defaults", + "motion_modifiers", + "predicate_fallbacks", + } + if set(value) != fields: + raise ValueError("Runtime policy fields do not match the policy schema.") + if value.get("schema_version") != RUNTIME_POLICY_SCHEMA: + raise ValueError("Runtime policy has an unexpected schema_version.") + arm_selection = value.get("arm_selection") + if not isinstance(arm_selection, Mapping): + raise ValueError("Runtime policy requires an arm_selection mapping.") + sections = { + name: value.get(name) + for name in fields + if name not in {"schema_version", "arm_selection"} + } + if not all(isinstance(section, Mapping) for section in sections.values()): + raise ValueError("Runtime policy sections must be mappings.") + resolved_sections = { + name: deepcopy(dict(section)) for name, section in sections.items() + } + predicate_fallbacks = resolved_sections["predicate_fallbacks"] + for key in _DEPRECATED_PREDICATE_KEYS: + predicate_fallbacks.pop(key, None) + return cls( + schema_version=RUNTIME_POLICY_SCHEMA, + arm_selection=ArmSelectionPolicyCfg.from_mapping(arm_selection), + **resolved_sections, + ) + + def as_mapping(self) -> dict[str, Any]: + """Return the canonical artifact snapshot.""" + return { + "schema_version": self.schema_version, + "execution": deepcopy(self.execution), + "planner": deepcopy(self.planner), + "arm_selection": self.arm_selection.as_mapping(), + "grounding": deepcopy(self.grounding), + "grasp": deepcopy(self.grasp), + "motion_defaults": deepcopy(self.motion_defaults), + "motion_modifiers": deepcopy(self.motion_modifiers), + "predicate_fallbacks": deepcopy(self.predicate_fallbacks), + } + + +def default_runtime_policy(robot_profile: str) -> RuntimePolicyCfg: + """Resolve a package policy for one canonical robot profile.""" + document = _load_defaults() + runtime = document.get("runtime") + if not isinstance(runtime, Mapping) or set(runtime) != {"common", "profiles"}: + raise ValueError("Runtime defaults require common and profiles mappings.") + common, profiles = runtime["common"], runtime["profiles"] + if not isinstance(common, Mapping) or not isinstance(profiles, Mapping): + raise ValueError("Runtime common and profiles must be mappings.") + override = profiles.get(str(robot_profile)) + if not isinstance(override, Mapping): + raise ValueError(f"Unknown runtime robot profile {robot_profile!r}.") + resolved = _deep_merge(common, override) + return RuntimePolicyCfg.from_mapping( + { + "schema_version": RUNTIME_POLICY_SCHEMA, + **resolved, + } + ) + + +def generation_defaults() -> dict[str, Any]: + """Return a detached generation-policy mapping.""" + value = _load_defaults().get("generation") + if not isinstance(value, Mapping): + raise ValueError("Action Engine defaults require a generation mapping.") + required = { + "task", + "environment", + "scene", + "physics", + "randomization", + "dataset", + } + if set(value) != required: + raise ValueError("Generation defaults do not match the expected sections.") + task = value.get("task") + if not isinstance(task, Mapping): + raise ValueError("Generation task defaults must be a mapping.") + get_gripper_profile(task.get("default_gripper_model")) + resolve_ik_solver_mode( + task.get("default_ik_solver"), + "dual_ur10", + ) + return deepcopy(dict(value)) + + +def _resolve_planner_policy( + planner_policy: Mapping[str, Any] | None = None, + *, + robot_profile: str = "dual_ur10", +) -> dict[str, Any]: + """Merge and validate a partial generation-time planner policy. + + ``mode`` is a generation-only shorthand for the backend and strategy + fields. The returned mapping is the canonical runtime snapshot fragment. + Backend-specific overrides are rejected when that backend is not selected, + while package defaults remain present but dormant for stable hashes. + + Args: + planner_policy: Optional partial planner mapping loaded from YAML. + robot_profile: Runtime profile used to resolve package defaults. + + Returns: + A detached, fully materialized planner policy mapping. + """ + base = default_runtime_policy(robot_profile).planner + if planner_policy is None: + return deepcopy(base) + if not isinstance(planner_policy, Mapping): + raise TypeError("planner policy must be a mapping.") + override = deepcopy(dict(planner_policy)) + mode = override.pop("mode", None) + if mode is not None: + mode = _validate_planner_mode(mode) + conflicts = set(override).intersection(_PLANNER_MODE_FIELDS) + if conflicts: + raise ValueError( + "planner.mode cannot be combined with mode-owned fields: " + f"{sorted(conflicts)}." + ) + override = _deep_merge(override, _PLANNER_MODE_PATCHES[mode]) + unknown = set(override).difference(_PLANNER_KEYS) + if unknown: + raise ValueError(f"planner policy contains unknown fields: {sorted(unknown)}.") + resolved = _deep_merge(base, override) + backend = resolved.get("backend") + if backend != "curobo" and "curobo" in override: + raise ValueError("planner.curobo options require the cuRobo backend.") + _validate_finite_numbers(resolved, "planner") + _validate_planner(resolved) + return resolved + + +def _planner_policy_with_mode( + planner_policy: Mapping[str, Any] | None, + planner_mode: str, +) -> dict[str, Any]: + """Apply an explicit CLI mode after YAML planner configuration.""" + mode = _validate_planner_mode(planner_mode) + if planner_policy is not None and not isinstance(planner_policy, Mapping): + raise TypeError("planner policy must be a mapping.") + result = deepcopy(dict(planner_policy or {})) + result.pop("mode", None) + for field_name in _PLANNER_MODE_FIELDS: + result.pop(field_name, None) + if mode != "curobo": + result.pop("curobo", None) + result["mode"] = mode + _resolve_planner_policy(result) + return result + + +def _validate_planner_mode(value: Any) -> str: + if not isinstance(value, str) or value not in _PLANNER_MODES: + raise ValueError( + f"planner mode must be one of {list(_PLANNER_MODES)}, got {value!r}." + ) + return value + + +def _load_defaults() -> dict[str, Any]: + document = load_config(_DEFAULTS_PATH) + if not isinstance(document, dict) or set(document) != { + "schema_version", + "generation", + "runtime", + }: + raise ValueError("Action Engine defaults do not match the package schema.") + if document.get("schema_version") != ACTION_ENGINE_DEFAULTS_SCHEMA: + raise ValueError("Action Engine defaults have an unexpected schema_version.") + return document + + +def _deep_merge( + base: Mapping[str, Any], + override: Mapping[str, Any], +) -> dict[str, Any]: + result = deepcopy(dict(base)) + for key, value in override.items(): + current = result.get(key) + result[key] = ( + _deep_merge(current, value) + if isinstance(current, Mapping) and isinstance(value, Mapping) + else deepcopy(value) + ) + return result + + +def _validate_finite_numbers(value: Any, path: str) -> None: + if isinstance(value, Mapping): + for key, item in value.items(): + _validate_finite_numbers(item, f"{path}.{key}") + elif isinstance(value, (list, tuple)): + for index, item in enumerate(value): + _validate_finite_numbers(item, f"{path}[{index}]") + elif isinstance(value, float) and not math.isfinite(value): + raise ValueError(f"{path} must be finite.") + + +def _require_keys( + value: Mapping[str, Any], + expected: set[str], + path: str, +) -> None: + if set(value) != expected: + raise ValueError(f"{path} fields do not match the defaults schema.") + + +def _validate_string_sequence(value: Any, path: str) -> None: + if not isinstance(value, (list, tuple)): + raise ValueError(f"{path} must be a list of object UIDs.") + normalized = [str(item) for item in value] + if any(not item.strip() for item in normalized): + raise ValueError(f"{path} entries must be non-empty strings.") + if any(not isinstance(item, str) for item in value): + raise ValueError(f"{path} entries must be strings.") + if len(set(normalized)) != len(normalized): + raise ValueError(f"{path} must not contain duplicate object UIDs.") + + +def _validate_planner(value: Mapping[str, Any]) -> None: + _require_keys(value, _PLANNER_KEYS, "planner") + backend = value.get("backend") + if backend not in {"curobo", "toppra"}: + raise ValueError("planner.backend must be 'curobo' or 'toppra'.") + for name in ("single_arm_strategy", "coordinated_strategy"): + if value.get(name) not in {"motion_gen", "ik_interp"}: + raise ValueError(f"planner.{name} must be 'motion_gen' or 'ik_interp'.") + if value.get("fallback_strategy") != "ik_interp": + raise ValueError("planner.fallback_strategy must be 'ik_interp'.") + if value.get("coordinated_strategy") != "ik_interp": + raise ValueError("planner.coordinated_strategy must be 'ik_interp'.") + for name in ("allow_fallback", "dynamic_collision"): + if not isinstance(value.get(name), bool): + raise ValueError(f"planner.{name} must be a boolean.") + if value.get("dynamic_collision") and backend != "curobo": + raise ValueError("planner.dynamic_collision requires the cuRobo backend.") + _validate_string_sequence( + value.get("static_obstacle_uids"), + "planner.static_obstacle_uids", + ) + _validate_string_sequence( + value.get("dynamic_obstacle_uids"), + "planner.dynamic_obstacle_uids", + ) + overlap = set(value["static_obstacle_uids"]) & set(value["dynamic_obstacle_uids"]) + if overlap: + raise ValueError( + "Planner obstacle UIDs cannot be both static and dynamic: " + f"{sorted(overlap)}." + ) + + curobo = value.get("curobo") + if not isinstance(curobo, Mapping): + raise ValueError("planner.curobo must be a mapping.") + _require_keys(curobo, _CUROBO_KEYS, "planner.curobo") + if curobo.get("log_level") not in { + "debug", + "info", + "warning", + "warn", + "error", + }: + raise ValueError("planner.curobo.log_level is unsupported.") + if curobo.get("obstacle_representation") not in {"sphere", "cuboid", "mesh"}: + raise ValueError( + "planner.curobo.obstacle_representation must be sphere, cuboid, or mesh." + ) + for name in ("multi_env", "use_cuda_graph", "preserve_plan_samples"): + if not isinstance(curobo.get(name), bool): + raise ValueError(f"planner.curobo.{name} must be a boolean.") + max_attempts = curobo.get("max_attempts") + if ( + isinstance(max_attempts, bool) + or not isinstance(max_attempts, int) + or max_attempts <= 0 + ): + raise ValueError("planner.curobo.max_attempts must be positive.") + activation_distance = curobo.get("collision_activation_distance") + if ( + isinstance(activation_distance, bool) + or not isinstance(activation_distance, (int, float)) + or float(activation_distance) < 0.0 + ): + raise ValueError( + "planner.curobo.collision_activation_distance must be non-negative." + ) + + +def _validate_motion_modifiers(value: Mapping[str, Any]) -> None: + _require_keys(value, set(MOTION_MODIFIER_MODES), "motion_modifiers") + for modifier_type, modes in MOTION_MODIFIER_MODES.items(): + configured_modes = value.get(modifier_type) + if not isinstance(configured_modes, Mapping): + raise ValueError(f"motion_modifiers.{modifier_type} must be a mapping.") + _require_keys( + configured_modes, + set(modes), + f"motion_modifiers.{modifier_type}", + ) + for mode, patches in configured_modes.items(): + path = f"motion_modifiers.{modifier_type}.{mode}" + if not isinstance(patches, Mapping) or not patches: + raise ValueError(f"{path} must contain action-specific patches.") + unknown_actions = set(patches) - _MOTION_DEFAULT_ACTIONS + if unknown_actions: + raise ValueError( + f"{path} references unknown actions: {sorted(unknown_actions)}." + ) + if not all( + isinstance(patch, Mapping) and patch for patch in patches.values() + ): + raise ValueError(f"Every {path} action patch must be non-empty.") + + +def runtime_policy_hash(policy: RuntimePolicyCfg | Mapping[str, Any]) -> str: + """Hash the canonical effective policy independently of the Seed graph.""" + resolved = ( + policy + if isinstance(policy, RuntimePolicyCfg) + else RuntimePolicyCfg.from_mapping(policy) + ) + return _mapping_hash(resolved.as_mapping()) + + +def resolve_agent_runtime_policy(agent_config: Mapping[str, Any]) -> RuntimePolicyCfg: + """Resolve a generated snapshot or fall back for a legacy v1 artifact.""" + snapshot = agent_config.get("runtime_policy") + expected_hash = agent_config.get("runtime_policy_hash") + if snapshot is None: + if expected_hash is not None: + raise ValueError("runtime_policy_hash requires a runtime_policy snapshot.") + return default_runtime_policy( + str(agent_config.get("robot_profile", "dual_ur10")) + ) + if not isinstance(snapshot, Mapping): + raise ValueError("agent_config.runtime_policy must be a mapping.") + if not isinstance(expected_hash, str) or not expected_hash: + raise ValueError( + "agent_config.runtime_policy requires a non-empty runtime_policy_hash." + ) + if _mapping_hash(snapshot) != expected_hash: + raise ValueError( + "agent_config runtime policy hash does not match its snapshot." + ) + if snapshot.get("schema_version") == _PRE_GRIPPER_PROFILE_RUNTIME_POLICY_SCHEMA: + migrated = deepcopy(dict(snapshot)) + migrated["schema_version"] = RUNTIME_POLICY_SCHEMA + grasp = deepcopy(dict(migrated.get("grasp", {}))) + for key in ("min_open_length", "max_open_length", "finger_length"): + grasp.pop(key, None) + migrated["grasp"] = grasp + return RuntimePolicyCfg.from_mapping(migrated) + if snapshot.get("schema_version") == _LEGACY_RUNTIME_POLICY_SCHEMA: + if set(snapshot) != {"schema_version", "arm_selection"} or not isinstance( + snapshot.get("arm_selection"), Mapping + ): + raise ValueError("Legacy runtime policy snapshot is malformed.") + policy = default_runtime_policy( + str(agent_config.get("robot_profile", "dual_ur10")) + ) + merged = policy.arm_selection.as_mapping() + merged.update(snapshot["arm_selection"]) + policy.arm_selection = ArmSelectionPolicyCfg.from_mapping(merged) + return policy + if snapshot.get("schema_version") == _PRE_AXIS_RUNTIME_POLICY_SCHEMA: + defaults = default_runtime_policy( + str(agent_config.get("robot_profile", "dual_ur10")) + ) + migrated = deepcopy(dict(snapshot)) + migrated["schema_version"] = RUNTIME_POLICY_SCHEMA + migrated_motion = deepcopy(dict(migrated.get("motion_defaults", {}))) + migrated_motion.setdefault( + "AxisAlign", + deepcopy(defaults.motion_defaults["AxisAlign"]), + ) + migrated["motion_defaults"] = migrated_motion + return RuntimePolicyCfg.from_mapping(migrated) + if snapshot.get("schema_version") == _PREVIOUS_RUNTIME_POLICY_SCHEMA: + defaults = default_runtime_policy( + str(agent_config.get("robot_profile", "dual_ur10")) + ) + migrated = deepcopy(dict(snapshot)) + migrated["schema_version"] = RUNTIME_POLICY_SCHEMA + migrated_execution = deepcopy(dict(migrated.get("execution", {}))) + for key in ( + "support_stability_samples", + "support_stability_interval_steps", + "support_linear_velocity_tolerance", + "support_angular_velocity_tolerance", + ): + migrated_execution[key] = defaults.execution[key] + migrated["execution"] = migrated_execution + migrated_grounding = deepcopy(dict(migrated.get("grounding", {}))) + migrated_placement = deepcopy(dict(migrated_grounding.get("placement", {}))) + for key, value in defaults.grounding["placement"].items(): + migrated_placement.setdefault(key, value) + migrated_grounding["placement"] = migrated_placement + migrated["grounding"] = migrated_grounding + migrated_predicates = deepcopy(dict(migrated.get("predicate_fallbacks", {}))) + for key in ( + "support_com_margin", + "support_max_vertical_gap", + "support_max_penetration", + "support_min_overlap_ratio", + ): + migrated_predicates[key] = defaults.predicate_fallbacks[key] + migrated["predicate_fallbacks"] = migrated_predicates + return RuntimePolicyCfg.from_mapping(migrated) + if snapshot.get("schema_version") == _PRE_GRASP_RUNTIME_POLICY_SCHEMA: + defaults = default_runtime_policy( + str(agent_config.get("robot_profile", "dual_ur10")) + ) + migrated = deepcopy(dict(snapshot)) + migrated["schema_version"] = RUNTIME_POLICY_SCHEMA + migrated_grasp = deepcopy(dict(migrated.get("grasp", {}))) + migrated_grasp["n_deviated_approach_directions"] = defaults.grasp[ + "n_deviated_approach_directions" + ] + migrated["grasp"] = migrated_grasp + migrated_execution = deepcopy(dict(migrated.get("execution", {}))) + for key in ( + "support_stability_samples", + "support_stability_interval_steps", + "support_linear_velocity_tolerance", + "support_angular_velocity_tolerance", + ): + migrated_execution[key] = defaults.execution[key] + migrated["execution"] = migrated_execution + migrated_grounding = deepcopy(dict(migrated.get("grounding", {}))) + migrated_placement = deepcopy(dict(migrated_grounding.get("placement", {}))) + for key, value in defaults.grounding["placement"].items(): + migrated_placement.setdefault(key, value) + migrated_grounding["placement"] = migrated_placement + migrated["grounding"] = migrated_grounding + migrated_predicates = deepcopy(dict(migrated.get("predicate_fallbacks", {}))) + for key in ( + "support_com_margin", + "support_max_vertical_gap", + "support_max_penetration", + "support_min_overlap_ratio", + ): + migrated_predicates[key] = defaults.predicate_fallbacks[key] + migrated["predicate_fallbacks"] = migrated_predicates + return RuntimePolicyCfg.from_mapping(migrated) + if snapshot.get("schema_version") == _PRE_PLANNER_RUNTIME_POLICY_SCHEMA: + expected_fields = { + "schema_version", + "execution", + "arm_selection", + "grounding", + "grasp", + "motion_defaults", + "motion_modifiers", + "predicate_fallbacks", + } + if set(snapshot) != expected_fields: + raise ValueError("Previous runtime policy snapshot is malformed.") + defaults = default_runtime_policy( + str(agent_config.get("robot_profile", "dual_ur10")) + ) + migrated = deepcopy(dict(snapshot)) + migrated["schema_version"] = RUNTIME_POLICY_SCHEMA + migrated["planner"] = deepcopy(defaults.planner) + migrated_execution = deepcopy(dict(migrated.get("execution", {}))) + for key in ( + "support_stability_samples", + "support_stability_interval_steps", + "support_linear_velocity_tolerance", + "support_angular_velocity_tolerance", + ): + migrated_execution[key] = defaults.execution[key] + migrated["execution"] = migrated_execution + migrated_grounding = deepcopy(dict(migrated.get("grounding", {}))) + migrated_placement = deepcopy(dict(migrated_grounding.get("placement", {}))) + for key, value in defaults.grounding["placement"].items(): + migrated_placement.setdefault(key, value) + migrated_grounding["placement"] = migrated_placement + migrated["grounding"] = migrated_grounding + migrated_grasp = deepcopy(dict(migrated["grasp"])) + migrated_grasp["n_deviated_approach_directions"] = defaults.grasp[ + "n_deviated_approach_directions" + ] + migrated["grasp"] = migrated_grasp + migrated_predicates = deepcopy(dict(migrated["predicate_fallbacks"])) + for key in ( + "support_com_margin", + "support_max_vertical_gap", + "support_max_penetration", + "support_min_overlap_ratio", + ): + migrated_predicates[key] = defaults.predicate_fallbacks[key] + migrated["predicate_fallbacks"] = migrated_predicates + return RuntimePolicyCfg.from_mapping(migrated) + policy = RuntimePolicyCfg.from_mapping(snapshot) + return policy + + +def _mapping_hash(value: Mapping[str, Any]) -> str: + payload = json.dumps( + value, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=True, + ).encode("utf-8") + return hashlib.sha256(payload).hexdigest() diff --git a/embodichain/gen_sim/action_engine/domain/__init__.py b/embodichain/gen_sim/action_engine/domain/__init__.py new file mode 100644 index 000000000..5f2c303ad --- /dev/null +++ b/embodichain/gen_sim/action_engine/domain/__init__.py @@ -0,0 +1,93 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Stable public contracts for Action Engine programs.""" + +from __future__ import annotations + +from .motion import ( + MOTION_MODIFIER_MODES, + MOTION_POLICY_VERSION, + motion_policy, + validate_motion_policy, +) +from .programs import ( + EXECUTION_PROGRAM_SCHEMA, + TASK_AGENT_SCHEMA, + execution_program_hash, + validate_execution_program, + validate_task_agent, +) +from .task_contracts import ( + PLACEMENT_RELATIONS, + RELATIONS, + TASK_CONTRACTS, + TERMINAL_BEHAVIORS, + TRANSPORT_DIRECTIONS, + TaskContract, + normalize_placement_relation, + task_contract, + task_success_type, +) +from .v2 import ( + REASONING_TYPES, + TASK_LEVELS, + TASK_TYPES, + public_task_spec, + seed_graph_hash, + validate_public_task_spec, + validate_scene_requirements, + validate_seed_graph, + validate_task_spec, +) +from .visual_contracts import ( + OCCLUSION_RELATION, + VISUAL_RELATION_PARTICIPANTS, + requested_visual_task_predicates, +) + +__all__ = [ + "EXECUTION_PROGRAM_SCHEMA", + "MOTION_POLICY_VERSION", + "MOTION_MODIFIER_MODES", + "OCCLUSION_RELATION", + "REASONING_TYPES", + "RELATIONS", + "PLACEMENT_RELATIONS", + "TASK_CONTRACTS", + "TASK_LEVELS", + "TASK_TYPES", + "TASK_AGENT_SCHEMA", + "TERMINAL_BEHAVIORS", + "TRANSPORT_DIRECTIONS", + "VISUAL_RELATION_PARTICIPANTS", + "TaskContract", + "execution_program_hash", + "motion_policy", + "normalize_placement_relation", + "public_task_spec", + "requested_visual_task_predicates", + "seed_graph_hash", + "task_contract", + "task_success_type", + "validate_public_task_spec", + "validate_scene_requirements", + "validate_seed_graph", + "validate_task_spec", + "validate_execution_program", + "validate_motion_policy", + "validate_task_agent", +] diff --git a/embodichain/gen_sim/action_engine/domain/motion.py b/embodichain/gen_sim/action_engine/domain/motion.py new file mode 100644 index 000000000..ae9ced35d --- /dev/null +++ b/embodichain/gen_sim/action_engine/domain/motion.py @@ -0,0 +1,111 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Typed, composable motion-policy references persisted in symbolic graphs.""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +from typing import Any, Final + +__all__ = [ + "MOTION_MODIFIER_MODES", + "MOTION_POLICY_VERSION", + "motion_policy", + "validate_motion_policy", +] + +MOTION_POLICY_VERSION: Final = "action_engine_motion_policy_v3" +MOTION_MODIFIER_MODES: Final = { + "orientation": frozenset({"upright"}), + "handover_role": frozenset({"transfer"}), +} + +_POLICY_KEYS = frozenset({"modifiers"}) +_MODIFIER_KEYS = frozenset({"type", "mode"}) + + +def motion_policy(*modifiers: tuple[str, str]) -> dict[str, Any]: + """Build one canonical policy reference from typed modifier pairs.""" + return validate_motion_policy( + { + "modifiers": [ + {"type": modifier_type, "mode": mode} + for modifier_type, mode in modifiers + ] + } + ) + + +def validate_motion_policy( + value: Any, + context: str = "motion_policy", +) -> dict[str, Any]: + """Validate and detach one symbolic motion-policy reference.""" + if not isinstance(value, Mapping): + raise ValueError( + f"{context} must be a mapping with typed modifiers; named string " + "policies are no longer supported. Regenerate the graph." + ) + if set(value) != _POLICY_KEYS: + raise ValueError(f"{context} fields must be {sorted(_POLICY_KEYS)}.") + raw_modifiers = value.get("modifiers") + if not isinstance(raw_modifiers, (list, tuple)): + raise ValueError(f"{context}.modifiers must be a sequence.") + + modifiers: list[dict[str, str]] = [] + seen: set[tuple[str, str]] = set() + seen_types: set[str] = set() + for index, raw_modifier in enumerate(raw_modifiers): + modifier_context = f"{context}.modifiers[{index}]" + if not isinstance(raw_modifier, Mapping): + raise ValueError(f"{modifier_context} must be a mapping.") + if set(raw_modifier) != _MODIFIER_KEYS: + raise ValueError( + f"{modifier_context} fields must be {sorted(_MODIFIER_KEYS)}." + ) + modifier_type = raw_modifier.get("type") + mode = raw_modifier.get("mode") + if not isinstance(modifier_type, str) or not modifier_type: + raise ValueError(f"{modifier_context}.type must be a non-empty string.") + if modifier_type not in MOTION_MODIFIER_MODES: + raise ValueError( + f"{modifier_context}.type {modifier_type!r} is unsupported; " + f"expected one of {sorted(MOTION_MODIFIER_MODES)}." + ) + if ( + not isinstance(mode, str) + or mode not in MOTION_MODIFIER_MODES[modifier_type] + ): + raise ValueError( + f"{modifier_context}.mode {mode!r} is unsupported for " + f"{modifier_type!r}; expected one of " + f"{sorted(MOTION_MODIFIER_MODES[modifier_type])}." + ) + key = (modifier_type, mode) + if key in seen: + raise ValueError(f"{modifier_context} duplicates modifier {key!r}.") + if modifier_type in seen_types: + raise ValueError( + f"{context} may select only one mode for modifier type " + f"{modifier_type!r}." + ) + seen.add(key) + seen_types.add(modifier_type) + modifiers.append({"type": modifier_type, "mode": mode}) + + return {"modifiers": deepcopy(modifiers)} diff --git a/embodichain/gen_sim/action_engine/domain/programs.py b/embodichain/gen_sim/action_engine/domain/programs.py new file mode 100644 index 000000000..a1243f1ea --- /dev/null +++ b/embodichain/gen_sim/action_engine/domain/programs.py @@ -0,0 +1,848 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Coordinate-free task and execution program contracts. + +The task agent is the only structure an LLM is allowed to influence. The +execution program is produced deterministically and contains the complete +symbolic action DAG consumed by runtime. Neither representation may contain +poses, trajectories, joint values, or other environment-specific geometry. +""" + +from __future__ import annotations + +import hashlib +import json +from collections import deque +from collections.abc import Collection, Mapping, Sequence +from copy import deepcopy +from typing import Any + +from embodichain.gen_sim.action_engine.protocol import ( + EXECUTION_PROGRAM_SCHEMA, + TASK_AGENT_SCHEMA, +) +from .motion import MOTION_POLICY_VERSION, validate_motion_policy + +__all__ = [ + "EXECUTION_PROGRAM_SCHEMA", + "MOTION_POLICY_VERSION", + "TASK_AGENT_SCHEMA", + "execution_program_hash", + "validate_execution_program", + "validate_task_agent", +] + +_ACTOR_MODES = frozenset({"auto", "required", "coordinated"}) +_CONTROL_MODES = frozenset({"arm", "hand", "coordinated"}) +_TASK_KEYS = frozenset( + {"schema_version", "task", "goal", "semantic_steps", "allocation_groups"} +) +_TASK_STEP_KEYS = frozenset( + {"id", "operator", "object", "objects", "actor", "goal", "depends_on"} +) +_EXECUTION_KEYS = frozenset( + { + "schema_version", + "task", + "goal_description", + "start", + "goal", + "nodes", + "edges", + "semantic_steps", + "allocation_groups", + "motion_policy_version", + } +) +_EXECUTION_STEP_KEYS = frozenset( + { + "id", + "parent_step_id", + "operator", + "object", + "actor", + "goal", + "depends_on", + "postcondition", + "edge_ids", + } +) +_EDGE_KEYS = frozenset( + { + "id", + "source", + "target", + "semantic_step_id", + "actions", + "depends_on", + "resources", + } +) +_ACTION_KEYS = frozenset( + { + "atomic_action_class", + "actor", + "control", + "target_binding", + "motion_policy", + "seed_node_id", + "failure_policy", + } +) +_FAILURE_POLICIES = frozenset({"task_required", "safety_required", "best_effort"}) +_TASK_ALLOCATION_GROUP_KEYS = frozenset({"id", "semantic_step_ids", "arm_constraint"}) +_BINDING_REQUIREMENTS = { + "articulation_goal": frozenset({"object"}), + "coordinated_goal": frozenset({"object"}), + "coordinated_placement_goal": frozenset({"placing_object", "support_object"}), + "current_held_pose": frozenset(), + "handover_goal": frozenset({"object"}), + "handover_staging": frozenset({"object"}), + "joint_state": frozenset({"source"}), + "object": frozenset({"object"}), + "policy_pose": frozenset(), + "pour_goal": frozenset({"object", "reference_object"}), + "semantic_goal": frozenset({"semantic_step"}), + "visual_constraint": frozenset({"camera_uid", "normalized_keypoint"}), +} +_POSTCONDITION_TYPES = frozenset( + { + "both_arms_at_initial_qpos", + "both_grippers_open", + "coordinated_placed", + "grippers_clear_of_object", + "held_by_both_grippers", + "line_member_placed", + "object_axis_near", + "object_axis_offset_near", + "object_held", + "object_held_by_both_grippers", + "object_held_by_gripper", + "object_in_container", + "object_lifted", + "object_not_fallen", + "object_on_object", + "object_supported_by", + "object_position_near", + "object_upright", + "object_xy_near", + "objects_collinear", + "objects_ordered", + "pressed", + "poured", + "articulation_joint_near", + "handover_complete", + "visual_relation", + "stable_unobstructed", + "sum_equals", + "semantic_goal", + "stack_layer_supported", + } +) +_OBJECT_REFERENCE_KEYS = frozenset( + { + "anchor", + "container", + "object", + "object_uid", + "orientation_reference_object", + "placing_object", + "reference", + "reference_object", + "support", + "support_object", + } +) + +# These fields indicate that planning-time or runtime geometry leaked into a +# symbolic program. Integers such as slot and layer indices remain valid. +_GROUNDED_FIELD_NAMES = frozenset( + { + "absolute_position", + "coordinates", + "joint_positions", + "object_target_pose", + "position", + "positions", + "pose", + "qpos", + "release_position", + "staging_position", + "target_pose", + "trajectory", + "waypoints", + "xpos", + } +) + + +def validate_task_agent( + program: Mapping[str, Any], + *, + known_objects: Collection[str] | None = None, +) -> dict[str, Any]: + """Validate and return a detached canonical TaskAgent mapping. + + Defaults are added only for structural fields that have one unambiguous + meaning: ``actor={"mode": "auto"}``, an empty goal, and no dependencies. + Operator-specific semantics are validated by the capability registry. + + Args: + program: Candidate route-free task agent. + known_objects: Optional runtime scene UIDs. When supplied, every object + reference is validated before compilation. + + Returns: + A deep-copied, canonical mapping safe for compilation. + + Raises: + ValueError: If the program violates the TaskAgent contract. + """ + value = _mapping_copy(program, "TaskAgent") + _reject_unknown_keys(value, _TASK_KEYS, "TaskAgent") + _require_schema(value, TASK_AGENT_SCHEMA, "TaskAgent") + _require_nonempty_string(value.get("task"), "TaskAgent.task") + _require_nonempty_string(value.get("goal"), "TaskAgent.goal") + + raw_steps = _sequence(value.get("semantic_steps"), "TaskAgent.semantic_steps") + if not raw_steps: + raise ValueError("TaskAgent.semantic_steps must not be empty.") + + steps: list[dict[str, Any]] = [] + for index, raw_step in enumerate(raw_steps): + context = f"TaskAgent.semantic_steps[{index}]" + step = _mapping_copy(raw_step, context) + _reject_unknown_keys(step, _TASK_STEP_KEYS, context) + _require_nonempty_string(step.get("id"), f"{context}.id") + _require_nonempty_string(step.get("operator"), f"{context}.operator") + + has_object = "object" in step + has_objects = "objects" in step + if has_object == has_objects: + raise ValueError( + f"{context} must contain exactly one of 'object' or 'objects'." + ) + if has_object: + _require_nonempty_string(step["object"], f"{context}.object") + else: + objects = _string_list(step["objects"], f"{context}.objects") + if not objects: + raise ValueError(f"{context}.objects must not be empty.") + _require_unique(objects, f"{context}.objects") + step["objects"] = objects + + step["actor"] = _validate_actor( + step.get("actor", {"mode": "auto"}), + f"{context}.actor", + ) + step["goal"] = _mapping_copy(step.get("goal", {}), f"{context}.goal") + step["depends_on"] = _string_list( + step.get("depends_on", []), + f"{context}.depends_on", + ) + _require_unique(step["depends_on"], f"{context}.depends_on") + steps.append(step) + + step_ids = [step["id"] for step in steps] + _require_unique(step_ids, "TaskAgent semantic step IDs") + dependencies = {step["id"]: step["depends_on"] for step in steps} + _validate_dependency_dag(dependencies, "TaskAgent semantic steps") + value["semantic_steps"] = steps + value["allocation_groups"] = _validate_task_allocation_groups( + value.get("allocation_groups", []), + set(step_ids), + ) + if known_objects is not None: + _validate_known_objects(value, known_objects) + _reject_grounded_values(value) + return value + + +def validate_execution_program(program: Mapping[str, Any]) -> dict[str, Any]: + """Validate and return a detached canonical ExecutionProgram mapping. + + Validation covers both dependency DAGs: semantic-step dependencies and + executable edge dependencies. It also proves node reachability, edge + ownership, resource declarations, and the symbolic action envelope. + + Args: + program: Candidate deterministic execution program. + + Returns: + A deep-copied mapping safe for runtime consumption or hashing. + + Raises: + ValueError: If the program violates the ExecutionProgram contract. + """ + value = _mapping_copy(program, "ExecutionProgram") + _reject_unknown_keys(value, _EXECUTION_KEYS, "ExecutionProgram") + _require_schema(value, EXECUTION_PROGRAM_SCHEMA, "ExecutionProgram") + _require_nonempty_string(value.get("task"), "ExecutionProgram.task") + _require_nonempty_string( + value.get("goal_description"), + "ExecutionProgram.goal_description", + ) + _require_nonempty_string(value.get("start"), "ExecutionProgram.start") + _require_nonempty_string(value.get("goal"), "ExecutionProgram.goal") + if value.get("motion_policy_version") != MOTION_POLICY_VERSION: + raise ValueError( + "ExecutionProgram.motion_policy_version must be " + f"{MOTION_POLICY_VERSION!r}." + ) + + nodes = _validate_nodes(value.get("nodes")) + node_ids = {node["id"] for node in nodes} + if value["start"] not in node_ids or value["goal"] not in node_ids: + raise ValueError("ExecutionProgram start and goal must reference nodes.") + + edges = _validate_edges(value.get("edges"), node_ids) + edge_by_id = {edge["id"]: edge for edge in edges} + _validate_dependency_dag( + {edge_id: edge["depends_on"] for edge_id, edge in edge_by_id.items()}, + "ExecutionProgram edges", + ) + _validate_node_reachability( + start=value["start"], + goal=value["goal"], + node_ids=node_ids, + edges=edges, + ) + + semantic_steps = _validate_execution_steps( + value.get("semantic_steps"), + edge_by_id, + ) + step_ids = {step["id"] for step in semantic_steps} + _validate_allocation_groups(value.get("allocation_groups"), step_ids) + + value["nodes"] = nodes + value["edges"] = edges + value["semantic_steps"] = semantic_steps + value["allocation_groups"] = deepcopy(list(value.get("allocation_groups", []))) + _reject_grounded_values(value) + return value + + +def execution_program_hash(program: Mapping[str, Any]) -> str: + """Return the stable SHA-256 hash of a validated ExecutionProgram.""" + canonical = validate_execution_program(program) + try: + payload = json.dumps( + canonical, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + ).encode("utf-8") + except (TypeError, ValueError) as exc: + raise ValueError( + "ExecutionProgram must contain JSON-serializable values." + ) from exc + return hashlib.sha256(payload).hexdigest() + + +def _validate_nodes(value: Any) -> list[dict[str, Any]]: + raw_nodes = _sequence(value, "ExecutionProgram.nodes") + if len(raw_nodes) < 2: + raise ValueError("ExecutionProgram.nodes must contain start and goal nodes.") + nodes: list[dict[str, Any]] = [] + for index, raw_node in enumerate(raw_nodes): + context = f"ExecutionProgram.nodes[{index}]" + node = _mapping_copy(raw_node, context) + _reject_unknown_keys(node, frozenset({"id", "semantic"}), context) + _require_nonempty_string(node.get("id"), f"{context}.id") + _require_nonempty_string(node.get("semantic"), f"{context}.semantic") + nodes.append(node) + _require_unique([node["id"] for node in nodes], "ExecutionProgram node IDs") + return nodes + + +def _validate_edges(value: Any, node_ids: set[str]) -> list[dict[str, Any]]: + raw_edges = _sequence(value, "ExecutionProgram.edges") + if not raw_edges: + raise ValueError("ExecutionProgram.edges must not be empty.") + edges: list[dict[str, Any]] = [] + for index, raw_edge in enumerate(raw_edges): + context = f"ExecutionProgram.edges[{index}]" + edge = _mapping_copy(raw_edge, context) + _reject_unknown_keys(edge, _EDGE_KEYS, context) + for key in ("id", "source", "target", "semantic_step_id"): + _require_nonempty_string(edge.get(key), f"{context}.{key}") + if edge["source"] not in node_ids or edge["target"] not in node_ids: + raise ValueError(f"{context} references an unknown graph node.") + + edge["depends_on"] = _string_list( + edge.get("depends_on", []), + f"{context}.depends_on", + ) + edge["resources"] = _string_list( + edge.get("resources", []), + f"{context}.resources", + ) + _require_unique(edge["depends_on"], f"{context}.depends_on") + _require_unique(edge["resources"], f"{context}.resources") + edge["actions"] = _validate_actions(edge.get("actions"), context) + edges.append(edge) + + edge_ids = [edge["id"] for edge in edges] + _require_unique(edge_ids, "ExecutionProgram edge IDs") + known_edges = set(edge_ids) + for edge in edges: + unknown = set(edge["depends_on"]) - known_edges + if unknown: + raise ValueError( + f"Edge {edge['id']!r} depends on unknown edges: {sorted(unknown)}." + ) + return edges + + +def _validate_actions(value: Any, edge_context: str) -> list[dict[str, Any]]: + raw_actions = _sequence(value, f"{edge_context}.actions") + if not raw_actions: + raise ValueError(f"{edge_context}.actions must not be empty.") + actions: list[dict[str, Any]] = [] + for index, raw_action in enumerate(raw_actions): + context = f"{edge_context}.actions[{index}]" + action = _mapping_copy(raw_action, context) + _reject_unknown_keys(action, _ACTION_KEYS, context) + _require_nonempty_string( + action.get("atomic_action_class"), + f"{context}.atomic_action_class", + ) + action["actor"] = _validate_actor(action.get("actor"), f"{context}.actor") + control = _require_nonempty_string(action.get("control"), f"{context}.control") + if control not in _CONTROL_MODES: + raise ValueError( + f"{context}.control must be one of {sorted(_CONTROL_MODES)}." + ) + binding = _mapping_copy( + action.get("target_binding"), + f"{context}.target_binding", + ) + _require_nonempty_string( + binding.get("kind"), + f"{context}.target_binding.kind", + ) + kind = binding["kind"] + required = _BINDING_REQUIREMENTS.get(kind) + if required is None: + raise ValueError(f"{context}.target_binding.kind {kind!r} is unsupported.") + missing = sorted( + key for key in required if not _is_present_binding_value(binding.get(key)) + ) + if missing: + raise ValueError( + f"{context}.target_binding is missing required fields: {missing}." + ) + action["target_binding"] = binding + action["motion_policy"] = validate_motion_policy( + action.get("motion_policy"), + f"{context}.motion_policy", + ) + if "seed_node_id" in action: + _require_nonempty_string( + action.get("seed_node_id"), + f"{context}.seed_node_id", + ) + failure_policy = action.get("failure_policy", "task_required") + if failure_policy not in _FAILURE_POLICIES: + raise ValueError( + f"{context}.failure_policy must be one of " + f"{sorted(_FAILURE_POLICIES)}." + ) + action["failure_policy"] = str(failure_policy) + actions.append(action) + return actions + + +def _validate_execution_steps( + value: Any, + edge_by_id: Mapping[str, Mapping[str, Any]], +) -> list[dict[str, Any]]: + raw_steps = _sequence(value, "ExecutionProgram.semantic_steps") + if not raw_steps: + raise ValueError("ExecutionProgram.semantic_steps must not be empty.") + steps: list[dict[str, Any]] = [] + for index, raw_step in enumerate(raw_steps): + context = f"ExecutionProgram.semantic_steps[{index}]" + step = _mapping_copy(raw_step, context) + _reject_unknown_keys(step, _EXECUTION_STEP_KEYS, context) + for key in ("id", "parent_step_id", "operator", "object"): + _require_nonempty_string(step.get(key), f"{context}.{key}") + step["actor"] = _validate_actor(step.get("actor"), f"{context}.actor") + step["goal"] = _mapping_copy(step.get("goal"), f"{context}.goal") + step["postcondition"] = _mapping_copy( + step.get("postcondition"), + f"{context}.postcondition", + ) + _require_nonempty_string( + step["postcondition"].get("type"), + f"{context}.postcondition.type", + ) + if step["postcondition"]["type"] not in _POSTCONDITION_TYPES: + raise ValueError( + f"{context}.postcondition.type " + f"{step['postcondition']['type']!r} is unsupported." + ) + step["depends_on"] = _string_list( + step.get("depends_on", []), + f"{context}.depends_on", + ) + step["edge_ids"] = _string_list( + step.get("edge_ids"), + f"{context}.edge_ids", + ) + if not step["edge_ids"]: + raise ValueError(f"{context}.edge_ids must not be empty.") + _require_unique(step["depends_on"], f"{context}.depends_on") + _require_unique(step["edge_ids"], f"{context}.edge_ids") + steps.append(step) + + step_ids = [step["id"] for step in steps] + _require_unique(step_ids, "ExecutionProgram semantic step IDs") + _validate_dependency_dag( + {step["id"]: step["depends_on"] for step in steps}, + "ExecutionProgram semantic steps", + ) + + covered_edges: list[str] = [] + for step in steps: + for edge_id in step["edge_ids"]: + edge = edge_by_id.get(edge_id) + if edge is None: + raise ValueError( + f"Semantic step {step['id']!r} owns unknown edge {edge_id!r}." + ) + if edge["semantic_step_id"] != step["id"]: + raise ValueError( + f"Edge {edge_id!r} is assigned to {edge['semantic_step_id']!r}, " + f"not {step['id']!r}." + ) + covered_edges.append(edge_id) + _require_unique(covered_edges, "ExecutionProgram semantic edge ownership") + if set(covered_edges) != set(edge_by_id): + missing = sorted(set(edge_by_id) - set(covered_edges)) + raise ValueError(f"ExecutionProgram has unowned edges: {missing}.") + return steps + + +def _validate_allocation_groups(value: Any, step_ids: set[str]) -> None: + groups = _sequence(value, "ExecutionProgram.allocation_groups") + group_ids: list[str] = [] + for index, raw_group in enumerate(groups): + context = f"ExecutionProgram.allocation_groups[{index}]" + group = _mapping_copy(raw_group, context) + allowed = frozenset( + { + "id", + "semantic_step_ids", + "arm_constraint", + "execution_policy", + "parallel_action_classes", + "workspace_policy", + } + ) + _reject_unknown_keys(group, allowed, context) + group_ids.append(_require_nonempty_string(group.get("id"), f"{context}.id")) + members = _string_list( + group.get("semantic_step_ids"), + f"{context}.semantic_step_ids", + ) + if len(members) < 2: + raise ValueError(f"{context} must contain at least two semantic steps.") + _require_unique(members, f"{context}.semantic_step_ids") + unknown = set(members) - step_ids + if unknown: + raise ValueError(f"{context} references unknown steps: {sorted(unknown)}.") + for key in ("arm_constraint", "execution_policy", "workspace_policy"): + _require_nonempty_string(group.get(key), f"{context}.{key}") + if group["arm_constraint"] != "distinct_arms": + raise ValueError(f"{context}.arm_constraint must be 'distinct_arms'.") + if group["execution_policy"] != "parallel_if_feasible": + raise ValueError( + f"{context}.execution_policy must be 'parallel_if_feasible'." + ) + if group["workspace_policy"] != "shared_target_serial": + raise ValueError( + f"{context}.workspace_policy must be 'shared_target_serial'." + ) + action_classes = _string_list( + group.get("parallel_action_classes"), + f"{context}.parallel_action_classes", + ) + if not action_classes: + raise ValueError(f"{context}.parallel_action_classes must not be empty.") + _require_unique(group_ids, "ExecutionProgram allocation group IDs") + + +def _validate_task_allocation_groups( + value: Any, + step_ids: set[str], +) -> list[dict[str, Any]]: + groups = _sequence(value, "TaskAgent.allocation_groups") + result: list[dict[str, Any]] = [] + ids: list[str] = [] + members_seen: set[str] = set() + for index, raw_group in enumerate(groups): + context = f"TaskAgent.allocation_groups[{index}]" + group = _mapping_copy(raw_group, context) + _reject_unknown_keys(group, _TASK_ALLOCATION_GROUP_KEYS, context) + group_id = _require_nonempty_string(group.get("id"), f"{context}.id") + members = _string_list( + group.get("semantic_step_ids"), + f"{context}.semantic_step_ids", + ) + if len(members) < 2: + raise ValueError(f"{context} must contain at least two semantic steps.") + _require_unique(members, f"{context}.semantic_step_ids") + unknown = set(members) - step_ids + if unknown: + raise ValueError(f"{context} references unknown steps: {sorted(unknown)}.") + overlap = set(members) & members_seen + if overlap: + raise ValueError( + f"TaskAgent allocation groups overlap at steps: {sorted(overlap)}." + ) + constraint = _require_nonempty_string( + group.get("arm_constraint"), + f"{context}.arm_constraint", + ) + if constraint != "distinct_arms": + raise ValueError(f"{context}.arm_constraint must be 'distinct_arms'.") + ids.append(group_id) + members_seen.update(members) + result.append( + { + "id": group_id, + "semantic_step_ids": members, + "arm_constraint": constraint, + } + ) + _require_unique(ids, "TaskAgent allocation group IDs") + return result + + +def _validate_known_objects( + program: Mapping[str, Any], + known_objects: Collection[str], +) -> None: + known = {str(uid) for uid in known_objects} + if not known: + raise ValueError("known_objects must not be empty when supplied.") + allowed_sentinels = {"self", "table", "table_center"} + references: list[tuple[str, str]] = [] + for index, step in enumerate(program["semantic_steps"]): + if "object" in step: + references.append((f"semantic_steps[{index}].object", step["object"])) + for item_index, uid in enumerate(step.get("objects", [])): + references.append((f"semantic_steps[{index}].objects[{item_index}]", uid)) + _collect_object_references( + step["goal"], + f"semantic_steps[{index}].goal", + references, + ) + unknown = [ + f"{path}={uid!r}" + for path, uid in references + if uid not in known and uid not in allowed_sentinels + ] + if unknown: + raise ValueError( + "TaskAgent references objects not present in the scene: " + + ", ".join(unknown) + + "." + ) + + +def _collect_object_references( + value: Any, + path: str, + output: list[tuple[str, str]], +) -> None: + if isinstance(value, Mapping): + for key, child in value.items(): + child_path = f"{path}.{key}" + if key in _OBJECT_REFERENCE_KEYS and isinstance(child, str): + output.append((child_path, child)) + elif ( + key in {"objects", "object_uids", "payloads"} + and isinstance(child, Sequence) + and not isinstance(child, (str, bytes, bytearray)) + ): + for index, item in enumerate(child): + uid = item.get("object") if isinstance(item, Mapping) else item + if isinstance(uid, str): + output.append((f"{child_path}[{index}]", uid)) + else: + _collect_object_references(child, child_path, output) + + +def _is_present_binding_value(value: Any) -> bool: + return value is not None and value != "" and value != [] + + +def _validate_actor(value: Any, context: str) -> dict[str, Any]: + actor = _mapping_copy(value, context) + mode = _require_nonempty_string(actor.get("mode"), f"{context}.mode") + if mode not in _ACTOR_MODES: + raise ValueError(f"{context}.mode must be one of {sorted(_ACTOR_MODES)}.") + if mode == "auto": + _reject_unknown_keys(actor, frozenset({"mode", "allocation_group"}), context) + elif mode == "required": + _reject_unknown_keys( + actor, + frozenset({"mode", "arm", "allocation_group"}), + context, + ) + _require_nonempty_string(actor.get("arm"), f"{context}.arm") + else: + _reject_unknown_keys(actor, frozenset({"mode", "arms"}), context) + arms = _string_list(actor.get("arms"), f"{context}.arms") + if len(arms) < 2: + raise ValueError(f"{context}.arms must contain at least two arms.") + _require_unique(arms, f"{context}.arms") + actor["arms"] = arms + if "allocation_group" in actor: + _require_nonempty_string( + actor["allocation_group"], + f"{context}.allocation_group", + ) + return actor + + +def _validate_dependency_dag( + dependencies: Mapping[str, Sequence[str]], + context: str, +) -> None: + known = set(dependencies) + outgoing: dict[str, list[str]] = {item_id: [] for item_id in known} + indegree = {item_id: 0 for item_id in known} + for item_id, required_ids in dependencies.items(): + unknown = set(required_ids) - known + if unknown: + raise ValueError(f"{context} reference unknown IDs: {sorted(unknown)}.") + if item_id in required_ids: + raise ValueError(f"{context} contain a self-dependency at {item_id!r}.") + for required_id in required_ids: + outgoing[required_id].append(item_id) + indegree[item_id] += 1 + + ready = deque( + sorted(item_id for item_id, degree in indegree.items() if degree == 0) + ) + visited = 0 + while ready: + item_id = ready.popleft() + visited += 1 + for dependent_id in sorted(outgoing[item_id]): + indegree[dependent_id] -= 1 + if indegree[dependent_id] == 0: + ready.append(dependent_id) + if visited != len(known): + cyclic = sorted(item_id for item_id, degree in indegree.items() if degree > 0) + raise ValueError(f"{context} contain a dependency cycle: {cyclic}.") + + +def _validate_node_reachability( + *, + start: str, + goal: str, + node_ids: set[str], + edges: Sequence[Mapping[str, Any]], +) -> None: + outgoing: dict[str, list[str]] = {node_id: [] for node_id in node_ids} + for edge in edges: + outgoing[edge["source"]].append(edge["target"]) + reachable = {start} + ready = deque([start]) + while ready: + node_id = ready.popleft() + for target_id in outgoing[node_id]: + if target_id not in reachable: + reachable.add(target_id) + ready.append(target_id) + if goal not in reachable: + raise ValueError("ExecutionProgram goal is unreachable from start.") + unreachable = sorted(node_ids - reachable) + if unreachable: + raise ValueError(f"ExecutionProgram contains unreachable nodes: {unreachable}.") + + +def _reject_grounded_values(value: Any, path: str = "program") -> None: + if isinstance(value, Mapping): + for key, child in value.items(): + normalized = str(key).strip().lower() + if normalized in _GROUNDED_FIELD_NAMES: + raise ValueError( + f"{path}.{key} is grounded runtime data and is not allowed." + ) + _reject_grounded_values(child, f"{path}.{key}") + return + if isinstance(value, list): + for index, child in enumerate(value): + _reject_grounded_values(child, f"{path}[{index}]") + return + if isinstance(value, float): + raise ValueError( + f"{path} contains a floating-point runtime value; use a named " + "motion policy or symbolic relation instead." + ) + + +def _mapping_copy(value: Any, context: str) -> dict[str, Any]: + if not isinstance(value, Mapping): + raise ValueError(f"{context} must be a mapping.") + return deepcopy(dict(value)) + + +def _sequence(value: Any, context: str) -> list[Any]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes, bytearray)): + raise ValueError(f"{context} must be a list.") + return list(value) + + +def _string_list(value: Any, context: str) -> list[str]: + items = _sequence(value, context) + result: list[str] = [] + for index, item in enumerate(items): + result.append(_require_nonempty_string(item, f"{context}[{index}]")) + return result + + +def _require_schema(value: Mapping[str, Any], expected: str, context: str) -> None: + if value.get("schema_version") != expected: + raise ValueError(f"{context}.schema_version must be {expected!r}.") + + +def _require_nonempty_string(value: Any, context: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{context} must be a non-empty string.") + return value + + +def _require_unique(values: Sequence[str], context: str) -> None: + if len(values) != len(set(values)): + raise ValueError(f"{context} must not contain duplicates.") + + +def _reject_unknown_keys( + value: Mapping[str, Any], + allowed: frozenset[str], + context: str, +) -> None: + unknown = sorted(set(value) - allowed) + if unknown: + raise ValueError(f"{context} contains unknown fields: {unknown}.") diff --git a/embodichain/gen_sim/action_engine/domain/task_contracts.py b/embodichain/gen_sim/action_engine/domain/task_contracts.py new file mode 100644 index 000000000..dccbb384e --- /dev/null +++ b/embodichain/gen_sim/action_engine/domain/task_contracts.py @@ -0,0 +1,155 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Action Engine recipes layered over the Task Engine semantic ontology.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +from types import MappingProxyType +from typing import Any + +from embodichain.gen_sim.task_engine.ontology import ( + RELATIONS, + TERMINAL_BEHAVIORS, + TRANSPORT_DIRECTIONS, + TaskContract as SemanticTaskContract, + task_success_type, +) +from embodichain.gen_sim.task_engine.ontology import ( + TASK_CONTRACTS as SEMANTIC_TASK_CONTRACTS, +) + +__all__ = [ + "PLACEMENT_RELATIONS", + "RELATIONS", + "TASK_CONTRACTS", + "TERMINAL_BEHAVIORS", + "TRANSPORT_DIRECTIONS", + "TaskContract", + "task_contract", + "task_success_type", + "normalize_placement_relation", +] + +PLACEMENT_RELATIONS = RELATIONS - {"none"} +_SUPPORTED_PLACEMENT_ALIASES = frozenset({"above", "on_top", "on_top_of"}) + + +def normalize_placement_relation(value: Any) -> str: + """Lower task-language relations to physically executable release goals. + + A released object cannot remain freely hovering. Task-language ``above`` + therefore lowers to the supported ``on`` relation for placement operators; + non-placement operators such as pouring retain their distinct ``above`` + semantics. + """ + relation = str(value) + if ( + relation not in PLACEMENT_RELATIONS + and relation not in _SUPPORTED_PLACEMENT_ALIASES + ): + raise ValueError(f"Unsupported placement relation {relation!r}.") + return "on" if relation in _SUPPORTED_PLACEMENT_ALIASES else relation + + +_CORE_ACTIONS: Mapping[str, tuple[str, ...]] = MappingProxyType( + { + "E1": ("PickUp", "MoveHeldObject", "Place"), + "E2": ("PickUp", "MoveHeldObject", "MoveJoints"), + "E3": ("PickUp", "MoveHeldObject", "Pour", "Place"), + "E4": ("PickUp", "MoveHeldObject", "HandOver", "Place"), + "E5": ("CoordinatedPickment",), + "E6": ("PullArticulatedPart",), + "E7": ("PushArticulatedPart",), + "E8": ("TurnKnob",), + "E9": ("Press",), + } +) + +_SIGNATURE_ACTIONS: Mapping[str, frozenset[str]] = MappingProxyType( + { + "E1": frozenset(), + "E2": frozenset(), + "E3": frozenset({"Pour"}), + "E4": frozenset({"HandOver"}), + "E5": frozenset(), + "E6": frozenset({"PullArticulatedPart"}), + "E7": frozenset({"PushArticulatedPart"}), + "E8": frozenset({"TurnKnob"}), + "E9": frozenset({"Press"}), + } +) + + +@dataclass(frozen=True, slots=True) +class TaskContract: + """Action-facing view of one Task Engine semantic contract.""" + + task_type: str + semantics: str + core_actions: tuple[str, ...] + applicable_intent_fields: frozenset[str] + source_structure: str + required_affordances: frozenset[str] + success_type: str + scene_affordances: frozenset[str] + primary_role_field: str + resource_mode: str + moves_primary_object: bool + accepts_direct_payloads: bool + direct_payload_relations: frozenset[str] + accepts_incoming_hold: bool + terminal_success_types: tuple[tuple[str, str], ...] + signature_actions: frozenset[str] = frozenset() + + +def _action_contract(value: SemanticTaskContract) -> TaskContract: + return TaskContract( + task_type=value.task_type, + semantics=value.semantics, + core_actions=_CORE_ACTIONS[value.task_type], + signature_actions=_SIGNATURE_ACTIONS[value.task_type], + applicable_intent_fields=value.applicable_intent_fields, + source_structure=value.source_structure, + required_affordances=value.required_affordances, + success_type=value.success_type, + scene_affordances=value.scene_affordances, + primary_role_field=value.primary_role_field, + resource_mode=value.resource_mode, + moves_primary_object=value.moves_primary_object, + accepts_direct_payloads=value.accepts_direct_payloads, + direct_payload_relations=value.direct_payload_relations, + accepts_incoming_hold=value.accepts_incoming_hold, + terminal_success_types=value.terminal_success_types, + ) + + +TASK_CONTRACTS: Mapping[str, TaskContract] = MappingProxyType( + { + task_type: _action_contract(contract) + for task_type, contract in SEMANTIC_TASK_CONTRACTS.items() + } +) + + +def task_contract(task_type: str) -> TaskContract: + """Return the Action Engine view of one canonical task contract.""" + try: + return TASK_CONTRACTS[str(task_type)] + except KeyError as exc: + raise ValueError(f"Unsupported task type {task_type!r}.") from exc diff --git a/embodichain/gen_sim/action_engine/domain/v2.py b/embodichain/gen_sim/action_engine/domain/v2.py new file mode 100644 index 000000000..8125d8e26 --- /dev/null +++ b/embodichain/gen_sim/action_engine/domain/v2.py @@ -0,0 +1,1265 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Strict coordinate-free contracts for Action Engine SeedGraph v3.""" + +from __future__ import annotations + +from collections import deque +from collections.abc import Collection, Mapping, Sequence +from copy import deepcopy +import hashlib +import json +import math +from typing import Any + +from embodichain.gen_sim.action_engine.protocol import ( + SCENE_REQUIREMENTS_SCHEMA, + SEED_GRAPH_SCHEMA, + TASK_SPEC_SCHEMA, +) +from .motion import validate_motion_policy + +__all__ = [ + "REASONING_TYPES", + "TASK_LEVELS", + "TASK_TYPES", + "public_task_spec", + "seed_graph_hash", + "validate_public_task_spec", + "validate_scene_requirements", + "validate_seed_graph", + "validate_task_spec", +] + +TASK_LEVELS = frozenset({"L1", "L2", "L3", "L4"}) +TASK_TYPES = frozenset({f"E{index}" for index in range(1, 10)}) +REASONING_TYPES = frozenset( + { + "none", + "memory", + "visual_semantics", + "pattern", + "logic", + "common_sense", + "constraint", + } +) + +_TASK_SPEC_KEYS = frozenset( + { + "schema_version", + "task_id", + "level", + "instruction", + "reasoning_type", + "task_instances", + "success", + "oracle", + "metadata", + } +) +_TASK_INSTANCE_KEYS = frozenset({"id", "task_type", "params", "depends_on", "role"}) +_SCENE_REQUIREMENTS_KEYS = frozenset( + { + "schema_version", + "task_id", + "objects", + "cameras", + "spatial_constraints", + "distractor_count", + "metadata", + } +) +_OBJECT_REQUIREMENT_KEYS = frozenset( + { + "role_id", + "category", + "count", + "affordances", + "initial_state", + "attributes", + } +) +_SEED_GRAPH_KEYS = frozenset( + { + "schema_version", + "task_id", + "instruction", + "level", + "reasoning_type", + "planner_route", + "nodes", + "task_groups", + "success", + "capability_catalog_hash", + "metadata", + } +) +_ACTION_NODE_KEYS = frozenset( + { + "id", + "atomic_action", + "object_uid", + "actor", + "control", + "target_binding", + "depends_on", + "contract", + "task_instance_id", + "task_type", + "role", + "precondition", + "postcondition", + "motion_policy", + "sync_group", + } +) +_TASK_GROUP_KEYS = frozenset( + { + "id", + "task_type", + "role", + "operator", + "object_uid", + "actor", + "goal", + "depends_on", + "parent_task_instance_id", + "node_ids", + "success", + "contract", + } +) +_ACTOR_MODES = frozenset({"auto", "required", "preferred", "coordinated"}) +_NODE_ROLES = frozenset({"primary", "recovery", "cleanup"}) +_GROUP_ROLES = frozenset({"primary", "recovery"}) +_PLANNER_ROUTES = frozenset({"offline", "online", "selected", "fused"}) +_ACTION_CONTRACT_KEYS = frozenset( + { + "version", + "requires", + "effects", + "claims", + "completion", + "failure_policy", + } +) +_TASK_GROUP_CONTRACT_KEYS = frozenset( + { + "entry_requires", + "exit_effects", + "claims", + "entry_node_ids", + "terminal_node_ids", + "completion", + } +) +_STATE_ATOM_KEYS = frozenset({"predicate", "object_uid", "arm"}) +_STATE_PREDICATES = frozenset( + { + "arm_free", + "object_free", + "object_held", + "object_coordinated_held", + "handover_complete", + "arm_clear", + "arm_home", + } +) +_EFFECT_KEYS = frozenset({"op", "atom"}) +_EFFECT_OPERATIONS = frozenset({"add", "delete"}) +_CLAIM_KEYS = frozenset({"resource", "access", "lifetime"}) +_CLAIM_ACCESS = frozenset({"shared_read", "exclusive"}) +_CLAIM_LIFETIMES = frozenset({"action", "until_release"}) +_ACTION_COMPLETION = frozenset({"ordinary", "cleanup", "terminal_barrier"}) +_FAILURE_POLICIES = frozenset({"task_required", "safety_required", "best_effort"}) +_GROUP_COMPLETION = frozenset({"ordinary", "terminal_barrier"}) +_OBJECT_REFERENCE_KEYS = frozenset( + { + "anchor", + "container", + "object", + "object_uid", + "placing_object", + "reference", + "reference_object", + "support", + "support_object", + } +) +_GROUNDED_FIELD_NAMES = frozenset( + { + "absolute_position", + "coordinates", + "grasp_pose", + "joint_positions", + "object_target_pose", + "position", + "positions", + "pose", + "qpos", + "release_position", + "staging_position", + "target_pose", + "trajectory", + "waypoints", + "xpos", + } +) + + +def validate_task_spec(value: Mapping[str, Any]) -> dict[str, Any]: + """Validate one task-first, scene-independent task specification.""" + result = _mapping(value, "TaskSpec") + _keys(result, _TASK_SPEC_KEYS, "TaskSpec") + _schema(result, TASK_SPEC_SCHEMA, "TaskSpec") + _string(result.get("task_id"), "TaskSpec.task_id") + level = _enum(result.get("level"), TASK_LEVELS, "TaskSpec.level") + _string(result.get("instruction"), "TaskSpec.instruction") + reasoning = _enum( + result.get("reasoning_type", "none"), + REASONING_TYPES, + "TaskSpec.reasoning_type", + ) + if level == "L4" and reasoning == "none": + raise ValueError("TaskSpec L4 tasks require a non-'none' reasoning_type.") + if level != "L4" and reasoning != "none": + raise ValueError("Only TaskSpec L4 tasks may declare reasoning_type.") + + instances: list[dict[str, Any]] = [] + for index, item in enumerate( + _sequence(result.get("task_instances"), "TaskSpec.task_instances") + ): + context = f"TaskSpec.task_instances[{index}]" + instance = _mapping(item, context) + _keys(instance, _TASK_INSTANCE_KEYS, context) + _string(instance.get("id"), f"{context}.id") + _enum(instance.get("task_type"), TASK_TYPES, f"{context}.task_type") + instance["params"] = _mapping(instance.get("params", {}), f"{context}.params") + instance["depends_on"] = _strings( + instance.get("depends_on", []), f"{context}.depends_on" + ) + instance["role"] = _enum( + instance.get("role", "primary"), _GROUP_ROLES, f"{context}.role" + ) + instances.append(instance) + if not instances: + raise ValueError("TaskSpec.task_instances must not be empty.") + _unique([item["id"] for item in instances], "TaskSpec task instance IDs") + _dag( + {item["id"]: item["depends_on"] for item in instances}, + "TaskSpec task instances", + ) + _validate_level_shape(level, instances) + result["task_instances"] = instances + result["success"] = _mapping(result.get("success"), "TaskSpec.success") + result["oracle"] = _mapping(result.get("oracle", {}), "TaskSpec.oracle") + result["metadata"] = _mapping(result.get("metadata", {}), "TaskSpec.metadata") + _reject_grounded(result) + _finite(result) + return result + + +def public_task_spec(value: Mapping[str, Any]) -> dict[str, Any]: + """Return the validated TaskSpec view safe to expose to an online agent.""" + if "task_instances" not in value and value.get("level") == "L4": + result = dict(value) + _strip_public_private_metadata(result) + return validate_public_task_spec(result) + result = validate_task_spec(value) + result.pop("oracle", None) + if result["level"] == "L4": + # L4 task instances are the hidden reference plan, not public intent. + result.pop("task_instances", None) + _strip_public_private_metadata(result) + return validate_public_task_spec(result) + + +def _strip_public_private_metadata(value: dict[str, Any]) -> None: + """Remove role/UID bindings that would turn the public view into an oracle.""" + metadata = value.get("metadata") + if not isinstance(metadata, Mapping): + return + private_keys = { + "role_bindings", + "uid_map", + "source_uid_map", + "reference_seed_graph", + "oracle", + } + + def strip(child: Any) -> Any: + if isinstance(child, Mapping): + return { + key: strip(nested) + for key, nested in child.items() + if str(key).lower() not in private_keys + } + if isinstance(child, list): + return [strip(item) for item in child] + if isinstance(child, tuple): + return [strip(item) for item in child] + return deepcopy(child) + + value["metadata"] = strip(metadata) + + +def validate_public_task_spec(value: Mapping[str, Any]) -> dict[str, Any]: + """Validate the oracle-free TaskSpec projection consumed online.""" + result = _mapping(value, "PublicTaskSpec") + allowed = _TASK_SPEC_KEYS - {"oracle"} + _keys(result, allowed, "PublicTaskSpec") + if "oracle" in result: + raise ValueError("PublicTaskSpec must not contain oracle data.") + _schema(result, TASK_SPEC_SCHEMA, "PublicTaskSpec") + _string(result.get("task_id"), "PublicTaskSpec.task_id") + level = _enum(result.get("level"), TASK_LEVELS, "PublicTaskSpec.level") + _string(result.get("instruction"), "PublicTaskSpec.instruction") + reasoning = _enum( + result.get("reasoning_type", "none"), + REASONING_TYPES, + "PublicTaskSpec.reasoning_type", + ) + if (level == "L4") != (reasoning != "none"): + raise ValueError( + "PublicTaskSpec reasoning_type must be non-'none' exactly for L4." + ) + if level == "L4": + if "task_instances" in result: + raise ValueError("Public L4 TaskSpec must hide reference task instances.") + else: + # Reuse the complete structural validator for explicit L1-L3 tasks. + normalized = validate_task_spec({**result, "oracle": {}}) + normalized.pop("oracle", None) + return normalized + result["success"] = _mapping(result.get("success"), "PublicTaskSpec.success") + result["metadata"] = _mapping(result.get("metadata", {}), "PublicTaskSpec.metadata") + _reject_grounded(result) + _finite(result) + return result + + +def validate_scene_requirements(value: Mapping[str, Any]) -> dict[str, Any]: + """Validate the structured hand-off contract consumed by a Scene Engine.""" + result = _mapping(value, "SceneRequirements") + _keys(result, _SCENE_REQUIREMENTS_KEYS, "SceneRequirements") + _schema(result, SCENE_REQUIREMENTS_SCHEMA, "SceneRequirements") + _string(result.get("task_id"), "SceneRequirements.task_id") + objects: list[dict[str, Any]] = [] + for index, item in enumerate( + _sequence(result.get("objects"), "SceneRequirements.objects") + ): + context = f"SceneRequirements.objects[{index}]" + requirement = _mapping(item, context) + _keys(requirement, _OBJECT_REQUIREMENT_KEYS, context) + _string(requirement.get("role_id"), f"{context}.role_id") + _string(requirement.get("category"), f"{context}.category") + count = requirement.get("count", 1) + if not isinstance(count, int) or isinstance(count, bool) or count < 1: + raise ValueError(f"{context}.count must be a positive integer.") + requirement["count"] = count + requirement["affordances"] = _strings( + requirement.get("affordances", []), f"{context}.affordances" + ) + requirement["initial_state"] = _mapping( + requirement.get("initial_state", {}), f"{context}.initial_state" + ) + requirement["attributes"] = _mapping( + requirement.get("attributes", {}), f"{context}.attributes" + ) + objects.append(requirement) + if not objects: + raise ValueError("SceneRequirements.objects must not be empty.") + _unique([item["role_id"] for item in objects], "SceneRequirements role IDs") + result["objects"] = objects + result["cameras"] = [ + _mapping(item, f"SceneRequirements.cameras[{index}]") + for index, item in enumerate( + _sequence(result.get("cameras", []), "SceneRequirements.cameras") + ) + ] + result["spatial_constraints"] = [ + _mapping(item, f"SceneRequirements.spatial_constraints[{index}]") + for index, item in enumerate( + _sequence( + result.get("spatial_constraints", []), + "SceneRequirements.spatial_constraints", + ) + ) + ] + distractors = result.get("distractor_count", 0) + if ( + not isinstance(distractors, int) + or isinstance(distractors, bool) + or distractors < 0 + ): + raise ValueError("SceneRequirements.distractor_count must be non-negative.") + result["distractor_count"] = distractors + result["metadata"] = _mapping( + result.get("metadata", {}), "SceneRequirements.metadata" + ) + _finite(result) + return result + + +def validate_seed_graph( + value: Mapping[str, Any], + *, + known_objects: Collection[str] | None = None, + known_actions: Collection[str] | None = None, + executable_actions: Collection[str] | None = None, + require_executable: bool = False, +) -> dict[str, Any]: + """Validate a direct, coordinate-free AtomicAction DAG.""" + result = _mapping(value, "SeedGraph") + _keys(result, _SEED_GRAPH_KEYS, "SeedGraph") + _schema(result, SEED_GRAPH_SCHEMA, "SeedGraph") + _string(result.get("task_id"), "SeedGraph.task_id") + _string(result.get("instruction"), "SeedGraph.instruction") + level = _enum(result.get("level"), TASK_LEVELS, "SeedGraph.level") + reasoning = _enum( + result.get("reasoning_type", "none"), + REASONING_TYPES, + "SeedGraph.reasoning_type", + ) + if (level == "L4") != (reasoning != "none"): + raise ValueError("SeedGraph reasoning_type must be non-'none' exactly for L4.") + result["planner_route"] = _enum( + result.get("planner_route"), _PLANNER_ROUTES, "SeedGraph.planner_route" + ) + _string(result.get("capability_catalog_hash"), "SeedGraph.capability_catalog_hash") + + nodes: list[dict[str, Any]] = [] + for index, item in enumerate(_sequence(result.get("nodes"), "SeedGraph.nodes")): + context = f"SeedGraph.nodes[{index}]" + node = _mapping(item, context) + _keys(node, _ACTION_NODE_KEYS, context) + _string(node.get("id"), f"{context}.id") + action = _string(node.get("atomic_action"), f"{context}.atomic_action") + if known_actions is not None and action not in set(known_actions): + raise ValueError(f"{context} references unknown AtomicAction {action!r}.") + if ( + require_executable + and executable_actions is not None + and action not in set(executable_actions) + ): + raise ValueError( + f"AtomicAction {action!r} is planning-only and cannot be executed." + ) + object_uid = _string(node.get("object_uid"), f"{context}.object_uid") + if known_objects is not None and object_uid not in set(known_objects): + raise ValueError(f"{context} references unknown object {object_uid!r}.") + node["actor"] = _actor(node.get("actor", {"mode": "auto"}), f"{context}.actor") + node["control"] = _string(node.get("control", "arm"), f"{context}.control") + binding = _mapping(node.get("target_binding"), f"{context}.target_binding") + _string(binding.get("kind"), f"{context}.target_binding.kind") + node["target_binding"] = binding + node["depends_on"] = _strings( + node.get("depends_on", []), f"{context}.depends_on" + ) + node["contract"] = _action_contract(node.get("contract"), f"{context}.contract") + node["task_instance_id"] = _string( + node.get("task_instance_id"), f"{context}.task_instance_id" + ) + node["task_type"] = _enum( + node.get("task_type"), TASK_TYPES, f"{context}.task_type" + ) + node["role"] = _enum( + node.get("role", "primary"), _NODE_ROLES, f"{context}.role" + ) + node["precondition"] = _mapping( + node.get("precondition", {}), f"{context}.precondition" + ) + node["postcondition"] = _mapping( + node.get("postcondition", {}), f"{context}.postcondition" + ) + node["motion_policy"] = validate_motion_policy( + node.get("motion_policy"), f"{context}.motion_policy" + ) + if "sync_group" in node: + node["sync_group"] = _string(node["sync_group"], f"{context}.sync_group") + nodes.append(node) + if not nodes: + raise ValueError("SeedGraph.nodes must not be empty.") + node_ids = [node["id"] for node in nodes] + _unique(node_ids, "SeedGraph node IDs") + _dag({node["id"]: node["depends_on"] for node in nodes}, "SeedGraph nodes") + + groups: list[dict[str, Any]] = [] + for index, item in enumerate( + _sequence(result.get("task_groups"), "SeedGraph.task_groups") + ): + context = f"SeedGraph.task_groups[{index}]" + group = _mapping(item, context) + _keys(group, _TASK_GROUP_KEYS, context) + group["id"] = _string(group.get("id"), f"{context}.id") + group["task_type"] = _enum( + group.get("task_type"), TASK_TYPES, f"{context}.task_type" + ) + group["role"] = _enum( + group.get("role", "primary"), _GROUP_ROLES, f"{context}.role" + ) + group["operator"] = _string(group.get("operator"), f"{context}.operator") + group["object_uid"] = _string(group.get("object_uid"), f"{context}.object_uid") + group["actor"] = _actor( + group.get("actor", {"mode": "auto"}), f"{context}.actor" + ) + group["goal"] = _mapping(group.get("goal", {}), f"{context}.goal") + group["depends_on"] = _strings( + group.get("depends_on", []), f"{context}.depends_on" + ) + if "parent_task_instance_id" in group: + group["parent_task_instance_id"] = _string( + group["parent_task_instance_id"], + f"{context}.parent_task_instance_id", + ) + group["node_ids"] = _strings(group.get("node_ids"), f"{context}.node_ids") + if not group["node_ids"]: + raise ValueError(f"{context}.node_ids must not be empty.") + group["success"] = _mapping(group.get("success"), f"{context}.success") + group["contract"] = _task_group_contract( + group.get("contract"), f"{context}.contract" + ) + groups.append(group) + if not groups: + raise ValueError("SeedGraph.task_groups must not be empty.") + _validate_groups(nodes, groups) + _validate_group_contract_topology(nodes, groups) + _validate_cleanup_barriers(nodes, groups) + _validate_task_group_semantics(nodes, groups) + _validate_ownership_transitions(nodes, groups) + _validate_resource_conflicts(nodes, groups, result.get("metadata", {})) + _dag( + {group["id"]: group["depends_on"] for group in groups}, + "SeedGraph task groups", + ) + _validate_group_dependency_alignment(nodes, groups) + result["nodes"] = nodes + result["task_groups"] = groups + result["success"] = _mapping(result.get("success"), "SeedGraph.success") + result["metadata"] = _mapping(result.get("metadata", {}), "SeedGraph.metadata") + _reject_grounded(result) + _finite(result) + if known_objects is not None: + _known_object_references(result, set(known_objects)) + return result + + +def seed_graph_hash(value: Mapping[str, Any]) -> str: + """Return a stable SHA-256 hash for one validated SeedGraph.""" + canonical = validate_seed_graph(value) + payload = json.dumps( + canonical, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode("utf-8") + return hashlib.sha256(payload).hexdigest() + + +def _validate_level_shape(level: str, instances: Sequence[Mapping[str, Any]]) -> None: + primary = [item for item in instances if item["role"] == "primary"] + types = {str(item["task_type"]) for item in primary} + if level == "L1" and len(primary) != 1: + raise ValueError("L1 requires exactly one primary task instance.") + if level == "L2" and (len(primary) < 2 or len(types) != 1): + raise ValueError("L2 requires at least two primary instances of one E type.") + if level == "L3" and (len(primary) < 2 or len(types) < 2): + raise ValueError( + "L3 requires at least two primary instances of different E types." + ) + + +def _validate_groups( + nodes: Sequence[Mapping[str, Any]], groups: Sequence[Mapping[str, Any]] +) -> None: + node_by_id = {str(node["id"]): node for node in nodes} + _unique([str(group["id"]) for group in groups], "SeedGraph task group IDs") + memberships: dict[str, str] = {} + for group in groups: + group_id = str(group["id"]) + for node_id in group["node_ids"]: + if node_id not in node_by_id: + raise ValueError( + f"SeedGraph task group {group_id!r} references unknown node {node_id!r}." + ) + if node_id in memberships: + raise ValueError( + f"SeedGraph node {node_id!r} belongs to multiple task groups." + ) + node = node_by_id[node_id] + if node["task_instance_id"] != group_id: + raise ValueError( + f"SeedGraph node {node_id!r} task_instance_id does not match {group_id!r}." + ) + if node["task_type"] != group["task_type"]: + raise ValueError( + f"SeedGraph node {node_id!r} task_type does not match its group." + ) + memberships[node_id] = group_id + missing = sorted(set(node_by_id) - set(memberships)) + if missing: + raise ValueError( + f"SeedGraph nodes are missing task group membership: {missing}." + ) + + +def _validate_task_group_semantics( + nodes: Sequence[Mapping[str, Any]], + groups: Sequence[Mapping[str, Any]], +) -> None: + from .task_contracts import task_contract + + node_by_id = {str(node["id"]): node for node in nodes} + for group in groups: + task_type = str(group["task_type"]) + contract = task_contract(task_type) + group_nodes = [node_by_id[node_id] for node_id in group["node_ids"]] + actions = {str(node["atomic_action"]) for node in group_nodes} + if group.get("role") == "recovery": + goal = group.get("goal", {}) + if goal.get("recovery_capability") != "object_upright": + raise ValueError( + f"Recovery TaskGroup {group['id']!r} requires a registered " + "recovery_capability." + ) + required = {"PickUp", "MoveHeldObject"} + if goal.get("terminal_behavior") == "place": + required.add("Place") + missing = required - actions + if missing: + raise ValueError( + f"Recovery TaskGroup {group['id']!r} is missing capability " + f"actions: {sorted(missing)}." + ) + continue + if contract.success_type == "poured": + goal = group.get("goal", {}) + unsupported = sorted( + {"pour_mode", "pouring_arm", "holding_arm"} & set(goal) + ) + if unsupported: + raise ValueError( + f"SeedGraph TaskGroup {group['id']!r} uses unsupported " + f"dual-arm pour fields {unsupported}; regenerate it as a " + "single-arm pour over a fixed target container." + ) + missing = set(contract.signature_actions) - actions + if ( + contract.resource_mode == "single_arm" + and contract.success_type == "semantic_goal" + and not contract.terminal_success_types + ): + if not actions.intersection({"MoveHeldObject", "Place"}): + missing = {"MoveHeldObject|Place"} + elif "PickUp" not in actions: + first = group_nodes[0] + precondition = first.get("precondition", {}) + if precondition.get("type") != "object_held": + missing = {"PickUp|object_held precondition"} + if contract.success_type == "object_upright" and "AxisAlign" not in actions: + missing = {"MoveHeldObject"} - actions + if not _has_single_object_release_effect(group_nodes): + missing.add("object release effect") + if "PickUp" not in actions: + first = group_nodes[0] + precondition = first.get("precondition", {}) + if precondition.get("type") != "object_held": + missing.add("PickUp|object_held precondition") + if contract.resource_mode == "handover": + terminal_behavior = str( + group.get("goal", {}).get("terminal_behavior", "hold") + ) + if terminal_behavior == "place" and "Place" not in actions: + missing.add("Place") + if terminal_behavior == "hold" and "Place" in actions: + raise ValueError( + f"SeedGraph TaskGroup {group['id']!r} cannot release during " + "terminal_behavior='hold'." + ) + if contract.resource_mode == "coordinated" and not actions.intersection( + {"CoordinatedPickment", "CoordinatedPlacement"} + ): + missing = {"CoordinatedPickment|CoordinatedPlacement"} + if missing: + raise ValueError( + f"SeedGraph TaskGroup {group['id']!r} is missing {task_type} " + f"core actions: {sorted(missing)}." + ) + + +def _has_single_object_release_effect(nodes: Sequence[Mapping[str, Any]]) -> bool: + """Return whether one node releases a single-arm held object by contract.""" + for node in nodes: + effects = node.get("contract", {}).get("effects", ()) + deleted_held = any( + effect.get("op") == "delete" + and effect.get("atom", {}).get("predicate") == "object_held" + for effect in effects + ) + added_free = any( + effect.get("op") == "add" + and effect.get("atom", {}).get("predicate") == "object_free" + for effect in effects + ) + if deleted_held and added_free: + return True + return False + + +def _validate_ownership_transitions( + nodes: Sequence[Mapping[str, Any]], + groups: Sequence[Mapping[str, Any]], +) -> None: + """Validate object ownership by folding persisted action contracts.""" + node_by_id = {str(node["id"]): node for node in nodes} + ownership: dict[str, str] = {} + + for group in groups: + group_id = str(group["id"]) + object_uid = str(group.get("object_uid")) + current = ownership.get(object_uid, "free") + may_rebase_recovery_entry = group.get("role") == "recovery" + for node_id in group["node_ids"]: + node = node_by_id[str(node_id)] + contract = node.get("contract", {}) + if not isinstance(contract, Mapping): + raise ValueError( + f"SeedGraph node {node_id!r} requires an Action Contract." + ) + for requirement in contract.get("requires", []): + if not isinstance(requirement, Mapping): + continue + if requirement.get("object_uid") != object_uid: + continue + predicate = str(requirement.get("predicate", "")) + expected = ( + "free" + if predicate == "object_free" + else ( + "coordinated" + if predicate == "object_coordinated_held" + else str(requirement.get("arm", "")) + ) + ) + if ( + predicate + in { + "object_free", + "object_held", + "object_coordinated_held", + } + and current != expected + ): + if may_rebase_recovery_entry: + current = expected + else: + raise ValueError( + f"SeedGraph group {group_id!r} requires {object_uid!r} " + f"ownership {expected!r}, but the preceding contract " + f"flow provides {current!r}." + ) + may_rebase_recovery_entry = False + for effect in contract.get("effects", []): + if not isinstance(effect, Mapping) or effect.get("op") != "add": + continue + atom = effect.get("atom", {}) + if ( + not isinstance(atom, Mapping) + or atom.get("object_uid") != object_uid + ): + continue + predicate = str(atom.get("predicate", "")) + if predicate == "object_free": + current = "free" + elif predicate == "object_coordinated_held": + current = "coordinated" + elif predicate == "object_held": + arm = str(atom.get("arm", "")) + if not arm: + raise ValueError( + f"SeedGraph node {node_id!r} adds object_held without " + "an ownership resource." + ) + current = arm + ownership[object_uid] = current + + +def _validate_group_dependency_alignment( + nodes: Sequence[Mapping[str, Any]], + groups: Sequence[Mapping[str, Any]], +) -> None: + group_by_node = { + str(node_id): str(group["id"]) + for group in groups + for node_id in group["node_ids"] + } + group_dependencies = { + str(group["id"]): set(str(parent) for parent in group["depends_on"]) + for group in groups + } + + def group_reaches(child: str, parent: str) -> bool: + pending = list(group_dependencies[child]) + visited = set() + while pending: + current = pending.pop() + if current == parent: + return True + if current not in visited: + visited.add(current) + pending.extend(group_dependencies[current]) + return False + + for node in nodes: + child_group = group_by_node[str(node["id"])] + for dependency in node["depends_on"]: + parent_group = group_by_node[str(dependency)] + if parent_group != child_group and not group_reaches( + child_group, parent_group + ): + raise ValueError( + f"SeedGraph node {node['id']!r} depends on TaskGroup " + f"{parent_group!r}, but TaskGroup {child_group!r} does not." + ) + + +def _validate_resource_conflicts( + nodes: Sequence[Mapping[str, Any]], + groups: Sequence[Mapping[str, Any]], + metadata: Any, +) -> None: + dependencies = { + str(node["id"]): set(str(item) for item in node["depends_on"]) for node in nodes + } + + def reaches(child: str, parent: str) -> bool: + pending = list(dependencies[child]) + visited = set() + while pending: + current = pending.pop() + if current == parent: + return True + if current not in visited: + visited.add(current) + pending.extend(dependencies[current]) + return False + + group_by_node = { + str(node_id): str(group["id"]) + for group in groups + for node_id in group["node_ids"] + } + distinct_arm_pairs = _distinct_arm_pairs(metadata) + for index, first in enumerate(nodes): + for second in nodes[index + 1 :]: + first_id = str(first["id"]) + second_id = str(second["id"]) + if reaches(first_id, second_id) or reaches(second_id, first_id): + continue + if ( + first.get("sync_group") == second.get("sync_group") + and first.get("sync_group") is not None + ): + continue + first_claims = { + str(claim["resource"]): str(claim["access"]) + for claim in first["contract"]["claims"] + } + second_claims = { + str(claim["resource"]): str(claim["access"]) + for claim in second["contract"]["claims"] + } + conflicts = sorted( + resource + for resource in set(first_claims) & set(second_claims) + if "exclusive" in {first_claims[resource], second_claims[resource]} + ) + if ( + frozenset({group_by_node[first_id], group_by_node[second_id]}) + in distinct_arm_pairs + ): + conflicts = [item for item in conflicts if item != "arm:auto"] + if conflicts: + raise ValueError( + f"SeedGraph concurrent nodes {first_id!r} and {second_id!r} " + f"have resource conflicts: {conflicts}." + ) + + +def _distinct_arm_pairs(value: Any) -> set[frozenset[str]]: + if not isinstance(value, Mapping): + return set() + groups = value.get("legacy_allocation_groups", value.get("allocation_groups", ())) + if not isinstance(groups, Sequence) or isinstance(groups, (str, bytes, bytearray)): + return set() + result: set[frozenset[str]] = set() + for group in groups: + if ( + not isinstance(group, Mapping) + or group.get("arm_constraint") != "distinct_arms" + ): + continue + members = group.get("semantic_step_ids", group.get("task_instance_ids", ())) + if not isinstance(members, Sequence) or isinstance( + members, (str, bytes, bytearray) + ): + continue + member_ids = [str(item) for item in members] + for index, first in enumerate(member_ids): + for second in member_ids[index + 1 :]: + result.add(frozenset({first, second})) + return result + + +def _action_contract(value: Any, context: str) -> dict[str, Any]: + contract = _mapping(value, context) + _keys(contract, _ACTION_CONTRACT_KEYS, context) + if set(contract) != _ACTION_CONTRACT_KEYS: + missing = sorted(_ACTION_CONTRACT_KEYS - set(contract)) + raise ValueError(f"{context} is missing required fields: {missing}.") + if contract["version"] != "action_contract_v2": + raise ValueError(f"{context}.version must be 'action_contract_v2'.") + contract["requires"] = [ + _state_atom(item, f"{context}.requires[{index}]") + for index, item in enumerate( + _sequence(contract["requires"], f"{context}.requires") + ) + ] + contract["effects"] = [ + _state_effect(item, f"{context}.effects[{index}]") + for index, item in enumerate( + _sequence(contract["effects"], f"{context}.effects") + ) + ] + contract["claims"] = [ + _resource_claim(item, f"{context}.claims[{index}]") + for index, item in enumerate(_sequence(contract["claims"], f"{context}.claims")) + ] + _unique( + [str(item["resource"]) for item in contract["claims"]], + f"{context}.claims resources", + ) + contract["completion"] = _enum( + contract["completion"], _ACTION_COMPLETION, f"{context}.completion" + ) + contract["failure_policy"] = _enum( + contract["failure_policy"], + _FAILURE_POLICIES, + f"{context}.failure_policy", + ) + return contract + + +def _task_group_contract(value: Any, context: str) -> dict[str, Any]: + contract = _mapping(value, context) + _keys(contract, _TASK_GROUP_CONTRACT_KEYS, context) + if set(contract) != _TASK_GROUP_CONTRACT_KEYS: + missing = sorted(_TASK_GROUP_CONTRACT_KEYS - set(contract)) + raise ValueError(f"{context} is missing required fields: {missing}.") + contract["entry_requires"] = [ + _state_atom(item, f"{context}.entry_requires[{index}]") + for index, item in enumerate( + _sequence(contract["entry_requires"], f"{context}.entry_requires") + ) + ] + contract["exit_effects"] = [ + _state_effect(item, f"{context}.exit_effects[{index}]") + for index, item in enumerate( + _sequence(contract["exit_effects"], f"{context}.exit_effects") + ) + ] + contract["claims"] = [ + _resource_claim(item, f"{context}.claims[{index}]") + for index, item in enumerate(_sequence(contract["claims"], f"{context}.claims")) + ] + _unique( + [str(item["resource"]) for item in contract["claims"]], + f"{context}.claims resources", + ) + contract["entry_node_ids"] = _strings( + contract["entry_node_ids"], f"{context}.entry_node_ids" + ) + contract["terminal_node_ids"] = _strings( + contract["terminal_node_ids"], f"{context}.terminal_node_ids" + ) + if not contract["entry_node_ids"] or not contract["terminal_node_ids"]: + raise ValueError(f"{context} requires entry and terminal node IDs.") + contract["completion"] = _enum( + contract["completion"], _GROUP_COMPLETION, f"{context}.completion" + ) + return contract + + +def _state_atom(value: Any, context: str) -> dict[str, str]: + atom = _mapping(value, context) + _keys(atom, _STATE_ATOM_KEYS, context) + predicate = _enum(atom.get("predicate"), _STATE_PREDICATES, f"{context}.predicate") + required = { + "arm_free": {"arm"}, + "object_free": {"object_uid"}, + "object_held": {"object_uid", "arm"}, + "object_coordinated_held": {"object_uid"}, + "handover_complete": {"object_uid"}, + "arm_clear": {"arm"}, + "arm_home": {"arm"}, + }[predicate] + present = set(atom) - {"predicate"} + if present != required: + raise ValueError( + f"{context} predicate {predicate!r} requires exactly {sorted(required)}." + ) + for field in required: + atom[field] = _string(atom.get(field), f"{context}.{field}") + return atom + + +def _state_effect(value: Any, context: str) -> dict[str, Any]: + effect = _mapping(value, context) + _keys(effect, _EFFECT_KEYS, context) + if set(effect) != _EFFECT_KEYS: + raise ValueError(f"{context} requires op and atom.") + effect["op"] = _enum(effect["op"], _EFFECT_OPERATIONS, f"{context}.op") + effect["atom"] = _state_atom(effect["atom"], f"{context}.atom") + return effect + + +def _resource_claim(value: Any, context: str) -> dict[str, str]: + claim = _mapping(value, context) + _keys(claim, _CLAIM_KEYS, context) + if set(claim) != _CLAIM_KEYS: + raise ValueError(f"{context} requires resource, access, and lifetime.") + claim["resource"] = _string(claim["resource"], f"{context}.resource") + claim["access"] = _enum(claim["access"], _CLAIM_ACCESS, f"{context}.access") + claim["lifetime"] = _enum( + claim["lifetime"], _CLAIM_LIFETIMES, f"{context}.lifetime" + ) + return claim + + +def _validate_group_contract_topology( + nodes: Sequence[Mapping[str, Any]], groups: Sequence[Mapping[str, Any]] +) -> None: + node_by_id = {str(node["id"]): node for node in nodes} + children: dict[str, set[str]] = {node_id: set() for node_id in node_by_id} + for node in nodes: + for dependency in node["depends_on"]: + children[str(dependency)].add(str(node["id"])) + for group in groups: + group_id = str(group["id"]) + node_ids = {str(item) for item in group["node_ids"]} + expected_entries = { + node_id + for node_id in node_ids + if not any( + str(parent) in node_ids for parent in node_by_id[node_id]["depends_on"] + ) + } + expected_terminals = { + node_id for node_id in node_ids if not (children[node_id] & node_ids) + } + contract = group["contract"] + if set(contract["entry_node_ids"]) != expected_entries: + raise ValueError( + f"SeedGraph TaskGroup {group_id!r} contract entry_node_ids do not " + "match its internal topology." + ) + if set(contract["terminal_node_ids"]) != expected_terminals: + raise ValueError( + f"SeedGraph TaskGroup {group_id!r} contract terminal_node_ids do not " + "match its internal topology." + ) + if contract["completion"] == "terminal_barrier" and not all( + node_by_id[node_id]["contract"]["completion"] == "terminal_barrier" + for node_id in expected_terminals + ): + raise ValueError( + f"SeedGraph TaskGroup {group_id!r} terminal barrier must end in " + "terminal_barrier AtomicActions." + ) + + +def _validate_cleanup_barriers( + nodes: Sequence[Mapping[str, Any]], groups: Sequence[Mapping[str, Any]] +) -> None: + node_by_id = {str(node["id"]): node for node in nodes} + for group in groups: + group_id = str(group["id"]) + group_nodes = [node_by_id[str(node_id)] for node_id in group["node_ids"]] + cleanup = [ + node for node in group_nodes if node["contract"]["completion"] == "cleanup" + ] + has_handover = any(node["atomic_action"] == "HandOver" for node in group_nodes) + recovery_successor = any( + candidate.get("role") == "recovery" + and candidate.get("parent_task_instance_id") == group_id + and group_id in candidate.get("depends_on", ()) + for candidate in groups + ) + if has_handover and not cleanup and recovery_successor: + continue + if has_handover and not cleanup: + raise ValueError( + f"SeedGraph HandOver TaskGroup {group_id!r} is missing retreat cleanup." + ) + if not cleanup and not has_handover: + continue + terminal_ids = group["contract"]["terminal_node_ids"] + if group["contract"]["completion"] != "terminal_barrier" or not all( + node_by_id[node_id]["contract"]["completion"] == "terminal_barrier" + for node_id in terminal_ids + ): + raise ValueError( + f"SeedGraph TaskGroup {group_id!r} cleanup must end at a home " + "terminal barrier." + ) + + +def _actor(value: Any, context: str) -> dict[str, Any]: + actor = _mapping(value, context) + mode = _enum(actor.get("mode"), _ACTOR_MODES, f"{context}.mode") + allowed = {"mode"} + if mode in {"required", "preferred"}: + allowed.add("arm") + _string(actor.get("arm"), f"{context}.arm") + elif mode == "coordinated": + allowed.add("arms") + arms = _strings(actor.get("arms"), f"{context}.arms") + if len(arms) < 2: + raise ValueError(f"{context}.arms must contain at least two arms.") + actor["arms"] = arms + _keys(actor, frozenset(allowed), context) + return actor + + +def _known_object_references( + value: Any, known: set[str], path: str = "SeedGraph" +) -> None: + if isinstance(value, Mapping): + for key, child in value.items(): + child_path = f"{path}.{key}" + if key in _OBJECT_REFERENCE_KEYS and isinstance(child, str): + if child not in known and child not in {"table_center", "world"}: + raise ValueError( + f"{child_path} references unknown object {child!r}." + ) + _known_object_references(child, known, child_path) + elif isinstance(value, Sequence) and not isinstance(value, (str, bytes, bytearray)): + for index, child in enumerate(value): + _known_object_references(child, known, f"{path}[{index}]") + + +def _reject_grounded(value: Any, path: str = "document") -> None: + if isinstance(value, Mapping): + for key, child in value.items(): + if str(key).lower() in _GROUNDED_FIELD_NAMES: + raise ValueError(f"{path}.{key} contains grounded motion data.") + _reject_grounded(child, f"{path}.{key}") + elif isinstance(value, Sequence) and not isinstance(value, (str, bytes, bytearray)): + for index, child in enumerate(value): + _reject_grounded(child, f"{path}[{index}]") + + +def _finite(value: Any, path: str = "document") -> None: + if isinstance(value, Mapping): + for key, child in value.items(): + _finite(child, f"{path}.{key}") + elif isinstance(value, Sequence) and not isinstance(value, (str, bytes, bytearray)): + for index, child in enumerate(value): + _finite(child, f"{path}[{index}]") + elif isinstance(value, float) and not math.isfinite(value): + raise ValueError(f"{path} must be finite.") + + +def _dag(dependencies: Mapping[str, Sequence[str]], context: str) -> None: + known = set(dependencies) + outgoing = {item_id: [] for item_id in known} + indegree = {item_id: 0 for item_id in known} + for item_id, required in dependencies.items(): + unknown = set(required) - known + if unknown: + raise ValueError(f"{context} reference unknown IDs: {sorted(unknown)}.") + if item_id in required: + raise ValueError(f"{context} contain a self-dependency at {item_id!r}.") + for parent in required: + outgoing[parent].append(item_id) + indegree[item_id] += 1 + ready = deque( + sorted(item_id for item_id, degree in indegree.items() if degree == 0) + ) + visited = 0 + while ready: + item_id = ready.popleft() + visited += 1 + for child in sorted(outgoing[item_id]): + indegree[child] -= 1 + if indegree[child] == 0: + ready.append(child) + if visited != len(known): + cyclic = sorted(item_id for item_id, degree in indegree.items() if degree) + raise ValueError(f"{context} contain a dependency cycle: {cyclic}.") + + +def _mapping(value: Any, context: str) -> dict[str, Any]: + if not isinstance(value, Mapping): + raise TypeError(f"{context} must be a mapping.") + return deepcopy(dict(value)) + + +def _sequence(value: Any, context: str) -> list[Any]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes, bytearray)): + raise TypeError(f"{context} must be a list.") + return list(value) + + +def _strings(value: Any, context: str) -> list[str]: + result = [ + _string(item, f"{context}[{index}]") + for index, item in enumerate(_sequence(value, context)) + ] + _unique(result, context) + return result + + +def _string(value: Any, context: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{context} must be a non-empty string.") + return value.strip() + + +def _enum(value: Any, allowed: Collection[str], context: str) -> str: + result = _string(value, context) + if result not in allowed: + raise ValueError(f"{context} must be one of {sorted(allowed)}.") + return result + + +def _unique(values: Sequence[str], context: str) -> None: + if len(values) != len(set(values)): + raise ValueError(f"{context} must be unique.") + + +def _keys(value: Mapping[str, Any], allowed: frozenset[str], context: str) -> None: + unknown = sorted(set(value) - allowed) + if unknown: + raise ValueError(f"{context} contains unsupported fields: {unknown}.") + + +def _schema(value: Mapping[str, Any], expected: str, context: str) -> None: + if value.get("schema_version") != expected: + raise ValueError(f"{context}.schema_version must be {expected!r}.") diff --git a/embodichain/gen_sim/action_engine/domain/visual_contracts.py b/embodichain/gen_sim/action_engine/domain/visual_contracts.py new file mode 100644 index 000000000..78ca4a0f1 --- /dev/null +++ b/embodichain/gen_sim/action_engine/domain/visual_contracts.py @@ -0,0 +1,60 @@ +# ---------------------------------------------------------------------------- +# 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 visual-fact contracts shared by planning and evaluation.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from types import MappingProxyType +from typing import Any + +__all__ = [ + "OCCLUSION_RELATION", + "VISUAL_RELATION_PARTICIPANTS", + "requested_visual_task_predicates", +] + + +OCCLUSION_RELATION = "occludes" + +# Participant order is semantic. For ``occludes`` it is +# ``[occluder_uid, occluded_uid]``. +VISUAL_RELATION_PARTICIPANTS: Mapping[str, tuple[str, ...]] = MappingProxyType( + {OCCLUSION_RELATION: ("occluder", "occluded")} +) + + +def requested_visual_task_predicates(task_spec: Mapping[str, Any]) -> frozenset[str]: + """Return task-level visual predicates explicitly requested by a TaskSpec.""" + result: set[str] = set() + + def collect(value: Any) -> None: + if isinstance(value, Mapping): + if value.get("type") == "visual_relation": + relation = value.get("relation") + if isinstance(relation, str) and relation: + result.add(relation) + for child in value.values(): + collect(child) + elif isinstance(value, Sequence) and not isinstance( + value, (str, bytes, bytearray) + ): + for child in value: + collect(child) + + collect(task_spec.get("success", {})) + return frozenset(result) diff --git a/embodichain/gen_sim/action_engine/generation/__init__.py b/embodichain/gen_sim/action_engine/generation/__init__.py new file mode 100644 index 000000000..086d479f7 --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/__init__.py @@ -0,0 +1,40 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Independent config generation for Action Engine.""" + +from __future__ import annotations + +from .config_builder import ( + VLM_CAMERA_UIDS, + canonical_gripper_model, + canonical_ik_solver, + canonical_robot_profile, +) +from .assets import normalize_scene_assets +from .generator import generate_action_engine_config +from .models import GeneratedConfigPaths, PreparedScene + +__all__ = [ + "GeneratedConfigPaths", + "PreparedScene", + "VLM_CAMERA_UIDS", + "canonical_gripper_model", + "canonical_ik_solver", + "canonical_robot_profile", + "generate_action_engine_config", + "normalize_scene_assets", +] diff --git a/embodichain/gen_sim/action_engine/generation/artifacts.py b/embodichain/gen_sim/action_engine/generation/artifacts.py new file mode 100644 index 000000000..0af961051 --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/artifacts.py @@ -0,0 +1,141 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Publish canonical generation artifacts without intermediate copies.""" + +from __future__ import annotations + +from collections.abc import Mapping +import json +import os +from pathlib import Path +import tempfile +from typing import Any + +from embodichain.gen_sim.action_engine.protocol import ( + AGENT_CONFIG_FILENAME, + EXECUTION_PROGRAM_FILENAME, + FAST_GYM_CONFIG_FILENAME, + SCENE_REQUIREMENTS_FILENAME, + SEED_TASK_GRAPH_PNG_FILENAME, + TASK_SPEC_FILENAME, +) + +from .models import GeneratedConfigPaths + +__all__ = ["artifact_paths", "write_generation_artifacts"] + + +def artifact_paths( + output_dir: str | Path, + *, + planning_mode: str = "offline", +) -> GeneratedConfigPaths: + """Return canonical resolved paths for one output directory.""" + directory = Path(output_dir).expanduser().resolve() + _validate_planning_mode(planning_mode) + graph_directory = directory if planning_mode == "offline" else directory / "offline" + return GeneratedConfigPaths( + gym_config=directory / FAST_GYM_CONFIG_FILENAME, + agent_config=directory / AGENT_CONFIG_FILENAME, + task_spec=directory / TASK_SPEC_FILENAME, + scene_requirements=directory / SCENE_REQUIREMENTS_FILENAME, + seed_task_graph=graph_directory / EXECUTION_PROGRAM_FILENAME, + seed_task_graph_png=graph_directory / SEED_TASK_GRAPH_PNG_FILENAME, + planning_mode=planning_mode, + ) + + +def write_generation_artifacts( + output_dir: str | Path, + *, + gym_config: Mapping[str, Any], + agent_config: Mapping[str, Any], + task_spec: Mapping[str, Any], + scene_requirements: Mapping[str, Any], + seed_task_graph: Mapping[str, Any], + seed_task_graph_png: bytes, + overwrite: bool, + planning_mode: str = "offline", +) -> GeneratedConfigPaths: + """Serialize validated artifacts and replace their destinations atomically.""" + paths = artifact_paths(output_dir, planning_mode=planning_mode) + if not isinstance(seed_task_graph_png, (bytes, bytearray)): + raise TypeError("seed_task_graph_png must be bytes.") + payloads = { + paths.gym_config: _serialize_json(gym_config), + paths.agent_config: _serialize_json(agent_config), + paths.task_spec: _serialize_json(task_spec), + paths.scene_requirements: _serialize_json(scene_requirements), + paths.seed_task_graph: _serialize_json(seed_task_graph), + paths.seed_task_graph_png: bytes(seed_task_graph_png), + } + existing = sorted(path for path in payloads if path.exists()) + if existing and not overwrite: + names = ", ".join(path.name for path in existing) + raise FileExistsError( + f"Generated artifacts already exist in {paths.gym_config.parent}: " + f"{names}. Pass --overwrite to replace them." + ) + + paths.gym_config.parent.mkdir(parents=True, exist_ok=True) + temporary: dict[Path, Path] = {} + try: + for destination, payload in payloads.items(): + destination.parent.mkdir(parents=True, exist_ok=True) + temporary[destination] = _write_temporary(destination.parent, payload) + for destination, temporary_path in temporary.items(): + os.replace(temporary_path, destination) + finally: + for temporary_path in temporary.values(): + temporary_path.unlink(missing_ok=True) + return paths + + +def _serialize_json(value: Mapping[str, Any]) -> str: + try: + return ( + json.dumps( + dict(value), + ensure_ascii=False, + indent=2, + sort_keys=False, + allow_nan=False, + ) + + "\n" + ) + except (TypeError, ValueError) as exc: + raise ValueError("Generated artifact is not strict JSON data.") from exc + + +def _write_temporary(directory: Path, payload: str | bytes) -> Path: + data = payload if isinstance(payload, bytes) else payload.encode("utf-8") + with tempfile.NamedTemporaryFile( + mode="wb", + dir=directory, + prefix=".action_engine_", + suffix=".tmp", + delete=False, + ) as stream: + stream.write(data) + stream.flush() + os.fsync(stream.fileno()) + return Path(stream.name) + + +def _validate_planning_mode(value: Any) -> None: + if value not in {"offline", "ab"}: + raise ValueError("planning_mode must be 'offline' or 'ab'.") diff --git a/embodichain/gen_sim/action_engine/generation/assets.py b/embodichain/gen_sim/action_engine/generation/assets.py new file mode 100644 index 000000000..d1b4da830 --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/assets.py @@ -0,0 +1,158 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Normalize GLB node transforms and body scale into reusable runtime assets.""" + +from __future__ import annotations + +from copy import deepcopy +from dataclasses import replace +import hashlib +import json +from pathlib import Path +from typing import Any + +import numpy as np + +from .models import PreparedScene + +__all__ = ["normalize_scene_assets"] + +_POLICY = "action_engine_glb_geometry_v2" + + +def normalize_scene_assets( + scene: PreparedScene, + output_dir: str | Path, +) -> PreparedScene: + """Return a scene whose valid GLB meshes have flattened runtime geometry. + + Source files are never modified. Cache names derive from source bytes, + object scale, and the normalization policy, so repeated generation reuses + identical assets. + """ + sections = { + "background": [deepcopy(value) for value in scene.background], + "rigid_object": [deepcopy(value) for value in scene.rigid_objects], + "articulation": [deepcopy(value) for value in scene.articulations], + } + cache_dir = Path(output_dir).expanduser().resolve() / "mesh_assets" / "normalized" + reports: list[dict[str, Any]] = [] + hashes = dict(scene.asset_hashes) + normalized_by_uid: dict[str, dict[str, Any]] = {} + for section in ("background", "rigid_object"): + for config in sections[section]: + report = _normalize_object(config, cache_dir) + if report is not None: + reports.append(report) + hashes[str(config["uid"])] = str(report["runtime_sha256"]) + normalized_by_uid[str(config["uid"])] = config + + planner = [deepcopy(value) for value in scene.planner_objects] + for item in planner: + runtime = normalized_by_uid.get(str(item["runtime_uid"])) + if runtime is None: + continue + item["shape"] = deepcopy(runtime.get("shape", {})) + item["body_scale"] = list(runtime.get("body_scale", [1.0, 1.0, 1.0])) + return replace( + scene, + planner_objects=tuple(planner), + background=tuple(sections["background"]), + rigid_objects=tuple(sections["rigid_object"]), + articulations=tuple(sections["articulation"]), + asset_hashes=hashes, + asset_provenance=tuple(reports), + ) + + +def _normalize_object( + config: dict[str, Any], + cache_dir: Path, +) -> dict[str, Any] | None: + shape = config.get("shape") + if not isinstance(shape, dict) or not shape.get("fpath"): + return None + source = Path(str(shape["fpath"])).expanduser().resolve() + if source.suffix.lower() not in {".glb", ".gltf"}: + return None + source_hash = _file_hash(source) + scale = [float(value) for value in config.get("body_scale", [1.0, 1.0, 1.0])] + key = hashlib.sha256( + json.dumps( + {"source": source_hash, "scale": scale, "policy": _POLICY}, + sort_keys=True, + separators=(",", ":"), + ).encode("utf-8") + ).hexdigest() + destination = cache_dir / f"{source.stem[:32]}_{key[:16]}.glb" + status = "reused" if destination.is_file() else "generated" + if status == "generated": + try: + _bake_glb(source, destination, scale) + except Exception as exc: + return { + "uid": str(config.get("uid", "")), + "source_path": source.as_posix(), + "source_sha256": source_hash, + "runtime_path": source.as_posix(), + "runtime_sha256": source_hash, + "body_scale": scale, + "status": "preserved_invalid_source", + "error": f"{type(exc).__name__}: {exc}", + "policy_version": _POLICY, + } + shape["fpath"] = destination.as_posix() + config["body_scale"] = [1.0, 1.0, 1.0] + return { + "uid": str(config.get("uid", "")), + "source_path": source.as_posix(), + "source_sha256": source_hash, + "runtime_path": destination.as_posix(), + "runtime_sha256": _file_hash(destination), + "body_scale": scale, + "status": status, + "policy_version": _POLICY, + } + + +def _bake_glb(source: Path, destination: Path, sim_scale: list[float]) -> None: + import trimesh + + source_scene = trimesh.load(source.as_posix(), force="scene") + baked = trimesh.Scene() + scale = np.diag([sim_scale[0], sim_scale[2], sim_scale[1], 1.0]) + for node_name in source_scene.graph.nodes_geometry: + node_transform, geometry_name = source_scene.graph.get(node_name) + mesh = source_scene.geometry[geometry_name].copy() + mesh.apply_transform(scale @ node_transform) + baked.add_geometry( + mesh, + node_name=str(node_name), + geom_name=f"geometry_{len(baked.geometry)}", + ) + if not baked.geometry: + raise ValueError(f"GLB contains no mesh geometry: {source}") + destination.parent.mkdir(parents=True, exist_ok=True) + baked.export(destination.as_posix(), file_type="glb") + + +def _file_hash(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() diff --git a/embodichain/gen_sim/action_engine/generation/config_builder.py b/embodichain/gen_sim/action_engine/generation/config_builder.py new file mode 100644 index 000000000..b056f1c5f --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/config_builder.py @@ -0,0 +1,1082 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Build the simulator and Action Engine artifact manifests.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +from functools import lru_cache +import json +import math +from pathlib import Path +from typing import Any + +from embodichain.gen_sim.action_engine.config import ( + ACTION_ENGINE_DEFAULTS_SCHEMA, + RuntimePolicyCfg, + default_runtime_policy, + generation_defaults, + runtime_policy_hash, +) +from embodichain.gen_sim.action_engine.config.runtime_policy import ( + _resolve_planner_policy, +) +from embodichain.gen_sim.action_engine.gripper_profiles import ( + GripperProfile, + get_gripper_profile, +) +from embodichain.gen_sim.action_engine.solver_profiles import ( + resolve_ik_solver_mode, + validate_robot_ik_solver_contract, +) +from embodichain.gen_sim.action_engine.protocol import ( + ACTION_ENGINE_CONFIG_SCHEMA, + ACTION_ENGINE_ENV_ID, + EXECUTION_PROGRAM_FILENAME, + SCENE_REQUIREMENTS_FILENAME, + TASK_SPEC_FILENAME, +) + +from .models import PreparedScene + +__all__ = [ + "build_agent_config", + "build_fast_gym_config", + "canonical_gripper_model", + "canonical_ik_solver", + "canonical_robot_profile", + "VLM_CAMERA_UIDS", + "validate_fast_gym_config", +] + +_TEMPLATE_DIR = Path(__file__).resolve().parent / "templates" +_GENERATION_DEFAULTS = generation_defaults() +_DEFAULT_TABLETOP_Z = float(_GENERATION_DEFAULTS["scene"]["default_tabletop_z"]) +_DEFAULT_GRIPPER_MODEL = str(_GENERATION_DEFAULTS["task"]["default_gripper_model"]) +_DEFAULT_IK_SOLVER = str(_GENERATION_DEFAULTS["task"]["default_ik_solver"]) +_USD_ARTICULATION_SUFFIXES = frozenset({".usd", ".usda", ".usdc"}) +_ARTICULATION_AUTHORING_KEYS = frozenset( + { + "attributes", + "category", + "description", + "is_articulated", + "name", + "proxy_body_scale", + "proxy_glb_fpath", + "role", + } +) + +_ARM_SLOTS = { + "left": {"arm": "left_arm", "eef": "left_eef"}, + "right": {"arm": "right_arm", "eef": "right_eef"}, +} + +# These IDs are part of the A/B runtime contract. Keep the order stable so +# visual-fact payloads and comparison reports are reproducible across runs. +VLM_CAMERA_UIDS = ( + "vlm_front", + "vlm_left", + "vlm_rear", + "vlm_right", +) + + +def canonical_robot_profile(profile: str) -> str: + """Normalize the supported CLI aliases to one runtime profile ID.""" + normalized = str(profile).strip().lower().replace("-", "_") + profiles = _robot_profiles() + if normalized in profiles: + return normalized + for profile_id, value in profiles.items(): + if normalized in value["aliases"]: + return profile_id + raise ValueError( + f"Unsupported robot profile {profile!r}; expected one of: " + f"{', '.join(sorted(profiles))}" + ) + + +def canonical_gripper_model(model: str) -> str: + """Validate and return one exact GenSim gripper model ID.""" + return get_gripper_profile(model).model.value + + +def canonical_ik_solver(mode: str, robot_profile: str) -> str: + """Resolve one generation-time IK solver mode for a robot profile.""" + return resolve_ik_solver_mode(mode, canonical_robot_profile(robot_profile)) + + +def build_agent_config( + *, + task_name: str, + robot_profile: str, + execution_program_hash: str, + source_config_path: Path, + uid_map: dict[str, str], + gripper_model: str = _DEFAULT_GRIPPER_MODEL, + ik_solver: str = _DEFAULT_IK_SOLVER, + static_obstacle_uids: Sequence[str] | None = None, + dynamic_obstacle_uids: Sequence[str] | None = None, + table_top_z: float | None = None, + articulation_settings: Mapping[str, Mapping[str, Sequence[float]]] | None = None, + planning_mode: str = "offline", + seed_task_graph_path: str | Path | None = EXECUTION_PROGRAM_FILENAME, + vlm_model: str | None = None, + vlm_camera_uids: Sequence[str] | None = None, + planner_policy: Mapping[str, Any] | None = None, +) -> dict[str, Any]: + """Build the small manifest consumed by ``run_agent``.""" + profile = canonical_robot_profile(robot_profile) + selected_gripper = canonical_gripper_model(gripper_model) + selected_ik_solver = resolve_ik_solver_mode(ik_solver, profile) + runtime_policy = default_runtime_policy(profile) + explicit_dynamic_collision = ( + planner_policy is not None and "dynamic_collision" in planner_policy + ) + if planner_policy is not None: + policy = runtime_policy.as_mapping() + policy["planner"] = _resolve_planner_policy( + planner_policy, + robot_profile=profile, + ) + runtime_policy = RuntimePolicyCfg.from_mapping(policy) + if ( + static_obstacle_uids is not None + or dynamic_obstacle_uids is not None + or table_top_z is not None + ): + policy = runtime_policy.as_mapping() + planner = policy["planner"] + if static_obstacle_uids is not None: + planner["static_obstacle_uids"] = [str(uid) for uid in static_obstacle_uids] + if dynamic_obstacle_uids is not None: + planner["dynamic_obstacle_uids"] = [ + str(uid) for uid in dynamic_obstacle_uids + ] + if not explicit_dynamic_collision: + planner["dynamic_collision"] = bool(dynamic_obstacle_uids) and ( + planner["backend"] == "curobo" + ) + if table_top_z is not None: + tabletop = float(table_top_z) + if not math.isfinite(tabletop): + raise ValueError("table_top_z must be finite when provided.") + height_offset = tabletop - _DEFAULT_TABLETOP_Z + height_policies = ( + policy["grounding"]["semantic_defaults"], + policy["grounding"]["handover"], + policy["motion_defaults"]["MoveEndEffector"], + policy["motion_modifiers"]["orientation"]["upright"]["MoveEndEffector"], + ) + for height_policy in height_policies: + height_policy["maximum_eef_height"] = round( + float(height_policy["maximum_eef_height"]) + height_offset, + 6, + ) + runtime_policy = RuntimePolicyCfg.from_mapping(policy) + _validate_planning_mode(planning_mode) + graph_path = _validate_seed_graph_path(seed_task_graph_path) + if planning_mode == "ab" and graph_path == EXECUTION_PROGRAM_FILENAME: + graph_path = f"offline/{EXECUTION_PROGRAM_FILENAME}" + result = { + "schema_version": ACTION_ENGINE_CONFIG_SCHEMA, + "task_name": task_name, + "robot_profile": profile, + "gripper_model": selected_gripper, + "ik_solver": selected_ik_solver, + "planning_mode": planning_mode, + "task_spec": TASK_SPEC_FILENAME, + "scene_requirements": SCENE_REQUIREMENTS_FILENAME, + "seed_task_graph": graph_path, + "seed_task_graph_hash": execution_program_hash, + "runtime_policy": runtime_policy.as_mapping(), + "runtime_policy_hash": runtime_policy_hash(runtime_policy), + "source": { + "gym_config": source_config_path.as_posix(), + "uid_map": dict(sorted(uid_map.items())), + }, + "articulation_settings": _normalize_articulation_settings( + articulation_settings or {} + ), + } + if planning_mode == "ab": + camera_uids = _normalize_vlm_camera_uids(vlm_camera_uids) + configured_model = _optional_model(vlm_model) + # Retain concise top-level aliases for early A/B bundles while keeping + # the nested section as the canonical runtime namespace. + result["offline_seed_task_graph"] = graph_path + result["vlm_model"] = configured_model + result["vlm_camera_uids"] = list(camera_uids) + result["online_planning"] = { + # Model names are deliberately persisted only when explicitly + # supplied by the generator. Runtime resolution can then apply + # the documented ACTION_ENGINE_VLM_MODEL/OPENAI_MODEL fallback. + "vlm_model": configured_model, + "camera_uids": camera_uids, + } + return result + + +def _normalize_articulation_settings( + value: Mapping[str, Mapping[str, Sequence[float]]], +) -> dict[str, dict[str, list[float]]]: + """Own finite per-joint ordinal setting calibrations for runtime grounding.""" + result: dict[str, dict[str, list[float]]] = {} + for uid, joints in value.items(): + if not isinstance(uid, str) or not uid or not isinstance(joints, Mapping): + raise ValueError("articulation_settings must map UIDs to joint mappings.") + normalized_joints = {} + for joint_name, settings in joints.items(): + if ( + not isinstance(joint_name, str) + or not joint_name + or not isinstance(settings, Sequence) + or isinstance(settings, (str, bytes, bytearray)) + or not settings + ): + raise ValueError( + "articulation_settings joints require non-empty setting lists." + ) + normalized = [float(item) for item in settings] + if any(not math.isfinite(item) for item in normalized): + raise ValueError("articulation setting values must be finite.") + normalized_joints[joint_name] = normalized + result[uid] = normalized_joints + return dict(sorted(result.items())) + + +def build_fast_gym_config( + scene: PreparedScene, + *, + task_name: str, + task_description: str, + robot_profile: str, + execution_program_hash: str, + max_episodes: int, + max_episode_steps: int, + gripper_model: str = _DEFAULT_GRIPPER_MODEL, + ik_solver: str = _DEFAULT_IK_SOLVER, + randomize_scene: bool = False, + randomize_table_material: bool = False, + planning_mode: str = "offline", + seed_task_graph_path: str | Path | None = EXECUTION_PROGRAM_FILENAME, +) -> dict[str, Any]: + """Build a runnable EmbodiChain gym config from a prepared source scene.""" + if max_episodes < 1: + raise ValueError("max_episodes must be at least 1.") + if max_episode_steps < 1: + raise ValueError("max_episode_steps must be at least 1.") + _validate_planning_mode(planning_mode) + graph_path = _validate_seed_graph_path(seed_task_graph_path) + if planning_mode == "ab" and graph_path == EXECUTION_PROGRAM_FILENAME: + graph_path = f"offline/{EXECUTION_PROGRAM_FILENAME}" + profile = canonical_robot_profile(robot_profile) + gripper_profile = get_gripper_profile(gripper_model) + selected_ik_solver = resolve_ik_solver_mode(ik_solver, profile) + + profile_config = _profile(profile) + robot = _make_robot( + profile, + profile_config, + scene.table_top_z, + gripper_profile=gripper_profile, + ik_solver=selected_ik_solver, + ) + observations = _make_observations(robot, gripper_profile) + # These two template fields describe serialization order to generation, not + # RobotCfg. Remove them after deriving observation IDs to avoid parser noise. + robot.pop("observation_joint_parts", None) + robot.pop("qpos_control_part_order", None) + sensors = _load_template("default_sensors.json") + if not isinstance(sensors, list) or not sensors: + raise ValueError("Default sensor template must define at least one camera.") + environment_policy = _GENERATION_DEFAULTS["environment"] + viewer_camera_uid = str(environment_policy["viewer_camera_uid"]) + sensors[0]["uid"] = viewer_camera_uid + if planning_mode == "ab": + vlm_sensors = _load_template("vlm_sensors.json") + if not isinstance(vlm_sensors, list) or len(vlm_sensors) != len( + VLM_CAMERA_UIDS + ): + raise ValueError("A/B planning requires exactly four VLM cameras.") + _validate_vlm_sensors(vlm_sensors) + _anchor_vlm_sensors(vlm_sensors, scene) + sensors.extend(vlm_sensors) + light = _load_template("default_lights.json") + + rigid_uids = [str(config["uid"]) for config in scene.rigid_objects] + background_uids = [str(config["uid"]) for config in scene.background] + engine_extension = { + "schema_version": "action_engine_runtime_v2", + "defaults_schema_version": ACTION_ENGINE_DEFAULTS_SCHEMA, + "task_name": task_name, + "robot_profile": profile, + "gripper_model": gripper_profile.model.value, + "ik_solver": selected_ik_solver, + "planning_mode": planning_mode, + "task_spec": TASK_SPEC_FILENAME, + "scene_requirements": SCENE_REQUIREMENTS_FILENAME, + "seed_task_graph": graph_path, + "seed_task_graph_hash": execution_program_hash, + "source_gym_config": scene.source_config_path.as_posix(), + "source_scene_z_rotation_degrees": scene.z_rotation_degrees, + "source_scene_xy_translation": list(scene.source_scene_xy_translation), + "body_scale_policy": scene.body_scale_policy, + "body_scale": list(scene.body_scale), + "asset_hashes": dict(sorted(scene.asset_hashes.items())), + "asset_provenance": [deepcopy(value) for value in scene.asset_provenance], + "uid_map": dict(sorted(scene.uid_map.items())), + } + extensions = { + "action_engine": engine_extension, + "agent_robot_profile": profile, + "agent_gripper_model": gripper_profile.model.value, + "agent_ik_solver": selected_ik_solver, + "agent_arm_slots": deepcopy(_ARM_SLOTS), + "agent_static_obstacle_uids": background_uids, + "agent_dynamic_obstacle_uids": rigid_uids, + "gripper_open_state": list(gripper_profile.open_positions), + "gripper_close_state": list(gripper_profile.close_positions), + "gripper_profile": gripper_profile.runtime_manifest( + tcp_parent_frames={ + "left": str(robot["solver_cfg"]["left_arm"]["end_link_name"]), + "right": str(robot["solver_cfg"]["right_arm"]["end_link_name"]), + } + ), + "arm_aim_yaw_offset": deepcopy(environment_policy["arm_aim_yaw_offset"]), + "ignore_terminations_during_agent": bool( + environment_policy["ignore_terminations_during_agent"] + ), + "viewer_camera_uid": viewer_camera_uid, + } + + config: dict[str, Any] = { + "id": ACTION_ENGINE_ENV_ID, + "max_episodes": int(max_episodes), + "max_episode_steps": int(max_episode_steps), + "env": { + "extensions": extensions, + "events": _make_events( + sensors[0], + rigid_uids, + planning_mode=planning_mode, + randomize_scene=randomize_scene, + randomize_table_material=randomize_table_material, + ), + "observations": observations, + "dataset": _make_dataset( + task_name=task_name, + task_description=task_description, + source_config_path=scene.source_config_path, + robot_type=str(robot["uid"]), + ), + }, + "robot": robot, + "sensor": sensors, + "light": light, + "background": [deepcopy(obj_config) for obj_config in scene.background], + "rigid_object": [deepcopy(obj_config) for obj_config in scene.rigid_objects], + } + if scene.articulations: + config["articulation"] = [ + _runtime_articulation_config(articulation) + for articulation in scene.articulations + ] + validate_fast_gym_config(config) + return config + + +def validate_fast_gym_config(config: dict[str, Any]) -> None: + """Check the cross-file and simulator-facing invariants generation owns.""" + if config.get("id") != ACTION_ENGINE_ENV_ID: + raise ValueError(f"Gym config id must be {ACTION_ENGINE_ENV_ID!r}.") + if not isinstance(config.get("robot"), dict) or not config["robot"].get("uid"): + raise ValueError("Gym config requires a concrete robot template.") + if not config.get("sensor"): + raise ValueError("Gym config requires at least one sensor.") + if not all(isinstance(sensor, dict) for sensor in config["sensor"]): + raise ValueError("Generated sensors must be object mappings.") + sensor_uids = [str(sensor.get("uid", "")) for sensor in config["sensor"]] + if not all(sensor_uids) or len(sensor_uids) != len(set(sensor_uids)): + raise ValueError("Generated sensor UIDs must be non-empty and unique.") + if not config.get("background"): + raise ValueError("Gym config requires at least one background object.") + + objects = [ + *config.get("background", []), + *config.get("rigid_object", []), + *config.get("articulation", []), + ] + uids = [str(obj.get("uid", "")) for obj in objects] + if not all(uids) or len(uids) != len(set(uids)): + raise ValueError("Generated scene object UIDs must be non-empty and unique.") + if "table" not in uids: + raise ValueError("Generated tabletop scene must expose runtime UID 'table'.") + + for obj in objects: + shape = obj.get("shape") + fpath = shape.get("fpath") if isinstance(shape, dict) else obj.get("fpath") + if fpath is None: + continue + path = Path(str(fpath)) + if not path.is_absolute() or not path.is_file(): + raise ValueError( + f"Generated asset path for {obj.get('uid')!r} is not an " + f"existing absolute file: {path}" + ) + + for articulation in config.get("articulation", []): + path = Path(str(articulation.get("fpath", ""))) + if ( + path.suffix.lower() in _USD_ARTICULATION_SUFFIXES + and articulation.get("build_pk_chain") is not False + ): + raise ValueError( + f"USD articulation {articulation.get('uid')!r} must set " + "build_pk_chain=false." + ) + + action_engine = config.get("env", {}).get("extensions", {}).get("action_engine", {}) + if action_engine.get("defaults_schema_version") != ACTION_ENGINE_DEFAULTS_SCHEMA: + raise ValueError("Gym config has an unexpected defaults schema version.") + if action_engine.get("task_spec") != TASK_SPEC_FILENAME: + raise ValueError("Gym config points to an unexpected TaskSpec artifact.") + if action_engine.get("scene_requirements") != SCENE_REQUIREMENTS_FILENAME: + raise ValueError("Gym config points to unexpected SceneRequirements.") + gripper_profile = get_gripper_profile(action_engine.get("gripper_model")) + extensions = config["env"]["extensions"] + if extensions.get("agent_gripper_model") != gripper_profile.model.value: + raise ValueError("Gym config gripper model fields do not match.") + _validate_robot_gripper_contract(config["robot"], gripper_profile) + ik_solver = action_engine.get("ik_solver") + if extensions.get("agent_ik_solver") != ik_solver: + raise ValueError("Gym config IK solver fields do not match.") + validate_robot_ik_solver_contract(config["robot"], str(ik_solver)) + graph_path = action_engine.get("seed_task_graph") + if ( + not isinstance(graph_path, str) + or Path(graph_path).name != EXECUTION_PROGRAM_FILENAME + ): + raise ValueError("Gym config points to an unexpected SeedGraph artifact.") + planning_mode = action_engine.get("planning_mode", "offline") + _validate_planning_mode(planning_mode) + if planning_mode == "ab": + sensors = config["sensor"] + vlm_sensors = [ + sensor + for sensor in sensors + if isinstance(sensor, dict) + and str(sensor.get("uid", "")).startswith("vlm_") + ] + _validate_vlm_sensors(vlm_sensors) + + registered = { + entry.get("entity_cfg", {}).get("uid") + for entry in ( + config.get("env", {}) + .get("events", {}) + .get("register_info_to_env", {}) + .get("params", {}) + .get("registry", []) + ) + } + rigid_uids = {obj["uid"] for obj in config.get("rigid_object", [])} + if registered != rigid_uids: + raise ValueError("Every rigid object must have one live-pose registry entry.") + + +def _runtime_articulation_config(value: Mapping[str, Any]) -> dict[str, Any]: + """Reduce one source articulation to the simulator-facing config contract.""" + result = { + key: deepcopy(item) + for key, item in value.items() + if key not in _ARTICULATION_AUTHORING_KEYS + } + path = Path(str(result.get("fpath", ""))) + if path.suffix.lower() in _USD_ARTICULATION_SUFFIXES: + result["build_pk_chain"] = False + return result + + +def _make_robot( + profile_id: str, + profile: dict[str, Any], + table_top_z: float | None, + *, + gripper_profile: GripperProfile, + ik_solver: str, +) -> dict[str, Any]: + robot = _load_template(str(profile["template"])) + tabletop_z = _DEFAULT_TABLETOP_Z if table_top_z is None else float(table_top_z) + robot["init_pos"][2] = round( + tabletop_z + + float(profile["tabletop_clearance"]) + - float(profile["arm_component_z"]), + 6, + ) + family = str(profile["robot_family"]) + if family.startswith("ur"): + display = family.upper() + urdf_dir = display + robot["uid"] = f"Dual{display}" + robot["urdf_cfg"][ + "fname" + ] = f"dual_{family}_{gripper_profile.assembly_name}_basket" + for component in robot["urdf_cfg"]["components"]: + if str(component.get("component_type", "")).endswith("_arm"): + component["urdf_path"] = f"UniversalRobots/{urdf_dir}/{urdf_dir}.urdf" + component["transform"][0][3] = float(profile["arm_base_x"]) + component["transform"][2][3] = float(profile["arm_component_z"]) + for arm in ("left_arm", "right_arm"): + robot["solver_cfg"][arm]["ur_type"] = family + robot["drive_pros"]["max_effort"][arm] = float(profile["max_effort"]) + robot["qpos_control_part_order"] = [ + "left_arm", + "right_arm", + "left_eef", + "right_eef", + ] + robot["observation_joint_parts"] = ["left_eef", "right_eef"] + else: + robot["urdf_cfg"][ + "fname" + ] = f"dual_{family}_{gripper_profile.assembly_name}_basket" + _apply_gripper_profile(robot, gripper_profile) + _apply_ik_solver(robot, ik_solver) + if profile_id != canonical_robot_profile(profile_id): + raise ValueError(f"Invalid canonical robot profile {profile_id!r}.") + return robot + + +def _apply_ik_solver(robot: dict[str, Any], mode: str) -> None: + """Materialize one concrete solver mode while preserving frames and TCPs.""" + solvers = robot.get("solver_cfg") + if not isinstance(solvers, dict): + raise ValueError("Robot template requires solver_cfg.") + if mode == "pytorch": + for arm in ("left_arm", "right_arm"): + current = solvers.get(arm) + if not isinstance(current, dict): + raise ValueError(f"Robot template requires solver_cfg.{arm}.") + if current.get("class_type") == "PytorchSolver": + continue + solvers[arm] = { + "class_type": "PytorchSolver", + "urdf_path": current.get("urdf_path"), + "end_link_name": current["end_link_name"], + "root_link_name": current["root_link_name"], + "tcp": deepcopy(current["tcp"]), + "num_samples": 30, + } + validate_robot_ik_solver_contract(robot, mode) + + +def _apply_gripper_profile( + robot: dict[str, Any], + profile: GripperProfile, +) -> None: + """Apply one profile atomically to simulator, controller, and solver config.""" + control_parts = robot.get("control_parts") + init_qpos = robot.get("init_qpos") + if not isinstance(control_parts, dict) or not isinstance(init_qpos, list): + raise ValueError("Robot template requires control_parts and init_qpos.") + arm_dof = sum( + len(control_parts.get(f"{side}_arm", ())) for side in ("left", "right") + ) + if arm_dof <= 0 or len(init_qpos) < arm_dof: + raise ValueError("Robot template has an invalid initial arm posture.") + arm_init_qpos = list(init_qpos[:arm_dof]) + + components = robot.get("urdf_cfg", {}).get("components") + if not isinstance(components, list): + raise ValueError("Robot template requires a URDF component list.") + hands = { + str(component.get("component_type")): component + for component in components + if str(component.get("component_type", "")).endswith("_hand") + } + if set(hands) != {"left_hand", "right_hand"}: + raise ValueError("Robot template requires exactly one left and right hand.") + for component in hands.values(): + component["urdf_path"] = profile.asset_path + + for side in ("left", "right"): + control_parts[f"{side}_eef"] = list(profile.control_joint_names(side)) + robot["init_qpos"] = ( + arm_init_qpos + list(profile.simulated_joint_initial_positions) * 2 + ) + + drive = robot.get("drive_pros") + if not isinstance(drive, dict): + raise ValueError("Robot template requires drive_pros.") + for section, value in ( + ("stiffness", profile.drive_stiffness), + ("damping", profile.drive_damping), + ("max_effort", profile.drive_max_effort), + ): + values = drive.get(section) + if not isinstance(values, dict): + raise ValueError(f"Robot drive_pros.{section} must be a mapping.") + for side in ("left", "right"): + values[f"{side}_eef"] = value + + solvers = robot.get("solver_cfg") + if not isinstance(solvers, dict): + raise ValueError("Robot template requires solver_cfg.") + tcp = [list(row) for row in profile.tcp_transform] + for arm in ("left_arm", "right_arm"): + if not isinstance(solvers.get(arm), dict): + raise ValueError(f"Robot template requires solver_cfg.{arm}.") + solvers[arm]["tcp"] = deepcopy(tcp) + + +def _validate_robot_gripper_contract( + robot: Mapping[str, Any], + profile: GripperProfile, +) -> None: + """Reject generated artifacts whose physical and planning profiles drift.""" + components = robot.get("urdf_cfg", {}).get("components", []) + hand_assets = { + str(component.get("urdf_path")) + for component in components + if isinstance(component, Mapping) + and str(component.get("component_type", "")).endswith("_hand") + } + if hand_assets != {profile.asset_path}: + raise ValueError("Robot hand assets do not match the selected gripper profile.") + control_parts = robot.get("control_parts", {}) + for side in ("left", "right"): + if control_parts.get(f"{side}_eef") != list(profile.control_joint_names(side)): + raise ValueError( + f"Robot {side} gripper controls do not match the selected profile." + ) + expected_tcp = [list(row) for row in profile.tcp_transform] + for arm in ("left_arm", "right_arm"): + if robot.get("solver_cfg", {}).get(arm, {}).get("tcp") != expected_tcp: + raise ValueError( + f"Robot {arm} TCP does not match the selected gripper profile." + ) + + +@lru_cache(maxsize=1) +def _robot_profiles() -> dict[str, dict[str, Any]]: + value = _read_template("robot_profiles.json") + if not isinstance(value, dict) or not value: + raise ValueError("robot_profiles.json must contain a non-empty object.") + return value + + +def _profile(profile_id: str) -> dict[str, Any]: + profile = deepcopy(_robot_profiles()[profile_id]) + required = { + "aliases", + "template", + "robot_family", + "tabletop_clearance", + "arm_component_z", + } + missing = sorted(required - set(profile)) + if missing: + raise ValueError(f"Robot profile {profile_id!r} is missing fields: {missing}.") + return profile + + +def _make_events( + camera: dict[str, Any], + rigid_uids: list[str], + *, + planning_mode: str, + randomize_scene: bool = False, + randomize_table_material: bool = False, +) -> dict[str, Any]: + extrinsics = camera["extrinsics"] + eye = list(extrinsics["eye"]) + target = list(extrinsics["target"]) + # The recording view mirrors the interactive viewer around its target. + audience_eye = [ + 2.0 * float(target[0]) - float(eye[0]), + 2.0 * float(target[1]) - float(eye[1]), + float(eye[2]), + ] + recording_enabled, recording_resolution, recording_interval = _recording_policy( + planning_mode + ) + source_width = int(camera["width"]) + source_height = int(camera["height"]) + if source_width <= 0 or source_height <= 0: + raise ValueError("Recording source camera resolution must be positive.") + intrinsics = camera.get("intrinsics") + if ( + not isinstance(intrinsics, Sequence) + or isinstance(intrinsics, (str, bytes, bytearray)) + or len(intrinsics) != 4 + ): + raise ValueError("Recording source camera intrinsics must be a 4-vector.") + scale_x = recording_resolution[0] / source_width + scale_y = recording_resolution[1] / source_height + recording_intrinsics = [ + float(intrinsics[0]) * scale_x, + float(intrinsics[1]) * scale_y, + float(intrinsics[2]) * scale_x, + float(intrinsics[3]) * scale_y, + ] + events = { + "record_camera": { + "func": "record_camera_data", + "mode": "interval", + "interval_step": recording_interval, + "params": { + "name": "record_cam_audience_view", + "resolution": list(recording_resolution), + "intrinsics": recording_intrinsics, + "eye": audience_eye, + "target": target, + "up": [ + -float(extrinsics["up"][0]), + -float(extrinsics["up"][1]), + float(extrinsics["up"][2]), + ], + }, + }, + "validation_cameras": { + "func": "validation_cameras", + "mode": "trigger", + "params": {}, + }, + "prepare_extra_attr": { + "func": "prepare_extra_attr", + "mode": "reset", + "params": { + "attrs": [ + { + "name": "object_lengths", + "mode": "callable", + "entity_uids": list(rigid_uids), + "func_name": "compute_object_length", + "func_kwargs": { + "is_svd_frame": True, + "sample_points": int( + _GENERATION_DEFAULTS["scene"][ + "object_length_sample_points" + ] + ), + }, + } + ] + }, + }, + "register_info_to_env": { + "func": "register_info_to_env", + "mode": "reset", + "params": { + "registry": [ + { + "entity_cfg": {"uid": uid}, + "pose_register_params": { + "compute_relative": False, + "compute_pose_object_to_arena": True, + "to_matrix": True, + }, + } + for uid in sorted(rigid_uids) + ], + "registration": "affordance_datas", + "sim_update": True, + }, + }, + } + if not recording_enabled: + events.pop("record_camera") + if randomize_table_material: + material = _GENERATION_DEFAULTS["randomization"]["table_material"] + events["randomize_table_material"] = { + "func": "randomize_visual_material", + "mode": "reset", + "params": { + "entity_cfg": {"uid": "table"}, + "random_texture_prob": float(material["random_texture_prob"]), + "base_color_range": deepcopy(material["base_color_range"]), + "metallic_range": list(material["metallic_range"]), + "roughness_range": list(material["roughness_range"]), + }, + } + if randomize_scene: + randomization = _GENERATION_DEFAULTS["randomization"] + for uid in sorted(rigid_uids): + events[f"randomize_{uid}_pose"] = { + "func": "randomize_rigid_object_pose", + "mode": "reset", + "params": { + "entity_cfg": {"uid": uid}, + "position_range": deepcopy( + randomization["rigid_object_position_range"] + ), + "rotation_range": deepcopy( + randomization["rigid_object_rotation_range"] + ), + "relative_position": True, + "relative_rotation": True, + }, + } + events["randomize_table_height"] = { + "func": "randomize_anchor_height", + "mode": "reset", + "params": { + "anchor_uid": "table", + "height_delta_range": deepcopy( + randomization["table_height_delta_range"] + ), + }, + } + return events + + +def _recording_policy(planning_mode: str) -> tuple[bool, tuple[int, int], int]: + """Resolve the bounded GenSim audience-recording policy.""" + value = _GENERATION_DEFAULTS["environment"].get("recording") + required = {"enabled", "resolution", "interval_step"} + if not isinstance(value, dict) or set(value) != required: + raise ValueError( + "generation.environment.recording must define enabled, resolution, " + "and interval_step." + ) + enabled = value["enabled"] + if not isinstance(enabled, bool): + raise ValueError("generation.environment.recording.enabled must be a boolean.") + resolution = value["resolution"] + if ( + not isinstance(resolution, Sequence) + or isinstance(resolution, (str, bytes, bytearray)) + or len(resolution) != 2 + or any( + isinstance(item, bool) or not isinstance(item, int) for item in resolution + ) + or any(int(item) <= 0 for item in resolution) + ): + raise ValueError( + "generation.environment.recording.resolution must contain two " + "positive integers." + ) + interval_step = value["interval_step"] + if ( + isinstance(interval_step, bool) + or not isinstance(interval_step, int) + or interval_step <= 0 + ): + raise ValueError( + "generation.environment.recording.interval_step must be positive." + ) + return ( + bool(enabled or planning_mode == "ab"), + (int(resolution[0]), int(resolution[1])), + int(interval_step), + ) + + +def _make_observations( + robot: dict[str, Any], + gripper_profile: GripperProfile, +) -> dict[str, Any]: + per_hand_dof = len(gripper_profile.simulated_joint_initial_positions) + arm_dof = len(robot["init_qpos"]) - 2 * per_hand_dof + if arm_dof <= 0: + raise ValueError("Robot initial posture does not contain arm joints.") + joint_ids: list[int] = [] + for side_index, side in enumerate(("left", "right")): + simulated = gripper_profile.simulated_joint_names(side) + base = arm_dof + side_index * per_hand_dof + joint_ids.extend( + base + simulated.index(name) + for name in gripper_profile.control_joint_names(side) + ) + return { + "norm_robot_eef_joint": { + "func": "normalize_robot_joint_data", + "mode": "modify", + "name": "robot/qpos", + "params": {"joint_ids": joint_ids}, + } + } + + +def _make_dataset( + *, + task_name: str, + task_description: str, + source_config_path: Path, + robot_type: str, +) -> dict[str, Any]: + dataset_policy = _GENERATION_DEFAULTS["dataset"] + return { + "lerobot": { + "func": "LeRobotRecorder", + "mode": "save", + "save_failed_episodes": bool(dataset_policy["save_failed_episodes"]), + "params": { + "robot_meta": { + "robot_type": robot_type, + "control_freq": int(dataset_policy["control_frequency"]), + }, + "instruction": {"lang": task_description}, + "extra": { + "scene_type": source_config_path.parent.name, + "task_name": task_name, + # LeRobotRecorder uses this legacy field as a directory label. + "task_description": task_name, + "data_type": "sim", + }, + "use_videos": bool(dataset_policy["use_videos"]), + }, + } + } + + +def _load_template(name: str) -> Any: + return deepcopy(_read_template(name)) + + +@lru_cache(maxsize=None) +def _read_template(name: str) -> Any: + path = _TEMPLATE_DIR / name + if not path.is_file(): + raise FileNotFoundError(f"Action Engine template not found: {path}") + return json.loads(path.read_text(encoding="utf-8")) + + +def _validate_planning_mode(value: Any) -> str: + """Validate and return the two supported generation/runtime modes.""" + if value not in {"offline", "ab"}: + raise ValueError("planning_mode must be 'offline' or 'ab'.") + return str(value) + + +def _validate_seed_graph_path(value: str | Path | None) -> str: + """Validate a relative or absolute path while preserving caller spelling.""" + if value is None: + return EXECUTION_PROGRAM_FILENAME + if not isinstance(value, (str, Path)): + raise ValueError("seed_task_graph_path must be a non-empty path string.") + path = str(value).strip() + if not path: + raise ValueError("seed_task_graph_path must be a non-empty path string.") + if Path(path).name != EXECUTION_PROGRAM_FILENAME: + raise ValueError("seed_task_graph_path must point to seed_task_graph.json.") + return path + + +def _optional_model(value: Any) -> str | None: + """Normalize optional model names without serializing blank strings.""" + if value is None: + return None + if not isinstance(value, str): + raise TypeError("Model name must be a string or None.") + normalized = value.strip() + return normalized or None + + +def _normalize_vlm_camera_uids(value: Sequence[str] | None) -> list[str]: + """Return the canonical four-camera list used by A/B execution.""" + if value is None: + return list(VLM_CAMERA_UIDS) + if isinstance(value, (str, bytes, bytearray)) or not isinstance(value, Sequence): + raise TypeError("vlm_camera_uids must be a list of strings.") + if not all(isinstance(item, str) for item in value): + raise TypeError("vlm_camera_uids must be a list of strings.") + normalized = [item.strip() for item in value] + if normalized != list(VLM_CAMERA_UIDS): + raise ValueError( + "A/B planning requires VLM cameras in canonical order: " + f"{list(VLM_CAMERA_UIDS)}." + ) + return normalized + + +def _validate_vlm_sensors(value: list[dict[str, Any]]) -> None: + """Validate camera template fields needed by visual fact extraction.""" + if len(value) != len(VLM_CAMERA_UIDS): + raise ValueError("A/B planning requires exactly four VLM cameras.") + if not all(isinstance(sensor, dict) for sensor in value): + raise ValueError("VLM sensors must be object mappings.") + uids = [str(sensor.get("uid", "")) for sensor in value] + if uids != list(VLM_CAMERA_UIDS): + raise ValueError("VLM camera UIDs must be exactly " f"{list(VLM_CAMERA_UIDS)}.") + for sensor in value: + if sensor.get("sensor_type", "Camera") != "Camera": + raise ValueError(f"VLM sensor {sensor.get('uid')!r} must be a Camera.") + if int(sensor.get("width", 0)) != 640 or int(sensor.get("height", 0)) != 480: + raise ValueError("VLM cameras must use 640x480 resolution.") + if not bool(sensor.get("enable_color")) or not bool(sensor.get("enable_depth")): + raise ValueError("VLM cameras must enable RGB and depth.") + extrinsics = sensor.get("extrinsics") + if not isinstance(extrinsics, dict) or not all( + key in extrinsics for key in ("eye", "target", "up") + ): + raise ValueError( + f"VLM sensor {sensor.get('uid')!r} requires eye/target/up extrinsics." + ) + for name in ("eye", "target", "up"): + vector = extrinsics[name] + if ( + not isinstance(vector, Sequence) + or isinstance(vector, (str, bytes, bytearray)) + or len(vector) != 3 + ): + raise ValueError( + f"VLM sensor {sensor.get('uid')!r} {name} must be a 3-vector." + ) + try: + values = [float(item) for item in vector] + except (TypeError, ValueError) as exc: + raise ValueError( + f"VLM sensor {sensor.get('uid')!r} {name} must be numeric." + ) from exc + if not all(math.isfinite(item) for item in values): + raise ValueError( + f"VLM sensor {sensor.get('uid')!r} {name} must be finite." + ) + + +def _anchor_vlm_sensors(sensors: list[dict[str, Any]], scene: PreparedScene) -> None: + """Aim the fixed high views at the normalized tabletop center.""" + table = next( + ( + item + for item in scene.background + if isinstance(item, dict) and str(item.get("uid")) == "table" + ), + None, + ) + init_pos = table.get("init_pos", [0.0, 0.0, 0.0]) if table else [0.0, 0.0, 0.0] + if not isinstance(init_pos, Sequence) or len(init_pos) != 3: + init_pos = [0.0, 0.0, 0.0] + center = [ + float(init_pos[0]), + float(init_pos[1]), + float(scene.table_top_z if scene.table_top_z is not None else 0.75), + ] + for sensor in sensors: + extrinsics = sensor["extrinsics"] + eye = [float(value) for value in extrinsics["eye"]] + target = [float(value) for value in extrinsics["target"]] + offset = [target[index] - 0.0 for index in range(3)] + extrinsics["target"] = list(center) + extrinsics["eye"] = [ + center[index] + eye[index] - offset[index] for index in range(3) + ] diff --git a/embodichain/gen_sim/action_engine/generation/generator.py b/embodichain/gen_sim/action_engine/generation/generator.py new file mode 100644 index 000000000..bd85978a7 --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/generator.py @@ -0,0 +1,905 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Orchestrate source-scene preparation, planning, compilation, and publication.""" + +from __future__ import annotations + +import json +from collections.abc import Mapping +from collections.abc import Sequence +from copy import deepcopy +from pathlib import Path +from typing import Any + +from embodichain.gen_sim.action_engine.config import ( + generation_defaults, + resolve_agent_runtime_policy, +) +from embodichain.gen_sim.action_engine.protocol import ( + ACTION_ENGINE_CONFIG_SCHEMA, + EXECUTION_PROGRAM_FILENAME, + SCENE_REQUIREMENTS_FILENAME, + SCENE_REQUIREMENTS_SCHEMA, + TASK_SPEC_FILENAME, +) + +from .artifacts import artifact_paths, write_generation_artifacts +from .assets import normalize_scene_assets +from .config_builder import ( + VLM_CAMERA_UIDS, + build_agent_config, + build_fast_gym_config, + canonical_robot_profile, +) +from .models import GeneratedConfigPaths +from .source_scene import prepare_scene + +__all__ = ["generate_action_engine_config"] + +_GENERATION_DEFAULTS = generation_defaults() +_TASK_DEFAULTS = _GENERATION_DEFAULTS["task"] +_SCENE_DEFAULTS = _GENERATION_DEFAULTS["scene"] +_DEFAULT_BODY_SCALE = tuple(float(value) for value in _SCENE_DEFAULTS["body_scale"]) + + +def generate_action_engine_config( + gym_project: str | Path, + output_dir: str | Path, + *, + task_name: str, + task_description: str | None = None, + task_spec: Mapping[str, Any] | str | Path | None = None, + robot_profile: str = str(_TASK_DEFAULTS["default_robot_profile"]), + gripper_model: str = str(_TASK_DEFAULTS["default_gripper_model"]), + ik_solver: str = str(_TASK_DEFAULTS["default_ik_solver"]), + llm_model: str | None = None, + source_scene_z_rotation_degrees: float | None = None, + source_scene_xy_translation: Sequence[float] | None = None, + body_scale_policy: str = str(_SCENE_DEFAULTS["body_scale_policy"]), + body_scale: Sequence[float] = _DEFAULT_BODY_SCALE, + overwrite: bool = False, + max_episodes: int = int(_TASK_DEFAULTS["max_episodes"]), + max_episode_steps: int = int(_TASK_DEFAULTS["max_episode_steps"]), + randomize_scene: bool = False, + randomize_table_material: bool = False, + planning_mode: str = "offline", + vlm_model: str | None = None, + planner_policy: Mapping[str, Any] | None = None, +) -> GeneratedConfigPaths: + """Generate the complete Action Engine input bundle. + + Natural-language input is interpreted and grounded by the structured LLM + path. Callers may instead provide an already grounded v2 TaskSpec; that + path never invokes a text model. + """ + task_name = str(task_name).strip() + task_description = "" if task_description is None else str(task_description).strip() + if not task_name: + raise ValueError("task_name must be a non-empty string.") + if task_spec is not None and task_description: + raise ValueError("task_spec cannot be combined with task_description.") + if task_spec is None and not task_description: + raise ValueError("task_description is required when task_spec is not supplied.") + if planning_mode not in {"offline", "ab"}: + raise ValueError("planning_mode must be 'offline' or 'ab'.") + from embodichain.gen_sim.action_engine.gripper_profiles import get_gripper_profile + from embodichain.gen_sim.action_engine.solver_profiles import ( + resolve_ik_solver_mode, + ) + + gripper_model = get_gripper_profile(gripper_model).model.value + ik_solver = resolve_ik_solver_mode( + ik_solver, + canonical_robot_profile(robot_profile), + ) + _raise_if_outputs_exist( + output_dir, + overwrite=overwrite, + planning_mode=planning_mode, + ) + + scene = prepare_scene( + gym_project, + z_rotation_degrees=source_scene_z_rotation_degrees, + source_scene_xy_translation=source_scene_xy_translation, + body_scale_policy=body_scale_policy, + body_scale=body_scale, + ) + + # Delayed imports keep scene/config tooling lightweight and avoid importing + # an LLM client when callers only inspect exported projects. + from embodichain.gen_sim.action_engine.capabilities import ( + build_atomic_capability_registry, + ) + from embodichain.gen_sim.action_engine.domain import ( + seed_graph_hash, + validate_seed_graph, + validate_scene_requirements, + validate_task_spec, + ) + from embodichain.gen_sim.action_engine.tasks import ( + interpret_and_ground_task_spec, + instantiate_seed_graph, + ) + + known_objects = [str(item["runtime_uid"]) for item in scene.planner_objects] + if task_spec is not None: + supplied_task_spec, source_path = _read_task_spec(task_spec) + task_spec = _validated_mapping( + supplied_task_spec, + validator=validate_task_spec, + label="TaskSpec", + ) + _require_matching_task_spec(task_spec, task_name) + task_description = str(task_spec["instruction"]) + supplied_requirements = _read_sibling_scene_requirements( + source_path, + task_name, + ) + if supplied_requirements is not None: + supplied_requirements = _validated_mapping( + supplied_requirements, + validator=validate_scene_requirements, + label="SceneRequirements", + ) + role_bindings = _task_spec_role_bindings( + task_spec, + known_objects, + scene_requirements=supplied_requirements, + scene_objects=scene.planner_objects, + robot_profile=robot_profile, + ) + task_spec = _with_role_bindings(task_spec, role_bindings) + if supplied_requirements is None: + scene_requirements = _scene_requirements_from_bindings( + task_name, + scene.planner_objects, + role_bindings, + ) + else: + scene_requirements = supplied_requirements + _validate_requirement_roles(scene_requirements, role_bindings) + compiled = instantiate_seed_graph(task_spec, role_bindings) + else: + planned = interpret_and_ground_task_spec( + task_name=task_name, + task_description=task_description, + scene_objects=[deepcopy(obj) for obj in scene.planner_objects], + robot_profile=robot_profile, + model=llm_model, + ) + task_spec = _validated_mapping( + planned.task_spec, + validator=validate_task_spec, + label="TaskSpec", + ) + # Persist the validated Scene-Engine hand-off alongside the shared + # semantic TaskSpec. The binding is not an oracle for online planning, + # but it is required for ``--regenerate`` and runtime-only loading. + task_spec = _with_role_bindings(task_spec, planned.role_bindings) + scene_requirements = _validated_mapping( + planned.scene_requirements, + validator=validate_scene_requirements, + label="SceneRequirements", + ) + compiled = instantiate_seed_graph( + task_spec, + planned.role_bindings, + ) + if planning_mode == "ab": + scene_requirements = _add_ab_camera_requirements(scene_requirements) + capabilities = build_atomic_capability_registry() + execution_program = _validated_mapping( + compiled, + validator=lambda value: validate_seed_graph( + value, + known_objects=known_objects, + known_actions=capabilities.names(), + ), + label="SeedGraph", + ) + if execution_program.get("task_id") != task_name: + raise ValueError("SeedGraph task_id does not match requested task_name.") + program_hash = str(seed_graph_hash(execution_program)) + if not program_hash: + raise ValueError("SeedGraph hash must be non-empty.") + + # Validate planning before materializing normalized meshes in output_dir so + # an ambiguous instruction cannot leave a half-generated bundle behind. + scene = normalize_scene_assets(scene, output_dir) + + # Rendering consumes the exact validated in-memory program that runtime + # consumes. The PNG is review-only and never appears in agent input fields. + from embodichain.gen_sim.action_engine.graph_visualization import ( + render_seed_task_graph_png, + ) + + seed_task_graph_png = render_seed_task_graph_png(execution_program) + if not isinstance(seed_task_graph_png, bytes): + raise TypeError("render_seed_task_graph_png must return bytes.") + + paths = artifact_paths(output_dir, planning_mode=planning_mode) + graph_relative_path = paths.seed_task_graph.relative_to( + paths.agent_config.parent + ).as_posix() + vlm_camera_uids = list(VLM_CAMERA_UIDS) + agent_config = build_agent_config( + task_name=task_name, + robot_profile=robot_profile, + gripper_model=gripper_model, + ik_solver=ik_solver, + execution_program_hash=program_hash, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + static_obstacle_uids=[str(config["uid"]) for config in scene.background], + dynamic_obstacle_uids=[str(config["uid"]) for config in scene.rigid_objects], + table_top_z=scene.table_top_z, + articulation_settings={ + str(config["uid"]): deepcopy( + config.get("attributes", {}).get("joint_settings", {}) + ) + for config in scene.planner_objects + if config.get("role") == "articulation" + and isinstance(config.get("attributes"), Mapping) + and config.get("attributes", {}).get("joint_settings") + }, + planning_mode=planning_mode, + seed_task_graph_path=graph_relative_path, + vlm_model=vlm_model, + vlm_camera_uids=vlm_camera_uids, + planner_policy=planner_policy, + ) + gym_config = build_fast_gym_config( + scene, + task_name=task_name, + task_description=task_description, + robot_profile=robot_profile, + gripper_model=gripper_model, + ik_solver=ik_solver, + execution_program_hash=program_hash, + max_episodes=max_episodes, + max_episode_steps=max_episode_steps, + randomize_scene=randomize_scene, + randomize_table_material=randomize_table_material, + planning_mode=planning_mode, + seed_task_graph_path=graph_relative_path, + ) + if planning_mode == "ab": + output_root = Path(output_dir).expanduser().resolve() + gym_config["env"]["events"]["record_camera"]["params"]["save_path"] = ( + output_root / ".ab_video_staging" + ).as_posix() + gym_config["env"]["dataset"]["lerobot"]["params"]["save_path"] = ( + output_root / ".ab_datasets" + ).as_posix() + _validate_agent_config(agent_config) + return write_generation_artifacts( + output_dir, + gym_config=gym_config, + agent_config=agent_config, + task_spec=task_spec, + scene_requirements=scene_requirements, + seed_task_graph=execution_program, + seed_task_graph_png=seed_task_graph_png, + overwrite=overwrite, + planning_mode=planning_mode, + ) + + +def _read_task_spec( + source: Mapping[str, Any] | str | Path, +) -> tuple[dict[str, Any], Path | None]: + """Read one existing v2 TaskSpec without invoking a text planner.""" + if isinstance(source, Mapping): + return deepcopy(dict(source)), None + path = Path(source).expanduser().resolve() + return _read_json_mapping(path, label="TaskSpec"), path + + +def _read_json_mapping(path: Path, *, label: str) -> dict[str, Any]: + try: + value = json.loads(path.read_text(encoding="utf-8")) + except OSError as exc: + raise ValueError(f"Unable to read {label} at {path}: {exc}") from exc + except json.JSONDecodeError as exc: + raise ValueError(f"{label} at {path} is not valid JSON: {exc}") from exc + if not isinstance(value, Mapping): + raise ValueError(f"{label} JSON must contain an object.") + return deepcopy(dict(value)) + + +def _read_sibling_scene_requirements( + task_spec_path: Path | None, + task_name: str, +) -> dict[str, Any] | None: + """Load the canonical sidecar when a task-first batch supplied one.""" + if task_spec_path is None: + return None + candidate = task_spec_path.parent / SCENE_REQUIREMENTS_FILENAME + if not candidate.is_file(): + return None + requirements = _read_json_mapping(candidate, label="SceneRequirements") + if requirements.get("task_id") != task_name: + raise ValueError( + "Sibling SceneRequirements task_id does not match the requested " + "task_name." + ) + return requirements + + +def _require_matching_task_spec(task_spec: Mapping[str, Any], task_name: str) -> None: + if task_spec.get("task_id") != task_name: + raise ValueError( + f"TaskSpec task_id {task_spec.get('task_id')!r} does not match " + f"requested task_name {task_name!r}." + ) + + +def _task_spec_role_bindings( + task_spec: Mapping[str, Any], + known_objects: Sequence[str], + *, + scene_requirements: Mapping[str, Any] | None = None, + scene_objects: Sequence[Mapping[str, Any]] | None = None, + robot_profile: str = "dual_ur10", +) -> dict[str, str]: + """Resolve v2 roles from explicit hand-off data or a strict sidecar match. + + Task-first artifacts may contain abstract role IDs rather than scene UIDs. + When their sibling SceneRequirements is available, match + every still-unbound role against the source scene's static category, + attributes, state, and affordance metadata. This is a deterministic + Scene-Engine hand-off, not a text-model fallback: missing or ambiguous + evidence remains an error. + """ + metadata = task_spec.get("metadata", {}) + if not isinstance(metadata, Mapping): + raise ValueError("TaskSpec.metadata must be a mapping.") + metadata_bindings = metadata.get("role_bindings", {}) + if not isinstance(metadata_bindings, Mapping): + raise ValueError("TaskSpec.metadata.role_bindings must be a mapping.") + candidates: list[tuple[str, Mapping[str, Any]]] = [] + if metadata_bindings: + candidates.append(("TaskSpec.metadata", metadata_bindings)) + + # Older grounded v2 TaskSpecs kept this private hand-off in ``oracle`` + # rather than metadata. Accept that representation while publishing the + # normalized binding in metadata for runtime regeneration. + oracle = task_spec.get("oracle", {}) + if isinstance(oracle, Mapping) and oracle.get("role_bindings"): + oracle_bindings = oracle["role_bindings"] + if not isinstance(oracle_bindings, Mapping): + raise ValueError("TaskSpec.oracle.role_bindings must be a mapping.") + candidates.append(("TaskSpec.oracle", oracle_bindings)) + if isinstance(oracle, Mapping): + reference = oracle.get("reference_seed_graph") + if isinstance(reference, Mapping): + graph_metadata = reference.get("metadata", {}) + if isinstance(graph_metadata, Mapping) and graph_metadata.get( + "role_bindings" + ): + graph_bindings = graph_metadata["role_bindings"] + if not isinstance(graph_bindings, Mapping): + raise ValueError( + "SeedGraph.metadata.role_bindings must be a mapping." + ) + candidates.append(("SeedGraph.metadata", graph_bindings)) + + if scene_requirements is not None: + requirement_metadata = scene_requirements.get("metadata", {}) + if isinstance(requirement_metadata, Mapping) and requirement_metadata.get( + "role_bindings" + ): + requirement_bindings = requirement_metadata["role_bindings"] + if not isinstance(requirement_bindings, Mapping): + raise ValueError( + "SceneRequirements.metadata.role_bindings must be a mapping." + ) + candidates.append(("SceneRequirements.metadata", requirement_bindings)) + + supplied: dict[str, Any] = {} + supplied_sources: dict[str, str] = {} + for source, candidate in candidates: + for raw_role, uid in candidate.items(): + if not isinstance(raw_role, str) or not raw_role.strip(): + raise ValueError(f"{source}.role_bindings must use non-empty role IDs.") + role = raw_role.strip() + if role in supplied and supplied[role] != uid: + raise ValueError( + "Conflicting role_bindings were supplied for " + f"{role!r} by {supplied_sources[role]} and {source}." + ) + supplied[role] = uid + supplied_sources[role] = source + + known = {str(uid) for uid in known_objects} + required = _task_spec_role_references(task_spec.get("task_instances", [])) + required.discard("table") + if not required: + raise ValueError("TaskSpec must reference at least one non-table object role.") + + bindings: dict[str, str] = {} + missing: list[str] = [] + for role in sorted(required): + raw_uid = supplied.get(role, role if role in known else None) + if raw_uid is None: + missing.append(role) + continue + if not isinstance(raw_uid, str) or not raw_uid.strip(): + raise ValueError( + "TaskSpec.metadata.role_bindings must map role IDs to non-empty " + "runtime UIDs." + ) + uid = raw_uid.strip() + if uid not in known: + raise ValueError(f"TaskSpec role {role!r} binds unknown scene UID {uid!r}.") + bindings[role] = uid + if missing and scene_requirements is not None and scene_objects is not None: + bindings.update( + _infer_role_bindings_from_scene_requirements( + missing, + known_objects=known, + scene_objects=scene_objects, + scene_requirements=scene_requirements, + existing_bindings=bindings, + robot_profile=robot_profile, + ) + ) + missing = [role for role in missing if role not in bindings] + if missing: + raise ValueError( + "TaskSpec requires explicit role_bindings or an unambiguous sibling " + f"SceneRequirements match for roles {missing}; a task-first spec must " + "be grounded by a Scene Engine before it can be compiled for this gym " + "project." + ) + if len(bindings.values()) != len(set(bindings.values())): + raise ValueError("TaskSpec role bindings must resolve to unique scene UIDs.") + if scene_requirements is not None and scene_objects is not None: + _validate_bound_role_requirements( + bindings, + scene_requirements=scene_requirements, + scene_objects=scene_objects, + robot_profile=robot_profile, + ) + return bindings + + +def _infer_role_bindings_from_scene_requirements( + roles: Sequence[str], + *, + known_objects: set[str], + scene_objects: Sequence[Mapping[str, Any]], + scene_requirements: Mapping[str, Any], + existing_bindings: Mapping[str, str], + robot_profile: str, +) -> dict[str, str]: + """Bind abstract task roles only when static evidence is unique.""" + from embodichain.gen_sim.action_engine.tasks.assembly import SceneInventory + + requirements = _requirements_by_role(scene_requirements) + inventory = SceneInventory(scene_objects, robot_profile=robot_profile) + entities = [entity for entity in inventory.entities if entity.uid in known_objects] + used_uids = set(existing_bindings.values()) + inferred: dict[str, str] = {} + for role in sorted(roles): + requirement = requirements.get(role) + if requirement is None: + raise ValueError( + "Sibling SceneRequirements is missing TaskSpec role " f"{role!r}." + ) + count = requirement.get("count", 1) + if count != 1: + raise ValueError( + f"TaskSpec role {role!r} has count={count}; direct SeedGraph " + "binding requires exactly one concrete scene UID." + ) + matches = [ + entity + for entity in entities + if entity.uid not in used_uids + and _entity_matches_requirement( + entity, + requirement, + require_complete_static_evidence=True, + ) + ] + if len(matches) != 1: + raise ValueError( + "TaskSpec role " + f"{role!r} requires one unambiguous scene match, found " + f"{[entity.uid for entity in matches]}." + ) + uid = matches[0].uid + inferred[role] = uid + used_uids.add(uid) + return inferred + + +def _validate_bound_role_requirements( + bindings: Mapping[str, str], + *, + scene_requirements: Mapping[str, Any], + scene_objects: Sequence[Mapping[str, Any]], + robot_profile: str, +) -> None: + """Ensure an explicit binding does not contradict its static sidecar.""" + from embodichain.gen_sim.action_engine.tasks.assembly import SceneInventory + + requirements = _requirements_by_role(scene_requirements) + entities = SceneInventory(scene_objects, robot_profile=robot_profile).by_uid + for role, uid in bindings.items(): + requirement = requirements.get(role) + if requirement is None: + raise ValueError( + "Sibling SceneRequirements is missing TaskSpec role " f"{role!r}." + ) + entity = entities.get(uid) + if entity is None: + raise ValueError( + f"TaskSpec role {role!r} binds unavailable scene UID {uid!r}." + ) + if not _entity_matches_requirement( + entity, + requirement, + require_complete_static_evidence=False, + ): + raise ValueError( + f"TaskSpec role {role!r} binding {uid!r} conflicts with its " + "SceneRequirements category, attributes, state, or affordances." + ) + + +def _requirements_by_role( + scene_requirements: Mapping[str, Any], +) -> dict[str, Mapping[str, Any]]: + objects = scene_requirements.get("objects", []) + if not isinstance(objects, Sequence) or isinstance(objects, (str, bytes)): + raise ValueError("SceneRequirements.objects must be a list.") + result: dict[str, Mapping[str, Any]] = {} + for requirement in objects: + if not isinstance(requirement, Mapping): + raise ValueError("SceneRequirements.objects must contain mappings.") + role = requirement.get("role_id") + if not isinstance(role, str) or not role: + raise ValueError("SceneRequirements role_id must be a non-empty string.") + result[role] = requirement + return result + + +def _entity_matches_requirement( + entity: Any, + requirement: Mapping[str, Any], + *, + require_complete_static_evidence: bool, +) -> bool: + """Match explicit metadata; UID inference requires complete evidence.""" + category = requirement.get("category") + expected_category = category.strip().casefold() if isinstance(category, str) else "" + actual_category = str(entity.category).strip().casefold() + if expected_category: + if not actual_category: + if require_complete_static_evidence: + return False + elif expected_category != actual_category: + return False + required_affordances = requirement.get("affordances", []) + if not isinstance(required_affordances, Sequence) or isinstance( + required_affordances, (str, bytes) + ): + return False + expected_affordances = { + str(value).strip().casefold() for value in required_affordances + } + if ( + expected_affordances + and (require_complete_static_evidence or entity.affordances) + and not expected_affordances.issubset(entity.affordances) + ): + return False + expected_attributes = requirement.get("attributes", {}) + if not isinstance(expected_attributes, Mapping): + return False + for name, expected in expected_attributes.items(): + if not _static_attribute_matches( + entity, + str(name), + expected, + require_complete_static_evidence=require_complete_static_evidence, + ): + return False + expected_state = requirement.get("initial_state", {}) + if not isinstance(expected_state, Mapping): + return False + missing = object() + for name, expected in expected_state.items(): + actual = entity.initial_state.get(str(name), missing) + if actual is missing: + if require_complete_static_evidence: + return False + elif actual != expected: + return False + return True + + +def _static_attribute_matches( + entity: Any, + name: str, + expected: Any, + *, + require_complete_static_evidence: bool, +) -> bool: + """Compare one requirement against explicit exported metadata only.""" + marker = object() + actual = entity.color if name == "color" else entity.attributes.get(name, marker) + if actual is marker or actual is None or actual == "": + return not require_complete_static_evidence + if name == "color" and isinstance(actual, str) and isinstance(expected, str): + return actual.strip().casefold() == expected.strip().casefold() + return actual == expected + + +def _task_spec_role_references(value: Any, key: str = "") -> set[str]: + if isinstance(value, Mapping): + return { + role + for child_key, child in value.items() + for role in _task_spec_role_references(child, str(child_key)) + } + if isinstance(value, Sequence) and not isinstance(value, (str, bytes)): + return { + role for child in value for role in _task_spec_role_references(child, key) + } + if isinstance(value, str) and (key.endswith("_role") or key.endswith("_roles")): + return {value} + return set() + + +def _with_role_bindings( + task_spec: Mapping[str, Any], + role_bindings: Mapping[str, str], +) -> dict[str, Any]: + result = deepcopy(dict(task_spec)) + metadata = result.setdefault("metadata", {}) + if not isinstance(metadata, dict): + raise ValueError("TaskSpec.metadata must be a mapping.") + metadata["role_bindings"] = dict(sorted(role_bindings.items())) + return result + + +def _validate_requirement_roles( + requirements: Mapping[str, Any], + role_bindings: Mapping[str, str], +) -> None: + requirement_roles = { + str(item["role_id"]) + for item in requirements["objects"] + if isinstance(item, Mapping) + } + missing = sorted(set(role_bindings) - requirement_roles) + if missing: + raise ValueError( + "SceneRequirements is missing TaskSpec role bindings for " f"{missing}." + ) + + +def _scene_requirements_from_bindings( + task_id: str, + planner_objects: Sequence[Mapping[str, Any]], + role_bindings: Mapping[str, str], +) -> dict[str, Any]: + """Derive a minimal concrete SceneRequirements view for grounded roles.""" + source = _scene_requirements_from_scene(task_id, planner_objects) + by_uid = {str(item["role_id"]): item for item in source["objects"]} + objects = [] + for role, uid in sorted(role_bindings.items()): + requirement = by_uid.get(uid) + if requirement is None: + raise ValueError( + f"TaskSpec role {role!r} binds UID {uid!r}, which has no " + "source-scene requirement." + ) + resolved = deepcopy(requirement) + resolved["role_id"] = role + objects.append(resolved) + return { + "schema_version": SCENE_REQUIREMENTS_SCHEMA, + "task_id": task_id, + "objects": objects, + "cameras": [], + "spatial_constraints": [{"type": "preserve_source_scene"}], + "distractor_count": 0, + "metadata": {"source": "task_spec_role_bindings"}, + } + + +def _validated_mapping( + value: Any, + *, + validator: Any, + label: str, +) -> dict[str, Any]: + if not isinstance(value, Mapping): + raise TypeError( + f"{label} producer returned {type(value).__name__}, not a mapping." + ) + candidate = deepcopy(dict(value)) + validated = validator(candidate) + if validated is None: + # Validators may either return a normalized mapping or validate in place. + validated = candidate + if not isinstance(validated, Mapping): + raise TypeError(f"{label} validator must return a mapping or None.") + return deepcopy(dict(validated)) + + +def _validate_agent_config(config: Mapping[str, Any]) -> None: + if config.get("schema_version") != ACTION_ENGINE_CONFIG_SCHEMA: + raise ValueError("Agent config has an unexpected schema_version.") + if config.get("task_spec") != TASK_SPEC_FILENAME: + raise ValueError("Agent config must point to the canonical TaskSpec.") + if config.get("scene_requirements") != SCENE_REQUIREMENTS_FILENAME: + raise ValueError("Agent config must point to canonical SceneRequirements.") + from embodichain.gen_sim.action_engine.gripper_profiles import get_gripper_profile + from embodichain.gen_sim.action_engine.solver_profiles import ( + resolve_ik_solver_mode, + ) + + get_gripper_profile(config.get("gripper_model")) + solver = config.get("ik_solver") + if resolve_ik_solver_mode(solver, str(config.get("robot_profile"))) != solver: + raise ValueError("Agent config must store a concrete IK solver mode.") + graph_path = config.get("seed_task_graph") + if ( + not isinstance(graph_path, str) + or Path(graph_path).name != EXECUTION_PROGRAM_FILENAME + ): + raise ValueError("Agent config must point to the canonical SeedGraph.") + planning_mode = config.get("planning_mode", "offline") + if planning_mode not in {"offline", "ab"}: + raise ValueError("Agent config planning_mode must be 'offline' or 'ab'.") + if planning_mode == "ab": + online = config.get("online_planning") + if not isinstance(online, Mapping): + raise ValueError("A/B agent config requires online_planning settings.") + camera_uids = online.get("camera_uids") + if camera_uids != list(VLM_CAMERA_UIDS): + raise ValueError( + "A/B agent config must list the canonical four VLM cameras." + ) + model = online.get("vlm_model") + if model is not None and (not isinstance(model, str) or not model.strip()): + raise ValueError("online_planning.vlm_model must be a string or null.") + if config.get("offline_seed_task_graph") != graph_path: + raise ValueError( + "A/B agent config offline_seed_task_graph must match seed_task_graph." + ) + if config.get("vlm_camera_uids") != camera_uids: + raise ValueError( + "A/B agent config vlm_camera_uids must match online_planning." + ) + if config.get("vlm_model") != model: + raise ValueError("A/B agent config vlm_model must match online_planning.") + resolve_agent_runtime_policy(config) + + +def _raise_if_outputs_exist( + output_dir: str | Path, + *, + overwrite: bool, + planning_mode: str = "offline", +) -> None: + if overwrite: + return + paths = artifact_paths(output_dir, planning_mode=planning_mode) + existing = [ + path + for path in ( + paths.gym_config, + paths.agent_config, + paths.task_spec, + paths.scene_requirements, + paths.seed_task_graph, + paths.seed_task_graph_png, + ) + if path.exists() + ] + if existing: + names = ", ".join(path.name for path in existing) + raise FileExistsError( + f"Generated artifacts already exist in {paths.gym_config.parent}: " + f"{names}. Pass --overwrite to replace them." + ) + + +def _scene_requirements_from_scene( + task_id: str, + planner_objects: Sequence[Mapping[str, Any]], +) -> dict[str, Any]: + objects = [] + for item in planner_objects: + uid = str(item.get("runtime_uid", item.get("uid", ""))).strip() + if not uid: + raise ValueError("Planner scene object is missing a runtime UID.") + role = str(item.get("role", "object")).strip().lower() + raw_category = item.get("category", item.get("object_category", "")) + category = str(raw_category).strip().lower() or role or "object" + raw_attributes = item.get("attributes", {}) + attributes = ( + deepcopy(dict(raw_attributes)) + if isinstance(raw_attributes, Mapping) + else {} + ) + color = item.get("color") + if color not in (None, ""): + attributes.setdefault("color", color) + objects.append( + { + "role_id": uid, + "category": category, + "count": 1, + "affordances": [], + "initial_state": {}, + "attributes": attributes, + } + ) + return { + "schema_version": SCENE_REQUIREMENTS_SCHEMA, + "task_id": task_id, + "objects": objects, + "cameras": [], + "spatial_constraints": [{"type": "preserve_source_scene"}], + "distractor_count": 0, + "metadata": {"source": "existing_gym_project"}, + } + + +def _add_ab_camera_requirements( + requirements: Mapping[str, Any], +) -> dict[str, Any]: + """Declare fixed multi-view inputs in the shared A/B hand-off.""" + from embodichain.gen_sim.action_engine.domain import validate_scene_requirements + + result = deepcopy(dict(requirements)) + cameras = result.get("cameras", []) + if not isinstance(cameras, list): + raise ValueError("SceneRequirements.cameras must be a list.") + existing_uids = { + str(item.get("uid")) + for item in cameras + if isinstance(item, Mapping) and item.get("uid") + } + for uid in VLM_CAMERA_UIDS: + if uid in existing_uids: + continue + cameras.append( + { + "uid": uid, + "role": "vlm_view", + "modalities": ["rgb", "depth"], + "coverage": "all_interaction_objects", + "resolution": [640, 480], + } + ) + result["cameras"] = cameras + metadata = result.setdefault("metadata", {}) + if not isinstance(metadata, dict): + metadata = {} + result["metadata"] = metadata + metadata["planning_mode"] = "ab" + metadata["vlm_camera_uids"] = list(VLM_CAMERA_UIDS) + return validate_scene_requirements(result) diff --git a/embodichain/gen_sim/action_engine/generation/models.py b/embodichain/gen_sim/action_engine/generation/models.py new file mode 100644 index 000000000..f12ae6440 --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/models.py @@ -0,0 +1,83 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Small value objects used by Action Engine config generation.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +__all__ = ["GeneratedConfigPaths", "PreparedScene"] + + +@dataclass(frozen=True) +class GeneratedConfigPaths: + """Paths written by one successful generation transaction.""" + + gym_config: Path + agent_config: Path + task_spec: Path + scene_requirements: Path + seed_task_graph: Path + seed_task_graph_png: Path + planning_mode: str = "offline" + + @property + def execution_program(self) -> Path: + """Retain the Python API alias for callers migrating to SeedGraph v3.""" + return self.seed_task_graph + + @property + def offline_seed_task_graph(self) -> Path: + """Explicit A/B alias for the immutable offline SeedGraph artifact.""" + return self.seed_task_graph + + @property + def seed_task_graph_path(self) -> Path: + """Path-style alias used by runtime config loaders.""" + return self.seed_task_graph + + @property + def offline_seed_task_graph_path(self) -> Path: + """Verbose alias for callers that distinguish A/B graph branches.""" + return self.seed_task_graph + + @property + def offline_seed_task_graph_png(self) -> Path: + """Explicit A/B alias for the review rendering of the offline graph.""" + return self.seed_task_graph_png + + +@dataclass(frozen=True) +class PreparedScene: + """A source scene normalized for both planning and simulator loading.""" + + source_config_path: Path + scene_dir: Path + planner_objects: tuple[dict[str, Any], ...] + background: tuple[dict[str, Any], ...] + rigid_objects: tuple[dict[str, Any], ...] + articulations: tuple[dict[str, Any], ...] + uid_map: dict[str, str] + table_top_z: float | None + z_rotation_degrees: float + body_scale_policy: str + body_scale: tuple[float, float, float] + asset_hashes: dict[str, str] + source_scene_xy_translation: tuple[float, float] = (0.0, 0.0) + asset_provenance: tuple[dict[str, Any], ...] = () diff --git a/embodichain/gen_sim/action_engine/generation/source_scene.py b/embodichain/gen_sim/action_engine/generation/source_scene.py new file mode 100644 index 000000000..fd2f8845b --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/source_scene.py @@ -0,0 +1,644 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Read and normalize an exported Prompt2Scene source scene. + +The source scene remains the authority for object geometry and initial poses. +Generation only makes asset paths absolute, gives runtime objects stable UIDs, +applies one explicit world-frame rotation, and adds conservative physics values +needed by manipulation tasks. +""" + +from __future__ import annotations + +import hashlib +from collections.abc import Mapping, Sequence +from copy import deepcopy +from dataclasses import dataclass +import json +import math +from pathlib import Path +import re +from typing import Any +import warnings + +from embodichain.data import get_data_path +from embodichain.gen_sim.action_engine.config import generation_defaults + +from .models import PreparedScene + +__all__ = [ + "ResolvedSceneSource", + "is_prompt2scene_export", + "prepare_scene", + "resolve_gym_config_path", + "resolve_source_scene", +] + +_LEGACY_CONFIG_FILENAMES = ("gym_config_merged.json", "gym_config.json") +_SCENE_CONFIG_FILENAME = "scene_config.json" +_CONFIG_FILENAMES = (*_LEGACY_CONFIG_FILENAMES, _SCENE_CONFIG_FILENAME) +_EXPORT_DIRECTORY_NAMES = ("gym_export", "scene_export") +_LEGACY_GYM_FORMAT = "legacy_gym_config" +_SCENE_EXPORT_FORMAT = "embodichain.scene-export/v1" +_SCENE_SECTIONS = ("background", "rigid_object", "articulation") +_UID_SUFFIX_RE = re.compile(r"_0$") +_UID_INVALID_RE = re.compile(r"[^0-9A-Za-z_.-]+") + +_GENERATION_DEFAULTS = generation_defaults() +_SCENE_DEFAULTS = _GENERATION_DEFAULTS["scene"] +_PHYSICS_DEFAULTS = _GENERATION_DEFAULTS["physics"] +_BACKGROUND_POLICY = _PHYSICS_DEFAULTS["background"] +_RIGID_POLICY = _PHYSICS_DEFAULTS["rigid_object"] +_BACKGROUND_ATTRS = { + key: value + for key, value in _BACKGROUND_POLICY.items() + if key != "max_convex_hull_num" +} +_RIGID_ATTRS = { + key: value + for key, value in _RIGID_POLICY.items() + if key not in {"max_convex_hull_num", "acd_method"} +} +_DEFAULT_BODY_SCALE = tuple(float(value) for value in _SCENE_DEFAULTS["body_scale"]) + + +@dataclass(frozen=True) +class ResolvedSceneSource: + """One validated source-scene config selected from an export layout. + + Attributes: + path: Absolute path to the selected source configuration. + source_format: Stable identifier for the detected source schema. + is_prompt2scene: Whether Prompt2Scene world alignment should be applied. + """ + + path: Path + source_format: str + is_prompt2scene: bool + + +def resolve_source_scene(gym_project: str | Path) -> ResolvedSceneSource: + """Resolve and classify one supported source-scene configuration. + + Args: + gym_project: Task root, export directory, or explicit configuration path. + + Returns: + The selected path together with its source format and provenance. + + Raises: + FileNotFoundError: If no supported source configuration exists. + ValueError: If a config is unsupported or recursive discovery is ambiguous. + """ + input_path = Path(gym_project).expanduser().resolve() + if input_path.is_file(): + return _classify_source_config(input_path) + if not input_path.is_dir(): + raise FileNotFoundError(f"Scene project does not exist: {input_path}") + + for directory in ( + input_path, + *(input_path / name for name in _EXPORT_DIRECTORY_NAMES), + ): + preferred = _preferred_config(directory) + if preferred is not None: + return _classify_source_config(preferred) + + matches = sorted( + { + candidate.parent + for filename in _CONFIG_FILENAMES + for candidate in input_path.rglob(filename) + } + ) + preferred = [ + config + for directory in matches + if (config := _preferred_config(directory)) is not None + ] + if len(preferred) == 1: + return _classify_source_config(preferred[0]) + if not preferred: + expected = ", ".join(_CONFIG_FILENAMES) + raise FileNotFoundError( + f"No supported scene config ({expected}) found under: {input_path}" + ) + paths = ", ".join(path.as_posix() for path in preferred) + raise ValueError(f"Multiple exported scene configs found: {paths}") + + +def resolve_gym_config_path(gym_project: str | Path) -> Path: + """Return the selected config path for callers using the legacy API name.""" + return resolve_source_scene(gym_project).path + + +def is_prompt2scene_export(gym_project: str | Path) -> bool: + """Return whether the input has Prompt2Scene export provenance.""" + try: + return resolve_source_scene(gym_project).is_prompt2scene + except (FileNotFoundError, ValueError): + return False + + +def prepare_scene( + gym_project: str | Path, + *, + z_rotation_degrees: float | None = None, + source_scene_xy_translation: Sequence[float] | None = None, + body_scale_policy: str = str(_SCENE_DEFAULTS["body_scale_policy"]), + body_scale: Sequence[float] = _DEFAULT_BODY_SCALE, +) -> PreparedScene: + """Load a source config and return planner/runtime views of one scene.""" + scale_policy = str(body_scale_policy).strip().lower() + if scale_policy not in {"preserve", "multiply", "absolute"}: + raise ValueError("body_scale_policy must be preserve, multiply, or absolute.") + requested_scale = _vector3(body_scale) + if any(value <= 0.0 for value in requested_scale): + raise ValueError("body_scale values must be positive.") + resolved_source = resolve_source_scene(gym_project) + source_path = resolved_source.path + source = _read_json_object(source_path) + scene_dir = source_path.parent + source_entries = _collect_source_entries(source) + if not source_entries: + raise ValueError( + "Source scene config has no background, rigid_object, or articulation." + ) + + table_source_uid = _find_table_source_uid(source_entries) + uid_map = _make_uid_map(source_entries, table_source_uid=table_source_uid) + source_robot = source.get("robot") + source_has_robot = isinstance(source_robot, Mapping) and bool(source_robot) + source_table = next( + ( + item + for role, item in source_entries + if role == "background" and str(item.get("uid", "")) == table_source_uid + ), + None, + ) + if source_scene_xy_translation is not None: + if len(source_scene_xy_translation) != 2 or any( + not math.isfinite(float(value)) for value in source_scene_xy_translation + ): + raise ValueError( + "source_scene_xy_translation must contain two finite values." + ) + resolved_xy_translation = tuple( + float(value) for value in source_scene_xy_translation + ) + elif source_has_robot and source_table is not None: + table_anchor = _vector3(source_table.get("init_pos", (0.0, 0.0, 0.0))) + resolved_xy_translation = (-table_anchor[0], -table_anchor[1]) + else: + resolved_xy_translation = (0.0, 0.0) + rotation = ( + float(_SCENE_DEFAULTS["prompt2scene_z_rotation_degrees"]) + if z_rotation_degrees is None and resolved_source.is_prompt2scene + else float(z_rotation_degrees or 0.0) + ) + + planner_objects: list[dict[str, Any]] = [] + runtime_sections: dict[str, list[dict[str, Any]]] = { + section: [] for section in _SCENE_SECTIONS + } + asset_hashes: dict[str, str] = {} + for role, source_config in source_entries: + source_uid = _require_uid(source_config, role=role) + normalized = deepcopy(source_config) + normalized["uid"] = uid_map[source_uid] + _make_asset_paths_absolute(normalized, scene_dir=scene_dir, role=role) + _normalize_pose_fields(normalized) + normalized["init_pos"][0] += resolved_xy_translation[0] + normalized["init_pos"][1] += resolved_xy_translation[1] + _apply_body_scale_policy( + normalized, + policy=scale_policy, + requested=requested_scale, + ) + _apply_world_z_rotation(normalized, rotation) + shape = normalized.get("shape") + if isinstance(shape, Mapping) and shape.get("fpath"): + asset_hashes[normalized["uid"]] = _file_hash(Path(str(shape["fpath"]))) + + planner_objects.append( + _planner_object( + normalized, + source_uid=source_uid, + role=role, + ) + ) + runtime_sections[role].append(_runtime_object(normalized, role=role)) + + table = next( + (obj for obj in runtime_sections["background"] if obj.get("uid") == "table"), + None, + ) + table_top_z = _estimate_mesh_top_z(table) if table is not None else None + return PreparedScene( + source_config_path=source_path, + scene_dir=scene_dir, + planner_objects=tuple(planner_objects), + background=tuple(runtime_sections["background"]), + rigid_objects=tuple(runtime_sections["rigid_object"]), + articulations=tuple(runtime_sections["articulation"]), + uid_map=uid_map, + table_top_z=table_top_z, + z_rotation_degrees=rotation, + body_scale_policy=scale_policy, + body_scale=tuple(requested_scale), + asset_hashes=asset_hashes, + source_scene_xy_translation=resolved_xy_translation, + ) + + +def _preferred_config(directory: Path) -> Path | None: + for filename in _CONFIG_FILENAMES: + candidate = directory / filename + if candidate.is_file(): + return candidate + return None + + +def _classify_source_config(path: Path) -> ResolvedSceneSource: + if path.name not in _CONFIG_FILENAMES: + source = _read_json_object(path) + if not any( + isinstance(source.get(section), Sequence) for section in _SCENE_SECTIONS + ): + expected = ", ".join(_CONFIG_FILENAMES) + raise ValueError( + f"Expected one of {expected} or an explicit legacy scene JSON, " + f"got: {path}" + ) + return ResolvedSceneSource( + path=path, + source_format=_LEGACY_GYM_FORMAT, + is_prompt2scene=False, + ) + if path.name == _SCENE_CONFIG_FILENAME: + source = _read_json_object(path) + source_format = source.get("format") + if source_format != _SCENE_EXPORT_FORMAT: + raise ValueError( + f"Scene config {path} has unsupported format {source_format!r}; " + f"expected {_SCENE_EXPORT_FORMAT!r}." + ) + return ResolvedSceneSource( + path=path, + source_format=_SCENE_EXPORT_FORMAT, + is_prompt2scene=True, + ) + return ResolvedSceneSource( + path=path, + source_format=_LEGACY_GYM_FORMAT, + is_prompt2scene=( + _has_legacy_prompt2scene_marker(path) or _has_scene_export_companion(path) + ), + ) + + +def _has_legacy_prompt2scene_marker(config_path: Path) -> bool: + config_dir = config_path.parent + directories = [config_dir, config_dir / "gym_export"] + return any( + (directory / "scene_state" / "result.json").is_file() + for directory in directories + ) + + +def _has_scene_export_companion(config_path: Path) -> bool: + config_dir = config_path.parent + candidates = [config_dir / _SCENE_CONFIG_FILENAME] + if config_dir.name == "gym_export": + candidates.append(config_dir.parent / "scene_export" / _SCENE_CONFIG_FILENAME) + else: + candidates.append(config_dir / "scene_export" / _SCENE_CONFIG_FILENAME) + return any(_is_scene_export_v1(candidate) for candidate in candidates) + + +def _is_scene_export_v1(path: Path) -> bool: + if not path.is_file(): + return False + try: + return _read_json_object(path).get("format") == _SCENE_EXPORT_FORMAT + except ValueError: + return False + + +def _read_json_object(path: Path) -> dict[str, Any]: + try: + value = json.loads(path.read_text(encoding="utf-8")) + except json.JSONDecodeError as exc: + raise ValueError(f"Invalid JSON in source scene config {path}: {exc}") from exc + if not isinstance(value, dict): + raise ValueError(f"Source scene config must contain a JSON object: {path}") + return value + + +def _collect_source_entries( + source: Mapping[str, Any], +) -> list[tuple[str, dict[str, Any]]]: + entries: list[tuple[str, dict[str, Any]]] = [] + for section in _SCENE_SECTIONS: + value = source.get(section, []) + if isinstance(value, Mapping): + value = [value] + if not isinstance(value, list): + raise ValueError(f"Source scene section {section!r} must be a list.") + for config in value: + if not isinstance(config, Mapping): + raise ValueError(f"Entries in {section!r} must be JSON objects.") + entries.append((section, dict(config))) + return entries + + +def _find_table_source_uid(entries: Sequence[tuple[str, Mapping[str, Any]]]) -> str: + backgrounds = [config for role, config in entries if role == "background"] + if len(backgrounds) != 1: + raise ValueError( + "A tabletop action scene requires exactly one background object; " + f"found {len(backgrounds)}." + ) + return _require_uid(backgrounds[0], role="background") + + +def _make_uid_map( + entries: Sequence[tuple[str, Mapping[str, Any]]], + *, + table_source_uid: str, +) -> dict[str, str]: + uid_map: dict[str, str] = {} + used: set[str] = set() + for role, config in entries: + source_uid = _require_uid(config, role=role) + if source_uid in uid_map: + raise ValueError(f"Duplicate scene object UID: {source_uid!r}") + candidate = ( + "table" if source_uid == table_source_uid else _normalize_uid(source_uid) + ) + runtime_uid = candidate + suffix = 2 + while runtime_uid in used: + runtime_uid = f"{candidate}_{suffix}" + suffix += 1 + uid_map[source_uid] = runtime_uid + used.add(runtime_uid) + return uid_map + + +def _normalize_uid(source_uid: str) -> str: + candidate = _UID_SUFFIX_RE.sub("", source_uid.strip()) + candidate = _UID_INVALID_RE.sub("_", candidate).strip("._-") + if not candidate: + raise ValueError(f"Cannot derive a runtime UID from {source_uid!r}.") + if candidate[0].isdigit(): + candidate = f"object_{candidate}" + return candidate + + +def _require_uid(config: Mapping[str, Any], *, role: str) -> str: + uid = str(config.get("uid", "")).strip() + if not uid: + raise ValueError(f"Scene object in {role!r} has no UID.") + return uid + + +def _make_asset_paths_absolute( + config: dict[str, Any], + *, + scene_dir: Path, + role: str, +) -> None: + shape = config.get("shape") + if isinstance(shape, Mapping): + normalized_shape = deepcopy(dict(shape)) + fpath = normalized_shape.get("fpath") + if fpath: + normalized_shape["fpath"] = _resolve_asset_path( + scene_dir, str(fpath) + ).as_posix() + config["shape"] = normalized_shape + if role == "articulation" and config.get("fpath"): + config["fpath"] = _resolve_asset_path( + scene_dir, str(config["fpath"]) + ).as_posix() + + +def _resolve_asset_path(scene_dir: Path, fpath: str) -> Path: + raw = Path(fpath).expanduser() + resolved = raw.resolve() if raw.is_absolute() else (scene_dir / raw).resolve() + if not resolved.is_file() and not raw.is_absolute(): + resolved = Path(get_data_path(fpath)).expanduser().resolve() + if not resolved.is_file(): + raise FileNotFoundError(f"Scene asset does not exist: {resolved}") + return resolved + + +def _normalize_pose_fields(config: dict[str, Any]) -> None: + config["init_pos"] = _vector3(config.get("init_pos", [0.0, 0.0, 0.0])) + config["init_rot"] = _vector3(config.get("init_rot", [0.0, 0.0, 0.0])) + if "body_scale" in config: + scale = _vector3(config["body_scale"]) + if any(value <= 0.0 for value in scale): + raise ValueError( + f"Object {config.get('uid')!r} has non-positive body_scale." + ) + config["body_scale"] = scale + + +def _apply_body_scale_policy( + config: dict[str, Any], + *, + policy: str, + requested: Sequence[float], +) -> None: + source = _vector3(config.get("body_scale", [1.0, 1.0, 1.0])) + if policy == "preserve": + result = source + elif policy == "multiply": + result = [left * right for left, right in zip(source, requested)] + else: + result = list(requested) + config["body_scale"] = [_clean_float(value) for value in result] + + +def _file_hash(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _apply_world_z_rotation(config: dict[str, Any], degrees: float) -> None: + if math.isclose(degrees, 0.0, abs_tol=1e-12): + return + theta = math.radians(degrees) + cos_theta, sin_theta = math.cos(theta), math.sin(theta) + x, y, z = _vector3(config["init_pos"]) + config["init_pos"] = [ + _clean_float(x * cos_theta - y * sin_theta), + _clean_float(x * sin_theta + y * cos_theta), + _clean_float(z), + ] + + # EmbodiChain and Prompt2Scene both interpret these values as intrinsic XYZ. + from scipy.spatial.transform import Rotation + + original = Rotation.from_euler("XYZ", config["init_rot"], degrees=True) + world_z = Rotation.from_rotvec([0.0, 0.0, theta]) + with warnings.catch_warnings(): + warnings.filterwarnings("ignore", message="Gimbal lock detected") + rotated = (world_z * original).as_euler("XYZ", degrees=True) + config["init_rot"] = [_clean_float(value) for value in rotated] + if "init_local_pose" in config: + # Keeping two pose representations risks the stale local matrix + # overriding the rotated Euler pose in ObjectBaseCfg.from_dict. + del config["init_local_pose"] + + +def _planner_object( + config: Mapping[str, Any], + *, + source_uid: str, + role: str, +) -> dict[str, Any]: + description = str(config.get("description", "")).strip() + shape = deepcopy(dict(config.get("shape", {}))) + raw_attributes = config.get("attributes", {}) + if not isinstance(raw_attributes, Mapping): + raw_attributes = {} + raw_initial_state = config.get("initial_state", config.get("state", {})) + if not isinstance(raw_initial_state, Mapping): + raw_initial_state = {} + raw_affordances = config.get("affordances", config.get("capabilities", [])) + affordances = ( + [str(value) for value in raw_affordances] + if isinstance(raw_affordances, Sequence) + and not isinstance(raw_affordances, (str, bytes)) + else [] + ) + return { + "uid": str(config["uid"]), + "runtime_uid": str(config["uid"]), + "source_uid": source_uid, + "role": role, + "name": str(config.get("name", "")).strip(), + "description": description, + "shape": shape, + "init_pos": list(config["init_pos"]), + "init_rot": list(config["init_rot"]), + "body_scale": list(config.get("body_scale", [1.0, 1.0, 1.0])), + "category": config.get("category", config.get("object_category", "")), + "color": config.get("color", raw_attributes.get("color")), + "attributes": deepcopy(dict(raw_attributes)), + "initial_state": deepcopy(dict(raw_initial_state)), + "affordances": affordances, + } + + +def _runtime_object(config: Mapping[str, Any], *, role: str) -> dict[str, Any]: + if role == "articulation": + # Articulation schemas vary by asset; preserve their source fields after + # path and pose normalization instead of guessing a reduced schema. + result = deepcopy(dict(config)) + result.pop("description", None) + return result + + result = { + key: deepcopy(config[key]) + for key in ( + "uid", + "shape", + "init_pos", + "init_rot", + "body_scale", + ) + if key in config + } + result.setdefault("body_scale", [1.0, 1.0, 1.0]) + source_attrs = dict(config.get("attrs", {})) + if role == "background": + result["attrs"] = {**source_attrs, **_BACKGROUND_ATTRS} + result["body_type"] = "kinematic" + result["max_convex_hull_num"] = int(_BACKGROUND_POLICY["max_convex_hull_num"]) + else: + result["attrs"] = {**source_attrs, **_RIGID_ATTRS} + result["body_type"] = "dynamic" + hull_limit = int(_RIGID_POLICY["max_convex_hull_num"]) + max_hulls = max( + 1, + min(int(config.get("max_convex_hull_num", hull_limit)), hull_limit), + ) + result["max_convex_hull_num"] = max_hulls + result["acd_method"] = str(_RIGID_POLICY["acd_method"]) + shape = result.get("shape") + if isinstance(shape, dict): + shape["acd_method"] = str(_RIGID_POLICY["acd_method"]) + shape["max_convex_hull_num"] = max_hulls + return result + + +def _estimate_mesh_top_z(config: Mapping[str, Any]) -> float | None: + shape = config.get("shape", {}) + if not isinstance(shape, Mapping) or not shape.get("fpath"): + return None + try: + import numpy as np + import trimesh + from scipy.spatial.transform import Rotation + + loaded = trimesh.load(str(shape["fpath"]), force="scene") + geometry = ( + loaded.to_geometry() + if hasattr(loaded, "to_geometry") + else loaded.dump(concatenate=True) + ) + vertices = np.asarray(geometry.vertices, dtype=np.float64) + if vertices.size == 0: + return None + # DexSim converts glTF Y-up vertices to its Z-up basis at load time. + sim_vertices = np.column_stack( + (vertices[:, 0], -vertices[:, 2], vertices[:, 1]) + ) + sim_vertices *= np.asarray( + config.get("body_scale", [1.0, 1.0, 1.0]), dtype=np.float64 + ) + rotated = Rotation.from_euler( + "XYZ", config.get("init_rot", [0.0, 0.0, 0.0]), degrees=True + ).apply(sim_vertices) + rotated += np.asarray(config.get("init_pos", [0.0, 0.0, 0.0]), dtype=np.float64) + return float(rotated[:, 2].max()) + except Exception: + # Mesh bounds improve robot placement but are not needed to preserve the + # exported scene. The robot builder has a conservative tabletop fallback. + return None + + +def _vector3(value: Any) -> list[float]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)): + raise ValueError(f"Expected a finite xyz vector, got: {value!r}") + values = [float(item) for item in value] + if len(values) != 3 or not all(math.isfinite(item) for item in values): + raise ValueError(f"Expected a finite xyz vector, got: {value!r}") + return values + + +def _clean_float(value: float) -> float: + rounded = round(float(value), 12) + return 0.0 if abs(rounded) < 1e-12 else rounded diff --git a/embodichain/gen_sim/action_engine/generation/templates/default_lights.json b/embodichain/gen_sim/action_engine/generation/templates/default_lights.json new file mode 100644 index 000000000..5ea73ee5b --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/templates/default_lights.json @@ -0,0 +1,3 @@ +{ + "direct": [] +} diff --git a/embodichain/gen_sim/action_engine/generation/templates/default_sensors.json b/embodichain/gen_sim/action_engine/generation/templates/default_sensors.json new file mode 100644 index 000000000..f9ad7aea8 --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/templates/default_sensors.json @@ -0,0 +1,14 @@ +[ + { + "sensor_type": "Camera", + "width": 960, + "height": 540, + "intrinsics": [420, 420, 480, 270], + "extrinsics": { + "pos": [0.4, 0.0, 2.2], + "eye": [-0.6, 0.0, 1.8], + "target": [0.0, 0.0, 0.75], + "up": [1.0, 0.0, 0.0] + } + } +] diff --git a/embodichain/gen_sim/action_engine/generation/templates/dual_franka_robot.json b/embodichain/gen_sim/action_engine/generation/templates/dual_franka_robot.json new file mode 100644 index 000000000..d5daac633 --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/templates/dual_franka_robot.json @@ -0,0 +1,163 @@ +{ + "uid": "DualFrankaPanda", + "urdf_cfg": { + "fname": "dual_franka_dh_pgi_140_80_basket", + "name_case": { + "joint": "original", + "link": "original" + }, + "components": [ + { + "component_type": "left_arm", + "urdf_path": "Franka/Panda/Panda.urdf", + "transform": [ + [1.0, 0.0, 0.0, -1.25], + [0.0, 1.0, 0.0, 0.3], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "left_hand", + "urdf_path": "DH_PGI_140_80/DH_PGI_140_80.urdf", + "transform": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "right_arm", + "urdf_path": "Franka/Panda/Panda.urdf", + "transform": [ + [1.0, 0.0, 0.0, -1.25], + [0.0, 1.0, 0.0, -0.3], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "right_hand", + "urdf_path": "DH_PGI_140_80/DH_PGI_140_80.urdf", + "transform": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0] + ] + } + ] + }, + "init_pos": [-0.7, 0.0, 0.0], + "init_rot": [0.0, 0.0, 180.0], + "init_qpos": [ + 0.0, + 0.0, + -0.569, + -0.569, + 0.0, + 0.0, + -2.81, + -2.81, + 0.0, + 0.0, + 3.037, + 3.037, + 0.0, + 0.0, + + 0.0, + 0.0, + 0.0, + 0.0 + ], + "drive_pros": { + "stiffness": { + "left_arm": 10000.0, + "right_arm": 10000.0, + "left_eef": 1000.0, + "right_eef": 1000.0 + }, + "damping": { + "left_arm": 1000.0, + "right_arm": 1000.0, + "left_eef": 100.0, + "right_eef": 100.0 + }, + "max_effort": { + "left_arm": 10000.0, + "right_arm": 10000.0, + "left_eef": 10000.0, + "right_eef": 10000.0 + } + }, + "control_parts": { + "left_arm": [ + "left_fr3_joint1", + "left_fr3_joint2", + "left_fr3_joint3", + "left_fr3_joint4", + "left_fr3_joint5", + "left_fr3_joint6", + "left_fr3_joint7" + ], + "left_eef": ["left_gripper_finger1_joint_1"], + "right_arm": [ + "right_fr3_joint1", + "right_fr3_joint2", + "right_fr3_joint3", + "right_fr3_joint4", + "right_fr3_joint5", + "right_fr3_joint6", + "right_fr3_joint7" + ], + "right_eef": ["right_gripper_finger1_joint_1"], + "dual_arm": [ + "left_fr3_joint1", + "left_fr3_joint2", + "left_fr3_joint3", + "left_fr3_joint4", + "left_fr3_joint5", + "left_fr3_joint6", + "left_fr3_joint7", + "right_fr3_joint1", + "right_fr3_joint2", + "right_fr3_joint3", + "right_fr3_joint4", + "right_fr3_joint5", + "right_fr3_joint6", + "right_fr3_joint7" + ] + }, + "observation_joint_parts": ["left_eef", "right_eef"], + "qpos_control_part_order": ["dual_arm", "left_eef", "right_eef"], + "solver_cfg": { + "left_arm": { + "class_type": "PytorchSolver", + "urdf_path": null, + "end_link_name": "left_fr3_link8", + "root_link_name": "left_base", + "tcp": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.121], + [0.0, 0.0, 0.0, 1.0] + ], + "num_samples": 15 + }, + "right_arm": { + "class_type": "PytorchSolver", + "urdf_path": null, + "end_link_name": "right_fr3_link8", + "root_link_name": "right_base", + "tcp": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.121], + [0.0, 0.0, 0.0, 1.0] + ], + "num_samples": 15 + } + } +} diff --git a/embodichain/gen_sim/action_engine/generation/templates/dual_ur_robot.json b/embodichain/gen_sim/action_engine/generation/templates/dual_ur_robot.json new file mode 100644 index 000000000..a8472d25e --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/templates/dual_ur_robot.json @@ -0,0 +1,118 @@ +{ + "uid": "DualUR5", + "urdf_cfg": { + "fname": "dual_ur5_dh_pgi_140_80_basket", + "name_case": {"joint": "lower", "link": "lower"}, + "components": [ + { + "component_type": "left_arm", + "urdf_path": "UniversalRobots/UR5/UR5.urdf", + "transform": [ + [1.0, 0.0, 0.0, -1.45], + [0.0, 1.0, 0.0, -0.3], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "left_hand", + "urdf_path": "DH_PGI_140_80/DH_PGI_140_80.urdf", + "transform": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "right_arm", + "urdf_path": "UniversalRobots/UR5/UR5.urdf", + "transform": [ + [1.0, 0.0, 0.0, -1.45], + [0.0, 1.0, 0.0, 0.3], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "right_hand", + "urdf_path": "DH_PGI_140_80/DH_PGI_140_80.urdf", + "transform": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0] + ] + } + ] + }, + "init_pos": [2.0, 0.0, 0.0], + "init_rot": [0.0, 0.0, 0.0], + "init_qpos": [ + 0, 0, -1.57, -1.57, 1.57, 1.57, -1.57, -1.57, + -1.57, -1.57, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0 + ], + "drive_pros": { + "stiffness": { + "left_arm": 10000.0, + "right_arm": 10000.0, + "left_eef": 1000.0, + "right_eef": 1000.0 + }, + "damping": { + "left_arm": 1000.0, + "right_arm": 1000.0, + "left_eef": 100.0, + "right_eef": 100.0 + }, + "max_effort": { + "left_arm": 10000.0, + "right_arm": 10000.0, + "left_eef": 10000.0, + "right_eef": 10000.0 + } + }, + "control_parts": { + "left_arm": [ + "left_joint1", "left_joint2", "left_joint3", + "left_joint4", "left_joint5", "left_joint6" + ], + "left_eef": ["left_gripper_finger1_joint_1"], + "right_arm": [ + "right_joint1", "right_joint2", "right_joint3", + "right_joint4", "right_joint5", "right_joint6" + ], + "right_eef": ["right_gripper_finger1_joint_1"] + }, + "solver_cfg": { + "left_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "urdf_path": null, + "end_link_name": "left_ee_link", + "root_link_name": "left_base_link", + "ik_nearest_weight": [1.0, 4.0, 1.0, 1.0, 1.0, 1.0], + "tcp": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.121], + [0.0, 0.0, 0.0, 1.0] + ] + }, + "right_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "urdf_path": null, + "end_link_name": "right_ee_link", + "root_link_name": "right_base_link", + "ik_nearest_weight": [1.0, 4.0, 1.0, 1.0, 1.0, 1.0], + "tcp": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.121], + [0.0, 0.0, 0.0, 1.0] + ] + } + } +} diff --git a/embodichain/gen_sim/action_engine/generation/templates/robot_profiles.json b/embodichain/gen_sim/action_engine/generation/templates/robot_profiles.json new file mode 100644 index 000000000..b8759243e --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/templates/robot_profiles.json @@ -0,0 +1,37 @@ +{ + "dual_franka": { + "aliases": ["franka", "panda", "dual_panda", "dual_franka_panda"], + "template": "dual_franka_robot.json", + "robot_family": "franka", + "tabletop_clearance": 0.05, + "arm_component_z": 0.4, + "arm_base_x": -1.45 + }, + "dual_ur3": { + "aliases": ["ur3", "dual_ur3_dh_pgi", "dual_ur3_robotiq", "dual_ur3_robotiq_arg2f_140"], + "template": "dual_ur_robot.json", + "robot_family": "ur3", + "tabletop_clearance": 0.05, + "arm_component_z": 0.4, + "arm_base_x": -1.45, + "max_effort": 56.0 + }, + "dual_ur5": { + "aliases": ["ur5", "dual_ur5_dh_pgi", "dual_ur5_robotiq", "dual_ur5_robotiq_arg2f_140"], + "template": "dual_ur_robot.json", + "robot_family": "ur5", + "tabletop_clearance": 0.05, + "arm_component_z": 0.4, + "arm_base_x": -1.45, + "max_effort": 10000.0 + }, + "dual_ur10": { + "aliases": ["ur10", "dual_ur10_dh_pgi", "dual_ur10_robotiq", "dual_ur10_robotiq_arg2f_140"], + "template": "dual_ur_robot.json", + "robot_family": "ur10", + "tabletop_clearance": 0.05, + "arm_component_z": 0.3, + "arm_base_x": -1.1, + "max_effort": 330.0 + } +} diff --git a/embodichain/gen_sim/action_engine/generation/templates/vlm_sensors.json b/embodichain/gen_sim/action_engine/generation/templates/vlm_sensors.json new file mode 100644 index 000000000..34e964569 --- /dev/null +++ b/embodichain/gen_sim/action_engine/generation/templates/vlm_sensors.json @@ -0,0 +1,58 @@ +[ + { + "uid": "vlm_front", + "sensor_type": "Camera", + "width": 640, + "height": 480, + "intrinsics": [520, 520, 320, 240], + "enable_color": true, + "enable_depth": true, + "extrinsics": { + "eye": [-1.2, 0.0, 1.65], + "target": [0.0, 0.0, 0.75], + "up": [0.0, 0.0, 1.0] + } + }, + { + "uid": "vlm_left", + "sensor_type": "Camera", + "width": 640, + "height": 480, + "intrinsics": [520, 520, 320, 240], + "enable_color": true, + "enable_depth": true, + "extrinsics": { + "eye": [0.0, 1.2, 1.65], + "target": [0.0, 0.0, 0.75], + "up": [0.0, 0.0, 1.0] + } + }, + { + "uid": "vlm_rear", + "sensor_type": "Camera", + "width": 640, + "height": 480, + "intrinsics": [520, 520, 320, 240], + "enable_color": true, + "enable_depth": true, + "extrinsics": { + "eye": [1.2, 0.0, 1.65], + "target": [0.0, 0.0, 0.75], + "up": [0.0, 0.0, 1.0] + } + }, + { + "uid": "vlm_right", + "sensor_type": "Camera", + "width": 640, + "height": 480, + "intrinsics": [520, 520, 320, 240], + "enable_color": true, + "enable_depth": true, + "extrinsics": { + "eye": [0.0, -1.2, 1.65], + "target": [0.0, 0.0, 0.75], + "up": [0.0, 0.0, 1.0] + } + } +] diff --git a/embodichain/gen_sim/action_engine/graph_visualization.py b/embodichain/gen_sim/action_engine/graph_visualization.py new file mode 100644 index 000000000..4ad262c60 --- /dev/null +++ b/embodichain/gen_sim/action_engine/graph_visualization.py @@ -0,0 +1,938 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Headless PNG rendering for direct AtomicAction SeedGraphs. + +The renderer consumes the same validated coordinate-free v3 graph as runtime, +then builds an internal display view without grounding symbolic targets. E +TaskGroups remain the semantic grouping labels over the rendered action nodes. +Single chains use a folded timeline; DAGs use stable actor swimlanes. +""" + +from __future__ import annotations + +from collections import defaultdict +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from functools import lru_cache +from io import BytesIO +from math import hypot +from typing import Any + +import matplotlib + +# Select the non-interactive backend before importing any canvas primitives. +matplotlib.use("Agg", force=True) + +from matplotlib import patheffects +from matplotlib.backends.backend_agg import FigureCanvasAgg +from matplotlib.font_manager import FontProperties, fontManager +from matplotlib.figure import Figure +from matplotlib.patches import Circle, FancyArrowPatch, FancyBboxPatch +import networkx as nx + +from embodichain.gen_sim.action_engine.domain import ( + EXECUTION_PROGRAM_SCHEMA, + validate_execution_program, +) +from embodichain.gen_sim.action_engine.protocol import SEED_GRAPH_SCHEMA + +__all__ = ["render_seed_task_graph_png", "render_task_graph_png"] + +_PNG_SIGNATURE = b"\x89PNG\r\n\x1a\n" +_EXECUTION_KEYS = frozenset( + { + "schema_version", + "task", + "goal_description", + "start", + "goal", + "nodes", + "edges", + "semantic_steps", + "allocation_groups", + "motion_policy_version", + } +) + +_BACKGROUND = "#F8FAFB" +_INK = "#17212B" +_MUTED = "#66727D" +_BORDER = "#CBD4DC" +_LEFT = "#168A78" +_RIGHT = "#D97706" +_AUTO = "#59636D" +_COORDINATED = "#7652A5" +_DEPENDENCY = "#8A94A0" + +# The figures are designed at this display width in inches; every type size +# below is chosen to stay readable when the PNG is shown at exactly this size. +_TARGET_WIDTH = 8.0 +_DPI = 300 +_LEVEL_STEP = 1.15 +_NODE_RADIUS = 0.16 +_SPECIAL_NODE_RADIUS = 0.20 +_SUCCESS = "#25834B" +_FAILED = "#C43E3E" +_SKIPPED = "#8B949C" +_LANE_COLORS = { + "left": _LEFT, + "auto": _AUTO, + "right": _RIGHT, + "coordinated": _COORDINATED, +} +_LANE_BACKGROUNDS = { + "left": "#EAF6F3", + "auto": "#F0F3F5", + "right": "#FFF4E6", +} +_LANE_LABELS = { + "left": "LEFT ARM [L]", + "auto": "WORLD / AUTO / COORDINATED", + "right": "RIGHT ARM [R]", +} +_STATUS_COLORS = { + "success": _SUCCESS, + "executed": _SUCCESS, + "failed": _FAILED, + "aborted": _FAILED, + "skipped": _SKIPPED, +} +_STATUS_BADGES = { + "success": "OK", + "executed": "OK", + "failed": "FAIL", + "aborted": "ABORT", + "skipped": "SKIP", +} + + +@dataclass(frozen=True) +class _RuntimeOverlay: + """Execution annotations kept separate from the immutable seed program.""" + + edge_status: Mapping[str, str] + edge_arm: Mapping[str, str] + step_status: Mapping[str, str] + graph_status: str | None = None + + +@dataclass(frozen=True) +class _GraphData: + """Validated program plus indices shared by both layout strategies.""" + + program: Mapping[str, Any] + graph: nx.MultiDiGraph + node_by_id: Mapping[str, Mapping[str, Any]] + edge_by_id: Mapping[str, Mapping[str, Any]] + step_by_id: Mapping[str, Mapping[str, Any]] + lane_override: Mapping[str, str] + runtime: _RuntimeOverlay + + +def render_seed_task_graph_png(seed_graph: Mapping[str, Any]) -> bytes: + """Render a v3 SeedGraph or package-owned legacy program through Agg.""" + program = _display_program(seed_graph) + return _render(program, _RuntimeOverlay({}, {}, {})) + + +def render_task_graph_png(task_graph: Mapping[str, Any]) -> bytes: + """Render an execution program with optional runtime event annotations. + + A bare program is accepted. Runtime events may be stored in its ``runtime`` + envelope, or beside a nested ``execution_program``, ``program``, or + ``seed_task_graph``. A record alone is rejected because it omits topology. + """ + program = _extract_execution_program(task_graph) + runtime = _extract_runtime_overlay(task_graph) + return _render(program, runtime) + + +def _render( + program: Mapping[str, Any], + runtime: _RuntimeOverlay, +) -> bytes: + data = _graph_data(program, runtime) + if _is_single_chain(data): + return _render_chain(data) + return _render_dag(data) + + +def _extract_execution_program(document: Mapping[str, Any]) -> dict[str, Any]: + """Find and validate the execution program embedded in a display document.""" + if not isinstance(document, Mapping): + raise ValueError("Task graph visualization input must be a mapping.") + + if document.get("schema_version") in {EXECUTION_PROGRAM_SCHEMA, SEED_GRAPH_SCHEMA}: + # A runtime artifact may preserve the program fields and add annotations. + if document.get("schema_version") == SEED_GRAPH_SCHEMA: + candidate = dict(document) + candidate.pop("runtime", None) + candidate.pop("runtime_record", None) + return _display_program(candidate) + candidate = {key: document[key] for key in _EXECUTION_KEYS if key in document} + return validate_execution_program(candidate) + + for key in ("execution_program", "program", "seed_task_graph"): + candidate = document.get(key) + if isinstance(candidate, Mapping): + return _display_program(candidate) + + # Supporting a full program plus a runtime schema at the top level keeps + # visualization useful for simple JSON joins without weakening validation. + if {"nodes", "edges", "semantic_steps"}.issubset(document): + candidate = {key: document[key] for key in _EXECUTION_KEYS if key in document} + return validate_execution_program(candidate) + + raise ValueError( + "Runtime records do not contain graph topology. Provide the matching " + "ExecutionProgram under 'execution_program', 'program', or " + "'seed_task_graph'." + ) + + +def _display_program(value: Mapping[str, Any]) -> dict[str, Any]: + if value.get("schema_version") == SEED_GRAPH_SCHEMA: + from embodichain.gen_sim.action_engine.compiler import ( + seed_graph_to_execution_program, + ) + + return seed_graph_to_execution_program(value, require_executable=False) + return validate_execution_program(value) + + +def _extract_runtime_overlay(document: Mapping[str, Any]) -> _RuntimeOverlay: + """Reduce a runtime record to the small set of display-only annotations.""" + record = document.get("runtime") + if record is None: + record = document.get("runtime_record", document) + if not isinstance(record, Mapping): + raise ValueError("runtime_record must be a mapping.") + raw_events = record.get("events", document.get("events", [])) + if not isinstance(raw_events, Sequence) or isinstance( + raw_events, (str, bytes, bytearray) + ): + raise ValueError("Runtime events must be a list.") + + edge_status: dict[str, str] = {} + edge_arm: dict[str, str] = {} + step_status: dict[str, str] = {} + for index, event in enumerate(raw_events): + if not isinstance(event, Mapping): + raise ValueError(f"Runtime events[{index}] must be a mapping.") + event_kind = event.get("event") + status = _optional_text(event.get("status")) + if event_kind == "edge": + edge_id = _optional_text(event.get("edge_id")) + if edge_id and status: + edge_status[edge_id] = status.lower() + arm = _optional_text(event.get("arm")) + if edge_id and arm: + edge_arm[edge_id] = arm + elif event_kind == "semantic_step": + step_id = _optional_text(event.get("semantic_step_id")) + if step_id and status: + step_status[step_id] = status.lower() + + graph_status = _optional_text(record.get("status")) + return _RuntimeOverlay( + edge_status=edge_status, + edge_arm=edge_arm, + step_status=step_status, + graph_status=graph_status.lower() if graph_status else None, + ) + + +def _graph_data( + program: Mapping[str, Any], + runtime: _RuntimeOverlay, +) -> _GraphData: + node_by_id = {str(node["id"]): node for node in program["nodes"]} + edge_by_id = {str(edge["id"]): edge for edge in program["edges"]} + step_by_id = {str(step["id"]): step for step in program["semantic_steps"]} + graph = nx.MultiDiGraph() + graph.add_nodes_from(node_by_id) + for edge in program["edges"]: + source = str(edge["source"]) + target = str(edge["target"]) + graph.add_edge(source, target, edge_id=str(edge["id"])) + if not nx.is_directed_acyclic_graph(graph): + raise ValueError("ExecutionProgram node topology must be a directed DAG.") + + return _GraphData( + program=program, + graph=graph, + node_by_id=node_by_id, + edge_by_id=edge_by_id, + step_by_id=step_by_id, + lane_override=_allocation_lane_overrides(program), + runtime=runtime, + ) + + +def _allocation_lane_overrides( + program: Mapping[str, Any], +) -> dict[str, str]: + """Give auto actors stable lanes when a distinct-arm group is declared.""" + result: dict[str, str] = {} + for group in program.get("allocation_groups", []): + if group.get("arm_constraint") != "distinct_arms": + continue + members = group.get("semantic_step_ids", []) + for index, step_id in enumerate(members): + result[str(step_id)] = "left" if index % 2 == 0 else "right" + return result + + +def _is_single_chain(data: _GraphData) -> bool: + graph = data.graph + if graph.number_of_edges() != graph.number_of_nodes() - 1: + return False + if any(graph.in_degree(node) > 1 for node in graph): + return False + if any(graph.out_degree(node) > 1 for node in graph): + return False + return ( + graph.in_degree(str(data.program["start"])) == 0 + and graph.out_degree(str(data.program["goal"])) == 0 + and nx.is_weakly_connected(graph) + ) + + +def _ordered_chain_edges(data: _GraphData) -> list[Mapping[str, Any]]: + current = str(data.program["start"]) + result: list[Mapping[str, Any]] = [] + while current != str(data.program["goal"]): + outgoing = list(data.graph.out_edges(current, data=True)) + if len(outgoing) != 1: + raise ValueError("ExecutionProgram chain has an incomplete path.") + _, target, attrs = outgoing[0] + result.append(data.edge_by_id[str(attrs["edge_id"])]) + current = str(target) + if len(result) != len(data.edge_by_id): + raise ValueError("ExecutionProgram chain does not cover every edge.") + return result + + +def _render_chain(data: _GraphData) -> bytes: + """Render a long linear program as a bounded, folded state timeline.""" + edges = _ordered_chain_edges(data) + nodes = [str(data.program["start"])] + nodes.extend(str(edge["target"]) for edge in edges) + + slots_per_row = 4 + row_count = (len(nodes) + slots_per_row - 1) // slots_per_row + width = _TARGET_WIDTH + height = max(3.4, 2.0 + row_count * 1.55) + figure, axis = _new_figure(width, height) + try: + _draw_header(axis, data, width) + left, right, first_y = 0.7, width - 0.7, 2.05 + spacing = (right - left) / (slots_per_row - 1) + positions: dict[str, tuple[float, float]] = {} + for index, node_id in enumerate(nodes): + row, column = divmod(index, slots_per_row) + visual_column = column if row % 2 == 0 else slots_per_row - 1 - column + positions[node_id] = ( + left + visual_column * spacing, + first_y + row * 1.55, + ) + + for edge in edges: + source = positions[str(edge["source"])] + target = positions[str(edge["target"])] + lane = _edge_lane(edge, data) + color = _edge_color(str(edge["id"]), lane, data.runtime) + label_position, label_align = _edge_label_position(source, target, width) + _draw_labeled_edge( + axis, + source, + target, + color=color, + label=_edge_label(edge, data), + label_position=label_position, + label_align=label_align, + ) + + for index, node_id in enumerate(nodes): + _draw_state_node( + axis, + positions[node_id], + start=node_id == str(data.program["start"]), + goal=node_id == str(data.program["goal"]), + fork=False, + join=False, + index=index, + ) + + _draw_legend(axis, width, height - 0.28) + return _figure_png_bytes(figure) + finally: + figure.clear() + + +def _render_dag(data: _GraphData) -> bytes: + """Render forks and joins against persistent actor swimlanes.""" + levels = _dag_levels(data.graph) + maximum_level = max(levels.values(), default=0) + width = _TARGET_WIDTH + height = max(5.2, 2.15 + maximum_level * _LEVEL_STEP + 1.35) + figure, axis = _new_figure(width, height) + try: + _draw_header(axis, data, width) + boundaries, lane_centers = _lane_geometry(width) + _draw_swimlanes(axis, height, boundaries, lane_centers) + positions = _dag_positions(data, levels, lane_centers) + + # Dependency arrows are drawn first and stay visually subordinate to + # physical state transitions; only constraints not already implied by + # the state topology are shown. + for source_id, target_id in _visible_dependencies(data): + _draw_dependency_arrow( + axis, + positions[source_id], + positions[target_id], + ) + + pair_groups: defaultdict[tuple[str, str], list[str]] = defaultdict(list) + for edge in data.edge_by_id.values(): + pair_groups[(str(edge["source"]), str(edge["target"]))].append( + str(edge["id"]) + ) + for edge in data.edge_by_id.values(): + edge_id = str(edge["id"]) + source_id = str(edge["source"]) + target_id = str(edge["target"]) + lane = _edge_lane(edge, data) + parallel_ids = pair_groups[(source_id, target_id)] + parallel_index = parallel_ids.index(edge_id) + curvature = (parallel_index - (len(parallel_ids) - 1) / 2.0) * 0.20 + label_position, label_align = _edge_label_position( + positions[source_id], + positions[target_id], + width, + ) + _draw_labeled_edge( + axis, + positions[source_id], + positions[target_id], + color=_edge_color(edge_id, lane, data.runtime), + label=_edge_label(edge, data), + label_position=label_position, + label_align=label_align, + curvature=curvature, + ) + + for index, node_id in enumerate(nx.topological_sort(data.graph)): + _draw_state_node( + axis, + positions[str(node_id)], + start=str(node_id) == str(data.program["start"]), + goal=str(node_id) == str(data.program["goal"]), + fork=data.graph.out_degree(node_id) > 1, + join=data.graph.in_degree(node_id) > 1, + index=index, + ) + + _draw_legend(axis, width, height - 0.28) + return _figure_png_bytes(figure) + finally: + figure.clear() + + +def _lane_geometry( + width: float, +) -> tuple[dict[str, tuple[float, float]], dict[str, float]]: + """Even thirds for lane boundaries with derived actor centers.""" + margin = 0.35 + area = width - 2 * margin + first = margin + area / 3.0 + second = margin + 2 * area / 3.0 + boundaries = { + "left": (margin, first), + "auto": (first, second), + "right": (second, width - margin), + } + centers = {lane: (left + right) / 2.0 for lane, (left, right) in boundaries.items()} + return boundaries, centers + + +def _edge_label_position( + source: tuple[float, float], + target: tuple[float, float], + width: float, +) -> tuple[tuple[float, float], str]: + """Place halo labels beside arrows instead of boxing them on the edge.""" + midpoint = _midpoint(source, target) + dx = target[0] - source[0] + dy = target[1] - source[1] + if abs(dx) < 0.3: + # Keep vertical-arrow labels inside the canvas: right side on the left + # half of the figure, left side on the right half. + if midpoint[0] > width / 2.0: + return (midpoint[0] - 0.14, midpoint[1]), "right" + return (midpoint[0] + 0.14, midpoint[1]), "left" + length = hypot(dx, dy) or 1.0 + normal_x, normal_y = dy / length, -dx / length + if normal_x < 0: + normal_x, normal_y = -normal_x, -normal_y + if abs(normal_x) < 0.2 and normal_y > 0: + # Horizontal arrows keep their label above the line in both directions. + normal_x, normal_y = -normal_x, -normal_y + return ( + (midpoint[0] + normal_x * 0.16, midpoint[1] + normal_y * 0.16), + "center", + ) + + +def _visible_dependencies(data: _GraphData) -> list[tuple[str, str]]: + """Node anchors for dependencies not implied by state continuity.""" + result: list[tuple[str, str]] = [] + for prerequisite_id, dependent_id in _dependency_pairs(data): + source = str(data.edge_by_id[prerequisite_id]["target"]) + target = str(data.edge_by_id[dependent_id]["source"]) + if source == target or nx.has_path(data.graph, source, target): + continue + result.append((source, target)) + return result + + +def _dag_levels(graph: nx.MultiDiGraph) -> dict[str, int]: + """Assign the longest-path depth so dependencies always flow downward.""" + levels: dict[str, int] = {} + for node in nx.topological_sort(graph): + predecessors = list(graph.predecessors(node)) + levels[str(node)] = ( + max(levels[str(parent)] for parent in predecessors) + 1 + if predecessors + else 0 + ) + return levels + + +def _dag_positions( + data: _GraphData, + levels: Mapping[str, int], + lane_centers: Mapping[str, float], +) -> dict[str, tuple[float, float]]: + """Place branch nodes in actor lanes and structural fork/join nodes centrally.""" + base: dict[str, tuple[str, int]] = {} + for node_id in data.node_by_id: + incoming = list(data.graph.in_edges(node_id, data=True)) + outgoing = list(data.graph.out_edges(node_id, data=True)) + if ( + node_id in {str(data.program["start"]), str(data.program["goal"])} + or len(incoming) > 1 + or len(outgoing) > 1 + ): + lane = "auto" + elif incoming: + edge = data.edge_by_id[str(incoming[0][2]["edge_id"])] + lane = _edge_lane(edge, data) + elif outgoing: + edge = data.edge_by_id[str(outgoing[0][2]["edge_id"])] + lane = _edge_lane(edge, data) + else: + lane = "auto" + if lane == "coordinated": + lane = "auto" + base[node_id] = (lane, levels[node_id]) + + groups: defaultdict[tuple[str, int], list[str]] = defaultdict(list) + for node_id, lane_level in base.items(): + groups[lane_level].append(node_id) + + result: dict[str, tuple[float, float]] = {} + for (lane, level), node_ids in groups.items(): + ordered = sorted(node_ids) + center = lane_centers[lane] + # Small symmetric offsets prevent same-level nodes from hiding each + # other while keeping every node visibly inside its actor lane. + offsets = [ + (index - (len(ordered) - 1) / 2.0) * 0.55 for index in range(len(ordered)) + ] + for node_id, offset in zip(ordered, offsets, strict=True): + result[node_id] = (center + offset, 2.15 + level * _LEVEL_STEP) + return result + + +def _dependency_pairs(data: _GraphData) -> list[tuple[str, str]]: + """Return explicit edge dependencies plus missing semantic dependencies.""" + result: list[tuple[str, str]] = [] + seen: set[tuple[str, str]] = set() + for edge in data.edge_by_id.values(): + dependent_id = str(edge["id"]) + for prerequisite_id in edge.get("depends_on", []): + pair = (str(prerequisite_id), dependent_id) + if pair not in seen: + seen.add(pair) + result.append(pair) + + for step in data.step_by_id.values(): + dependent_edges = step.get("edge_ids", []) + if not dependent_edges: + continue + for prerequisite_step_id in step.get("depends_on", []): + prerequisite = data.step_by_id[str(prerequisite_step_id)] + pair = ( + str(prerequisite["edge_ids"][-1]), + str(dependent_edges[0]), + ) + if pair not in seen: + seen.add(pair) + result.append(pair) + return result + + +def _edge_lane(edge: Mapping[str, Any], data: _GraphData) -> str: + edge_id = str(edge["id"]) + observed_arm = data.runtime.edge_arm.get(edge_id) + if observed_arm: + return _arm_lane(observed_arm) + + action_lanes = { + _actor_lane(action.get("actor", {})) for action in edge.get("actions", []) + } + action_lanes.discard("auto") + if action_lanes == {"left"}: + return "left" + if action_lanes == {"right"}: + return "right" + if "coordinated" in action_lanes or action_lanes == {"left", "right"}: + return "coordinated" + return data.lane_override.get(str(edge["semantic_step_id"]), "auto") + + +def _actor_lane(actor: Any) -> str: + if not isinstance(actor, Mapping): + return "auto" + mode = str(actor.get("mode", "auto")).lower() + if mode == "required": + return _arm_lane(str(actor.get("arm", ""))) + if mode == "coordinated": + return "coordinated" + return "auto" + + +def _arm_lane(arm: str) -> str: + normalized = arm.strip().lower() + if "left" in normalized: + return "left" + if "right" in normalized: + return "right" + if normalized in {"both", "coordinated", "dual_arm", "dual"}: + return "coordinated" + return "auto" + + +def _edge_color( + edge_id: str, + lane: str, + runtime: _RuntimeOverlay, +) -> str: + status = runtime.edge_status.get(edge_id) + return _STATUS_COLORS.get(status or "", _LANE_COLORS[lane]) + + +def _edge_label(edge: Mapping[str, Any], data: _GraphData) -> str: + """One-line semantic phrase; execution details live in the JSON artifacts.""" + edge_id = str(edge["id"]) + step = data.step_by_id[str(edge["semantic_step_id"])] + status = data.runtime.edge_status.get(edge_id) or data.runtime.step_status.get( + str(step["id"]) + ) + status_badge = f" [{_STATUS_BADGES.get(status, status.upper())}]" if status else "" + return _clip(f"{step['operator']}: {step['object']}", 40) + status_badge + + +def _draw_header(axis: Any, data: _GraphData, width: float) -> None: + status = data.runtime.graph_status + status_text = f" [{status.upper()}]" if status else "" + axis.text( + 0.4, + 0.42, + _clip(f"ACTION ENGINE / {data.program['task']}{status_text}", 84), + ha="left", + va="center", + color=_INK, + fontproperties=_font(10.0, "bold"), + zorder=20, + ) + axis.text( + 0.4, + 0.80, + _clip(str(data.program["goal_description"]), 115), + ha="left", + va="top", + color=_MUTED, + fontproperties=_font(7.0), + linespacing=1.25, + zorder=20, + ) + axis.plot( + [0.4, width - 0.4], + [1.28, 1.28], + color=_BORDER, + linewidth=0.7, + zorder=19, + ) + + +def _draw_swimlanes( + axis: Any, + height: float, + boundaries: Mapping[str, tuple[float, float]], + centers: Mapping[str, float], +) -> None: + for lane in ("left", "auto", "right"): + left, right = boundaries[lane] + axis.add_patch( + FancyBboxPatch( + (left, 1.50), + right - left, + height - 2.0, + boxstyle="round,pad=0.0,rounding_size=0.05", + facecolor=_LANE_BACKGROUNDS[lane], + edgecolor=_BORDER, + linewidth=0.6, + zorder=-10, + ) + ) + axis.plot( + [left, right], + [1.50, 1.50], + color=_LANE_COLORS[lane], + linewidth=1.1, + zorder=-9, + ) + axis.text( + centers[lane], + 1.74, + _LANE_LABELS[lane], + ha="center", + va="center", + color=_LANE_COLORS[lane], + fontproperties=_font(6.8, "bold"), + zorder=10, + ) + + +def _draw_legend(axis: Any, width: float, y: float) -> None: + """Single-row edge-type legend; START/GOAL labels are self-explanatory.""" + entries = ( + ("left", "left action", False), + ("right", "right action", False), + ("coordinated", "coordinated", False), + ("auto", "auto / world", False), + ("dependency", "dependency", True), + ) + slot = 1.32 + start = (width - slot * len(entries)) / 2.0 + for index, (key, label, dashed) in enumerate(entries): + x = start + index * slot + color = _DEPENDENCY if dashed else _LANE_COLORS[key] + axis.add_patch( + FancyArrowPatch( + (x, y), + (x + 0.3, y), + arrowstyle="-|>", + mutation_scale=7, + color=color, + linewidth=1.0, + linestyle=(0, (3.0, 2.6)) if dashed else "-", + zorder=20, + ) + ) + axis.text( + x + 0.38, + y, + label, + ha="left", + va="center", + color=_MUTED, + fontproperties=_font(6.2), + zorder=20, + ) + + +def _draw_labeled_edge( + axis: Any, + source: tuple[float, float], + target: tuple[float, float], + *, + color: str, + label: str, + label_position: tuple[float, float], + label_align: str = "center", + curvature: float = 0.0, +) -> None: + """Draw one solid state transition and its halo-backed one-line label.""" + axis.add_patch( + FancyArrowPatch( + source, + target, + arrowstyle="-|>", + mutation_scale=9, + color=color, + linewidth=1.15, + shrinkA=12, + shrinkB=12, + connectionstyle=f"arc3,rad={curvature}", + zorder=3, + ) + ) + axis.text( + *label_position, + label, + ha=label_align, + va="center", + color=_INK, + fontproperties=_font(6.5), + path_effects=[patheffects.withStroke(linewidth=1.7, foreground=_BACKGROUND)], + zorder=8, + ) + + +def _draw_dependency_arrow( + axis: Any, + source: tuple[float, float], + target: tuple[float, float], +) -> None: + if source == target: + return + axis.add_patch( + FancyArrowPatch( + source, + target, + arrowstyle="-|>", + mutation_scale=7, + color=_DEPENDENCY, + linewidth=0.9, + linestyle=(0, (3.0, 2.6)), + shrinkA=12, + shrinkB=12, + connectionstyle="arc3,rad=-0.2", + alpha=0.9, + zorder=1, + ) + ) + + +def _draw_state_node( + axis: Any, + center: tuple[float, float], + *, + start: bool, + goal: bool, + fork: bool, + join: bool, + index: int, +) -> None: + fill = "#DDEFEA" if start else ("#E7F2DD" if goal else "#FFFFFF") + edge = _SUCCESS if goal else (_LEFT if start else _INK) + radius = _SPECIAL_NODE_RADIUS if (start or goal or fork or join) else _NODE_RADIUS + axis.add_patch( + Circle( + center, + radius=radius, + facecolor=fill, + edgecolor=edge, + linewidth=1.1, + zorder=12, + ) + ) + axis.text( + center[0], + center[1], + str(index), + ha="center", + va="center", + color=_INK, + fontproperties=_font(6.5, "bold"), + zorder=13, + ) + role = ( + "START" + if start + else ("GOAL" if goal else ("FORK" if fork else "JOIN" if join else "")) + ) + if role: + axis.text( + center[0], + center[1] + radius + 0.12, + role, + ha="center", + va="top", + color=edge, + fontproperties=_font(6.0, "bold"), + zorder=13, + ) + + +def _new_figure(width: float, height: float) -> tuple[Figure, Any]: + figure = Figure(figsize=(width, height), dpi=_DPI, facecolor=_BACKGROUND) + axis = figure.subplots() + axis.set_facecolor(_BACKGROUND) + axis.set_axis_off() + axis.set_xlim(0.0, width) + axis.set_ylim(height, 0.0) + return figure, axis + + +def _figure_png_bytes(figure: Figure) -> bytes: + buffer = BytesIO() + FigureCanvasAgg(figure).print_png(buffer) + payload = buffer.getvalue() + if not payload.startswith(_PNG_SIGNATURE): + raise RuntimeError("Matplotlib did not produce a valid PNG payload.") + return payload + + +@lru_cache(maxsize=1) +def _font_family() -> str: + """Prefer a CJK-capable font while retaining a portable fallback.""" + available = {font.name for font in fontManager.ttflist} + for family in ( + "Noto Sans CJK SC", + "Noto Sans CJK JP", + "Source Han Sans CN", + "WenQuanYi Micro Hei", + "Microsoft YaHei", + "Arial Unicode MS", + "DejaVu Sans", + ): + if family in available: + return family + return "sans-serif" + + +def _font(size: float, weight: str = "normal") -> FontProperties: + return FontProperties(family=_font_family(), size=size, weight=weight) + + +def _optional_text(value: Any) -> str | None: + return value.strip() if isinstance(value, str) and value.strip() else None + + +def _clip(value: str, length: int) -> str: + return value if len(value) <= length else f"{value[: max(1, length - 3)]}..." + + +def _midpoint( + first: tuple[float, float], + second: tuple[float, float], +) -> tuple[float, float]: + return ((first[0] + second[0]) / 2.0, (first[1] + second[1]) / 2.0) diff --git a/embodichain/gen_sim/action_engine/gripper_profiles.py b/embodichain/gen_sim/action_engine/gripper_profiles.py new file mode 100644 index 000000000..e26fa8c5c --- /dev/null +++ b/embodichain/gen_sim/action_engine/gripper_profiles.py @@ -0,0 +1,367 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Validated GenSim gripper assets, controls, TCPs, and grasp geometry.""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import Enum +from typing import Any + +__all__ = [ + "GraspModelSpec", + "GripperModel", + "GripperProfile", + "get_gripper_profile", +] + +_Side = str +_Transform = tuple[ + tuple[float, float, float, float], + tuple[float, float, float, float], + tuple[float, float, float, float], + tuple[float, float, float, float], +] + + +class GripperModel(str, Enum): + """Gripper models supported by the GenSim composition root.""" + + PGI = "pgi" + ROBOTIQ = "robotiq" + + +@dataclass(frozen=True, slots=True) +class GraspModelSpec: + """Collision geometry used to interpret sampled poses as gripper TCP poses.""" + + model_id: str + min_opening_width: float + max_opening_width: float + finger_length: float + finger_width: float + finger_thickness: float + palm_depth: float + opening_margin: float + + def as_mapping(self) -> dict[str, float | str]: + """Return a detached JSON-compatible geometry description.""" + return { + "model_id": self.model_id, + "min_opening_width": self.min_opening_width, + "max_opening_width": self.max_opening_width, + "finger_length": self.finger_length, + "finger_width": self.finger_width, + "finger_thickness": self.finger_thickness, + "palm_depth": self.palm_depth, + "opening_margin": self.opening_margin, + } + + +@dataclass(frozen=True, slots=True) +class GripperProfile: + """One indivisible simulator, controller, kinematics, and grasp contract. + + ``tcp_transform`` is the row-major homogeneous transform from each solver's + configured ``end_link_name`` frame to the tool center point. No quaternion + conversion is involved in this contract. + """ + + model: GripperModel + asset_path: str + assembly_name: str + tcp_transform: _Transform + left_control_joints: tuple[str, ...] + right_control_joints: tuple[str, ...] + left_state_joints: tuple[str, ...] + right_state_joints: tuple[str, ...] + left_mimic_joints: tuple[str, ...] + right_mimic_joints: tuple[str, ...] + mimic_multipliers: tuple[float, ...] + mimic_offsets: tuple[float, ...] + simulated_joint_initial_positions: tuple[float, ...] + open_positions: tuple[float, ...] + close_positions: tuple[float, ...] + control_limits: tuple[tuple[float, float], ...] + drive_stiffness: float + drive_damping: float + drive_max_effort: float + release_open_fraction_tolerance: float + grasp_model: GraspModelSpec + + def __post_init__(self) -> None: + control_count = len(self.left_control_joints) + if not control_count or len(self.right_control_joints) != control_count: + raise ValueError( + "Gripper profiles require matching non-empty hand controls." + ) + if not ( + len(self.open_positions) + == len(self.close_positions) + == len(self.control_limits) + == control_count + ): + raise ValueError( + "Gripper control states and limits must match control joints." + ) + if not 0.0 < self.release_open_fraction_tolerance <= 1.0: + raise ValueError("release_open_fraction_tolerance must be in (0, 1].") + mimic_count = len(self.left_mimic_joints) + if not ( + len(self.right_mimic_joints) + == len(self.mimic_multipliers) + == len(self.mimic_offsets) + == mimic_count + ): + raise ValueError("Gripper mimic metadata must have matching lengths.") + state_count = len(self.left_state_joints) + if not state_count or len(self.right_state_joints) != state_count: + raise ValueError( + "Gripper profiles require matching non-empty state joints." + ) + if len(self.simulated_joint_initial_positions) != len( + self.simulated_joint_names("left") + ): + raise ValueError( + "Gripper simulated initial positions must match physical joints." + ) + for side in ("left", "right"): + controls = set(self.control_joint_names(side)) + states = set(self.state_joint_names(side)) + if not states <= controls: + raise ValueError(f"Gripper {side} state joints must be control joints.") + overlap = states & set(self.mimic_joint_names(side)) + if overlap: + raise ValueError( + "Gripper state and mimic joints must be disjoint; " + f"{side} overlaps: {sorted(overlap)}." + ) + + def control_joint_names(self, side: _Side) -> tuple[str, ...]: + """Return the exact assembled control-joint names for one hand.""" + self._validate_side(side) + return self.left_control_joints if side == "left" else self.right_control_joints + + def mimic_joint_names(self, side: _Side) -> tuple[str, ...]: + """Return exact assembled mimic-joint names for one hand.""" + self._validate_side(side) + return self.left_mimic_joints if side == "left" else self.right_mimic_joints + + def state_joint_names(self, side: _Side) -> tuple[str, ...]: + """Return joints that define semantic open/closed state for one hand.""" + self._validate_side(side) + return self.left_state_joints if side == "left" else self.right_state_joints + + def state_joint_indices(self, side: _Side) -> tuple[int, ...]: + """Return semantic-state indices within one hand's command vector.""" + controls = self.control_joint_names(side) + return tuple(controls.index(name) for name in self.state_joint_names(side)) + + def simulated_joint_names(self, side: _Side) -> tuple[str, ...]: + """Return physical movable joints in assembled qpos order for one hand.""" + return tuple( + dict.fromkeys( + (*self.control_joint_names(side), *self.mimic_joint_names(side)) + ) + ) + + def runtime_manifest( + self, + *, + tcp_parent_frames: dict[str, str], + ) -> dict[str, Any]: + """Describe the selected physical and planning contract for diagnostics.""" + if set(tcp_parent_frames) != {"left", "right"} or not all( + isinstance(value, str) and value for value in tcp_parent_frames.values() + ): + raise ValueError( + "TCP parent frames require non-empty left and right links." + ) + return { + "model": self.model.value, + "asset_path": self.asset_path, + "control_joints": { + side: list(self.control_joint_names(side)) for side in ("left", "right") + }, + "state_joints": { + side: list(self.state_joint_names(side)) for side in ("left", "right") + }, + "mimic_joints": { + side: [ + { + "name": name, + "source": self.control_joint_names(side)[0], + "multiplier": self.mimic_multipliers[index], + "offset": self.mimic_offsets[index], + } + for index, name in enumerate(self.mimic_joint_names(side)) + ] + for side in ("left", "right") + }, + "open_positions": list(self.open_positions), + "close_positions": list(self.close_positions), + "control_limits": [list(limit) for limit in self.control_limits], + "release_open_fraction_tolerance": (self.release_open_fraction_tolerance), + "tcp": { + "parent_frames": dict(tcp_parent_frames), + "transform_direction": "parent_link_to_tcp", + "matrix_layout": "row_major_homogeneous_4x4", + "quaternion_order": "not_applicable", + "transform": [list(row) for row in self.tcp_transform], + }, + "grasp_model": self.grasp_model.as_mapping(), + } + + @staticmethod + def _validate_side(side: _Side) -> None: + if side not in {"left", "right"}: + raise ValueError("Gripper side must be 'left' or 'right'.") + + +_PGI_PROFILE = GripperProfile( + model=GripperModel.PGI, + asset_path="DH_PGI_140_80/DH_PGI_140_80.urdf", + assembly_name="dh_pgi_140_80", + tcp_transform=( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.121), + (0.0, 0.0, 0.0, 1.0), + ), + left_control_joints=("left_gripper_finger1_joint_1",), + right_control_joints=("right_gripper_finger1_joint_1",), + left_state_joints=("left_gripper_finger1_joint_1",), + right_state_joints=("right_gripper_finger1_joint_1",), + left_mimic_joints=("left_gripper_finger2_joint_1",), + right_mimic_joints=("right_gripper_finger2_joint_1",), + mimic_multipliers=(1.0,), + mimic_offsets=(0.0,), + simulated_joint_initial_positions=(0.0, 0.0), + open_positions=(0.0,), + close_positions=(0.04,), + control_limits=((0.0, 0.04),), + drive_stiffness=1.0e3, + drive_damping=1.0e2, + drive_max_effort=1.0e4, + release_open_fraction_tolerance=0.03, + grasp_model=GraspModelSpec( + model_id="dh_pgi_140_80", + min_opening_width=0.003, + max_opening_width=0.100, + finger_length=0.10, + finger_width=0.040, + finger_thickness=0.01, + palm_depth=0.096, + opening_margin=0.03, + ), +) + +_ROBOTIQ_MIMIC_MULTIPLIERS = (-1.0, 1.0, -1.0, -1.0, 1.0) +_ROBOTIQ_PROFILE = GripperProfile( + model=GripperModel.ROBOTIQ, + asset_path="Robotiq/robotiq_arg2f_140/robotiq_arg2f_140.urdf", + assembly_name="robotiq_arg2f_140", + tcp_transform=( + (0.0, -1.0, 0.0, 0.0), + (1.0, 0.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.2), + (0.0, 0.0, 0.0, 1.0), + ), + left_control_joints=( + "left_finger_joint", + "left_inner_knuckle_joint", + "left_inner_finger_joint", + "left_right_outer_knuckle_joint", + "left_right_inner_knuckle_joint", + "left_right_inner_finger_joint", + ), + right_control_joints=( + "right_finger_joint", + "right_left_inner_knuckle_joint", + "right_left_inner_finger_joint", + "right_outer_knuckle_joint", + "right_inner_knuckle_joint", + "right_inner_finger_joint", + ), + left_state_joints=("left_finger_joint",), + right_state_joints=("right_finger_joint",), + left_mimic_joints=( + "left_inner_knuckle_joint", + "left_inner_finger_joint", + "left_right_outer_knuckle_joint", + "left_right_inner_knuckle_joint", + "left_right_inner_finger_joint", + ), + right_mimic_joints=( + "right_left_inner_knuckle_joint", + "right_left_inner_finger_joint", + "right_outer_knuckle_joint", + "right_inner_knuckle_joint", + "right_inner_finger_joint", + ), + mimic_multipliers=_ROBOTIQ_MIMIC_MULTIPLIERS, + mimic_offsets=(0.0, 0.0, 0.0, 0.0, 0.0), + simulated_joint_initial_positions=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + open_positions=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + close_positions=(0.7, -0.7, 0.7, -0.7, -0.7, 0.7), + control_limits=( + (0.0, 0.7), + (-0.8757, 0.8757), + (-0.8757, 0.8757), + (-0.725, 0.725), + (-0.8757, 0.8757), + (-0.8757, 0.8757), + ), + drive_stiffness=50.0, + drive_damping=5.0, + drive_max_effort=500.0, + release_open_fraction_tolerance=0.03, + grasp_model=GraspModelSpec( + model_id="robotiq_arg2f_140", + min_opening_width=0.01, + max_opening_width=0.15, + finger_length=0.13, + finger_width=0.03, + finger_thickness=0.01, + palm_depth=0.08, + opening_margin=0.01, + ), +) + +_GRIPPER_PROFILES = { + GripperModel.PGI: _PGI_PROFILE, + GripperModel.ROBOTIQ: _ROBOTIQ_PROFILE, +} + + +def get_gripper_profile(model: GripperModel | str) -> GripperProfile: + """Return one strictly selected GenSim gripper profile.""" + if isinstance(model, GripperModel): + selected = model + elif isinstance(model, str): + try: + selected = GripperModel(model) + except ValueError as exc: + raise ValueError( + f"Unsupported gripper model {model!r}; expected one of: pgi, robotiq." + ) from exc + else: + raise TypeError( + f"Gripper model must be a string; expected one of: pgi, robotiq, got " + f"{type(model).__name__}." + ) + return _GRIPPER_PROFILES[selected] diff --git a/embodichain/gen_sim/action_engine/orientation.py b/embodichain/gen_sim/action_engine/orientation.py new file mode 100644 index 000000000..465384dcb --- /dev/null +++ b/embodichain/gen_sim/action_engine/orientation.py @@ -0,0 +1,243 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Compile task-facing orientation goals into a small runtime contract.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +import math +from typing import Any + +__all__ = [ + "AlignAxisConstraint", + "MatchRotationConstraint", + "OrientationConstraint", + "compile_orientation_constraint", +] + +_LONG_AXES = frozenset({"long", "long_axis", "longest"}) +_SCOPES = frozenset({"terminal"}) + + +@dataclass(frozen=True) +class AlignAxisConstraint: + """Require one local object axis to align with a target axis.""" + + local_axis: str + target_axis: str = "world_up" + directed: bool = True + tolerance: float | None = None + scope: str = "terminal" + + +@dataclass(frozen=True) +class MatchRotationConstraint: + """Require a complete object rotation relative to a captured reference.""" + + reference: str + equivalence: str = "none" + tolerance: float | None = None + scope: str = "terminal" + + +OrientationTerm = AlignAxisConstraint | MatchRotationConstraint + + +@dataclass(frozen=True) +class OrientationConstraint: + """Canonical hard constraints plus a separate planning preference.""" + + terms: tuple[OrientationTerm, ...] + planning_preference: str = "minimize_rotation_from_current" + + @property + def requires_reference(self) -> bool: + """Return whether execution must capture a step-start rotation.""" + return any( + isinstance(term, MatchRotationConstraint) and term.reference == "step_start" + for term in self.terms + ) + + @property + def allows_yaw_search(self) -> bool: + """Return whether hard constraints leave world-Z yaw unconstrained.""" + return all( + isinstance(term, AlignAxisConstraint) and term.target_axis == "world_up" + for term in self.terms + ) + + @property + def requires_upright_axis_alignment(self) -> bool: + """Return whether a hard term requires alignment with world up.""" + return bool(self.terms) and all( + isinstance(term, AlignAxisConstraint) and term.target_axis == "world_up" + for term in self.terms + ) + + +def compile_orientation_constraint( + goal: Mapping[str, Any], +) -> OrientationConstraint: + """Compile legacy goal enums or a composable serialized constraint. + + Existing persisted graphs continue to carry explicit ``orientation_goal`` + values. New tasks may omit the field, which intentionally means no hard + orientation constraint while retaining a minimum-rotation preference. + """ + serialized = goal.get("orientation_constraint") + if serialized is not None: + return _compile_serialized(serialized) + + orientation_goal = str(goal.get("orientation_goal", "none")) + if orientation_goal == "none": + terms: tuple[OrientationTerm, ...] = () + elif orientation_goal == "preserve": + terms = (MatchRotationConstraint(reference="step_start"),) + elif orientation_goal == "upright": + local_axis = str(goal.get("upright_local_axis", "long_axis")) + if local_axis == "auto": + local_axis = "long_axis" + directed = goal.get( + "orientation_directed", local_axis.lower() not in _LONG_AXES + ) + if not isinstance(directed, bool): + raise ValueError("orientation_directed must be a boolean.") + terms = ( + AlignAxisConstraint( + local_axis=local_axis, + target_axis="world_up", + directed=directed, + ), + ) + elif orientation_goal == "lay_flat": + terms = ( + AlignAxisConstraint( + local_axis="short_axis", + target_axis="world_up", + directed=False, + ), + ) + elif orientation_goal == "axis_align": + # axis_align explicitly requests a horizontal heading. Its established + # target-pose contract remains strict until it is replaced by a typed + # non-world-up axis term. + terms = (MatchRotationConstraint(reference="target_pose"),) + else: + raise ValueError(f"Unsupported orientation_goal {orientation_goal!r}.") + return OrientationConstraint(terms=terms) + + +def _compile_serialized(value: Any) -> OrientationConstraint: + if not isinstance(value, Mapping): + raise ValueError("orientation_constraint must be a mapping.") + unknown = set(value) - {"terms", "planning_preference"} + if unknown: + raise ValueError( + "orientation_constraint contains unsupported fields: " f"{sorted(unknown)}." + ) + raw_terms = value.get("terms", ()) + if not isinstance(raw_terms, Sequence) or isinstance( + raw_terms, (str, bytes, bytearray) + ): + raise ValueError("orientation_constraint.terms must be a list.") + terms = tuple(_compile_term(item, index) for index, item in enumerate(raw_terms)) + preference = str(value.get("planning_preference", "minimize_rotation_from_current")) + if preference not in {"minimize_rotation_from_current", "none"}: + raise ValueError( + "orientation_constraint.planning_preference must be " + "'minimize_rotation_from_current' or 'none'." + ) + return OrientationConstraint(terms=terms, planning_preference=preference) + + +def _compile_term(value: Any, index: int) -> OrientationTerm: + context = f"orientation_constraint.terms[{index}]" + if not isinstance(value, Mapping): + raise ValueError(f"{context} must be a mapping.") + kind = str(value.get("type", "")) + scope = str(value.get("scope", "terminal")) + if scope not in _SCOPES: + raise ValueError( + f"{context}.scope {scope!r} is unsupported by the current runtime." + ) + if kind == "align_axis": + unknown = set(value) - { + "type", + "local_axis", + "target_axis", + "directed", + "tolerance", + "scope", + } + if unknown: + raise ValueError( + f"{context} contains unsupported fields: {sorted(unknown)}." + ) + local_axis = str(value.get("local_axis", "")) + if local_axis not in {"x", "y", "z", "long_axis", "short_axis"}: + raise ValueError(f"{context}.local_axis {local_axis!r} is unsupported.") + target_axis = str(value.get("target_axis", "world_up")) + if target_axis != "world_up": + raise ValueError(f"{context}.target_axis {target_axis!r} is unsupported.") + directed = value.get("directed", True) + if not isinstance(directed, bool): + raise ValueError(f"{context}.directed must be a boolean.") + return AlignAxisConstraint( + local_axis=local_axis, + target_axis=target_axis, + directed=directed, + tolerance=_optional_tolerance(value, context), + scope=scope, + ) + if kind == "match_rotation": + unknown = set(value) - { + "type", + "reference", + "equivalence", + "tolerance", + "scope", + } + if unknown: + raise ValueError( + f"{context} contains unsupported fields: {sorted(unknown)}." + ) + reference = str(value.get("reference", "")) + if reference not in {"step_start", "target_pose"}: + raise ValueError(f"{context}.reference {reference!r} is unsupported.") + equivalence = str(value.get("equivalence", "none")) + if equivalence != "none": + raise ValueError(f"{context}.equivalence {equivalence!r} is unsupported.") + return MatchRotationConstraint( + reference=reference, + equivalence=equivalence, + tolerance=_optional_tolerance(value, context), + scope=scope, + ) + raise ValueError(f"{context}.type {kind!r} is unsupported.") + + +def _optional_tolerance(value: Mapping[str, Any], context: str) -> float | None: + raw = value.get("tolerance") + if raw is None: + return None + if isinstance(raw, bool) or not isinstance(raw, (int, float)): + raise ValueError(f"{context}.tolerance must be a finite positive number.") + tolerance = float(raw) + if not math.isfinite(tolerance) or tolerance <= 0.0: + raise ValueError(f"{context}.tolerance must be a finite positive number.") + return tolerance diff --git a/embodichain/gen_sim/action_engine/planning/__init__.py b/embodichain/gen_sim/action_engine/planning/__init__.py new file mode 100644 index 000000000..a02f8ce3b --- /dev/null +++ b/embodichain/gen_sim/action_engine/planning/__init__.py @@ -0,0 +1,62 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Stable route-free task planning API.""" + +from __future__ import annotations + +from .online import plan_online_seed_graph +from .dual import CandidatePair, plan_candidates_parallel +from .linker import ( + CONTRACT_LINKER_VERSION, + link_seed_graph, + link_task_dependencies, + validate_persisted_contracts, +) +from .planner import plan_task +from .selection import ( + CandidateEvaluation, + evaluate_candidate, + fuse_seed_graphs, + select_seed_graph, +) +from .vision import ( + CameraObservation, + SceneObservation, + analyze_visual_scene, + collect_scene_observation, + validate_visual_facts, +) + +__all__ = [ + "CONTRACT_LINKER_VERSION", + "CameraObservation", + "CandidatePair", + "CandidateEvaluation", + "SceneObservation", + "analyze_visual_scene", + "collect_scene_observation", + "evaluate_candidate", + "fuse_seed_graphs", + "link_seed_graph", + "link_task_dependencies", + "plan_online_seed_graph", + "plan_candidates_parallel", + "plan_task", + "select_seed_graph", + "validate_visual_facts", + "validate_persisted_contracts", +] diff --git a/embodichain/gen_sim/action_engine/planning/dual.py b/embodichain/gen_sim/action_engine/planning/dual.py new file mode 100644 index 000000000..d852464fd --- /dev/null +++ b/embodichain/gen_sim/action_engine/planning/dual.py @@ -0,0 +1,218 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Parallel offline/online candidate planning with isolated task views.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping +from concurrent.futures import ThreadPoolExecutor +from copy import deepcopy +from dataclasses import dataclass +from time import perf_counter +from typing import Any + +from embodichain.gen_sim.action_engine.capabilities import ( + AtomicCapabilityRegistry, + build_atomic_capability_registry, +) +from embodichain.gen_sim.action_engine.domain import ( + public_task_spec, + seed_graph_hash, + validate_seed_graph, + validate_task_spec, +) +from embodichain.gen_sim.action_engine.runtime.motion_policy import ( + resolve_motion_policy, +) + +from .linker import validate_persisted_contracts + +__all__ = ["CandidatePair", "plan_candidates_parallel"] + +CandidatePlanner = Callable[..., Mapping[str, Any]] + + +@dataclass(frozen=True) +class CandidatePair: + """Two independently planned graphs and branch-local planning metrics.""" + + offline: dict[str, Any] + online: dict[str, Any] + planning_metrics: dict[str, dict[str, Any]] + + +def plan_candidates_parallel( + task_spec: Mapping[str, Any], + *, + offline_planner: CandidatePlanner, + online_planner: CandidatePlanner, + known_objects: set[str] | None = None, + robot_profile: str = "dual_ur10", + registry: AtomicCapabilityRegistry | None = None, + require_executable: bool = False, +) -> CandidatePair: + """Plan both routes concurrently while hiding the oracle from online. + + Both returned graphs are validated against the same capability catalog and + motion-policy table before the pair is published. ``require_executable`` + is intentionally opt-in here: product planning may retain planning-only + candidates for inspection, while strict A/B execution enables the flag in + its final preflight. + """ + task = validate_task_spec(task_spec) + online_view = public_task_spec(task) + _reject_private_or_live_fields(online_view, "PublicTaskSpec") + capabilities = registry or build_atomic_capability_registry() + + def invoke(route: str) -> tuple[dict[str, Any], float]: + planner = offline_planner if route == "offline" else online_planner + # A planner is user/LLM supplied code. Give each route a detached + # copy so accidental mutation cannot change the other route's input or + # reintroduce private oracle fields after validation. + planner_input = deepcopy(task if route == "offline" else online_view) + started = perf_counter() + try: + result = planner(task_spec=planner_input) + except Exception as exc: + raise RuntimeError(f"{route} planner failed: {exc}") from exc + elapsed = perf_counter() - started + _reject_private_or_live_fields(result, f"{route} SeedGraph") + graph = validate_seed_graph( + result, + known_objects=known_objects, + known_actions=capabilities.names(), + executable_actions=capabilities.executable_names(), + require_executable=require_executable, + ) + if graph["planner_route"] != route: + raise ValueError( + f"{route} planner returned route {graph['planner_route']!r}." + ) + if graph["task_id"] != task["task_id"]: + raise ValueError(f"{route} planner returned a graph for another task.") + if graph["level"] != task["level"]: + raise ValueError(f"{route} planner returned a graph for another level.") + if graph["reasoning_type"] != task["reasoning_type"]: + raise ValueError( + f"{route} planner returned a graph with incompatible reasoning_type." + ) + if graph["capability_catalog_hash"] != capabilities.catalog_hash(): + raise ValueError( + f"{route} SeedGraph capability catalog does not match runtime." + ) + validate_persisted_contracts(graph, capabilities) + _validate_task_group_coverage(task, graph, route=route) + for node in graph["nodes"]: + capabilities.validate_binding(node) + if capabilities.get(str(node["atomic_action"])).runtime_available: + resolve_motion_policy( + robot_profile, + node["atomic_action"], + node["motion_policy"], + ) + return graph, elapsed + + with ThreadPoolExecutor( + max_workers=2, thread_name_prefix="action-engine-plan" + ) as pool: + futures = {route: pool.submit(invoke, route) for route in ("offline", "online")} + results: dict[str, tuple[dict[str, Any], float]] = {} + for route, future in futures.items(): + try: + results[route] = future.result() + except Exception as exc: + # Do not expose a bare Future exception; callers need to know + # which route invalidated the pair before any environment is + # allowed to move. + for other_route, other in futures.items(): + if other_route != route: + other.cancel() + raise RuntimeError( + f"A/B {route} planning/preflight failed: {exc}" + ) from exc + + metrics = { + route: { + "planning_seconds": elapsed, + "vlm_call_count": int(graph.get("metadata", {}).get("vlm_call_count", 0)), + "seed_graph_hash": seed_graph_hash(graph), + "node_count": len(graph["nodes"]), + "task_group_count": len(graph["task_groups"]), + } + for route, (graph, elapsed) in results.items() + } + return CandidatePair( + offline=results["offline"][0], + online=results["online"][0], + planning_metrics=metrics, + ) + + +def _validate_task_group_coverage( + task: Mapping[str, Any], graph: Mapping[str, Any], *, route: str +) -> None: + """Ensure every explicit L1-L3 task instance has one complete group.""" + if task.get("level") == "L4": + # L4's reference instances are intentionally hidden from the online + # route; the graph validator still enforces non-empty, coherent groups. + return + expected = { + str(item["id"]) + for item in task.get("task_instances", ()) + if isinstance(item, Mapping) + } + actual = {str(group["id"]) for group in graph.get("task_groups", ())} + missing = expected - actual + unexpected = actual - expected + if missing or unexpected: + raise ValueError( + f"{route} SeedGraph TaskGroup coverage mismatch; " + f"missing={sorted(missing)}, unexpected={sorted(unexpected)}." + ) + + +_PRIVATE_OR_LIVE_KEYS = frozenset( + { + "absolute_position", + "coordinates", + "grasp_pose", + "joint_positions", + "live_pose", + "live_transform", + "object_pose", + "oracle", + "pose", + "positions", + "qpos", + "target_pose", + "trajectory", + "waypoints", + "xpos", + } +) + + +def _reject_private_or_live_fields(value: Any, context: str) -> None: + """Reject private-oracle and grounded state fields in online inputs/outputs.""" + if isinstance(value, Mapping): + for key, child in value.items(): + if str(key).lower() in _PRIVATE_OR_LIVE_KEYS: + raise ValueError(f"{context} contains private/live field {key!r}.") + _reject_private_or_live_fields(child, f"{context}.{key}") + elif isinstance(value, (list, tuple)): + for index, child in enumerate(value): + _reject_private_or_live_fields(child, f"{context}[{index}]") diff --git a/embodichain/gen_sim/action_engine/planning/linker.py b/embodichain/gen_sim/action_engine/planning/linker.py new file mode 100644 index 000000000..6a391ae0f --- /dev/null +++ b/embodichain/gen_sim/action_engine/planning/linker.py @@ -0,0 +1,1023 @@ +# ---------------------------------------------------------------------------- +# 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 causal and resource linking for SeedGraph v3.""" + +from __future__ import annotations + +from collections.abc import Collection, Mapping, Sequence +from copy import deepcopy +import hashlib +import json +from typing import Any + +from embodichain.gen_sim.action_engine.capabilities import ( + AtomicCapabilityRegistry, + build_atomic_capability_registry, +) +from embodichain.gen_sim.action_engine.domain import ( + task_contract, + validate_seed_graph, + validate_task_spec, +) +from embodichain.gen_sim.action_engine.protocol import SEED_GRAPH_SCHEMA + +__all__ = [ + "CONTRACT_LINKER_VERSION", + "link_seed_graph", + "link_task_dependencies", + "validate_persisted_contracts", +] + +CONTRACT_LINKER_VERSION = "action_contract_linker_v2" +_INITIAL_PREDICATES = frozenset({"arm_free", "object_free"}) +_REFERENCE_KEYS = frozenset( + { + "anchor", + "container", + "reference", + "reference_object", + "support", + "support_object", + "target", + "target_object", + } +) + + +def link_task_dependencies( + task_spec: Mapping[str, Any], + role_bindings: Mapping[str, str], + *, + registry: AtomicCapabilityRegistry | None = None, +) -> dict[str, Any]: + """Add the minimal stable TaskGroup dependencies implied by contracts.""" + del registry # Reserved for task-level capability specialization. + task = validate_task_spec(task_spec) + bindings = {str(key): str(value) for key, value in role_bindings.items()} + bindings_hash = hashlib.sha256( + json.dumps( + bindings, sort_keys=True, separators=(",", ":"), allow_nan=False + ).encode("utf-8") + ).hexdigest() + existing_metadata = task.get("metadata", {}) + existing_linker = ( + existing_metadata.get("action_contract_task_linker", {}) + if isinstance(existing_metadata, Mapping) + else {} + ) + if ( + isinstance(existing_linker, Mapping) + and existing_linker.get("version") == CONTRACT_LINKER_VERSION + and existing_linker.get("role_bindings_hash") == bindings_hash + ): + return task + instances = task["task_instances"] + order = [str(item["id"]) for item in instances] + dependencies = { + str(item["id"]): set(str(value) for value in item["depends_on"]) + for item in instances + } + dependency_order = { + str(item["id"]): [str(value) for value in item["depends_on"]] + for item in instances + } + claims = {str(item["id"]): _task_claims(item, bindings) for item in instances} + distinct_arm_pairs = _distinct_arm_pairs(task.get("metadata", {})) + linked: list[dict[str, str]] = [] + + latest_by_object: dict[str, str] = {} + for instance in instances: + instance_id = str(instance["id"]) + primary = _task_primary_object(instance, bindings) + previous = latest_by_object.get(primary) + if ( + previous is not None + and previous not in dependencies[instance_id] + and not _reaches(dependencies, previous, instance_id) + ): + dependencies[instance_id].add(previous) + dependency_order[instance_id].append(previous) + linked.append( + { + "from": previous, + "to": instance_id, + "reason": "causal", + "detail": f"object_flow:{primary}", + } + ) + _assert_acyclic(dependencies, "TaskSpec causal linking") + latest_by_object[primary] = instance_id + + for later_index, later_id in enumerate(order): + for earlier_id in order[:later_index]: + if _reaches(dependencies, later_id, earlier_id) or _reaches( + dependencies, earlier_id, later_id + ): + continue + conflicts = _claim_conflicts(claims[earlier_id], claims[later_id]) + if frozenset({earlier_id, later_id}) in distinct_arm_pairs: + conflicts = [item for item in conflicts if item != "arm:auto"] + if not conflicts: + continue + dependencies[later_id].add(earlier_id) + dependency_order[later_id].append(earlier_id) + linked.append( + { + "from": earlier_id, + "to": later_id, + "reason": "resource", + "detail": ",".join(conflicts), + } + ) + _assert_acyclic(dependencies, "TaskSpec contract linking") + + for instance in instances: + instance_id = str(instance["id"]) + instance["depends_on"] = dependency_order[instance_id] + metadata = dict(task.get("metadata", {})) + metadata["action_contract_task_linker"] = { + "version": CONTRACT_LINKER_VERSION, + "role_bindings_hash": bindings_hash, + "linked_dependencies": linked, + } + task["metadata"] = metadata + return validate_task_spec(task) + + +def link_seed_graph( + draft: Mapping[str, Any], + *, + registry: AtomicCapabilityRegistry | None = None, + task_order: Sequence[str] = (), + completed_nodes: Collection[str] = (), + known_objects: Collection[str] | None = None, +) -> dict[str, Any]: + """Resolve contracts, link a draft graph, and return validated SeedGraph v3.""" + if not isinstance(draft, Mapping): + raise TypeError("SeedGraph draft must be a mapping.") + if draft.get("schema_version") != SEED_GRAPH_SCHEMA: + raise ValueError(f"Contract linker accepts only {SEED_GRAPH_SCHEMA!r} drafts.") + capabilities = registry or build_atomic_capability_registry() + graph = deepcopy(dict(draft)) + nodes = graph.get("nodes") + groups = graph.get("task_groups") + if not isinstance(nodes, list) or not nodes: + raise ValueError("SeedGraph draft nodes must be a non-empty list.") + if not isinstance(groups, list) or not groups: + raise ValueError("SeedGraph draft task_groups must be a non-empty list.") + + already_linked = _already_linked(graph) + for index, node in enumerate(nodes): + if not isinstance(node, dict): + raise TypeError(f"SeedGraph draft node {index} must be a mapping.") + action = str(node.get("atomic_action", "")) + expected = capabilities.get(action).resolve_contract(node).as_mapping() + persisted = node.get("contract") + if persisted is not None and persisted != expected: + raise ValueError( + f"SeedGraph node {node.get('id')!r} persisted Action Contract " + "does not match the current capability resolver." + ) + node["contract"] = expected + node.pop("resources", None) + if already_linked: + linked = validate_seed_graph( + graph, + known_objects=known_objects, + known_actions=capabilities.names(), + ) + validate_persisted_contracts(linked, capabilities) + return linked + + completed = {str(item) for item in completed_nodes} + node_by_id = _unique_by_id(nodes, "SeedGraph draft nodes") + group_by_id = _unique_by_id(groups, "SeedGraph draft task_groups") + ordered_groups = _ordered_group_ids(groups, task_order) + node_reasons: list[dict[str, str]] = [] + group_reasons: list[dict[str, str]] = [] + + for group in groups: + group_id = str(group.get("id", "")) + node_ids = [str(item) for item in group.get("node_ids", ())] + if not node_ids or any(node_id not in node_by_id for node_id in node_ids): + raise ValueError( + f"SeedGraph TaskGroup {group_id!r} has missing or unknown node IDs." + ) + _link_internal_nodes( + node_ids, + node_by_id, + completed=completed, + reasons=node_reasons, + ) + _validate_internal_symbolic_state(node_ids, node_by_id) + group.pop("contract", None) + + group_dependencies = { + group_id: set(str(item) for item in group_by_id[group_id].get("depends_on", ())) + for group_id in ordered_groups + } + original_group_dependencies = { + group_id: [str(item) for item in group_by_id[group_id].get("depends_on", ())] + for group_id in ordered_groups + } + _assert_acyclic(group_dependencies, "SeedGraph TaskGroups") + summaries = { + group_id: _summarize_group(group_by_id[group_id], node_by_id) + for group_id in ordered_groups + } + distinct_arm_pairs = _distinct_arm_pairs(graph.get("metadata", {})) + + for later_index, later_id in enumerate(ordered_groups): + for earlier_id in ordered_groups[:later_index]: + if _reaches(group_dependencies, later_id, earlier_id) or _reaches( + group_dependencies, earlier_id, later_id + ): + continue + conflicts = _claim_conflicts( + summaries[earlier_id]["claims"], summaries[later_id]["claims"] + ) + if frozenset({earlier_id, later_id}) in distinct_arm_pairs: + conflicts = [item for item in conflicts if item != "arm:auto"] + if conflicts: + _add_group_dependency( + earlier_id, + later_id, + group_dependencies, + completed, + summaries, + group_reasons, + reason="resource", + detail=",".join(conflicts), + ) + + for later_index, group_id in enumerate(ordered_groups): + for requirement in summaries[group_id]["entry_requires"]: + if requirement["predicate"] in _INITIAL_PREDICATES: + continue + candidates = [ + candidate + for candidate in ordered_groups[:later_index] + if _adds_atom(summaries[candidate]["exit_effects"], requirement) + ] + if not candidates: + raise ValueError( + f"SeedGraph TaskGroup {group_id!r} has no producer for state " + f"{requirement}." + ) + maximal = [ + candidate + for candidate in candidates + if not any( + candidate != other + and _reaches(group_dependencies, other, candidate) + for other in candidates + ) + ] + if len(maximal) != 1: + raise ValueError( + f"SeedGraph TaskGroup {group_id!r} has multiple unordered " + f"producers for state {requirement}: {maximal}." + ) + producer = maximal[0] + if not _reaches(group_dependencies, group_id, producer): + _add_group_dependency( + producer, + group_id, + group_dependencies, + completed, + summaries, + group_reasons, + reason="causal", + detail=_atom_key(requirement), + ) + + _assert_acyclic(group_dependencies, "SeedGraph contract linking") + for group_id in ordered_groups: + group = group_by_id[group_id] + group["depends_on"] = original_group_dependencies[group_id] + [ + candidate + for candidate in ordered_groups + if candidate in group_dependencies[group_id] + and candidate not in original_group_dependencies[group_id] + ] + + _link_group_boundaries( + ordered_groups, + group_dependencies, + summaries, + node_by_id, + completed, + node_reasons, + ) + for group_id in ordered_groups: + summaries[group_id] = _summarize_group(group_by_id[group_id], node_by_id) + group_by_id[group_id]["contract"] = summaries[group_id] + + _validate_symbolic_state(ordered_groups, group_dependencies, summaries, group_by_id) + metadata = dict(graph.get("metadata", {})) + metadata["action_contract_linker"] = { + "version": CONTRACT_LINKER_VERSION, + "group_dependencies": _sorted_reasons(group_reasons), + "node_dependencies": _sorted_reasons(node_reasons), + } + graph["metadata"] = metadata + graph["schema_version"] = SEED_GRAPH_SCHEMA + graph["nodes"] = nodes + graph["task_groups"] = groups + return validate_seed_graph( + graph, + known_objects=known_objects, + known_actions=capabilities.names(), + ) + + +def validate_persisted_contracts( + graph: Mapping[str, Any], registry: AtomicCapabilityRegistry +) -> None: + """Reject persisted contracts that differ from the active capability catalog.""" + metadata = graph.get("metadata", {}) + linker = ( + metadata.get("action_contract_linker", {}) + if isinstance(metadata, Mapping) + else {} + ) + if ( + not isinstance(linker, Mapping) + or linker.get("version") != CONTRACT_LINKER_VERSION + ): + raise ValueError( + "SeedGraph was not produced by the current deterministic Contract Linker; " + "regenerate the configuration bundle." + ) + for node in graph.get("nodes", ()): + expected = ( + registry.get(str(node["atomic_action"])).resolve_contract(node).as_mapping() + ) + if node.get("contract") != expected: + raise ValueError( + f"SeedGraph node {node.get('id')!r} persisted Action Contract " + "does not match the current capability resolver." + ) + node_by_id = { + str(node["id"]): node + for node in graph.get("nodes", ()) + if isinstance(node, Mapping) and "id" in node + } + for group in graph.get("task_groups", ()): + expected = _summarize_group(group, node_by_id) + if group.get("contract") != expected: + raise ValueError( + f"SeedGraph TaskGroup {group.get('id')!r} persisted contract " + "does not match its linked AtomicAction topology." + ) + + +def _task_claims( + instance: Mapping[str, Any], bindings: Mapping[str, str] +) -> list[dict[str, str]]: + task_type = str(instance["task_type"]) + contract = task_contract(task_type) + params = _resolve_roles(instance.get("params", {}), bindings) + primary_key = contract.primary_role_field + primary = params.get(primary_key) + claims: list[dict[str, str]] = [] + if isinstance(primary, str) and primary: + claims.append(_claim(f"object:{primary}", "exclusive")) + target = params.get("target_role") + if isinstance(target, str) and target and target != primary: + claims.append(_claim(f"object:{target}", "shared_read")) + payloads = params.get("payload_roles", []) + if isinstance(payloads, Sequence) and not isinstance( + payloads, (str, bytes, bytearray) + ): + for payload in payloads: + if isinstance(payload, str) and payload and payload != primary: + claims.append(_claim(f"object:{payload}", "exclusive")) + if contract.resource_mode == "handover": + transfer = str(params.get("transfer_arm", "")) + receive = str(params.get("receive_arm", "")) + if transfer not in {"left_arm", "right_arm"} or receive not in { + "left_arm", + "right_arm", + }: + raise ValueError( + "Handover resource mode requires explicit transfer/receive arms." + ) + if transfer == receive: + raise ValueError("Handover transfer_arm and receive_arm must be distinct.") + claims.extend((_claim(f"arm:{transfer}"), _claim(f"arm:{receive}"))) + elif contract.resource_mode == "coordinated": + claims.extend((_claim("arm:left_arm"), _claim("arm:right_arm"))) + elif contract.resource_mode == "single_arm": + required_arm = params.get("required_arm") + if required_arm in {"left_arm", "right_arm"}: + claims.append(_claim(f"arm:{required_arm}")) + else: + claims.append(_claim("arm:auto")) + else: + raise ValueError( + f"TaskGroup {instance.get('id')!r} has unsupported resource mode " + f"{contract.resource_mode!r}." + ) + return _merge_claims(claims) + + +def _task_primary_object( + instance: Mapping[str, Any], bindings: Mapping[str, str] +) -> str: + task_type = str(instance["task_type"]) + contract = task_contract(task_type) + params = _resolve_roles(instance.get("params", {}), bindings) + key = contract.primary_role_field + value = params.get(key) + if not isinstance(value, str) or not value: + raise ValueError( + f"TaskGroup {instance.get('id')!r} requires a resolved {key!r}." + ) + return value + + +def _link_internal_nodes( + node_ids: Sequence[str], + node_by_id: Mapping[str, dict[str, Any]], + *, + completed: set[str], + reasons: list[dict[str, str]], +) -> None: + positions = {node_id: index for index, node_id in enumerate(node_ids)} + for later_index, later_id in enumerate(node_ids): + later = node_by_id[later_id] + for requirement in later["contract"]["requires"]: + producers = [ + earlier_id + for earlier_id in node_ids[:later_index] + if node_by_id[earlier_id]["contract"]["failure_policy"] != "best_effort" + if _adds_atom( + node_by_id[earlier_id]["contract"]["effects"], requirement + ) + ] + if producers: + _add_node_dependency( + producers[-1], + later_id, + node_by_id, + completed, + reasons, + "causal", + _atom_key(requirement), + ) + for earlier_id in node_ids[:later_index]: + earlier = node_by_id[earlier_id] + if earlier.get("sync_group") is not None and earlier.get( + "sync_group" + ) == later.get("sync_group"): + continue + if _node_reaches(node_by_id, later_id, earlier_id) or _node_reaches( + node_by_id, earlier_id, later_id + ): + continue + conflicts = _claim_conflicts( + earlier["contract"]["claims"], later["contract"]["claims"] + ) + if conflicts: + _add_node_dependency( + earlier_id, + later_id, + node_by_id, + completed, + reasons, + "resource", + ",".join(conflicts), + ) + dependencies = { + node_id: { + str(parent) + for parent in node_by_id[node_id].get("depends_on", ()) + if str(parent) in positions + } + for node_id in node_ids + } + _assert_acyclic(dependencies, "AtomicAction contract linking") + + +def _summarize_group( + group: Mapping[str, Any], node_by_id: Mapping[str, Mapping[str, Any]] +) -> dict[str, Any]: + node_ids = [str(item) for item in group["node_ids"]] + node_set = set(node_ids) + entries = [ + node_id + for node_id in node_ids + if not any( + str(parent) in node_set for parent in node_by_id[node_id]["depends_on"] + ) + ] + depended = { + str(parent) + for node_id in node_ids + for parent in node_by_id[node_id]["depends_on"] + if str(parent) in node_set + } + terminals = [node_id for node_id in node_ids if node_id not in depended] + entry_requires: list[dict[str, str]] = [] + for node_id in node_ids: + node = node_by_id[node_id] + for requirement in node["contract"]["requires"]: + if any( + producer_id in node_set + and _node_reaches(node_by_id, node_id, producer_id) + and _adds_atom( + node_by_id[producer_id]["contract"]["effects"], requirement + ) + for producer_id in node_ids + ): + continue + if requirement not in entry_requires: + entry_requires.append(deepcopy(requirement)) + + last_effect: dict[str, dict[str, Any]] = {} + effect_order: list[str] = [] + for node_id in node_ids: + if node_by_id[node_id]["contract"]["failure_policy"] == "best_effort": + continue + for effect in node_by_id[node_id]["contract"]["effects"]: + key = _atom_key(effect["atom"]) + if key not in last_effect: + effect_order.append(key) + last_effect[key] = deepcopy(effect) + claims = [ + deepcopy(claim) + for node_id in node_ids + for claim in node_by_id[node_id]["contract"]["claims"] + ] + claims.extend(_goal_read_claims(group.get("goal", {}))) + merged_claims = _merge_claims(claims) + free_resources = { + ( + f"arm:{effect['atom']['arm']}" + if effect["atom"]["predicate"] == "arm_free" + else f"object:{effect['atom']['object_uid']}" + ) + for effect in last_effect.values() + if effect["op"] == "add" + and effect["atom"]["predicate"] in {"arm_free", "object_free"} + } + for claim in merged_claims: + if claim["resource"] in free_resources: + claim["lifetime"] = "action" + completion = ( + "terminal_barrier" + if terminals + and all( + node_by_id[node_id]["contract"]["completion"] == "terminal_barrier" + for node_id in terminals + ) + else "ordinary" + ) + return { + "entry_requires": entry_requires, + "exit_effects": [last_effect[key] for key in effect_order], + "claims": merged_claims, + "entry_node_ids": entries, + "terminal_node_ids": terminals, + "completion": completion, + } + + +def _validate_internal_symbolic_state( + node_ids: Sequence[str], node_by_id: Mapping[str, Mapping[str, Any]] +) -> None: + node_set = set(node_ids) + dependencies = { + node_id: { + str(parent) + for parent in node_by_id[node_id].get("depends_on", ()) + if str(parent) in node_set + } + for node_id in node_ids + } + entry_atoms = set() + for node_id in node_ids: + for requirement in node_by_id[node_id]["contract"]["requires"]: + has_prior_producer = any( + producer_id != node_id + and _node_reaches(node_by_id, node_id, producer_id) + and _adds_atom( + node_by_id[producer_id]["contract"]["effects"], requirement + ) + for producer_id in node_ids + ) + if not has_prior_producer: + entry_atoms.add(_atom_key(requirement)) + state = set(entry_atoms) + for node_id in _stable_topological(node_ids, dependencies): + contract = node_by_id[node_id]["contract"] + for requirement in contract["requires"]: + if _atom_key(requirement) not in state: + raise ValueError( + f"SeedGraph node {node_id!r} requires unavailable state " + f"{requirement}." + ) + for effect in contract["effects"]: + key = _atom_key(effect["atom"]) + if effect["op"] == "delete": + if key not in state: + raise ValueError( + f"SeedGraph node {node_id!r} deletes unavailable state " + f"{effect['atom']}." + ) + state.remove(key) + else: + state.add(key) + + +def _add_group_dependency( + parent: str, + child: str, + dependencies: dict[str, set[str]], + completed: set[str], + summaries: Mapping[str, Mapping[str, Any]], + reasons: list[dict[str, str]], + *, + reason: str, + detail: str, +) -> None: + if any(node_id in completed for node_id in summaries[child]["entry_node_ids"]): + raise ValueError( + f"Contract linking cannot add dependency into completed TaskGroup {child!r}." + ) + dependencies[child].add(parent) + _assert_acyclic(dependencies, "SeedGraph contract linking") + reasons.append({"from": parent, "to": child, "reason": reason, "detail": detail}) + + +def _link_group_boundaries( + ordered_groups: Sequence[str], + dependencies: Mapping[str, set[str]], + summaries: Mapping[str, Mapping[str, Any]], + node_by_id: Mapping[str, dict[str, Any]], + completed: set[str], + reasons: list[dict[str, str]], +) -> None: + for child in ordered_groups: + for parent in ordered_groups: + if parent not in dependencies[child]: + continue + for child_node in summaries[child]["entry_node_ids"]: + for parent_node in summaries[parent]["terminal_node_ids"]: + _add_node_dependency( + parent_node, + child_node, + node_by_id, + completed, + reasons, + "cleanup", + f"TaskGroup {parent} terminal barrier", + ) + + +def _add_node_dependency( + parent: str, + child: str, + node_by_id: Mapping[str, dict[str, Any]], + completed: set[str], + reasons: list[dict[str, str]], + reason: str, + detail: str, +) -> None: + if parent == child: + raise ValueError(f"Contract linker cannot add self-dependency {child!r}.") + dependencies = node_by_id[child].setdefault("depends_on", []) + if parent in dependencies or _node_reaches(node_by_id, child, parent): + return + if _node_reaches(node_by_id, parent, child): + raise ValueError( + f"Contract dependency {parent!r} -> {child!r} would create a cycle." + ) + if child in completed: + raise ValueError(f"Contract linker cannot modify completed node {child!r}.") + dependencies.append(parent) + reasons.append({"from": parent, "to": child, "reason": reason, "detail": detail}) + + +def _validate_symbolic_state( + ordered_groups: Sequence[str], + dependencies: Mapping[str, set[str]], + summaries: Mapping[str, Mapping[str, Any]], + groups: Mapping[str, Mapping[str, Any]], +) -> None: + atoms = [ + atom + for summary in summaries.values() + for atom in [ + *summary["entry_requires"], + *(effect["atom"] for effect in summary["exit_effects"]), + ] + ] + state = { + _atom_key({"predicate": "arm_free", "arm": str(atom["arm"])}) + for atom in atoms + if "arm" in atom + } + state.update( + _atom_key({"predicate": "object_free", "object_uid": str(atom["object_uid"])}) + for atom in atoms + if "object_uid" in atom + ) + for group_id in _stable_topological(ordered_groups, dependencies): + if groups[group_id].get("role") == "recovery": + for requirement in summaries[group_id]["entry_requires"]: + if requirement["predicate"] == "object_free": + object_uid = str(requirement["object_uid"]) + state = { + item + for item in state + if not ( + item.startswith("object_held|") + or item.startswith("object_coordinated_held|") + ) + or f"|{object_uid}|" not in f"|{item}|" + } + state.add(_atom_key(requirement)) + for requirement in summaries[group_id]["entry_requires"]: + key = _atom_key(requirement) + if key not in state: + raise ValueError( + _unavailable_group_state_message( + group_id, + requirement, + state, + groups[group_id], + ) + ) + for effect in summaries[group_id]["exit_effects"]: + key = _atom_key(effect["atom"]) + if effect["op"] == "add": + state.add(key) + else: + state.discard(key) + + +def _unavailable_group_state_message( + group_id: str, + requirement: Mapping[str, Any], + state: Collection[str], + group: Mapping[str, Any], +) -> str: + """Explain held-object conflicts without weakening symbolic validation.""" + if requirement.get("predicate") != "arm_free": + return ( + f"SeedGraph TaskGroup {group_id!r} requires unavailable state " + f"{dict(requirement)}." + ) + arm = str(requirement.get("arm", "")) + held_objects = sorted( + parts[1] + for item in state + if len(parts := item.split("|", maxsplit=2)) == 3 + and parts[0] == "object_held" + and parts[2] == arm + and parts[1] + ) + if not held_objects: + return ( + f"SeedGraph TaskGroup {group_id!r} requires unavailable state " + f"{dict(requirement)}." + ) + primary = str(group.get("object_uid", "")) + held = ", ".join(repr(item) for item in held_objects) + return ( + f"SeedGraph TaskGroup {group_id!r} requires arm {arm!r} to be free, " + f"but it currently holds {held}; the group's primary object is " + f"{primary!r}. A post-handover continuation must preserve object " + "identity and consume object_held instead of scheduling a fresh pickup." + ) + + +def _goal_read_claims(value: Any) -> list[dict[str, str]]: + claims: list[dict[str, str]] = [] + if isinstance(value, Mapping): + for key, child in value.items(): + if key in _REFERENCE_KEYS and isinstance(child, str): + if child not in {"table_center", "world"}: + claims.append(_claim(f"object:{child}", "shared_read")) + claims.extend(_goal_read_claims(child)) + elif isinstance(value, Sequence) and not isinstance(value, (str, bytes, bytearray)): + for child in value: + claims.extend(_goal_read_claims(child)) + return claims + + +def _claim( + resource: str, access: str = "exclusive", lifetime: str = "action" +) -> dict[str, str]: + return {"resource": resource, "access": access, "lifetime": lifetime} + + +def _merge_claims(claims: Sequence[Mapping[str, Any]]) -> list[dict[str, str]]: + merged: dict[str, dict[str, str]] = {} + order: list[str] = [] + for claim in claims: + resource = str(claim["resource"]) + if resource not in merged: + order.append(resource) + merged[resource] = _claim( + resource, + str(claim.get("access", "exclusive")), + str(claim.get("lifetime", "action")), + ) + continue + current = merged[resource] + if claim.get("access") == "exclusive": + current["access"] = "exclusive" + if claim.get("lifetime") == "until_release": + current["lifetime"] = "until_release" + return [merged[resource] for resource in order] + + +def _claim_conflicts( + first: Sequence[Mapping[str, Any]], second: Sequence[Mapping[str, Any]] +) -> list[str]: + first_by_resource = {str(item["resource"]): str(item["access"]) for item in first} + second_by_resource = {str(item["resource"]): str(item["access"]) for item in second} + conflicts = { + resource + for resource in set(first_by_resource) & set(second_by_resource) + if "exclusive" in {first_by_resource[resource], second_by_resource[resource]} + } + first_arms = {item for item in first_by_resource if item.startswith("arm:")} + second_arms = {item for item in second_by_resource if item.startswith("arm:")} + if "arm:auto" in first_arms and second_arms: + conflicts.add("arm:auto") + if "arm:auto" in second_arms and first_arms: + conflicts.add("arm:auto") + return sorted(conflicts) + + +def _distinct_arm_pairs(value: Any) -> set[frozenset[str]]: + if not isinstance(value, Mapping): + return set() + groups = value.get("legacy_allocation_groups", value.get("allocation_groups", ())) + if not isinstance(groups, Sequence) or isinstance(groups, (str, bytes, bytearray)): + return set() + result: set[frozenset[str]] = set() + for group in groups: + if ( + not isinstance(group, Mapping) + or group.get("arm_constraint") != "distinct_arms" + ): + continue + members = group.get("semantic_step_ids", group.get("task_instance_ids", ())) + if not isinstance(members, Sequence) or isinstance( + members, (str, bytes, bytearray) + ): + continue + member_ids = [str(item) for item in members] + for index, first in enumerate(member_ids): + for second in member_ids[index + 1 :]: + result.add(frozenset({first, second})) + return result + + +def _resolve_roles(value: Any, bindings: Mapping[str, str]) -> Any: + if isinstance(value, Mapping): + return { + str(key): _resolve_roles(child, bindings) for key, child in value.items() + } + if isinstance(value, list): + return [_resolve_roles(child, bindings) for child in value] + if isinstance(value, tuple): + return tuple(_resolve_roles(child, bindings) for child in value) + if isinstance(value, str): + return bindings.get(value, value) + return value + + +def _adds_atom(effects: Sequence[Mapping[str, Any]], atom: Mapping[str, Any]) -> bool: + return any( + effect.get("op") == "add" and effect.get("atom") == atom for effect in effects + ) + + +def _atom_key(atom: Mapping[str, Any]) -> str: + return "|".join( + str(atom.get(key, "")) for key in ("predicate", "object_uid", "arm") + ) + + +def _unique_by_id(items: Sequence[Mapping[str, Any]], context: str) -> dict[str, Any]: + result: dict[str, Any] = {} + for item in items: + item_id = str(item.get("id", "")) + if not item_id: + raise ValueError(f"{context} require non-empty IDs.") + if item_id in result: + raise ValueError(f"{context} contain duplicate ID {item_id!r}.") + result[item_id] = item + return result + + +def _ordered_group_ids( + groups: Sequence[Mapping[str, Any]], task_order: Sequence[str] +) -> list[str]: + available = [str(group["id"]) for group in groups] + requested = [str(item) for item in task_order] + unknown = set(requested) - set(available) + if unknown: + raise ValueError( + f"task_order references unknown TaskGroups: {sorted(unknown)}." + ) + return requested + [item for item in available if item not in set(requested)] + + +def _reaches(dependencies: Mapping[str, set[str]], child: str, parent: str) -> bool: + pending = list(dependencies.get(child, ())) + visited: set[str] = set() + while pending: + current = pending.pop() + if current == parent: + return True + if current not in visited: + visited.add(current) + pending.extend(dependencies.get(current, ())) + return False + + +def _node_reaches( + node_by_id: Mapping[str, Mapping[str, Any]], child: str, parent: str +) -> bool: + pending = [str(item) for item in node_by_id[child].get("depends_on", ())] + visited: set[str] = set() + while pending: + current = pending.pop() + if current == parent: + return True + if current not in visited and current in node_by_id: + visited.add(current) + pending.extend( + str(item) for item in node_by_id[current].get("depends_on", ()) + ) + return False + + +def _assert_acyclic(dependencies: Mapping[str, set[str]], context: str) -> None: + for item_id in dependencies: + if _reaches(dependencies, item_id, item_id): + raise ValueError(f"{context} produced a dependency cycle at {item_id!r}.") + + +def _stable_topological( + order: Sequence[str], dependencies: Mapping[str, set[str]] +) -> list[str]: + remaining = set(order) + result: list[str] = [] + while remaining: + ready = [ + item + for item in order + if item in remaining and not (dependencies[item] & remaining) + ] + if not ready: + raise ValueError("SeedGraph TaskGroups contain a dependency cycle.") + result.extend(ready) + remaining.difference_update(ready) + return result + + +def _already_linked(graph: Mapping[str, Any]) -> bool: + metadata = graph.get("metadata", {}) + linker = ( + metadata.get("action_contract_linker", {}) + if isinstance(metadata, Mapping) + else {} + ) + return ( + isinstance(linker, Mapping) + and linker.get("version") == CONTRACT_LINKER_VERSION + and all("contract" in node for node in graph.get("nodes", ())) + and all("contract" in group for group in graph.get("task_groups", ())) + ) + + +def _sorted_reasons(reasons: Sequence[Mapping[str, str]]) -> list[dict[str, str]]: + unique = { + (item["from"], item["to"], item["reason"], item["detail"]) for item in reasons + } + return [ + {"from": source, "to": target, "reason": reason, "detail": detail} + for source, target, reason, detail in sorted(unique) + ] diff --git a/embodichain/gen_sim/action_engine/planning/online.py b/embodichain/gen_sim/action_engine/planning/online.py new file mode 100644 index 000000000..fc8b36080 --- /dev/null +++ b/embodichain/gen_sim/action_engine/planning/online.py @@ -0,0 +1,371 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Online planner producing a complete direct AtomicAction SeedGraph.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from copy import deepcopy +import json +from time import perf_counter +from typing import Any + +from embodichain.gen_sim.action_engine.capabilities import ( + AtomicCapabilityRegistry, + build_atomic_capability_registry, +) +from embodichain.gen_sim.action_engine.domain import ( + TASK_CONTRACTS, + public_task_spec, + requested_visual_task_predicates, + validate_public_task_spec, + validate_task_spec, +) +from embodichain.gen_sim.action_engine.protocol import SEED_GRAPH_SCHEMA +from embodichain.gen_sim.action_engine.runtime.motion_policy import ( + resolve_motion_policy, +) + +from .vision import ( + SceneObservation, + _reject_live_fields as _reject_visual_live_fields, + analyze_visual_scene, + validate_visual_facts, +) +from .linker import link_seed_graph + +__all__ = ["plan_online_seed_graph"] + +GraphCaller = Callable[..., Mapping[str, Any]] + +_GRAPH_OUTPUT_SCHEMA = { + "title": "ActionEngineOnlineSeedGraphBody", + "type": "object", + "additionalProperties": False, + "required": ["nodes", "task_groups", "success"], + "properties": { + "nodes": {"type": "array", "items": {"type": "object"}}, + "task_groups": {"type": "array", "items": {"type": "object"}}, + "success": {"type": "object"}, + }, +} + + +def plan_online_seed_graph( + task_spec: Mapping[str, Any], + observation: SceneObservation, + *, + visual_facts: Mapping[str, Any] | None = None, + vlm_model: str | None = None, + fact_caller: GraphCaller | None = None, + graph_caller: GraphCaller | None = None, + registry: AtomicCapabilityRegistry | None = None, + robot_profile: str = "dual_ur10", +) -> tuple[dict[str, Any], dict[str, Any]]: + """Extract visual facts and produce one validated online SeedGraph.""" + started = perf_counter() + task = ( + validate_public_task_spec(task_spec) + if "task_instances" not in task_spec and task_spec.get("level") == "L4" + else validate_task_spec(task_spec) + ) + _reject_private_or_live_fields(public_task_spec(task), "online TaskSpec") + capabilities = registry or build_atomic_capability_registry() + _reject_visual_live_fields(observation.entities, "SceneObservation.entities") + known_uids = {str(item["uid"]) for item in observation.entities} + if len(known_uids) != len(observation.entities): + raise ValueError("Online scene observation contains duplicate entity UIDs.") + if not known_uids: + raise ValueError("Online scene observation contains no simulator entities.") + visual_call_counter = [0] + allowed_task_predicates = requested_visual_task_predicates(task) + facts = ( + validate_visual_facts( + visual_facts, + known_uids=known_uids, + camera_uids={camera.uid for camera in observation.cameras}, + allowed_task_predicates=allowed_task_predicates, + ) + if visual_facts is not None + else analyze_visual_scene( + observation, + task, + model=vlm_model, + caller=fact_caller, + call_counter=visual_call_counter, + ) + ) + _validate_fact_information(facts) + prompt = _prompt(task, facts, capabilities, robot_profile=robot_profile) + if graph_caller is None: + # Facts remain the auditable planner input, but the production VLM also + # needs the same reset-time RGB/depth evidence to bind semantic TaskSpec + # roles (for example, "the purple can") to the known simulator UIDs. + # An injected graph caller keeps the compact facts-only contract used by + # deterministic tests and alternative planners. + def caller(**kwargs: Any) -> Mapping[str, Any]: + return _default_graph_caller(observation=observation, **kwargs) + + else: + caller = graph_caller + first_error: Exception | None = None + graph_call_count = 0 + for attempt in range(2): + current_prompt = prompt + if first_error is not None: + current_prompt += ( + "\n\nThe previous graph was invalid. Correct only the JSON body. " + f"Validation error: {first_error}" + ) + graph_call_count += 1 + try: + response = caller( + prompt=current_prompt, + schema=_GRAPH_OUTPUT_SCHEMA, + model=vlm_model, + ) + graph = _wrap_graph(response, task, capabilities) + _reject_private_or_live_fields(graph, "online SeedGraph") + graph = link_seed_graph( + graph, + registry=capabilities, + task_order=[str(item["id"]) for item in task.get("task_instances", ())], + known_objects=known_uids, + ) + _validate_explicit_task_group_coverage(task, graph) + for node in graph["nodes"]: + capabilities.validate_binding(node) + if capabilities.get(str(node["atomic_action"])).runtime_available: + resolve_motion_policy( + robot_profile, + node["atomic_action"], + node["motion_policy"], + ) + graph["metadata"].update( + { + "planning_latency_seconds": perf_counter() - started, + "vlm_call_count": graph_call_count + visual_call_counter[0], + "visual_fact_call_count": visual_call_counter[0], + "graph_call_count": graph_call_count, + } + ) + return graph, facts + except (TypeError, ValueError) as error: + if attempt: + raise ValueError( + "Online SeedGraph failed validation after one repair: " f"{error}" + ) from error + first_error = error + raise AssertionError("unreachable") + + +def _prompt( + task: Mapping[str, Any], + facts: Mapping[str, Any], + capabilities: AtomicCapabilityRegistry, + *, + robot_profile: str, +) -> str: + from embodichain.gen_sim.action_engine.config import default_runtime_policy + + runtime_policy = default_runtime_policy(robot_profile) + motion_modifiers: dict[str, list[dict[str, str]]] = { + action: [] for action in runtime_policy.motion_defaults + } + for modifier_type, modes in runtime_policy.motion_modifiers.items(): + for mode, action_patches in modes.items(): + for action in action_patches: + motion_modifiers[action].append({"type": modifier_type, "mode": mode}) + grouping_instruction = ( + "Infer the necessary E TaskGroups from the abstract goal; the private " + "reference task instances are intentionally hidden." + if task["level"] == "L4" + else "Every public TaskSpec task instance must correspond to exactly one TaskGroup." + ) + return ( + "Produce the body of one coordinate-free direct AtomicAction SeedGraph. " + f"{grouping_instruction} " + "Nodes may contain only symbolic target bindings and scene UIDs; never " + "emit world coordinates, poses, qpos, trajectories, or grasp poses. " + "Do not emit Action Contracts or resource claims; the deterministic " + "Contract Linker owns those fields. " + "Use the supplied reset-time multi-view image evidence only to bind the " + "public task semantics to known UIDs; use normalized visual constraints " + "only when the facts justify them. " + "Do not output reasoning. Planning-only actions may appear but must not " + "be replaced with invented primitives.\n\n" + f"Public TaskSpec:\n{json.dumps(public_task_spec(task), ensure_ascii=False, sort_keys=True)}\n\n" + f"Visual facts:\n{json.dumps(facts, ensure_ascii=False, sort_keys=True)}\n\n" + f"E1-E9 task semantics:\n{json.dumps(_task_capability_catalog(), ensure_ascii=False, sort_keys=True)}\n\n" + f"Atomic capabilities:\n{json.dumps(capabilities.catalog(), ensure_ascii=False, sort_keys=True)}\n\n" + "Every node motion_policy must be an object with a modifiers list; " + "the AtomicAction selects its base policy implicitly. Use only the " + "typed modifiers supported by that action.\n" + f"Allowed motion modifiers by AtomicAction:\n" + f"{json.dumps(motion_modifiers, sort_keys=True)}" + ) + + +def _task_capability_catalog() -> dict[str, dict[str, Any]]: + """Return the Action Engine's runtime-aware E-task view.""" + executable = set(build_atomic_capability_registry().executable_names()) + return { + task_type: { + "semantics": contract.semantics, + "core_actions": list(contract.core_actions), + "runtime_available": set(contract.core_actions) <= executable, + } + for task_type, contract in TASK_CONTRACTS.items() + } + + +def _wrap_graph( + response: Mapping[str, Any], + task: Mapping[str, Any], + capabilities: AtomicCapabilityRegistry, +) -> dict[str, Any]: + if not isinstance(response, Mapping): + raise TypeError("Online planner output must be a mapping.") + if set(response) != {"nodes", "task_groups", "success"}: + raise ValueError( + "Online planner must return nodes, task_groups, and success only." + ) + for index, node in enumerate(response.get("nodes", ())): + if not isinstance(node, Mapping): + raise TypeError(f"Online planner node {index} must be a mapping.") + forbidden = sorted({"contract", "resources"} & set(node)) + if forbidden: + raise ValueError( + f"Online planner node {index} may not author linker-owned fields: " + f"{forbidden}." + ) + for index, group in enumerate(response.get("task_groups", ())): + if not isinstance(group, Mapping): + raise TypeError(f"Online planner TaskGroup {index} must be a mapping.") + if "contract" in group: + raise ValueError( + f"Online planner TaskGroup {index} may not author its contract." + ) + return { + "schema_version": SEED_GRAPH_SCHEMA, + "task_id": task["task_id"], + "instruction": task["instruction"], + "level": task["level"], + "reasoning_type": task["reasoning_type"], + "planner_route": "online", + "nodes": deepcopy(response["nodes"]), + "task_groups": deepcopy(response["task_groups"]), + "success": deepcopy(response["success"]), + "capability_catalog_hash": capabilities.catalog_hash(), + "metadata": { + "oracle_exposed": False, + "visual_facts_used": True, + "allocation_groups": deepcopy( + task.get("metadata", {}).get("allocation_groups", []) + ), + }, + } + + +def _default_graph_caller( + *, + prompt: str, + schema: Mapping[str, Any], + model: str | None, + observation: SceneObservation | None = None, +) -> Mapping[str, Any]: + from .vision import _camera_evidence, _default_structured_caller, _vlm_model + + images: list[str] = [] + if observation is not None: + _, images = _camera_evidence(observation) + + return _default_structured_caller( + prompt=prompt, + images=images, + schema=schema, + model=_vlm_model(model), + ) + + +def _validate_fact_information(facts: Mapping[str, Any]) -> None: + """Reject low-information visual outputs before graph planning.""" + confidence = facts.get("confidence", 0.0) + if float(confidence) < 0.5: + raise ValueError( + "VLM visual facts confidence is below the required 0.5 threshold." + ) + entities = facts.get("entities", ()) + if not any( + bool(item.get("visible", True)) and float(item.get("confidence", 0.0)) >= 0.5 + for item in entities + if isinstance(item, Mapping) + ): + raise ValueError("VLM visual facts contain no reliable visible entity.") + + +def _validate_explicit_task_group_coverage( + task: Mapping[str, Any], graph: Mapping[str, Any] +) -> None: + """Reject an online graph that drops or invents an explicit L1-L3 step.""" + if task.get("level") == "L4": + return + expected = { + str(item["id"]) + for item in task.get("task_instances", ()) + if isinstance(item, Mapping) + } + actual = {str(group["id"]) for group in graph.get("task_groups", ())} + if expected != actual: + raise ValueError( + "Online SeedGraph TaskGroup coverage mismatch; " + f"missing={sorted(expected - actual)}, " + f"unexpected={sorted(actual - expected)}." + ) + + +_PRIVATE_OR_LIVE_KEYS = frozenset( + { + "absolute_position", + "coordinates", + "grasp_pose", + "joint_positions", + "live_pose", + "live_transform", + "object_pose", + "oracle", + "pose", + "positions", + "qpos", + "target_pose", + "trajectory", + "waypoints", + "xpos", + } +) + + +def _reject_private_or_live_fields(value: Any, context: str) -> None: + """Reject private oracle and grounded simulator fields recursively.""" + if isinstance(value, Mapping): + for key, child in value.items(): + if str(key).lower() in _PRIVATE_OR_LIVE_KEYS: + raise ValueError(f"{context} contains private/live field {key!r}.") + _reject_private_or_live_fields(child, f"{context}.{key}") + elif isinstance(value, Sequence) and not isinstance(value, (str, bytes, bytearray)): + for index, child in enumerate(value): + _reject_private_or_live_fields(child, f"{context}[{index}]") diff --git a/embodichain/gen_sim/action_engine/planning/planner.py b/embodichain/gen_sim/action_engine/planning/planner.py new file mode 100644 index 000000000..a3e71d532 --- /dev/null +++ b/embodichain/gen_sim/action_engine/planning/planner.py @@ -0,0 +1,821 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Route-free LLM planning boundary for Action Engine.""" + +from __future__ import annotations + +import json +import os +import re +from collections.abc import Callable, Mapping, Sequence +from copy import deepcopy +from pathlib import Path +from string import Template +from typing import Any + +from embodichain.gen_sim.action_engine.capabilities import build_default_registry +from embodichain.gen_sim.action_engine.domain import ( + TASK_AGENT_SCHEMA, + validate_task_agent, +) +from embodichain.gen_sim.action_engine.orientation import ( + compile_orientation_constraint, +) + +from .task_planner_prompt import TASK_PLANNER_PROMPT + +__all__ = ["plan_task"] + +LLMCaller = Callable[..., Mapping[str, Any]] + +_GEN_CONFIG_PATH = ( + Path(__file__).resolve().parents[2] + / "simready_pipeline" + / "configs" + / "gen_config.json" +) +_GEN_SIM_ENV_PATH = Path(__file__).resolve().parents[2] / ".env" +_UNSAFE_ID_RE = re.compile(r"[^0-9a-z]+") +_MODEL_STEP_KEYS = frozenset( + {"id", "operator", "object", "objects", "actor", "goal", "depends_on"} +) + +_MODEL_OUTPUT_SCHEMA: dict[str, Any] = { + "title": "ActionEngineSemanticPlan", + "type": "object", + "additionalProperties": False, + "required": ["semantic_steps", "allocation_groups"], + "properties": { + "semantic_steps": { + "type": "array", + "minItems": 1, + "items": { + "type": "object", + "additionalProperties": False, + "required": ["operator"], + "properties": { + "id": {"type": "string"}, + "operator": {"type": "string"}, + "object": {"type": "string"}, + "objects": { + "type": "array", + "items": {"type": "string"}, + }, + "actor": {"type": "object"}, + "goal": {"type": "object"}, + "depends_on": { + "type": "array", + "items": {"type": "string"}, + }, + }, + }, + }, + "allocation_groups": { + "type": "array", + "items": { + "type": "object", + "additionalProperties": False, + "required": ["id", "semantic_step_ids", "arm_constraint"], + "properties": { + "id": {"type": "string"}, + "semantic_step_ids": { + "type": "array", + "items": {"type": "string"}, + }, + "arm_constraint": {"const": "distinct_arms"}, + }, + }, + }, + }, +} + + +def plan_task( + task_description: str, + scene_objects: Sequence[Mapping[str, Any]], + *, + task_name: str = "task", + model: str | None = None, + llm_caller: LLMCaller | None = None, +) -> dict[str, Any]: + """Plan a natural-language task as route-free semantic steps. + + The model is intentionally prohibited from emitting atomic actions, graph + edges, resources, target coordinates, or motion-policy parameters. + ``compile_task_agent`` owns all of those deterministic decisions. + + Args: + task_description: User goal in natural language. + scene_objects: JSON-like scene inventory. ``runtime_uid`` is preferred + over ``uid`` and ``source_uid`` for all generated references. + task_name: Stable task identifier stored in the TaskAgent. + model: Optional model-name override for the default LLM caller. + llm_caller: Optional injected callable accepting ``prompt=`` and + ``model=`` keyword arguments. It must return a mapping whose only + top-level key is ``semantic_steps``. + Returns: + A validated ``action_engine_task_agent_v1`` mapping. + """ + task_name = _nonempty(task_name, "task_name") + task_description = _nonempty(task_description, "task_description") + scene = _normalize_scene_objects(scene_objects) + + prompt = _render_prompt( + task_name=task_name, + task_description=task_description, + scene_objects=scene, + ) + caller = llm_caller or _default_llm_caller + response = caller(prompt=prompt, model=model) + try: + return _task_agent_from_response( + response, + task_name=task_name, + task_description=task_description, + scene=scene, + ) + except (TypeError, ValueError) as first_error: + # One bounded repair gives the model the verifier's exact complaint + # without turning generation into an unbounded conversation. + repair_prompt = ( + f"{prompt}\n\n" + "Your previous JSON did not satisfy the TaskAgent contract.\n" + f"Validation error: {first_error}\n" + "Return one corrected JSON object. Do not explain the correction." + ) + repaired = caller(prompt=repair_prompt, model=model) + try: + return _task_agent_from_response( + repaired, + task_name=task_name, + task_description=task_description, + scene=scene, + ) + except (TypeError, ValueError) as second_error: + raise ValueError( + "Action Engine planner failed validation after one repair: " + f"{second_error}" + ) from second_error + + +def _task_agent_from_response( + response: Any, + *, + task_name: str, + task_description: str, + scene: Sequence[Mapping[str, Any]], +) -> dict[str, Any]: + """Normalize and validate one model response as a TaskAgent.""" + if not isinstance(response, Mapping): + raise ValueError("Action Engine planner output must be a JSON object.") + allowed_fields = {"semantic_steps", "allocation_groups"} + if not set(response) <= allowed_fields or "semantic_steps" not in response: + raise ValueError( + "Action Engine planner output may contain only 'semantic_steps' " + "and 'allocation_groups'; " + f"received fields {sorted(str(key) for key in response)}." + ) + raw_steps = response["semantic_steps"] + if not isinstance(raw_steps, Sequence) or isinstance( + raw_steps, (str, bytes, bytearray) + ): + raise ValueError("Planner semantic_steps must be a list.") + visible_operators = set(build_default_registry().operator_names()) + for index, step in enumerate(raw_steps): + operator = step.get("operator") if isinstance(step, Mapping) else None + if operator not in visible_operators: + raise ValueError( + f"Planner semantic_steps[{index}].operator must be one of " + f"{sorted(visible_operators)}; got {operator!r}." + ) + return _wrap_agent( + task_name, + task_description, + raw_steps, + scene, + allocation_groups=response.get("allocation_groups", []), + ) + + +def _wrap_agent( + task_name: str, + task_description: str, + raw_steps: Sequence[Any], + scene: Sequence[Mapping[str, Any]], + *, + allocation_groups: Any, +) -> dict[str, Any]: + steps = _normalize_semantic_steps(raw_steps, scene) + groups = deepcopy(allocation_groups) + task_agent = validate_task_agent( + { + "schema_version": TASK_AGENT_SCHEMA, + "task": task_name, + "goal": task_description, + "semantic_steps": steps, + "allocation_groups": groups, + }, + known_objects=[_scene_runtime_uid(item) for item in scene], + ) + _validate_operator_contracts(task_agent) + return task_agent + + +def _validate_operator_contracts(task_agent: Mapping[str, Any]) -> None: + """Validate capability-specific step shapes inside the planner repair loop.""" + registry = build_default_registry() + for step in task_agent["semantic_steps"]: + operator = str(step["operator"]) + try: + expanded = registry.operator(operator).expand(step) + except (TypeError, ValueError) as error: + raise ValueError( + f"Semantic step {step['id']!r} violates the {operator!r} " + f"operator contract: {error}" + ) from error + if not expanded: + raise ValueError( + f"Semantic step {step['id']!r} produced no executable " + f"{operator!r} operation." + ) + + +def _normalize_semantic_steps( + raw_steps: Sequence[Any], + scene: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + if not raw_steps: + raise ValueError("Planner semantic_steps must not be empty.") + aliases = _scene_uid_aliases(scene) + normalized: list[dict[str, Any]] = [] + known_ids: set[str] = set() + previous_id: str | None = None + + for index, raw_step in enumerate(raw_steps, start=1): + if not isinstance(raw_step, Mapping): + raise ValueError(f"Planner semantic_steps[{index - 1}] must be an object.") + step = deepcopy(dict(raw_step)) + unknown = sorted(set(step) - _MODEL_STEP_KEYS) + if unknown: + raise ValueError( + f"Planner semantic_steps[{index - 1}] contains unsupported " + f"fields: {unknown}." + ) + operator = _nonempty( + step.get("operator"), + f"semantic_steps[{index - 1}].operator", + ) + configured_id = str(step.get("id", "")).strip() + step_id = configured_id or f"s{index:02d}_{_slug(operator)}" + if step_id in known_ids: + raise ValueError( + f"Planner produced duplicate semantic step ID {step_id!r}." + ) + known_ids.add(step_id) + + result: dict[str, Any] = {"id": step_id, "operator": operator} + if "object" in step: + result["object"] = _resolve_scene_uid( + step["object"], + aliases, + f"semantic step {step_id!r} object", + ) + if "objects" in step: + objects = step["objects"] + if not isinstance(objects, Sequence) or isinstance( + objects, (str, bytes, bytearray) + ): + raise ValueError(f"Semantic step {step_id!r} objects must be a list.") + result["objects"] = [ + _resolve_scene_uid( + object_uid, + aliases, + f"semantic step {step_id!r} objects", + ) + for object_uid in objects + ] + + actor = step.get("actor", {"mode": "auto"}) + if not isinstance(actor, Mapping): + raise ValueError(f"Semantic step {step_id!r} actor must be an object.") + result["actor"] = deepcopy(dict(actor)) + raw_goal = step.get("goal", {}) + if not isinstance(raw_goal, Mapping): + raise ValueError(f"Semantic step {step_id!r} goal must be an object.") + goal = deepcopy(dict(raw_goal)) + for key in ( + "anchor", + "orientation_reference_object", + "reference_object", + "support_object", + ): + if key not in goal or goal[key] in {"table_center", "self"}: + continue + goal[key] = _resolve_scene_uid( + goal[key], + aliases, + f"semantic step {step_id!r} goal.{key}", + ) + result["goal"] = goal + + if "depends_on" in step: + depends_on = step["depends_on"] + if not isinstance(depends_on, Sequence) or isinstance( + depends_on, (str, bytes, bytearray) + ): + raise ValueError( + f"Semantic step {step_id!r} depends_on must be a list." + ) + result["depends_on"] = [str(value) for value in depends_on] + else: + # Sequential is the conservative default. The LLM must explicitly + # emit an empty list when two semantic operations are independent. + result["depends_on"] = [previous_id] if previous_id is not None else [] + normalized.append(result) + previous_id = step_id + return normalized + + +def _fuse_redundant_hold_place_steps( + steps: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + """Remove a preparatory hold that a complete placement would repeat. + + ``place_relative`` already owns the pickup, transport, release, retreat, + and home phases. A model may nevertheless emit ``hold_hover(object)`` + followed by ``place_relative(object)`` as if the operators were individual + motion commands. The runtime cannot safely transfer that implicit held + state between semantic steps, so normalize the unambiguous one-consumer + pattern before TaskAgent validation. + + A hold with multiple consumers is intentionally left intact because it may + reserve one arm while unrelated branches continue. Compilation rejects any + later reuse of the held object rather than guessing an implicit handover. + """ + result = [deepcopy(dict(step)) for step in steps] + by_id = {step["id"]: step for step in result} + dependents: dict[str, list[str]] = {step_id: [] for step_id in by_id} + for step in result: + for dependency in step["depends_on"]: + if dependency in dependents: + dependents[dependency].append(step["id"]) + + removable: set[str] = set() + claimed_places: set[str] = set() + for hold in result: + if hold["operator"] != "hold_hover": + continue + consumers = dependents[hold["id"]] + if len(consumers) != 1: + continue + place = by_id[consumers[0]] + if place["operator"] != "place_relative" or place.get("object") != hold.get( + "object" + ): + continue + if place["id"] in claimed_places: + raise ValueError( + f"Semantic step {place['id']!r} cannot consume more than one " + "hold_hover state." + ) + if not _is_default_hold_goal(hold): + raise ValueError( + f"Cannot fuse {hold['id']!r} into {place['id']!r}: a " + "non-default hold_hover goal would be discarded." + ) + + place["actor"] = _merge_fused_actors( + hold["actor"], + place["actor"], + hold_id=hold["id"], + place_id=place["id"], + ) + rewritten_dependencies: list[str] = [] + for dependency in place["depends_on"]: + replacements = ( + hold["depends_on"] if dependency == hold["id"] else [dependency] + ) + for replacement in replacements: + if replacement not in rewritten_dependencies: + rewritten_dependencies.append(replacement) + place["depends_on"] = rewritten_dependencies + removable.add(hold["id"]) + claimed_places.add(place["id"]) + + return [step for step in result if step["id"] not in removable] + + +def _is_default_hold_goal(hold: Mapping[str, Any]) -> bool: + """Return whether removing a preparatory hover loses no requested state.""" + goal = hold["goal"] + if set(goal) - { + "orientation_constraint", + "orientation_axis", + "orientation_directed", + "orientation_goal", + "reference_object", + "reference_state", + }: + return False + return ( + goal.get("orientation_axis", "none") == "none" + and not compile_orientation_constraint(goal).terms + and goal.get("reference_state", "initial") == "initial" + and goal.get("reference_object", "self") in ("self", hold.get("object")) + ) + + +def _merge_fused_actors( + hold_actor: Mapping[str, Any], + place_actor: Mapping[str, Any], + *, + hold_id: str, + place_id: str, +) -> dict[str, Any]: + """Preserve an explicit arm requirement while fusing semantic steps.""" + hold = deepcopy(dict(hold_actor)) + place = deepcopy(dict(place_actor)) + hold_mode = hold.get("mode") + place_mode = place.get("mode") + hold_group = hold.get("allocation_group") + place_group = place.get("allocation_group") + if hold_group is not None and place_group is not None and hold_group != place_group: + raise ValueError( + f"Cannot fuse {hold_id!r} into {place_id!r}: conflicting " + "allocation groups would lose explicit arm-allocation intent." + ) + if hold_mode == "required" and place_mode == "required": + if hold.get("arm") != place.get("arm"): + raise ValueError( + f"Cannot fuse {hold_id!r} into {place_id!r}: conflicting " + "required arms would require an unsupported handover." + ) + merged = place + elif hold_mode == "required" and place_mode == "auto": + merged = hold + else: + merged = place + allocation_group = hold_group if hold_group is not None else place_group + if allocation_group is not None: + merged["allocation_group"] = allocation_group + return merged + + +def _render_prompt( + *, + task_name: str, + task_description: str, + scene_objects: Sequence[Mapping[str, Any]], +) -> str: + capabilities = build_default_registry() + return Template(TASK_PLANNER_PROMPT).substitute( + task_name=task_name, + task_description=task_description, + scene_objects=json.dumps( + list(scene_objects), + ensure_ascii=False, + indent=2, + sort_keys=True, + ), + operator_catalog=json.dumps( + capabilities.operator_descriptions(), + ensure_ascii=False, + indent=2, + sort_keys=True, + ), + ) + + +def _default_llm_caller(*, prompt: str, model: str | None) -> Mapping[str, Any]: + """Invoke the configured OpenAI-compatible model with structured output.""" + # Heavy client imports remain lazy so validation and deterministic + # compilation work in minimal simulation test environments. + from langchain_core.messages import HumanMessage, SystemMessage + from langchain_openai import ChatOpenAI + + settings = _load_llm_settings(model=model) + kwargs: dict[str, Any] = { + "api_key": settings["api_key"], + "model": settings["model"], + "temperature": 0, + "http_socket_options": (), + } + if settings["base_url"]: + kwargs["base_url"] = settings["base_url"] + if settings["default_query"]: + kwargs["default_query"] = settings["default_query"] + if _is_mimo_compatible(settings): + # MiMo's OpenAI-compatible endpoint supports JSON mode but not the + # OpenAI ``json_schema`` response format. Disable hidden reasoning so + # the bounded semantic response is not truncated to a few fields. + kwargs.update( + { + "max_completion_tokens": _MIMO_MAX_COMPLETION_TOKENS, + "extra_body": {"thinking": {"type": "disabled"}}, + } + ) + client = ChatOpenAI(**kwargs) + structured = _structured_output_runnable( + client, _MODEL_OUTPUT_SCHEMA, settings=settings + ) + response = structured.invoke( + [ + SystemMessage( + content=( + "Return only the requested route-free semantic plan. " + "Never emit coordinates, atomic actions, or graph edges." + ) + ), + HumanMessage(content=prompt), + ] + ) + return _coerce_model_response(response) + + +_MIMO_MAX_COMPLETION_TOKENS = 4096 + + +def _is_mimo_compatible(settings: Mapping[str, Any]) -> bool: + """Identify MiMo models or regional compatible endpoints without secrets.""" + model = str(settings.get("model", "")).casefold() + base_url = str(settings.get("base_url", "")).casefold() + return "mimo" in model or "xiaomimimo.com" in base_url + + +def _structured_output_runnable( + client: Any, + schema: Mapping[str, Any], + *, + settings: Mapping[str, Any], +) -> Any: + """Bind a portable JSON contract while retaining local strict validation. + + OpenAI-compatible providers do not share the same structured-output + dialect. MiMo documents ``json_object`` JSON mode rather than + ``json_schema``; using the latter can return HTTP 200 with sparse nested + objects. The caller still validates the decoded object against its local + schema after this transport-level binding. + """ + if not hasattr(client, "with_structured_output"): + return client + method = "json_mode" if _is_mimo_compatible(settings) else "json_schema" + try: + return client.with_structured_output(schema, method=method) + except (TypeError, ValueError): + if method == "json_mode" and hasattr(client, "bind"): + # Compatibility with older LangChain adapters that do not expose + # the ``method`` keyword but do support response_format binding. + from langchain_core.output_parsers import JsonOutputParser + + return ( + client.bind(response_format={"type": "json_object"}) + | JsonOutputParser() + ) + # Preserve the historical adapter behavior for non-MiMo providers. + return client.with_structured_output(schema) + + +def _load_llm_settings(*, model: str | None) -> dict[str, Any]: + local_env = _load_env_file(_GEN_SIM_ENV_PATH) + config: dict[str, Any] = {} + if _GEN_CONFIG_PATH.exists(): + with _GEN_CONFIG_PATH.open("r", encoding="utf-8") as stream: + raw = json.load(stream) + if isinstance(raw, Mapping): + llm = raw.get("llm", {}) + if isinstance(llm, Mapping): + configured = llm.get("openai_compatible", {}) + if isinstance(configured, Mapping): + config = dict(configured) + + # A key and endpoint identify one provider transport and must not be mixed + # across process, dotenv, and JSON configuration sources. + api_key, base_url = _resolve_transport_settings(local_env, config) + selected_model = ( + (model.strip() if isinstance(model, str) else "") + or _first_env_value( + local_env, + "ACTION_ENGINE_LLM_MODEL", + "OPENAI_MODEL", + "LLM_MODEL", + ) + or str(config.get("model", "")).strip() + ) + default_query = config.get("default_query", {}) or {} + if not api_key: + raise ValueError( + "OPENAI_API_KEY is required for Action Engine planning. Set it in " + f"the process environment or {_GEN_SIM_ENV_PATH}." + ) + if not selected_model: + raise ValueError( + "An LLM model is required through model=, OPENAI_MODEL, LLM_MODEL, " + f"or {_GEN_CONFIG_PATH}." + ) + if not isinstance(default_query, Mapping): + raise ValueError("LLM default_query must be a mapping.") + return { + "api_key": api_key, + "model": selected_model, + "base_url": base_url, + "default_query": dict(default_query), + } + + +def _resolve_transport_settings( + local_env: Mapping[str, str], + config: Mapping[str, Any], +) -> tuple[str, str]: + """Resolve an API key and endpoint from one configuration source.""" + transports = ( + ( + _mapping_value(os.environ, "OPENAI_API_KEY"), + _mapping_value( + os.environ, + "OPENAI_BASE_URL", + "OPENAI_API_BASE", + "LLM_URL", + ), + ), + ( + _mapping_value(local_env, "OPENAI_API_KEY"), + _mapping_value( + local_env, + "OPENAI_BASE_URL", + "OPENAI_API_BASE", + "LLM_URL", + ), + ), + ( + _mapping_value(config, "api_key"), + _mapping_value(config, "base_url"), + ), + ) + for api_key, base_url in transports: + if api_key and base_url: + return api_key, base_url.rstrip("/") + for api_key, base_url in transports: + if api_key: + return api_key, base_url.rstrip("/") + return "", "" + + +def _mapping_value(source: Mapping[str, Any], *names: str) -> str: + for name in names: + value = source.get(name) + if isinstance(value, str) and value.strip(): + return value.strip() + return "" + + +def _load_env_file(path: Path) -> dict[str, str]: + """Read a local dotenv file without exporting credentials process-wide.""" + if not path.is_file(): + return {} + values: dict[str, str] = {} + for line_number, raw_line in enumerate( + path.read_text(encoding="utf-8").splitlines(), + start=1, + ): + line = raw_line.strip() + if not line or line.startswith("#"): + continue + if line.startswith("export "): + line = line[len("export ") :].lstrip() + if "=" not in line: + continue + key, raw_value = line.split("=", 1) + key = key.strip() + if not re.fullmatch(r"[A-Za-z_][A-Za-z0-9_]*", key): + raise ValueError(f"Invalid dotenv key at {path}:{line_number}.") + value = raw_value.strip() + if len(value) >= 2 and value[0] == value[-1] and value[0] in {"'", '"'}: + value = value[1:-1] + elif " #" in value: + value = value.split(" #", 1)[0].rstrip() + values[key] = value + return values + + +def _first_env_value(local_env: Mapping[str, str], *names: str) -> str | None: + """Resolve aliases while keeping every shell value above local dotenv.""" + for source in (os.environ, local_env): + for name in names: + value = source.get(name) + if isinstance(value, str) and value.strip(): + return value.strip() + return None + + +def _coerce_model_response(response: Any) -> Mapping[str, Any]: + if isinstance(response, Mapping): + return dict(response) + model_dump = getattr(response, "model_dump", None) + if callable(model_dump): + dumped = model_dump() + if isinstance(dumped, Mapping): + return dict(dumped) + content = getattr(response, "content", response) + if isinstance(content, Mapping): + return dict(content) + if isinstance(content, list): + content = "\n".join( + str(item.get("text", "")) + for item in content + if isinstance(item, Mapping) and item.get("type") == "text" + ) + if not isinstance(content, str): + raise ValueError( + f"Planner model output has unsupported type {type(content).__name__}." + ) + text = content.strip() + if text.startswith("```"): + lines = text.splitlines() + lines = lines[1:] if lines else lines + lines = lines[:-1] if lines and lines[-1].startswith("```") else lines + text = "\n".join(lines).strip() + parsed = json.loads(text) + if not isinstance(parsed, Mapping): + raise ValueError("Planner model output must decode to a JSON object.") + return dict(parsed) + + +def _normalize_scene_objects( + scene_objects: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + if not isinstance(scene_objects, Sequence) or isinstance( + scene_objects, (str, bytes, bytearray) + ): + raise ValueError("scene_objects must be a list of mappings.") + normalized: list[dict[str, Any]] = [] + runtime_uids: set[str] = set() + for index, raw_object in enumerate(scene_objects): + if not isinstance(raw_object, Mapping): + raise ValueError(f"scene_objects[{index}] must be a mapping.") + item = deepcopy(dict(raw_object)) + runtime_uid = _scene_runtime_uid(item) + if runtime_uid in runtime_uids: + raise ValueError(f"Duplicate scene runtime UID {runtime_uid!r}.") + runtime_uids.add(runtime_uid) + item["runtime_uid"] = runtime_uid + normalized.append(item) + if not normalized: + raise ValueError("scene_objects must not be empty.") + return normalized + + +def _scene_uid_aliases( + scene_objects: Sequence[Mapping[str, Any]], +) -> dict[str, str]: + aliases: dict[str, str] = {} + for item in scene_objects: + runtime_uid = _scene_runtime_uid(item) + for key in ("runtime_uid", "uid", "source_uid"): + alias = item.get(key) + if isinstance(alias, str) and alias: + existing = aliases.get(alias) + if existing is not None and existing != runtime_uid: + raise ValueError(f"Ambiguous scene object alias {alias!r}.") + aliases[alias] = runtime_uid + return aliases + + +def _resolve_scene_uid(value: Any, aliases: Mapping[str, str], context: str) -> str: + uid = _nonempty(value, context) + try: + return aliases[uid] + except KeyError as exc: + raise ValueError(f"{context} references unknown scene object {uid!r}.") from exc + + +def _scene_runtime_uid(item: Mapping[str, Any]) -> str: + for key in ("runtime_uid", "uid", "source_uid"): + value = item.get(key) + if isinstance(value, str) and value.strip(): + return value + raise ValueError("Every scene object requires runtime_uid, uid, or source_uid.") + + +def _nonempty(value: Any, context: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{context} must be a non-empty string.") + return value.strip() + + +def _slug(value: str) -> str: + slug = _UNSAFE_ID_RE.sub("_", value.lower()).strip("_") + return slug[:48].rstrip("_") or "step" diff --git a/embodichain/gen_sim/action_engine/planning/selection.py b/embodichain/gen_sim/action_engine/planning/selection.py new file mode 100644 index 000000000..6ef92f089 --- /dev/null +++ b/embodichain/gen_sim/action_engine/planning/selection.py @@ -0,0 +1,364 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Score, select, and conservatively fuse whole TaskGroups.""" + +from __future__ import annotations + +from collections import defaultdict, deque +from collections.abc import Collection, Mapping +from copy import deepcopy +from dataclasses import dataclass +from typing import Any + +from embodichain.gen_sim.action_engine.capabilities import ( + AtomicCapabilityRegistry, + build_atomic_capability_registry, +) +from embodichain.gen_sim.action_engine.domain import ( + validate_seed_graph, + validate_task_spec, +) +from embodichain.gen_sim.action_engine.runtime.motion_policy import ( + resolve_motion_policy, +) + +from .linker import link_seed_graph, validate_persisted_contracts + +__all__ = [ + "CandidateEvaluation", + "evaluate_candidate", + "fuse_seed_graphs", + "select_seed_graph", +] + + +@dataclass(frozen=True) +class CandidateEvaluation: + """Auditable static candidate score before any physical execution.""" + + route: str + valid: bool + executable: bool + coverage: float + visual_confidence: float + estimated_cost: float + score: float + errors: tuple[str, ...] = () + + +def evaluate_candidate( + graph: Mapping[str, Any], + task_spec: Mapping[str, Any], + *, + known_objects: Collection[str], + visual_confidence: float = 1.0, + exact_template_match: bool = False, + registry: AtomicCapabilityRegistry | None = None, + robot_profile: str = "dual_ur10", +) -> CandidateEvaluation: + """Apply schema, capabilities, object identity, coverage, and cost scoring.""" + task = validate_task_spec(task_spec) + capabilities = registry or build_atomic_capability_registry() + errors = [] + try: + seed = validate_seed_graph( + graph, + known_objects=known_objects, + known_actions=capabilities.names(), + ) + if seed["capability_catalog_hash"] != capabilities.catalog_hash(): + raise ValueError("SeedGraph capability catalog does not match runtime.") + validate_persisted_contracts(seed, capabilities) + for node in seed["nodes"]: + capabilities.validate_binding(node) + if capabilities.get(str(node["atomic_action"])).runtime_available: + resolve_motion_policy( + robot_profile, + node["atomic_action"], + node["motion_policy"], + ) + except (TypeError, ValueError) as error: + return CandidateEvaluation( + route=str(graph.get("planner_route", "unknown")), + valid=False, + executable=False, + coverage=0.0, + visual_confidence=0.0, + estimated_cost=float("inf"), + score=float("-inf"), + errors=(str(error),), + ) + + required = {str(item["id"]) for item in task["task_instances"]} + provided = {str(group["id"]) for group in seed["task_groups"]} + if task["level"] == "L4": + coverage = 1.0 if provided and seed["success"] else 0.0 + unexpected = set() + mismatched_types = {} + else: + coverage = len(required & provided) / max(len(required), 1) + unexpected = provided - required + if unexpected: + errors.append(f"unexpected task groups: {sorted(unexpected)}") + expected_types = { + str(item["id"]): str(item["task_type"]) for item in task["task_instances"] + } + mismatched_types = { + str(group["id"]): str(group["task_type"]) + for group in seed["task_groups"] + if group["id"] in expected_types + and group["task_type"] != expected_types[group["id"]] + } + if mismatched_types: + errors.append(f"task group type mismatches: {mismatched_types}") + unavailable = sorted( + { + str(node["atomic_action"]) + for node in seed["nodes"] + if not capabilities.get(str(node["atomic_action"])).runtime_available + } + ) + executable = not unavailable + if unavailable: + errors.append(f"planning-only actions: {unavailable}") + confidence = min(max(float(visual_confidence), 0.0), 1.0) + estimated_cost = float(len(seed["nodes"])) + score = coverage * 100.0 - estimated_cost + route = str(seed["planner_route"]) + if exact_template_match and route == "offline": + score += 15.0 + if task["level"] == "L4" and route == "online": + score += 20.0 * confidence + if not executable: + score -= 30.0 + return CandidateEvaluation( + route=route, + valid=not unexpected and not mismatched_types and coverage == 1.0, + executable=executable, + coverage=coverage, + visual_confidence=confidence, + estimated_cost=estimated_cost, + score=score, + errors=tuple(errors), + ) + + +def select_seed_graph( + offline: Mapping[str, Any], + online: Mapping[str, Any], + task_spec: Mapping[str, Any], + *, + known_objects: Collection[str], + visual_confidence: float = 1.0, + exact_template_match: bool = False, + registry: AtomicCapabilityRegistry | None = None, + robot_profile: str = "dual_ur10", +) -> tuple[dict[str, Any], dict[str, CandidateEvaluation]]: + """Choose one complete candidate; ties prefer mature offline templates.""" + evaluations = { + "offline": evaluate_candidate( + offline, + task_spec, + known_objects=known_objects, + visual_confidence=1.0, + exact_template_match=exact_template_match, + registry=registry, + robot_profile=robot_profile, + ), + "online": evaluate_candidate( + online, + task_spec, + known_objects=known_objects, + visual_confidence=visual_confidence, + registry=registry, + robot_profile=robot_profile, + ), + } + valid = [item for item in evaluations.items() if item[1].valid] + if not valid: + messages = {name: evaluation.errors for name, evaluation in evaluations.items()} + raise ValueError(f"Neither SeedGraph candidate is valid: {messages}.") + valid.sort( + key=lambda item: ( + item[1].score, + item[0] == "offline", + ), + reverse=True, + ) + selected = deepcopy(dict(offline if valid[0][0] == "offline" else online)) + selected["planner_route"] = "selected" + selected.setdefault("metadata", {})["selected_from"] = valid[0][0] + return selected, evaluations + + +def fuse_seed_graphs( + offline: Mapping[str, Any], + online: Mapping[str, Any], + group_routes: Mapping[str, str], + *, + registry: AtomicCapabilityRegistry | None = None, +) -> dict[str, Any]: + """Fuse candidates only at complete TaskGroup boundaries.""" + capabilities = registry or build_atomic_capability_registry() + if offline.get("task_id") != online.get("task_id"): + raise ValueError("Cannot fuse graphs for different tasks.") + for field in ("instruction", "level", "reasoning_type", "capability_catalog_hash"): + if offline.get(field) != online.get(field): + raise ValueError(f"Cannot fuse graphs with different {field} values.") + by_route = { + "offline": validate_seed_graph(offline, known_actions=capabilities.names()), + "online": validate_seed_graph(online, known_actions=capabilities.names()), + } + for graph in by_route.values(): + validate_persisted_contracts(graph, capabilities) + groups_by_route = { + route: {str(group["id"]): group for group in graph["task_groups"]} + for route, graph in by_route.items() + } + expected = set(groups_by_route["offline"]) + if set(groups_by_route["online"]) != expected or set(group_routes) != expected: + raise ValueError( + "Fusion requires the same complete TaskGroup set in both graphs." + ) + if set(group_routes.values()) - {"offline", "online"}: + raise ValueError("Every fused TaskGroup route must be offline or online.") + + selected_groups = { + group_id: deepcopy(groups_by_route[route][group_id]) + for group_id, route in group_routes.items() + } + _reject_state_conflicts(selected_groups) + source_nodes = { + route: {str(node["id"]): node for node in graph["nodes"]} + for route, graph in by_route.items() + } + selected_nodes_by_group: dict[str, list[dict[str, Any]]] = {} + id_map: dict[tuple[str, str], str] = {} + for group_id, route in group_routes.items(): + group = selected_groups[group_id] + group.pop("contract", None) + selected_nodes_by_group[group_id] = [] + for node_id in group["node_ids"]: + node = deepcopy(source_nodes[route][node_id]) + fused_id = f"{route}_{node_id}" + id_map[(route, node_id)] = fused_id + node["id"] = fused_id + selected_nodes_by_group[group_id].append(node) + + terminals = {} + for group_id, route in group_routes.items(): + original_ids = set(selected_groups[group_id]["node_ids"]) + referenced = { + dependency + for node_id in original_ids + for dependency in source_nodes[route][node_id]["depends_on"] + if dependency in original_ids + } + terminals[group_id] = [ + id_map[(route, node_id)] + for node_id in selected_groups[group_id]["node_ids"] + if node_id not in referenced + ] + nodes = [] + groups = [] + for group_id in _topological_groups(selected_groups): + route = group_routes[group_id] + group = selected_groups[group_id] + own_original_ids = set(group["node_ids"]) + group_nodes = selected_nodes_by_group[group_id] + for node in group_nodes: + original_id = node["id"][len(route) + 1 :] + original = source_nodes[route][original_id] + internal = [ + id_map[(route, dependency)] + for dependency in original["depends_on"] + if dependency in own_original_ids + ] + external = [ + terminal + for parent in group["depends_on"] + for terminal in terminals[parent] + ] + node["depends_on"] = list(dict.fromkeys([*internal, *external])) + nodes.append(node) + group["node_ids"] = [node["id"] for node in group_nodes] + groups.append(group) + + fused = deepcopy(by_route["offline"]) + fused["planner_route"] = "fused" + fused["nodes"] = nodes + fused["task_groups"] = groups + fused["success"] = {"op": "all", "terms": [group["success"] for group in groups]} + fused["metadata"] = { + "fusion_routes": dict(sorted(group_routes.items())), + "fusion_boundary": "task_group", + } + return link_seed_graph( + fused, + registry=capabilities, + task_order=[str(group["id"]) for group in groups], + ) + + +def _reject_state_conflicts(groups: Mapping[str, Mapping[str, Any]]) -> None: + by_object: dict[str, list[str]] = defaultdict(list) + for group_id, group in groups.items(): + by_object[str(group["object_uid"])].append(group_id) + dependencies = { + group_id: set(group["depends_on"]) for group_id, group in groups.items() + } + + def reaches(child: str, parent: str) -> bool: + pending = list(dependencies[child]) + visited = set() + while pending: + current = pending.pop() + if current == parent: + return True + if current not in visited: + visited.add(current) + pending.extend(dependencies[current]) + return False + + for object_uid, group_ids in by_object.items(): + for index, first in enumerate(group_ids): + for second in group_ids[index + 1 :]: + if not reaches(first, second) and not reaches(second, first): + raise ValueError( + f"Fusion has unordered state changes for object {object_uid!r}." + ) + + +def _topological_groups(groups: Mapping[str, Mapping[str, Any]]) -> list[str]: + outgoing = {group_id: [] for group_id in groups} + indegree = {group_id: 0 for group_id in groups} + for group_id, group in groups.items(): + for parent in group["depends_on"]: + outgoing[parent].append(group_id) + indegree[group_id] += 1 + ready = deque( + sorted(group_id for group_id, degree in indegree.items() if degree == 0) + ) + result = [] + while ready: + group_id = ready.popleft() + result.append(group_id) + for child in sorted(outgoing[group_id]): + indegree[child] -= 1 + if indegree[child] == 0: + ready.append(child) + return result diff --git a/embodichain/gen_sim/action_engine/planning/task_planner_prompt.py b/embodichain/gen_sim/action_engine/planning/task_planner_prompt.py new file mode 100644 index 000000000..9dce0b612 --- /dev/null +++ b/embodichain/gen_sim/action_engine/planning/task_planner_prompt.py @@ -0,0 +1,159 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Prompt template for the Action Engine semantic planner.""" + +from __future__ import annotations + +__all__ = ["TASK_PLANNER_PROMPT"] + +TASK_PLANNER_PROMPT = """You are the semantic planner for a tabletop robot Action Engine. + +Return exactly one JSON object with exactly these two top-level fields: + +{ + "semantic_steps": [ + { + "id": "s01_short_stable_name", + "operator": "", + "object": "", + "actor": {"mode": "auto"}, + "goal": {}, + "depends_on": [] + } + ], + "allocation_groups": [] +} + +For collective operators, replace "object" with "objects": + +{ + "id": "s01_collective_goal", + "operator": "arrange_line", + "objects": ["object_a", "object_b"], + "actor": {"mode": "auto"}, + "goal": {}, + "depends_on": [] +} + +Hard rules: + +- Plan a sequence or DAG of semantic operators. Do not select a task route. +- Emit semantic_steps and allocation_groups only. Do not emit explanations, + confidence, warnings, + atomic actions, graph nodes, graph edges, resources, motion policies, poses, + coordinates, offsets, distances, joint values, trajectories, or tolerances. +- Use runtime_uid values from the scene inventory. Never invent object IDs. +- Preserve every explicit before/after/then dependency with depends_on. +- Use depends_on=[] for genuinely independent operations that may run in + parallel. Otherwise depend on the preceding required semantic step. +- actor.mode is "auto" unless the user explicitly requires one arm. +- allocation_groups expresses an explicit distinct-arm constraint across + independent semantic steps. Use + {"id":"dual_arms_1","semantic_step_ids":["s01","s02"], + "arm_constraint":"distinct_arms"} only when the user explicitly requests + different arms. Merely independent steps must not receive a group. +- An explicitly required arm uses + {"mode": "required", "arm": "left_arm"} or "right_arm". +- Coordinated operators use + {"mode": "coordinated", "arms": ["left_arm", "right_arm"]}. +- Use named symbolic relations and policies only. Runtime observes geometry. +- Every operator is a complete skill, not an individual motion command. + place_relative already picks, transports, releases, retreats, and returns + home. Never emit individual robot motions. +- When the user asks both arms to handle two independent objects, emit two + direct object-level operators with + actor={"mode":"auto"} and depends_on=[], then reference their step IDs in one + allocation_groups entry. The deterministic compiler assigns distinct arms; + do not guess left/right from object positions. +- Spatial phrases that place objects on opposite sides describe object + locations, not an arm-allocation constraint. Emit an allocation group only + when the user explicitly requests both or distinct arms. + +Built-in operator shapes: + +1. arrange_line + - objects: at least two movable objects in requested order. + - goal fields: anchor="table_center"; axis="world_x"|"world_y"| + "table_long_axis"; order_constraint="free"|"ordered"; + order_by="explicit"|"size"|"color"; order_direction="given"| + "ascending"|"descending"; orientation_goal="none"|"preserve"|"upright"| + "lay_flat"|"axis_align"; orientation_axis="none"|"x"|"y"| + "long_axis"|"short_axis". + - In the rotated robot view, world_y is the horizontal left-to-right axis + and world_x is the front-to-back depth axis. For an unspecified line or + row direction, always use axis="world_y". Use axis="world_x" only when + the user explicitly requests a front-to-back, depth-wise, column, or + x-axis layout. Use table_long_axis only when the user explicitly names the + table's long axis; never infer it from a generic line request. + - Use order_constraint="free" when the user wants a line but does not care + which object occupies each slot. + - A line layout does not imply an orientation acceptance requirement. Use + orientation_goal="none" and orientation_axis="none" unless the task + explicitly asks to preserve orientation, make objects upright, lay them + flat, or align an axis. + +2. build_stack + - objects: bottom-to-top movable object order. + - goal fields: stack_mode="on_top"|"nested"; anchor="table_center" or a + passive support runtime_uid; orientation_goal and orientation_axis. + - A vertical stack chain is exactly one build_stack step. Always use the + plural "objects" list, never singular "object", and do not include the + passive anchor in that list. + - Repeated clauses such as "put A on anchor, then put B on top" describe one + chain: objects=[A,B], anchor=anchor. Use separate place_relative steps only + when every object should independently contact the same support. + +3. place_relative + - object: one movable object. + - goal fields: reference_object; relation="inside"|"on"|"left_of"| + "right_of"|"front_of"|"behind"|"front_left_of"|"front_right_of"| + "back_left_of"|"back_right_of"; reference_state="live"|"initial"; + orientation_goal; orientation_axis; optional + orientation_reference_object. + +4. orient_object + - object: one movable object. + - goal fields: orientation_goal="upright"|"lay_flat"|"axis_align"; + orientation_axis="none"|"x"|"y"|"long_axis"|"short_axis"; + support_object=; position_anchor="initial_xy"|"live_xy"; + upright_local_axis="auto"|"long_axis"|"x"|"y"|"z". + - Use orientation_goal="upright" only when the instruction explicitly asks + to make the object upright. + - Use support_object="table" and position_anchor="initial_xy" for an + in-place tabletop orientation request. Use upright_local_axis="auto" + unless the scene inventory explicitly supplies a local semantic axis; + never infer a mesh-local axis from an object name. + +5. coordinated_transport + - object: one shared object moved by both arms. + - goal fields: direction="none"|"world_x"|"world_y"|"front"|"back"| + "left"|"right"|"front_left"|"front_right"|"back_left"|"back_right"| + "up"|"down"; terminal_behavior="hold"|"place"; optional reference_object + and relation; orientation_goal and orientation_axis. + +Available operators: +$operator_catalog + +Task name: +$task_name + +Task description: +$task_description + +Scene inventory: +$scene_objects +""" diff --git a/embodichain/gen_sim/action_engine/planning/vision.py b/embodichain/gen_sim/action_engine/planning/vision.py new file mode 100644 index 000000000..cba17b9b7 --- /dev/null +++ b/embodichain/gen_sim/action_engine/planning/vision.py @@ -0,0 +1,808 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Auditable multi-view observation and VLM fact extraction.""" + +from __future__ import annotations + +import base64 +from collections.abc import Callable, Collection, Mapping, Sequence +from copy import deepcopy +from dataclasses import dataclass +from io import BytesIO +import json +import math +import os +from typing import Any + +import torch + +from embodichain.gen_sim.action_engine.domain import ( + VISUAL_RELATION_PARTICIPANTS, + public_task_spec, + requested_visual_task_predicates, +) + +__all__ = [ + "CameraObservation", + "SceneObservation", + "analyze_visual_scene", + "collect_scene_observation", + "validate_visual_facts", +] + +StructuredCaller = Callable[..., Mapping[str, Any]] + +_VISUAL_ENTITY_KEYS = frozenset( + { + "uid", + "camera_uid", + "bbox", + "keypoints", + "visible", + "confidence", + } +) +_VISUAL_RELATION_KEYS = frozenset({"type", "uids", "confidence"}) +_VISUAL_TASK_PREDICATE_KEYS = frozenset({"type", "confidence"}) + + +@dataclass(frozen=True) +class CameraObservation: + """One live camera sample with calibration for one vectorized env row.""" + + uid: str + rgb: torch.Tensor + depth: torch.Tensor | None + intrinsics: torch.Tensor | None + extrinsics: torch.Tensor | None + + +@dataclass(frozen=True) +class SceneObservation: + """Multi-view evidence and stable simulator entity IDs for online planning.""" + + cameras: tuple[CameraObservation, ...] + entities: tuple[dict[str, Any], ...] + env_id: int = 0 + + +_VISUAL_FACTS_SCHEMA = { + "title": "ActionEngineVisualFacts", + "type": "object", + "additionalProperties": False, + "required": ["entities", "relations", "task_predicates", "confidence"], + "properties": { + "entities": { + "type": "array", + "items": { + "type": "object", + "additionalProperties": False, + "required": ["uid", "camera_uid", "confidence"], + "properties": { + "uid": {"type": "string"}, + "camera_uid": {"type": "string"}, + "bbox": { + "type": "array", + "minItems": 4, + "maxItems": 4, + "items": {"type": "number"}, + }, + "keypoints": { + "type": "object", + "additionalProperties": { + "type": "array", + "minItems": 2, + "maxItems": 2, + "items": {"type": "number"}, + }, + }, + "visible": {"type": "boolean"}, + "confidence": {"type": "number", "minimum": 0.0, "maximum": 1.0}, + }, + }, + }, + "relations": { + "type": "array", + "items": { + "type": "object", + "additionalProperties": False, + "required": ["type", "uids", "confidence"], + "properties": { + "type": { + "type": "string", + "enum": sorted(VISUAL_RELATION_PARTICIPANTS), + }, + "uids": { + "type": "array", + "minItems": 2, + "maxItems": 2, + "items": {"type": "string"}, + }, + "confidence": {"type": "number", "minimum": 0.0, "maximum": 1.0}, + }, + }, + }, + "task_predicates": { + "type": "array", + "items": { + "type": "object", + "additionalProperties": False, + "required": ["type", "confidence"], + "properties": { + "type": {"type": "string"}, + "confidence": {"type": "number", "minimum": 0.0, "maximum": 1.0}, + }, + }, + }, + "confidence": {"type": "number", "minimum": 0.0, "maximum": 1.0}, + }, +} + +# Visual facts are deliberately a much smaller contract than a simulator +# snapshot. In particular, accepting arbitrary nested ``attributes`` would +# let a caller smuggle poses/qpos into the online planner while still passing +# the top-level schema. Keep the deny-list here (rather than relying only on +# the SeedGraph validator) because visual facts are persisted and may be +# consumed by an independent planner implementation. +_FORBIDDEN_LIVE_KEYS = frozenset( + { + "absolute_position", + "coordinates", + "extrinsics", + "grasp_pose", + "joint_positions", + "live_pose", + "live_transform", + "object_pose", + "oracle", + "pose", + "positions", + "qpos", + "target_pose", + "trajectory", + "transform", + "waypoints", + "xpos", + } +) + + +def collect_scene_observation( + env: Any, + *, + camera_uids: Sequence[str] | None = None, + env_id: int = 0, +) -> SceneObservation: + """Capture current RGB/depth/calibration and a simulator entity inventory.""" + if env_id < 0 or env_id >= int(env.num_envs): + raise ValueError("env_id is outside the vectorized environment range.") + sim = env.sim + uids = ( + list(camera_uids) + if camera_uids is not None + else list(sim.get_sensor_uid_list()) + ) + cameras = [] + for uid in uids: + sensor = sim.get_sensor(str(uid)) + if sensor is None: + raise ValueError(f"Unknown camera UID {uid!r}.") + update = getattr(sensor, "update", None) + if callable(update): + update() + data = sensor.get_data() + if not isinstance(data, Mapping): + raise TypeError(f"Camera {uid!r} returned non-mapping sensor data.") + rgb_data = data.get("color", data.get("rgb")) + if rgb_data is None: + raise ValueError(f"Camera {uid!r} does not provide RGB data.") + rgb = ( + _env_row( + rgb_data, + env_id, + num_envs=int(env.num_envs), + unbatched_ndim=3, + ) + .detach() + .cpu() + ) + depth = ( + _env_row( + data["depth"], + env_id, + num_envs=int(env.num_envs), + unbatched_ndim=2, + ) + .detach() + .cpu() + if data.get("depth") is not None + else None + ) + intrinsics = _optional_call( + sensor, "get_intrinsics", env_id, num_envs=int(env.num_envs) + ) + extrinsics = _optional_call( + sensor, + "get_arena_pose", + env_id, + num_envs=int(env.num_envs), + to_matrix=True, + ) + cameras.append( + CameraObservation( + uid=str(uid), + rgb=rgb, + depth=depth, + intrinsics=intrinsics, + extrinsics=extrinsics, + ) + ) + if not cameras: + raise ValueError("Online visual planning requires at least one camera.") + + entity_uids = list(sim.get_rigid_object_uid_list()) + articulation_uids = getattr(sim, "get_articulation_uid_list", lambda: [])() + entities = [] + for uid in [*entity_uids, *articulation_uids]: + item: dict[str, Any] = {"uid": str(uid)} + # Do not expose live simulator transforms to the online planner. The + # VLM receives RGB/depth evidence and stable UIDs only; JIT grounding + # resolves world-space targets inside the runtime immediately before + # each action. This also prevents an accidental pose oracle through + # the entity inventory prompt. + entities.append(item) + return SceneObservation(tuple(cameras), tuple(entities), env_id=env_id) + + +def analyze_visual_scene( + observation: SceneObservation, + task_spec: Mapping[str, Any], + *, + model: str | None = None, + caller: StructuredCaller | None = None, + call_counter: list[int] | None = None, +) -> dict[str, Any]: + """Ask a VLM for auditable facts, never hidden reasoning or an action plan.""" + _reject_live_fields(observation.entities, "SceneObservation.entities") + public = public_task_spec(task_spec) + allowed_task_predicates = requested_visual_task_predicates(public) + relation_contracts = { + name: list(participants) + for name, participants in VISUAL_RELATION_PARTICIPANTS.items() + } + _reject_live_fields(public, "PublicTaskSpec") + camera_manifest, images = _camera_evidence(observation) + prompt = ( + "Inspect every supplied camera view. Return only observable facts needed " + "for the task. Refer to simulator entities only by the supplied UID. " + "Use normalized [0,1] bbox/keypoint values, state uncertainty explicitly, " + "and do not provide reasoning or actions. The image blocks appear in the " + "camera_evidence order: each RGB image is followed by that camera's " + "normalized depth image when depth_image_index is present. Camera " + "calibration is input evidence only; never reproduce it in the facts. " + "Use only these canonical spatial relation contracts, whose values give " + "the ordered UID participants: " + f"{json.dumps(relation_contracts, sort_keys=True)}. Put task-level visual " + "judgments in task_predicates, never in relations; their allowed types " + f"are {json.dumps(sorted(allowed_task_predicates))}.\n\n" + f"TaskSpec:\n{json.dumps(public, ensure_ascii=False, sort_keys=True)}\n\n" + f"Entity inventory:\n{json.dumps(observation.entities, ensure_ascii=False, sort_keys=True)}\n\n" + f"Camera evidence:\n{json.dumps(camera_manifest, ensure_ascii=False, sort_keys=True)}" + ) + invoke = caller or _default_structured_caller + # Test/mocked callers own their transport and may intentionally receive no + # configured model. The production caller must resolve strictly through + # the visual-model priority rather than falling back to a text-only model. + selected_model = model if caller is not None else _vlm_model(model) + first_error: Exception | None = None + for attempt in range(2): + current_prompt = prompt + if first_error is not None: + current_prompt += ( + "\n\nThe previous visual-facts JSON was invalid. Return corrected " + f"JSON only. Validation error: {first_error}" + ) + try: + if call_counter is not None: + call_counter[0] += 1 + response = invoke( + prompt=current_prompt, + images=images, + schema=_visual_facts_schema(allowed_task_predicates), + model=selected_model, + ) + facts = validate_visual_facts( + response, + known_uids={str(item["uid"]) for item in observation.entities}, + camera_uids={camera.uid for camera in observation.cameras}, + allowed_task_predicates=allowed_task_predicates, + ) + if facts["confidence"] < 0.5: + raise ValueError( + "VLM visual facts confidence is below the required 0.5 threshold." + ) + if not any( + item.get("visible", True) and item["confidence"] >= 0.5 + for item in facts["entities"] + ): + raise ValueError("VLM visual facts contain no reliable visible entity.") + return facts + except (TypeError, ValueError) as error: + if attempt: + raise ValueError( + "VLM visual facts failed validation after one repair: " f"{error}" + ) from error + first_error = error + raise AssertionError("unreachable") + + +def validate_visual_facts( + value: Mapping[str, Any], + *, + known_uids: set[str], + camera_uids: set[str], + allowed_task_predicates: Collection[str] = (), +) -> dict[str, Any]: + """Validate entity identity and normalized image-space evidence.""" + if not isinstance(value, Mapping): + raise TypeError("VLM visual facts must be a mapping.") + required_fields = {"entities", "relations", "task_predicates", "confidence"} + if set(value) != required_fields: + raise ValueError( + "VLM visual facts require exactly fields " + f"{sorted(required_fields)}; received {sorted(value)}." + ) + confidence = _confidence(value.get("confidence"), "confidence") + entities = value.get("entities") + relations = value.get("relations") + task_predicates = value.get("task_predicates") + if not isinstance(entities, Sequence) or isinstance(entities, (str, bytes)): + raise ValueError("VLM visual facts entities must be a list.") + if not isinstance(relations, Sequence) or isinstance(relations, (str, bytes)): + raise ValueError("VLM visual facts relations must be a list.") + if not isinstance(task_predicates, Sequence) or isinstance( + task_predicates, (str, bytes) + ): + raise ValueError("VLM visual facts task_predicates must be a list.") + normalized_entities = [] + for index, item in enumerate(entities): + if not isinstance(item, Mapping): + raise ValueError(f"visual entities[{index}] must be a mapping.") + unsupported = set(item) - _VISUAL_ENTITY_KEYS + if unsupported: + raise ValueError( + f"visual entities[{index}] contains unsupported fields " + f"{sorted(unsupported)}." + ) + uid = item.get("uid") + camera_uid = item.get("camera_uid") + if not isinstance(uid, str) or not uid: + raise ValueError( + f"visual entities[{index}].uid must be a non-empty string." + ) + if not isinstance(camera_uid, str) or not camera_uid: + raise ValueError( + f"visual entities[{index}].camera_uid must be a non-empty string." + ) + if uid not in known_uids: + raise ValueError( + f"visual entities[{index}] references unknown UID {uid!r}." + ) + if camera_uid not in camera_uids: + raise ValueError( + f"visual entities[{index}] references unknown camera {camera_uid!r}." + ) + normalized = dict(item) + _reject_live_fields(normalized, f"visual entities[{index}]") + if "visible" in normalized and not isinstance(normalized["visible"], bool): + raise ValueError(f"visual entities[{index}].visible must be a boolean.") + if "bbox" in normalized: + normalized["bbox"] = _normalized_vector( + normalized["bbox"], 4, f"visual entities[{index}].bbox" + ) + x_min, y_min, x_max, y_max = normalized["bbox"] + if x_min >= x_max or y_min >= y_max: + raise ValueError( + f"visual entities[{index}].bbox must have non-zero ordered bounds." + ) + keypoints = normalized.get("keypoints", {}) + if not isinstance(keypoints, Mapping): + raise ValueError(f"visual entities[{index}].keypoints must be a mapping.") + normalized["keypoints"] = { + str(name): _normalized_vector(point, 2, f"keypoint {name!r}") + for name, point in keypoints.items() + } + if ( + normalized.get("visible", True) + and "bbox" not in normalized + and not normalized["keypoints"] + ): + raise ValueError( + f"visual entities[{index}] must include a bbox or keypoint evidence." + ) + normalized["confidence"] = _confidence( + normalized.get("confidence"), f"visual entities[{index}].confidence" + ) + normalized_entities.append(normalized) + normalized_relations = [] + for index, relation in enumerate(relations): + if not isinstance(relation, Mapping): + raise ValueError(f"visual relations[{index}] must be a mapping.") + unsupported = set(relation) - _VISUAL_RELATION_KEYS + if unsupported: + raise ValueError( + f"visual relations[{index}] contains unsupported fields " + f"{sorted(unsupported)}." + ) + relation_type = relation.get("type") + if ( + not isinstance(relation_type, str) + or relation_type not in VISUAL_RELATION_PARTICIPANTS + ): + raise ValueError( + f"visual relations[{index}] relation type must be one of " + f"{sorted(VISUAL_RELATION_PARTICIPANTS)}." + ) + participants = relation.get("uids", []) + if not isinstance(participants, Sequence) or isinstance( + participants, (str, bytes) + ): + raise ValueError(f"visual relations[{index}].uids must be a list.") + if any(not isinstance(uid, str) or not uid for uid in participants): + raise ValueError( + f"visual relations[{index}].uids must contain non-empty strings." + ) + expected_count = len(VISUAL_RELATION_PARTICIPANTS[relation_type]) + if len(participants) != expected_count: + raise ValueError( + f"visual relations[{index}].uids must contain exactly " + f"{expected_count} UIDs in canonical participant order." + ) + if len(set(participants)) != len(participants): + raise ValueError( + f"visual relations[{index}].uids must contain distinct UIDs." + ) + invalid = set(participants) - known_uids + if invalid: + raise ValueError( + f"visual relations[{index}] has unknown UIDs {sorted(invalid)}." + ) + normalized = dict(relation) + _reject_live_fields(normalized, f"visual relations[{index}]") + normalized["confidence"] = _confidence( + normalized.get("confidence"), f"visual relations[{index}].confidence" + ) + normalized_relations.append(normalized) + normalized_task_predicates = [] + allowed_predicates = {str(item) for item in allowed_task_predicates} + for index, predicate in enumerate(task_predicates): + if not isinstance(predicate, Mapping): + raise ValueError(f"visual task_predicates[{index}] must be a mapping.") + unsupported = set(predicate) - _VISUAL_TASK_PREDICATE_KEYS + if unsupported or set(predicate) != _VISUAL_TASK_PREDICATE_KEYS: + raise ValueError( + f"visual task_predicates[{index}] requires exactly fields " + f"{sorted(_VISUAL_TASK_PREDICATE_KEYS)}." + ) + predicate_type = predicate.get("type") + if ( + not isinstance(predicate_type, str) + or predicate_type not in allowed_predicates + ): + raise ValueError( + f"visual task_predicates[{index}].type must be one of " + f"{sorted(allowed_predicates)}." + ) + normalized = dict(predicate) + _reject_live_fields(normalized, f"visual task_predicates[{index}]") + normalized["confidence"] = _confidence( + normalized.get("confidence"), + f"visual task_predicates[{index}].confidence", + ) + normalized_task_predicates.append(normalized) + _reject_live_fields( + { + "entities": normalized_entities, + "relations": normalized_relations, + "task_predicates": normalized_task_predicates, + }, + "VLM visual facts", + ) + return { + "entities": normalized_entities, + "relations": normalized_relations, + "task_predicates": normalized_task_predicates, + "confidence": confidence, + } + + +def _visual_facts_schema( + allowed_task_predicates: Collection[str], +) -> dict[str, Any]: + """Return the visual-fact schema specialized for the current task.""" + schema = deepcopy(_VISUAL_FACTS_SCHEMA) + predicate_schema = schema["properties"]["task_predicates"] + allowed = sorted(str(item) for item in allowed_task_predicates) + if allowed: + predicate_schema["items"]["properties"]["type"]["enum"] = allowed + else: + predicate_schema["maxItems"] = 0 + return schema + + +def _default_structured_caller( + *, + prompt: str, + images: Sequence[str], + schema: Mapping[str, Any], + model: str | None, +) -> Mapping[str, Any]: + from langchain_core.messages import HumanMessage, SystemMessage + from langchain_openai import ChatOpenAI + + from .planner import ( + _coerce_model_response, + _is_mimo_compatible, + _load_llm_settings, + _structured_output_runnable, + ) + + settings = _load_llm_settings(model=model) + kwargs: dict[str, Any] = { + "api_key": settings["api_key"], + "model": settings["model"], + "temperature": 0, + "http_socket_options": (), + } + for key in ("base_url", "default_query"): + if settings[key]: + kwargs[key] = settings[key] + if _is_mimo_compatible(settings): + kwargs.update( + { + "max_completion_tokens": 4096, + "extra_body": {"thinking": {"type": "disabled"}}, + } + ) + client = ChatOpenAI(**kwargs) + structured = _structured_output_runnable( + client, + schema, + settings=settings, + ) + schema_prompt = ( + f"{prompt}\n\nReturn one JSON object conforming exactly to this JSON " + f"Schema:\n{json.dumps(schema, ensure_ascii=False, sort_keys=True)}" + ) + content: list[dict[str, Any]] = [{"type": "text", "text": prompt}] + content[0]["text"] = schema_prompt + content.extend( + {"type": "image_url", "image_url": {"url": image}} for image in images + ) + response = structured.invoke( + [ + SystemMessage( + content="Report visual facts only. Never reveal chain-of-thought." + ), + HumanMessage(content=content), + ] + ) + return _coerce_model_response(response) + + +def _vlm_model(explicit: str | None) -> str: + if isinstance(explicit, str) and explicit.strip(): + return explicit.strip() + from .planner import _GEN_SIM_ENV_PATH, _load_env_file + + local_env = _load_env_file(_GEN_SIM_ENV_PATH) + # A VLM-specific choice wins over the generic OpenAI default regardless of + # whether it comes from the shell or the project dotenv. Within each name, + # process variables retain their normal override behavior. + for key in ("ACTION_ENGINE_VLM_MODEL", "OPENAI_MODEL"): + for source in (os.environ, local_env): + value = source.get(key) + if isinstance(value, str) and value.strip(): + return value.strip() + raise ValueError( + "A VLM model is required through --vlm-model, agent_config.vlm_model, " + "ACTION_ENGINE_VLM_MODEL, or OPENAI_MODEL." + ) + + +def _rgb_data_url(value: torch.Tensor) -> str: + from PIL import Image + + image = value + if image.ndim != 3 or image.shape[-1] not in {3, 4}: + raise ValueError("Camera RGB must have shape (H, W, 3|4).") + if image.dtype != torch.uint8: + image = image.float() + if float(image.max()) <= 1.0: + image = image * 255.0 + image = image.clamp(0, 255).to(torch.uint8) + stream = BytesIO() + Image.fromarray(image.numpy()).convert("RGB").save(stream, format="PNG") + return "data:image/png;base64," + base64.b64encode(stream.getvalue()).decode( + "ascii" + ) + + +def _camera_evidence( + observation: SceneObservation, +) -> tuple[list[dict[str, Any]], list[str]]: + """Package calibrated RGB/depth evidence in a stable camera order.""" + manifest: list[dict[str, Any]] = [] + images: list[str] = [] + for camera in observation.cameras: + rgb_index = len(images) + images.append(_rgb_data_url(camera.rgb)) + item: dict[str, Any] = { + "uid": camera.uid, + "rgb_image_index": rgb_index, + "depth_available": camera.depth is not None, + "intrinsics": _calibration_list(camera.intrinsics), + "extrinsics": _calibration_list(camera.extrinsics), + } + if camera.depth is not None: + item["depth_image_index"] = len(images) + images.append(_depth_data_url(camera.depth)) + manifest.append(item) + return manifest, images + + +def _calibration_list(value: torch.Tensor | None) -> list[Any] | None: + """Serialize finite calibration tensors for the transient VLM prompt.""" + if value is None: + return None + tensor = torch.as_tensor(value).detach().cpu() + if not bool(torch.isfinite(tensor).all()): + raise ValueError("Camera calibration contains non-finite values.") + return tensor.tolist() + + +def _depth_data_url(value: torch.Tensor) -> str: + """Render one depth frame as a normalized grayscale VLM evidence image.""" + from PIL import Image + + depth = torch.as_tensor(value).detach().cpu().float() + if depth.ndim == 3 and depth.shape[-1] == 1: + depth = depth[..., 0] + elif depth.ndim == 3 and depth.shape[0] == 1: + depth = depth[0] + if depth.ndim != 2: + raise ValueError("Camera depth must have shape (H, W) or a singleton channel.") + finite = torch.isfinite(depth) + if not bool(finite.any()): + raise ValueError("Camera depth contains no finite values.") + minimum = depth[finite].min() + maximum = depth[finite].max() + normalized = torch.zeros_like(depth) + if float(maximum - minimum) > 0.0: + normalized[finite] = (depth[finite] - minimum) / (maximum - minimum) + image = (normalized.clamp(0.0, 1.0) * 255.0).to(torch.uint8).numpy() + stream = BytesIO() + Image.fromarray(image, mode="L").save(stream, format="PNG") + return "data:image/png;base64," + base64.b64encode(stream.getvalue()).decode( + "ascii" + ) + + +def _env_row( + value: Any, + env_id: int, + *, + num_envs: int | None = None, + unbatched_ndim: int | tuple[int, ...] | None = None, +) -> torch.Tensor: + """Select one vectorized environment row without slicing image dimensions. + + Sensor APIs return either ``(num_envs, ...)`` or an unbatched ``(...)`` + tensor. The old ``shape[0] > env_id`` heuristic sliced the first image row + for an unbatched ``(H, W, C)`` RGB tensor and similarly corrupted 4x4 poses. + Prefer the known environment count and only use the legacy heuristic when + no count is available. + """ + tensor = torch.as_tensor(value) + if unbatched_ndim is not None: + allowed_ndim = ( + (unbatched_ndim,) + if isinstance(unbatched_ndim, int) + else tuple(unbatched_ndim) + ) + if tensor.ndim in allowed_ndim: + return tensor + if tensor.ndim and num_envs is not None and tensor.shape[0] == int(num_envs): + if env_id >= tensor.shape[0]: + raise ValueError("env_id is outside the sensor batch dimension.") + return tensor[env_id] + if num_envs is None and tensor.ndim and tensor.shape[0] > env_id: + return tensor[env_id] + return tensor + + +def _optional_call( + sensor: Any, name: str, env_id: int, *, num_envs: int | None = None, **kwargs: Any +) -> torch.Tensor | None: + method = getattr(sensor, name, None) + if not callable(method): + return None + try: + value = method(env_id=env_id, **kwargs) + except TypeError: + try: + value = method(env_id, **kwargs) + except TypeError: + value = method(**kwargs) + value = torch.as_tensor(value) + # Calibration methods commonly return an unbatched matrix even for a + # vectorized simulator. Select a leading environment row only when the + # shape cannot itself be a canonical calibration matrix. This preserves + # 3x3/4x4 matrices while correctly handling batched compact vectors such as + # ``(num_envs, 4)``. + unbatched_matrix = value.ndim == 2 and tuple(value.shape) in { + (3, 3), + (4, 4), + } + if ( + num_envs is not None + and value.ndim >= 1 + and value.shape[0] == int(num_envs) + and not unbatched_matrix + ): + value = value[env_id] + return value.detach().cpu() + + +def _reject_live_fields(value: Any, context: str) -> None: + """Reject nested simulator state/geometry fields in VLM facts.""" + if isinstance(value, Mapping): + for key, child in value.items(): + if str(key).lower() in _FORBIDDEN_LIVE_KEYS: + raise ValueError( + f"{context} contains forbidden live-state field {key!r}." + ) + _reject_live_fields(child, f"{context}.{key}") + elif isinstance(value, Sequence) and not isinstance(value, (str, bytes, bytearray)): + for index, child in enumerate(value): + _reject_live_fields(child, f"{context}[{index}]") + + +def _normalized_vector(value: Any, size: int, context: str) -> list[float]: + if ( + not isinstance(value, Sequence) + or isinstance(value, (str, bytes)) + or len(value) != size + ): + raise ValueError(f"{context} must contain {size} normalized values.") + if any( + not isinstance(item, (int, float)) or isinstance(item, bool) for item in value + ): + raise ValueError(f"{context} values must be numeric.") + result = [float(item) for item in value] + if any(not math.isfinite(item) or item < 0.0 or item > 1.0 for item in result): + raise ValueError(f"{context} values must lie in [0, 1].") + return result + + +def _confidence(value: Any, context: str) -> float: + if not isinstance(value, (int, float)) or isinstance(value, bool): + raise ValueError(f"{context} must be a number in [0, 1].") + result = float(value) + if not math.isfinite(result) or result < 0.0 or result > 1.0: + raise ValueError(f"{context} must lie in [0, 1].") + return result diff --git a/embodichain/gen_sim/action_engine/protocol.py b/embodichain/gen_sim/action_engine/protocol.py new file mode 100644 index 000000000..ef704952d --- /dev/null +++ b/embodichain/gen_sim/action_engine/protocol.py @@ -0,0 +1,60 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Cross-layer identifiers owned by Action Engine. + +These values are serialized into generated artifacts, so changing one is a +protocol migration rather than a local rename. +""" + +from __future__ import annotations + +from typing import Final + +__all__ = [ + "ACTION_ENGINE_CONFIG_SCHEMA", + "ACTION_ENGINE_ENV_ID", + "AGENT_CONFIG_FILENAME", + "COMPARISON_FILENAME", + "EXECUTION_PROGRAM_FILENAME", + "EXECUTION_PROGRAM_SCHEMA", + "FAST_GYM_CONFIG_FILENAME", + "SCENE_REQUIREMENTS_FILENAME", + "SCENE_REQUIREMENTS_SCHEMA", + "SEED_TASK_GRAPH_PNG_FILENAME", + "SEED_GRAPH_SCHEMA", + "TASK_SPEC_FILENAME", + "TASK_SPEC_SCHEMA", + "TASK_AGENT_FILENAME", + "TASK_AGENT_SCHEMA", +] + +ACTION_ENGINE_ENV_ID: Final = "ActionEngine-v1" +ACTION_ENGINE_CONFIG_SCHEMA: Final = "action_engine_config_v2" +TASK_AGENT_SCHEMA: Final = "action_engine_task_agent_v1" +EXECUTION_PROGRAM_SCHEMA: Final = "action_engine_execution_graph_v1" +SEED_GRAPH_SCHEMA: Final = "action_engine_seed_graph_v3" +TASK_SPEC_SCHEMA: Final = "action_engine_task_spec_v2" +SCENE_REQUIREMENTS_SCHEMA: Final = "action_engine_scene_requirements_v2" + +FAST_GYM_CONFIG_FILENAME: Final = "fast_gym_config.json" +AGENT_CONFIG_FILENAME: Final = "agent_config.json" +TASK_AGENT_FILENAME: Final = "task_agent.json" +EXECUTION_PROGRAM_FILENAME: Final = "seed_task_graph.json" +SEED_TASK_GRAPH_PNG_FILENAME: Final = "seed_task_graph.png" +TASK_SPEC_FILENAME: Final = "task_spec.json" +SCENE_REQUIREMENTS_FILENAME: Final = "scene_requirements.json" +COMPARISON_FILENAME: Final = "comparison.json" diff --git a/embodichain/gen_sim/action_engine/runtime/__init__.py b/embodichain/gen_sim/action_engine/runtime/__init__.py new file mode 100644 index 000000000..98e30f364 --- /dev/null +++ b/embodichain/gen_sim/action_engine/runtime/__init__.py @@ -0,0 +1,32 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Persisted execution-program loading contracts.""" + +from __future__ import annotations + +from .loader import load_agent_execution_program, load_execution_program +from .models import ExecutionProgram, ExecutionReport, ExecutionResult +from .state import ExecutionState + +__all__ = [ + "ExecutionProgram", + "ExecutionReport", + "ExecutionResult", + "ExecutionState", + "load_agent_execution_program", + "load_execution_program", +] diff --git a/embodichain/gen_sim/action_engine/runtime/loader.py b/embodichain/gen_sim/action_engine/runtime/loader.py new file mode 100644 index 000000000..a157e8dee --- /dev/null +++ b/embodichain/gen_sim/action_engine/runtime/loader.py @@ -0,0 +1,296 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Load or compile execution programs without publishing intermediate copies.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import replace +import json +from pathlib import Path +from typing import Any + +from embodichain.gen_sim.action_engine.protocol import ( + ACTION_ENGINE_CONFIG_SCHEMA, + EXECUTION_PROGRAM_SCHEMA, + SEED_GRAPH_SCHEMA, +) + +from .models import ExecutionProgram + +__all__ = [ + "load_agent_execution_program", + "load_execution_program", +] + + +def _read_json(path: Path, *, label: str) -> dict[str, Any]: + try: + value = json.loads(path.read_text(encoding="utf-8")) + except OSError as exc: + raise ValueError(f"Unable to read {label} at {path}: {exc}") from exc + except json.JSONDecodeError as exc: + raise ValueError(f"{label} at {path} is not valid JSON: {exc}") from exc + if not isinstance(value, Mapping): + raise ValueError(f"{label} must contain a JSON object.") + return dict(value) + + +def load_execution_program( + source: Mapping[str, Any] | str | Path, + *, + known_objects: set[str] | None = None, + registry: Any | None = None, + require_executable: bool = True, +) -> ExecutionProgram: + """Load a v3 SeedGraph and reject every legacy execution schema.""" + value = ( + dict(source) + if isinstance(source, Mapping) + else _read_json(Path(source).expanduser().resolve(), label="execution program") + ) + schema = value.get("schema_version") + if schema == SEED_GRAPH_SCHEMA: + from embodichain.gen_sim.action_engine.capabilities import ( + build_atomic_capability_registry, + ) + from embodichain.gen_sim.action_engine.compiler import ( + seed_graph_to_execution_program, + ) + from embodichain.gen_sim.action_engine.domain import validate_seed_graph + from embodichain.gen_sim.action_engine.planning.linker import ( + validate_persisted_contracts, + ) + + registry = registry or build_atomic_capability_registry() + seed = validate_seed_graph( + value, + known_objects=known_objects, + known_actions=registry.names(), + executable_actions=registry.executable_names(), + require_executable=require_executable, + ) + validate_persisted_contracts(seed, registry) + internal = seed_graph_to_execution_program( + seed, + known_objects=known_objects, + registry=registry, + require_executable=require_executable, + ) + return replace(ExecutionProgram.from_mapping(internal), seed_graph=seed) + if schema == "action_engine_seed_graph_v2": + raise ValueError( + "SeedGraph v2 lacks persisted Action Contracts and cannot be loaded; " + "regenerate seed_task_graph.json and agent_config.json with the current " + "generator to produce action_engine_seed_graph_v3." + ) + if schema == EXECUTION_PROGRAM_SCHEMA: + raise ValueError( + "Action Engine v1 execution programs are no longer accepted; " + "regenerate the task to produce action_engine_seed_graph_v3." + ) + raise ValueError(f"Unsupported Action Engine graph schema {schema!r}.") + + +def _resolve_config_path( + config: Mapping[str, Any], + config_path: str | Path, + *keys: str, +) -> Path | None: + base = Path(config_path).expanduser().resolve().parent + for key in keys: + value = config.get(key) + if value is None: + continue + if not isinstance(value, str) or not value: + raise ValueError(f"agent_config.{key} must be a non-empty path string.") + path = Path(value).expanduser() + return path.resolve() if path.is_absolute() else (base / path).resolve() + return None + + +def load_agent_execution_program( + agent_config: Mapping[str, Any], + *, + agent_config_path: str | Path, + regenerate: bool = False, + require_executable: bool = True, +) -> ExecutionProgram: + """Resolve an agent config and optionally rebuild its SeedGraph in memory. + + ``--regenerate`` intentionally does not write a second graph artifact. The + deterministic compiler result is validated and handed directly to runtime. + """ + if agent_config.get("schema_version") != ACTION_ENGINE_CONFIG_SCHEMA: + raise ValueError( + "This Action Engine runtime accepts only v2 bundles. Regenerate " + "task_spec.json, scene_requirements.json, seed_task_graph.json, " + "and agent_config.json " + "with the current generator." + ) + known_objects = _known_objects(agent_config) + task_path = _resolve_config_path( + agent_config, + agent_config_path, + "task_spec", + "task_spec_path", + ) + execution_path = _resolve_config_path( + agent_config, + agent_config_path, + "seed_task_graph", + "seed_task_graph_path", + "offline_seed_task_graph", + "offline_seed_task_graph_path", + ) + if regenerate: + if task_path is None: + raise ValueError("--regenerate requires agent_config.task_spec.") + task_spec = _read_json(task_path, label="task specification") + reference_graph = ( + _read_json(execution_path, label="SeedGraph") + if execution_path is not None and execution_path.is_file() + else None + ) + program = load_execution_program( + _regenerate_seed_graph(task_spec, reference_graph=reference_graph), + known_objects=known_objects, + require_executable=require_executable, + ) + elif execution_path is None: + if task_path is None: + raise ValueError("agent_config requires seed_task_graph or task_spec.") + task_spec = _read_json(task_path, label="task specification") + program = load_execution_program( + _regenerate_seed_graph(task_spec), + known_objects=known_objects, + require_executable=require_executable, + ) + else: + program = load_execution_program( + execution_path, + known_objects=known_objects, + require_executable=require_executable, + ) + _verify_agent_program(agent_config, program) + _verify_program_objects(agent_config, program) + return program + + +def _known_objects(agent_config: Mapping[str, Any]) -> set[str] | None: + source = agent_config.get("source") + if not isinstance(source, Mapping): + return None + uid_map = source.get("uid_map") + if not isinstance(uid_map, Mapping): + return None + values = {str(uid) for uid in uid_map.values() if str(uid)} + return values or None + + +def _verify_agent_program( + agent_config: Mapping[str, Any], + program: ExecutionProgram, +) -> None: + """Reject a valid program that belongs to a different generated bundle.""" + configured_task = agent_config.get("task_name") + if configured_task is not None and configured_task != program.task: + raise ValueError( + f"agent_config.task_name {configured_task!r} does not match " + f"execution program task {program.task!r}." + ) + expected_hash = agent_config.get("seed_task_graph_hash") + if expected_hash is None: + return + if not isinstance(expected_hash, str) or not expected_hash: + raise ValueError( + "agent_config.seed_task_graph_hash must be a non-empty string." + ) + if program.seed_graph is not None: + from embodichain.gen_sim.action_engine.domain import seed_graph_hash + + actual_hash = seed_graph_hash(program.seed_graph) + else: + from embodichain.gen_sim.action_engine.domain import execution_program_hash + + actual_hash = execution_program_hash(program.raw) + if actual_hash != expected_hash: + raise ValueError( + "SeedGraph hash does not match agent_config; regenerate the " + "configuration bundle before running it." + ) + + +def _regenerate_seed_graph( + task_spec: Mapping[str, Any], + *, + reference_graph: Mapping[str, Any] | None = None, +) -> dict[str, Any]: + from embodichain.gen_sim.action_engine.domain import validate_task_spec + + task = validate_task_spec(task_spec) + oracle = task.get("oracle", {}) + reference = oracle.get("reference_seed_graph") + if isinstance(reference, Mapping): + return dict(reference) + metadata = task.get("metadata", {}) + bindings = metadata.get("role_bindings", {}) + if not isinstance(bindings, Mapping): + raise ValueError("TaskSpec.metadata.role_bindings must be a mapping.") + if not bindings and reference_graph is not None: + graph_metadata = reference_graph.get("metadata", {}) + if not isinstance(graph_metadata, Mapping): + raise ValueError("SeedGraph.metadata must be a mapping.") + bindings = graph_metadata.get("role_bindings", {}) + if not isinstance(bindings, Mapping): + raise ValueError("SeedGraph.metadata.role_bindings must be a mapping.") + from embodichain.gen_sim.action_engine.tasks import instantiate_seed_graph + + return instantiate_seed_graph(task, bindings) + + +def _verify_program_objects( + agent_config: Mapping[str, Any], + program: ExecutionProgram, +) -> None: + known = _known_objects(agent_config) + if known is None: + return + references = {step.object_uid for step in program.semantic_steps} + for step in program.semantic_steps: + for key in ( + "reference_object", + "support_object", + "orientation_reference_object", + ): + value = step.goal.get(key) + if isinstance(value, str): + references.add(value) + for payload in step.goal.get("payloads", []): + value = payload.get("object") if isinstance(payload, Mapping) else payload + if isinstance(value, str): + references.add(value) + for content in step.goal.get("contents", []): + value = content.get("object") if isinstance(content, Mapping) else content + if isinstance(value, str): + references.add(value) + unknown = references - known - {"self", "table", "table_center"} + if unknown: + raise ValueError( + "Execution Program references objects not present in the scene: " + f"{sorted(unknown)}. Regenerate the configuration bundle." + ) diff --git a/embodichain/gen_sim/action_engine/runtime/models.py b/embodichain/gen_sim/action_engine/runtime/models.py new file mode 100644 index 000000000..103613a83 --- /dev/null +++ b/embodichain/gen_sim/action_engine/runtime/models.py @@ -0,0 +1,316 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Small typed runtime views over the serialized execution program.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +from dataclasses import dataclass, field +from typing import Any + +import numpy as np +import torch + +from embodichain.lab.sim.atomic_actions import StateDelta + +from .state import ExecutionState + +__all__ = [ + "ActionOutcome", + "ExecutionEdge", + "ExecutionProgram", + "ExecutionReport", + "ExecutionResult", + "GroundedAction", + "SemanticStep", +] + + +@dataclass(frozen=True) +class ExecutionEdge: + """One executable DAG edge containing symbolic atomic actions.""" + + id: str + source: str + target: str + actions: tuple[dict[str, Any], ...] + depends_on: tuple[str, ...] = () + resources: tuple[str, ...] = () + + +@dataclass(frozen=True) +class SemanticStep: + """One closed-loop intent expanded into one or more execution edges.""" + + id: str + parent_step_id: str + operator: str + object_uid: str + actor: dict[str, Any] + goal: dict[str, Any] + depends_on: tuple[str, ...] + postcondition: dict[str, Any] + edge_ids: tuple[str, ...] + + +@dataclass(frozen=True) +class ExecutionProgram: + """Validated in-memory form of ``action_engine_execution_program_v1``.""" + + raw: dict[str, Any] + task: str + start: str + goal: str + nodes: tuple[dict[str, Any], ...] + edges: tuple[ExecutionEdge, ...] + semantic_steps: tuple[SemanticStep, ...] + allocation_groups: tuple[dict[str, Any], ...] + seed_graph: dict[str, Any] | None = None + + @classmethod + def from_mapping(cls, value: Mapping[str, Any]) -> "ExecutionProgram": + """Construct an immutable runtime view from a validated mapping.""" + raw = deepcopy(dict(value)) + edges = tuple( + ExecutionEdge( + id=str(edge["id"]), + source=str(edge["source"]), + target=str(edge["target"]), + actions=tuple( + deepcopy(dict(action)) + for action in edge.get("actions", edge.get("symbolic_actions", ())) + ), + depends_on=tuple(str(item) for item in edge.get("depends_on", ())), + resources=tuple(str(item) for item in edge.get("resources", ())), + ) + for edge in raw["edges"] + ) + steps = tuple( + SemanticStep( + id=str(step["id"]), + parent_step_id=str(step["parent_step_id"]), + operator=str(step["operator"]), + object_uid=str(step.get("object", step.get("object_uid", ""))), + actor=deepcopy(dict(step["actor"])), + goal=deepcopy(dict(step.get("goal", {}))), + depends_on=tuple(str(item) for item in step.get("depends_on", ())), + postcondition=deepcopy(dict(step.get("postcondition", {}))), + edge_ids=tuple(str(item) for item in step["edge_ids"]), + ) + for step in raw["semantic_steps"] + ) + return cls( + raw=raw, + task=str(raw.get("task", raw.get("task_name", "task"))), + start=str(raw["start"]), + goal=str(raw["goal"]), + nodes=tuple(deepcopy(raw["nodes"])), + edges=edges, + semantic_steps=steps, + allocation_groups=tuple( + deepcopy(dict(group)) for group in raw.get("allocation_groups", ()) + ), + seed_graph=None, + ) + + +@dataclass(frozen=True) +class GroundedAction: + """A public atomic-action target resolved from the current simulator state.""" + + action_class: str + arm: str + control: str + target: Any + cfg: dict[str, Any] + object_pose: torch.Tensor | None = None + reference_pose: torch.Tensor | None = None + target_object_pose: torch.Tensor | None = None + motion_policy: dict[str, Any] = field(default_factory=dict) + object_uid: str | None = None + """Scene UID of the object whose semantic step produced this action.""" + allow_yaw_search: bool = False + """Whether planning may vary world-Z yaw without changing task semantics.""" + + +@dataclass +class ActionOutcome: + """Planning output kept in full-robot coordinates.""" + + trajectory: torch.Tensor + success: torch.Tensor + next_state: ExecutionState + grounded: GroundedAction + prior_state: ExecutionState | None = None + expected_effects: StateDelta | None = None + planner_trace: dict[str, Any] = field(default_factory=dict) + + def state_after(self, verified: torch.Tensor) -> ExecutionState: + """Commit expected effects only for physically verified rows.""" + if self.prior_state is None or self.expected_effects is None: + return self.next_state + mask = torch.as_tensor( + verified, + dtype=torch.bool, + device=self.trajectory.device, + ).reshape(-1) + if mask.numel() != self.trajectory.shape[0]: + raise ValueError("Verified mask must match the ActionOutcome batch.") + terminal_qpos = ( + self.trajectory[:, -1] + if self.trajectory.shape[1] + else self.prior_state.last_qpos + ) + qpos = torch.where( + mask[:, None], + terminal_qpos, + self.prior_state.last_qpos, + ) + task = self.expected_effects.apply( + self.prior_state.to_task_state(), + mask, + ) + return ExecutionState.from_task_state(task, last_qpos=qpos) + + @property + def cost(self) -> torch.Tensor: + """Return joint-path length for each vectorized environment.""" + if self.trajectory.shape[1] < 2: + return torch.zeros( + self.trajectory.shape[0], + dtype=torch.float32, + device=self.trajectory.device, + ) + return torch.linalg.vector_norm( + torch.diff(self.trajectory, dim=1), + dim=-1, + ).sum(dim=1) + + +@dataclass +class ExecutionResult(Sequence[torch.Tensor]): + """Result marker used by the existing demonstration-runner contract.""" + + actions: list[torch.Tensor] + success: torch.Tensor + semantic_success: dict[str, torch.Tensor] + record_dir: str | None = None + already_executed: bool = True + retry_count: int = 0 + recovery_count: int = 0 + revision_count: int = 0 + failure_events: list[dict[str, Any]] = field(default_factory=list) + runtime_revisions: list[dict[str, Any]] = field(default_factory=list) + retry_counts: list[int] = field(default_factory=list) + + @property + def runtime_success(self) -> torch.Tensor: + return self.success + + @property + def runtime_graph_output_dir(self) -> str | None: + return self.record_dir + + def __len__(self) -> int: + return len(self.actions) + + def __iter__(self): + return iter(self.actions) + + def __getitem__(self, index): + return self.actions[index] + + +@dataclass(frozen=True) +class ExecutionReport: + """JSON-safe Task Engine result built from an ``ExecutionResult``. + + The runtime result deliberately keeps tensors because the legacy demo + runner consumes them. The Task Engine boundary instead exposes only a + compact, serializable audit view and never retains the action tensors. + """ + + task_id: str + plan_hash: str + action_graph_hash: str + status: str + run_id: str + episode_id: str + provenance: dict[str, Any] + environments: tuple[dict[str, Any], ...] = () + action_count: int = 0 + retry_count: int = 0 + recovery_count: int = 0 + revision_count: int = 0 + failure_events: tuple[dict[str, Any], ...] = () + graph_revisions: tuple[dict[str, Any], ...] = () + record_dir: str | None = None + error: str | None = None + schema_version: str = "action_engine_execution_report_v2" + + def as_mapping(self) -> dict[str, Any]: + """Return a detached mapping suitable for strict JSON serialization.""" + return { + "schema_version": self.schema_version, + "task_id": self.task_id, + "plan_hash": self.plan_hash, + "action_graph_hash": self.action_graph_hash, + "status": self.status, + "run_id": self.run_id, + "episode_id": self.episode_id, + "provenance": deepcopy(self.provenance), + "environments": deepcopy(list(self.environments)), + "action_count": self.action_count, + "retry_count": self.retry_count, + "recovery_count": self.recovery_count, + "revision_count": self.revision_count, + "failure_events": deepcopy(list(self.failure_events)), + "graph_revisions": deepcopy(list(self.graph_revisions)), + "record_dir": self.record_dir, + "error": self.error, + } + + def to_dict(self) -> dict[str, Any]: + """Compatibility spelling for artifact and CLI publishers.""" + return self.as_mapping() + + +def success_mask(value: bool | torch.Tensor, count: int, device: Any) -> torch.Tensor: + """Normalize a primitive's scalar or batched success result.""" + mask = torch.as_tensor(value, dtype=torch.bool, device=device).reshape(-1) + if mask.numel() == 1: + return mask.repeat(count) + if mask.numel() != count: + raise ValueError( + f"Atomic action success has {mask.numel()} values; expected {count}." + ) + return mask + + +def trajectory_cost_numpy(value: torch.Tensor) -> np.ndarray: + """Expose trajectory costs to assignment solvers without retaining gradients.""" + if value.shape[1] < 2: + return np.zeros(value.shape[0], dtype=np.float64) + diffs = torch.diff(value.detach(), dim=1) + return ( + torch.linalg.vector_norm(diffs, dim=-1) + .sum(dim=1) + .cpu() + .numpy() + .astype(np.float64) + ) diff --git a/embodichain/gen_sim/action_engine/runtime/motion_policy.py b/embodichain/gen_sim/action_engine/runtime/motion_policy.py new file mode 100644 index 000000000..8de7cd4bf --- /dev/null +++ b/embodichain/gen_sim/action_engine/runtime/motion_policy.py @@ -0,0 +1,106 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +from typing import Any + +from embodichain.gen_sim.action_engine.config import default_runtime_policy +from embodichain.gen_sim.action_engine.domain.motion import validate_motion_policy + +__all__ = ["resolve_motion_policy", "with_motion_modifiers"] + +_PROFILE_ALIASES = dict( + franka="dual_franka", ur3="dual_ur3", ur5="dual_ur5", ur10="dual_ur10" +) + + +def resolve_motion_policy( + robot_profile: str, + atomic_action: str, + policy_spec: Mapping[str, Any], + *, + motion_defaults: Mapping[str, Mapping[str, Any]] | None = None, + motion_modifiers: ( + Mapping[str, Mapping[str, Mapping[str, Mapping[str, Any]]]] | None + ) = None, + inline_overrides: Mapping[str, Any] | None = None, +) -> dict[str, Any]: + """Resolve an action base policy plus its composable typed modifiers.""" + profile = _PROFILE_ALIASES.get(str(robot_profile), str(robot_profile)) + runtime_policy = ( + default_runtime_policy(profile) + if motion_defaults is None or motion_modifiers is None + else None + ) + defaults = ( + runtime_policy.motion_defaults + if motion_defaults is None and runtime_policy is not None + else motion_defaults + ) + modifiers = ( + runtime_policy.motion_modifiers + if motion_modifiers is None and runtime_policy is not None + else motion_modifiers + ) + if defaults is None or modifiers is None: + raise ValueError("Motion defaults and modifiers must be provided together.") + action = str(atomic_action) + if action not in defaults: + raise ValueError(f"Unknown Action Engine motion base {action!r}.") + + spec = validate_motion_policy(policy_spec) + policy = deepcopy(dict(defaults[action])) + modifier_values: dict[str, Any] = {} + modifier_sources: dict[str, tuple[str, str]] = {} + for modifier in spec["modifiers"]: + modifier_type = modifier["type"] + mode = modifier["mode"] + patch = modifiers.get(modifier_type, {}).get(mode, {}).get(action) + if not isinstance(patch, Mapping): + raise ValueError( + f"Motion modifier {(modifier_type, mode)!r} is not supported " + f"by AtomicAction {action!r}." + ) + for key, value in patch.items(): + if key in modifier_values and modifier_values[key] != value: + raise ValueError( + f"Motion modifiers {modifier_sources[key]!r} and " + f"{(modifier_type, mode)!r} conflict on parameter {key!r}." + ) + modifier_values[key] = deepcopy(value) + modifier_sources[key] = (modifier_type, mode) + policy.update(modifier_values) + if inline_overrides is not None: + policy.update(deepcopy(dict(inline_overrides))) + return policy + + +def with_motion_modifiers( + policy_spec: Mapping[str, Any], + *modifiers: tuple[str, str], +) -> dict[str, Any]: + """Return a validated policy reference with missing modifiers appended.""" + policy = validate_motion_policy(policy_spec) + existing = { + (modifier["type"], modifier["mode"]) for modifier in policy["modifiers"] + } + for modifier_type, mode in modifiers: + if (modifier_type, mode) not in existing: + policy["modifiers"].append({"type": modifier_type, "mode": mode}) + return validate_motion_policy(policy) diff --git a/embodichain/gen_sim/action_engine/runtime/state.py b/embodichain/gen_sim/action_engine/runtime/state.py new file mode 100644 index 000000000..dbafc4748 --- /dev/null +++ b/embodichain/gen_sim/action_engine/runtime/state.py @@ -0,0 +1,84 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Action Engine execution state at the atomic-planning boundary.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Mapping + +import torch + +from embodichain.lab.sim.atomic_actions import ( + HeldObjectState, + TaskState, +) + +__all__ = ["ExecutionState"] + + +@dataclass(slots=True, eq=False) +class ExecutionState: + """Projected task state paired with the next full-robot planning seed. + + The simulation atomic-action package deliberately no longer exposes the + legacy ``WorldState`` compatibility object. Action Engine keeps this narrow + orchestration state locally and converts it to immutable ``TaskState`` and + ``PlanningContext`` values immediately before invoking the shared planner. + """ + + last_qpos: torch.Tensor + held_objects: dict[str, HeldObjectState] = field(default_factory=dict) + + def get_held_object(self, control_part: str) -> HeldObjectState | None: + """Return the held-object relation for one control part.""" + return self.held_objects.get(control_part) + + def with_updates( + self, + *, + last_qpos: torch.Tensor | None = None, + held_objects: Mapping[str, HeldObjectState] | None = None, + ) -> ExecutionState: + """Return a detached successor state.""" + return ExecutionState( + last_qpos=self.last_qpos if last_qpos is None else last_qpos, + held_objects=dict( + self.held_objects if held_objects is None else held_objects + ), + ) + + def to_task_state(self) -> TaskState: + """Convert this state to the shared immutable symbolic task contract.""" + return TaskState( + batch_size=int(self.last_qpos.shape[0]), + device=self.last_qpos.device, + held_objects=self.held_objects, + ) + + @classmethod + def from_task_state( + cls, + task: TaskState, + *, + last_qpos: torch.Tensor, + ) -> ExecutionState: + """Build an orchestration state from a committed or projected task state.""" + return cls( + last_qpos=last_qpos, + held_objects=dict(task.held_objects), + ) diff --git a/embodichain/gen_sim/action_engine/solver_profiles.py b/embodichain/gen_sim/action_engine/solver_profiles.py new file mode 100644 index 000000000..120fbea77 --- /dev/null +++ b/embodichain/gen_sim/action_engine/solver_profiles.py @@ -0,0 +1,84 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Generation-time IK solver selection for GenSim robot bundles.""" + +from __future__ import annotations + +from collections.abc import Mapping +from typing import Any, Final + +__all__ = [ + "IK_SOLVER_MODES", + "expected_ik_solver_class", + "resolve_ik_solver_mode", + "validate_robot_ik_solver_contract", +] + +IK_SOLVER_MODES: Final = ("auto", "ur", "pytorch") +_UR_PROFILES = frozenset({"dual_ur3", "dual_ur5", "dual_ur10"}) +_SUPPORTED_PROFILES = _UR_PROFILES | {"dual_franka"} +_CLASS_BY_MODE = {"ur": "URSolver", "pytorch": "PytorchSolver"} + + +def resolve_ik_solver_mode(mode: str, robot_profile: str) -> str: + """Resolve one requested mode to a concrete solver for a robot profile.""" + if not isinstance(mode, str): + raise TypeError( + "IK solver mode must be a string; expected one of: auto, ur, pytorch." + ) + if mode not in IK_SOLVER_MODES: + raise ValueError( + f"Unsupported IK solver mode {mode!r}; expected one of: " + "auto, ur, pytorch." + ) + profile = str(robot_profile) + if profile not in _SUPPORTED_PROFILES: + raise ValueError(f"Unsupported IK solver robot profile {profile!r}.") + resolved = "pytorch" if mode == "auto" and profile == "dual_franka" else mode + if resolved == "auto": + resolved = "ur" + if resolved == "ur" and profile == "dual_franka": + raise ValueError("Franka does not support the analytical URSolver.") + return resolved + + +def expected_ik_solver_class(mode: str) -> str: + """Return the serialized/runtime class name for one concrete mode.""" + if mode not in _CLASS_BY_MODE: + raise ValueError("Concrete IK solver mode must be 'ur' or 'pytorch'.") + return _CLASS_BY_MODE[mode] + + +def validate_robot_ik_solver_contract( + robot: Mapping[str, Any], + mode: str, +) -> None: + """Validate that both generated arms use the declared concrete solver.""" + expected = expected_ik_solver_class(mode) + solvers = robot.get("solver_cfg") + if not isinstance(solvers, Mapping): + raise ValueError("Generated robot requires a solver_cfg mapping.") + for arm in ("left_arm", "right_arm"): + solver = solvers.get(arm) + if not isinstance(solver, Mapping): + raise ValueError(f"Generated robot requires solver_cfg.{arm}.") + actual = solver.get("class_type") + if actual != expected: + raise ValueError( + f"Generated robot {arm} must use {expected} for ik_solver={mode!r}, " + f"got {actual!r}." + ) diff --git a/embodichain/gen_sim/action_engine/tasks/__init__.py b/embodichain/gen_sim/action_engine/tasks/__init__.py new file mode 100644 index 000000000..843a55efa --- /dev/null +++ b/embodichain/gen_sim/action_engine/tasks/__init__.py @@ -0,0 +1,50 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Task-first generation and scene hand-off for Action Engine v2.""" + +from __future__ import annotations + +from .assembly import GroundedTaskSpec +from .interpretation import ( + GroundingCaller, + INSTRUCTION_INTENT_SCHEMA, + InstructionDraftResult, + InstructionCaller, + InstructionIntent, + ground_instruction_draft, + interpret_instruction_draft, + interpret_and_ground_task_spec, + validate_instruction_intent, +) +from .recipes import instantiate_seed_graph +from .scene import SceneHandoff, validate_scene_handoff + +__all__ = [ + "GroundedTaskSpec", + "GroundingCaller", + "INSTRUCTION_INTENT_SCHEMA", + "InstructionDraftResult", + "InstructionCaller", + "InstructionIntent", + "SceneHandoff", + "ground_instruction_draft", + "instantiate_seed_graph", + "interpret_instruction_draft", + "interpret_and_ground_task_spec", + "validate_instruction_intent", + "validate_scene_handoff", +] diff --git a/embodichain/gen_sim/action_engine/tasks/assembly.py b/embodichain/gen_sim/action_engine/tasks/assembly.py new file mode 100644 index 000000000..f4f954488 --- /dev/null +++ b/embodichain/gen_sim/action_engine/tasks/assembly.py @@ -0,0 +1,417 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Language-neutral scene inventory and grounded TaskSpec assembly.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +from dataclasses import dataclass, field +from typing import Any + +from embodichain.gen_sim.action_engine.domain import ( + TASK_CONTRACTS, + task_contract, + task_success_type, + validate_scene_requirements, + validate_task_spec, +) +from embodichain.gen_sim.action_engine.generation.config_builder import ( + canonical_robot_profile, +) +from embodichain.gen_sim.action_engine.protocol import ( + SCENE_REQUIREMENTS_SCHEMA, + TASK_SPEC_SCHEMA, +) + +__all__ = [ + "GroundedTaskBuilder", + "GroundedTaskSpec", + "SceneEntity", + "SceneInventory", + "validate_source_compatibility", + "validate_target_compatibility", +] + + +@dataclass(frozen=True) +class GroundedTaskSpec: + """One explicit TaskSpec plus verified scene role bindings.""" + + task_spec: dict[str, Any] + scene_requirements: dict[str, Any] + role_bindings: dict[str, str] + + +@dataclass(frozen=True) +class SceneEntity: + """One scene entity with source semantics preserved verbatim.""" + + uid: str + role: str + name: str + description: str + category: str + color: str | None + position: tuple[float, float, float] + affordances: frozenset[str] = frozenset() + initial_state: Mapping[str, Any] = field(default_factory=dict) + attributes: Mapping[str, Any] = field(default_factory=dict) + source_uid: str = "" + + +class SceneInventory: + """Structural scene index without natural-language matching rules.""" + + _PASSIVE_ROLES = frozenset( + { + "background", + "camera", + "light", + "robot", + "sensor", + "support_surface", + "table", + } + ) + + def __init__( + self, + scene_objects: Sequence[Mapping[str, Any]], + *, + robot_profile: str, + ) -> None: + self.profile = canonical_robot_profile(robot_profile) + self.entities = tuple(_scene_entity(item) for item in scene_objects) + self.by_uid = {entity.uid: entity for entity in self.entities} + if len(self.by_uid) != len(self.entities): + raise ValueError("Scene inventory contains duplicate runtime UIDs.") + self.support = tuple( + entity + for entity in self.entities + if entity.uid == "table" or entity.role in {"table", "support_surface"} + ) + self.passive = tuple( + entity + for entity in self.entities + if entity in self.support or entity.role in self._PASSIVE_ROLES + ) + self.interactive = tuple( + entity for entity in self.entities if entity not in self.passive + ) + if not self.interactive: + raise ValueError("Task planning requires at least one interaction object.") + + @property + def movable(self) -> tuple[SceneEntity, ...]: + """Compatibility alias for callers that mean source candidates.""" + return self.interactive + + def left_score(self, entity: SceneEntity) -> float: + """Return robot-relative lateral score; positive values are left. + + Generated dual-arm profiles share one final world layout: the semantic + left arm is on world ``-Y`` after all robot-level transforms. + """ + return -entity.position[1] + + +class GroundedTaskBuilder: + """Assemble grounded E1-E9 instances without parsing instruction text.""" + + def __init__( + self, + task_id: str, + instruction: str, + inventory: SceneInventory, + *, + planner: str = "structured_llm_v2", + ) -> None: + self.task_id = task_id + self.instruction = instruction + self.inventory = inventory + self.planner = planner + self.instances: list[dict[str, Any]] = [] + self.role_by_uid: dict[str, str] = {} + self.requirements: dict[str, dict[str, Any]] = {} + self.previous_object_uid: str | None = None + self.previous_arm: str | None = None + self.last_task_by_object_uid: dict[str, str] = {} + + def add( + self, + task_type: str, + object_entity: SceneEntity, + *, + target: SceneEntity | None = None, + params: Mapping[str, Any] | None = None, + depends_on: Sequence[str] | None = None, + ) -> str: + values = deepcopy(dict(params or {})) + contract = task_contract(task_type) + relation = str(values.get("relation", "none")) + validate_source_compatibility(task_type, (object_entity,)) + validate_target_compatibility(task_type, target, relation=relation) + + instance_id = f"task_{len(self.instances) + 1:02d}" + object_role = self._role( + object_entity, + required_affordances=contract.required_affordances, + initial_state={"orientation": "fallen"} if task_type == "E2" else {}, + ) + values = {contract.primary_role_field: object_role, **values} + if target is not None: + values["target_role"] = self._role( + target, + required_affordances=_target_affordances(task_type, relation), + ) + if depends_on is None: + dependencies = [self.instances[-1]["id"]] if self.instances else [] + else: + dependencies = list(depends_on) + previous_for_object = self.last_task_by_object_uid.get(object_entity.uid) + if previous_for_object is not None and previous_for_object not in dependencies: + dependencies.append(previous_for_object) + self.instances.append( + { + "id": instance_id, + "task_type": task_type, + "params": values, + "depends_on": dependencies, + "role": "primary", + } + ) + self.last_task_by_object_uid[object_entity.uid] = instance_id + self.previous_object_uid = object_entity.uid + if contract.resource_mode == "handover": + receive_arm = str(values.get("receive_arm", "")) + self.previous_arm = ( + receive_arm if receive_arm in {"left_arm", "right_arm"} else None + ) + elif str(values.get("required_arm", "")) in {"left_arm", "right_arm"}: + self.previous_arm = str(values["required_arm"]) + return instance_id + + def build(self) -> GroundedTaskSpec: + types = {item["task_type"] for item in self.instances} + if len(self.instances) == 1: + level = "L1" + elif len(types) == 1: + level = "L2" + else: + level = "L3" + success_terms = [ + { + "type": task_success_type(item["task_type"], item.get("params")), + "task_instance_id": item["id"], + } + for item in self.instances + ] + task = validate_task_spec( + { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": self.task_id, + "level": level, + "instruction": self.instruction, + "reasoning_type": "none", + "task_instances": self.instances, + "success": {"op": "all", "terms": success_terms}, + "oracle": { + "task_order": [item["id"] for item in self.instances], + "role_bindings": dict(sorted(self.role_bindings().items())), + }, + "metadata": {"planner": self.planner}, + } + ) + requirements = validate_scene_requirements( + { + "schema_version": SCENE_REQUIREMENTS_SCHEMA, + "task_id": self.task_id, + "objects": list(self.requirements.values()), + "cameras": [], + "spatial_constraints": [{"type": "preserve_source_scene"}], + "distractor_count": max( + 0, + len(self.inventory.interactive) - len(self.role_by_uid), + ), + "metadata": {"source": "existing_gym_project"}, + } + ) + return GroundedTaskSpec(task, requirements, self.role_bindings()) + + def role_bindings(self) -> dict[str, str]: + return {role: uid for uid, role in self.role_by_uid.items()} + + def _role( + self, + entity: SceneEntity, + task_type: str | None = None, + *, + required_affordances: Sequence[str] = (), + initial_state: Mapping[str, Any] | None = None, + ) -> str: + if task_type in TASK_CONTRACTS: + required_affordances = tuple( + set(required_affordances) + | set(task_contract(str(task_type)).required_affordances) + ) + if task_type == "E2": + initial_state = {"orientation": "fallen", **dict(initial_state or {})} + existing = self.role_by_uid.get(entity.uid) + if existing is not None: + requirement = self.requirements[existing] + requirement["affordances"] = sorted( + set(requirement["affordances"]) | set(required_affordances) + ) + requirement["initial_state"].update(dict(initial_state or {})) + return existing + role = f"object_{len(self.role_by_uid) + 1:02d}" + self.role_by_uid[entity.uid] = role + attributes = deepcopy(dict(entity.attributes)) + if entity.color is not None: + attributes.setdefault("color", entity.color) + self.requirements[role] = { + "role_id": role, + "category": entity.category or entity.role, + "count": 1, + "affordances": sorted(set(required_affordances)), + "initial_state": dict(initial_state or {}), + "attributes": attributes, + } + return role + + +def validate_source_compatibility( + task_type: str, + objects: Sequence[SceneEntity], +) -> None: + """Apply structural/explicit-affordance checks without a category taxonomy.""" + contract = task_contract(task_type) + if contract.source_structure == "articulation": + invalid = [entity.uid for entity in objects if entity.role != "articulation"] + else: + invalid = [ + entity.uid + for entity in objects + if entity.role not in {"object", "rigid_object"} + ] + if invalid: + structure_label = ( + "articulation" + if contract.source_structure == "articulation" + else "movable rigid-object" + ) + raise ValueError( + f"{task_type} requires {structure_label} structure; " + f"incompatible scene objects are {invalid}." + ) + required = set(contract.required_affordances) + for entity in objects: + if entity.affordances: + missing = required - set(entity.affordances) + if missing: + raise ValueError( + f"{task_type} is incompatible with scene object {entity.uid!r}; " + f"missing affordances {sorted(missing)}." + ) + + +def validate_target_compatibility( + task_type: str, + target: SceneEntity | None, + *, + relation: str, +) -> None: + """Reject only structural or explicitly declared target contradictions.""" + if relation == "on" and target is not None: + # Support is a relation between two concrete bodies at a candidate + # pose. A positive affordance list is not a closed-world inventory, so + # omission of ``support_surface`` cannot prove incompatibility here. + return + requires_container = task_type == "E3" or relation == "inside" + if requires_container and target is None: + raise ValueError( + f"{task_type} {relation} relation requires a target container." + ) + if not requires_container or target is None: + return + if target.role in SceneInventory._PASSIVE_ROLES: + raise ValueError( + f"{task_type} target {target.uid!r} is structurally incompatible " + "with containment." + ) + if target.affordances: + compatible = {"container", "fillable", "liquid_container", "receptacle"} + if set(target.affordances).isdisjoint(compatible): + raise ValueError( + f"{task_type} target {target.uid!r} has explicit affordances but " + f"none support containment; expected one of {sorted(compatible)}." + ) + + +def _target_affordances(task_type: str, relation: str) -> tuple[str, ...]: + if task_type == "E3" or relation == "inside": + return ("container",) + return () + + +def _scene_entity(raw: Mapping[str, Any]) -> SceneEntity: + uid = str(raw.get("runtime_uid", raw.get("uid", ""))).strip() + if not uid: + raise ValueError("Every scene object requires a runtime UID.") + role = str(raw.get("role", raw.get("source_role", "object"))).strip().lower() + raw_category = raw.get("category", raw.get("object_category", "")) + category = "" if raw_category is None else str(raw_category).strip() + raw_color = raw.get("color") + attributes = raw.get("attributes", {}) + if not isinstance(attributes, Mapping): + raise ValueError(f"Scene object {uid!r} attributes must be a mapping.") + if raw_color is None: + raw_color = attributes.get("color") + color = str(raw_color).strip() if raw_color not in (None, "") else None + position = raw.get("init_pos", raw.get("position", (0.0, 0.0, 0.0))) + if ( + not isinstance(position, Sequence) + or isinstance(position, (str, bytes)) + or len(position) != 3 + ): + raise ValueError(f"Scene object {uid!r} requires a three-value init_pos.") + raw_affordances = raw.get("affordances", raw.get("capabilities", ())) + affordances = ( + frozenset( + str(item).strip().lower() for item in raw_affordances if str(item).strip() + ) + if isinstance(raw_affordances, Sequence) + and not isinstance(raw_affordances, (str, bytes)) + else frozenset() + ) + initial_state = raw.get("initial_state", raw.get("state", {})) + if not isinstance(initial_state, Mapping): + raise ValueError(f"Scene object {uid!r} initial_state must be a mapping.") + return SceneEntity( + uid=uid, + role=role, + name=str(raw.get("name", "")).strip(), + description=str(raw.get("description", "")).strip(), + category=category, + color=color, + position=tuple(float(value) for value in position), + affordances=affordances, + initial_state=dict(initial_state), + attributes=dict(attributes), + source_uid=str(raw.get("source_uid", "")).strip(), + ) diff --git a/embodichain/gen_sim/action_engine/tasks/grounding.py b/embodichain/gen_sim/action_engine/tasks/grounding.py new file mode 100644 index 000000000..de412e987 --- /dev/null +++ b/embodichain/gen_sim/action_engine/tasks/grounding.py @@ -0,0 +1,513 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Task-conditioned scene-UID grounding for structured instruction intents.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from copy import deepcopy +from dataclasses import dataclass +import json +import math +from time import perf_counter +from typing import Any + +from .assembly import SceneInventory + +__all__ = ["GroundingCaller", "GroundingResult", "ground_scene_references"] + +GroundingCaller = Callable[..., Mapping[str, Any]] + +_BINDING_KEYS = frozenset({"reference_id", "status", "uids", "confidence"}) +_QUANTIFIERS = frozenset({"one", "all", "count"}) +_REDACTED_KEYS = frozenset( + { + "absolute_position", + "bbox", + "bboxes", + "bounding_box", + "camera_matrix", + "center", + "centroid", + "coordinates", + "depth", + "dimensions", + "extrinsics", + "grasp_pose", + "init_local_pose", + "init_pos", + "init_rot", + "intrinsics", + "joint_positions", + "joints", + "keypoint", + "keypoints", + "location", + "matrix", + "pose", + "position", + "position_xyz", + "qpos", + "quaternion", + "rotation", + "scale", + "target_pose", + "trajectory", + "transform", + "translation", + "waypoints", + "world_x", + "world_y", + "world_z", + "x", + "y", + "z", + } +) + +_GROUNDING_SCHEMA: dict[str, Any] = { + "title": "ActionEngineSceneGrounding", + "type": "object", + "additionalProperties": False, + "required": ["bindings"], + "properties": { + "bindings": { + "type": "array", + "items": { + "type": "object", + "additionalProperties": False, + "required": sorted(_BINDING_KEYS), + "properties": { + "reference_id": {"type": "string"}, + "status": { + "type": "string", + "enum": ["resolved", "ambiguous", "not_found"], + }, + "uids": { + "type": "array", + "items": {"type": "string"}, + "uniqueItems": True, + }, + "confidence": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0, + }, + }, + }, + } + }, +} + + +@dataclass(frozen=True) +class GroundingResult: + """Validated scene bindings and aggregate call statistics. + + Attributes: + bindings: Mapping from ``.`` to scene UIDs. + attempts: Number of grounding-model calls, including one repair call. + latency_seconds: Total elapsed wall-clock time across the grounding stage. + """ + + bindings: dict[str, tuple[str, ...]] + attempts: int + latency_seconds: float + + +def ground_scene_references( + instruction: str, + intent: Mapping[str, Any], + inventory: SceneInventory, + scene_objects: Sequence[Mapping[str, Any]], + model: str | None, + caller: GroundingCaller, +) -> GroundingResult: + """Resolve every ``scene_ref`` selector in one task-conditioned batch. + + The grounding model can only select stable UIDs from a redacted inventory. + Its output does not add affordances, physical state, coordinates, or poses. + One failed local validation is repaired with one additional model call. + + Args: + instruction: Original user instruction for task-level context. + intent: Validated structured instruction intent. + inventory: Structural scene inventory defining authoritative candidates. + scene_objects: Original semantic inventory used to retain open labels. + model: Model name forwarded unchanged to the injected caller. + caller: Structured model transport accepting ``prompt``, ``schema``, and + ``model`` keyword arguments. + + Returns: + Validated UID bindings together with call-count and latency statistics. + + Raises: + TypeError: If the intent or response has an invalid container type. + ValueError: If requests are malformed or grounding remains invalid after + one repair attempt. + """ + if not isinstance(instruction, str) or not instruction.strip(): + raise ValueError("Grounding instruction must be a non-empty string.") + if not callable(caller): + raise TypeError("Grounding caller must be callable.") + + requests = _collect_requests(intent) + prompt_inventory = _grounding_inventory(inventory, scene_objects) + prompt = _grounding_prompt(instruction.strip(), requests, prompt_inventory) + started = perf_counter() + first_error: Exception | None = None + + for attempt in range(2): + current_prompt = prompt + if first_error is not None: + current_prompt += ( + "\n\nREPAIR OVERRIDE: the previous grounding JSON failed local " + "validation. Return one corrected JSON object only. Preserve the " + "exact output fields bindings/reference_id/status/uids/confidence, " + "cover every requested reference exactly once, and select only " + "UIDs from the supplied candidate inventory. Validation error: " + f"{first_error}" + ) + try: + response = caller( + prompt=current_prompt, + schema=deepcopy(_GROUNDING_SCHEMA), + model=model, + ) + bindings = _validate_response( + response, + requests=requests, + inventory=inventory, + ) + return GroundingResult( + bindings=bindings, + attempts=attempt + 1, + latency_seconds=perf_counter() - started, + ) + except (TypeError, ValueError) as error: + if attempt: + raise ValueError( + "Scene grounding failed validation after one repair: " f"{error}" + ) from error + first_error = error + raise AssertionError("unreachable") + + +def _collect_requests(intent: Mapping[str, Any]) -> list[dict[str, Any]]: + if not isinstance(intent, Mapping): + raise TypeError("Instruction intent must be a mapping.") + steps = intent.get("steps") + if not isinstance(steps, Sequence) or isinstance(steps, (str, bytes)): + raise ValueError("Instruction intent steps must be a list.") + + requests: list[dict[str, Any]] = [] + request_ids: set[str] = set() + for step_index, step in enumerate(steps): + context = f"InstructionIntent.steps[{step_index}]" + if not isinstance(step, Mapping): + raise ValueError(f"{context} must be a mapping.") + step_id = step.get("id") + if not isinstance(step_id, str) or not step_id.strip(): + raise ValueError(f"{context}.id must be a non-empty string.") + task_type = step.get("task_type") + if not isinstance(task_type, str) or not task_type.strip(): + raise ValueError(f"{context}.task_type must be a non-empty string.") + relation = step.get("relation", "none") + if not isinstance(relation, str): + raise ValueError(f"{context}.relation must be a string.") + + for slot in ("object", "target"): + selector = step.get(slot) + if not isinstance(selector, Mapping): + raise ValueError(f"{context}.{slot} must be a mapping.") + if selector.get("kind") != "scene_ref": + continue + reference = selector.get("reference") + if not isinstance(reference, str) or not reference.strip(): + raise ValueError( + f"{context}.{slot}.reference must be a non-empty string." + ) + quantifier = selector.get("quantifier") + if quantifier not in _QUANTIFIERS: + raise ValueError( + f"{context}.{slot}.quantifier must be one of " + f"{sorted(_QUANTIFIERS)}." + ) + count = selector.get("count") + if not isinstance(count, int) or isinstance(count, bool) or count < 0: + raise ValueError(f"{context}.{slot}.count must be an integer >= 0.") + if quantifier == "count" and count < 1: + raise ValueError( + f"{context}.{slot} quantifier=count requires count>=1." + ) + if quantifier != "count" and count != 0: + raise ValueError( + f"{context}.{slot} quantifier={quantifier} requires count=0." + ) + + request_id = f"{step_id}.{slot}" + if request_id in request_ids: + raise ValueError(f"Duplicate grounding request ID {request_id!r}.") + request_ids.add(request_id) + requests.append( + { + "reference_id": request_id, + "step_id": step_id, + "slot": slot, + "task_type": task_type, + "relation": relation, + "reference": reference.strip(), + "quantifier": quantifier, + "count": count, + } + ) + return requests + + +def _grounding_inventory( + inventory: SceneInventory, + scene_objects: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + raw_by_uid: dict[str, Mapping[str, Any]] = {} + for item_index, raw in enumerate(scene_objects): + if not isinstance(raw, Mapping): + raise ValueError(f"Scene inventory item {item_index} must be a mapping.") + uid = str(raw.get("runtime_uid", raw.get("uid", ""))).strip() + if uid: + raw_by_uid[uid] = raw + + ranked = sorted( + inventory.entities, + key=lambda entity: (-inventory.left_score(entity), entity.uid), + ) + rank_by_uid = {entity.uid: rank for rank, entity in enumerate(ranked, start=1)} + payload = [] + for entity in sorted(inventory.entities, key=lambda item: item.uid): + raw = raw_by_uid.get(entity.uid, {}) + score = inventory.left_score(entity) + side = "left" if score > 0.0 else "right" if score < 0.0 else "center" + raw_category = raw.get( + "category", + raw.get("object_category", entity.category), + ) + attributes = _redact_semantic_mapping(entity.attributes) + if entity.color is not None: + attributes.setdefault("color", entity.color) + payload.append( + { + "uid": entity.uid, + "role": entity.role, + "name": str(raw.get("name", entity.name)).strip(), + "category": str(raw_category).strip() or entity.category, + "description": entity.description, + "affordances": sorted(entity.affordances), + "attributes": attributes, + "initial_state": _redact_semantic_mapping(entity.initial_state), + "side": side, + "rank": rank_by_uid[entity.uid], + } + ) + return payload + + +def _grounding_prompt( + instruction: str, + requests: Sequence[Mapping[str, Any]], + inventory: Sequence[Mapping[str, Any]], +) -> str: + return ( + "Ground the requested natural-language scene references to the supplied " + "scene inventory. Resolve all requests together using the original task, " + "step type, relation, quantifier, and reference text as context. Select " + "only exact inventory UIDs. The inventory's affordances and states are " + "source evidence only: never infer, add, authorize, or return an " + "affordance, capability, physical state, coordinate, pose, orientation, " + "path, or action. The side and rank fields are discrete robot-relative " + "labels; rank 1 is leftmost. Object requests may select only movable " + "inventory entities. Target requests may also select support surfaces. " + "Use status=ambiguous or status=not_found instead of guessing when the " + "evidence is insufficient. Return exactly one binding per reference_id " + "with only reference_id, status, uids, and confidence.\n\n" + f"Instruction:\n{instruction}\n\n" + "Grounding requests:\n" + f"{json.dumps(list(requests), ensure_ascii=False, sort_keys=True)}\n\n" + "Redacted scene inventory:\n" + f"{json.dumps(list(inventory), ensure_ascii=False, sort_keys=True)}" + ) + + +def _validate_response( + value: Mapping[str, Any], + *, + requests: Sequence[Mapping[str, Any]], + inventory: SceneInventory, +) -> dict[str, tuple[str, ...]]: + if not isinstance(value, Mapping): + raise TypeError("Scene grounding output must be a mapping.") + if set(value) != {"bindings"}: + raise ValueError( + "Scene grounding output must contain exactly the 'bindings' field." + ) + raw_bindings = value["bindings"] + if not isinstance(raw_bindings, Sequence) or isinstance(raw_bindings, (str, bytes)): + raise ValueError("Scene grounding bindings must be a list.") + + request_by_id = {str(request["reference_id"]): request for request in requests} + bindings: dict[str, tuple[str, ...]] = {} + for binding_index, raw in enumerate(raw_bindings): + context = f"SceneGrounding.bindings[{binding_index}]" + if not isinstance(raw, Mapping): + raise ValueError(f"{context} must be a mapping.") + if set(raw) != _BINDING_KEYS: + missing = sorted(_BINDING_KEYS - set(raw)) + extra = sorted(set(raw) - _BINDING_KEYS) + raise ValueError( + f"{context} fields must be exactly {sorted(_BINDING_KEYS)}; " + f"missing={missing}, unsupported={extra}." + ) + reference_id = raw["reference_id"] + if not isinstance(reference_id, str) or not reference_id: + raise ValueError(f"{context}.reference_id must be a non-empty string.") + if reference_id not in request_by_id: + raise ValueError(f"{context} references unknown request {reference_id!r}.") + if reference_id in bindings: + raise ValueError(f"Duplicate grounding binding for {reference_id!r}.") + + status = raw["status"] + if status not in {"resolved", "ambiguous", "not_found"}: + raise ValueError( + f"{context}.status must be resolved, ambiguous, or not_found." + ) + if status != "resolved": + raise ValueError( + f"Grounding request {reference_id!r} was not resolved: {status}." + ) + confidence = raw["confidence"] + if ( + not isinstance(confidence, (int, float)) + or isinstance(confidence, bool) + or not math.isfinite(float(confidence)) + or not 0.0 <= float(confidence) <= 1.0 + ): + raise ValueError(f"{context}.confidence must be a number in [0, 1].") + if float(confidence) < 0.5: + raise ValueError( + f"Grounding request {reference_id!r} confidence is below 0.5." + ) + + raw_uids = raw["uids"] + if not isinstance(raw_uids, Sequence) or isinstance(raw_uids, (str, bytes)): + raise ValueError(f"{context}.uids must be a list.") + uids = tuple(raw_uids) + if any(not isinstance(uid, str) or not uid for uid in uids): + raise ValueError(f"{context}.uids must contain non-empty strings.") + if len(set(uids)) != len(uids): + raise ValueError( + f"Grounding request {reference_id!r} contains duplicate UIDs." + ) + unknown = sorted(set(uids) - set(inventory.by_uid)) + if unknown: + raise ValueError( + f"Grounding request {reference_id!r} selected unknown UIDs {unknown}." + ) + + request = request_by_id[reference_id] + allowed = ( + {entity.uid for entity in inventory.interactive} + if request["slot"] == "object" + else {entity.uid for entity in (*inventory.interactive, *inventory.support)} + ) + disallowed = sorted(set(uids) - allowed) + if disallowed: + raise ValueError( + f"Grounding request {reference_id!r} selected UIDs outside its " + f"{request['slot']} candidate range: {disallowed}." + ) + _validate_cardinality(request, uids) + bindings[reference_id] = uids + + missing = sorted(set(request_by_id) - set(bindings)) + if missing: + raise ValueError(f"Scene grounding omitted requests {missing}.") + _reject_self_references(requests, bindings) + return bindings + + +def _validate_cardinality( + request: Mapping[str, Any], + uids: Sequence[str], +) -> None: + request_id = str(request["reference_id"]) + quantifier = str(request["quantifier"]) + if quantifier == "one" and len(uids) != 1: + raise ValueError( + f"Grounding request {request_id!r} quantifier=one requires exactly one UID." + ) + if quantifier == "count" and len(uids) != int(request["count"]): + raise ValueError( + f"Grounding request {request_id!r} requires exactly " + f"{request['count']} UIDs." + ) + if quantifier == "all" and not uids: + raise ValueError( + f"Grounding request {request_id!r} quantifier=all requires at " + "least one UID." + ) + + +def _reject_self_references( + requests: Sequence[Mapping[str, Any]], + bindings: Mapping[str, tuple[str, ...]], +) -> None: + slots_by_step: dict[str, dict[str, str]] = {} + for request in requests: + slots_by_step.setdefault(str(request["step_id"]), {})[str(request["slot"])] = ( + str(request["reference_id"]) + ) + for step_id, slots in slots_by_step.items(): + object_id = slots.get("object") + target_id = slots.get("target") + if object_id is None or target_id is None: + continue + overlap = sorted(set(bindings[object_id]) & set(bindings[target_id])) + if overlap: + raise ValueError( + f"Grounding step {step_id!r} uses the same UID as object and " + f"target: {overlap}." + ) + + +def _redact_semantic_mapping(value: Mapping[str, Any]) -> dict[str, Any]: + result: dict[str, Any] = {} + for key, child in value.items(): + name = str(key) + normalized = name.strip().lower().replace("-", "_") + if normalized in _REDACTED_KEYS: + continue + if isinstance(child, Mapping): + nested = _redact_semantic_mapping(child) + if nested: + result[name] = nested + elif isinstance(child, (str, int, float, bool)) and not isinstance( + child, complex + ): + result[name] = child + elif isinstance(child, Sequence) and not isinstance(child, (str, bytes)): + semantic_values = [item for item in child if isinstance(item, (str, bool))] + if semantic_values and len(semantic_values) == len(child): + result[name] = semantic_values + return result diff --git a/embodichain/gen_sim/action_engine/tasks/interpretation.py b/embodichain/gen_sim/action_engine/tasks/interpretation.py new file mode 100644 index 000000000..1a5da8467 --- /dev/null +++ b/embodichain/gen_sim/action_engine/tasks/interpretation.py @@ -0,0 +1,410 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Compatibility bridge from Task Engine drafts to Action Engine TaskSpec v2.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from typing import Any + +from embodichain.gen_sim.task_engine.interpretation import ( + INSTRUCTION_INTENT_SCHEMA, + InstructionCaller, + InstructionDraftResult, + InstructionIntent, + _default_instruction_caller, + _instruction_prompt, + _instruction_selector_rules, + interpret_instruction_draft, + validate_instruction_intent, +) + +from .assembly import ( + GroundedTaskBuilder, + GroundedTaskSpec, + SceneEntity, + SceneInventory, + validate_source_compatibility, + validate_target_compatibility, +) +from .grounding import GroundingCaller, ground_scene_references + +__all__ = [ + "GroundingCaller", + "INSTRUCTION_INTENT_SCHEMA", + "InstructionCaller", + "InstructionDraftResult", + "InstructionIntent", + "ground_instruction_draft", + "interpret_and_ground_task_spec", + "interpret_instruction_draft", + "validate_instruction_intent", +] + + +def interpret_and_ground_task_spec( + task_name: str, + task_description: str, + scene_objects: Sequence[Mapping[str, Any]], + *, + robot_profile: str, + model: str | None = None, + caller: InstructionCaller | None = None, + grounding_caller: GroundingCaller | None = None, +) -> GroundedTaskSpec: + """Interpret through Task Engine, then ground through Action Engine.""" + task_id = str(task_name).strip() + instruction = str(task_description).strip() + if not task_id or not instruction: + raise ValueError("task_name and task_description must be non-empty.") + inventory = SceneInventory(scene_objects, robot_profile=robot_profile) + draft = interpret_instruction_draft(instruction, model=model, caller=caller) + invoke = caller or _default_instruction_caller + selected_model = None if draft.model == "injected_caller" else draft.model + grounding = ground_scene_references( + instruction=instruction, + intent=draft.intent, + inventory=inventory, + scene_objects=scene_objects, + model=selected_model, + caller=grounding_caller or invoke, + ) + grounded = _ground_intent( + task_id, + instruction, + draft.intent, + inventory, + grounding.bindings, + ) + grounded.task_spec["metadata"].update( + { + "instruction_interpreter": "structured_llm_v2", + "instruction_model": draft.model, + "instruction_call_count": draft.attempts, + "instruction_latency_seconds": draft.latency_seconds, + "scene_grounding_model": selected_model or "injected_caller", + "scene_grounding_call_count": grounding.attempts, + "scene_grounding_latency_seconds": grounding.latency_seconds, + } + ) + if draft.normalizations: + grounded.task_spec["metadata"]["instruction_intent_normalizations"] = list( + draft.normalizations + ) + return grounded + + +def ground_instruction_draft( + task_id: str, + instruction: str, + intent: Mapping[str, Any], + scene_objects: Sequence[Mapping[str, Any]], + *, + robot_profile: str, + reference_bindings: Mapping[str, Sequence[str]], +) -> GroundedTaskSpec: + """Lower a Task Engine draft using verified scene bindings.""" + normalized_task_id = str(task_id).strip() + normalized_instruction = str(instruction).strip() + if not normalized_task_id or not normalized_instruction: + raise ValueError("task_id and instruction must be non-empty.") + inventory = SceneInventory(scene_objects, robot_profile=robot_profile) + return _ground_intent( + normalized_task_id, + normalized_instruction, + validate_instruction_intent(intent), + inventory, + reference_bindings, + ) + + +def _ground_intent( + task_id: str, + instruction: str, + intent: Mapping[str, Any], + inventory: SceneInventory, + scene_bindings: Mapping[str, Sequence[str]], +) -> GroundedTaskSpec: + builder = GroundedTaskBuilder( + task_id, + instruction, + inventory, + planner="structured_llm_v2", + ) + objects_by_step: dict[str, list[SceneEntity]] = {} + task_ids_by_step: dict[str, list[str]] = {} + for step in _topological_steps(intent["steps"]): + step_id = str(step["id"]) + objects = _resolve_reference( + step["object"], + inventory, + objects_by_step, + context=f"instruction step {step_id!r} object", + reference_id=f"{step_id}.object", + scene_bindings=scene_bindings, + ) + validate_source_compatibility(str(step["task_type"]), objects) + target_objects = _resolve_reference( + step["target"], + inventory, + objects_by_step, + context=f"instruction step {step_id!r} target", + reference_id=f"{step_id}.target", + scene_bindings=scene_bindings, + allow_none=True, + exclude={item.uid for item in objects}, + allow_support=True, + ) + if len(target_objects) > 1: + raise ValueError(f"Instruction step {step_id!r} target is ambiguous.") + validate_target_compatibility( + str(step["task_type"]), + target_objects[0] if target_objects else None, + relation=str(step["relation"]), + ) + dependencies_by_step = list(step["depends_on"]) + for selector in (step["object"], step["target"]): + if selector["kind"] == "step_result": + reference = str(selector["step_id"]) + if reference not in dependencies_by_step: + dependencies_by_step.append(reference) + dependencies = [ + emitted_id + for dependency in dependencies_by_step + for emitted_id in task_ids_by_step[str(dependency)] + ] + emitted = _emit_step( + builder, + step, + objects, + target_objects[0] if target_objects else None, + dependencies, + ) + objects_by_step[step_id] = objects + task_ids_by_step[step_id] = emitted + return builder.build() + + +def _emit_step( + builder: GroundedTaskBuilder, + step: Mapping[str, Any], + objects: Sequence[SceneEntity], + target: SceneEntity | None, + dependencies: Sequence[str], +) -> list[str]: + task_type = str(step["task_type"]) + if step["layout"] == "line": + roles = [builder._role(entity, "E1") for entity in objects] + parent = str(step["id"]) + return [ + builder.add( + "E1", + entity, + params={ + "target_role": "table", + "relation": "on", + "layout": "line", + "objects_roles": roles, + "axis": "world_y" if step["axis"] == "none" else step["axis"], + "order_by": "explicit", + "order_direction": "given", + "order_constraint": "free", + "orientation_goal": step["orientation_goal"], + "orientation_axis": "none", + "nominal_slot_index": slot, + "slot_constraint": "free_reassignable", + "parent_task_instance_id": parent, + }, + depends_on=dependencies, + ) + for slot, entity in enumerate(objects) + ] + + emitted = [] + for entity in objects: + params: dict[str, Any] = {} + required_arm = str(step["required_arm"]) + if required_arm in {"left_arm", "right_arm"}: + params["required_arm"] = required_arm + if task_type == "E1": + relation = str(step["relation"]) + if relation == "none": + if target is None or target not in builder.inventory.support: + raise ValueError( + "E1 omitted relation is only valid for a unique table " + "support target." + ) + relation = "on" + params.update( + { + "relation": relation, + "relation_frame": "robot", + "orientation_goal": step["orientation_goal"], + "orientation_axis": "none", + } + ) + elif task_type == "E2": + params.update( + { + "orientation_goal": "upright", + "support_role": "table", + "upright_local_axis": "auto", + } + ) + elif task_type == "E3": + params.update({"relation": "above", "relation_frame": "robot"}) + elif task_type == "E4": + terminal_behavior = str(step["terminal_behavior"]) + if terminal_behavior == "none": + terminal_behavior = "place" if target is not None else "hold" + params.update( + { + "transfer_arm": step["transfer_arm"], + "receive_arm": step["receive_arm"], + "orientation_goal": step["orientation_goal"], + "terminal_behavior": terminal_behavior, + "relation": step["relation"], + "relation_frame": "robot", + } + ) + elif task_type == "E5": + params.update( + { + "direction": step["direction"], + "terminal_behavior": step["terminal_behavior"], + "relation": step["relation"], + "relation_frame": "robot", + } + ) + elif task_type in {"E6", "E7"}: + params["target_state"] = step["target_state"] + elif task_type == "E8": + params["target_setting"] = int(step["target_setting"]) + elif task_type == "E9": + params["terminal_state"] = step["target_state"] + emitted.append( + builder.add( + task_type, + entity, + target=target, + params=params, + depends_on=dependencies, + ) + ) + return emitted + + +def _resolve_reference( + selector: Mapping[str, Any], + inventory: SceneInventory, + objects_by_step: Mapping[str, Sequence[SceneEntity]], + *, + context: str, + reference_id: str, + scene_bindings: Mapping[str, Sequence[str]], + allow_none: bool = False, + exclude: set[str] | None = None, + allow_support: bool = False, +) -> list[SceneEntity]: + kind = str(selector["kind"]) + if kind == "none": + if allow_none: + return [] + raise ValueError(f"{context} is required.") + if kind == "step_result": + step_id = str(selector["step_id"]) + if step_id not in objects_by_step: + raise ValueError(f"{context} references unavailable step {step_id!r}.") + objects = list(objects_by_step[step_id]) + if len(objects) != 1: + raise ValueError( + f"{context} references step {step_id!r}, which has {len(objects)} objects." + ) + if exclude and objects[0].uid in exclude: + raise ValueError( + f"{context} references the same object as its source; " + "self-referential placement is not allowed." + ) + return objects + + if reference_id not in scene_bindings: + raise ValueError(f"{context} has no verified scene-grounding binding.") + excluded = exclude or set() + source_uids = ( + {entity.uid for entity in inventory.entities} + if allow_support + else {entity.uid for entity in inventory.interactive} + ) + resolved_uids = tuple(str(uid) for uid in scene_bindings[reference_id]) + pool = [ + inventory.by_uid[uid] + for uid in resolved_uids + if uid in source_uids and uid not in excluded + ] + pool = sorted(pool, key=lambda item: item.uid) + if not pool: + raise ValueError(f"{context} did not bind an eligible scene object.") + quantifier = str(selector["quantifier"]) + count = int(selector["count"]) + if quantifier == "one" and len(pool) != 1: + raise ValueError( + f"{context} is ambiguous; matched scene UIDs {[item.uid for item in pool]}." + ) + if quantifier == "count" and (count < 1 or len(pool) != count): + raise ValueError( + f"{context} requested exactly {count} objects but matched {len(pool)}." + ) + if quantifier == "all" and count not in {0, len(pool)}: + raise ValueError( + f"{context} quantifier=all cannot carry count={count}; use count for an exact quantity." + ) + return pool + + +def _topological_steps( + steps: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + by_id = {str(step["id"]): dict(step) for step in steps} + effective_dependencies: dict[str, tuple[str, ...]] = {} + for step_id, step in by_id.items(): + deps = [str(dep) for dep in step["depends_on"]] + for selector in (step["object"], step["target"]): + if selector["kind"] == "step_result": + reference = str(selector["step_id"]) + if reference not in deps: + deps.append(reference) + effective_dependencies[step_id] = tuple(deps) + pending = set(by_id) + ordered: list[dict[str, Any]] = [] + original = [str(step["id"]) for step in steps] + while pending: + ready = [ + step_id + for step_id in original + if step_id in pending + and all(str(dep) not in pending for dep in effective_dependencies[step_id]) + ] + if not ready: + raise ValueError("Instruction intent dependencies contain a cycle.") + # Select one earliest-ready step at a time. Emitting the whole ready + # frontier lets a later independent step leapfrog an earlier step that + # becomes ready after its predecessor, changing the instruction's + # resource-order tie break without any causal reason. + step_id = ready[0] + ordered.append(by_id[step_id]) + pending.remove(step_id) + return ordered diff --git a/embodichain/gen_sim/action_engine/tasks/recipes.py b/embodichain/gen_sim/action_engine/tasks/recipes.py new file mode 100644 index 000000000..217d97992 --- /dev/null +++ b/embodichain/gen_sim/action_engine/tasks/recipes.py @@ -0,0 +1,1510 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Direct AtomicAction recipes for E1-E9 TaskSpec instances.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from copy import deepcopy +from typing import Any + +from embodichain.gen_sim.action_engine.capabilities import ( + AtomicCapabilityRegistry, + build_atomic_capability_registry, + capability_precondition, +) +from embodichain.gen_sim.action_engine.domain import ( + TERMINAL_BEHAVIORS, + TaskContract, + TRANSPORT_DIRECTIONS, + motion_policy, + task_contract, + task_success_type, + validate_task_spec, +) +from embodichain.gen_sim.action_engine.planning.linker import ( + link_seed_graph, + link_task_dependencies, +) +from embodichain.gen_sim.action_engine.protocol import SEED_GRAPH_SCHEMA + +__all__ = ["instantiate_seed_graph"] + + +def instantiate_seed_graph( + task_spec: Mapping[str, Any], + role_bindings: Mapping[str, str], + *, + planner_route: str = "offline", + registry: AtomicCapabilityRegistry | None = None, +) -> dict[str, Any]: + """Instantiate a coordinate-free SeedGraph after Scene Engine hand-off.""" + task = validate_task_spec(task_spec) + bindings = _validate_bindings(task, role_bindings) + capabilities = registry or build_atomic_capability_registry() + task, payload_links = _propagate_direct_payloads(task, bindings) + task = link_task_dependencies(task, bindings, registry=capabilities) + instances = _topological_instances(task["task_instances"]) + nodes: list[dict[str, Any]] = [] + groups = [] + terminal_by_group: dict[str, list[str]] = {} + held_after_group: dict[str, tuple[str, str] | None] = {} + for instance in instances: + group_id = str(instance["id"]) + task_type = str(instance["task_type"]) + params = _resolve_params(instance["params"], bindings) + object_uid = _primary_object(task_type, params) + incoming_held_arm = _incoming_held_arm( + task_type, + object_uid, + instance["depends_on"], + held_after_group, + ) + actor = _actor(task_type, params, incoming_held_arm=incoming_held_arm) + dependency_nodes = [ + node_id + for dependency in instance["depends_on"] + for node_id in terminal_by_group[str(dependency)] + ] + recipe_nodes, operator, goal, success = _recipe( + group_id, + task_type, + object_uid, + actor, + params, + dependency_nodes, + role=str(instance["role"]), + incoming_held_arm=incoming_held_arm, + ) + for node in recipe_nodes: + node["precondition"] = capability_precondition( + capabilities.get(str(node["atomic_action"])), + object_uid=str(node["object_uid"]), + actor=node["actor"], + target_binding=node["target_binding"], + ) + nodes.extend(recipe_nodes) + terminal_by_group[group_id] = _terminal_nodes(recipe_nodes) + held_after_group[group_id] = _terminal_hold_from_contracts( + object_uid, + recipe_nodes, + capabilities, + ) + groups.append( + { + "id": group_id, + "task_type": task_type, + "role": str(instance["role"]), + "operator": operator, + "object_uid": object_uid, + "actor": actor, + "goal": goal, + "depends_on": list(instance["depends_on"]), + "parent_task_instance_id": str( + params.get("parent_task_instance_id", group_id) + ), + "node_ids": [node["id"] for node in recipe_nodes], + "success": success, + } + ) + + graph_metadata = { + "task_spec_id": task["task_id"], + "role_bindings": dict(sorted(bindings.items())), + "allocation_groups": deepcopy( + task.get("metadata", {}).get("allocation_groups", []) + ), + "direct_payload_links": payload_links, + "oracle_exposed": False, + "planning_latency_seconds": 0.0, + "vlm_call_count": 0, + } + task_linker = task.get("metadata", {}).get("action_contract_task_linker") + if isinstance(task_linker, Mapping): + graph_metadata["action_contract_task_linker"] = deepcopy(dict(task_linker)) + + graph = { + "schema_version": SEED_GRAPH_SCHEMA, + "task_id": task["task_id"], + "instruction": task["instruction"], + "level": task["level"], + "reasoning_type": task["reasoning_type"], + "planner_route": planner_route, + "nodes": nodes, + "task_groups": groups, + "success": { + "op": "all", + "terms": [deepcopy(group["success"]) for group in groups], + }, + "capability_catalog_hash": capabilities.catalog_hash(), + "metadata": graph_metadata, + } + known_objects = set(bindings.values()) | {"table"} + graph = link_seed_graph( + graph, + registry=capabilities, + task_order=[str(instance["id"]) for instance in instances], + known_objects=known_objects, + ) + for node in graph["nodes"]: + capabilities.validate_binding(node) + return graph + + +def _topological_instances( + instances: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + """Emit task groups in dependency order even for externally authored specs.""" + by_id = {str(instance["id"]): dict(instance) for instance in instances} + original = [str(instance["id"]) for instance in instances] + pending = set(by_id) + ordered: list[dict[str, Any]] = [] + while pending: + ready = [ + instance_id + for instance_id in original + if instance_id in pending + and all( + str(dependency) not in pending + for dependency in by_id[instance_id]["depends_on"] + ) + ] + if not ready: + raise ValueError("TaskSpec task instances contain a dependency cycle.") + for instance_id in ready: + ordered.append(by_id[instance_id]) + pending.remove(instance_id) + return ordered + + +def _propagate_direct_payloads( + task: Mapping[str, Any], + bindings: Mapping[str, str], + *, + contract_resolver: Callable[[str], TaskContract] = task_contract, +) -> tuple[dict[str, Any], list[dict[str, str]]]: + """Propagate direct support through declared carrier-flow contracts. + + This is intentionally a one-hop physical relation rather than a general + scene-state planner: an object placed on or inside a carrier becomes that + carrier's direct payload until the object itself is manipulated again. + """ + result = deepcopy(dict(task)) + role_by_uid = {uid: role for role, uid in bindings.items()} + direct_by_carrier: dict[str, list[tuple[str, str, str]]] = {} + carrier_by_payload: dict[str, str] = {} + links: list[dict[str, str]] = [] + changed = False + + for instance in _topological_instances(result["task_instances"]): + task_type = str(instance["task_type"]) + contract = contract_resolver(task_type) + params = instance["params"] + primary_key = contract.primary_role_field + primary_role = params.get(primary_key) + if not isinstance(primary_role, str) or not primary_role: + continue + primary_uid = bindings.get(primary_role, primary_role) + direct_payloads = list(direct_by_carrier.get(primary_uid, ())) + if direct_payloads: + if not contract.accepts_direct_payloads: + raise ValueError( + f"TaskGroup {instance['id']!r} consumes carrier " + f"{primary_uid!r}, but its {task_type!r} contract does not " + "accept direct payloads." + ) + payload_roles = [payload_role for _, payload_role, _ in direct_payloads] + if params.get("payload_roles") != payload_roles: + params["payload_roles"] = payload_roles + changed = True + for payload_uid, _payload_role, producer_id in direct_payloads: + if producer_id not in instance["depends_on"]: + instance["depends_on"].append(producer_id) + changed = True + links.append( + { + "producer": producer_id, + "consumer": str(instance["id"]), + "carrier": primary_uid, + "payload": payload_uid, + "relation": "direct_support", + } + ) + + if contract.moves_primary_object: + old_carrier = carrier_by_payload.pop(primary_uid, None) + if old_carrier is not None: + direct_by_carrier[old_carrier] = [ + item + for item in direct_by_carrier.get(old_carrier, ()) + if item[0] != primary_uid + ] + + if str(params.get("relation")) not in contract.direct_payload_relations: + continue + target_role = params.get("target_role") + if not isinstance(target_role, str) or not target_role: + continue + target_uid = bindings.get(target_role, target_role) + if target_uid in {"table", "table_center"} or target_uid == primary_uid: + continue + payload_role = role_by_uid.get(primary_uid, primary_role) + direct_by_carrier.setdefault(target_uid, []).append( + (primary_uid, payload_role, str(instance["id"])) + ) + carrier_by_payload[primary_uid] = target_uid + + if changed: + metadata = dict(result.get("metadata", {})) + metadata.pop("action_contract_task_linker", None) + result["metadata"] = metadata + return validate_task_spec(result), links + + +def _payload_goal(params: Mapping[str, Any], object_uid: str) -> list[dict[str, str]]: + raw_payloads = params.get("payload_roles", []) + if not isinstance(raw_payloads, Sequence) or isinstance( + raw_payloads, (str, bytes, bytearray) + ): + raise ValueError("payload_roles must be a list.") + payloads = [str(value) for value in raw_payloads] + if any(not value for value in payloads): + raise ValueError("payload_roles must contain non-empty object IDs.") + if object_uid in payloads: + raise ValueError("A carrier cannot be its own payload.") + if len(payloads) != len(set(payloads)): + raise ValueError("Direct payload objects must be unique.") + return [{"object": value, "slot": "center"} for value in payloads] + + +def _orientation_extensions(params: Mapping[str, Any]) -> dict[str, Any]: + """Copy optional compiled-orientation fields from one task instance.""" + return { + key: deepcopy(params[key]) + for key in ("orientation_constraint", "orientation_directed") + if key in params + } + + +def _recipe( + group_id: str, + task_type: str, + object_uid: str, + actor: Mapping[str, Any], + params: Mapping[str, Any], + dependencies: list[str], + *, + role: str, + incoming_held_arm: str | None = None, +) -> tuple[list[dict[str, Any]], str, dict[str, Any], dict[str, Any]]: + if task_type == "E1": + target = str(params.get("target_role", "table")) + relation = str(params.get("relation", "on")) + layout = str(params.get("layout", "")) + if layout == "line": + goal = { + "layout": "line", + "objects": list(params["objects_roles"]), + "axis": str(params.get("axis", "world_y")), + "anchor": "table_center", + "order_by": str(params.get("order_by", "explicit")), + "order_direction": str(params.get("order_direction", "given")), + "order_constraint": str(params.get("order_constraint", "free")), + "participation": str(params.get("participation", "auto")), + "orientation_goal": str(params.get("orientation_goal", "none")), + "orientation_axis": str(params.get("orientation_axis", "none")), + **_orientation_extensions(params), + "nominal_slot_index": int(params["nominal_slot_index"]), + "slot_constraint": str( + params.get("slot_constraint", "free_reassignable") + ), + } + payloads = _payload_goal(params, object_uid) + if payloads: + goal["payloads"] = payloads + success = { + "type": "line_member_placed", + "nominal_slot_index": goal["nominal_slot_index"], + "slot_constraint": goal["slot_constraint"], + "order_constraint": goal["order_constraint"], + } + return ( + _single_arm_manipulation( + group_id, + task_type, + object_uid, + actor, + dependencies, + role=role, + already_held=incoming_held_arm is not None, + payloads=payloads, + orientation_goal=str(params.get("orientation_goal", "none")), + ), + "arrange_line", + goal, + success, + ) + goal = { + "reference_object": target, + "support_object": ( + target + if relation in {"on", "inside"} + else str(params.get("support_role", "table")) + ), + "reference_state": "live", + "relation": relation, + "relation_frame": str(params.get("relation_frame", "world")), + "orientation_goal": str(params.get("orientation_goal", "none")), + "orientation_axis": str(params.get("orientation_axis", "none")), + **_orientation_extensions(params), + "slot": str(params.get("slot", "auto")), + } + if "visual_constraint" in params: + goal["visual_constraint"] = deepcopy(params["visual_constraint"]) + payloads = _payload_goal(params, object_uid) + if payloads: + goal["payloads"] = payloads + success = { + "type": "semantic_goal", + "relation": relation, + "reference_object": target, + } + return ( + _single_arm_manipulation( + group_id, + task_type, + object_uid, + actor, + dependencies, + role=role, + already_held=incoming_held_arm is not None, + payloads=payloads, + orientation_goal=str(params.get("orientation_goal", "none")), + ), + "place_relative", + goal, + success, + ) + if task_type == "E2": + terminal_behavior = str(params.get("terminal_behavior", "place")) + if terminal_behavior == "hold" and role != "recovery": + raise ValueError( + "Ordinary E2 groups must release their supported object at the " + "TaskGroup boundary." + ) + goal = { + "relation": "none", + "reference_state": "live", + "orientation_goal": str(params.get("orientation_goal", "upright")), + "orientation_axis": str(params.get("orientation_axis", "none")), + "position_anchor": "initial_xy", + "support_object": str(params.get("support_role", "table")), + "upright_local_axis": str(params.get("upright_local_axis", "auto")), + **_orientation_extensions(params), + } + if terminal_behavior == "hold": + goal["terminal_behavior"] = "hold" + success = { + "type": task_success_type(task_type, params), + "object": object_uid, + "local_axis": goal["upright_local_axis"], + } + if incoming_held_arm is None and terminal_behavior == "place": + upright_policy = motion_policy(("orientation", "upright")) + pickup = _node( + group_id, + 1, + "PickUp", + task_type, + object_uid, + actor, + "arm", + {"kind": "object", "object": object_uid}, + dependencies, + role, + {}, + upright_policy, + ) + staging = _node( + group_id, + 2, + "MoveHeldObject", + task_type, + object_uid, + actor, + "arm", + { + "kind": "semantic_goal", + "semantic_step": group_id, + "phase": "staging", + }, + [pickup["id"]], + role, + {}, + upright_policy, + ) + descend = _node( + group_id, + 3, + "MoveHeldObject", + task_type, + object_uid, + actor, + "arm", + { + "kind": "semantic_goal", + "semantic_step": group_id, + "phase": "final", + }, + [staging["id"]], + role, + {}, + upright_policy, + ) + release = _node( + group_id, + 4, + "MoveJoints", + task_type, + object_uid, + actor, + "hand", + { + "kind": "joint_state", + "source": "gripper_open", + "single_release": True, + }, + [descend["id"]], + role, + success, + motion_policy(), + ) + lift_clear = _node( + group_id, + 5, + "MoveEndEffector", + task_type, + object_uid, + actor, + "arm", + { + "kind": "policy_pose", + "source": "release", + "operation": "lift_clear", + }, + [release["id"]], + "cleanup", + {}, + motion_policy(), + ) + reorient = _node( + group_id, + 6, + "MoveEndEffector", + task_type, + object_uid, + actor, + "arm", + { + "kind": "policy_pose", + "source": "release", + "operation": "reorient_tool_down", + }, + [lift_clear["id"]], + "cleanup", + {}, + motion_policy(("orientation", "upright")), + ) + post_reorient_lift = _node( + group_id, + 7, + "MoveEndEffector", + task_type, + object_uid, + actor, + "arm", + { + "kind": "policy_pose", + "source": "release", + "operation": "lift_clear", + "requires_arm_clear": True, + }, + [reorient["id"]], + "cleanup", + {}, + motion_policy(), + ) + retreat = _node( + group_id, + 8, + "MoveEndEffector", + task_type, + object_uid, + actor, + "arm", + { + "kind": "policy_pose", + "source": "release", + "operation": "retreat_after_lift", + }, + [post_reorient_lift["id"]], + "cleanup", + {}, + motion_policy(("orientation", "upright")), + ) + home = _node( + group_id, + 9, + "MoveJoints", + task_type, + object_uid, + actor, + "arm", + { + "kind": "joint_state", + "source": "initial", + "operation": "e2_home", + "required_home": True, + }, + [retreat["id"]], + "cleanup", + {}, + motion_policy(), + ) + return ( + [ + pickup, + staging, + descend, + release, + lift_clear, + reorient, + post_reorient_lift, + retreat, + home, + ], + "orient_object", + goal, + success, + ) + return ( + _single_arm_manipulation( + group_id, + task_type, + object_uid, + actor, + dependencies, + role=role, + already_held=incoming_held_arm is not None, + leave_held=terminal_behavior == "hold", + orientation_goal=str(params.get("orientation_goal", "upright")), + ), + "orient_object", + goal, + success, + ) + if task_type == "E3": + unsupported = sorted({"pour_mode", "pouring_arm", "holding_arm"} & set(params)) + if unsupported: + raise ValueError( + "Dual-arm E3 is not supported; remove fields " + f"{unsupported} and use required_arm with a fixed target container." + ) + target = str(params["target_role"]) + goal = { + "reference_object": target, + "relation": "above", + "amount": "task_defined", + } + success = { + "type": "poured", + "verification": "action_completion", + "object": object_uid, + "reference_object": target, + } + specs: list[tuple[str, Mapping[str, Any], str]] = [] + if incoming_held_arm is None: + specs.append( + ( + "PickUp", + {"kind": "object", "object": object_uid}, + role, + ) + ) + specs.extend( + ( + ( + "MoveHeldObject", + { + "kind": "semantic_goal", + "semantic_step": group_id, + "phase": "final", + }, + role, + ), + ( + "Pour", + { + "kind": "pour_goal", + "object": object_uid, + "reference_object": target, + }, + role, + ), + ( + "MoveHeldObject", + { + "kind": "semantic_goal", + "semantic_step": group_id, + "phase": "return", + }, + role, + ), + ( + "Place", + {"kind": "current_held_pose"}, + role, + ), + ( + "MoveEndEffector", + { + "kind": "policy_pose", + "source": "release", + "operation": "retreat", + }, + "cleanup", + ), + ( + "MoveJoints", + { + "kind": "joint_state", + "source": "initial", + "operation": "e3_home", + "required_home": True, + }, + "cleanup", + ), + ) + ) + nodes = [] + previous = list(dependencies) + for index, (action, binding, node_role) in enumerate(specs, start=1): + node = _node( + group_id, + index, + action, + task_type, + object_uid, + actor, + "arm", + binding, + previous, + node_role, + success if index == len(specs) else {}, + motion_policy(), + ) + nodes.append(node) + previous = [node["id"]] + return ( + nodes, + "pour", + goal, + success, + ) + if task_type == "E4": + transfer = str(params.get("transfer_arm", "left_arm")) + receive = str(params.get("receive_arm", "right_arm")) + terminal_behavior = str(params.get("terminal_behavior", "hold")) + if terminal_behavior not in TERMINAL_BEHAVIORS - {"none"}: + raise ValueError("E4 terminal_behavior must be 'hold' or 'place'.") + target = params.get("target_role") + relation = str(params.get("relation", "none")) + if terminal_behavior == "place": + if not isinstance(target, str) or not target or relation == "none": + raise ValueError( + "E4 terminal_behavior='place' requires target_role and relation." + ) + elif target is not None or relation != "none": + raise ValueError( + "E4 terminal_behavior='hold' cannot carry target_role or relation." + ) + if incoming_held_arm == "coordinated": + raise ValueError( + "E4 cannot consume a coordinated hold; an explicit single-arm " + "handover state is required." + ) + if incoming_held_arm is not None and transfer != incoming_held_arm: + raise ValueError( + f"E4 transfer_arm {transfer!r} conflicts with the predecessor " + f"holder {incoming_held_arm!r}." + ) + transfer_actor = {"mode": "required", "arm": transfer} + receive_actor = {"mode": "required", "arm": receive} + nodes: list[dict[str, Any]] = [] + previous = list(dependencies) + next_index = 1 + if incoming_held_arm is None: + pickup = _node( + group_id, + next_index, + "PickUp", + task_type, + object_uid, + transfer_actor, + "arm", + {"kind": "object", "object": object_uid}, + previous, + role, + {"type": "object_held", "object": object_uid, "arm": transfer}, + motion_policy(("handover_role", "transfer")), + ) + nodes.append(pickup) + previous = [pickup["id"]] + next_index += 1 + staging = _node( + group_id, + next_index, + "MoveHeldObject", + task_type, + object_uid, + transfer_actor, + "arm", + { + "kind": "handover_staging", + "object": object_uid, + "transfer_arm": transfer, + "receive_arm": receive, + }, + previous, + role, + {"type": "object_held", "object": object_uid, "arm": transfer}, + motion_policy(), + ) + nodes.append(staging) + previous = [staging["id"]] + next_index += 1 + handover = _node( + group_id, + next_index, + "HandOver", + task_type, + object_uid, + {"mode": "coordinated", "arms": ["left_arm", "right_arm"]}, + "coordinated", + { + "kind": "handover_goal", + "object": object_uid, + "transfer_arm": transfer, + "receive_arm": receive, + }, + previous, + role, + {"type": "handover_complete", "object": object_uid, "arm": receive}, + motion_policy(), + ) + nodes.append(handover) + previous = [handover["id"]] + next_index += 1 + # Grounding configures HandOver as exchange-to-exchange, so its receiver + # stays at the grasp while the transfer arm performs the built-in lift. + # This ordered retreat/home suffix then verifies and completes clearance + # before any receiver-side continuation may carry the object away. + retreat = _node( + group_id, + next_index, + "MoveEndEffector", + task_type, + object_uid, + transfer_actor, + "arm", + { + "kind": "policy_pose", + "source": "handover", + "operation": "retreat", + }, + previous, + "cleanup", + {}, + motion_policy(), + ) + nodes.append(retreat) + previous = [retreat["id"]] + next_index += 1 + home = _node( + group_id, + next_index, + "MoveJoints", + task_type, + object_uid, + transfer_actor, + "arm", + { + "kind": "joint_state", + "source": "initial", + "operation": "handover_home", + }, + previous, + "cleanup", + {}, + motion_policy(), + ) + nodes.append(home) + previous = [home["id"]] + next_index += 1 + + orientation_modifiers: tuple[tuple[str, str], ...] = ( + (("orientation", "upright"),) + if params.get("orientation_goal") == "upright" + else () + ) + receiver_policy = motion_policy(*orientation_modifiers) + if terminal_behavior == "hold": + receiver_exit = _node( + group_id, + next_index, + "MoveHeldObject", + task_type, + object_uid, + receive_actor, + "arm", + { + "kind": "semantic_goal", + "semantic_step": group_id, + "phase": "handover_exit", + "receive_arm": receive, + "terminal_hold": True, + }, + previous, + role, + {"type": "handover_complete", "object": object_uid, "arm": receive}, + receiver_policy, + ) + nodes.append(receiver_exit) + goal = { + "relation": "handover", + "orientation_goal": str(params.get("orientation_goal", "none")), + "orientation_axis": "none", + **_orientation_extensions(params), + "transfer_arm": transfer, + "receive_arm": receive, + "terminal_behavior": "hold", + } + return ( + nodes, + "handover", + goal, + {"type": "handover_complete", "object": object_uid, "arm": receive}, + ) + + assert isinstance(target, str) + for phase in ("staging", "final"): + receiver_move = _node( + group_id, + next_index, + "MoveHeldObject", + task_type, + object_uid, + receive_actor, + "arm", + { + "kind": "semantic_goal", + "semantic_step": group_id, + "phase": phase, + }, + previous, + role, + {}, + receiver_policy, + ) + nodes.append(receiver_move) + previous = [receiver_move["id"]] + next_index += 1 + release = _node( + group_id, + next_index, + "Place", + task_type, + object_uid, + receive_actor, + "arm", + {"kind": "current_held_pose"}, + previous, + role, + {}, + receiver_policy, + ) + nodes.append(release) + previous = [release["id"]] + next_index += 1 + receiver_retreat = _node( + group_id, + next_index, + "MoveEndEffector", + task_type, + object_uid, + receive_actor, + "arm", + {"kind": "policy_pose", "source": "release", "operation": "retreat"}, + previous, + "cleanup", + {}, + receiver_policy, + ) + nodes.append(receiver_retreat) + previous = [receiver_retreat["id"]] + next_index += 1 + receiver_home = _node( + group_id, + next_index, + "MoveJoints", + task_type, + object_uid, + receive_actor, + "arm", + {"kind": "joint_state", "source": "initial"}, + previous, + "cleanup", + {}, + motion_policy(), + ) + nodes.append(receiver_home) + success = { + "type": task_success_type(task_type, params), + "relation": relation, + "reference_object": target, + } + return ( + nodes, + "handover", + { + "reference_object": target, + "support_object": (target if relation in {"on", "inside"} else "table"), + "reference_state": "live", + "relation": relation, + "relation_frame": str(params.get("relation_frame", "robot")), + "orientation_goal": str(params.get("orientation_goal", "none")), + "orientation_axis": "none", + **_orientation_extensions(params), + "transfer_arm": transfer, + "receive_arm": receive, + "terminal_behavior": "place", + }, + success, + ) + if task_type == "E5": + terminal_behavior = str(params.get("terminal_behavior", "hold")) + if terminal_behavior not in TERMINAL_BEHAVIORS - {"none"}: + raise ValueError("E5 terminal_behavior must be 'hold' or 'place'.") + direction = str(params.get("direction", "up")) + if direction not in TRANSPORT_DIRECTIONS: + raise ValueError(f"E5 direction {direction!r} is unsupported.") + goal = { + "direction": direction, + "terminal_behavior": terminal_behavior, + "orientation_goal": str(params.get("orientation_goal", "none")), + "orientation_axis": "none", + **_orientation_extensions(params), + "relation_frame": str(params.get("relation_frame", "robot")), + } + payloads = _payload_goal(params, object_uid) + if payloads: + goal["payloads"] = payloads + target = params.get("target_role") + relation = str(params.get("relation", "none")) + if isinstance(target, str) and target: + if relation == "none": + raise ValueError("E5 target_role requires a symbolic relation.") + goal.update( + { + "reference_object": target, + "reference_state": "live", + "relation": relation, + "direction": "none", + } + ) + elif direction == "none" and terminal_behavior != "place": + raise ValueError("E5 requires a direction or target_role relation.") + pick = _node( + group_id, + 1, + "CoordinatedPickment", + task_type, + object_uid, + actor, + "coordinated", + { + "kind": "coordinated_goal", + "object": object_uid, + **({"payloads": deepcopy(payloads)} if payloads else {}), + }, + dependencies, + role, + {"type": "held_by_both_grippers", "object": object_uid}, + motion_policy(), + ) + nodes = [pick] + if terminal_behavior == "place": + release_sync_group = f"{group_id}__dual_release" + releases = [] + for index, arm, release_role in ( + (2, "left_arm", "participant"), + (3, "right_arm", "commit"), + ): + release = _node( + group_id, + index, + "MoveJoints", + task_type, + object_uid, + {"mode": "required", "arm": arm}, + "hand", + { + "kind": "joint_state", + "source": "gripper_open", + "coordinated_release_role": release_role, + }, + [pick["id"]], + role, + {}, + motion_policy(), + ) + release["sync_group"] = release_sync_group + nodes.append(release) + releases.append(release) + release_ids = [release["id"] for release in releases] + lifts = [] + for index, arm in ((4, "left_arm"), (5, "right_arm")): + lift = _node( + group_id, + index, + "MoveEndEffector", + task_type, + object_uid, + {"mode": "required", "arm": arm}, + "arm", + { + "kind": "policy_pose", + "source": "release", + "operation": "lift_clear", + "verify_lift_clear": True, + }, + release_ids, + "cleanup", + {}, + motion_policy(), + ) + nodes.append(lift) + lifts.append(lift) + lift_ids = [lift["id"] for lift in lifts] + for index, arm in ((6, "left_arm"), (7, "right_arm")): + nodes.append( + _node( + group_id, + index, + "MoveJoints", + task_type, + object_uid, + {"mode": "required", "arm": arm}, + "arm", + { + "kind": "joint_state", + "source": "initial", + "operation": "e5_home", + "required_home": True, + }, + lift_ids, + "cleanup", + {}, + motion_policy(), + ) + ) + success_type = task_success_type(task_type, params) + success = ( + {"type": success_type, "object": object_uid} + if success_type == "held_by_both_grippers" + else { + "type": success_type, + "relation": relation, + **( + {"reference_object": target} + if isinstance(target, str) and target + else {} + ), + } + ) + return ( + nodes, + "coordinated_transport", + goal, + success, + ) + planning = { + "E6": ("PullArticulatedPart", "pull_articulated_part"), + "E7": ("PushArticulatedPart", "push_articulated_part"), + "E8": ("TurnKnob", "turn_knob"), + } + if task_type in planning: + action_name, operator = planning[task_type] + success = {"type": "articulation_joint_near", "object": object_uid} + if task_type == "E8": + success["target_setting"] = int(params["target_setting"]) + else: + success["target_state"] = params["target_state"] + return ( + [ + _node( + group_id, + 1, + action_name, + task_type, + object_uid, + actor, + "arm", + {"kind": "articulation_goal", "object": object_uid}, + dependencies, + role, + success, + motion_policy(), + ) + ], + operator, + { + key: deepcopy(value) + for key, value in params.items() + if not key.endswith("_role") + }, + success, + ) + if task_type == "E9": + success = { + "type": "pressed", + "object": object_uid, + "terminal_state": str(params.get("terminal_state", "activated")), + } + return ( + [ + _node( + group_id, + 1, + "Press", + task_type, + object_uid, + actor, + "arm", + {"kind": "object", "object": object_uid}, + dependencies, + role, + success, + motion_policy(), + ) + ], + "press", + {"terminal_state": success["terminal_state"]}, + success, + ) + raise ValueError(f"Unsupported task type {task_type!r}.") + + +def _single_arm_manipulation( + group_id: str, + task_type: str, + object_uid: str, + actor: Mapping[str, Any], + dependencies: list[str], + *, + role: str, + already_held: bool = False, + leave_held: bool = False, + payloads: Sequence[Mapping[str, Any]] = (), + orientation_goal: str = "none", +) -> list[dict[str, Any]]: + orientation_modifiers: tuple[tuple[str, str], ...] = ( + (("orientation", "upright"),) if orientation_goal == "upright" else () + ) + payload_binding = deepcopy(list(payloads)) + specs = ( + ( + "PickUp", + { + "kind": "object", + "object": object_uid, + **({"payloads": payload_binding} if payload_binding else {}), + }, + motion_policy(*orientation_modifiers), + ), + ( + "MoveHeldObject", + { + "kind": "semantic_goal", + "semantic_step": group_id, + "phase": "staging", + **({"payloads": payload_binding} if payload_binding else {}), + }, + motion_policy(*orientation_modifiers), + ), + ( + "MoveHeldObject", + { + "kind": "semantic_goal", + "semantic_step": group_id, + "phase": "final", + **({"payloads": payload_binding} if payload_binding else {}), + }, + motion_policy(*orientation_modifiers), + ), + ( + "Place", + { + "kind": "current_held_pose", + **({"payloads": payload_binding} if payload_binding else {}), + }, + motion_policy(*orientation_modifiers), + ), + ( + "MoveEndEffector", + {"kind": "policy_pose", "source": "release", "operation": "retreat"}, + motion_policy(*orientation_modifiers), + ), + ( + "MoveJoints", + {"kind": "joint_state", "source": "initial"}, + motion_policy(), + ), + ) + if already_held: + specs = specs[1:] + if leave_held: + # A held continuation must not retreat or home the arm after the final + # semantic move: those cleanup phases would move away from the + # handover staging state while still owning the object. + place_index = next( + (index for index, spec in enumerate(specs) if spec[0] == "Place"), + len(specs), + ) + specs = specs[:place_index] + nodes = [] + previous = list(dependencies) + for index, (action, binding, policy) in enumerate(specs, start=1): + node_role = "cleanup" if action in {"MoveEndEffector", "MoveJoints"} else role + node = _node( + group_id, + index, + action, + task_type, + object_uid, + actor, + "arm", + binding, + previous, + node_role, + {}, + policy, + ) + nodes.append(node) + previous = [node["id"]] + return nodes + + +def _node( + group_id: str, + index: int, + action: str, + task_type: str, + object_uid: str, + actor: Mapping[str, Any], + control: str, + binding: Mapping[str, Any], + dependencies: list[str], + role: str, + postcondition: Mapping[str, Any], + motion_policy: Mapping[str, Any], +) -> dict[str, Any]: + return { + "id": f"{group_id}__a{index:02d}", + "atomic_action": action, + "object_uid": object_uid, + "actor": deepcopy(dict(actor)), + "control": control, + "target_binding": deepcopy(dict(binding)), + "depends_on": list(dependencies), + "task_instance_id": group_id, + "task_type": task_type, + "role": role, + "precondition": {}, + "postcondition": deepcopy(dict(postcondition)), + "motion_policy": deepcopy(dict(motion_policy)), + } + + +def _terminal_nodes(nodes: list[Mapping[str, Any]]) -> list[str]: + depended = {dependency for node in nodes for dependency in node["depends_on"]} + return [str(node["id"]) for node in nodes if node["id"] not in depended] + + +def _primary_object(task_type: str, params: Mapping[str, Any]) -> str: + key = task_contract(task_type).primary_role_field + value = params.get(key) + if not isinstance(value, str) or not value: + raise ValueError(f"{task_type} requires resolved parameter {key!r}.") + return value + + +def _actor( + task_type: str, + params: Mapping[str, Any], + *, + incoming_held_arm: str | None = None, +) -> dict[str, Any]: + contract = task_contract(task_type) + required_arm = params.get("required_arm") + if ( + incoming_held_arm is not None + and required_arm in {"left_arm", "right_arm"} + and str(required_arm) != incoming_held_arm + ): + raise ValueError( + f"Continuation requires {incoming_held_arm!r}, but the task " + f"requested {required_arm!r}." + ) + if incoming_held_arm is not None: + return {"mode": "required", "arm": incoming_held_arm} + if required_arm in {"left_arm", "right_arm"}: + return {"mode": "required", "arm": str(required_arm)} + if contract.resource_mode == "coordinated": + return {"mode": "coordinated", "arms": ["left_arm", "right_arm"]} + if contract.resource_mode == "handover": + return {"mode": "required", "arm": str(params.get("transfer_arm", "left_arm"))} + return {"mode": "auto"} + + +def _incoming_held_arm( + task_type: str, + object_uid: str, + dependencies: list[str], + held_after_group: Mapping[str, tuple[str, str] | None], +) -> str | None: + """Resolve a predecessor-provided hold for a continuation recipe.""" + if not task_contract(task_type).accepts_incoming_hold: + return None + candidates = { + held[1] + for dependency in dependencies + if (held := held_after_group.get(str(dependency))) is not None + and held[0] == object_uid + } + if len(candidates) > 1: + raise ValueError( + f"Task instance has conflicting predecessor holders for {object_uid!r}." + ) + holder = next(iter(candidates), None) + if holder is not None and holder not in {"left_arm", "right_arm"}: + raise ValueError( + f"Task instance cannot consume holder kind {holder!r} for " + f"{object_uid!r}; its contract accepts only single-arm ownership." + ) + return holder + + +def _terminal_hold_from_contracts( + object_uid: str, + nodes: Sequence[Mapping[str, Any]], + capabilities: AtomicCapabilityRegistry, +) -> tuple[str, str] | None: + """Fold action effects into the terminal holder of one recipe.""" + holders: set[str] = set() + coordinated = False + for node in nodes: + contract = capabilities.get(str(node["atomic_action"])).resolve_contract(node) + for effect in contract.effects: + atom = effect.atom + if atom.object_uid != object_uid: + continue + if atom.predicate == "object_held" and atom.arm is not None: + if effect.op == "add": + holders.add(atom.arm) + else: + holders.discard(atom.arm) + elif atom.predicate == "object_coordinated_held": + coordinated = effect.op == "add" + elif atom.predicate == "object_free" and effect.op == "add": + holders.clear() + coordinated = False + if coordinated and holders: + raise ValueError( + f"Recipe for {object_uid!r} ends with conflicting single and " + "coordinated ownership effects." + ) + if coordinated: + return object_uid, "coordinated" + if len(holders) > 1: + raise ValueError( + f"Recipe for {object_uid!r} ends with multiple single-arm holders." + ) + return (object_uid, next(iter(holders))) if holders else None + + +def _validate_bindings( + task: Mapping[str, Any], + role_bindings: Mapping[str, str], +) -> dict[str, str]: + bindings = dict(role_bindings) + for role, uid in bindings.items(): + if not isinstance(role, str) or not role or not isinstance(uid, str) or not uid: + raise ValueError("role_bindings must map non-empty role IDs to scene UIDs.") + required = set() + for instance in task["task_instances"]: + references = _role_references(instance["params"]) + if instance["task_type"] == "E3": + references -= _role_references( + instance["params"].get("content_roles", []), + "content_roles", + ) + required.update(references) + required.discard("table") + missing = sorted(required - set(bindings)) + if missing: + raise ValueError(f"Scene hand-off is missing role bindings: {missing}.") + if len(bindings.values()) != len(set(bindings.values())): + raise ValueError("Scene role bindings must resolve to unique object UIDs.") + return bindings + + +def _role_references(value: Any, key: str = "") -> set[str]: + if isinstance(value, Mapping): + return { + role + for child_key, child in value.items() + for role in _role_references(child, str(child_key)) + } + if isinstance(value, list): + return {role for child in value for role in _role_references(child, key)} + if isinstance(value, str) and (key.endswith("_role") or key.endswith("_roles")): + return {value} + return set() + + +def _resolve_params(value: Any, bindings: Mapping[str, str], key: str = "") -> Any: + if isinstance(value, Mapping): + return { + child_key: _resolve_params(child, bindings, str(child_key)) + for child_key, child in value.items() + } + if isinstance(value, list): + return [_resolve_params(child, bindings, key) for child in value] + if isinstance(value, str) and (key.endswith("_role") or key.endswith("_roles")): + return bindings.get(value, value) + return deepcopy(value) diff --git a/embodichain/gen_sim/action_engine/tasks/scene.py b/embodichain/gen_sim/action_engine/tasks/scene.py new file mode 100644 index 000000000..242b15dd4 --- /dev/null +++ b/embodichain/gen_sim/action_engine/tasks/scene.py @@ -0,0 +1,177 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Validate a Scene Engine result against task-first requirements.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from typing import Any + +from embodichain.gen_sim.action_engine.domain import validate_scene_requirements + +__all__ = ["SceneHandoff", "validate_scene_handoff"] + + +@dataclass(frozen=True) +class SceneHandoff: + """Validated role-to-UID resolution returned by an external Scene Engine.""" + + task_id: str + role_bindings: dict[str, Any] + object_uids: tuple[str, ...] + camera_uids: tuple[str, ...] + + +def validate_scene_handoff( + requirements: Mapping[str, Any], + scene: Mapping[str, Any], + role_bindings: Mapping[str, str], +) -> SceneHandoff: + """Reject scenes that do not satisfy roles, affordances, state, or cameras.""" + required = validate_scene_requirements(requirements) + objects = scene.get("objects") + if not isinstance(objects, Sequence) or isinstance(objects, (str, bytes)): + raise ValueError("Scene hand-off requires an objects list.") + object_by_uid: dict[str, Mapping[str, Any]] = {} + for index, item in enumerate(objects): + if not isinstance(item, Mapping): + raise ValueError(f"Scene objects[{index}] must be a mapping.") + uid = item.get("uid") + if not isinstance(uid, str) or not uid: + raise ValueError(f"Scene objects[{index}] requires a UID.") + if uid in object_by_uid: + raise ValueError(f"Scene contains duplicate object UID {uid!r}.") + object_by_uid[uid] = item + + bindings = dict(role_bindings) + required_roles = {item["role_id"] for item in required["objects"]} + if set(bindings) != required_roles: + missing = sorted(required_roles - set(bindings)) + extra = sorted(set(bindings) - required_roles) + raise ValueError( + f"Scene role bindings mismatch; missing={missing}, extra={extra}." + ) + normalized_bindings: dict[str, str | tuple[str, ...]] = {} + assigned_uids: list[str] = [] + for requirement in required["objects"]: + role = requirement["role_id"] + count = int(requirement["count"]) + binding = bindings[role] + if isinstance(binding, str): + uids = [binding] + elif isinstance(binding, Sequence) and not isinstance(binding, (str, bytes)): + uids = [str(uid) for uid in binding] + else: + raise ValueError(f"Scene role {role!r} has an invalid UID binding.") + if len(uids) != count or any(not uid for uid in uids): + raise ValueError( + f"Scene role {role!r} requires exactly {count} UID binding(s)." + ) + normalized_bindings[role] = uids[0] if count == 1 else tuple(uids) + assigned_uids.extend(uids) + for uid in uids: + _validate_bound_object(object_by_uid, uid, role, requirement) + if len(assigned_uids) != len(set(assigned_uids)): + raise ValueError("Each scene requirement role must resolve to unique UIDs.") + + cameras = scene.get("cameras", []) + if not isinstance(cameras, Sequence) or isinstance(cameras, (str, bytes)): + raise ValueError("Scene cameras must be a list.") + camera_uids = [] + normalized_cameras = [] + for camera in cameras: + if not isinstance(camera, Mapping) or not isinstance(camera.get("uid"), str): + raise ValueError("Every scene camera requires a UID.") + camera_uids.append(str(camera["uid"])) + normalized_cameras.append(camera) + for camera_requirement in required["cameras"]: + modalities = set(camera_requirement.get("modalities", ())) + coverage = camera_requirement.get("coverage") + if not any( + modalities <= set(camera.get("modalities", ())) + and (coverage is None or camera.get("coverage") == coverage) + for camera in normalized_cameras + ): + raise ValueError( + "Scene cameras do not satisfy requirement " + f"{dict(camera_requirement)!r}." + ) + reported_constraints = scene.get("satisfied_spatial_constraints", []) + if not isinstance(reported_constraints, Sequence) or isinstance( + reported_constraints, (str, bytes) + ): + raise ValueError("Scene satisfied_spatial_constraints must be a list.") + reported = {_canonical(item) for item in reported_constraints} + missing_constraints = [ + constraint + for constraint in required["spatial_constraints"] + if _canonical(constraint) not in reported + ] + if missing_constraints: + raise ValueError( + "Scene does not satisfy spatial constraints: " f"{missing_constraints}." + ) + return SceneHandoff( + task_id=required["task_id"], + role_bindings=normalized_bindings, + object_uids=tuple(sorted(object_by_uid)), + camera_uids=tuple(sorted(camera_uids)), + ) + + +def _validate_bound_object( + object_by_uid: Mapping[str, Mapping[str, Any]], + uid: str, + role: str, + requirement: Mapping[str, Any], +) -> None: + if uid not in object_by_uid: + raise ValueError(f"Scene role {role!r} references unknown UID {uid!r}.") + actual = object_by_uid[uid] + if actual.get("category") != requirement["category"]: + raise ValueError( + f"Scene object {uid!r} category does not satisfy role {role!r}." + ) + missing_affordances = set(requirement["affordances"]) - set( + actual.get("affordances", ()) + ) + if missing_affordances: + raise ValueError( + f"Scene object {uid!r} lacks affordances {sorted(missing_affordances)}." + ) + for field in ("initial_state", "attributes"): + actual_values = actual.get(field, {}) + if not isinstance(actual_values, Mapping): + raise ValueError(f"Scene object {uid!r} {field} must be a mapping.") + mismatched = { + key: expected + for key, expected in requirement[field].items() + if actual_values.get(key) != expected + } + if mismatched: + raise ValueError( + f"Scene object {uid!r} does not satisfy {field} {mismatched}." + ) + + +def _canonical(value: Any) -> str: + import json + + if not isinstance(value, Mapping): + raise ValueError("Every satisfied spatial constraint must be a mapping.") + return json.dumps(dict(value), sort_keys=True, separators=(",", ":")) diff --git a/tests/gen_sim/action_engine/__init__.py b/tests/gen_sim/action_engine/__init__.py new file mode 100644 index 000000000..e2bb4c0aa --- /dev/null +++ b/tests/gen_sim/action_engine/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +"""Action Engine tests.""" diff --git a/tests/gen_sim/action_engine/compiler/__init__.py b/tests/gen_sim/action_engine/compiler/__init__.py new file mode 100644 index 000000000..e7977347e --- /dev/null +++ b/tests/gen_sim/action_engine/compiler/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +"""Action Engine compiler tests.""" diff --git a/tests/gen_sim/action_engine/compiler/test_compiler.py b/tests/gen_sim/action_engine/compiler/test_compiler.py new file mode 100644 index 000000000..6144fcc3f --- /dev/null +++ b/tests/gen_sim/action_engine/compiler/test_compiler.py @@ -0,0 +1,572 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from collections.abc import Mapping +from typing import Any + +import pytest + +from embodichain.gen_sim.action_engine.compiler import compile_task_agent +from embodichain.gen_sim.action_engine.domain import ( + EXECUTION_PROGRAM_SCHEMA, + TASK_AGENT_SCHEMA, +) + + +def _program(step: Mapping[str, Any]) -> dict[str, Any]: + return { + "schema_version": TASK_AGENT_SCHEMA, + "task": "operator_demo", + "goal": "Exercise one semantic operator.", + "semantic_steps": [dict(step)], + } + + +def test_place_relative_carries_payloads_through_single_arm_action_bindings() -> None: + execution = compile_task_agent( + _program( + { + "id": "s01_place_carrier", + "operator": "place_relative", + "object": "paper_cup", + "goal": { + "reference_object": "popcorn_bucket", + "relation": "on", + "payloads": [{"object": "glue_stick", "slot": "center"}], + }, + } + ) + ) + + step = execution["semantic_steps"][0] + assert step["goal"]["payloads"] == [{"object": "glue_stick", "slot": "center"}] + carrying_actions = [ + action + for edge in execution["edges"] + for action in edge["actions"] + if action["atomic_action_class"] in {"PickUp", "MoveHeldObject", "Place"} + ] + assert carrying_actions + assert all( + action["target_binding"]["payloads"] == step["goal"]["payloads"] + for action in carrying_actions + ) + + +@pytest.mark.parametrize( + ("step", "expected_action"), + [ + ( + { + "id": "s01_line", + "operator": "arrange_line", + "objects": ["can_a", "can_b"], + "goal": {"axis": "world_y", "anchor": "table_center"}, + }, + "MoveHeldObject", + ), + ( + { + "id": "s01_stack", + "operator": "build_stack", + "objects": ["block_a", "block_b"], + "goal": {"stack_mode": "on_top", "anchor": "table_center"}, + }, + "PickUp", + ), + ( + { + "id": "s01_place", + "operator": "place_relative", + "object": "cup", + "goal": {"reference_object": "tray", "relation": "on"}, + }, + "Place", + ), + ( + { + "id": "s01_hover", + "operator": "hold_hover", + "object": "cup", + "goal": {}, + }, + "MoveJoints", + ), + ( + { + "id": "s01_transport", + "operator": "coordinated_transport", + "object": "tray", + "goal": {"direction": "front", "terminal_behavior": "place"}, + }, + "CoordinatedPickment", + ), + ( + { + "id": "s01_orient", + "operator": "orient_object", + "object": "cup", + "goal": { + "orientation_goal": "upright", + "orientation_axis": "none", + }, + }, + "MoveHeldObject", + ), + ( + { + "id": "s01_press", + "operator": "press", + "object": "button", + "goal": {"terminal_state": "activated"}, + }, + "Press", + ), + ( + { + "id": "s01_coordinated_place", + "operator": "coordinated_place", + "object": "cup", + "goal": {"support_object": "tray", "relation": "on"}, + }, + "CoordinatedPlacement", + ), + ], +) +def test_every_builtin_operator_compiles( + step: Mapping[str, Any], + expected_action: str, +) -> None: + execution = compile_task_agent(_program(step)) + action_classes = { + action["atomic_action_class"] + for edge in execution["edges"] + for action in edge["actions"] + } + + assert execution["schema_version"] == EXECUTION_PROGRAM_SCHEMA + assert expected_action in action_classes + assert execution["nodes"][0]["id"] == execution["start"] + assert execution["goal"] in {node["id"] for node in execution["nodes"]} + + +def test_collective_operator_expands_and_composes_with_press() -> None: + program = { + "schema_version": TASK_AGENT_SCHEMA, + "task": "arrange_then_press", + "goal": "Arrange both cans, then press the button.", + "semantic_steps": [ + { + "id": "s01_line", + "operator": "arrange_line", + "objects": ["can_a", "can_b"], + "goal": {"axis": "world_y", "anchor": "table_center"}, + "depends_on": [], + }, + { + "id": "s02_press", + "operator": "press", + "object": "button", + "goal": {}, + "depends_on": ["s01_line"], + }, + ], + } + + execution = compile_task_agent(program) + steps = {step["id"]: step for step in execution["semantic_steps"]} + + assert list(steps) == ["s01_line__01", "s01_line__02", "s02_press"] + assert steps["s02_press"]["depends_on"] == [ + "s01_line__01", + "s01_line__02", + ] + assert execution["edges"][-1]["depends_on"] == [ + steps["s01_line__01"]["edge_ids"][-1], + steps["s01_line__02"]["edge_ids"][-1], + ] + assert "route" not in repr(execution) + + +def test_coordinated_place_picks_both_objects_before_placement() -> None: + execution = compile_task_agent( + _program( + { + "id": "s01_coordinated_place", + "operator": "coordinated_place", + "object": "cup", + "goal": {"support_object": "tray", "relation": "on"}, + } + ) + ) + step = execution["semantic_steps"][0] + first_edge, placement_edge = [ + next(edge for edge in execution["edges"] if edge["id"] == edge_id) + for edge_id in step["edge_ids"] + ] + + assert [action["atomic_action_class"] for action in first_edge["actions"]] == [ + "PickUp", + "PickUp", + ] + assert [action["actor"] for action in first_edge["actions"]] == [ + {"mode": "required", "arm": "left_arm"}, + {"mode": "required", "arm": "right_arm"}, + ] + assert [action["target_binding"]["object"] for action in first_edge["actions"]] == [ + "cup", + "tray", + ] + assert [action["motion_policy"] for action in first_edge["actions"]] == [ + {"modifiers": []}, + {"modifiers": []}, + ] + assert placement_edge["actions"][0]["atomic_action_class"] == ( + "CoordinatedPlacement" + ) + assert placement_edge["depends_on"] == [first_edge["id"]] + + +def test_independent_required_arms_create_allocation_group() -> None: + program = { + "schema_version": TASK_AGENT_SCHEMA, + "task": "parallel_place", + "goal": "Place two objects with opposite arms.", + "semantic_steps": [ + { + "id": "s01_left", + "operator": "place_relative", + "object": "left_object", + "actor": {"mode": "required", "arm": "left_arm"}, + "goal": {"reference_object": "left_tray", "relation": "on"}, + "depends_on": [], + }, + { + "id": "s02_right", + "operator": "place_relative", + "object": "right_object", + "actor": {"mode": "required", "arm": "right_arm"}, + "goal": {"reference_object": "right_tray", "relation": "on"}, + "depends_on": [], + }, + ], + } + + execution = compile_task_agent(program) + + assert execution["allocation_groups"] == [ + { + "id": "g01_distinct_arms", + "semantic_step_ids": ["s01_left", "s02_right"], + "arm_constraint": "distinct_arms", + "execution_policy": "parallel_if_feasible", + "parallel_action_classes": ["PickUp"], + "workspace_policy": "shared_target_serial", + } + ] + + +def test_orient_object_composes_upright_motion_modifier() -> None: + execution = compile_task_agent( + { + "schema_version": TASK_AGENT_SCHEMA, + "task": "upright", + "goal": "Stand the can upright.", + "semantic_steps": [ + { + "id": "s01_orient", + "operator": "orient_object", + "object": "can", + "actor": {"mode": "auto"}, + "goal": { + "orientation_goal": "upright", + "orientation_axis": "none", + }, + "depends_on": [], + } + ], + "allocation_groups": [], + } + ) + + assert [edge["actions"][0]["motion_policy"] for edge in execution["edges"]] == [ + {"modifiers": [{"type": "orientation", "mode": "upright"}]}, + {"modifiers": [{"type": "orientation", "mode": "upright"}]}, + {"modifiers": [{"type": "orientation", "mode": "upright"}]}, + {"modifiers": [{"type": "orientation", "mode": "upright"}]}, + {"modifiers": [{"type": "orientation", "mode": "upright"}]}, + {"modifiers": []}, + ] + move_phases = [ + edge["actions"][0]["target_binding"]["phase"] + for edge in execution["edges"] + if edge["actions"][0]["atomic_action_class"] == "MoveHeldObject" + ] + assert move_phases == ["staging", "final"] + assert execution["semantic_steps"][0]["goal"] == { + "relation": "none", + "reference_state": "live", + "orientation_goal": "upright", + "orientation_axis": "none", + "position_anchor": "initial_xy", + "support_object": "table", + "upright_local_axis": "auto", + } + + +def test_auto_pickups_require_shared_explicit_allocation_group() -> None: + program = { + "schema_version": TASK_AGENT_SCHEMA, + "task": "dual_arm_basket", + "goal": "Use both arms to place the cube and cup in the basket.", + "semantic_steps": [ + { + "id": "s01_cube", + "operator": "place_relative", + "object": "cube", + "actor": { + "mode": "auto", + "allocation_group": "dual_arms_1", + }, + "goal": {"reference_object": "basket", "relation": "inside"}, + "depends_on": [], + }, + { + "id": "s02_cup", + "operator": "place_relative", + "object": "cup", + "actor": { + "mode": "auto", + "allocation_group": "dual_arms_1", + }, + "goal": {"reference_object": "basket", "relation": "inside"}, + "depends_on": [], + }, + ], + } + + execution = compile_task_agent(program) + pickup_edges = [ + next( + edge + for edge in execution["edges"] + if edge["semantic_step_id"] == step_id + and edge["actions"][0]["atomic_action_class"] == "PickUp" + ) + for step_id in ("s01_cube", "s02_cup") + ] + transport_edges = [ + next( + edge + for edge in execution["edges"] + if edge["semantic_step_id"] == step_id + and edge["actions"][0]["atomic_action_class"] == "MoveHeldObject" + ) + for step_id in ("s01_cube", "s02_cup") + ] + + assert execution["allocation_groups"] == [ + { + "id": "g01_distinct_arms", + "semantic_step_ids": ["s01_cube", "s02_cup"], + "arm_constraint": "distinct_arms", + "execution_policy": "parallel_if_feasible", + "parallel_action_classes": ["PickUp"], + "workspace_policy": "shared_target_serial", + } + ] + assert all("workspace:basket" not in edge["resources"] for edge in pickup_edges) + assert all("workspace:basket" in edge["resources"] for edge in transport_edges) + + for step in program["semantic_steps"]: + step["actor"].pop("allocation_group") + assert compile_task_agent(program)["allocation_groups"] == [] + + +def test_unrelated_dependent_is_allowed_while_hold_reserves_arm() -> None: + program = { + "schema_version": TASK_AGENT_SCHEMA, + "task": "hold_then_press", + "goal": "Hold the cube and then press the button.", + "semantic_steps": [ + { + "id": "s01_hold", + "operator": "hold_hover", + "object": "cube", + "actor": {"mode": "auto"}, + "goal": {}, + "depends_on": [], + }, + { + "id": "s02_press", + "operator": "press", + "object": "button", + "actor": {"mode": "auto"}, + "goal": {}, + "depends_on": ["s01_hold"], + }, + ], + } + + execution = compile_task_agent(program) + steps = {step["id"]: step for step in execution["semantic_steps"]} + + assert steps["s01_hold"]["postcondition"] == { + "type": "object_held", + "object": "cube", + } + assert steps["s02_press"]["depends_on"] == ["s01_hold"] + + +def test_hold_may_follow_an_ancestor_that_previously_used_the_same_object() -> None: + program = { + "schema_version": TASK_AGENT_SCHEMA, + "task": "place_then_hold", + "goal": "Place the cube, then pick it up and keep holding it.", + "semantic_steps": [ + { + "id": "s01_place", + "operator": "place_relative", + "object": "cube", + "actor": {"mode": "auto"}, + "goal": {"reference_object": "tray", "relation": "on"}, + "depends_on": [], + }, + { + "id": "s02_hold", + "operator": "hold_hover", + "object": "cube", + "actor": {"mode": "required", "arm": "left_arm"}, + "goal": {}, + "depends_on": ["s01_place"], + }, + ], + } + + execution = compile_task_agent(program) + + assert execution["semantic_steps"][-1]["postcondition"]["type"] == "object_held" + + +@pytest.mark.parametrize( + ("operator", "actor", "goal"), + [ + ("press", {"mode": "required", "arm": "left_arm"}, {}), + ( + "coordinated_transport", + {"mode": "coordinated", "arms": ["left_arm", "right_arm"]}, + {"direction": "none", "terminal_behavior": "hold"}, + ), + ], +) +def test_required_hold_rejects_later_steps_that_need_its_arm( + operator: str, + actor: dict, + goal: dict, +) -> None: + program = { + "schema_version": TASK_AGENT_SCHEMA, + "task": "occupied_arm", + "goal": "Keep holding the cube, then operate the button.", + "semantic_steps": [ + { + "id": "s01_hold", + "operator": "hold_hover", + "object": "cube", + "actor": {"mode": "required", "arm": "left_arm"}, + "goal": {}, + "depends_on": [], + }, + { + "id": "s02_other", + "operator": operator, + "object": "button", + "actor": actor, + "goal": goal, + "depends_on": ["s01_hold"], + }, + ], + } + + with pytest.raises(ValueError, match="reserves arm 'left_arm'"): + compile_task_agent(program) + + +def test_held_object_cannot_be_reused_by_an_independent_step() -> None: + program = { + "schema_version": TASK_AGENT_SCHEMA, + "task": "conflicting_object_ownership", + "goal": "Hold and place the same cube.", + "semantic_steps": [ + { + "id": "s01_hold", + "operator": "hold_hover", + "object": "cube", + "actor": {"mode": "auto"}, + "goal": {}, + "depends_on": [], + }, + { + "id": "s02_place", + "operator": "place_relative", + "object": "cube", + "actor": {"mode": "auto"}, + "goal": {"reference_object": "tray", "relation": "on"}, + "depends_on": [], + }, + ], + } + + with pytest.raises(ValueError, match="reserves object 'cube'"): + compile_task_agent(program) + + +def test_unknown_operator_is_rejected_before_graph_construction() -> None: + with pytest.raises(ValueError, match="Unknown semantic operator"): + compile_task_agent( + _program( + { + "id": "s01_unknown", + "operator": "teleport", + "object": "cube", + "goal": {}, + } + ) + ) + + +def test_coordinated_transport_rejects_unknown_direction() -> None: + with pytest.raises(ValueError, match="direction"): + compile_task_agent( + _program( + { + "id": "s01_transport", + "operator": "coordinated_transport", + "object": "shared_box", + "actor": { + "mode": "coordinated", + "arms": ["left_arm", "right_arm"], + }, + "goal": { + "direction": "somewhere_vague", + "terminal_behavior": "hold", + }, + "depends_on": [], + } + ) + ) diff --git a/tests/gen_sim/action_engine/compiler/test_v2.py b/tests/gen_sim/action_engine/compiler/test_v2.py new file mode 100644 index 000000000..6409a6a7a --- /dev/null +++ b/tests/gen_sim/action_engine/compiler/test_v2.py @@ -0,0 +1,172 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest + +from embodichain.gen_sim.action_engine.compiler import ( + compile_task_agent, + compile_task_agent_v2, + seed_graph_to_execution_program, +) +from embodichain.gen_sim.action_engine.domain import TASK_AGENT_SCHEMA +from embodichain.gen_sim.action_engine.protocol import SEED_GRAPH_SCHEMA + + +def _task_agent() -> dict: + return { + "schema_version": TASK_AGENT_SCHEMA, + "task": "place-cup", + "goal": "Place the cup in the tray.", + "semantic_steps": [ + { + "id": "s01_place_cup", + "operator": "place_relative", + "object": "cup", + "actor": {"mode": "auto"}, + "goal": { + "relation": "inside", + "reference_object": "tray", + "reference_state": "live", + "slot": "auto", + "orientation_goal": "preserve", + "orientation_axis": "none", + }, + "depends_on": [], + } + ], + "allocation_groups": [], + } + + +def test_v2_compiler_preserves_mature_atomic_action_topology() -> None: + known = {"cup", "tray"} + legacy = compile_task_agent(_task_agent(), known_objects=known) + seed = compile_task_agent_v2(_task_agent(), known_objects=known) + materialized = seed_graph_to_execution_program(seed, known_objects=known) + + legacy_actions = [ + action["atomic_action_class"] + for edge in legacy["edges"] + for action in edge["actions"] + ] + seed_actions = [node["atomic_action"] for node in seed["nodes"]] + materialized_actions = [ + action["atomic_action_class"] + for edge in materialized["edges"] + for action in edge["actions"] + ] + assert seed["schema_version"] == SEED_GRAPH_SCHEMA + assert seed_actions == legacy_actions + assert materialized_actions == legacy_actions + assert seed["task_groups"][0]["task_type"] == "E1" + + +@pytest.mark.parametrize( + ("operator", "objects", "goal", "actor"), + [ + ( + "orient_object", + ["can"], + { + "orientation_goal": "upright", + "orientation_axis": "long_axis", + "position_anchor": "initial_xy", + "support_object": "table", + "upright_local_axis": "long_axis", + }, + {"mode": "auto"}, + ), + ( + "coordinated_transport", + ["tray"], + { + "direction": "up", + "terminal_behavior": "hold", + "orientation_goal": "preserve", + "orientation_axis": "none", + }, + {"mode": "coordinated", "arms": ["left_arm", "right_arm"]}, + ), + ( + "build_stack", + ["cube_a", "cube_b"], + { + "anchor": "table_center", + "stack_mode": "on_top", + "orientation_goal": "preserve", + "orientation_axis": "none", + }, + {"mode": "auto"}, + ), + ( + "arrange_line", + ["cube_a", "cube_b"], + { + "anchor": "table_center", + "axis": "world_y", + "order_by": "explicit", + "order_constraint": "ordered", + "order_direction": "given", + "orientation_goal": "preserve", + "orientation_axis": "none", + "participation": "auto", + }, + {"mode": "auto"}, + ), + ], +) +def test_v2_preserves_all_current_task_recipe_topologies( + operator: str, + objects: list[str], + goal: dict, + actor: dict, +) -> None: + step = { + "id": "task_01", + "operator": operator, + "actor": actor, + "goal": goal, + "depends_on": [], + } + if operator in {"build_stack", "arrange_line"}: + step["objects"] = objects + else: + step["object"] = objects[0] + task = { + "schema_version": TASK_AGENT_SCHEMA, + "task": f"regression-{operator}", + "goal": f"Regression task for {operator}.", + "semantic_steps": [step], + "allocation_groups": [], + } + known = {*objects, "table"} + legacy = compile_task_agent(task, known_objects=known) + seed = compile_task_agent_v2(task, known_objects=known) + rematerialized = seed_graph_to_execution_program(seed, known_objects=known) + + def signature(program: dict) -> dict[str, list[list[str]]]: + edges = {edge["id"]: edge for edge in program["edges"]} + return { + step["id"]: [ + [action["atomic_action_class"] for action in edges[edge_id]["actions"]] + for edge_id in step["edge_ids"] + ] + for step in program["semantic_steps"] + } + + assert signature(rematerialized) == signature(legacy) diff --git a/tests/gen_sim/action_engine/config/__init__.py b/tests/gen_sim/action_engine/config/__init__.py new file mode 100644 index 000000000..355d915ff --- /dev/null +++ b/tests/gen_sim/action_engine/config/__init__.py @@ -0,0 +1,17 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations diff --git a/tests/gen_sim/action_engine/config/test_runtime_policy.py b/tests/gen_sim/action_engine/config/test_runtime_policy.py new file mode 100644 index 000000000..b2ffe9a7b --- /dev/null +++ b/tests/gen_sim/action_engine/config/test_runtime_policy.py @@ -0,0 +1,420 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from copy import deepcopy +import hashlib +import json + +import pytest + +from embodichain.gen_sim.action_engine.config import ( + ArmSelectionPolicyCfg, + RuntimePolicyCfg, + default_runtime_policy, + generation_defaults, + resolve_agent_runtime_policy, + runtime_policy_hash, +) +from embodichain.lab.sim.atomic_actions.primitives.place import PlaceOptions + + +def test_default_runtime_policy_preserves_current_arm_selection_behavior() -> None: + policy = default_runtime_policy("dual_ur10") + + assert policy.arm_selection.as_mapping() == { + "crossing_deadband_ratio": 0.08, + "allow_cross_side_fallback": False, + "pickup_crossing_weight": 1.0, + "placement_crossing_weight": 1.5, + "motion_cost_scale": pytest.approx(3.141592653589793), + "fallback_workspace_half_width": 0.5, + "orient_object_preferred_arm_deadband": 0.02, + } + + +def test_defaults_cover_current_execution_and_generation_policy() -> None: + runtime = default_runtime_policy("dual_ur10") + generation = generation_defaults() + + assert runtime.execution == { + "max_transitions": 1000, + "semantic_step_settle_steps": 10, + "max_retries_per_action": 2, + "max_graph_revisions": 8, + "max_recovery_actions": 12, + "support_stability_samples": 3, + "support_stability_interval_steps": 5, + "support_linear_velocity_tolerance": pytest.approx(0.02), + "support_angular_velocity_tolerance": pytest.approx(0.2), + } + assert runtime.planner == { + "backend": "curobo", + "single_arm_strategy": "motion_gen", + "coordinated_strategy": "ik_interp", + "fallback_strategy": "ik_interp", + "allow_fallback": True, + "dynamic_collision": False, + "static_obstacle_uids": [], + "dynamic_obstacle_uids": [], + "curobo": { + "log_level": "error", + "obstacle_representation": "cuboid", + "multi_env": False, + "use_cuda_graph": True, + "preserve_plan_samples": False, + "max_attempts": 5, + "collision_activation_distance": pytest.approx(0.01), + }, + } + assert runtime.grounding["arrangement"]["row_search_radius"] == 0.25 + assert runtime.grasp["antipodal_n_sample"] == 10000 + assert "max_open_length" not in runtime.grasp + assert "min_open_length" not in runtime.grasp + assert "finger_length" not in runtime.grasp + assert runtime.motion_modifiers["orientation"]["upright"]["MoveHeldObject"][ + "surface_clearance" + ] == pytest.approx(0.05) + assert ( + "upright_yaw_samples" + not in runtime.motion_modifiers["orientation"]["upright"]["PickUp"] + ) + assert ( + "upright_yaw_samples" + not in runtime.motion_modifiers["orientation"]["upright"]["MoveHeldObject"] + ) + assert runtime.motion_modifiers["handover_role"]["transfer"]["PickUp"] == { + "sample_interval": 80, + "hand_interp_steps": 5, + "pick_object_part": "top", + } + assert runtime.motion_defaults["HandOver"]["receive_pick_object_part"] == "bottom" + assert runtime.motion_defaults["CoordinatedPickment"][ + "middle_empty_ratio" + ] == pytest.approx(0.4) + assert ( + runtime.motion_defaults["CoordinatedPickment"]["is_filter_ground_collision"] + is False + ) + assert ( + runtime.motion_defaults["CoordinatedPickment"]["release_sample_interval"] == 60 + ) + assert runtime.motion_defaults["CoordinatedPickment"][ + "release_gripper_tolerance" + ] == pytest.approx(0.08) + assert runtime.predicate_fallbacks["upright_max_tilt"] == pytest.approx( + 0.2617993877991494 + ) + assert generation["physics"]["rigid_object"]["mass"] == pytest.approx(0.1) + assert generation["task"]["default_gripper_model"] == "pgi" + assert generation["task"]["default_ik_solver"] == "auto" + assert generation["environment"]["arm_aim_yaw_offset"] == { + "left": pytest.approx(0.0), + "right": pytest.approx(0.0), + } + assert generation["environment"]["recording"] == { + "enabled": True, + "resolution": [640, 360], + "interval_step": 5, + } + assert generation["scene"]["object_length_sample_points"] == 5000 + assert generation["dataset"]["control_frequency"] == 25 + assert generation["randomization"]["table_height_delta_range"] == [ + [-0.05], + [0.05], + ] + + +def test_e1_motion_defaults_match_atomic_action_tutorial_cadence() -> None: + policy = default_runtime_policy("dual_franka") + motion = policy.motion_defaults + + assert motion["PickUp"] == { + "pre_grasp_distance": pytest.approx(0.15), + "lift_height": pytest.approx(0.16), + "sample_interval": 120, + "hand_interp_steps": 12, + } + assert motion["MoveHeldObject"]["sample_interval"] == 120 + assert motion["Place"] == { + "sample_interval": 120, + "lift_height": pytest.approx(0.14), + "post_hold_steps": 60, + "cartesian_waypoint_count": 2, + "hand_interp_steps": 12, + } + assert policy.motion_modifiers["orientation"]["upright"]["Place"] == { + "sample_interval": 120, + "post_hold_steps": 60, + "hand_interp_steps": 12, + } + + +def test_axis_align_defaults_preserve_action_engine_clearance_policy() -> None: + axis_align = default_runtime_policy("dual_franka").motion_defaults["AxisAlign"] + + assert axis_align == { + "sample_interval": 180, + "pre_grasp_distance": pytest.approx(0.15), + "lift_height": pytest.approx(0.16), + "lower_distance": pytest.approx(0.16), + "hand_interp_steps": 12, + } + + +def test_place_defaults_fit_the_mainline_motion_sample_budget() -> None: + place = default_runtime_policy("dual_ur10").motion_defaults["Place"] + sample_count = int(place["sample_interval"]) + hand_steps = PlaceOptions().hand_interp_steps + motion_steps = sample_count - hand_steps + down_steps = int(round(motion_steps) * 0.6) + back_steps = motion_steps - down_steps + cartesian_count = int(place["cartesian_waypoint_count"]) + + assert 1 + 2 * cartesian_count <= down_steps + assert 1 + cartesian_count <= back_steps + + +def test_default_runtime_policy_returns_detached_profile_snapshots() -> None: + first = default_runtime_policy("dual_ur10") + second = default_runtime_policy("dual_ur10") + franka = default_runtime_policy("dual_franka") + + first.arm_selection.pickup_crossing_weight = 9.0 + first.motion_defaults["PickUp"]["lift_height"] = 9.0 + + assert second.arm_selection.pickup_crossing_weight == 1.0 + assert second.motion_defaults["PickUp"]["lift_height"] == 0.16 + assert franka.arm_selection.pickup_crossing_weight == 1.0 + assert franka.motion_defaults["MoveEndEffector"]["retreat_height"] == 0.10 + + +def test_generation_defaults_return_detached_values() -> None: + first = generation_defaults() + second = generation_defaults() + + first["physics"]["rigid_object"]["mass"] = 9.0 + + assert second["physics"]["rigid_object"]["mass"] == pytest.approx(0.1) + + +@pytest.mark.parametrize( + ("field", "value", "message"), + [ + ("crossing_deadband_ratio", 1.0, "crossing_deadband_ratio"), + ("pickup_crossing_weight", -0.1, "pickup_crossing_weight"), + ("placement_crossing_weight", -0.1, "placement_crossing_weight"), + ("motion_cost_scale", 0.0, "motion_cost_scale"), + ("fallback_workspace_half_width", 0.0, "fallback_workspace_half_width"), + ], +) +def test_arm_selection_policy_rejects_invalid_values( + field: str, + value: float, + message: str, +) -> None: + values = default_runtime_policy("dual_ur10").arm_selection.as_mapping() + values[field] = value + + with pytest.raises(ValueError, match=message): + ArmSelectionPolicyCfg.from_mapping(values) + + +def test_arm_selection_policy_requires_boolean_cross_side_fallback() -> None: + values = default_runtime_policy("dual_ur10").arm_selection.as_mapping() + values["allow_cross_side_fallback"] = "false" + + with pytest.raises(TypeError, match="allow_cross_side_fallback"): + ArmSelectionPolicyCfg.from_mapping(values) + + +def test_arm_selection_policy_loads_old_snapshot_without_fallback_field() -> None: + snapshot = default_runtime_policy("dual_ur10").as_mapping() + snapshot["arm_selection"].pop("allow_cross_side_fallback") + payload = json.dumps( + snapshot, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=True, + ).encode("utf-8") + + policy = resolve_agent_runtime_policy( + { + "robot_profile": "dual_ur10", + "runtime_policy": snapshot, + "runtime_policy_hash": hashlib.sha256(payload).hexdigest(), + } + ) + + assert policy.arm_selection.allow_cross_side_fallback is False + + +def test_agent_policy_snapshot_is_hash_verified_and_legacy_config_falls_back() -> None: + policy = default_runtime_policy("dual_ur5") + snapshot = policy.as_mapping() + config = { + "robot_profile": "dual_ur5", + "runtime_policy": snapshot, + "runtime_policy_hash": runtime_policy_hash(policy), + } + + resolved = resolve_agent_runtime_policy(config) + assert resolved.as_mapping() == snapshot + + tampered = deepcopy(config) + tampered["runtime_policy"]["motion_defaults"]["PickUp"]["lift_height"] = 8.0 + with pytest.raises(ValueError, match="hash does not match"): + resolve_agent_runtime_policy(tampered) + + legacy = resolve_agent_runtime_policy({"robot_profile": "dual_ur5"}) + assert legacy.as_mapping() == snapshot + + +def test_v6_policy_snapshot_adds_axis_align_defaults_without_rewriting_e1() -> None: + snapshot = default_runtime_policy("dual_franka").as_mapping() + snapshot["schema_version"] = "action_engine_runtime_policy_v6" + snapshot["motion_defaults"].pop("AxisAlign") + snapshot["motion_defaults"]["PickUp"]["lift_height"] = 0.11 + payload = json.dumps( + snapshot, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=True, + ).encode("utf-8") + + resolved = resolve_agent_runtime_policy( + { + "robot_profile": "dual_franka", + "runtime_policy": snapshot, + "runtime_policy_hash": hashlib.sha256(payload).hexdigest(), + } + ) + + assert resolved.schema_version == "action_engine_runtime_policy_v8" + assert resolved.motion_defaults["AxisAlign"]["sample_interval"] == 180 + assert resolved.motion_defaults["PickUp"]["lift_height"] == pytest.approx(0.11) + + +def test_v7_policy_snapshot_drops_legacy_gripper_geometry() -> None: + snapshot = default_runtime_policy("dual_ur10").as_mapping() + snapshot["schema_version"] = "action_engine_runtime_policy_v7" + snapshot["grasp"].update( + { + "min_open_length": 0.01, + "max_open_length": 0.15, + "finger_length": 0.13, + } + ) + payload = json.dumps( + snapshot, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=True, + ).encode("utf-8") + + resolved = resolve_agent_runtime_policy( + { + "robot_profile": "dual_ur10", + "runtime_policy": snapshot, + "runtime_policy_hash": hashlib.sha256(payload).hexdigest(), + } + ) + + assert resolved.schema_version == "action_engine_runtime_policy_v8" + assert "min_open_length" not in resolved.grasp + assert "max_open_length" not in resolved.grasp + assert "finger_length" not in resolved.grasp + + +def test_narrow_v1_policy_snapshot_is_migrated_to_complete_runtime_policy() -> None: + snapshot = { + "schema_version": "action_engine_runtime_policy_v1", + "arm_selection": { + "crossing_deadband_ratio": 0.08, + "pickup_crossing_weight": 2.0, + "placement_crossing_weight": 1.5, + "motion_cost_scale": 3.141592653589793, + "fallback_workspace_half_width": 0.5, + }, + } + payload = json.dumps( + snapshot, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=True, + ).encode("utf-8") + + resolved = resolve_agent_runtime_policy( + { + "robot_profile": "dual_ur10", + "runtime_policy": snapshot, + "runtime_policy_hash": hashlib.sha256(payload).hexdigest(), + } + ) + + assert resolved.arm_selection.pickup_crossing_weight == 2.0 + assert resolved.motion_defaults["PickUp"]["lift_height"] == 0.16 + + +def test_v3_policy_snapshot_is_migrated_with_default_planner_policy() -> None: + expected = default_runtime_policy("dual_ur10") + snapshot = expected.as_mapping() + snapshot.pop("planner") + snapshot["schema_version"] = "action_engine_runtime_policy_v3" + payload = json.dumps( + snapshot, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=True, + ).encode("utf-8") + + resolved = resolve_agent_runtime_policy( + { + "robot_profile": "dual_ur10", + "runtime_policy": snapshot, + "runtime_policy_hash": hashlib.sha256(payload).hexdigest(), + } + ) + + assert resolved.schema_version == "action_engine_runtime_policy_v8" + assert resolved.planner == expected.planner + + +def test_curobo_policy_rejects_coordinated_motion_generation() -> None: + snapshot = default_runtime_policy("dual_ur10").as_mapping() + snapshot["planner"]["coordinated_strategy"] = "motion_gen" + + with pytest.raises(ValueError, match="coordinated_strategy"): + RuntimePolicyCfg.from_mapping(snapshot) + + +@pytest.mark.parametrize( + ("patch", "message"), + [ + ({"fallback_strategy": "motion_gen"}, "fallback_strategy"), + ({"backend": "toppra", "dynamic_collision": True}, "dynamic_collision"), + ], +) +def test_planner_policy_rejects_unsupported_combinations( + patch: dict[str, object], + message: str, +) -> None: + snapshot = default_runtime_policy("dual_ur10").as_mapping() + snapshot["planner"].update(patch) + + with pytest.raises(ValueError, match=message): + RuntimePolicyCfg.from_mapping(snapshot) diff --git a/tests/gen_sim/action_engine/domain/__init__.py b/tests/gen_sim/action_engine/domain/__init__.py new file mode 100644 index 000000000..c8e03f284 --- /dev/null +++ b/tests/gen_sim/action_engine/domain/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +"""Action Engine domain tests.""" diff --git a/tests/gen_sim/action_engine/domain/test_programs.py b/tests/gen_sim/action_engine/domain/test_programs.py new file mode 100644 index 000000000..8fdf8074b --- /dev/null +++ b/tests/gen_sim/action_engine/domain/test_programs.py @@ -0,0 +1,150 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from copy import deepcopy + +import pytest + +from embodichain.gen_sim.action_engine.compiler import compile_task_agent +from embodichain.gen_sim.action_engine.domain import ( + TASK_AGENT_SCHEMA, + execution_program_hash, + validate_execution_program, + validate_task_agent, +) + + +def _task_agent() -> dict: + return { + "schema_version": TASK_AGENT_SCHEMA, + "task": "place_demo", + "goal": "Place the cup on the tray.", + "semantic_steps": [ + { + "id": "s01_place", + "operator": "place_relative", + "object": "cup", + "actor": {"mode": "auto"}, + "goal": {"reference_object": "tray", "relation": "on"}, + "depends_on": [], + } + ], + } + + +def test_task_validation_is_detached_and_adds_unambiguous_defaults() -> None: + source = _task_agent() + del source["semantic_steps"][0]["actor"] + del source["semantic_steps"][0]["depends_on"] + + validated = validate_task_agent(source) + validated["semantic_steps"][0]["goal"]["relation"] = "inside" + + assert source["semantic_steps"][0]["goal"]["relation"] == "on" + assert validated["semantic_steps"][0]["actor"] == {"mode": "auto"} + assert validated["semantic_steps"][0]["depends_on"] == [] + + +@pytest.mark.parametrize( + "actor", + [ + {"mode": "auto", "allocation_group": "dual_arms_1"}, + { + "mode": "required", + "arm": "left_arm", + "allocation_group": "dual_arms_1", + }, + ], +) +def test_single_arm_allocation_group_is_validated_and_preserved(actor: dict) -> None: + source = _task_agent() + source["semantic_steps"][0]["actor"] = actor + + validated = validate_task_agent(source) + execution = compile_task_agent(validated) + + assert validated["semantic_steps"][0]["actor"] == actor + assert execution["semantic_steps"][0]["actor"] == actor + assert all( + action["actor"] == actor + for edge in execution["edges"] + for action in edge["actions"] + ) + + +def test_allocation_group_must_be_nonempty_and_single_arm_only() -> None: + source = _task_agent() + source["semantic_steps"][0]["actor"]["allocation_group"] = " " + with pytest.raises(ValueError, match="allocation_group"): + validate_task_agent(source) + + source["semantic_steps"][0]["actor"] = { + "mode": "coordinated", + "arms": ["left_arm", "right_arm"], + "allocation_group": "dual_arms_1", + } + with pytest.raises(ValueError, match="unknown fields"): + validate_task_agent(source) + + +def test_task_validation_rejects_cycles_and_grounded_values() -> None: + cyclic = _task_agent() + cyclic["semantic_steps"].extend( + [ + { + "id": "s02", + "operator": "press", + "object": "button", + "depends_on": ["s03"], + }, + { + "id": "s03", + "operator": "press", + "object": "button", + "depends_on": ["s02"], + }, + ] + ) + with pytest.raises(ValueError, match="cycle"): + validate_task_agent(cyclic) + + grounded = _task_agent() + grounded["semantic_steps"][0]["goal"]["target_pose"] = [0.0, 0.0, 0.0] + with pytest.raises(ValueError, match="grounded runtime data"): + validate_task_agent(grounded) + + +def test_execution_hash_is_stable_and_validation_is_strict() -> None: + execution = compile_task_agent(_task_agent()) + reordered = {key: execution[key] for key in reversed(list(execution))} + + assert execution_program_hash(execution) == execution_program_hash(reordered) + assert len(execution_program_hash(execution)) == 64 + + broken = deepcopy(execution) + broken["edges"][0]["target_binding"] = {} + with pytest.raises(ValueError, match="unknown fields"): + validate_execution_program(broken) + + +def test_execution_validation_rejects_unowned_edges() -> None: + execution = compile_task_agent(_task_agent()) + execution["semantic_steps"][0]["edge_ids"].pop() + + with pytest.raises(ValueError, match="unowned edges"): + validate_execution_program(execution) diff --git a/tests/gen_sim/action_engine/domain/test_task_contracts.py b/tests/gen_sim/action_engine/domain/test_task_contracts.py new file mode 100644 index 000000000..daa54a53c --- /dev/null +++ b/tests/gen_sim/action_engine/domain/test_task_contracts.py @@ -0,0 +1,97 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest + +from embodichain.gen_sim.action_engine.domain import ( + RELATIONS, + TASK_CONTRACTS, + TASK_TYPES, + TERMINAL_BEHAVIORS, + TRANSPORT_DIRECTIONS, + normalize_placement_relation, + task_contract, + task_success_type, +) + + +def test_task_contract_catalog_covers_the_canonical_protocol() -> None: + assert set(TASK_CONTRACTS) == set(TASK_TYPES) + assert all(contract.core_actions for contract in TASK_CONTRACTS.values()) + assert {contract.source_structure for contract in TASK_CONTRACTS.values()} == { + "articulation", + "rigid_object", + } + assert task_contract("E2").success_type == "object_upright" + assert task_contract("E5").scene_affordances == { + "dual_graspable", + "rigid", + } + + +def test_task_contracts_declare_carrier_flow_and_resource_semantics() -> None: + e1 = task_contract("E1") + e3 = task_contract("E3") + e5 = task_contract("E5") + + assert e1.direct_payload_relations == {"on", "inside"} + assert e1.accepts_direct_payloads + assert e3.primary_role_field == "source_role" + assert e5.accepts_direct_payloads + assert e5.moves_primary_object + assert e5.resource_mode == "coordinated" + assert not task_contract("E2").accepts_direct_payloads + + +def test_e5_success_depends_only_on_terminal_behavior() -> None: + assert task_success_type("E5", {"terminal_behavior": "hold"}) == ( + "held_by_both_grippers" + ) + assert task_success_type("E5", {"terminal_behavior": "place"}) == "semantic_goal" + with pytest.raises(ValueError, match="terminal_behavior"): + task_success_type("E5", {"terminal_behavior": "none"}) + + +def test_e4_success_depends_on_its_own_terminal_behavior() -> None: + contract = task_contract("E4") + + assert {"target", "relation", "terminal_behavior"} <= set( + contract.applicable_intent_fields + ) + assert task_success_type("E4", {"terminal_behavior": "hold"}) == ( + "handover_complete" + ) + assert task_success_type("E4", {"terminal_behavior": "place"}) == "semantic_goal" + + +def test_symbolic_transport_values_are_language_neutral_protocol_enums() -> None: + assert {"on", "inside", "behind", "left_of"} <= RELATIONS + assert {"none", "up", "left", "world_y"} <= TRANSPORT_DIRECTIONS + assert TERMINAL_BEHAVIORS == {"none", "hold", "place"} + + +@pytest.mark.parametrize("relation", ["above", "on_top", "on_top_of"]) +def test_released_hover_and_legacy_support_relations_normalize_to_on( + relation: str, +) -> None: + assert normalize_placement_relation(relation) == "on" + + +def test_placement_relation_normalization_rejects_non_spatial_semantics() -> None: + with pytest.raises(ValueError, match="Unsupported placement relation"): + normalize_placement_relation("visual_slot") diff --git a/tests/gen_sim/action_engine/domain/test_v2.py b/tests/gen_sim/action_engine/domain/test_v2.py new file mode 100644 index 000000000..84cb02b7f --- /dev/null +++ b/tests/gen_sim/action_engine/domain/test_v2.py @@ -0,0 +1,280 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from copy import deepcopy + +import pytest + +from embodichain.gen_sim.action_engine.capabilities import ( + build_atomic_capability_registry, +) +from embodichain.gen_sim.action_engine.domain import ( + motion_policy, + public_task_spec, + seed_graph_hash, + validate_public_task_spec, + validate_scene_requirements, + validate_seed_graph, + validate_task_spec, +) +from embodichain.gen_sim.action_engine.protocol import ( + SCENE_REQUIREMENTS_SCHEMA, + SEED_GRAPH_SCHEMA, + TASK_SPEC_SCHEMA, +) +from embodichain.gen_sim.action_engine.planning.linker import link_seed_graph + + +def _seed_graph() -> dict: + registry = build_atomic_capability_registry() + draft = { + "schema_version": SEED_GRAPH_SCHEMA, + "task_id": "place-cup", + "instruction": "Place the cup in the tray.", + "level": "L1", + "reasoning_type": "none", + "planner_route": "offline", + "nodes": [ + { + "id": "pick_cup", + "atomic_action": "PickUp", + "object_uid": "cup", + "actor": {"mode": "auto"}, + "control": "arm", + "target_binding": {"kind": "object", "object": "cup"}, + "depends_on": [], + "task_instance_id": "e1_001", + "task_type": "E1", + "role": "primary", + "precondition": {"type": "object_not_fallen", "object": "cup"}, + "postcondition": {"type": "object_held", "object": "cup"}, + "motion_policy": motion_policy(), + }, + { + "id": "place_cup", + "atomic_action": "Place", + "object_uid": "cup", + "actor": {"mode": "auto"}, + "control": "arm", + "target_binding": {"kind": "current_held_pose"}, + "depends_on": ["pick_cup"], + "task_instance_id": "e1_001", + "task_type": "E1", + "role": "primary", + "precondition": {"type": "object_held", "object": "cup"}, + "postcondition": { + "type": "object_in_container", + "object": "cup", + "container": "tray", + }, + "motion_policy": motion_policy(), + }, + ], + "task_groups": [ + { + "id": "e1_001", + "task_type": "E1", + "role": "primary", + "operator": "place_relative", + "object_uid": "cup", + "actor": {"mode": "auto"}, + "goal": {"relation": "inside", "reference_object": "tray"}, + "depends_on": [], + "node_ids": ["pick_cup", "place_cup"], + "success": { + "type": "object_in_container", + "object": "cup", + "container": "tray", + }, + } + ], + "success": { + "type": "object_in_container", + "object": "cup", + "container": "tray", + }, + "capability_catalog_hash": registry.catalog_hash(), + "metadata": {}, + } + return link_seed_graph( + draft, + registry=registry, + task_order=["e1_001"], + known_objects={"cup", "tray"}, + ) + + +def test_seed_graph_validates_direct_atomic_action_nodes() -> None: + registry = build_atomic_capability_registry() + graph = validate_seed_graph( + _seed_graph(), + known_objects={"cup", "tray"}, + known_actions=registry.names(), + executable_actions=registry.executable_names(), + require_executable=True, + ) + assert [node["atomic_action"] for node in graph["nodes"]] == ["PickUp", "Place"] + assert graph["task_groups"][0]["node_ids"] == ["pick_cup", "place_cup"] + + +def test_seed_graph_rejects_grounded_motion_and_cycles() -> None: + grounded = _seed_graph() + grounded["nodes"][0]["target_binding"]["target_pose"] = [0.0, 0.0, 0.0] + with pytest.raises(ValueError, match="grounded motion data"): + validate_seed_graph(grounded) + + cyclic = _seed_graph() + cyclic["nodes"][0]["depends_on"] = ["place_cup"] + with pytest.raises(ValueError, match="dependency cycle"): + validate_seed_graph(cyclic) + + +def test_seed_graph_rejects_unknown_uids_illegal_groups_and_resource_conflicts() -> ( + None +): + with pytest.raises(ValueError, match="unknown object"): + validate_seed_graph(_seed_graph(), known_objects={"tray"}) + + illegal_group = _seed_graph() + illegal_group["task_groups"][0]["task_type"] = "E9" + for node in illegal_group["nodes"]: + node["task_type"] = "E9" + with pytest.raises(ValueError, match="core actions"): + validate_seed_graph(illegal_group) + + conflicting = _seed_graph() + conflicting["nodes"][1]["depends_on"] = [] + conflicting["task_groups"][0]["contract"]["entry_node_ids"] = [ + "pick_cup", + "place_cup", + ] + conflicting["task_groups"][0]["contract"]["terminal_node_ids"] = [ + "pick_cup", + "place_cup", + ] + with pytest.raises(ValueError, match="resource conflicts"): + validate_seed_graph(conflicting) + + +def test_seed_graph_hash_is_order_stable_and_detached() -> None: + graph = _seed_graph() + original = deepcopy(graph) + first = seed_graph_hash(graph) + second = seed_graph_hash({key: graph[key] for key in reversed(graph)}) + assert first == second + assert graph == original + + +def test_task_spec_enforces_reasoning_level_and_repetition_shape() -> None: + spec = { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": "upright-cans", + "level": "L2", + "instruction": "Stand both cans upright.", + "reasoning_type": "none", + "task_instances": [ + { + "id": "e2_1", + "task_type": "E2", + "params": {"object_role": "can_1"}, + "depends_on": [], + }, + { + "id": "e2_2", + "task_type": "E2", + "params": {"object_role": "can_2"}, + "depends_on": [], + }, + ], + "success": {"type": "all_upright"}, + "oracle": {"object_roles": ["can_1", "can_2"]}, + "metadata": {}, + } + assert validate_task_spec(spec)["level"] == "L2" + spec["level"] = "L4" + with pytest.raises(ValueError, match="non-'none'"): + validate_task_spec(spec) + + +def test_public_l4_task_hides_oracle_and_reference_instances() -> None: + spec = { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": "complete-mouth", + "level": "L4", + "instruction": "Complete the missing mouth.", + "reasoning_type": "visual_semantics", + "task_instances": [ + { + "id": "hidden_e1", + "task_type": "E1", + "params": {"object_role": "mouth", "target_role": "face"}, + "depends_on": [], + "role": "primary", + } + ], + "success": {"type": "visual_part_complete"}, + "oracle": {"missing_part": "mouth"}, + "metadata": {}, + } + + public = public_task_spec(spec) + + assert "oracle" not in public + assert "task_instances" not in public + assert validate_public_task_spec(public) == public + + +def test_scene_requirements_validate_task_first_handoff() -> None: + requirements = validate_scene_requirements( + { + "schema_version": SCENE_REQUIREMENTS_SCHEMA, + "task_id": "upright-cans", + "objects": [ + { + "role_id": "can", + "category": "can", + "count": 2, + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {}, + } + ], + "cameras": [{"role": "overview", "requires_rgb": True}], + "spatial_constraints": [], + "distractor_count": 0, + "metadata": {}, + } + ) + assert requirements["objects"][0]["count"] == 2 + + +def test_planning_only_capability_is_rejected_before_execution() -> None: + registry = build_atomic_capability_registry() + graph = _seed_graph() + graph["nodes"][0]["atomic_action"] = "TurnKnob" + graph["nodes"][0]["target_binding"] = { + "kind": "articulation_goal", + "object": "cup", + } + with pytest.raises(ValueError, match="planning-only"): + validate_seed_graph( + graph, + known_actions=registry.names(), + executable_actions=(set(registry.executable_names()) - {"TurnKnob"}), + require_executable=True, + ) diff --git a/tests/gen_sim/action_engine/generation/test_generation.py b/tests/gen_sim/action_engine/generation/test_generation.py new file mode 100644 index 000000000..0f8b8b292 --- /dev/null +++ b/tests/gen_sim/action_engine/generation/test_generation.py @@ -0,0 +1,1975 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Focused tests for the independent Action Engine generation boundary.""" + +from __future__ import annotations + +import inspect +import json +from pathlib import Path +import sys +from types import ModuleType + +import numpy as np +import pytest + +from embodichain.gen_sim.action_engine.protocol import ( + SCENE_REQUIREMENTS_SCHEMA, + TASK_SPEC_SCHEMA, +) +from embodichain.gen_sim.action_engine.cli import ( + generate_action_agent_config as cli_module, +) +from embodichain.gen_sim.action_engine.cli.generate_action_agent_config import ( + _load_planner_config, + build_parser, +) +from embodichain.gen_sim.action_engine.generation.artifacts import ( + artifact_paths, + write_generation_artifacts, +) +from embodichain.gen_sim.action_engine.generation import ( + config_builder as config_builder_module, +) +from embodichain.gen_sim.action_engine.generation.config_builder import ( + build_agent_config, + build_fast_gym_config, + validate_fast_gym_config, +) +from embodichain.gen_sim.action_engine.gripper_profiles import get_gripper_profile +from embodichain.gen_sim.action_engine.generation.generator import ( + _add_ab_camera_requirements, + _scene_requirements_from_bindings, + _task_spec_role_bindings, + generate_action_engine_config, +) +from embodichain.gen_sim.action_engine.generation.source_scene import ( + prepare_scene, + resolve_gym_config_path, + resolve_source_scene, +) +from embodichain.gen_sim.action_engine.tasks import GroundedTaskSpec + + +@pytest.fixture +def gym_export(tmp_path: Path) -> Path: + export = tmp_path / "gym_export" + assets = export / "mesh_assets" + assets.mkdir(parents=True) + (assets / "table.glb").write_bytes(b"not-a-real-glb") + (assets / "can.glb").write_bytes(b"not-a-real-glb") + state = export / "scene_state" + state.mkdir() + (state / "result.json").write_text("{}\n", encoding="utf-8") + + config = { + "id": "Prompt2Scene-test-v0", + "env": {"events": {}, "observations": {}, "dataset": {}}, + "robot": {}, + "sensor": [], + "light": {}, + "background": [ + { + "uid": "table_0", + "description": "A white table.", + "shape": {"shape_type": "Mesh", "fpath": "mesh_assets/table.glb"}, + "init_pos": [0.0, 0.0, 0.0], + "init_rot": [0.0, 0.0, 0.0], + "body_scale": [1.0, 1.0, 1.0], + } + ], + "rigid_object": [ + { + "uid": "interact_can_0", + "description": "A red soda can.", + "shape": { + "shape_type": "Mesh", + "fpath": "mesh_assets/can.glb", + "acd_method": "coacd", + "max_convex_hull_num": 32, + }, + "attrs": {"mass": 0.01}, + "init_pos": [1.0, 2.0, 0.7], + "init_rot": [0.0, 0.0, 0.0], + "body_scale": [1.0, 1.0, 1.0], + "max_convex_hull_num": 32, + } + ], + } + (export / "gym_config.json").write_text( + json.dumps(config), + encoding="utf-8", + ) + return export + + +@pytest.fixture +def scene_export(tmp_path: Path) -> Path: + export = tmp_path / "scene_export" + assets = export / "mesh_assets" + assets.mkdir(parents=True) + (assets / "table.glb").write_bytes(b"not-a-real-glb") + (assets / "bottle_001.glb").write_bytes(b"not-a-real-glb") + (assets / "bottle_002.glb").write_bytes(b"not-a-real-glb") + + config = { + "format": "embodichain.scene-export/v1", + "scene_id": "scene-export-test", + "background": [ + { + "uid": "table", + "description": "A white table.", + "shape": {"shape_type": "Mesh", "fpath": "mesh_assets/table.glb"}, + "init_pos": [0.0, 0.0, 0.0], + "init_rot": [0.0, 0.0, 0.0], + "body_scale": [1.0, 1.0, 1.0], + } + ], + "rigid_object": [ + { + "uid": uid, + "name": f"Bottle {index}", + "description": f"Bottle instance {index}.", + "shape": { + "shape_type": "Mesh", + "fpath": f"mesh_assets/{uid}.glb", + }, + "init_pos": [float(index), float(index + 1), 0.7], + "init_rot": [0.0, 0.0, 0.0], + "body_scale": [1.0, 1.0, 1.0], + } + for index, uid in enumerate(("bottle_001", "bottle_002"), start=1) + ], + } + (export / "scene_config.json").write_text( + json.dumps(config), + encoding="utf-8", + ) + return export + + +def _existing_v2_task_spec(task_id: str = "direct_task") -> dict[str, object]: + return { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": task_id, + "level": "L1", + "instruction": "test-instruction", + "reasoning_type": "none", + "task_instances": [ + { + "id": "task_01", + "task_type": "E2", + "params": {"object_role": "object_01"}, + "depends_on": [], + "role": "primary", + } + ], + "success": {"type": "semantic_goal", "task_instance_id": "task_01"}, + "oracle": {}, + "metadata": {"role_bindings": {"object_01": "interact_can"}}, + } + + +def test_prepare_scene_normalizes_uid_paths_and_prompt2scene_transform( + gym_export: Path, +) -> None: + scene = prepare_scene(gym_export) + + assert scene.uid_map == { + "table_0": "table", + "interact_can_0": "interact_can", + } + assert scene.z_rotation_degrees == -90.0 + assert scene.rigid_objects[0]["init_pos"] == [2.0, -1.0, 0.7] + assert scene.rigid_objects[0]["max_convex_hull_num"] == 16 + assert scene.rigid_objects[0]["acd_method"] == "vhacd" + assert scene.rigid_objects[0]["shape"]["acd_method"] == "vhacd" + assert scene.rigid_objects[0]["shape"]["max_convex_hull_num"] == 16 + mesh_path = Path(scene.rigid_objects[0]["shape"]["fpath"]) + assert mesh_path.is_absolute() + assert mesh_path.is_file() + assert scene.planner_objects[1]["source_uid"] == "interact_can_0" + assert scene.planner_objects[1]["uid"] == "interact_can" + + +def test_prepare_scene_supports_scene_export_v1(scene_export: Path) -> None: + scene = prepare_scene(scene_export.parent) + + assert scene.source_config_path == scene_export / "scene_config.json" + assert scene.uid_map == { + "table": "table", + "bottle_001": "bottle_001", + "bottle_002": "bottle_002", + } + assert scene.planner_objects[1]["name"] == "Bottle 1" + assert scene.z_rotation_degrees == -90.0 + assert scene.rigid_objects[0]["init_pos"] == [2.0, -1.0, 0.7] + assert all( + Path(config["shape"]["fpath"]).is_file() + for config in (*scene.background, *scene.rigid_objects) + ) + + +def test_prepare_scene_requires_exactly_one_background(gym_export: Path) -> None: + source_path = gym_export / "gym_config.json" + source = json.loads(source_path.read_text(encoding="utf-8")) + source["background"].append( + { + **source["background"][0], + "uid": "floor_0", + "description": "A floor beneath the work surface.", + } + ) + source_path.write_text(json.dumps(source), encoding="utf-8") + + with pytest.raises(ValueError, match="exactly one background"): + prepare_scene(gym_export) + + +def test_prepare_scene_does_not_treat_physics_attrs_as_semantics( + gym_export: Path, +) -> None: + scene = prepare_scene(gym_export) + rigid_object = next( + item for item in scene.planner_objects if item["role"] == "rigid_object" + ) + + assert rigid_object["attributes"] == {} + + +@pytest.mark.parametrize( + "companion_relative_path", + ( + Path("gym_export/scene_config.json"), + Path("scene_export/scene_config.json"), + ), +) +def test_source_scene_resolution_prefers_gym_config_in_mixed_export( + tmp_path: Path, + companion_relative_path: Path, +) -> None: + gym_export = tmp_path / "gym_export" + gym_export.mkdir(parents=True) + gym_config = gym_export / "gym_config.json" + gym_config.write_text("{}", encoding="utf-8") + companion = tmp_path / companion_relative_path + companion.parent.mkdir(parents=True, exist_ok=True) + companion.write_text( + json.dumps({"format": "embodichain.scene-export/v1"}), encoding="utf-8" + ) + + resolved = resolve_source_scene(tmp_path) + + assert resolved.path == gym_config + assert resolved.source_format == "legacy_gym_config" + assert resolved.is_prompt2scene is True + assert resolve_gym_config_path(tmp_path) == resolved.path + + +def test_explicit_scene_export_config_overrides_mixed_layout( + gym_export: Path, + scene_export: Path, +) -> None: + resolved = resolve_source_scene(scene_export / "scene_config.json") + + assert resolved.path == scene_export / "scene_config.json" + assert resolved.source_format == "embodichain.scene-export/v1" + assert resolved.is_prompt2scene is True + + +def test_explicit_named_legacy_scene_config_is_supported(gym_export: Path) -> None: + config_path = gym_export / "official_task_config.json" + config_path.write_text( + (gym_export / "gym_config.json").read_text(encoding="utf-8"), + encoding="utf-8", + ) + + resolved = resolve_source_scene(config_path) + scene = prepare_scene(config_path) + + assert resolved.path == config_path + assert resolved.source_format == "legacy_gym_config" + assert resolved.is_prompt2scene is False + assert scene.source_config_path == config_path + + +def test_explicit_robot_scene_is_centered_on_its_table_anchor( + gym_export: Path, +) -> None: + source = json.loads((gym_export / "gym_config.json").read_text(encoding="utf-8")) + source["robot"] = {"uid": "source_robot"} + source["background"][0]["init_pos"] = [1.0, 2.0, 0.0] + source["rigid_object"][0]["init_pos"] = [1.2, 2.3, 0.7] + config_path = gym_export / "official_task_config.json" + config_path.write_text(json.dumps(source), encoding="utf-8") + + scene = prepare_scene(config_path) + table = scene.background[0] + moved = scene.rigid_objects[0] + + assert scene.source_scene_xy_translation == pytest.approx((-1.0, -2.0)) + assert table["init_pos"][:2] == pytest.approx([0.0, 0.0]) + assert moved["init_pos"][:2] == pytest.approx([0.2, 0.3]) + + +def test_scene_export_config_rejects_unknown_format(scene_export: Path) -> None: + config_path = scene_export / "scene_config.json" + config = json.loads(config_path.read_text(encoding="utf-8")) + config["format"] = "embodichain.scene-export/v2" + config_path.write_text(json.dumps(config), encoding="utf-8") + + with pytest.raises(ValueError, match="unsupported format"): + resolve_source_scene(config_path) + + +def test_fast_gym_config_has_runnable_franka_contract(gym_export: Path) -> None: + scene = prepare_scene(gym_export) + config = build_fast_gym_config( + scene, + task_name="line_task", + task_description="Arrange the can.", + robot_profile="franka", + execution_program_hash="a" * 64, + max_episodes=1, + max_episode_steps=2000, + randomize_scene=True, + ) + + assert config["id"] == "ActionEngine-v1" + assert config["robot"]["uid"] == "DualFrankaPanda" + assert config["robot"]["init_pos"][2] == pytest.approx(0.35) + assert config["sensor"][0]["uid"] == "cam_high" + assert config["env"]["extensions"]["agent_robot_profile"] == "dual_franka" + assert config["env"]["extensions"]["agent_static_obstacle_uids"] == ["table"] + assert config["env"]["extensions"]["agent_dynamic_obstacle_uids"] == [ + "interact_can" + ] + assert "agent_grasp_runtime_defaults" not in config["env"]["extensions"] + assert config["env"]["extensions"]["agent_arm_slots"] == { + "left": {"arm": "left_arm", "eef": "left_eef"}, + "right": {"arm": "right_arm", "eef": "right_eef"}, + } + assert config["env"]["extensions"]["arm_aim_yaw_offset"] == { + "left": pytest.approx(0.0), + "right": pytest.approx(0.0), + } + assert config["env"]["extensions"]["action_engine"]["seed_task_graph"] == ( + "seed_task_graph.json" + ) + assert ( + config["env"]["extensions"]["action_engine"]["defaults_schema_version"] + == "action_engine_defaults_v1" + ) + registry = config["env"]["events"]["register_info_to_env"]["params"]["registry"] + assert [entry["entity_cfg"]["uid"] for entry in registry] == ["interact_can"] + assert "randomize_interact_can_pose" in config["env"]["events"] + assert "randomize_table_height" in config["env"]["events"] + recorder = config["env"]["events"]["record_camera"] + assert recorder["interval_step"] == 5 + assert recorder["params"]["resolution"] == [640, 360] + assert recorder["params"]["intrinsics"] == pytest.approx( + [280.0, 280.0, 320.0, 180.0] + ) + object_length = config["env"]["events"]["prepare_extra_attr"]["params"]["attrs"][0] + assert object_length["entity_uids"] == ["interact_can"] + assert object_length["func_kwargs"]["sample_points"] == 5000 + assert ( + config["env"]["dataset"]["lerobot"]["params"]["robot_meta"]["control_freq"] + == 25 + ) + assert config["env"]["observations"]["norm_robot_eef_joint"]["params"][ + "joint_ids" + ] == [14, 16] + + +def test_fast_gym_config_normalizes_usdc_articulation_runtime_fields( + gym_export: Path, +) -> None: + usdc_path = gym_export / "microwave.usdc" + usdc_path.write_bytes(b"PXR-USDC") + source_path = gym_export / "gym_config.json" + source = json.loads(source_path.read_text(encoding="utf-8")) + source["articulation"] = [ + { + "uid": "microwave_001", + "category": "microwave", + "name": "silver microwave", + "description": "A countertop microwave.", + "is_articulated": True, + "fpath": usdc_path.name, + "proxy_glb_fpath": "mesh_assets/can.glb", + "proxy_body_scale": [1.0, 1.0, 1.0], + "init_pos": [0.2, 0.0, 0.7], + "init_rot": [0.0, 0.0, 0.0], + "body_scale": [1.0, 1.0, 1.0], + "fix_base": True, + } + ] + source_path.write_text(json.dumps(source), encoding="utf-8") + + config = build_fast_gym_config( + prepare_scene(gym_export), + task_name="microwave_reference", + task_description="Place the can beside the microwave.", + robot_profile="franka", + execution_program_hash="a" * 64, + max_episodes=1, + max_episode_steps=2000, + ) + + articulation = config["articulation"][0] + assert articulation["fpath"] == usdc_path.resolve().as_posix() + assert articulation["build_pk_chain"] is False + assert ( + not { + "category", + "name", + "is_articulated", + "proxy_glb_fpath", + "proxy_body_scale", + } + & articulation.keys() + ) + + +def test_fast_gym_config_rejects_usdc_with_pk_chain(gym_export: Path) -> None: + config = build_fast_gym_config( + prepare_scene(gym_export), + task_name="invalid_usdc", + task_description="Reject an invalid runtime articulation.", + robot_profile="franka", + execution_program_hash="a" * 64, + max_episodes=1, + max_episode_steps=2000, + ) + usdc_path = gym_export / "invalid.usdc" + usdc_path.write_bytes(b"PXR-USDC") + config["articulation"] = [ + { + "uid": "microwave_001", + "fpath": usdc_path.resolve().as_posix(), + "build_pk_chain": True, + } + ] + + with pytest.raises(ValueError, match="must set build_pk_chain=false"): + validate_fast_gym_config(config) + + +def test_offline_recording_can_be_disabled_but_ab_keeps_audience_recorder( + gym_export: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + defaults = dict(config_builder_module._GENERATION_DEFAULTS) + environment = dict(defaults["environment"]) + recording = dict(environment["recording"]) + recording["enabled"] = False + environment["recording"] = recording + defaults["environment"] = environment + monkeypatch.setattr(config_builder_module, "_GENERATION_DEFAULTS", defaults) + scene = prepare_scene(gym_export) + + offline = build_fast_gym_config( + scene, + task_name="offline_task", + task_description="Offline recording policy.", + robot_profile="franka", + execution_program_hash="a" * 64, + max_episodes=1, + max_episode_steps=100, + ) + ab = build_fast_gym_config( + scene, + task_name="ab_task", + task_description="A/B recording policy.", + robot_profile="franka", + execution_program_hash="b" * 64, + max_episodes=1, + max_episode_steps=100, + planning_mode="ab", + seed_task_graph_path="offline/seed_task_graph.json", + ) + + assert "record_camera" not in offline["env"]["events"] + assert ab["env"]["events"]["record_camera"]["params"]["name"] == ( + "record_cam_audience_view" + ) + assert ab["env"]["events"]["record_camera"]["interval_step"] == 5 + + +@pytest.mark.parametrize( + ("override", "message"), + [ + ({"enabled": "yes"}, "enabled must be a boolean"), + ({"resolution": [640]}, "resolution must contain two positive integers"), + ({"interval_step": 0}, "interval_step must be positive"), + ], +) +def test_recording_policy_rejects_invalid_generation_defaults( + gym_export: Path, + monkeypatch: pytest.MonkeyPatch, + override: dict[str, object], + message: str, +) -> None: + defaults = dict(config_builder_module._GENERATION_DEFAULTS) + environment = dict(defaults["environment"]) + recording = dict(environment["recording"]) + recording.update(override) + environment["recording"] = recording + defaults["environment"] = environment + monkeypatch.setattr(config_builder_module, "_GENERATION_DEFAULTS", defaults) + + with pytest.raises(ValueError, match=message): + build_fast_gym_config( + prepare_scene(gym_export), + task_name="invalid_recording", + task_description="Invalid recording policy.", + robot_profile="franka", + execution_program_hash="c" * 64, + max_episodes=1, + max_episode_steps=100, + ) + + +def test_fast_gym_config_preserves_unicode_instruction_and_uses_task_name_label( + gym_export: Path, +) -> None: + scene = prepare_scene(gym_export) + task_name = "task1000" + task_description = "unicode-λ-instruction" + + config = build_fast_gym_config( + scene, + task_name=task_name, + task_description=task_description, + robot_profile="franka", + execution_program_hash="a" * 64, + max_episodes=1, + max_episode_steps=2000, + ) + + params = config["env"]["dataset"]["lerobot"]["params"] + assert params["instruction"]["lang"] == task_description + assert params["extra"]["task_name"] == task_name + assert params["extra"]["task_description"] == task_name + + +@pytest.mark.parametrize( + ("name", "planner_policy", "expected"), + [ + ( + "curobo", + {"mode": "curobo"}, + ("curobo", "motion_gen", "ik_interp", True), + ), + ( + "toppra", + {"mode": "toppra"}, + ("toppra", "motion_gen", "ik_interp", False), + ), + ( + "ik_interp", + {"mode": "ik_interp"}, + ("toppra", "ik_interp", "ik_interp", False), + ), + ], +) +def test_agent_config_materializes_yaml_planner_policy_and_hash( + tmp_path: Path, + name: str, + planner_policy: dict[str, object], + expected: tuple[str, str, str, bool], +) -> None: + config = build_agent_config( + task_name=name, + robot_profile="ur10", + execution_program_hash="d" * 64, + source_config_path=tmp_path / "gym_config.json", + uid_map={"table": "table", "cube": "cube"}, + static_obstacle_uids=["table"], + dynamic_obstacle_uids=["cube"], + planner_policy=planner_policy, + ) + + planner = config["runtime_policy"]["planner"] + assert ( + planner["backend"], + planner["single_arm_strategy"], + planner["coordinated_strategy"], + planner["dynamic_collision"], + ) == expected + assert planner["static_obstacle_uids"] == ["table"] + assert planner["dynamic_obstacle_uids"] == ["cube"] + assert len(config["runtime_policy_hash"]) == 64 + + from embodichain.gen_sim.action_engine.config import resolve_agent_runtime_policy + + resolved = resolve_agent_runtime_policy(config) + assert resolved.planner == planner + + +def test_agent_config_rejects_toppra_dynamic_collision_before_writing( + tmp_path: Path, +) -> None: + with pytest.raises(ValueError, match="dynamic_collision.*cuRobo"): + build_agent_config( + task_name="invalid_toppra", + robot_profile="ur10", + execution_program_hash="d" * 64, + source_config_path=tmp_path / "gym_config.json", + uid_map={"cube": "cube"}, + dynamic_obstacle_uids=["cube"], + planner_policy={"backend": "toppra", "dynamic_collision": True}, + ) + + +def test_planner_yaml_loader_accepts_wrapped_policy_and_rejects_backend_leaks( + tmp_path: Path, +) -> None: + config_path = tmp_path / "toppra.yaml" + config_path.write_text( + """\ +planner: + mode: toppra +""", + encoding="utf-8", + ) + + assert _load_planner_config(str(config_path)) == { + "mode": "toppra", + } + + config_path.write_text( + """\ +planner: + mode: toppra + curobo: + max_attempts: 2 +""", + encoding="utf-8", + ) + with pytest.raises(ValueError, match="curobo.*cuRobo backend"): + _load_planner_config(str(config_path)) + + +def test_ab_config_uses_offline_branch_and_four_vlm_cameras( + gym_export: Path, + tmp_path: Path, +) -> None: + scene = prepare_scene(gym_export) + graph_path = "offline/seed_task_graph.json" + config = build_fast_gym_config( + scene, + task_name="ab_task", + task_description="test-instruction", + robot_profile="ur10", + execution_program_hash="d" * 64, + max_episodes=1, + max_episode_steps=100, + planning_mode="ab", + seed_task_graph_path=graph_path, + ) + agent = build_agent_config( + task_name="ab_task", + robot_profile="ur10", + execution_program_hash="d" * 64, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + planning_mode="ab", + seed_task_graph_path=graph_path, + vlm_model="mimo-vlm", + vlm_camera_uids=[ + "vlm_front", + "vlm_left", + "vlm_rear", + "vlm_right", + ], + ) + paths = artifact_paths(tmp_path, planning_mode="ab") + + assert paths.seed_task_graph == tmp_path.resolve() / graph_path + assert config["env"]["extensions"]["action_engine"]["planning_mode"] == "ab" + assert config["env"]["extensions"]["action_engine"]["seed_task_graph"] == ( + graph_path + ) + vlm_sensors = [ + sensor for sensor in config["sensor"] if sensor["uid"].startswith("vlm_") + ] + assert [sensor["uid"] for sensor in vlm_sensors] == [ + "vlm_front", + "vlm_left", + "vlm_rear", + "vlm_right", + ] + assert all( + sensor["enable_color"] and sensor["enable_depth"] for sensor in vlm_sensors + ) + assert agent["planning_mode"] == "ab" + assert agent["offline_seed_task_graph"] == graph_path + assert agent["vlm_model"] == "mimo-vlm" + assert agent["vlm_camera_uids"] == [ + "vlm_front", + "vlm_left", + "vlm_rear", + "vlm_right", + ] + assert agent["online_planning"] == { + "vlm_model": "mimo-vlm", + "camera_uids": ["vlm_front", "vlm_left", "vlm_rear", "vlm_right"], + } + + +def test_ab_builders_default_to_the_offline_graph_path(gym_export: Path) -> None: + scene = prepare_scene(gym_export) + config = build_fast_gym_config( + scene, + task_name="ab_default_path", + task_description="A/B path smoke test.", + robot_profile="ur10", + execution_program_hash="e" * 64, + max_episodes=1, + max_episode_steps=10, + planning_mode="ab", + ) + agent = build_agent_config( + task_name="ab_default_path", + robot_profile="ur10", + execution_program_hash="e" * 64, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + planning_mode="ab", + ) + + expected = "offline/seed_task_graph.json" + assert config["env"]["extensions"]["action_engine"]["seed_task_graph"] == expected + assert agent["seed_task_graph"] == expected + assert agent["online_planning"]["camera_uids"] == [ + "vlm_front", + "vlm_left", + "vlm_rear", + "vlm_right", + ] + + +def test_agent_config_owns_articulation_setting_calibration( + gym_export: Path, +) -> None: + scene = prepare_scene(gym_export) + settings = {"microwave": {"timer_joint": [-1.0, 0.0, 1.0]}} + + agent = build_agent_config( + task_name="turn_knob", + robot_profile="franka", + execution_program_hash="f" * 64, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + articulation_settings=settings, + ) + settings["microwave"]["timer_joint"][0] = 99.0 + + assert agent["articulation_settings"] == { + "microwave": {"timer_joint": [-1.0, 0.0, 1.0]} + } + + +def test_ab_scene_requirements_declare_four_vlm_views() -> None: + requirements = { + "schema_version": "action_engine_scene_requirements_v2", + "task_id": "ab", + "objects": [ + { + "role_id": "object_01", + "category": "can", + "count": 1, + "affordances": ["graspable"], + "initial_state": {}, + "attributes": {}, + } + ], + "cameras": [], + "spatial_constraints": [], + "distractor_count": 0, + "metadata": {}, + } + output = _add_ab_camera_requirements(requirements) + assert [item["uid"] for item in output["cameras"]] == [ + "vlm_front", + "vlm_left", + "vlm_rear", + "vlm_right", + ] + assert all(item["modalities"] == ["rgb", "depth"] for item in output["cameras"]) + + +def test_ab_builder_rejects_noncanonical_vlm_camera_ids(gym_export: Path) -> None: + scene = prepare_scene(gym_export) + with pytest.raises(ValueError, match="canonical"): + build_agent_config( + task_name="ab_invalid_cameras", + robot_profile="ur10", + execution_program_hash="f" * 64, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + planning_mode="ab", + vlm_camera_uids=["front", "left", "rear", "right"], + ) + + +@pytest.mark.parametrize( + ("profile", "robot_uid", "solver_type"), + [ + ("dual_ur3", "DualUR3", "ur3"), + ("dual_ur5", "DualUR5", "ur5"), + ("dual_ur10", "DualUR10", "ur10"), + ("dual_franka", "DualFrankaPanda", None), + ], +) +def test_fast_gym_config_supports_all_robot_profiles( + gym_export: Path, + profile: str, + robot_uid: str, + solver_type: str | None, +) -> None: + pgi = get_gripper_profile("pgi") + identity_hand_mount = [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + scene = prepare_scene(gym_export) + config = build_fast_gym_config( + scene, + task_name="profile_task", + task_description="Profile smoke test.", + robot_profile=profile, + execution_program_hash="b" * 64, + max_episodes=1, + max_episode_steps=20, + ) + + assert config["robot"]["uid"] == robot_uid + assert config["env"]["extensions"]["agent_robot_profile"] == profile + assert config["env"]["extensions"]["agent_gripper_model"] == "pgi" + for arm in ("left_arm", "right_arm"): + assert config["robot"]["solver_cfg"][arm]["tcp"] == [ + list(row) for row in pgi.tcp_transform + ] + components = { + component["component_type"]: component + for component in config["robot"]["urdf_cfg"]["components"] + } + for hand in ("left_hand", "right_hand"): + assert components[hand]["transform"] == identity_hand_mount + if solver_type is not None: + assert config["robot"]["solver_cfg"]["left_arm"]["ur_type"] == solver_type + + +@pytest.mark.parametrize("gripper_model", ["pgi", "robotiq"]) +def test_fast_gym_config_applies_one_complete_gripper_profile( + gym_export: Path, + gripper_model: str, +) -> None: + scene = prepare_scene(gym_export) + config = build_fast_gym_config( + scene, + task_name="gripper_profile_task", + task_description="Profile smoke test.", + robot_profile="ur10", + gripper_model=gripper_model, + execution_program_hash="a" * 64, + max_episodes=1, + max_episode_steps=20, + ) + profile = get_gripper_profile(gripper_model) + robot = config["robot"] + extensions = config["env"]["extensions"] + components = { + component["component_type"]: component + for component in robot["urdf_cfg"]["components"] + } + + assert extensions["agent_gripper_model"] == gripper_model + assert extensions["gripper_open_state"] == list(profile.open_positions) + assert extensions["gripper_close_state"] == list(profile.close_positions) + assert extensions["gripper_profile"]["model"] == gripper_model + assert robot["control_parts"]["left_eef"] == list( + profile.control_joint_names("left") + ) + assert robot["control_parts"]["right_eef"] == list( + profile.control_joint_names("right") + ) + assert components["left_hand"]["urdf_path"] == profile.asset_path + assert components["right_hand"]["urdf_path"] == profile.asset_path + assert robot["solver_cfg"]["left_arm"]["tcp"] == [ + list(row) for row in profile.tcp_transform + ] + assert robot["solver_cfg"]["right_arm"]["tcp"] == [ + list(row) for row in profile.tcp_transform + ] + assert robot["urdf_cfg"]["fname"].endswith(f"{profile.assembly_name}_basket") + + +def test_config_builders_reject_unknown_gripper_before_materialization( + gym_export: Path, +) -> None: + scene = prepare_scene(gym_export) + + with pytest.raises(ValueError, match="pgi.*robotiq"): + build_fast_gym_config( + scene, + task_name="invalid_gripper", + task_description="Invalid profile.", + robot_profile="ur10", + gripper_model="parallel_jaw", + execution_program_hash="a" * 64, + max_episodes=1, + max_episode_steps=20, + ) + + +@pytest.mark.parametrize( + ("ik_solver", "class_type"), + (("ur", "URSolver"), ("pytorch", "PytorchSolver")), +) +def test_fast_gym_config_materializes_selected_ur10_ik_solver( + gym_export: Path, + ik_solver: str, + class_type: str, +) -> None: + scene = prepare_scene(gym_export) + config = build_fast_gym_config( + scene, + task_name="solver_profile_task", + task_description="Exercise one generated IK solver.", + robot_profile="ur10", + gripper_model="robotiq", + ik_solver=ik_solver, + execution_program_hash="d" * 64, + max_episodes=1, + max_episode_steps=20, + ) + robot = config["robot"] + extension = config["env"]["extensions"] + tcp = [list(row) for row in get_gripper_profile("robotiq").tcp_transform] + + assert extension["action_engine"]["ik_solver"] == ik_solver + assert extension["agent_ik_solver"] == ik_solver + for arm in ("left_arm", "right_arm"): + solver = robot["solver_cfg"][arm] + assert solver["class_type"] == class_type + assert solver["tcp"] == tcp + assert solver["end_link_name"] == f"{arm.split('_')[0]}_ee_link" + assert solver["root_link_name"] == f"{arm.split('_')[0]}_base_link" + if ik_solver == "pytorch": + assert solver["num_samples"] == 30 + + +def test_fast_gym_config_rejects_analytic_ur_solver_for_franka( + gym_export: Path, +) -> None: + scene = prepare_scene(gym_export) + + with pytest.raises(ValueError, match="Franka.*URSolver"): + build_fast_gym_config( + scene, + task_name="invalid_solver", + task_description="Reject incompatible IK.", + robot_profile="franka", + ik_solver="ur", + execution_program_hash="e" * 64, + max_episodes=1, + max_episode_steps=20, + ) + + +def test_agent_config_serializes_concrete_ik_solver(gym_export: Path) -> None: + scene = prepare_scene(gym_export) + + ur = build_agent_config( + task_name="ur_solver_task", + robot_profile="ur10", + ik_solver="auto", + execution_program_hash="f" * 64, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + ) + pytorch = build_agent_config( + task_name="pytorch_solver_task", + robot_profile="ur10", + ik_solver="pytorch", + execution_program_hash="1" * 64, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + ) + + assert ur["ik_solver"] == "ur" + assert pytorch["ik_solver"] == "pytorch" + + +def test_agent_config_serializes_selected_gripper(gym_export: Path) -> None: + scene = prepare_scene(gym_export) + config = build_agent_config( + task_name="gripper_profile_task", + robot_profile="ur10", + gripper_model="robotiq", + execution_program_hash="a" * 64, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + ) + + assert config["gripper_model"] == "robotiq" + + +@pytest.mark.parametrize( + ( + "profile", + "expected_position_xy", + "expected_rotation", + "expected_world_x", + ), + [ + ("ur10", [2.0, 0.0], [0.0, 0.0, 0.0], 0.9), + ("franka", [-0.7, 0.0], [0.0, 0.0, 180.0], 0.55), + ], +) +def test_dual_robot_profiles_use_identity_mounts_and_same_side_arm_names( + gym_export: Path, + profile: str, + expected_position_xy: list[float], + expected_rotation: list[float], + expected_world_x: float, +) -> None: + scene = prepare_scene(gym_export) + config = build_fast_gym_config( + scene, + task_name="dual_ur_frame_task", + task_description="Verify the Dual-UR world frame.", + robot_profile=profile, + execution_program_hash="c" * 64, + max_episodes=1, + max_episode_steps=20, + ) + + robot = config["robot"] + robot_yaw = np.deg2rad(float(robot["init_rot"][2])) + robot_rotation = np.array( + [ + [np.cos(robot_yaw), -np.sin(robot_yaw), 0.0], + [np.sin(robot_yaw), np.cos(robot_yaw), 0.0], + [0.0, 0.0, 1.0], + ] + ) + robot_position = np.asarray(robot["init_pos"], dtype=np.float64) + components = { + component["component_type"]: np.asarray( + component["transform"], dtype=np.float64 + ) + for component in robot["urdf_cfg"]["components"] + if component["component_type"] in {"left_arm", "right_arm"} + } + world_transforms = {} + for side, component in components.items(): + world = np.eye(4) + world[:3, :3] = robot_rotation @ component[:3, :3] + world[:3, 3] = robot_position + robot_rotation @ component[:3, 3] + world_transforms[side] = world + + assert robot["init_pos"][:2] == pytest.approx(expected_position_xy) + assert robot["init_rot"] == pytest.approx(expected_rotation) + assert world_transforms["left_arm"][:3, 3] == pytest.approx( + [expected_world_x, -0.3, world_transforms["left_arm"][2, 3]] + ) + assert world_transforms["right_arm"][:3, 3] == pytest.approx( + [expected_world_x, 0.3, world_transforms["right_arm"][2, 3]] + ) + np.testing.assert_allclose(components["left_arm"][:3, :3], np.eye(3), atol=1.0e-12) + np.testing.assert_allclose(components["right_arm"][:3, :3], np.eye(3), atol=1.0e-12) + np.testing.assert_allclose( + world_transforms["left_arm"][:3, :3], robot_rotation, atol=1.0e-12 + ) + np.testing.assert_allclose( + world_transforms["right_arm"][:3, :3], robot_rotation, atol=1.0e-12 + ) + + +def test_fast_gym_config_keeps_scene_deterministic_by_default( + gym_export: Path, +) -> None: + scene = prepare_scene(gym_export) + config = build_fast_gym_config( + scene, + task_name="deterministic_task", + task_description="Keep the source scene fixed.", + robot_profile="ur10", + execution_program_hash="d" * 64, + max_episodes=1, + max_episode_steps=20, + ) + + events = config["env"]["events"] + assert "randomize_interact_can_pose" not in events + assert "randomize_table_height" not in events + + +@pytest.mark.parametrize( + ("alias", "canonical"), + [ + ("franka", "dual_franka"), + ("ur5", "dual_ur5"), + ("ur10", "dual_ur10"), + ], +) +def test_required_cli_robot_aliases_build_runnable_profiles( + gym_export: Path, + alias: str, + canonical: str, +) -> None: + scene = prepare_scene(gym_export) + config = build_fast_gym_config( + scene, + task_name="profile_alias_task", + task_description="Profile alias smoke test.", + robot_profile=alias, + execution_program_hash="c" * 64, + max_episodes=1, + max_episode_steps=20, + ) + + assert config["env"]["extensions"]["agent_robot_profile"] == canonical + + +def test_source_scene_scale_policies_are_deterministic(gym_export: Path) -> None: + preserved = prepare_scene(gym_export) + multiplied = prepare_scene( + gym_export, + body_scale_policy="multiply", + body_scale=(2.0, 3.0, 4.0), + ) + absolute = prepare_scene( + gym_export, + body_scale_policy="absolute", + body_scale=(2.0, 3.0, 4.0), + ) + + assert preserved.body_scale_policy == "preserve" + assert multiplied.rigid_objects[0]["body_scale"] == [2.0, 3.0, 4.0] + assert absolute.rigid_objects[0]["body_scale"] == [2.0, 3.0, 4.0] + assert multiplied.asset_hashes == absolute.asset_hashes + + +def test_artifact_writer_refuses_implicit_overwrite(tmp_path: Path) -> None: + payload = {"value": 1} + paths = write_generation_artifacts( + tmp_path, + gym_config=payload, + agent_config=payload, + task_spec=payload, + scene_requirements=payload, + seed_task_graph=payload, + seed_task_graph_png=b"\x89PNG\r\n\x1a\nold", + overwrite=False, + ) + assert json.loads(paths.gym_config.read_text(encoding="utf-8")) == payload + assert paths.seed_task_graph_png.read_bytes().startswith(b"\x89PNG") + + # A leftover PNG participates in the same preflight as every JSON artifact. + for path in ( + paths.gym_config, + paths.agent_config, + paths.task_spec, + paths.scene_requirements, + paths.execution_program, + ): + path.unlink() + with pytest.raises(FileExistsError, match="--overwrite"): + write_generation_artifacts( + tmp_path, + gym_config=payload, + agent_config=payload, + task_spec=payload, + scene_requirements=payload, + seed_task_graph=payload, + seed_task_graph_png=b"\x89PNG\r\n\x1a\nnew", + overwrite=False, + ) + + replaced = write_generation_artifacts( + tmp_path, + gym_config=payload, + agent_config=payload, + task_spec=payload, + scene_requirements=payload, + seed_task_graph=payload, + seed_task_graph_png=b"\x89PNG\r\n\x1a\nnew", + overwrite=True, + ) + assert replaced.seed_task_graph_png.read_bytes().endswith(b"new") + + +def test_artifact_writer_creates_ab_branch_directory(tmp_path: Path) -> None: + payload = {"value": "ab"} + paths = write_generation_artifacts( + tmp_path, + gym_config=payload, + agent_config=payload, + task_spec=payload, + scene_requirements=payload, + seed_task_graph=payload, + seed_task_graph_png=b"\x89PNG\r\n\x1a\nab", + overwrite=False, + planning_mode="ab", + ) + + assert paths.seed_task_graph.parent == tmp_path / "offline" + assert json.loads(paths.seed_task_graph.read_text(encoding="utf-8")) == payload + + +def test_generation_calls_interpreter_recipe_and_renderer_once( + gym_export: Path, + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + from embodichain.gen_sim.action_engine import tasks + from embodichain.gen_sim.action_engine.generation import generator + + planner_call: dict[str, object] = {} + recipe_calls: list[tuple[object, object]] = [] + rendered: dict[str, object] = {} + published: dict[str, object] = {} + + def fake_interpret_and_ground(**kwargs): + planner_call.update(kwargs) + task_spec = _existing_v2_task_spec(str(kwargs["task_name"])) + task_spec["instruction"] = str(kwargs["task_description"]) + bindings = {"object_01": "interact_can"} + requirements = _scene_requirements_from_bindings( + str(kwargs["task_name"]), + kwargs["scene_objects"], + bindings, + ) + return GroundedTaskSpec(task_spec, requirements, bindings) + + monkeypatch.setattr( + tasks, + "interpret_and_ground_task_spec", + fake_interpret_and_ground, + ) + real_recipe = tasks.instantiate_seed_graph + + def capture_recipe(task_spec, role_bindings): + recipe_calls.append((task_spec, role_bindings)) + return real_recipe(task_spec, role_bindings) + + monkeypatch.setattr(tasks, "instantiate_seed_graph", capture_recipe) + renderer_module = ModuleType( + "embodichain.gen_sim.action_engine.graph_visualization" + ) + + def fake_renderer(program): + rendered["program"] = program + return b"\x89PNG\r\n\x1a\nseed" + + renderer_module.render_seed_task_graph_png = fake_renderer + monkeypatch.setitem(sys.modules, renderer_module.__name__, renderer_module) + real_writer = generator.write_generation_artifacts + + def capture_writer(*args, **kwargs): + published["program"] = kwargs["seed_task_graph"] + return real_writer(*args, **kwargs) + + monkeypatch.setattr(generator, "write_generation_artifacts", capture_writer) + output_dir = tmp_path / "configs" + paths = generate_action_engine_config( + gym_export, + output_dir, + task_name="line_task", + task_description="test-instruction", + robot_profile="franka", + ) + + assert planner_call["task_name"] == "line_task" + assert planner_call["task_description"] == "test-instruction" + assert planner_call["robot_profile"] == "franka" + assert len(recipe_calls) == 1 + planner_objects = planner_call["scene_objects"] + assert isinstance(planner_objects, list) + assert {obj["uid"] for obj in planner_objects} == {"table", "interact_can"} + assert {path.name for path in output_dir.iterdir()} == { + "fast_gym_config.json", + "agent_config.json", + "task_spec.json", + "scene_requirements.json", + "seed_task_graph.json", + "seed_task_graph.png", + } + assert paths.seed_task_graph_png.read_bytes() == b"\x89PNG\r\n\x1a\nseed" + assert rendered["program"] is published["program"] + + agent_config = json.loads(paths.agent_config.read_text(encoding="utf-8")) + assert agent_config["schema_version"] == "action_engine_config_v2" + assert agent_config["task_spec"] == "task_spec.json" + assert agent_config["scene_requirements"] == "scene_requirements.json" + assert agent_config["seed_task_graph"] == "seed_task_graph.json" + assert len(agent_config["seed_task_graph_hash"]) == 64 + assert agent_config["runtime_policy"]["schema_version"] == ( + "action_engine_runtime_policy_v8" + ) + assert agent_config["runtime_policy"]["planner"]["dynamic_collision"] is True + assert agent_config["runtime_policy"]["planner"]["static_obstacle_uids"] == [ + "table" + ] + assert agent_config["runtime_policy"]["planner"]["dynamic_obstacle_uids"] == [ + "interact_can" + ] + assert len(agent_config["runtime_policy_hash"]) == 64 + assert "png" not in json.dumps(agent_config).lower() + + from embodichain.gen_sim.action_engine.runtime import ( + load_agent_execution_program, + ) + + regenerated = load_agent_execution_program( + agent_config, + agent_config_path=paths.agent_config, + regenerate=True, + ) + assert regenerated.task == "line_task" + assert regenerated.seed_graph is not None + + +def test_existing_v2_task_spec_bypasses_text_planner_and_derives_scene_requirements( + gym_export: Path, + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + from embodichain.gen_sim.action_engine import tasks + + renderer_module = ModuleType( + "embodichain.gen_sim.action_engine.graph_visualization" + ) + renderer_module.render_seed_task_graph_png = lambda _program: b"direct-task-png" + monkeypatch.setitem(sys.modules, renderer_module.__name__, renderer_module) + + def unexpected_text_planner(**_kwargs): + raise AssertionError("an existing TaskSpec must not invoke text planning") + + monkeypatch.setattr( + tasks, "interpret_and_ground_task_spec", unexpected_text_planner + ) + input_path = tmp_path / "task_spec.json" + input_path.write_text( + json.dumps(_existing_v2_task_spec()), + encoding="utf-8", + ) + + paths = generate_action_engine_config( + gym_export, + tmp_path / "generated", + task_name="direct_task", + task_spec=input_path, + robot_profile="ur10", + ) + + persisted_task = json.loads(paths.task_spec.read_text(encoding="utf-8")) + persisted_requirements = json.loads( + paths.scene_requirements.read_text(encoding="utf-8") + ) + assert persisted_task["metadata"]["role_bindings"] == {"object_01": "interact_can"} + assert [item["role_id"] for item in persisted_requirements["objects"]] == [ + "object_01" + ] + assert persisted_requirements["metadata"]["source"] == ("task_spec_role_bindings") + gym_config = json.loads(paths.gym_config.read_text(encoding="utf-8")) + assert ( + gym_config["env"]["dataset"]["lerobot"]["params"]["instruction"]["lang"] + == "test-instruction" + ) + + +def test_existing_v2_task_spec_uses_validated_scene_requirements_sidecar( + gym_export: Path, + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + renderer_module = ModuleType( + "embodichain.gen_sim.action_engine.graph_visualization" + ) + renderer_module.render_seed_task_graph_png = lambda _program: b"sidecar-png" + monkeypatch.setitem(sys.modules, renderer_module.__name__, renderer_module) + + input_dir = tmp_path / "task-first" + input_dir.mkdir() + task = _existing_v2_task_spec("sidecar_task") + requirements = { + "schema_version": SCENE_REQUIREMENTS_SCHEMA, + "task_id": "sidecar_task", + "objects": [ + { + "role_id": "object_01", + "category": "can", + "count": 1, + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {"color": "red"}, + } + ], + "cameras": [], + "spatial_constraints": [], + "distractor_count": 0, + "metadata": {"task_first": True}, + } + (input_dir / "task_spec.json").write_text( + json.dumps(task), + encoding="utf-8", + ) + (input_dir / "scene_requirements.json").write_text( + json.dumps(requirements), + encoding="utf-8", + ) + + paths = generate_action_engine_config( + gym_export, + tmp_path / "generated-sidecar", + task_name="sidecar_task", + task_spec=input_dir / "task_spec.json", + robot_profile="ur10", + ) + + assert json.loads(paths.scene_requirements.read_text(encoding="utf-8")) == ( + requirements + ) + + +def test_task_factory_style_sidecar_binds_roles_without_text_llm( + gym_export: Path, + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + renderer_module = ModuleType( + "embodichain.gen_sim.action_engine.graph_visualization" + ) + renderer_module.render_seed_task_graph_png = lambda _program: b"task-first-png" + monkeypatch.setitem(sys.modules, renderer_module.__name__, renderer_module) + source_path = gym_export / "gym_config.json" + source = json.loads(source_path.read_text(encoding="utf-8")) + source["rigid_object"][0]["category"] = "can" + source["rigid_object"][0]["attributes"] = {"color": "red"} + source["rigid_object"][0]["affordances"] = ["graspable", "orientable"] + source["rigid_object"][0]["initial_state"] = {"orientation": "fallen"} + source_path.write_text(json.dumps(source), encoding="utf-8") + + input_dir = tmp_path / "task-first-unbound" + input_dir.mkdir() + task = _existing_v2_task_spec("task_first_unbound") + task["metadata"] = {"fixture": "abstract-task"} + requirements = { + "schema_version": SCENE_REQUIREMENTS_SCHEMA, + "task_id": "task_first_unbound", + "objects": [ + { + "role_id": "object_01", + "category": "can", + "count": 1, + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {"color": "red"}, + } + ], + "cameras": [], + "spatial_constraints": [], + "distractor_count": 0, + "metadata": {"task_first": True}, + } + (input_dir / "task_spec.json").write_text(json.dumps(task), encoding="utf-8") + (input_dir / "scene_requirements.json").write_text( + json.dumps(requirements), encoding="utf-8" + ) + + paths = generate_action_engine_config( + gym_export, + tmp_path / "generated-unbound-sidecar", + task_name="task_first_unbound", + task_spec=input_dir / "task_spec.json", + robot_profile="ur10", + ) + + task_artifact = json.loads(paths.task_spec.read_text(encoding="utf-8")) + assert task_artifact["metadata"]["role_bindings"] == {"object_01": "interact_can"} + + +def test_task_spec_input_rejects_natural_language_conflict( + gym_export: Path, + tmp_path: Path, +) -> None: + task = _existing_v2_task_spec() + with pytest.raises(ValueError, match="task_spec cannot be combined"): + generate_action_engine_config( + gym_export, + tmp_path / "conflict-description", + task_name="direct_task", + task_description="do something", + task_spec=task, + robot_profile="ur10", + ) + + +def test_task_spec_role_binding_accepts_legacy_oracle_and_rejects_conflicts() -> None: + task = _existing_v2_task_spec() + task["metadata"] = {} + task["oracle"] = {"role_bindings": {"object_01": "interact_can"}} + assert _task_spec_role_bindings(task, ["table", "interact_can"]) == { + "object_01": "interact_can" + } + + task["metadata"] = {"role_bindings": {"object_01": "table"}} + with pytest.raises(ValueError, match="Conflicting role_bindings"): + _task_spec_role_bindings(task, ["table", "interact_can"]) + + +def test_task_spec_role_binding_merges_non_overlapping_handoffs() -> None: + task = _existing_v2_task_spec() + task["task_instances"][0]["params"]["target_role"] = "object_02" + task["metadata"] = {"role_bindings": {"object_01": "interact_can"}} + task["oracle"] = {"role_bindings": {"object_02": "interact_target"}} + + assert _task_spec_role_bindings( + task, + ["table", "interact_can", "interact_target"], + ) == {"object_01": "interact_can", "object_02": "interact_target"} + + +def test_task_factory_sidecar_requires_static_affordance_and_state_evidence() -> None: + task = _existing_v2_task_spec("missing-static-evidence") + task["metadata"] = {} + requirements = { + "objects": [ + { + "role_id": "object_01", + "category": "can", + "count": 1, + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {"color": "red"}, + } + ] + } + scene = [ + { + "runtime_uid": "interact_can", + "description": "A red soda can.", + "init_pos": [0.0, 0.0, 0.7], + } + ] + + with pytest.raises(ValueError, match="requires one unambiguous scene match"): + _task_spec_role_bindings( + task, + ["interact_can"], + scene_requirements=requirements, + scene_objects=scene, + robot_profile="ur10", + ) + + +@pytest.mark.parametrize( + ("scene_metadata", "required_attributes"), + ( + ({}, {}), + ({"category": "can"}, {"color": "red"}), + ), +) +def test_task_factory_sidecar_does_not_infer_semantics_from_description( + scene_metadata: dict, + required_attributes: dict, +) -> None: + task = _existing_v2_task_spec("no-text-evidence") + task["metadata"] = {} + requirements = { + "objects": [ + { + "role_id": "object_01", + "category": "can", + "count": 1, + "affordances": [], + "initial_state": {}, + "attributes": required_attributes, + } + ] + } + scene = [ + { + "runtime_uid": "mystery_object", + "role": "rigid_object", + "description": "A red soda can.", + "init_pos": [0.0, 0.0, 0.7], + **scene_metadata, + } + ] + + with pytest.raises(ValueError, match="requires one unambiguous scene match"): + _task_spec_role_bindings( + task, + ["mystery_object"], + scene_requirements=requirements, + scene_objects=scene, + robot_profile="ur10", + ) + + +def test_ab_generation_writes_shared_and_offline_branch_artifacts( + gym_export: Path, + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + renderer_module = ModuleType( + "embodichain.gen_sim.action_engine.graph_visualization" + ) + renderer_module.render_seed_task_graph_png = lambda _program: b"ab-seed-png" + monkeypatch.setitem(sys.modules, renderer_module.__name__, renderer_module) + + output_dir = tmp_path / "ab-config" + paths = generate_action_engine_config( + gym_export, + output_dir, + task_name="ab_task", + task_spec=_existing_v2_task_spec("ab_task"), + robot_profile="ur10", + planning_mode="ab", + vlm_model="mimo-vlm", + ) + + assert paths.seed_task_graph == output_dir / "offline/seed_task_graph.json" + assert paths.seed_task_graph_png == output_dir / "offline/seed_task_graph.png" + assert not (output_dir / "seed_task_graph.json").exists() + agent_config = json.loads(paths.agent_config.read_text(encoding="utf-8")) + assert agent_config["planning_mode"] == "ab" + assert agent_config["offline_seed_task_graph"] == "offline/seed_task_graph.json" + assert agent_config["online_planning"]["vlm_model"] == "mimo-vlm" + scene_requirements = json.loads( + paths.scene_requirements.read_text(encoding="utf-8") + ) + assert [camera["uid"] for camera in scene_requirements["cameras"]] == [ + "vlm_front", + "vlm_left", + "vlm_rear", + "vlm_right", + ] + gym_config = json.loads(paths.gym_config.read_text(encoding="utf-8")) + assert [ + sensor["uid"] + for sensor in gym_config["sensor"] + if sensor["uid"].startswith("vlm_") + ] == ["vlm_front", "vlm_left", "vlm_rear", "vlm_right"] + + +def test_invalid_explicit_task_fails_before_output_asset_materialization( + gym_export: Path, + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + from embodichain.gen_sim.action_engine import tasks + from embodichain.gen_sim.action_engine.generation import generator + + normalized = False + recipe_called = False + writer_called = False + + def reject_task(**_kwargs): + raise ValueError("object selector is ambiguous") + + def record_normalization(*_args, **_kwargs): + nonlocal normalized + normalized = True + raise AssertionError("normalization must not run after planning failure") + + def unexpected_recipe(*_args, **_kwargs): + nonlocal recipe_called + recipe_called = True + raise AssertionError("recipe must not run after interpretation failure") + + def unexpected_writer(*_args, **_kwargs): + nonlocal writer_called + writer_called = True + raise AssertionError("writer must not run after interpretation failure") + + monkeypatch.setattr(tasks, "interpret_and_ground_task_spec", reject_task) + monkeypatch.setattr(tasks, "instantiate_seed_graph", unexpected_recipe) + monkeypatch.setattr(generator, "normalize_scene_assets", record_normalization) + monkeypatch.setattr(generator, "write_generation_artifacts", unexpected_writer) + output_dir = tmp_path / "invalid" + + with pytest.raises(ValueError, match="ambiguous"): + generate_action_engine_config( + gym_export, + output_dir, + task_name="invalid_task", + task_description="test-instruction", + robot_profile="franka", + ) + + assert normalized is False + assert recipe_called is False + assert writer_called is False + assert not output_dir.exists() + + +def test_agent_config_uses_relative_program_paths(gym_export: Path) -> None: + scene = prepare_scene(gym_export) + config = build_agent_config( + task_name="line_task", + robot_profile="franka", + execution_program_hash="b" * 64, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + ) + assert config["task_spec"] == "task_spec.json" + assert config["scene_requirements"] == "scene_requirements.json" + assert config["seed_task_graph"] == "seed_task_graph.json" + assert config["runtime_policy"]["arm_selection"]["pickup_crossing_weight"] == 1.0 + assert config["runtime_policy"]["motion_defaults"]["PickUp"][ + "lift_height" + ] == pytest.approx(0.16) + assert "max_open_length" not in config["runtime_policy"]["grasp"] + assert len(config["runtime_policy_hash"]) == 64 + + +def test_agent_config_anchors_absolute_motion_heights_to_tabletop( + gym_export: Path, +) -> None: + scene = prepare_scene(gym_export) + config = build_agent_config( + task_name="high_table_task", + robot_profile="franka", + execution_program_hash="c" * 64, + source_config_path=scene.source_config_path, + uid_map=scene.uid_map, + table_top_z=1.05, + ) + + policy = config["runtime_policy"] + assert policy["motion_defaults"]["MoveEndEffector"][ + "maximum_eef_height" + ] == pytest.approx(1.45) + assert policy["grounding"]["handover"]["maximum_eef_height"] == pytest.approx(1.85) + + +def test_documented_cli_accepts_franka_profile() -> None: + args = build_parser().parse_args( + [ + "--gym_project", + "gym_export", + "--output_dir", + "configs/task4_2", + "--task_name", + "task4_2", + "--task_description", + "Arrange the cans in a line.", + "--robot-profile", + "franka", + "--overwrite", + ] + ) + assert args.robot_profile == "franka" + assert args.overwrite is True + + +def test_generation_cli_defaults_to_mature_robot_without_scene_randomization() -> None: + args = build_parser().parse_args( + [ + "--gym_project", + "gym_export", + "--output_dir", + "configs/task2_3", + "--task_name", + "task2_3", + "--task_description", + "Upright both objects.", + ] + ) + + assert args.robot_profile == "ur10" + assert args.randomize_scene is False + assert args.planning_mode == "offline" + assert args.planner_mode is None + assert args.ik_solver == "auto" + assert not hasattr(args, "instruction_parser") + assert not hasattr(args, "task_agent") + + +def test_generation_cli_accepts_ab_models() -> None: + args = build_parser().parse_args( + [ + "--gym_project", + "gym_export", + "--output_dir", + "configs/ab", + "--task_name", + "ab", + "--task_description", + "test-instruction", + "--planning-mode", + "ab", + "--llm-model", + "text-model", + "--vlm-model", + "vision-model", + ] + ) + + assert args.planning_mode == "ab" + assert args.llm_model == "text-model" + assert args.vlm_model == "vision-model" + + +def test_generation_cli_accepts_existing_task_spec_without_description() -> None: + args = build_parser().parse_args( + [ + "--gym_project", + "gym_export", + "--output_dir", + "configs/direct", + "--task_name", + "direct_task", + "--task-spec", + "tasks/direct_task/task_spec.json", + ] + ) + + assert args.task_spec == "tasks/direct_task/task_spec.json" + assert cli_module._resolve_task_description(args) == "" + + +def test_generation_cli_reports_seed_png_path( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], +) -> None: + paths = artifact_paths(tmp_path) + monkeypatch.setattr( + cli_module, + "generate_action_engine_config", + lambda *_args, **_kwargs: paths, + ) + monkeypatch.setattr( + sys, + "argv", + [ + "generate_action_agent_config", + "--gym_project", + "gym_export", + "--output_dir", + str(tmp_path), + "--task_name", + "task4_2", + "--task_description", + "Arrange cans.", + ], + ) + + cli_module.cli() + + assert ( + f"Generated Seed graph PNG: {paths.seed_task_graph_png}" + in capsys.readouterr().out + ) + + +def test_generation_cli_explicit_mode_overrides_planner_yaml( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + paths = artifact_paths(tmp_path / "output") + planner_path = tmp_path / "planner.yaml" + planner_path.write_text( + """\ +planner: + backend: curobo + single_arm_strategy: motion_gen + coordinated_strategy: ik_interp + dynamic_collision: true + allow_fallback: false + curobo: + max_attempts: 2 +""", + encoding="utf-8", + ) + captured: dict[str, object] = {} + + def generate(*_args, **kwargs): + captured.update(kwargs) + return paths + + monkeypatch.setattr(cli_module, "generate_action_engine_config", generate) + monkeypatch.setattr( + sys, + "argv", + [ + "generate_action_agent_config", + "--gym-project", + "gym_export", + "--output-dir", + str(tmp_path / "output"), + "--task-name", + "task", + "--task-description", + "test-instruction", + "--planner-config", + str(planner_path), + "--planner-mode", + "ik_interp", + ], + ) + + cli_module.cli() + + assert captured["planner_policy"] == { + "allow_fallback": False, + "mode": "ik_interp", + } + + +@pytest.mark.parametrize( + "removed_args", + [ + ["--instruction-parser", "llm"], + ["--instruction_parser", "llm"], + ["--task-agent", "task-agent.json"], + ["--task_agent", "task-agent.json"], + ], +) +def test_generation_cli_rejects_removed_arguments(removed_args: list[str]) -> None: + base_args = [ + "--gym-project", + "gym_export", + "--output-dir", + "configs/task", + "--task-name", + "task", + "--task-description", + "Upright the can.", + ] + + with pytest.raises(SystemExit, match="2"): + build_parser().parse_args([*base_args, *removed_args]) + + +def test_removed_python_parameters_are_absent() -> None: + parameters = inspect.signature(generate_action_engine_config).parameters + + assert "instruction_parser" not in parameters + assert "task_agent" not in parameters diff --git a/tests/gen_sim/action_engine/planning/__init__.py b/tests/gen_sim/action_engine/planning/__init__.py new file mode 100644 index 000000000..de3758a5b --- /dev/null +++ b/tests/gen_sim/action_engine/planning/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Action Engine planning tests.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/tests/gen_sim/action_engine/planning/test_linker.py b/tests/gen_sim/action_engine/planning/test_linker.py new file mode 100644 index 000000000..dbc711602 --- /dev/null +++ b/tests/gen_sim/action_engine/planning/test_linker.py @@ -0,0 +1,407 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from copy import deepcopy + +import pytest + +from embodichain.gen_sim.action_engine.domain import seed_graph_hash +from embodichain.gen_sim.action_engine.planning.linker import ( + link_seed_graph, + link_task_dependencies, +) +from embodichain.gen_sim.action_engine.protocol import ( + SEED_GRAPH_SCHEMA, + TASK_SPEC_SCHEMA, +) +from embodichain.gen_sim.action_engine.runtime.loader import load_execution_program +from embodichain.gen_sim.action_engine.tasks.recipes import instantiate_seed_graph + + +def _handover_task() -> dict: + return { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": "handover_then_place", + "level": "L3", + "instruction": "Stand both cans, hand over the purple can, then place it.", + "reasoning_type": "none", + "task_instances": [ + { + "id": "task_01", + "task_type": "E2", + "params": { + "object_role": "purple", + "required_arm": "right_arm", + }, + "depends_on": [], + "role": "primary", + }, + { + "id": "task_02", + "task_type": "E2", + "params": { + "object_role": "orange", + "required_arm": "left_arm", + }, + "depends_on": [], + "role": "primary", + }, + { + "id": "task_03", + "task_type": "E4", + "params": { + "object_role": "purple", + "transfer_arm": "right_arm", + "receive_arm": "left_arm", + }, + "depends_on": ["task_02"], + "role": "primary", + }, + { + "id": "task_04", + "task_type": "E1", + "params": { + "object_role": "purple", + "target_role": "orange", + "relation": "left_of", + "required_arm": "left_arm", + }, + "depends_on": ["task_03"], + "role": "primary", + }, + ], + "success": {"type": "all_complete"}, + "oracle": {}, + "metadata": {}, + } + + +def _handover_graph() -> dict: + return instantiate_seed_graph( + _handover_task(), + {"purple": "purple_can", "orange": "orange_can"}, + ) + + +def _unlink_for_rebuild(graph: dict) -> None: + graph["metadata"].pop("action_contract_linker", None) + for group in graph["task_groups"]: + group.pop("contract", None) + + +def test_task_linker_preserves_parallel_arms_and_waits_for_both_before_handover() -> ( + None +): + linked = link_task_dependencies( + _handover_task(), + {"purple": "purple_can", "orange": "orange_can"}, + ) + by_id = {item["id"]: item for item in linked["task_instances"]} + + assert by_id["task_01"]["depends_on"] == [] + assert by_id["task_02"]["depends_on"] == [] + assert by_id["task_03"]["depends_on"] == ["task_02", "task_01"] + + +def test_resource_dependency_provenance_is_persisted_in_seed_graph() -> None: + task = _handover_task() + task["task_instances"] = task["task_instances"][:3] + handover = task["task_instances"][2] + handover["params"] = { + "object_role": "orange", + "transfer_arm": "left_arm", + "receive_arm": "right_arm", + } + + graph = instantiate_seed_graph( + task, + {"purple": "purple_can", "orange": "orange_can"}, + ) + + provenance = graph["metadata"]["action_contract_task_linker"] + assert provenance["linked_dependencies"] == [ + { + "from": "task_01", + "to": "task_03", + "reason": "resource", + "detail": "arm:right_arm", + } + ] + + +def test_same_object_e2_handover_gets_direct_causal_edge_through_a_chain() -> None: + task = _handover_task() + task["task_instances"][1]["depends_on"] = ["task_01"] + linked = link_task_dependencies( + task, + {"purple": "purple_can", "orange": "orange_can"}, + ) + handover = next( + item for item in linked["task_instances"] if item["id"] == "task_03" + ) + + assert handover["depends_on"] == ["task_02", "task_01"] + + +def test_handover_ownership_flows_through_home_terminal_barrier() -> None: + graph = _handover_graph() + groups = {group["id"]: group for group in graph["task_groups"]} + nodes = {node["id"]: node for node in graph["nodes"]} + handover_group = groups["task_03"] + terminal_id = handover_group["contract"]["terminal_node_ids"][0] + terminal = nodes[terminal_id] + receiver_entry = nodes[groups["task_04"]["contract"]["entry_node_ids"][0]] + handover = next( + node + for node in graph["nodes"] + if node["task_instance_id"] == "task_03" and node["atomic_action"] == "HandOver" + ) + + assert terminal["atomic_action"] == "MoveHeldObject" + assert terminal["contract"]["completion"] == "terminal_barrier" + assert terminal["contract"]["failure_policy"] == "task_required" + retreat = next( + node + for node in graph["nodes"] + if node["task_instance_id"] == "task_03" + and node["atomic_action"] == "MoveEndEffector" + ) + assert retreat["contract"]["failure_policy"] == "safety_required" + assert terminal_id in receiver_entry["depends_on"] + assert { + (effect["op"], effect["atom"]["predicate"], effect["atom"].get("arm")) + for effect in handover["contract"]["effects"] + } >= { + ("delete", "object_held", "right_arm"), + ("add", "object_held", "left_arm"), + } + + +def test_linker_is_idempotent_and_hash_stable() -> None: + graph = _handover_graph() + relinked = link_seed_graph( + graph, + task_order=["task_01", "task_02", "task_03", "task_04"], + known_objects={"purple_can", "orange_can", "table"}, + ) + + assert relinked == graph + assert seed_graph_hash(relinked) == seed_graph_hash(graph) + + +def test_linker_rejects_missing_cleanup_wrong_holder_and_duplicate_pickup() -> None: + missing_cleanup = deepcopy(_handover_graph()) + home = next( + node + for node in missing_cleanup["nodes"] + if node["task_instance_id"] == "task_03" + and node["atomic_action"] == "MoveJoints" + ) + missing_cleanup["nodes"].remove(home) + next(group for group in missing_cleanup["task_groups"] if group["id"] == "task_03")[ + "node_ids" + ].remove(home["id"]) + for node in missing_cleanup["nodes"]: + node["depends_on"] = [ + dependency for dependency in node["depends_on"] if dependency != home["id"] + ] + _unlink_for_rebuild(missing_cleanup) + with pytest.raises(ValueError, match="terminal barrier"): + link_seed_graph(missing_cleanup) + + wrong_holder = deepcopy(_handover_graph()) + staging = next( + node + for node in wrong_holder["nodes"] + if node["task_instance_id"] == "task_03" + and node["atomic_action"] == "MoveHeldObject" + ) + staging["actor"] = {"mode": "required", "arm": "left_arm"} + staging.pop("contract") + _unlink_for_rebuild(wrong_holder) + with pytest.raises(ValueError, match="no producer|unavailable state"): + link_seed_graph(wrong_holder) + + duplicate_pickup = deepcopy(_handover_graph()) + pickup = next( + node + for node in duplicate_pickup["nodes"] + if node["task_instance_id"] == "task_03" and node["atomic_action"] == "PickUp" + ) + staging = next( + node + for node in duplicate_pickup["nodes"] + if node["task_instance_id"] == "task_03" + and node["atomic_action"] == "MoveHeldObject" + ) + repeated = deepcopy(pickup) + repeated["id"] = "task_01__duplicate_pickup" + repeated["depends_on"] = [pickup["id"]] + repeated.pop("contract") + staging["depends_on"] = [repeated["id"]] + group = next( + group for group in duplicate_pickup["task_groups"] if group["id"] == "task_03" + ) + pickup_index = group["node_ids"].index(pickup["id"]) + group["node_ids"].insert(pickup_index + 1, repeated["id"]) + duplicate_pickup["nodes"].insert( + duplicate_pickup["nodes"].index(pickup) + 1, repeated + ) + _unlink_for_rebuild(duplicate_pickup) + with pytest.raises(ValueError, match="requires unavailable state"): + link_seed_graph(duplicate_pickup) + + +def test_unavailable_arm_reports_current_holder_and_requested_object() -> None: + task = _handover_task() + placement = task["task_instances"][3] + placement["params"].update( + { + "object_role": "orange", + "target_role": "purple", + "required_arm": "left_arm", + } + ) + + with pytest.raises( + ValueError, + match=( + "left_arm.*currently holds 'purple_can'.*" "primary object is 'orange_can'" + ), + ): + instantiate_seed_graph( + task, + {"purple": "purple_can", "orange": "orange_can"}, + ) + + +def test_readers_remain_parallel_and_writer_waits_for_both() -> None: + task = { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": "read_write", + "level": "L3", + "instruction": "Inspect a shared target, then manipulate it.", + "reasoning_type": "none", + "task_instances": [ + { + "id": "read_left", + "task_type": "E1", + "params": { + "object_role": "a", + "target_role": "target", + "required_arm": "left_arm", + }, + "depends_on": [], + "role": "primary", + }, + { + "id": "read_right", + "task_type": "E1", + "params": { + "object_role": "b", + "target_role": "target", + "required_arm": "right_arm", + }, + "depends_on": [], + "role": "primary", + }, + { + "id": "write_target", + "task_type": "E2", + "params": { + "object_role": "target", + "required_arm": "left_arm", + }, + "depends_on": [], + "role": "primary", + }, + ], + "success": {}, + "oracle": {}, + "metadata": {}, + } + linked = link_task_dependencies( + task, + {"a": "object_a", "b": "object_b", "target": "shared_target"}, + ) + by_id = {item["id"]: item for item in linked["task_instances"]} + + assert by_id["read_left"]["depends_on"] == [] + assert by_id["read_right"]["depends_on"] == [] + assert by_id["write_target"]["depends_on"] == ["read_left", "read_right"] + + +def test_explicit_distinct_arm_allocation_keeps_auto_groups_parallel() -> None: + task = { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": "allocated_auto", + "level": "L2", + "instruction": "Stand both objects upright in parallel.", + "reasoning_type": "none", + "task_instances": [ + { + "id": "first", + "task_type": "E2", + "params": {"object_role": "first_object"}, + "depends_on": [], + "role": "primary", + }, + { + "id": "second", + "task_type": "E2", + "params": {"object_role": "second_object"}, + "depends_on": [], + "role": "primary", + }, + ], + "success": {}, + "oracle": {}, + "metadata": { + "allocation_groups": [ + { + "id": "distinct_pair", + "task_instance_ids": ["first", "second"], + "arm_constraint": "distinct_arms", + } + ] + }, + } + bindings = {"first_object": "first_uid", "second_object": "second_uid"} + linked = link_task_dependencies(task, bindings) + graph = instantiate_seed_graph(linked, bindings) + + assert all(not item["depends_on"] for item in linked["task_instances"]) + assert all(not group["depends_on"] for group in graph["task_groups"]) + + +def test_v2_and_resolver_mismatch_require_regeneration() -> None: + with pytest.raises( + ValueError, match="lacks persisted Action Contracts.*regenerate" + ): + load_execution_program({"schema_version": "action_engine_seed_graph_v2"}) + + graph = _handover_graph() + graph["nodes"][0]["contract"]["claims"][0]["access"] = "shared_read" + with pytest.raises( + ValueError, match="does not match the current capability resolver" + ): + load_execution_program(graph) + + +def test_seed_graph_schema_is_v3() -> None: + assert _handover_graph()["schema_version"] == SEED_GRAPH_SCHEMA diff --git a/tests/gen_sim/action_engine/planning/test_online_v2.py b/tests/gen_sim/action_engine/planning/test_online_v2.py new file mode 100644 index 000000000..bfa7b8938 --- /dev/null +++ b/tests/gen_sim/action_engine/planning/test_online_v2.py @@ -0,0 +1,480 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from copy import deepcopy +from threading import Barrier + +import pytest +import torch + +import embodichain.gen_sim.action_engine.planning.online as online_module +import embodichain.gen_sim.action_engine.planning.planner as planner_module +import embodichain.gen_sim.action_engine.planning.vision as vision_module +from embodichain.gen_sim.action_engine.domain import public_task_spec +from embodichain.gen_sim.action_engine.planning import ( + CameraObservation, + SceneObservation, + analyze_visual_scene, + fuse_seed_graphs, + plan_candidates_parallel, + plan_online_seed_graph, + select_seed_graph, + validate_visual_facts, +) +from embodichain.gen_sim.action_engine.tasks import instantiate_seed_graph + +from ..task_fixtures import make_task_level + + +def _task(level: str, *, reasoning: str | None = None): + task, requirements = make_task_level(level, reasoning=reasoning) + bindings = { + item["role_id"]: f"uid_{item['role_id']}" for item in requirements["objects"] + } + return task, requirements, bindings + + +def test_online_planner_sees_public_task_and_returns_complete_seed_graph() -> None: + task, _, bindings = _task("L4", reasoning="visual_semantics") + offline = instantiate_seed_graph(task, bindings) + body = {key: deepcopy(offline[key]) for key in ("nodes", "task_groups", "success")} + for node in body["nodes"]: + node.pop("contract") + for group in body["task_groups"]: + group.pop("contract") + visual_move = next( + node for node in body["nodes"] if node["atomic_action"] == "MoveHeldObject" + ) + visual_move["target_binding"] = { + "kind": "visual_constraint", + "camera_uid": "front", + "normalized_keypoint": [0.2, 0.3], + } + camera = CameraObservation( + "front", + torch.zeros((8, 8, 3), dtype=torch.uint8), + None, + None, + None, + ) + observation = SceneObservation( + (camera,), + tuple({"uid": uid} for uid in bindings.values()), + ) + uid = next(iter(bindings.values())) + facts = { + "entities": [ + { + "uid": uid, + "camera_uid": "front", + "bbox": [0.1, 0.2, 0.3, 0.4], + "keypoints": {"center": [0.2, 0.3]}, + "confidence": 0.9, + } + ], + "relations": [], + "task_predicates": [], + "confidence": 0.9, + } + prompts = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return body + + graph, observed_facts = plan_online_seed_graph( + public_task_spec(task), + observation, + visual_facts=facts, + graph_caller=caller, + ) + + assert graph["planner_route"] == "online" + assert observed_facts == facts + assert "oracle" not in prompts[0] + assert '"task_instances"' not in prompts[0] + assert '"E4"' in prompts[0] + assert "Transfer one object between arms" in prompts[0] + assert graph["metadata"]["oracle_exposed"] is False + assert any( + node["target_binding"]["kind"] == "visual_constraint" for node in graph["nodes"] + ) + + +def test_offline_and_online_candidates_plan_concurrently_with_isolated_views() -> None: + task, _, bindings = _task("L1") + offline = instantiate_seed_graph(task, bindings) + online = deepcopy(offline) + online["planner_route"] = "online" + barrier = Barrier(2) + views = {} + + def offline_planner(*, task_spec): + views["offline"] = task_spec + barrier.wait(timeout=2.0) + return offline + + def online_planner(*, task_spec): + views["online"] = task_spec + barrier.wait(timeout=2.0) + return online + + pair = plan_candidates_parallel( + task, + offline_planner=offline_planner, + online_planner=online_planner, + ) + + assert "oracle" in views["offline"] + assert "oracle" not in views["online"] + assert pair.offline["planner_route"] == "offline" + assert pair.online["planner_route"] == "online" + + +def test_visual_facts_reject_unknown_uid_and_out_of_range_keypoint() -> None: + value = { + "entities": [ + { + "uid": "unknown", + "camera_uid": "front", + "bbox": [0.0, 0.0, 1.2, 1.0], + "keypoints": {}, + "confidence": 1.0, + } + ], + "relations": [], + "task_predicates": [], + "confidence": 1.0, + } + with pytest.raises(ValueError, match="unknown UID"): + validate_visual_facts(value, known_uids={"known"}, camera_uids={"front"}) + + +def test_visual_facts_reject_visible_entity_without_image_evidence() -> None: + value = { + "entities": [ + { + "uid": "known", + "camera_uid": "front", + "visible": True, + "confidence": 0.9, + } + ], + "relations": [], + "task_predicates": [], + "confidence": 0.9, + } + + with pytest.raises(ValueError, match="bbox or keypoint"): + validate_visual_facts(value, known_uids={"known"}, camera_uids={"front"}) + + +def test_visual_facts_reject_non_numeric_image_coordinates() -> None: + value = { + "entities": [ + { + "uid": "known", + "camera_uid": "front", + "bbox": ["0.1", 0.2, 0.3, 0.4], + "confidence": 0.9, + } + ], + "relations": [], + "task_predicates": [], + "confidence": 0.9, + } + + with pytest.raises(ValueError, match="must be numeric"): + validate_visual_facts(value, known_uids={"known"}, camera_uids={"front"}) + + +def test_visual_fact_caller_receives_rgb_depth_and_calibration_evidence() -> None: + task, _, bindings = _task("L1") + uid = next(iter(bindings.values())) + observation = SceneObservation( + ( + CameraObservation( + "front", + torch.zeros((4, 5, 3), dtype=torch.uint8), + torch.linspace(0.0, 1.0, 20, dtype=torch.float32).reshape(4, 5), + torch.eye(3), + torch.eye(4), + ), + ), + ({"uid": uid},), + ) + captured = {} + + def caller(**kwargs): + captured.update(kwargs) + return { + "entities": [ + { + "uid": uid, + "camera_uid": "front", + "bbox": [0.1, 0.2, 0.3, 0.4], + "confidence": 0.9, + } + ], + "relations": [], + "task_predicates": [], + "confidence": 0.9, + } + + facts = analyze_visual_scene(observation, task, caller=caller) + + assert facts["entities"][0]["uid"] == uid + assert len(captured["images"]) == 2 + assert '"depth_image_index": 1' in captured["prompt"] + assert '"intrinsics": [[1.0, 0.0, 0.0]' in captured["prompt"] + assert captured["schema"]["properties"]["task_predicates"]["maxItems"] == 0 + + +def test_visual_task_predicates_are_limited_to_the_current_task() -> None: + task, _, bindings = _task("L4", reasoning="visual_semantics") + uid = next(iter(bindings.values())) + observation = SceneObservation( + ( + CameraObservation( + "front", + torch.zeros((4, 5, 3), dtype=torch.uint8), + None, + None, + None, + ), + ), + ({"uid": uid},), + ) + captured = {} + + def caller(**kwargs): + captured.update(kwargs) + return { + "entities": [ + { + "uid": uid, + "camera_uid": "front", + "bbox": [0.1, 0.2, 0.3, 0.4], + "confidence": 0.9, + } + ], + "relations": [], + "task_predicates": [{"type": "mouth_completed", "confidence": 0.9}], + "confidence": 0.9, + } + + facts = analyze_visual_scene(observation, task, caller=caller) + + predicate_type = captured["schema"]["properties"]["task_predicates"]["items"][ + "properties" + ]["type"] + assert predicate_type["enum"] == ["mouth_completed"] + assert facts["task_predicates"][0]["type"] == "mouth_completed" + + +def test_visual_facts_reject_unrequested_task_predicate() -> None: + value = { + "entities": [], + "relations": [], + "task_predicates": [{"type": "mouth_completed", "confidence": 0.9}], + "confidence": 0.9, + } + + with pytest.raises(ValueError, match="task_predicates.*must be one of"): + validate_visual_facts( + value, + known_uids={"known"}, + camera_uids={"front"}, + ) + + +def test_production_online_graph_caller_receives_reset_time_multiview_evidence( + monkeypatch: pytest.MonkeyPatch, +) -> None: + observation = SceneObservation( + ( + CameraObservation( + "front", + torch.zeros((4, 5, 3), dtype=torch.uint8), + torch.zeros((4, 5), dtype=torch.float32), + torch.eye(3), + torch.eye(4), + ), + ), + ({"uid": "known"},), + ) + captured = {} + + def caller(**kwargs): + captured.update(kwargs) + return {"nodes": [], "task_groups": [], "success": {}} + + monkeypatch.setattr(vision_module, "_default_structured_caller", caller) + monkeypatch.setattr(vision_module, "_vlm_model", lambda model: f"resolved:{model}") + + result = online_module._default_graph_caller( + prompt="plan", + schema={"type": "object"}, + model="mimo", + observation=observation, + ) + + assert result == {"nodes": [], "task_groups": [], "success": {}} + assert captured["model"] == "resolved:mimo" + assert len(captured["images"]) == 2 + + +def test_default_vision_caller_disables_custom_socket_transport( + monkeypatch: pytest.MonkeyPatch, +) -> None: + import langchain_openai + + captured = {} + + class FakeRunnable: + def invoke(self, _messages): + return {"facts": []} + + class FakeChatOpenAI: + def __init__(self, **kwargs): + captured.update(kwargs) + + def with_structured_output(self, _schema, **_kwargs): + return FakeRunnable() + + monkeypatch.setattr(langchain_openai, "ChatOpenAI", FakeChatOpenAI) + monkeypatch.setattr( + planner_module, + "_load_llm_settings", + lambda *, model: { + "api_key": "test-key", + "model": model or "test-model", + "base_url": "https://example.test/v1", + "default_query": {}, + }, + ) + + vision_module._default_structured_caller( + prompt="inspect", + images=(), + schema={"type": "object"}, + model="test-model", + ) + + assert captured["http_socket_options"] == () + + +def test_visual_facts_reject_unstructured_entity_fields() -> None: + value = { + "entities": [ + { + "uid": "known", + "camera_uid": "front", + "bbox": [0.1, 0.2, 0.3, 0.4], + "semantic_label": "can", + "confidence": 0.9, + } + ], + "relations": [], + "task_predicates": [], + "confidence": 0.9, + } + + with pytest.raises(ValueError, match="unsupported fields"): + validate_visual_facts(value, known_uids={"known"}, camera_uids={"front"}) + + +def test_visual_facts_reject_noncanonical_relation_type() -> None: + value = { + "entities": [], + "relations": [ + {"type": "obstructs", "uids": ["box", "sign"], "confidence": 0.9} + ], + "task_predicates": [], + "confidence": 0.9, + } + + with pytest.raises(ValueError, match="relation type"): + validate_visual_facts( + value, + known_uids={"box", "sign"}, + camera_uids={"front"}, + ) + + +def test_visual_facts_require_ordered_relation_participants() -> None: + value = { + "entities": [], + "relations": [{"type": "occludes", "uids": ["box"], "confidence": 0.9}], + "task_predicates": [], + "confidence": 0.9, + } + + with pytest.raises(ValueError, match="exactly 2 UIDs"): + validate_visual_facts( + value, + known_uids={"box"}, + camera_uids={"front"}, + ) + + +def test_selection_prefers_exact_offline_and_l4_online() -> None: + task, _, bindings = _task("L1") + offline = instantiate_seed_graph(task, bindings) + online = deepcopy(offline) + online["planner_route"] = "online" + selected, evaluations = select_seed_graph( + offline, + online, + task, + known_objects=set(bindings.values()) | {"table"}, + exact_template_match=True, + ) + assert selected["metadata"]["selected_from"] == "offline" + assert evaluations["offline"].score > evaluations["online"].score + + l4, _, l4_bindings = _task("L4", reasoning="logic") + l4_offline = instantiate_seed_graph(l4, l4_bindings) + l4_online = deepcopy(l4_offline) + l4_online["planner_route"] = "online" + selected, _ = select_seed_graph( + l4_offline, + l4_online, + l4, + known_objects=set(l4_bindings.values()) | {"table"}, + visual_confidence=0.95, + ) + assert selected["metadata"]["selected_from"] == "online" + + +def test_fusion_keeps_whole_task_groups() -> None: + task, _, bindings = _task("L2") + offline = instantiate_seed_graph(task, bindings) + online = deepcopy(offline) + online["planner_route"] = "online" + routes = { + group["id"]: ("offline" if index % 2 == 0 else "online") + for index, group in enumerate(offline["task_groups"]) + } + fused = fuse_seed_graphs(offline, online, routes) + + assert fused["planner_route"] == "fused" + assert all( + all(node_id.startswith(routes[group["id"]]) for node_id in group["node_ids"]) + for group in fused["task_groups"] + ) diff --git a/tests/gen_sim/action_engine/planning/test_planner.py b/tests/gen_sim/action_engine/planning/test_planner.py new file mode 100644 index 000000000..c089ccef8 --- /dev/null +++ b/tests/gen_sim/action_engine/planning/test_planner.py @@ -0,0 +1,730 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import inspect +import json +from pathlib import Path +from typing import Any + +import pytest + +from embodichain.gen_sim.action_engine.domain import TASK_AGENT_SCHEMA +from embodichain.gen_sim.action_engine.planning import plan_task +from embodichain.gen_sim.action_engine.planning import planner as planner_module + + +def _scene() -> list[dict[str, Any]]: + return [ + { + "uid": "table", + "runtime_uid": "table", + "source_uid": "table", + "role": "background", + "description": "A table.", + }, + *[ + { + "uid": f"interact_soda_can_{index}_0", + "runtime_uid": f"interact_soda_can_{index}", + "source_uid": f"interact_soda_can_{index}_0", + "role": "rigid_object", + "description": "An aluminum soda can.", + } + for index in range(5) + ], + ] + + +def _dual_arm_scene() -> list[dict[str, Any]]: + return [ + { + "uid": uid, + "runtime_uid": uid, + "source_uid": uid, + "role": "rigid_object", + "description": description, + } + for uid, description in ( + ("cube", "A cube on the left side of the table."), + ("cup", "A paper cup on the right side of the table."), + ("basket", "A basket near the center of the table."), + ) + ] + + +def _stack_scene() -> list[dict[str, Any]]: + return [ + { + "uid": uid, + "runtime_uid": uid, + "source_uid": f"{uid}_0", + "role": role, + "description": description, + } + for uid, role, description in ( + ("table", "background", "A table."), + ("paper_cup", "rigid_object", "A paper cup."), + ("popcorn_bucket", "rigid_object", "A popcorn bucket."), + ("earbuds_case", "rigid_object", "A blue earbuds case."), + ) + ] + + +def test_injected_planner_returns_only_semantics_and_resolves_aliases() -> None: + observed: dict[str, Any] = {} + + def caller(*, prompt: str, model: str | None) -> dict[str, Any]: + observed.update(prompt=prompt, model=model) + return { + "semantic_steps": [ + { + "id": "s01_place", + "operator": "place_relative", + "object": "interact_soda_can_0_0", + "goal": {"reference_object": "table", "relation": "on"}, + }, + { + "id": "s02_orient", + "operator": "orient_object", + "object": "interact_soda_can_1_0", + "goal": { + "orientation_goal": "upright", + "orientation_axis": "none", + }, + }, + ] + } + + program = plan_task( + task_name="injected", + task_description="Place one object and then orient another.", + scene_objects=_scene(), + model="test-model", + llm_caller=caller, + ) + + assert program["schema_version"] == TASK_AGENT_SCHEMA + assert program["semantic_steps"][0]["object"] == "interact_soda_can_0" + assert program["semantic_steps"][1]["depends_on"] == ["s01_place"] + assert "Do not select a task route" in observed["prompt"] + assert observed["model"] == "test-model" + + +def test_planner_repairs_a_non_visible_skill_once() -> None: + calls = 0 + + def caller(**_kwargs: Any) -> dict[str, Any]: + nonlocal calls + calls += 1 + if calls == 1: + return { + "semantic_steps": [ + { + "id": "s01_hold", + "operator": "hold_hover", + "object": "cube", + "goal": {}, + } + ] + } + return { + "semantic_steps": [ + { + "id": "s01_place_cube", + "operator": "place_relative", + "object": "cube", + "goal": { + "reference_object": "basket", + "relation": "inside", + "orientation_goal": "preserve", + "orientation_axis": "none", + }, + "depends_on": [], + }, + { + "id": "s02_place_cup", + "operator": "place_relative", + "object": "cup", + "goal": { + "reference_object": "basket", + "relation": "inside", + }, + "depends_on": [], + }, + ], + "allocation_groups": [ + { + "id": "dual_arms_1", + "semantic_step_ids": ["s01_place_cube", "s02_place_cup"], + "arm_constraint": "distinct_arms", + } + ], + } + + program = plan_task( + task_name="dual_arm_basket", + task_description="test-instruction", + scene_objects=_dual_arm_scene(), + llm_caller=caller, + ) + + assert [ + (step["id"], step["operator"], step["object"], step["depends_on"]) + for step in program["semantic_steps"] + ] == [ + ("s01_place_cube", "place_relative", "cube", []), + ("s02_place_cup", "place_relative", "cup", []), + ] + assert calls == 2 + assert program["allocation_groups"][0]["arm_constraint"] == "distinct_arms" + + +def test_planner_repairs_build_stack_singular_object_contract() -> None: + prompts: list[str] = [] + + def caller(*, prompt: str, **_kwargs: Any) -> dict[str, Any]: + prompts.append(prompt) + if len(prompts) == 1: + return { + "semantic_steps": [ + { + "id": "s01_build_stack", + "operator": "build_stack", + "object": "paper_cup", + "goal": { + "anchor": "popcorn_bucket", + "stack_mode": "on_top", + }, + "depends_on": [], + } + ], + "allocation_groups": [], + } + return { + "semantic_steps": [ + { + "id": "s01_build_stack", + "operator": "build_stack", + "objects": ["paper_cup", "earbuds_case"], + "goal": { + "anchor": "popcorn_bucket", + "stack_mode": "on_top", + "orientation_goal": "preserve", + "orientation_axis": "none", + }, + "depends_on": [], + } + ], + "allocation_groups": [], + } + + program = plan_task( + task_name="task3_2", + task_description="test-instruction", + scene_objects=_stack_scene(), + llm_caller=caller, + ) + + assert len(prompts) == 2 + assert "build_stack requires an 'objects' list" in prompts[1] + assert program["semantic_steps"][0]["objects"] == [ + "paper_cup", + "earbuds_case", + ] + assert program["semantic_steps"][0]["goal"]["anchor"] == "popcorn_bucket" + + +def test_planner_rejects_a_non_visible_skill_after_one_repair() -> None: + def caller(**_kwargs: Any) -> dict[str, Any]: + return { + "semantic_steps": [ + { + "id": "s01_hold", + "operator": "hold_hover", + "object": "cube", + "actor": {"mode": "required", "arm": "left_arm"}, + "goal": {}, + "depends_on": [], + }, + { + "id": "s02_place", + "operator": "place_relative", + "object": "cube", + "actor": {"mode": "required", "arm": "right_arm"}, + "goal": { + "reference_object": "basket", + "relation": "inside", + }, + "depends_on": ["s01_hold"], + }, + ] + } + + with pytest.raises(ValueError, match="after one repair"): + plan_task( + task_name="conflicting_arms", + task_description="Hold the cube with the left arm, then place it " + "with the right arm.", + scene_objects=_dual_arm_scene(), + llm_caller=caller, + ) + + +def test_spatial_two_sided_phrase_does_not_invent_arm_constraint() -> None: + def caller(**_kwargs: Any) -> dict[str, Any]: + return { + "semantic_steps": [ + { + "id": "s01_left", + "operator": "orient_object", + "object": "cube", + "goal": { + "orientation_goal": "upright", + "orientation_axis": "none", + }, + "depends_on": [], + }, + { + "id": "s02_right", + "operator": "orient_object", + "object": "cup", + "goal": { + "orientation_goal": "upright", + "orientation_axis": "none", + }, + "depends_on": [], + }, + ], + "allocation_groups": [], + } + + program = plan_task( + task_name="two_sided_upright", + task_description="test-instruction", + scene_objects=_dual_arm_scene(), + llm_caller=caller, + ) + + assert program["allocation_groups"] == [] + + +def test_planner_does_not_infer_arm_group_from_instruction_text() -> None: + def caller(**_kwargs: Any) -> dict[str, Any]: + return { + "semantic_steps": [ + { + "id": step_id, + "operator": "orient_object", + "object": object_uid, + "goal": { + "orientation_goal": "upright", + "orientation_axis": "none", + }, + "depends_on": [], + } + for step_id, object_uid in (("s01", "cube"), ("s02", "cup")) + ], + "allocation_groups": [], + } + + program = plan_task( + task_name="explicit_both_arms", + task_description="test-instruction", + scene_objects=_dual_arm_scene(), + llm_caller=caller, + ) + + assert program["allocation_groups"] == [] + + +def test_planner_does_not_expose_internal_operator_contracts() -> None: + def caller(**_kwargs: Any) -> dict[str, Any]: + return { + "semantic_steps": [ + { + "id": "s01_hold", + "operator": "hold_hover", + "object": "cube", + "actor": {"mode": "auto"}, + "goal": {}, + "depends_on": [], + }, + { + "id": "s02_place", + "operator": "place_relative", + "object": "cube", + "actor": {"mode": "auto"}, + "goal": { + "reference_object": "basket", + "relation": "inside", + }, + "depends_on": ["s01_hold"], + }, + ] + } + + with pytest.raises(ValueError, match="after one repair"): + plan_task( + task_name="nondefault_hover", + task_description="Hold the cube in a special pose, then place it.", + scene_objects=_dual_arm_scene(), + llm_caller=caller, + ) + + +def test_plan_task_has_no_rule_fallback_parameter() -> None: + assert "deterministic_fallback" not in inspect.signature(plan_task).parameters + + +def test_arrange_line_preserves_structured_orientation_output() -> None: + def caller(**_kwargs: Any) -> dict[str, Any]: + return { + "semantic_steps": [ + { + "id": "s01_line", + "operator": "arrange_line", + "objects": [ + "interact_soda_can_0", + "interact_soda_can_1", + ], + "goal": { + "axis": "world_y", + "order_constraint": "free", + "orientation_goal": "upright", + "orientation_axis": "long_axis", + }, + "depends_on": [], + } + ], + "allocation_groups": [], + } + + program = plan_task( + task_name="neutral_line", + task_description="test-instruction", + scene_objects=_scene(), + llm_caller=caller, + ) + + goal = program["semantic_steps"][0]["goal"] + assert goal["orientation_goal"] == "upright" + assert goal["orientation_axis"] == "long_axis" + + +def test_arrange_line_preserves_structured_axis_output() -> None: + def caller(**_kwargs: Any) -> dict[str, Any]: + return { + "semantic_steps": [ + { + "id": "s01_line", + "operator": "arrange_line", + "objects": [ + "interact_soda_can_0", + "interact_soda_can_1", + ], + "goal": { + "anchor": "table_center", + "axis": "world_x", + "order_constraint": "free", + }, + "depends_on": [], + } + ], + "allocation_groups": [], + } + + program = plan_task( + task_name="ambiguous_line_axis", + task_description="test-instruction", + scene_objects=_scene(), + llm_caller=caller, + ) + + assert program["semantic_steps"][0]["goal"]["axis"] == "world_x" + + +def test_instruction_text_does_not_override_structured_axis_output() -> None: + def caller(**_kwargs: Any) -> dict[str, Any]: + return { + "semantic_steps": [ + { + "id": "s01_line", + "operator": "arrange_line", + "objects": [ + "interact_soda_can_0", + "interact_soda_can_1", + ], + "goal": { + "anchor": "table_center", + "axis": "world_y", + "order_constraint": "free", + }, + "depends_on": [], + } + ], + "allocation_groups": [], + } + + program = plan_task( + task_name="front_to_back_line_axis", + task_description="test-instruction", + scene_objects=_scene(), + llm_caller=caller, + ) + + assert program["semantic_steps"][0]["goal"]["axis"] == "world_y" + + +def test_arrange_line_preserves_explicit_orientation_request() -> None: + def caller(**_kwargs: Any) -> dict[str, Any]: + return { + "semantic_steps": [ + { + "id": "s01_line", + "operator": "arrange_line", + "objects": [ + "interact_soda_can_0", + "interact_soda_can_1", + ], + "goal": { + "axis": "world_y", + "order_constraint": "free", + "orientation_goal": "upright", + "orientation_axis": "none", + }, + "depends_on": [], + } + ], + "allocation_groups": [], + } + + program = plan_task( + task_name="upright_line", + task_description="test-instruction", + scene_objects=_scene(), + llm_caller=caller, + ) + + assert program["semantic_steps"][0]["goal"]["orientation_goal"] == "upright" + + +def test_planner_rejects_route_or_graph_output() -> None: + def caller(**_kwargs: Any) -> dict[str, Any]: + return {"route": "arrangement_line", "semantic_steps": []} + + with pytest.raises(ValueError, match="only 'semantic_steps'"): + plan_task( + task_name="bad", + task_description="Arrange objects.", + scene_objects=_scene(), + llm_caller=caller, + ) + + +def test_llm_settings_read_gen_sim_dotenv( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + env_path = tmp_path / ".env" + env_path.write_text( + "\n".join( + ( + "# Local Action Engine credentials", + 'export OPENAI_API_KEY="dotenv-key"', + "OPENAI_BASE_URL=https://dotenv.example/v1/", + "OPENAI_MODEL=dotenv-model", + ) + ), + encoding="utf-8", + ) + config_path = tmp_path / "gen_config.json" + config_path.write_text( + json.dumps( + { + "llm": { + "openai_compatible": { + "api_key": "json-key", + "base_url": "https://json.example/v1", + "model": "json-model", + "default_query": {"api-version": "test"}, + } + } + } + ), + encoding="utf-8", + ) + monkeypatch.setattr(planner_module, "_GEN_SIM_ENV_PATH", env_path) + monkeypatch.setattr(planner_module, "_GEN_CONFIG_PATH", config_path) + for name in ( + "OPENAI_API_KEY", + "OPENAI_BASE_URL", + "OPENAI_API_BASE", + "OPENAI_MODEL", + "LLM_MODEL", + "LLM_URL", + ): + monkeypatch.delenv(name, raising=False) + + settings = planner_module._load_llm_settings(model=None) + + assert settings == { + "api_key": "dotenv-key", + "base_url": "https://dotenv.example/v1", + "model": "dotenv-model", + "default_query": {"api-version": "test"}, + } + + +def test_process_environment_and_model_argument_override_dotenv( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + env_path = tmp_path / ".env" + env_path.write_text( + "\n".join( + ( + "OPENAI_API_KEY=dotenv-key", + "OPENAI_BASE_URL=https://dotenv.example/v1", + "OPENAI_MODEL=dotenv-model", + ) + ), + encoding="utf-8", + ) + missing_config = tmp_path / "missing.json" + monkeypatch.setattr(planner_module, "_GEN_SIM_ENV_PATH", env_path) + monkeypatch.setattr(planner_module, "_GEN_CONFIG_PATH", missing_config) + monkeypatch.setenv("OPENAI_API_KEY", "shell-key") + monkeypatch.setenv("OPENAI_API_BASE", "https://shell.example/v1/") + monkeypatch.setenv("LLM_MODEL", "shell-model") + monkeypatch.delenv("OPENAI_BASE_URL", raising=False) + monkeypatch.delenv("OPENAI_MODEL", raising=False) + + settings = planner_module._load_llm_settings(model="argument-model") + + assert settings["api_key"] == "shell-key" + assert settings["base_url"] == "https://shell.example/v1" + assert settings["model"] == "argument-model" + + +def test_partial_process_transport_does_not_mix_with_dotenv( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + env_path = tmp_path / ".env" + env_path.write_text( + "\n".join( + ( + "OPENAI_API_KEY=dotenv-key", + "OPENAI_BASE_URL=https://dotenv.example/v1", + "OPENAI_MODEL=dotenv-model", + ) + ), + encoding="utf-8", + ) + monkeypatch.setattr(planner_module, "_GEN_SIM_ENV_PATH", env_path) + monkeypatch.setattr( + planner_module, + "_GEN_CONFIG_PATH", + tmp_path / "missing.json", + ) + for name in ( + "OPENAI_API_KEY", + "OPENAI_BASE_URL", + "OPENAI_API_BASE", + "OPENAI_MODEL", + "LLM_MODEL", + "LLM_URL", + ): + monkeypatch.delenv(name, raising=False) + monkeypatch.setenv("OPENAI_API_KEY", "unrelated-process-key") + + settings = planner_module._load_llm_settings(model=None) + + assert settings["api_key"] == "dotenv-key" + assert settings["base_url"] == "https://dotenv.example/v1" + assert settings["model"] == "dotenv-model" + + +def test_default_llm_caller_disables_custom_socket_transport( + monkeypatch: pytest.MonkeyPatch, +) -> None: + import langchain_openai + + captured: dict[str, Any] = {} + + class FakeRunnable: + def invoke(self, _messages: Any) -> dict[str, list[Any]]: + return {"semantic_steps": [], "allocation_groups": []} + + class FakeChatOpenAI: + def __init__(self, **kwargs: Any) -> None: + captured.update(kwargs) + + def with_structured_output( + self, + _schema: dict[str, Any], + **_kwargs: Any, + ) -> FakeRunnable: + return FakeRunnable() + + monkeypatch.setattr(langchain_openai, "ChatOpenAI", FakeChatOpenAI) + monkeypatch.setattr( + planner_module, + "_load_llm_settings", + lambda *, model: { + "api_key": "test-key", + "model": model or "test-model", + "base_url": "https://example.test/v1", + "default_query": {}, + }, + ) + + planner_module._default_llm_caller(prompt="plan", model="test-model") + + assert captured["http_socket_options"] == () + + +def test_structured_output_transport_selects_json_mode_only_for_mimo() -> None: + calls: list[dict[str, Any]] = [] + + class FakeClient: + def with_structured_output(self, schema: dict[str, Any], **kwargs: Any) -> str: + calls.append({"schema": schema, "kwargs": kwargs}) + return "structured" + + schema = {"type": "object"} + client = FakeClient() + mimo = planner_module._structured_output_runnable( + client, + schema, + settings={ + "model": "mimo-v2.5", + "base_url": "https://token-plan-cn.xiaomimimo.com/v1", + }, + ) + generic = planner_module._structured_output_runnable( + client, + schema, + settings={"model": "gpt-test", "base_url": "https://example.test/v1"}, + ) + + assert mimo == generic == "structured" + assert [call["kwargs"] for call in calls] == [ + {"method": "json_mode"}, + {"method": "json_schema"}, + ] diff --git a/tests/gen_sim/action_engine/task_fixtures.py b/tests/gen_sim/action_engine/task_fixtures.py new file mode 100644 index 000000000..001b4a91f --- /dev/null +++ b/tests/gen_sim/action_engine/task_fixtures.py @@ -0,0 +1,448 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Language-neutral structured fixtures for Action Engine tests.""" + +from __future__ import annotations + +from typing import Any + +from embodichain.gen_sim.action_engine.domain import ( + task_success_type, + validate_scene_requirements, + validate_task_spec, +) +from embodichain.gen_sim.action_engine.protocol import ( + SCENE_REQUIREMENTS_SCHEMA, + TASK_SPEC_SCHEMA, +) + +__all__ = [ + "TASK2_1_HISTORICAL_ROLE_BINDINGS", + "TASK2_1_HISTORICAL_SCENE_FINGERPRINT", + "make_task2_1_historical_spec", + "make_task_level", + "make_task_spec", +] + + +TASK2_1_HISTORICAL_ROLE_BINDINGS = { + "object_01": "interact_purple_soda_can", + "object_02": "interact_orange_soda_can", + "object_03": "interact_spiral_notebook", +} +"""Runtime role bindings from clean v14 run ``20260820_233507``.""" + +TASK2_1_HISTORICAL_SCENE_FINGERPRINT = { + "config_sha256": "1042967c9b7021f518e82ace62aa824015a3ad50639fa8a326b7dc0474277481", + "asset_sha256": { + "interact_orange_soda_can": ( + "ae6c8b9922d4a20746241daf0a607d29f33eb5d96f3dfa2244ffa6ba1d89f5ce" + ), + "interact_purple_soda_can": ( + "ee3c0d53d298f2be2db778d8a1227122b57b9492651e1825cd7335a8bf4cec42" + ), + "interact_spiral_notebook": ( + "25cbbf49e89da935c32ea939e523997ca538f09c4a29cff334dcbf785380afc8" + ), + "table": "99caec34e31d43e34f9326fb16c6e8660288d24e0482f463ac6d90c99368b76a", + }, +} +"""Path-independent source fingerprint for the historical Task 2-1 scene.""" + +_OBJECT_FIXTURES = { + "E1": ("can", ["graspable", "placeable"], {}), + "E2": ("can", ["graspable", "orientable"], {"orientation": "fallen"}), + "E3": ("container", ["graspable", "pourable"], {"held_by": "left_arm"}), + "E4": ("cup", ["graspable", "handover"], {}), + "E5": ("tray", ["dual_graspable", "rigid"], {}), + "E6": ("drawer", ["articulated", "pullable"], {"joint_state": "closed"}), + "E7": ("drawer", ["articulated", "pushable"], {"joint_state": "open"}), + "E8": ("knob", ["turnable"], {}), + "E9": ("button", ["pressable"], {"activation": "inactive"}), +} + + +def make_task2_1_historical_spec() -> dict[str, Any]: + """Build the deterministic ten-step Task 2-1 behavior from clean v14. + + The fixture preserves task semantics and ownership transitions from commit + ``f70138c6`` run ``20260820_233507``. It intentionally does not freeze the + Atomic Action node count or the v14 E2 lowering topology. + """ + instances = [ + { + "id": "task_01", + "task_type": "E2", + "params": { + "object_role": "object_01", + "required_arm": "right_arm", + "orientation_goal": "upright", + "support_role": "table", + "upright_local_axis": "long_axis", + }, + "depends_on": [], + "role": "primary", + }, + { + "id": "task_02", + "task_type": "E2", + "params": { + "object_role": "object_02", + "required_arm": "left_arm", + "orientation_goal": "upright", + "support_role": "table", + "upright_local_axis": "long_axis", + }, + "depends_on": [], + "role": "primary", + }, + { + "id": "task_03", + "task_type": "E4", + "params": { + "object_role": "object_02", + "transfer_arm": "left_arm", + "receive_arm": "right_arm", + "orientation_goal": "none", + }, + "depends_on": ["task_02"], + "role": "primary", + }, + { + "id": "task_04", + "task_type": "E1", + "params": { + "object_role": "object_02", + "target_role": "object_01", + "relation": "behind", + "relation_frame": "robot", + "required_arm": "right_arm", + "orientation_goal": "none", + "orientation_axis": "none", + }, + "depends_on": ["task_03"], + "role": "primary", + }, + { + "id": "task_05", + "task_type": "E4", + "params": { + "object_role": "object_01", + "transfer_arm": "right_arm", + "receive_arm": "left_arm", + "orientation_goal": "none", + }, + "depends_on": ["task_04", "task_01"], + "role": "primary", + }, + { + "id": "task_06", + "task_type": "E1", + "params": { + "object_role": "object_01", + "target_role": "object_03", + "relation": "left_of", + "relation_frame": "robot", + "required_arm": "left_arm", + "orientation_goal": "none", + "orientation_axis": "none", + }, + "depends_on": ["task_05"], + "role": "primary", + }, + { + "id": "task_07", + "task_type": "E1", + "params": { + "object_role": "object_02", + "target_role": "object_03", + "relation": "front_of", + "relation_frame": "robot", + "required_arm": "right_arm", + "orientation_goal": "none", + "orientation_axis": "none", + }, + "depends_on": ["task_04"], + "role": "primary", + }, + { + "id": "task_08", + "task_type": "E1", + "params": { + "object_role": "object_02", + "target_role": "object_03", + "relation": "on", + "relation_frame": "robot", + "required_arm": "left_arm", + "orientation_goal": "none", + "orientation_axis": "none", + }, + "depends_on": ["task_07"], + "role": "primary", + }, + { + "id": "task_09", + "task_type": "E4", + "params": { + "object_role": "object_01", + "transfer_arm": "left_arm", + "receive_arm": "right_arm", + "orientation_goal": "none", + }, + "depends_on": ["task_06"], + "role": "primary", + }, + { + "id": "task_10", + "task_type": "E1", + "params": { + "object_role": "object_01", + "target_role": "object_02", + "relation": "above", + "relation_frame": "robot", + "required_arm": "right_arm", + "orientation_goal": "none", + "orientation_axis": "none", + }, + "depends_on": ["task_09", "task_08"], + "role": "primary", + }, + ] + success_types = ( + "object_upright", + "object_upright", + "handover_complete", + "semantic_goal", + "handover_complete", + "semantic_goal", + "semantic_goal", + "semantic_goal", + "handover_complete", + "semantic_goal", + ) + return validate_task_spec( + { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": "task2_1", + "level": "L3", + "instruction": "historical-task2_1-ten-step-regression", + "reasoning_type": "none", + "task_instances": instances, + "success": { + "op": "all", + "terms": [ + {"type": success_type, "task_instance_id": instance["id"]} + for instance, success_type in zip(instances, success_types) + ], + }, + "oracle": { + "task_order": [instance["id"] for instance in instances], + "role_bindings": dict(TASK2_1_HISTORICAL_ROLE_BINDINGS), + }, + "metadata": { + "fixture": True, + "historical_commit": "f70138c626daf84918b15b954765493000cb40a5", + "historical_run": "20260820_233507", + "role_bindings": dict(TASK2_1_HISTORICAL_ROLE_BINDINGS), + }, + } + ) + + +def make_task_spec( + task_type: str = "E1", + *, + task_id: str | None = None, +) -> tuple[dict[str, Any], dict[str, Any]]: + """Build one validated L1 TaskSpec and matching scene requirements.""" + if task_type not in _OBJECT_FIXTURES: + raise ValueError(f"Unsupported fixture task type {task_type!r}.") + category, affordances, initial_state = _OBJECT_FIXTURES[task_type] + object_role = "object_01" + params: dict[str, Any] = {"object_role": object_role} + objects = [ + { + "role_id": object_role, + "category": category, + "count": 1, + "affordances": affordances, + "initial_state": initial_state, + "attributes": {}, + } + ] + if task_type in {"E1", "E3"}: + target_role = "target_01" + params.update({"target_role": target_role, "relation": "inside"}) + if task_type == "E3": + params["source_role"] = params.pop("object_role") + objects.append( + { + "role_id": target_role, + "category": "container", + "count": 1, + "affordances": ["container", "support_surface"], + "initial_state": {}, + "attributes": {}, + } + ) + elif task_type == "E2": + params.update( + { + "orientation_goal": "upright", + "support_role": "table", + "upright_local_axis": "long_axis", + } + ) + elif task_type == "E4": + params.update( + { + "transfer_arm": "left_arm", + "receive_arm": "right_arm", + "orientation_goal": "none", + "terminal_behavior": "hold", + } + ) + elif task_type == "E5": + params.update({"direction": "up", "terminal_behavior": "hold"}) + elif task_type == "E6": + params["target_state"] = "open" + elif task_type == "E7": + params["target_state"] = "closed" + elif task_type == "E8": + params["target_setting"] = 2 + elif task_type == "E9": + params["target_state"] = "activated" + + effective_id = task_id or f"fixture-{task_type.lower()}" + task = validate_task_spec( + { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": effective_id, + "level": "L1", + "instruction": "test-instruction", + "reasoning_type": "none", + "task_instances": [ + { + "id": "task_01", + "task_type": task_type, + "params": params, + "depends_on": [], + "role": "primary", + } + ], + "success": { + "type": task_success_type(task_type, params), + "task_instance_id": "task_01", + }, + "oracle": {}, + "metadata": {"fixture": True}, + } + ) + requirements = validate_scene_requirements( + { + "schema_version": SCENE_REQUIREMENTS_SCHEMA, + "task_id": effective_id, + "objects": objects, + "cameras": [], + "spatial_constraints": [ + {"type": "reachable", "roles": "all_interaction_objects"} + ], + "distractor_count": 0, + "metadata": {"fixture": True}, + } + ) + return task, requirements + + +def make_task_level( + level: str, + *, + reasoning: str | None = None, +) -> tuple[dict[str, Any], dict[str, Any]]: + """Build a validated fixture for one public TaskSpec level.""" + if level == "L1": + return make_task_spec("E1") + first, requirements = make_task_spec("E1", task_id=f"fixture-{level.lower()}") + if level == "L2": + second = { + "id": "task_02", + "task_type": "E1", + "params": { + "object_role": "object_02", + "target_role": "target_02", + "relation": "inside", + }, + "depends_on": ["task_01"], + "role": "primary", + } + first["level"] = "L2" + first["task_instances"].append(second) + first["success"] = { + "op": "all", + "terms": [ + {"type": "semantic_goal", "task_instance_id": "task_01"}, + {"type": "semantic_goal", "task_instance_id": "task_02"}, + ], + } + requirements["objects"].extend( + [ + { + "role_id": "object_02", + "category": "can", + "count": 1, + "affordances": ["graspable", "placeable"], + "initial_state": {}, + "attributes": {}, + }, + { + "role_id": "target_02", + "category": "container", + "count": 1, + "affordances": ["container", "support_surface"], + "initial_state": {}, + "attributes": {}, + }, + ] + ) + return validate_task_spec(first), validate_scene_requirements(requirements) + if level == "L4": + first["level"] = "L4" + first["reasoning_type"] = reasoning or "visual_semantics" + first["success"] = { + "visual_semantics": { + "type": "visual_relation", + "relation": "mouth_completed", + }, + "pattern": { + "type": "visual_relation", + "relation": "pattern_completed", + }, + "logic": {"type": "sum_equals", "value": 5}, + "memory": {"type": "original_order_restored"}, + "common_sense": {"type": "functional_place_setting"}, + "constraint": {"type": "stable_unobstructed"}, + }[first["reasoning_type"]] + first["oracle"] = {"fixture": True} + requirements["cameras"] = [ + { + "role": "reasoning_view", + "modalities": ["rgb", "depth"], + "coverage": "all_interaction_objects", + } + ] + return validate_task_spec(first), validate_scene_requirements(requirements) + raise ValueError(f"Unsupported fixture task level {level!r}.") diff --git a/tests/gen_sim/action_engine/tasks/__init__.py b/tests/gen_sim/action_engine/tasks/__init__.py new file mode 100644 index 000000000..d9480994f --- /dev/null +++ b/tests/gen_sim/action_engine/tasks/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +"""Action Engine task generation tests.""" diff --git a/tests/gen_sim/action_engine/tasks/test_factory.py b/tests/gen_sim/action_engine/tasks/test_factory.py new file mode 100644 index 000000000..7a8e7b8ed --- /dev/null +++ b/tests/gen_sim/action_engine/tasks/test_factory.py @@ -0,0 +1,929 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Task-first generation, instantiation, and scene hand-off contracts.""" + +from __future__ import annotations + +from copy import deepcopy +from dataclasses import replace + +import embodichain.gen_sim.action_engine.domain.task_contracts as task_contracts_module +import embodichain.gen_sim.action_engine.domain.v2 as domain_v2_module +from embodichain.gen_sim.action_engine.runtime import load_execution_program +from embodichain.gen_sim.action_engine.tasks import ( + ground_instruction_draft, + instantiate_seed_graph, +) +from tests.gen_sim.action_engine.task_fixtures import ( + TASK2_1_HISTORICAL_ROLE_BINDINGS, + TASK2_1_HISTORICAL_SCENE_FINGERPRINT, + make_task2_1_historical_spec, +) + + +def _selector( + kind: str = "none", + *, + reference: str = "", + step_id: str = "", + quantifier: str = "one", +) -> dict: + return { + "kind": kind, + "step_id": step_id, + "reference": reference, + "quantifier": quantifier, + "count": 0, + } + + +def _intent_step( + step_id: str, + task_type: str, + object_selector: dict, + **updates, +) -> dict: + step = { + "id": step_id, + "task_type": task_type, + "object": object_selector, + "target": _selector(), + "relation": "none", + "required_arm": "auto", + "transfer_arm": "none", + "receive_arm": "none", + "orientation_goal": "upright" if task_type == "E2" else "none", + "target_state": "none", + "target_setting": 0, + "layout": "none", + "axis": "none", + "direction": "none", + "terminal_behavior": "hold" if task_type == "E4" else "none", + "depends_on": [], + } + step.update(updates) + return step + + +def _ground_draft( + task_id: str, + instruction: str, + scene_objects: list[dict], + steps: list[dict], + bindings: dict[str, list[str]], +): + return ground_instruction_draft( + task_id, + instruction, + {"steps": steps}, + scene_objects, + robot_profile="ur10", + reference_bindings=bindings, + ) + + +def _historical_task2_1_graph() -> dict: + return instantiate_seed_graph( + make_task2_1_historical_spec(), + TASK2_1_HISTORICAL_ROLE_BINDINGS, + ) + + +def _actions_by_task_group(graph: dict) -> dict[str, list[str]]: + nodes = {node["id"]: node for node in graph["nodes"]} + return { + group["id"]: [nodes[node_id]["atomic_action"] for node_id in group["node_ids"]] + for group in graph["task_groups"] + } + + +def test_historical_task2_1_fixture_preserves_ten_step_semantics() -> None: + task = make_task2_1_historical_spec() + + assert [instance["task_type"] for instance in task["task_instances"]] == [ + "E2", + "E2", + "E4", + "E1", + "E4", + "E1", + "E1", + "E1", + "E4", + "E1", + ] + assert [term["task_instance_id"] for term in task["success"]["terms"]] == [ + instance["id"] for instance in task["task_instances"] + ] + assert TASK2_1_HISTORICAL_SCENE_FINGERPRINT["config_sha256"] == ( + "1042967c9b7021f518e82ace62aa824015a3ad50639fa8a326b7dc0474277481" + ) + + +def test_historical_task2_1_uses_split_upright_transport_and_handover_arms() -> None: + task = make_task2_1_historical_spec() + graph = _historical_task2_1_graph() + actions = _actions_by_task_group(graph) + + expected_orient_actions = [ + "PickUp", + "MoveHeldObject", + "MoveHeldObject", + "MoveJoints", + "MoveEndEffector", + "MoveEndEffector", + "MoveEndEffector", + "MoveEndEffector", + "MoveJoints", + ] + assert actions["task_01"] == expected_orient_actions + assert actions["task_02"] == expected_orient_actions + assert [ + ( + instance["params"]["transfer_arm"], + instance["params"]["receive_arm"], + ) + for instance in task["task_instances"] + if instance["task_type"] == "E4" + ] == [ + ("left_arm", "right_arm"), + ("right_arm", "left_arm"), + ("left_arm", "right_arm"), + ] + + +def test_historical_task2_1_handover_continuations_preserve_receiver_hold() -> None: + graph = _historical_task2_1_graph() + actions = _actions_by_task_group(graph) + groups = {group["id"]: group for group in graph["task_groups"]} + + for group_id, expected_arm in ( + ("task_04", "right_arm"), + ("task_06", "left_arm"), + ("task_10", "right_arm"), + ): + assert actions[group_id][0] == "MoveHeldObject" + assert "PickUp" not in actions[group_id] + assert groups[group_id]["contract"]["entry_requires"] == [ + { + "predicate": "object_held", + "object_uid": groups[group_id]["object_uid"], + "arm": expected_arm, + } + ] + + +def test_historical_task2_1_reacquires_objects_after_release() -> None: + graph = _historical_task2_1_graph() + actions = _actions_by_task_group(graph) + + assert actions["task_05"][0] == "PickUp" + assert actions["task_07"][0] == "PickUp" + assert actions["task_08"][0] == "PickUp" + assert actions["task_09"][0] == "PickUp" + + +def test_orient_then_handover_releases_then_reacquires_with_role_side_pickup() -> None: + task = { + "schema_version": "action_engine_task_spec_v2", + "task_id": "orient_then_handover", + "level": "L3", + "instruction": "test-instruction-orient-handover", + "reasoning_type": "none", + "task_instances": [ + { + "id": "orient", + "task_type": "E2", + "params": { + "object_role": "can", + "required_arm": "left_arm", + }, + "depends_on": [], + "role": "primary", + }, + { + "id": "handover", + "task_type": "E4", + "params": { + "object_role": "can", + "transfer_arm": "right_arm", + "receive_arm": "left_arm", + }, + "depends_on": ["orient"], + "role": "primary", + }, + ], + "success": {"type": "handover_complete"}, + "oracle": {}, + "metadata": {}, + } + + graph = instantiate_seed_graph(task, {"can": "interact_can"}) + orient_nodes = [ + node for node in graph["nodes"] if node["task_instance_id"] == "orient" + ] + handover_nodes = [ + node for node in graph["nodes"] if node["task_instance_id"] == "handover" + ] + orient = next(group for group in graph["task_groups"] if group["id"] == "orient") + + assert [node["atomic_action"] for node in orient_nodes] == [ + "PickUp", + "MoveHeldObject", + "MoveHeldObject", + "MoveJoints", + "MoveEndEffector", + "MoveEndEffector", + "MoveEndEffector", + "MoveEndEffector", + "MoveJoints", + ] + assert orient["goal"]["upright_local_axis"] == "auto" + assert orient_nodes[0]["postcondition"] == {} + assert orient_nodes[3]["postcondition"]["local_axis"] == "auto" + upright_policy = {"modifiers": [{"type": "orientation", "mode": "upright"}]} + assert orient_nodes[0]["motion_policy"] == upright_policy + assert [node["role"] for node in orient_nodes] == [ + "primary", + "primary", + "primary", + "primary", + "cleanup", + "cleanup", + "cleanup", + "cleanup", + "cleanup", + ] + assert orient_nodes[1]["target_binding"] == { + "kind": "semantic_goal", + "semantic_step": "orient", + "phase": "staging", + } + assert orient_nodes[1]["motion_policy"] == upright_policy + assert orient_nodes[2]["target_binding"] == { + "kind": "semantic_goal", + "semantic_step": "orient", + "phase": "final", + } + assert orient_nodes[2]["motion_policy"] == upright_policy + assert orient_nodes[3]["target_binding"] == { + "kind": "joint_state", + "source": "gripper_open", + "single_release": True, + } + assert orient_nodes[3]["control"] == "hand" + assert orient_nodes[4]["target_binding"] == { + "kind": "policy_pose", + "source": "release", + "operation": "lift_clear", + } + assert orient_nodes[4]["motion_policy"] == {"modifiers": []} + assert orient_nodes[5]["target_binding"] == { + "kind": "policy_pose", + "source": "release", + "operation": "reorient_tool_down", + } + assert orient_nodes[6]["target_binding"] == { + "kind": "policy_pose", + "source": "release", + "operation": "lift_clear", + "requires_arm_clear": True, + } + assert orient_nodes[7]["target_binding"] == { + "kind": "policy_pose", + "source": "release", + "operation": "retreat_after_lift", + } + assert orient_nodes[8]["target_binding"] == { + "kind": "joint_state", + "source": "initial", + "operation": "e2_home", + "required_home": True, + } + for previous, current in zip(orient_nodes, orient_nodes[1:]): + assert current["depends_on"] == [previous["id"]] + assert [node["atomic_action"] for node in handover_nodes] == [ + "PickUp", + "MoveHeldObject", + "HandOver", + "MoveEndEffector", + "MoveJoints", + "MoveHeldObject", + ] + assert handover_nodes[0]["motion_policy"] == { + "modifiers": [{"type": "handover_role", "mode": "transfer"}] + } + assert handover_nodes[0]["depends_on"] == [orient["node_ids"][-1]] + assert orient["actor"] == {"mode": "required", "arm": "left_arm"} + assert handover_nodes[0]["actor"] == {"mode": "required", "arm": "right_arm"} + assert orient_nodes[-1]["contract"]["completion"] == "terminal_barrier" + assert orient_nodes[0]["contract"]["failure_policy"] == "task_required" + assert orient_nodes[1]["contract"]["failure_policy"] == "task_required" + assert orient_nodes[2]["contract"]["failure_policy"] == "task_required" + assert orient_nodes[3]["contract"]["failure_policy"] == "task_required" + assert orient_nodes[4]["contract"]["failure_policy"] == "safety_required" + assert orient_nodes[5]["contract"]["failure_policy"] == "safety_required" + assert orient_nodes[6]["contract"]["failure_policy"] == "safety_required" + assert orient_nodes[7]["contract"]["failure_policy"] == "safety_required" + assert orient_nodes[-1]["contract"]["failure_policy"] == "safety_required" + assert orient_nodes[1]["contract"]["requires"] == [ + { + "predicate": "object_held", + "object_uid": "interact_can", + "arm": "left_arm", + } + ] + assert orient_nodes[2]["contract"]["requires"] == [ + { + "predicate": "object_held", + "object_uid": "interact_can", + "arm": "left_arm", + } + ] + assert [ + (effect["op"], effect["atom"]["predicate"]) + for effect in orient_nodes[0]["contract"]["effects"] + ] == [ + ("delete", "arm_free"), + ("delete", "object_free"), + ("add", "object_held"), + ] + assert [ + (effect["op"], effect["atom"]["predicate"]) + for effect in orient_nodes[3]["contract"]["effects"] + ] == [ + ("delete", "object_held"), + ("add", "arm_free"), + ("add", "object_free"), + ] + assert orient_nodes[3]["contract"]["requires"] == [ + { + "predicate": "object_held", + "object_uid": "interact_can", + "arm": "left_arm", + } + ] + assert orient_nodes[4]["contract"]["requires"] == [ + {"predicate": "arm_free", "arm": "left_arm"} + ] + assert orient_nodes[5]["contract"]["requires"] == [ + {"predicate": "arm_clear", "arm": "left_arm"} + ] + assert orient_nodes[6]["contract"]["requires"] == [ + {"predicate": "arm_clear", "arm": "left_arm"} + ] + assert orient_nodes[7]["contract"]["requires"] == [ + {"predicate": "arm_clear", "arm": "left_arm"} + ] + assert any( + effect["atom"]["predicate"] == "arm_home" + for effect in orient["contract"]["exit_effects"] + ) + assert any( + requirement["predicate"] == "object_free" + for requirement in handover_nodes[0]["contract"]["requires"] + ) + + +def test_pour_recipe_completes_release_and_home_without_observable_contents() -> None: + task = { + "schema_version": "action_engine_task_spec_v2", + "task_id": "pour_contents", + "level": "L1", + "instruction": "Pour the ball from the cup into the bin.", + "reasoning_type": "none", + "task_instances": [ + { + "id": "pour", + "task_type": "E3", + "params": { + "source_role": "cup", + "target_role": "bin", + "content_roles": ["unmodeled_liquid"], + "required_arm": "right_arm", + }, + "depends_on": [], + "role": "primary", + } + ], + "success": {"type": "poured"}, + "oracle": {}, + "metadata": {}, + } + + graph = instantiate_seed_graph( + task, + {"cup": "source_cup", "bin": "target_bin"}, + ) + nodes = graph["nodes"] + group = graph["task_groups"][0] + + assert [node["atomic_action"] for node in nodes] == [ + "PickUp", + "MoveHeldObject", + "Pour", + "MoveHeldObject", + "Place", + "MoveEndEffector", + "MoveJoints", + ] + assert all( + node["depends_on"] == [nodes[index - 1]["id"]] + for index, node in enumerate(nodes[1:], start=1) + ) + assert nodes[2]["contract"]["failure_policy"] == "task_required" + assert nodes[3]["target_binding"]["phase"] == "return" + assert nodes[4]["contract"]["effects"][-1]["atom"] == { + "predicate": "object_free", + "object_uid": "source_cup", + } + assert nodes[-1]["contract"]["completion"] == "terminal_barrier" + assert group["success"]["verification"] == "action_completion" + assert "contents" not in group["goal"] + assert all("payloads" not in node["target_binding"] for node in nodes) + + +def test_handover_to_place_uses_receiver_hold_without_repickup() -> None: + task = { + "schema_version": "action_engine_task_spec_v2", + "task_id": "handover_then_place", + "level": "L3", + "instruction": "test-instruction-handover-place", + "reasoning_type": "none", + "task_instances": [ + { + "id": "task_01", + "task_type": "E4", + "params": { + "object_role": "yellow_can", + "transfer_arm": "left_arm", + "receive_arm": "right_arm", + "orientation_goal": "preserve", + }, + "depends_on": [], + "role": "primary", + }, + { + "id": "task_02", + "task_type": "E1", + "params": { + "object_role": "yellow_can", + "target_role": "purple_can", + "relation": "right_of", + }, + "depends_on": ["task_01"], + "role": "primary", + }, + ], + "success": { + "op": "all", + "terms": [ + {"type": "handover_complete", "task_instance_id": "task_01"}, + {"type": "semantic_goal", "task_instance_id": "task_02"}, + ], + }, + "oracle": {"task_order": ["task_01", "task_02"]}, + "metadata": {}, + } + + graph = instantiate_seed_graph( + task, + { + "yellow_can": "interact_yellow_can", + "purple_can": "interact_purple_can", + }, + ) + + handover = next(group for group in graph["task_groups"] if group["id"] == "task_01") + placement = next( + group for group in graph["task_groups"] if group["id"] == "task_02" + ) + placement_nodes = [ + node for node in graph["nodes"] if node["task_instance_id"] == "task_02" + ] + + assert [ + node["atomic_action"] + for node in graph["nodes"] + if node["task_instance_id"] == "task_01" + ] == [ + "PickUp", + "MoveHeldObject", + "HandOver", + "MoveEndEffector", + "MoveJoints", + "MoveHeldObject", + ] + assert handover["actor"] == {"mode": "required", "arm": "left_arm"} + assert graph["nodes"][0]["motion_policy"] == { + "modifiers": [{"type": "handover_role", "mode": "transfer"}] + } + assert graph["nodes"][1]["target_binding"]["kind"] == "handover_staging" + assert graph["nodes"][2]["motion_policy"] == {"modifiers": []} + handover_retreat = graph["nodes"][3] + assert handover_retreat["actor"] == {"mode": "required", "arm": "left_arm"} + assert handover_retreat["target_binding"] == { + "kind": "policy_pose", + "source": "handover", + "operation": "retreat", + } + assert handover_retreat["motion_policy"] == {"modifiers": []} + assert handover_retreat["depends_on"] == [graph["nodes"][2]["id"]] + handover_home = graph["nodes"][4] + assert handover_home["actor"] == {"mode": "required", "arm": "left_arm"} + assert handover_home["target_binding"] == { + "kind": "joint_state", + "source": "initial", + "operation": "handover_home", + } + assert handover_home["motion_policy"] == {"modifiers": []} + assert handover_home["depends_on"] == [handover_retreat["id"]] + assert [node["atomic_action"] for node in placement_nodes] == [ + "MoveHeldObject", + "MoveHeldObject", + "Place", + "MoveEndEffector", + "MoveJoints", + ] + assert placement["actor"] == {"mode": "required", "arm": "right_arm"} + assert placement_nodes[0]["precondition"] == { + "type": "object_held", + "object": "interact_yellow_can", + "arm": "right_arm", + } + assert placement_nodes[0]["depends_on"] == [handover["node_ids"][-1]] + + +def test_e4_hold_owns_receiver_safe_exit() -> None: + task = { + "schema_version": "action_engine_task_spec_v2", + "task_id": "handover_hold", + "level": "L1", + "instruction": "Hand the can to the right arm and keep holding it.", + "reasoning_type": "none", + "task_instances": [ + { + "id": "handover", + "task_type": "E4", + "params": { + "object_role": "can", + "transfer_arm": "left_arm", + "receive_arm": "right_arm", + "terminal_behavior": "hold", + }, + "depends_on": [], + "role": "primary", + } + ], + "success": {}, + "oracle": {}, + "metadata": {}, + } + + graph = instantiate_seed_graph(task, {"can": "can_uid"}) + group = graph["task_groups"][0] + receiver_exit = graph["nodes"][-1] + + assert [node["atomic_action"] for node in graph["nodes"]] == [ + "PickUp", + "MoveHeldObject", + "HandOver", + "MoveEndEffector", + "MoveJoints", + "MoveHeldObject", + ] + assert receiver_exit["actor"] == {"mode": "required", "arm": "right_arm"} + assert receiver_exit["target_binding"]["phase"] == "handover_exit" + assert receiver_exit["target_binding"]["terminal_hold"] is True + assert group["success"]["type"] == "handover_complete" + assert group["contract"]["completion"] == "terminal_barrier" + + +def test_e4_place_owns_receiver_placement_without_e1(monkeypatch) -> None: + task = { + "schema_version": "action_engine_task_spec_v2", + "task_id": "handover_place", + "level": "L1", + "instruction": "Hand the can to the right arm and place it on the notebook.", + "reasoning_type": "none", + "task_instances": [ + { + "id": "handover_place", + "task_type": "E4", + "params": { + "object_role": "can", + "target_role": "notebook", + "relation": "on", + "transfer_arm": "left_arm", + "receive_arm": "right_arm", + "terminal_behavior": "place", + }, + "depends_on": [], + "role": "primary", + } + ], + "success": {}, + "oracle": {}, + "metadata": {}, + } + + graph = instantiate_seed_graph( + task, + {"can": "can_uid", "notebook": "notebook_uid"}, + ) + group = graph["task_groups"][0] + + assert {node["task_type"] for node in graph["nodes"]} == {"E4"} + assert len(graph["task_groups"]) == 1 + assert [node["atomic_action"] for node in graph["nodes"]] == [ + "PickUp", + "MoveHeldObject", + "HandOver", + "MoveEndEffector", + "MoveJoints", + "MoveHeldObject", + "MoveHeldObject", + "Place", + "MoveEndEffector", + "MoveJoints", + ] + assert all( + node["actor"] == {"mode": "required", "arm": "right_arm"} + for node in graph["nodes"][5:] + ) + assert group["goal"]["terminal_behavior"] == "place" + assert group["goal"]["reference_object"] == "notebook_uid" + assert group["success"] == { + "type": "semantic_goal", + "relation": "on", + "reference_object": "notebook_uid", + } + + renamed = deepcopy(graph) + renamed["task_groups"][0]["operator"] = "equivalent_transfer_and_place" + program = load_execution_program(renamed) + assert program.semantic_steps[0].operator == "equivalent_transfer_and_place" + assert [ + action["atomic_action_class"] + for edge in program.edges + for action in edge.actions + ] == [node["atomic_action"] for node in graph["nodes"]] + + alias_contract = replace( + task_contracts_module.task_contract("E4"), + task_type="transfer_and_place", + ) + monkeypatch.setattr( + domain_v2_module, + "TASK_TYPES", + frozenset({*domain_v2_module.TASK_TYPES, "transfer_and_place"}), + ) + monkeypatch.setattr( + task_contracts_module, + "TASK_CONTRACTS", + { + **dict(task_contracts_module.TASK_CONTRACTS), + "transfer_and_place": alias_contract, + }, + ) + renamed_type = deepcopy(graph) + for node in renamed_type["nodes"]: + node["task_type"] = "transfer_and_place" + renamed_type["task_groups"][0]["task_type"] = "transfer_and_place" + + alias_program = load_execution_program(renamed_type) + assert [ + action["atomic_action_class"] + for edge in alias_program.edges + for action in edge.actions + ] == [node["atomic_action"] for node in graph["nodes"]] + + +def test_structured_draft_grounds_handover_then_receiver_placement() -> None: + scene = [ + { + "runtime_uid": "table", + "role": "background", + "description": "A table.", + "init_pos": [0.0, 0.0, 0.0], + }, + { + "runtime_uid": "interact_yellow_can", + "role": "rigid_object", + "description": "A yellow soda can.", + "init_pos": [0.0, -0.25, 0.75], + }, + { + "runtime_uid": "interact_purple_can", + "role": "rigid_object", + "description": "A purple soda can.", + "init_pos": [0.0, 0.25, 0.75], + }, + ] + + planned = _ground_draft( + "handover_then_place", + "test-instruction-handover-place", + scene, + [ + _intent_step( + "handover", + "E4", + _selector("scene_ref", reference="object-alpha"), + transfer_arm="left_arm", + receive_arm="right_arm", + ), + _intent_step( + "place", + "E1", + _selector("step_result", step_id="handover"), + target=_selector("scene_ref", reference="object-beta"), + relation="right_of", + required_arm="right_arm", + ), + ], + { + "handover.object": ["interact_yellow_can"], + "place.target": ["interact_purple_can"], + }, + ) + graph = instantiate_seed_graph(planned.task_spec, planned.role_bindings) + + assert planned.task_spec["level"] == "L3" + assert [item["task_type"] for item in planned.task_spec["task_instances"]] == [ + "E4", + "E1", + ] + assert planned.role_bindings == { + "object_01": "interact_yellow_can", + "object_02": "interact_purple_can", + } + assert [node["atomic_action"] for node in graph["nodes"]] == [ + "PickUp", + "MoveHeldObject", + "HandOver", + "MoveEndEffector", + "MoveJoints", + "MoveHeldObject", + "MoveHeldObject", + "MoveHeldObject", + "Place", + "MoveEndEffector", + "MoveJoints", + ] + assert graph["task_groups"][1]["goal"]["relation"] == "right_of" + assert graph["task_groups"][1]["goal"]["relation_frame"] == "robot" + + +def test_seed_graph_adds_missing_same_object_e2_handover_dependency() -> None: + scene = [ + { + "runtime_uid": "purple_can", + "role": "rigid_object", + "description": "A purple soda can.", + "init_pos": [0.0, -0.25, 0.7], + }, + { + "runtime_uid": "orange_can", + "role": "rigid_object", + "description": "An orange soda can.", + "init_pos": [0.0, 0.2, 0.7], + }, + ] + planned = _ground_draft( + "missing_same_object_edge", + "test-instruction-multi-step", + scene, + [ + _intent_step( + "orient_purple", + "E2", + _selector("scene_ref", reference="object-alpha"), + required_arm="right_arm", + ), + _intent_step( + "orient_orange", + "E2", + _selector("scene_ref", reference="object-beta"), + required_arm="left_arm", + depends_on=["orient_purple"], + ), + _intent_step( + "handover_purple", + "E4", + _selector("step_result", step_id="orient_purple"), + transfer_arm="right_arm", + receive_arm="left_arm", + depends_on=["orient_orange"], + ), + _intent_step( + "place_purple", + "E1", + _selector("step_result", step_id="handover_purple"), + target=_selector("scene_ref", reference="object-beta"), + relation="left_of", + required_arm="left_arm", + ), + ], + { + "orient_purple.object": ["purple_can"], + "orient_orange.object": ["orange_can"], + "place_purple.target": ["orange_can"], + }, + ) + underconstrained = deepcopy(planned.task_spec) + underconstrained["task_instances"][2]["depends_on"] = ["task_02"] + + graph = instantiate_seed_graph(underconstrained, planned.role_bindings) + handover = next(group for group in graph["task_groups"] if group["id"] == "task_03") + purple = next(group for group in graph["task_groups"] if group["id"] == "task_01") + staging = next( + node + for node in graph["nodes"] + if node["task_instance_id"] == "task_03" + and node["atomic_action"] == "MoveHeldObject" + ) + assert handover["depends_on"] == ["task_02", "task_01"] + assert [ + node["atomic_action"] + for node in graph["nodes"] + if node["task_instance_id"] == "task_03" + ] == [ + "PickUp", + "MoveHeldObject", + "HandOver", + "MoveEndEffector", + "MoveJoints", + "MoveHeldObject", + ] + pickup = next( + node + for node in graph["nodes"] + if node["task_instance_id"] == "task_03" and node["atomic_action"] == "PickUp" + ) + assert purple["node_ids"][-1] in pickup["depends_on"] + assert staging["depends_on"] == [pickup["id"]] + + +def test_structured_draft_treats_table_as_support_in_generic_line_task() -> None: + scene = [ + { + "runtime_uid": "table", + "role": "background", + "description": "A table.", + "init_pos": [0.0, 0.0, 0.0], + }, + { + "runtime_uid": "interact_red_can", + "role": "rigid_object", + "description": "A red soda can.", + "init_pos": [0.0, -0.25, 0.75], + }, + { + "runtime_uid": "interact_blue_cup", + "role": "rigid_object", + "description": "A blue cup.", + "init_pos": [0.0, 0.25, 0.75], + }, + ] + + planned = _ground_draft( + "arrange_line", + "test-instruction-line", + scene, + [ + _intent_step( + "line", + "E1", + _selector( + "scene_ref", + reference="object-set", + quantifier="all", + ), + layout="line", + ) + ], + {"line.object": ["interact_red_can", "interact_blue_cup"]}, + ) + graph = instantiate_seed_graph(planned.task_spec, planned.role_bindings) + + assert planned.task_spec["level"] == "L2" + assert set(planned.role_bindings.values()) == { + "interact_red_can", + "interact_blue_cup", + } + assert all(group["operator"] == "arrange_line" for group in graph["task_groups"]) diff --git a/tests/gen_sim/action_engine/tasks/test_grounding.py b/tests/gen_sim/action_engine/tasks/test_grounding.py new file mode 100644 index 000000000..e927c5fb6 --- /dev/null +++ b/tests/gen_sim/action_engine/tasks/test_grounding.py @@ -0,0 +1,339 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from copy import deepcopy +import json + +import pytest + +from embodichain.gen_sim.action_engine.tasks.grounding import ( + ground_scene_references, +) +from embodichain.gen_sim.action_engine.tasks.assembly import SceneInventory + + +def _selector( + reference: str, + *, + quantifier: str = "one", + count: int = 0, +) -> dict: + return { + "kind": "scene_ref", + "step_id": "", + "reference": reference, + "quantifier": quantifier, + "count": count, + } + + +def _scene() -> list[dict]: + return [ + { + "runtime_uid": "table", + "uid": "table", + "role": "background", + "category": "dining_table", + "name": "work table", + "description": "A rectangular work table.", + "init_pos": [0.0, 0.0, 0.0], + }, + { + "runtime_uid": "cutting_board", + "uid": "cutting_board", + "role": "rigid_object", + "category": "cutting_board", + "name": "wood board", + "description": "A large rectangular wooden cutting board.", + "attributes": { + "size": "large", + "geometry": {"position": [0.0, 0.2, 0.7], "note": "flat"}, + }, + "initial_state": {"orientation": "fallen"}, + "init_pos": [0.0, 0.2, 0.7], + }, + { + "runtime_uid": "salt_shaker", + "uid": "salt_shaker", + "role": "rigid_object", + "category": "salt_shaker", + "description": "A small glass salt shaker.", + "affordances": ["graspable"], + "init_pos": [0.0, -0.2, 0.7], + }, + ] + + +def _intent( + *, + object_selector: dict | None = None, + target_selector: dict | None = None, +) -> dict: + return { + "steps": [ + { + "id": "move", + "task_type": "E1", + "object": object_selector or _selector("object-alpha"), + "target": target_selector or _selector("target-alpha"), + "relation": "on", + } + ] + } + + +def _binding( + reference_id: str, + uids: list[str], + *, + status: str = "resolved", + confidence: float = 1.0, + **extra: object, +) -> dict: + return { + "reference_id": reference_id, + "status": status, + "uids": uids, + "confidence": confidence, + **extra, + } + + +def _run(intent: dict, caller) -> object: + scene = _scene() + return ground_scene_references( + instruction="test-instruction", + intent=intent, + inventory=SceneInventory(scene, robot_profile="franka"), + scene_objects=scene, + model="test-model", + caller=caller, + ) + + +def test_grounding_prompt_preserves_open_semantics_and_redacts_geometry() -> None: + captured: dict[str, object] = {} + + def caller(**kwargs): + captured.update(kwargs) + return { + "bindings": [ + _binding("move.object", ["cutting_board"]), + _binding("move.target", ["table"]), + ] + } + + result = _run(_intent(), caller) + + prompt = str(captured["prompt"]) + assert result.bindings == { + "move.object": ("cutting_board",), + "move.target": ("table",), + } + assert '"category": "cutting_board"' in prompt + assert '"category": "salt_shaker"' in prompt + assert '"name": "wood board"' in prompt + assert '"orientation": "fallen"' in prompt + assert '"size": "large"' in prompt + prompt_inventory = json.loads(prompt.split("Redacted scene inventory:\n", 1)[1]) + side_by_uid = {item["uid"]: item["side"] for item in prompt_inventory} + assert side_by_uid["cutting_board"] == "right" + assert side_by_uid["salt_shaker"] == "left" + assert '"position"' not in prompt + assert '"init_pos"' not in prompt + + +@pytest.mark.parametrize("robot_profile", ["ur5", "ur10", "franka"]) +def test_scene_inventory_uses_the_shared_final_world_lateral_axis( + robot_profile: str, +) -> None: + inventory = SceneInventory(_scene(), robot_profile=robot_profile) + + assert inventory.left_score(inventory.by_uid["salt_shaker"]) > 0.0 + assert inventory.left_score(inventory.by_uid["cutting_board"]) < 0.0 + + +def test_grounding_repairs_one_invalid_uid_in_the_same_batch() -> None: + responses = [ + { + "bindings": [ + _binding("move.object", ["invented"]), + _binding("move.target", ["table"]), + ] + }, + { + "bindings": [ + _binding("move.object", ["cutting_board"]), + _binding("move.target", ["table"]), + ] + }, + ] + prompts: list[str] = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return deepcopy(responses[len(prompts) - 1]) + + result = _run(_intent(), caller) + + assert result.attempts == 2 + assert "previous grounding JSON failed" in prompts[1] + assert result.bindings["move.object"] == ("cutting_board",) + + +@pytest.mark.parametrize( + "response,error", + [ + ( + { + "bindings": [ + _binding( + "move.object", + ["cutting_board"], + status="ambiguous", + ), + _binding("move.target", ["table"]), + ] + }, + "was not resolved", + ), + ( + { + "bindings": [ + _binding( + "move.object", + [], + status="not_found", + confidence=0.0, + ), + _binding("move.target", ["table"]), + ] + }, + "was not resolved", + ), + ( + { + "bindings": [ + _binding("move.object", ["cutting_board"], confidence=0.49), + _binding("move.target", ["table"]), + ] + }, + "confidence is below", + ), + ( + { + "bindings": [ + _binding("move.object", ["cutting_board", "cutting_board"]), + _binding("move.target", ["table"]), + ] + }, + "duplicate UIDs", + ), + ( + { + "bindings": [ + _binding("move.object", ["cutting_board"]), + _binding("move.object", ["salt_shaker"]), + _binding("move.target", ["table"]), + ] + }, + "Duplicate grounding binding", + ), + ( + { + "bindings": [ + _binding("move.object", ["cutting_board", "salt_shaker"]), + _binding("move.target", ["table"]), + ] + }, + "quantifier=one requires exactly one UID", + ), + ( + {"bindings": [_binding("move.object", ["cutting_board"])]}, + "omitted requests", + ), + ( + { + "bindings": [ + _binding("move.object", ["table"]), + _binding("move.target", ["cutting_board"]), + ] + }, + "candidate range", + ), + ( + { + "bindings": [ + _binding("move.object", ["cutting_board"]), + _binding("move.target", ["cutting_board"]), + ] + }, + "same UID", + ), + ( + { + "bindings": [ + _binding( + "move.object", + ["cutting_board"], + affordances=["graspable"], + ), + _binding("move.target", ["table"]), + ] + }, + "unsupported", + ), + ], +) +def test_grounding_fails_closed_after_one_repair(response: dict, error: str) -> None: + with pytest.raises(ValueError, match=f"after one repair.*{error}"): + _run(_intent(), lambda **_kwargs: deepcopy(response)) + + +def test_grounding_enforces_count_and_accepts_an_open_world_set() -> None: + intent = _intent( + object_selector=_selector("object-set", quantifier="count", count=2) + ) + response = { + "bindings": [ + _binding("move.object", ["cutting_board", "salt_shaker"]), + _binding("move.target", ["table"]), + ] + } + + result = _run(intent, lambda **_kwargs: response) + assert result.bindings["move.object"] == ("cutting_board", "salt_shaker") + + invalid = deepcopy(response) + invalid["bindings"][0]["uids"] = ["cutting_board"] + with pytest.raises(ValueError, match="requires exactly 2 UIDs"): + _run(intent, lambda **_kwargs: invalid) + + +def test_grounding_accepts_a_nonempty_all_binding() -> None: + intent = _intent(object_selector=_selector("object-set", quantifier="all")) + response = { + "bindings": [ + _binding("move.object", ["cutting_board", "salt_shaker"]), + _binding("move.target", ["table"]), + ] + } + + result = _run(intent, lambda **_kwargs: response) + + assert result.bindings["move.object"] == ("cutting_board", "salt_shaker") diff --git a/tests/gen_sim/action_engine/tasks/test_interpretation.py b/tests/gen_sim/action_engine/tasks/test_interpretation.py new file mode 100644 index 000000000..9932625b9 --- /dev/null +++ b/tests/gen_sim/action_engine/tasks/test_interpretation.py @@ -0,0 +1,2070 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from copy import deepcopy + +import pytest + +import embodichain.gen_sim.action_engine.tasks.interpretation as interpretation_module +import embodichain.gen_sim.task_engine.interpretation as task_interpretation_module +from embodichain.gen_sim.action_engine.tasks.assembly import SceneInventory +from embodichain.gen_sim.action_engine.tasks import ( + INSTRUCTION_INTENT_SCHEMA, + instantiate_seed_graph, + interpret_and_ground_task_spec, + validate_instruction_intent, +) + + +def _selector(kind: str = "none", **values): + legacy_kind = kind + if kind == "selector": + kind = "scene_ref" + reference = values.pop("reference", "") + if legacy_kind == "selector": + uid = str(values.pop("uid", "")).strip() + legacy_terms = [ + str(values.pop(field, "")).strip() + for field in ("side", "color", "category") + ] + reference = reference or uid + if not reference: + reference = " ".join( + term for term in legacy_terms if term not in {"", "none"} + ) + result = { + "kind": kind, + "step_id": "", + "reference": reference, + "quantifier": "one", + "count": 0, + } + result.update(values) + return result + + +def _grounding(**bindings): + return { + "bindings": [ + { + "reference_id": reference_id, + "status": "resolved", + "uids": [uid] if isinstance(uid, str) else list(uid), + "confidence": 1.0, + } + for reference_id, uid in bindings.items() + ] + } + + +def _grounding_caller(**bindings): + response = _grounding(**bindings) + return lambda **_kwargs: deepcopy(response) + + +def _step(step_id: str, task_type: str, object_selector: dict, **values): + result = { + "id": step_id, + "task_type": task_type, + "object": object_selector, + "target": _selector(), + "relation": "none", + "required_arm": "auto", + "transfer_arm": "none", + "receive_arm": "none", + "orientation_goal": "upright" if task_type == "E2" else "none", + "target_state": "none", + "target_setting": 0, + "layout": "none", + "axis": "none", + "direction": "none", + "terminal_behavior": "hold" if task_type == "E4" else "none", + "depends_on": [], + } + result.update(values) + return result + + +def _scene(): + return [ + { + "runtime_uid": "purple_can", + "uid": "purple_can", + "role": "rigid_object", + "description": "A purple soda can.", + "init_pos": [0.0, -0.25, 0.7], + }, + { + "runtime_uid": "orange_can", + "uid": "orange_can", + "role": "rigid_object", + "description": "An orange soda can.", + "init_pos": [0.0, 0.2, 0.7], + }, + ] + + +def _scene_with_table(): + return [ + { + "runtime_uid": "table", + "uid": "table", + "role": "background", + "description": "A table.", + "init_pos": [0.0, 0.0, 0.0], + }, + *_scene(), + ] + + +def _scene_export_style_scene(): + return [ + { + "runtime_uid": "table", + "uid": "table", + "role": "background", + "description": "A light grey dining table.", + "init_pos": [0.0, 0.0, 0.0], + }, + { + "runtime_uid": "carrot_001", + "uid": "carrot_001", + "role": "rigid_object", + "category": "carrot", + "description": ( + "A single orange carrot with a green top located at the top left " + "of the table." + ), + "init_pos": [0.28, 0.47, 1.06], + }, + { + "runtime_uid": "cutting_board_001", + "uid": "cutting_board_001", + "role": "rigid_object", + "category": "cutting_board", + "description": ( + "A rectangular cutting board located in the upper middle-left " + "area of the table." + ), + "init_pos": [0.14, 0.21, 1.07], + }, + { + "runtime_uid": "peeler_001", + "uid": "peeler_001", + "role": "rigid_object", + "category": "vegetable_peeler", + "description": "A black-handled vegetable peeler.", + "init_pos": [-0.13, -0.61, 1.07], + }, + ] + + +def _payload_scene(): + return [ + { + "runtime_uid": "glue_stick", + "uid": "glue_stick", + "role": "object", + "category": "glue_stick", + "description": "A solid glue stick.", + "init_pos": [0.0, -0.2, 0.7], + }, + { + "runtime_uid": "paper_cup", + "uid": "paper_cup", + "role": "object", + "category": "cup", + "description": "A paper cup.", + "init_pos": [0.0, 0.0, 0.7], + }, + { + "runtime_uid": "popcorn_bucket", + "uid": "popcorn_bucket", + "role": "object", + "category": "bucket", + "description": "A popcorn bucket.", + "init_pos": [0.0, 0.25, 0.7], + }, + ] + + +def _handover_intent(): + return { + "steps": [ + _step( + "orient", + "E2", + _selector("scene_ref", reference="object-alpha"), + required_arm="right_arm", + ), + _step( + "handover", + "E4", + _selector("step_result", step_id="orient"), + transfer_arm="right_arm", + receive_arm="left_arm", + depends_on=["orient"], + ), + _step( + "place", + "E1", + _selector("step_result", step_id="handover"), + target=_selector("scene_ref", reference="object-beta"), + relation="left_of", + required_arm="left_arm", + depends_on=["handover"], + ), + ] + } + + +def _two_object_handover_intent_with_missing_place_target(): + return { + "steps": [ + _step( + "orient_purple", + "E2", + _selector("scene_ref", reference="object-alpha"), + required_arm="right_arm", + ), + _step( + "orient_orange", + "E2", + _selector("scene_ref", reference="object-beta"), + required_arm="left_arm", + depends_on=["orient_purple"], + ), + _step( + "handover", + "E4", + _selector("step_result", step_id="orient_purple"), + transfer_arm="right_arm", + receive_arm="left_arm", + depends_on=["orient_orange"], + ), + _step( + "place", + "E1", + _selector("step_result", step_id="handover"), + relation="left_of", + required_arm="left_arm", + depends_on=["handover"], + ), + ] + } + + +def _two_object_handover_intent(): + intent = _two_object_handover_intent_with_missing_place_target() + intent["steps"][3]["target"] = _selector( + "scene_ref", + reference="object-beta", + ) + return intent + + +def test_llm_intent_handles_handover_pronoun_and_elliptical_place() -> None: + calls = [] + + def caller(**kwargs): + calls.append(kwargs) + return _handover_intent() + + grounded = interpret_and_ground_task_spec( + "handover_task", + "instruction-marker", + _scene(), + robot_profile="ur10", + model="test-model", + caller=caller, + grounding_caller=_grounding_caller( + **{ + "orient.object": "purple_can", + "place.target": "orange_can", + } + ), + ) + graph = instantiate_seed_graph(grounded.task_spec, grounded.role_bindings) + + assert [item["task_type"] for item in grounded.task_spec["task_instances"]] == [ + "E2", + "E4", + "E1", + ] + assert grounded.role_bindings == { + "object_01": "purple_can", + "object_02": "orange_can", + } + assert ( + grounded.task_spec["task_instances"][0]["params"]["upright_local_axis"] + == "auto" + ) + placement_actions = [ + node["atomic_action"] for node in graph["nodes"] if node["task_type"] == "E1" + ] + orient_actions = [ + node["atomic_action"] for node in graph["nodes"] if node["task_type"] == "E2" + ] + handover_nodes = [node for node in graph["nodes"] if node["task_type"] == "E4"] + assert orient_actions == [ + "PickUp", + "MoveHeldObject", + "MoveHeldObject", + "MoveJoints", + "MoveEndEffector", + "MoveEndEffector", + "MoveEndEffector", + "MoveEndEffector", + "MoveJoints", + ] + assert [node["atomic_action"] for node in handover_nodes] == [ + "PickUp", + "MoveHeldObject", + "HandOver", + "MoveEndEffector", + "MoveJoints", + "MoveHeldObject", + ] + assert handover_nodes[0]["motion_policy"] == { + "modifiers": [{"type": "handover_role", "mode": "transfer"}] + } + assert any( + requirement["predicate"] == "object_free" + for requirement in handover_nodes[0]["contract"]["requires"] + ) + assert placement_actions == [ + "MoveHeldObject", + "MoveHeldObject", + "Place", + "MoveEndEffector", + "MoveJoints", + ] + out_of_order = deepcopy(grounded.task_spec) + out_of_order["task_instances"] = list(reversed(out_of_order["task_instances"])) + reordered_graph = instantiate_seed_graph( + out_of_order, + grounded.role_bindings, + ) + assert [group["task_type"] for group in reordered_graph["task_groups"]] == [ + "E2", + "E4", + "E1", + ] + assert "instruction-marker" in calls[0]["prompt"] + assert calls[0]["model"] == "test-model" + + +def test_complete_handover_place_intent_emits_one_e4_task() -> None: + intent = { + "steps": [ + _step( + "handover_place", + "E4", + _selector("scene_ref", reference="object-alpha"), + target=_selector("scene_ref", reference="object-beta"), + relation="on", + transfer_arm="left_arm", + receive_arm="right_arm", + terminal_behavior="place", + ) + ] + } + prompts: list[str] = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return deepcopy(intent) + + grounded = interpret_and_ground_task_spec( + "complete_handover_place", + "Use the left arm to hand the can to the right arm, then place it on the notebook.", + _scene(), + robot_profile="ur10", + caller=caller, + grounding_caller=_grounding_caller( + **{ + "handover_place.object": "purple_can", + "handover_place.target": "orange_can", + } + ), + ) + graph = instantiate_seed_graph(grounded.task_spec, grounded.role_bindings) + + assert [item["task_type"] for item in grounded.task_spec["task_instances"]] == [ + "E4" + ] + assert {node["task_type"] for node in graph["nodes"]} == {"E4"} + assert graph["task_groups"][0]["goal"]["terminal_behavior"] == "place" + assert "do not emit a trailing E1" in prompts[0] + + +def test_six_step_repeated_objects_preserve_two_handover_continuations() -> None: + intent = { + "steps": [ + _step( + "orient_purple", + "E2", + _selector("scene_ref", reference="object-alpha"), + required_arm="right_arm", + ), + _step( + "orient_orange", + "E2", + _selector("scene_ref", reference="object-beta"), + required_arm="left_arm", + ), + _step( + "handover_orange", + "E4", + _selector("step_result", step_id="orient_orange"), + transfer_arm="left_arm", + receive_arm="right_arm", + depends_on=["orient_orange"], + ), + _step( + "place_orange", + "E1", + _selector("step_result", step_id="handover_orange"), + target=_selector("scene_ref", reference="target-gamma"), + relation="on", + required_arm="right_arm", + depends_on=["handover_orange"], + ), + _step( + "handover_purple", + "E4", + _selector("step_result", step_id="orient_purple"), + transfer_arm="right_arm", + receive_arm="left_arm", + # The model may preserve only the object-lineage dependency. + # Stable lowering must not let this step leapfrog an earlier + # placement that releases its transfer arm. + depends_on=["orient_purple"], + ), + _step( + "place_purple", + "E1", + _selector("step_result", step_id="handover_purple"), + target=_selector("scene_ref", reference="object-beta"), + relation="on", + required_arm="left_arm", + depends_on=["handover_purple"], + ), + ] + } + scene = [ + *_scene(), + { + "runtime_uid": "notebook", + "uid": "notebook", + "role": "rigid_object", + "description": "A spiral notebook.", + "init_pos": [0.2, 0.0, 0.7], + }, + ] + + grounded = interpret_and_ground_task_spec( + "two_handover_task", + "test-instruction-multi-step", + scene, + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: deepcopy(intent), + grounding_caller=_grounding_caller( + **{ + "orient_purple.object": "purple_can", + "orient_orange.object": "orange_can", + "place_orange.target": "notebook", + "place_purple.target": "orange_can", + } + ), + ) + assert [ + instance["task_type"] for instance in grounded.task_spec["task_instances"] + ] == ["E2", "E2", "E4", "E1", "E4", "E1"] + graph = instantiate_seed_graph(grounded.task_spec, grounded.role_bindings) + groups = {group["id"]: group for group in graph["task_groups"]} + actions_by_group = { + group_id: [ + node["atomic_action"] + for node in graph["nodes"] + if node["task_instance_id"] == group_id + ] + for group_id in groups + } + + assert actions_by_group["task_04"][0] == "MoveHeldObject" + assert "PickUp" not in actions_by_group["task_04"] + assert actions_by_group["task_05"][0] == "PickUp" + assert actions_by_group["task_06"][0] == "MoveHeldObject" + assert "PickUp" not in actions_by_group["task_06"] + assert groups["task_04"]["contract"]["entry_requires"] == [ + { + "predicate": "object_held", + "object_uid": "orange_can", + "arm": "right_arm", + } + ] + assert groups["task_06"]["contract"]["entry_requires"] == [ + { + "predicate": "object_held", + "object_uid": "purple_can", + "arm": "left_arm", + } + ] + + +def test_e5_relative_transport_emits_pickment_and_optional_release() -> None: + scene = [ + { + "runtime_uid": "table", + "uid": "table", + "role": "background", + "category": "table", + "description": "table", + "init_pos": [0.0, 0.0, 0.0], + }, + { + "runtime_uid": "plastic_tray", + "uid": "plastic_tray", + "role": "object", + "category": "tray", + "description": "plastic tray", + "init_pos": [0.0, 0.0, 0.7], + }, + { + "runtime_uid": "banana_left", + "uid": "banana_left", + "role": "object", + "category": "banana", + "description": "left banana", + "init_pos": [0.0, 0.25, 0.7], + }, + ] + intent = { + "steps": [ + _step( + "move_tray", + "E5", + _selector("selector", uid="plastic_tray"), + target=_selector("selector", uid="banana_left"), + relation="behind", + direction="none", + terminal_behavior="hold", + ) + ] + } + grounded = interpret_and_ground_task_spec( + "dual_tray", + "test-instruction-relative-transport", + scene, + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller( + **{ + "move_tray.object": "plastic_tray", + "move_tray.target": "banana_left", + } + ), + ) + + graph = instantiate_seed_graph(grounded.task_spec, grounded.role_bindings) + assert [node["atomic_action"] for node in graph["nodes"]] == ["CoordinatedPickment"] + assert graph["task_groups"][0]["operator"] == "coordinated_transport" + assert graph["task_groups"][0]["goal"] == { + "direction": "none", + "terminal_behavior": "hold", + "orientation_goal": "none", + "orientation_axis": "none", + "relation_frame": "robot", + "reference_object": "banana_left", + "reference_state": "live", + "relation": "behind", + } + + released_spec = deepcopy(grounded.task_spec) + released_spec["task_instances"][0]["params"]["terminal_behavior"] = "place" + released = instantiate_seed_graph(released_spec, grounded.role_bindings) + assert [node["atomic_action"] for node in released["nodes"]] == [ + "CoordinatedPickment", + "MoveJoints", + "MoveJoints", + "MoveEndEffector", + "MoveEndEffector", + "MoveJoints", + "MoveJoints", + ] + release_nodes = released["nodes"][1:3] + assert all( + node["depends_on"] == [released["nodes"][0]["id"]] for node in release_nodes + ) + assert {node["actor"]["arm"] for node in release_nodes} == { + "left_arm", + "right_arm", + } + assert {node["control"] for node in release_nodes} == {"hand"} + assert len({node["sync_group"] for node in release_nodes}) == 1 + assert all(node["precondition"] == {} for node in release_nodes) + assert { + node["target_binding"]["coordinated_release_role"] for node in release_nodes + } == {"participant", "commit"} + contracts = { + node["target_binding"]["coordinated_release_role"]: node["contract"] + for node in release_nodes + } + coordinated_hold = { + "predicate": "object_coordinated_held", + "object_uid": "plastic_tray", + } + assert contracts["participant"]["requires"] == [coordinated_hold] + assert contracts["participant"]["effects"] == [] + assert contracts["commit"]["requires"] == [coordinated_hold] + assert { + ( + effect["op"], + effect["atom"]["predicate"], + effect["atom"].get("arm"), + ) + for effect in contracts["commit"]["effects"] + } == { + ("delete", "object_coordinated_held", None), + ("add", "object_free", None), + ("add", "arm_free", "left_arm"), + ("add", "arm_free", "right_arm"), + } + lift_nodes = released["nodes"][3:5] + release_ids = {node["id"] for node in release_nodes} + assert {node["actor"]["arm"] for node in lift_nodes} == { + "left_arm", + "right_arm", + } + assert all(node["role"] == "cleanup" for node in lift_nodes) + assert all(node["control"] == "arm" for node in lift_nodes) + assert all(set(node["depends_on"]) == release_ids for node in lift_nodes) + assert all( + node["target_binding"] + == { + "kind": "policy_pose", + "source": "release", + "operation": "lift_clear", + "verify_lift_clear": True, + } + for node in lift_nodes + ) + assert all( + node["contract"]["requires"] + == [{"predicate": "arm_free", "arm": node["actor"]["arm"]}] + for node in lift_nodes + ) + assert all( + node["contract"]["effects"] + == [ + { + "op": "add", + "atom": {"predicate": "arm_clear", "arm": node["actor"]["arm"]}, + } + ] + for node in lift_nodes + ) + assert all( + node["contract"]["failure_policy"] == "safety_required" for node in lift_nodes + ) + home_nodes = released["nodes"][5:] + lift_ids = {node["id"] for node in lift_nodes} + assert {node["actor"]["arm"] for node in home_nodes} == { + "left_arm", + "right_arm", + } + assert all(node["role"] == "cleanup" for node in home_nodes) + assert all(node["control"] == "arm" for node in home_nodes) + assert all(set(node["depends_on"]) == lift_ids for node in home_nodes) + assert all( + node["target_binding"] + == { + "kind": "joint_state", + "source": "initial", + "operation": "e5_home", + "required_home": True, + } + for node in home_nodes + ) + assert all( + node["contract"]["requires"] + == [{"predicate": "arm_clear", "arm": node["actor"]["arm"]}] + for node in home_nodes + ) + assert all( + node["contract"]["failure_policy"] == "safety_required" for node in home_nodes + ) + from embodichain.gen_sim.action_engine.runtime import load_execution_program + + program = load_execution_program(released) + assert [ + action["atomic_action_class"] + for edge in program.edges + for action in edge.actions + ] == [ + "CoordinatedPickment", + "MoveJoints", + "MoveJoints", + "MoveEndEffector", + "MoveEndEffector", + "MoveJoints", + "MoveJoints", + ] + assert len(program.edges[1].actions) == 2 + assert all(len(edge.actions) == 1 for edge in program.edges[2:]) + + in_place_spec = deepcopy(released_spec) + in_place_params = in_place_spec["task_instances"][0]["params"] + in_place_params.pop("target_role") + in_place_params.update({"direction": "none", "relation": "none"}) + in_place = instantiate_seed_graph(in_place_spec, grounded.role_bindings) + assert [node["atomic_action"] for node in in_place["nodes"]] == [ + "CoordinatedPickment", + "MoveJoints", + "MoveJoints", + "MoveEndEffector", + "MoveEndEffector", + "MoveJoints", + "MoveJoints", + ] + assert "reference_object" not in in_place["task_groups"][0]["goal"] + + +def test_e5_accepts_generic_rigid_object_without_exported_affordances() -> None: + scene = [ + { + "runtime_uid": "interact_wooden_block", + "uid": "interact_wooden_block", + "role": "rigid_object", + "description": "A long rectangular wooden block.", + "init_pos": [0.0, 0.0, 0.7], + } + ] + intent = { + "steps": [ + _step( + "move_block", + "E5", + _selector("selector", uid="interact_wooden_block"), + direction="left", + terminal_behavior="hold", + ) + ] + } + + grounded = interpret_and_ground_task_spec( + "dual_block", + "test-instruction-directional-transport", + scene, + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller( + **{"move_block.object": "interact_wooden_block"} + ), + ) + + instance = grounded.task_spec["task_instances"][0] + assert instance["task_type"] == "E5" + assert grounded.role_bindings[instance["params"]["object_role"]] == ( + "interact_wooden_block" + ) + assert instance["params"]["direction"] == "left" + + +def test_task1_2_open_reference_generates_coordinated_pick_move_and_release() -> None: + scene = [ + { + "runtime_uid": "table", + "uid": "table", + "role": "background", + "description": "A white table.", + "init_pos": [0.0, 0.0, 0.0], + }, + { + "runtime_uid": "interact_apple", + "uid": "interact_apple", + "role": "rigid_object", + "description": "A red apple.", + "init_pos": [0.0, -0.2, 0.7], + }, + { + "runtime_uid": "interact_wooden_tray", + "uid": "interact_wooden_tray", + "role": "rigid_object", + "description": "A long rectangular wooden tray.", + "init_pos": [0.0, 0.0, 0.7], + }, + { + "runtime_uid": "interact_rubiks_cube", + "uid": "interact_rubiks_cube", + "role": "rigid_object", + "description": "A Rubik's cube.", + "init_pos": [0.0, 0.2, 0.7], + }, + ] + intent = { + "steps": [ + _step( + "move_block", + "E5", + _selector("scene_ref", reference="object-alpha"), + direction="left", + terminal_behavior="place", + ) + ] + } + + grounded = interpret_and_ground_task_spec( + "task1_2", + "test-instruction-directional-place", + scene, + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller( + **{"move_block.object": "interact_wooden_tray"} + ), + ) + graph = instantiate_seed_graph(grounded.task_spec, grounded.role_bindings) + + instance = grounded.task_spec["task_instances"][0] + assert instance["params"]["direction"] == "left" + assert instance["params"]["terminal_behavior"] == "place" + assert grounded.task_spec["success"]["terms"] == [ + {"type": "semantic_goal", "task_instance_id": instance["id"]} + ] + assert [node["atomic_action"] for node in graph["nodes"]] == [ + "CoordinatedPickment", + "MoveJoints", + "MoveJoints", + "MoveEndEffector", + "MoveEndEffector", + "MoveJoints", + "MoveJoints", + ] + assert grounded.scene_requirements["objects"][0]["category"] == "rigid_object" + assert grounded.task_spec["metadata"]["instruction_call_count"] == 1 + assert grounded.task_spec["metadata"]["scene_grounding_call_count"] == 1 + + +def test_e5_pick_and_hold_defaults_missing_direction_to_up() -> None: + scene = [ + { + "runtime_uid": "table", + "uid": "table", + "role": "background", + "description": "A wooden table.", + "init_pos": [0.0, 0.0, 0.0], + }, + { + "runtime_uid": "wooden_tray", + "uid": "wooden_tray", + "role": "rigid_object", + "description": "A shallow round wooden serving tray.", + "init_pos": [0.0, 0.0, 0.7], + }, + ] + intent = { + "steps": [ + _step( + "lift_tray", + "E5", + _selector("scene_ref", reference="object-alpha"), + required_arm="none", + direction="none", + terminal_behavior="hold", + ) + ] + } + + grounded = interpret_and_ground_task_spec( + "lift_tray", + "test-instruction-hold", + scene, + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: deepcopy(intent), + grounding_caller=_grounding_caller(**{"lift_tray.object": "wooden_tray"}), + ) + graph = instantiate_seed_graph(grounded.task_spec, grounded.role_bindings) + + instance = grounded.task_spec["task_instances"][0] + assert instance["params"]["direction"] == "up" + assert instance["params"]["terminal_behavior"] == "hold" + assert [node["atomic_action"] for node in graph["nodes"]] == ["CoordinatedPickment"] + assert grounded.task_spec["success"]["terms"] == [ + {"type": "held_by_both_grippers", "task_instance_id": instance["id"]} + ] + assert grounded.task_spec["metadata"]["instruction_call_count"] == 1 + assert grounded.task_spec["metadata"]["instruction_intent_normalizations"] == [ + { + "path": "steps[0].direction", + "from": "none", + "to": "up", + "reason": "e5_hold_defaults_to_lift", + } + ] + + +@pytest.mark.parametrize( + ("scene_update", "error"), + ( + ({"affordances": ["rigid"]}, "missing affordances.*dual_graspable"), + ({"role": "articulation"}, "requires .*rigid.object structure"), + ), +) +def test_e5_rejects_explicitly_incompatible_scene_evidence( + scene_update: dict, + error: str, +) -> None: + scene_object = { + "runtime_uid": "candidate", + "uid": "candidate", + "role": "rigid_object", + "description": "A candidate object.", + "init_pos": [0.0, 0.0, 0.7], + **scene_update, + } + intent = { + "steps": [ + _step( + "move_candidate", + "E5", + _selector("selector", uid="candidate"), + direction="left", + terminal_behavior="hold", + ) + ] + } + + with pytest.raises(ValueError, match=error): + interpret_and_ground_task_spec( + "invalid_dual_object", + "test-instruction-missing-object", + [scene_object], + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller( + **{"move_candidate.object": "candidate"} + ), + ) + + +@pytest.mark.parametrize( + ("scene_update", "should_succeed", "error"), + ( + ({"role": "articulation"}, True, ""), + ( + {"role": "articulation", "affordances": ["articulated"]}, + False, + "missing affordances.*pullable", + ), + ({"role": "rigid_object"}, False, "requires articulation structure"), + ), +) +def test_articulated_task_uses_structural_and_explicit_affordance_evidence( + scene_update: dict, + should_succeed: bool, + error: str, +) -> None: + scene_object = { + "runtime_uid": "cabinet_part", + "uid": "cabinet_part", + "description": "A cabinet moving part.", + "init_pos": [0.0, 0.0, 0.7], + **scene_update, + } + intent = { + "steps": [ + _step( + "open_part", + "E6", + _selector("scene_ref", reference="object-alpha"), + target_state="open", + ) + ] + } + + invoke = lambda: interpret_and_ground_task_spec( + "open_part", + "test-instruction-articulation", + [scene_object], + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller(**{"open_part.object": "cabinet_part"}), + ) + if should_succeed: + assert invoke().task_spec["task_instances"][0]["task_type"] == "E6" + else: + with pytest.raises(ValueError, match=error): + invoke() + + +def test_open_container_target_is_allowed_until_runtime_when_metadata_is_unknown() -> ( + None +): + scene = [ + { + "runtime_uid": "source_pitcher", + "uid": "source_pitcher", + "role": "rigid_object", + "category": "ceramic_pitcher", + "description": "A ceramic pitcher with water.", + "init_pos": [0.0, -0.2, 0.7], + }, + { + "runtime_uid": "custom_receiver", + "uid": "custom_receiver", + "role": "rigid_object", + "category": "handmade_vessel", + "description": "A handmade receiving vessel.", + "init_pos": [0.0, 0.2, 0.7], + }, + ] + intent = { + "steps": [ + _step( + "pour", + "E3", + _selector("scene_ref", reference="object-alpha"), + target=_selector("scene_ref", reference="target-alpha"), + relation="above", + ) + ] + } + + grounded = interpret_and_ground_task_spec( + "open_container", + "test-instruction-pour", + scene, + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller( + **{ + "pour.object": "source_pitcher", + "pour.target": "custom_receiver", + } + ), + ) + assert grounded.task_spec["task_instances"][0]["task_type"] == "E3" + + explicit = deepcopy(scene) + explicit[1]["affordances"] = ["support_surface"] + with pytest.raises(ValueError, match="none support containment"): + interpret_and_ground_task_spec( + "explicit_non_container", + "test-instruction-pour", + explicit, + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller( + **{ + "pour.object": "source_pitcher", + "pour.target": "custom_receiver", + } + ), + ) + + +def test_open_scene_reference_is_not_limited_by_fixed_selector_fields() -> None: + intent = { + "steps": [ + _step( + "orient", + "E2", + _selector("scene_ref", reference="object-alpha"), + ) + ] + } + grounded = interpret_and_ground_task_spec( + "open_reference", + "test-instruction-orient", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller(**{"orient.object": "purple_can"}), + ) + assert grounded.role_bindings == {"object_01": "purple_can"} + + +def test_intent_rejects_atomic_actions_coordinates_and_extra_fields() -> None: + intent = _handover_intent() + intent["steps"][0]["atomic_action"] = "PickUp" + with pytest.raises(ValueError, match="forbidden fields"): + validate_instruction_intent(intent) + + intent = _handover_intent() + intent["steps"][0]["object"]["target_pose"] = [0.0, 0.0, 0.0] + with pytest.raises(ValueError, match="forbidden fields"): + validate_instruction_intent(intent) + + +def test_invalid_intent_gets_one_repair_attempt() -> None: + responses = [{"steps": []}, _handover_intent()] + prompts = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return deepcopy(responses[len(prompts) - 1]) + + grounded = interpret_and_ground_task_spec( + "repair", + "test-instruction-repair", + _scene(), + robot_profile="ur10", + caller=caller, + grounding_caller=_grounding_caller( + **{ + "orient.object": "purple_can", + "place.target": "orange_can", + } + ), + ) + assert len(prompts) == 2 + assert "previous JSON was invalid" in prompts[1] + assert grounded.task_spec["metadata"]["instruction_call_count"] == 2 + + +def test_interpreter_normalizes_registry_inapplicable_e4_required_arm() -> None: + intent = _handover_intent() + intent["steps"][1]["required_arm"] = "right_arm" + + with pytest.raises(ValueError, match="uses transfer_arm/receive_arm"): + validate_instruction_intent(intent) + + grounded = interpret_and_ground_task_spec( + "normalized_handover", + "test-instruction-handover-repair", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: deepcopy(intent), + grounding_caller=_grounding_caller( + **{ + "orient.object": "purple_can", + "place.target": "orange_can", + } + ), + ) + + assert grounded.task_spec["metadata"]["instruction_call_count"] == 1 + assert grounded.task_spec["metadata"]["instruction_intent_normalizations"] == [ + { + "path": "steps[1].required_arm", + "from": "right_arm", + "to": "none", + "reason": "inapplicable_for_E4", + } + ] + + +def test_interpreter_does_not_infer_handover_arms_from_adjacent_tasks() -> None: + intent = { + "steps": [ + _step( + "orient_coke", + "E2", + _selector("scene_ref", reference="object-alpha"), + required_arm="right_arm", + ), + _step( + "orient_sprite", + "E2", + _selector("scene_ref", reference="object-beta"), + required_arm="left_arm", + ), + _step( + "handover_sprite", + "E4", + _selector("step_result", step_id="orient_sprite"), + required_arm="none", + transfer_arm="left_arm", + receive_arm="left_arm", + depends_on=["orient_sprite"], + ), + _step( + "place_sprite", + "E1", + _selector("step_result", step_id="handover_sprite"), + target=_selector("step_result", step_id="orient_coke"), + relation="on", + required_arm="right_arm", + depends_on=["orient_coke", "handover_sprite"], + ), + ] + } + + with pytest.raises(ValueError, match="transfer and receive arms must differ"): + validate_instruction_intent(intent) + + with pytest.raises(ValueError, match="failed validation after one repair"): + task_interpretation_module.interpret_instruction_draft( + "test-instruction-invalid-handover", + model="test-model", + caller=lambda **_kwargs: deepcopy(intent), + ) + + +def test_interpreter_repairs_direct_reference_handover_from_later_arm_semantics() -> ( + None +): + invalid_intent = { + "steps": [ + _step( + "orient_sprite", + "E2", + _selector("scene_ref", reference="object-beta"), + required_arm="left_arm", + ), + _step( + "handover_sprite", + "E4", + _selector("scene_ref", reference="object-beta"), + required_arm="none", + transfer_arm="left_arm", + receive_arm="left_arm", + depends_on=["orient_sprite"], + ), + _step( + "place_sprite", + "E1", + _selector("scene_ref", reference="object-beta"), + target=_selector("scene_ref", reference="object-alpha"), + relation="on", + required_arm="right_arm", + depends_on=["handover_sprite"], + ), + ] + } + repaired_intent = deepcopy(invalid_intent) + repaired_intent["steps"][1]["receive_arm"] = "right_arm" + prompts: list[str] = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return deepcopy(invalid_intent if len(prompts) == 1 else repaired_intent) + + result = task_interpretation_module.interpret_instruction_draft( + "test-instruction-same-arm-handover", + model="test-model", + caller=caller, + ) + + handover = result.intent["steps"][1] + assert (handover["transfer_arm"], handover["receive_arm"]) == ( + "left_arm", + "right_arm", + ) + assert result.attempts == 2 + assert result.normalizations == () + assert "Same-arm handover repair rule" in prompts[1] + + +def test_interpreter_does_not_merge_repeated_scene_reference_identity() -> None: + intent = { + "steps": [ + _step( + "orient_first_can", + "E2", + _selector("scene_ref", reference="object-token"), + required_arm="left_arm", + ), + _step( + "handover_second_can", + "E4", + _selector("scene_ref", reference="object-token"), + required_arm="none", + transfer_arm="left_arm", + receive_arm="left_arm", + depends_on=["orient_first_can"], + ), + _step( + "place_first_can", + "E1", + _selector("scene_ref", reference="object-token"), + target=_selector("scene_ref", reference="target-alpha"), + relation="on", + required_arm="right_arm", + depends_on=["handover_second_can"], + ), + ] + } + calls = 0 + + def caller(**_kwargs): + nonlocal calls + calls += 1 + return deepcopy(intent) + + with pytest.raises(ValueError, match="after one repair.*arms must differ"): + task_interpretation_module.interpret_instruction_draft( + "test-instruction-repeated-reference", + model="test-model", + caller=caller, + ) + + assert calls == 2 + + +def test_interpreter_does_not_guess_an_unconstrained_same_arm_handover() -> None: + intent = { + "steps": [ + _step( + "handover", + "E4", + _selector("scene_ref", reference="object-beta"), + transfer_arm="left_arm", + receive_arm="left_arm", + ) + ] + } + calls = 0 + + def caller(**_kwargs): + nonlocal calls + calls += 1 + return deepcopy(intent) + + with pytest.raises(ValueError, match="after one repair.*arms must differ"): + task_interpretation_module.interpret_instruction_draft( + "test-instruction-invalid-same-arm", + model="test-model", + caller=caller, + ) + + assert calls == 2 + + +def test_invalid_step_result_gets_repair_with_selector_rules() -> None: + """A malformed cross-step selector should reach the structured repair call.""" + invalid_intent = _handover_intent() + invalid_intent["steps"][1]["object"]["reference"] = "object-alpha" + responses = [invalid_intent, _handover_intent()] + prompts: list[str] = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return deepcopy(responses[len(prompts) - 1]) + + grounded = interpret_and_ground_task_spec( + "repair_step_result", + "test-instruction-step-result-repair", + _scene(), + robot_profile="ur10", + caller=caller, + grounding_caller=_grounding_caller( + **{ + "orient.object": "purple_can", + "place.target": "orange_can", + } + ), + ) + + assert len(prompts) == 2 + repair_prompt = prompts[1] + for term in ("step_result", "step_id", "reference"): + assert term in repair_prompt + assert "none" in repair_prompt + assert grounded.task_spec["metadata"]["instruction_call_count"] == 2 + + +def test_repeated_missing_e1_target_fails_without_local_guessing() -> None: + invalid_intent = _two_object_handover_intent_with_missing_place_target() + prompts: list[str] = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return deepcopy(invalid_intent) + + with pytest.raises(ValueError, match="after one repair"): + interpret_and_ground_task_spec( + "missing_target", + "test-instruction-missing-target", + _scene(), + robot_profile="ur10", + caller=caller, + ) + + assert len(prompts) == 2 + assert "Missing-target repair rule" in prompts[1] + + +def test_missing_e3_target_repair_preserves_pour_semantics() -> None: + invalid = { + "steps": [ + _step( + "pour", + "E3", + _selector("scene_ref", reference="the source cup"), + relation="above", + ) + ] + } + prompts: list[str] = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return deepcopy(invalid) + + with pytest.raises(ValueError, match="after one repair"): + task_interpretation_module.interpret_instruction_draft( + "Grab the cup and pour its contents into the bin.", + model="test-model", + caller=caller, + ) + + assert "exactly one E3 step" in prompts[0] + assert "Missing-target repair rule for E3" in prompts[1] + assert "part of the same E3 task" in prompts[1] + + +def test_missing_e6_object_reaches_targeted_repair() -> None: + invalid = { + "steps": [ + _step( + "open_drawer", + "E6", + _selector(), + target_state="open", + required_arm="right_arm", + ) + ] + } + repaired = deepcopy(invalid) + repaired["steps"][0]["object"] = _selector("scene_ref", reference="the drawer") + prompts: list[str] = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return deepcopy(invalid if len(prompts) == 1 else repaired) + + result = task_interpretation_module.interpret_instruction_draft( + "Open the drawer with the right arm.", + model="test-model", + caller=caller, + ) + + assert result.attempts == 2 + assert result.intent == repaired + assert "Opening or pulling out a drawer is E6" in prompts[0] + assert "Missing-object repair rule" in prompts[1] + + +def test_missing_target_completion_rejects_other_semantic_disagreement() -> None: + invalid_intent = _two_object_handover_intent_with_missing_place_target() + invalid_intent["steps"][-1]["required_arm"] = "right_arm" + + with pytest.raises(ValueError, match="after one repair"): + interpret_and_ground_task_spec( + "unsafe_target_completion", + "test-instruction-missing-target", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: deepcopy(invalid_intent), + ) + + +def test_second_invalid_intent_fails_without_rule_fallback() -> None: + with pytest.raises(ValueError, match="after one repair"): + interpret_and_ground_task_spec( + "invalid", + "test-instruction-invalid", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: {"steps": []}, + ) + + +def test_intent_infers_pronoun_dependency_from_canonical_symbols() -> None: + intent = { + "steps": [ + _step( + "handover", + "E4", + _selector("scene_ref", reference="object-alpha"), + transfer_arm="right_arm", + receive_arm="left_arm", + ), + _step( + "place", + "E1", + _selector("step_result", step_id="handover"), + target=_selector("scene_ref", reference="object-beta"), + relation="left_of", + required_arm="left_arm", + depends_on=["handover"], + ), + ] + } + + grounded = interpret_and_ground_task_spec( + "implicit_dependency", + "test-instruction-pronoun-dependency", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller( + **{ + "handover.object": "purple_can", + "place.target": "orange_can", + } + ), + ) + + instances = grounded.task_spec["task_instances"] + assert [item["task_type"] for item in instances] == ["E4", "E1"] + assert instances[1]["depends_on"] == [instances[0]["id"]] + assert instances[1]["params"]["relation"] == "left_of" + assert instances[1]["params"]["required_arm"] == "left_arm" + + +def test_scene_grounding_rejects_unknown_uid() -> None: + intent = { + "steps": [ + _step( + "orient", + "E2", + _selector("scene_ref", reference="object-alpha"), + ) + ] + } + with pytest.raises(ValueError, match="after one repair.*unknown UIDs"): + interpret_and_ground_task_spec( + "unknown_uid", + "test-instruction-unknown-uid", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller(**{"orient.object": "invented_uid"}), + ) + + +def test_instruction_intent_rejects_legacy_selector_protocol() -> None: + intent = _handover_intent() + intent["steps"][0]["object"] = { + "kind": "selector", + "step_id": "", + "uid": "purple_can", + "category": "can", + "color": "purple", + "side": "none", + "quantifier": "one", + "count": 0, + } + with pytest.raises(ValueError, match="requires exactly fields"): + validate_instruction_intent(intent) + + +def test_step_result_must_reference_a_preceding_step() -> None: + intent = { + "steps": [ + _step( + "handover", + "E4", + _selector("step_result", step_id="orient"), + transfer_arm="right_arm", + receive_arm="left_arm", + ), + _step( + "orient", + "E2", + _selector("scene_ref", reference="object-alpha"), + ), + ] + } + with pytest.raises(ValueError, match="preceding step"): + validate_instruction_intent(intent) + + +def test_step_result_selector_rejects_object_constraints() -> None: + intent = _handover_intent() + intent["steps"][1]["object"]["reference"] = "object-alpha" + + with pytest.raises(ValueError, match="may identify only a prior step_id"): + validate_instruction_intent(intent) + + +def test_instruction_intent_rejects_non_e_specific_parameters() -> None: + invalid_e9 = _step( + "press", + "E9", + _selector("selector", category="button"), + target_state="activated", + orientation_goal="upright", + ) + with pytest.raises(ValueError, match="orientation_goal"): + validate_instruction_intent({"steps": [invalid_e9]}) + + invalid_line = _step( + "line", + "E1", + _selector("selector", category="can", quantifier="all"), + layout="line", + relation="on", + ) + with pytest.raises(ValueError, match="line arrangement cannot carry a relation"): + validate_instruction_intent({"steps": [invalid_line]}) + + +def test_implicit_e1_relation_requires_an_unambiguous_support_target() -> None: + intent = { + "steps": [ + _step( + "place", + "E1", + _selector("selector", category="can", color="purple"), + target=_selector("selector", category="can", color="orange"), + ) + ] + } + + with pytest.raises(ValueError, match="omitted relation"): + interpret_and_ground_task_spec( + "ambiguous_implicit_place", + "test-instruction-implicit-relation", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller( + **{ + "place.object": "purple_can", + "place.target": "orange_can", + } + ), + ) + + +def test_instruction_and_grounding_prompts_keep_their_boundaries() -> None: + captured: dict[str, dict] = {} + intent = { + "steps": [ + _step( + "place", + "E1", + _selector("selector", category="can", color="purple"), + target=_selector("selector", uid="table", category="table"), + relation="on", + ) + ] + } + + def caller(**kwargs): + captured["intent"] = kwargs + return intent + + def grounding_caller(**kwargs): + captured["grounding"] = kwargs + return _grounding(**{"place.object": "purple_can", "place.target": "table"}) + + grounded = interpret_and_ground_task_spec( + "onto_table", + "Put the purple can on the table.", + _scene_with_table(), + robot_profile="ur10", + caller=caller, + grounding_caller=grounding_caller, + ) + + assert '"uid": "table"' not in captured["intent"]["prompt"] + assert '"uid": "table"' in captured["grounding"]["prompt"] + assert '"core_actions"' not in captured["intent"]["prompt"] + assert captured["intent"]["schema"] == INSTRUCTION_INTENT_SCHEMA + assert grounded.role_bindings["object_02"] == "table" + + +def test_instruction_intent_schema_declares_every_required_selector_field() -> None: + selector_schema = INSTRUCTION_INTENT_SCHEMA["properties"]["steps"]["items"][ + "properties" + ]["object"] + + assert set(selector_schema["required"]) == set(selector_schema["properties"]) + assert "quantifier" in selector_schema["properties"] + + +def test_grounding_prompt_redacts_nested_scene_geometry() -> None: + scene = _scene() + scene[0]["attributes"] = { + "label": "purple", + "geometry": {"position": [0.0, 0.0, 0.7], "note": "can"}, + } + captured: dict[str, str] = {} + + def grounding_caller(**kwargs): + captured["prompt"] = kwargs["prompt"] + return _grounding(**{"orient.object": "purple_can"}) + + interpret_and_ground_task_spec( + "redacted_inventory", + "test-instruction-grounding-redaction", + scene, + robot_profile="ur10", + caller=lambda **_kwargs: { + "steps": [ + _step( + "orient", + "E2", + _selector("scene_ref", reference="object-alpha"), + ) + ] + }, + grounding_caller=grounding_caller, + ) + assert '"position"' not in captured["prompt"] + assert '"label": "purple"' in captured["prompt"] + + +def test_default_llm_parser_requires_the_documented_model_configuration( + monkeypatch: pytest.MonkeyPatch, +) -> None: + monkeypatch.delenv("ACTION_ENGINE_LLM_MODEL", raising=False) + monkeypatch.delenv("OPENAI_MODEL", raising=False) + monkeypatch.setattr(task_interpretation_module, "_load_local_env", lambda: {}) + + with pytest.raises(ValueError, match="text LLM model is required"): + interpret_and_ground_task_spec( + "missing_model", + "test-instruction-model-config", + _scene(), + robot_profile="ur10", + ) + + +def test_injected_caller_skips_production_model_resolution( + monkeypatch: pytest.MonkeyPatch, +) -> None: + def unexpected_model_resolution(_explicit: str | None) -> str | None: + raise AssertionError( + "injected callers must not resolve production model config" + ) + + monkeypatch.setattr( + task_interpretation_module, + "_instruction_model", + unexpected_model_resolution, + ) + + grounded = interpret_and_ground_task_spec( + "injected_caller", + "test-instruction-injected-caller", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: { + "steps": [ + _step( + "orient", + "E2", + _selector("scene_ref", reference="object-alpha"), + ) + ] + }, + grounding_caller=_grounding_caller(**{"orient.object": "purple_can"}), + ) + + assert grounded.task_spec["metadata"]["instruction_model"] == "injected_caller" + + +def test_mimo_instruction_caller_uses_json_mode_and_disables_thinking( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """MiMo-compatible endpoints must not use the lossy JSON-schema route.""" + import langchain_openai + + calls: list[dict] = [] + responses = [ + { + "steps": [ + { + "id": "orient", + "task_type": "E2", + "object": _selector("scene_ref", reference="object-alpha"), + } + ] + }, + _handover_intent(), + _grounding( + **{ + "orient.object": "purple_can", + "place.target": "orange_can", + } + ), + ] + + class FakeRunnable: + def invoke(self, messages): + calls[-1]["messages"] = messages + return deepcopy(responses.pop(0)) + + class FakeChatOpenAI: + def __init__(self, **kwargs): + calls.append({"kwargs": kwargs}) + + def with_structured_output(self, schema, **kwargs): + calls[-1]["schema"] = schema + calls[-1]["structured_kwargs"] = kwargs + return FakeRunnable() + + monkeypatch.setattr(langchain_openai, "ChatOpenAI", FakeChatOpenAI) + monkeypatch.setattr( + task_interpretation_module, + "_load_llm_settings", + lambda *, model: { + "api_key": "test-key", + "model": model or "mimo-v2.5", + "base_url": "https://token-plan-cn.xiaomimimo.com/v1", + "default_query": {}, + }, + ) + + grounded = interpret_and_ground_task_spec( + "mimo_repair", + "test-instruction-json-mode", + _scene(), + robot_profile="ur10", + model="mimo-v2.5", + ) + + assert [item["task_type"] for item in grounded.task_spec["task_instances"]] == [ + "E2", + "E4", + "E1", + ] + assert len(calls) == 3 + for call in calls: + assert call["structured_kwargs"] == {"method": "json_mode"} + assert call["kwargs"]["http_socket_options"] == () + assert call["kwargs"]["max_completion_tokens"] == 4096 + assert call["kwargs"]["extra_body"] == {"thinking": {"type": "disabled"}} + repair_messages = calls[1]["messages"] + assert "previous JSON was invalid" in repair_messages[1].content + + +def test_instruction_prompt_contains_a_complete_shape_example() -> None: + prompt = interpretation_module._instruction_prompt("instruction-marker") + selector_rules = interpretation_module._instruction_selector_rules() + assert '"target_setting": 0' in prompt + assert '"depends_on": []' in prompt + assert "every step has all 16 step keys" in prompt + assert "step_result" in prompt + assert "open scene_ref.reference" in prompt + assert "Do not classify it or emit a scene UID" in prompt + assert "example object A" in prompt + assert "stale-object-reference" not in prompt + assert "step_result" in selector_rules + assert "step_id" in selector_rules + assert "reference" in selector_rules + + +def test_scene_export_spatial_descriptions_do_not_create_false_supports() -> None: + index = SceneInventory(_scene_export_style_scene(), robot_profile="franka") + + assert [entity.uid for entity in index.support] == ["table"] + assert {entity.uid for entity in index.movable} == { + "carrot_001", + "cutting_board_001", + "peeler_001", + } + + +def test_scene_export_exact_uids_ground_pick_and_place() -> None: + intent = { + "steps": [ + _step( + "step_1", + "E1", + _selector("selector", uid="carrot_001"), + target=_selector("selector", uid="cutting_board_001"), + relation="on", + required_arm="left_arm", + ) + ] + } + + grounded = interpret_and_ground_task_spec( + "scene_export_pick_place", + "test-instruction-exact-uids", + _scene_export_style_scene(), + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: intent, + grounding_caller=_grounding_caller( + **{ + "step_1.object": "carrot_001", + "step_1.target": "cutting_board_001", + } + ), + ) + + assert set(grounded.role_bindings.values()) == { + "carrot_001", + "cutting_board_001", + } + assert grounded.task_spec["task_instances"][0]["params"]["required_arm"] == ( + "left_arm" + ) + assert {item["category"] for item in grounded.scene_requirements["objects"]} == { + "carrot", + "cutting_board", + } + + +def test_multi_object_handover_keeps_both_order_and_holder_dependencies() -> None: + intent = _two_object_handover_intent() + grounded = interpret_and_ground_task_spec( + "multi_object_handover", + "test-instruction-multi-object-handover", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: deepcopy(intent), + grounding_caller=_grounding_caller( + **{ + "orient_purple.object": "purple_can", + "orient_orange.object": "orange_can", + "place.target": "orange_can", + } + ), + ) + + instances = grounded.task_spec["task_instances"] + assert instances[2]["depends_on"] == ["task_02", "task_01"] + graph = instantiate_seed_graph(grounded.task_spec, grounded.role_bindings) + handover = next(group for group in graph["task_groups"] if group["id"] == "task_03") + handover_nodes = [ + node for node in graph["nodes"] if node["task_instance_id"] == "task_03" + ] + assert handover["depends_on"] == ["task_02", "task_01"] + assert [node["atomic_action"] for node in handover_nodes] == [ + "PickUp", + "MoveHeldObject", + "HandOver", + "MoveEndEffector", + "MoveJoints", + "MoveHeldObject", + ] + purple = next(group for group in graph["task_groups"] if group["id"] == "task_01") + orange = next(group for group in graph["task_groups"] if group["id"] == "task_02") + assert handover_nodes[0]["depends_on"] == [ + orange["node_ids"][-1], + purple["node_ids"][-1], + ] + + +def test_single_arm_e1_propagates_direct_payload_into_goal_and_contracts() -> None: + intent = { + "steps": [ + _step( + "handover_glue", + "E4", + _selector("selector", uid="glue_stick"), + required_arm="left_arm", + transfer_arm="left_arm", + receive_arm="right_arm", + ), + _step( + "place_glue", + "E1", + _selector("step_result", step_id="handover_glue"), + target=_selector("selector", uid="paper_cup"), + relation="on", + required_arm="right_arm", + depends_on=["handover_glue"], + ), + _step( + "place_cup", + "E1", + _selector("selector", uid="paper_cup"), + target=_selector("selector", uid="popcorn_bucket"), + relation="on", + required_arm="right_arm", + depends_on=["place_glue"], + ), + ] + } + grounded = interpret_and_ground_task_spec( + "payload_chain", + "test-instruction-payload-propagation", + _payload_scene(), + robot_profile="ur10", + caller=lambda **_kwargs: deepcopy(intent), + grounding_caller=_grounding_caller( + **{ + "handover_glue.object": "glue_stick", + "place_glue.target": "paper_cup", + "place_cup.object": "paper_cup", + "place_cup.target": "popcorn_bucket", + } + ), + ) + + graph = instantiate_seed_graph(grounded.task_spec, grounded.role_bindings) + carrier_group = next( + group for group in graph["task_groups"] if group["id"] == "task_03" + ) + assert carrier_group["goal"]["payloads"] == [ + {"object": "glue_stick", "slot": "center"} + ] + carrier_nodes = [ + node + for node in graph["nodes"] + if node["task_instance_id"] == carrier_group["id"] + and node["atomic_action"] in {"PickUp", "MoveHeldObject", "Place"} + ] + assert carrier_nodes + for node in carrier_nodes: + assert node["target_binding"]["payloads"] == carrier_group["goal"]["payloads"] + assert any( + claim["resource"] == "object:glue_stick" and claim["access"] == "exclusive" + for claim in node["contract"]["claims"] + ) + + +def test_seed_graph_repairs_missing_e2_handover_lifecycle_edge() -> None: + intent = _two_object_handover_intent() + grounded = interpret_and_ground_task_spec( + "missing_lifecycle_edge", + "test-instruction-lifecycle-repair", + _scene(), + robot_profile="ur10", + caller=lambda **_kwargs: deepcopy(intent), + grounding_caller=_grounding_caller( + **{ + "orient_purple.object": "purple_can", + "orient_orange.object": "orange_can", + "place.target": "orange_can", + } + ), + ) + underconstrained = deepcopy(grounded.task_spec) + underconstrained["task_instances"][2]["depends_on"] = ["task_02"] + + graph = instantiate_seed_graph(underconstrained, grounded.role_bindings) + handover = next(group for group in graph["task_groups"] if group["id"] == "task_03") + purple = next(group for group in graph["task_groups"] if group["id"] == "task_01") + assert handover["depends_on"] == ["task_02", "task_01"] + pickup = next( + node + for node in graph["nodes"] + if node["task_instance_id"] == "task_03" and node["atomic_action"] == "PickUp" + ) + assert purple["node_ids"][-1] in pickup["depends_on"] diff --git a/tests/gen_sim/action_engine/tasks/test_language_decoupling.py b/tests/gen_sim/action_engine/tasks/test_language_decoupling.py new file mode 100644 index 000000000..519a5e526 --- /dev/null +++ b/tests/gen_sim/action_engine/tasks/test_language_decoupling.py @@ -0,0 +1,428 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Acceptance tests for the structured-LLM language boundary.""" + +from __future__ import annotations + +from copy import deepcopy +from pathlib import Path + +import pytest + +import embodichain.gen_sim.action_engine.tasks as action_engine_tasks +from embodichain.gen_sim.action_engine.tasks import ( + instantiate_seed_graph, + interpret_and_ground_task_spec, + validate_instruction_intent, +) +from embodichain.gen_sim.action_engine.tasks.assembly import SceneInventory + + +def _selector( + kind: str = "none", + *, + reference: str = "", + step_id: str = "", +) -> dict: + return { + "kind": kind, + "step_id": step_id, + "reference": reference, + "quantifier": "one", + "count": 0, + } + + +def _step(step_id: str, task_type: str, reference: str, **updates: object) -> dict: + step = { + "id": step_id, + "task_type": task_type, + "object": _selector("scene_ref", reference=reference), + "target": _selector(), + "relation": "none", + "required_arm": "none", + "transfer_arm": "none", + "receive_arm": "none", + "orientation_goal": "upright" if task_type == "E2" else "preserve", + "target_state": "none", + "target_setting": 0, + "layout": "none", + "axis": "none", + "direction": "none", + "terminal_behavior": "none", + "depends_on": [], + } + step.update(updates) + return step + + +def _binding(reference_id: str, *uids: str) -> dict: + return { + "reference_id": reference_id, + "status": "resolved", + "uids": list(uids), + "confidence": 1.0, + } + + +def _grounding_caller(*bindings: dict): + response = {"bindings": list(bindings)} + return lambda **_kwargs: deepcopy(response) + + +def _open_scene() -> list[dict]: + return [ + { + "runtime_uid": "work_surface", + "uid": "work_surface", + "role": "support_surface", + "category": "obsidian_dock", + "name": "the landing ledge", + "description": "A flat black ledge used as a work surface.", + "init_pos": [0.0, 0.0, 0.0], + }, + { + "runtime_uid": "aerogel_fixture_7", + "uid": "aerogel_fixture_7", + "role": "rigid_object", + "category": "aerogel_fixture", + "name": "translucent fixture", + "description": "A translucent rectangular fixture with a frosted edge.", + "init_pos": [0.0, 0.1, 0.7], + }, + { + "runtime_uid": "plantain_marker", + "uid": "plantain_marker", + "role": "rigid_object", + "category": "plantain_marker", + "description": "A curved yellow marker behind the fixture.", + "init_pos": [0.1, -0.2, 0.7], + }, + ] + + +def test_scene_inventory_preserves_open_category_labels() -> None: + scene = _open_scene() + scene[1]["category"] = "Prototype.Fixture/V2" + inventory = SceneInventory(scene, robot_profile="franka") + + assert inventory.by_uid["aerogel_fixture_7"].category == ("Prototype.Fixture/V2") + + +@pytest.mark.parametrize( + ("step", "invalid_field"), + [ + ( + _step( + "place", + "E1", + "object-alpha", + target=_selector("scene_ref", reference="target-alpha"), + relation="invalid-relation", + ), + "relation", + ), + ( + _step("orient", "E2", "object-alpha", required_arm="invalid-arm"), + "required_arm", + ), + ( + _step( + "orient", + "E2", + "object-alpha", + orientation_goal="invalid-orientation", + ), + "orientation_goal", + ), + ], +) +def test_llm_intent_rejects_natural_language_aliases( + step: dict, + invalid_field: str, +) -> None: + """Canonical protocol fields are not a second local language parser.""" + with pytest.raises(ValueError, match=invalid_field): + validate_instruction_intent({"steps": [step]}) + + +def test_noncanonical_llm_value_is_repaired_instead_of_locally_normalized() -> None: + invalid = { + "steps": [ + _step( + "place", + "E1", + "object-alpha", + target=_selector("scene_ref", reference="target-alpha"), + relation="invalid-relation", + ) + ] + } + valid = deepcopy(invalid) + valid["steps"][0]["relation"] = "left_of" + responses = [invalid, valid] + prompts: list[str] = [] + + def caller(**kwargs): + prompts.append(kwargs["prompt"]) + return deepcopy(responses[len(prompts) - 1]) + + grounded = interpret_and_ground_task_spec( + "strict_canonical_repair", + "test-instruction-invalid-relation", + _open_scene(), + robot_profile="franka", + model="test-model", + caller=caller, + grounding_caller=_grounding_caller( + _binding("place.object", "aerogel_fixture_7"), + _binding("place.target", "work_surface"), + ), + ) + + assert len(prompts) == 2 + assert "previous JSON was invalid" in prompts[1] + assert grounded.task_spec["metadata"]["instruction_call_count"] == 2 + assert grounded.task_spec["task_instances"][0]["params"]["relation"] == ("left_of") + assert "instruction_intent_normalizations" not in grounded.task_spec["metadata"] + + +def test_two_noncanonical_llm_responses_fail_without_grounding_or_rule_fallback() -> ( + None +): + invalid = { + "steps": [ + _step( + "place", + "E1", + "object-alpha", + target=_selector("scene_ref", reference="target-alpha"), + relation="invalid-relation", + ) + ] + } + grounding_called = False + + def unexpected_grounding(**_kwargs): + nonlocal grounding_called + grounding_called = True + raise AssertionError("invalid canonical intent must not reach grounding") + + with pytest.raises(ValueError, match="after one repair.*relation"): + interpret_and_ground_task_spec( + "strict_canonical_failure", + "test-instruction-invalid-relation", + _open_scene(), + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: deepcopy(invalid), + grounding_caller=unexpected_grounding, + ) + + assert grounding_called is False + + +def test_legacy_instruction_parser_modules_and_api_are_absent() -> None: + tasks_dir = Path(action_engine_tasks.__file__).resolve().parent + + assert not (tasks_dir / "deterministic.py").exists() + assert not (tasks_dir / "planning.py").exists() + assert not hasattr(action_engine_tasks, "plan_grounded_task_spec") + + +def test_production_sources_do_not_reference_legacy_instruction_parser() -> None: + action_engine_dir = Path(action_engine_tasks.__file__).resolve().parent.parent + forbidden = ( + "tasks.deterministic", + "tasks.planning", + "plan_grounded_task_spec", + "instruction_parser", + "deterministic_fallback", + ) + offenders: dict[str, list[str]] = {} + for path in action_engine_dir.rglob("*.py"): + source = path.read_text(encoding="utf-8") + matches = [term for term in forbidden if term in source] + if matches: + offenders[str(path.relative_to(action_engine_dir))] = matches + + assert offenders == {} + + +def test_llm_caller_exception_propagates_without_scene_grounding() -> None: + expected = RuntimeError("model unavailable") + grounding_called = False + + def fail_model(**_kwargs): + raise expected + + def unexpected_grounding(**_kwargs): + nonlocal grounding_called + grounding_called = True + raise AssertionError("failed interpretation must not reach grounding") + + with pytest.raises(RuntimeError) as caught: + interpret_and_ground_task_spec( + "model_failure", + "test-instruction-caller-error", + _open_scene(), + robot_profile="franka", + model="test-model", + caller=fail_model, + grounding_caller=unexpected_grounding, + ) + + assert caught.value is expected + assert grounding_called is False + + +def test_unfamiliar_wording_and_categories_flow_through_injected_llm_stages() -> None: + intent = { + "steps": [ + _step( + "relocate_fixture", + "E1", + "object-alpha", + target=_selector("scene_ref", reference="target-alpha"), + relation="on", + required_arm="auto", + ) + ] + } + + grounded = interpret_and_ground_task_spec( + "open_world_fixture", + "test-instruction-open-reference", + _open_scene(), + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: deepcopy(intent), + grounding_caller=_grounding_caller( + _binding("relocate_fixture.object", "aerogel_fixture_7"), + _binding("relocate_fixture.target", "work_surface"), + ), + ) + + assert set(grounded.role_bindings.values()) == { + "aerogel_fixture_7", + "work_surface", + } + assert {item["category"] for item in grounded.scene_requirements["objects"]} == { + "aerogel_fixture", + "obsidian_dock", + } + assert grounded.task_spec["metadata"]["instruction_call_count"] == 1 + assert grounded.task_spec["metadata"]["scene_grounding_call_count"] == 1 + + +@pytest.mark.parametrize( + ("name", "instruction", "step", "bindings", "actions", "success"), + [ + ( + "dual_lift", + "test-instruction-hold", + _step( + "lift_fixture", + "E5", + "object-alpha", + terminal_behavior="hold", + ), + [_binding("lift_fixture.object", "aerogel_fixture_7")], + ["CoordinatedPickment"], + "held_by_both_grippers", + ), + ( + "dual_move_place", + "test-instruction-directional-place", + _step( + "move_fixture", + "E5", + "object-alpha", + direction="left", + terminal_behavior="place", + ), + [_binding("move_fixture.object", "aerogel_fixture_7")], + [ + "CoordinatedPickment", + "MoveJoints", + "MoveJoints", + "MoveEndEffector", + "MoveEndEffector", + "MoveJoints", + "MoveJoints", + ], + "semantic_goal", + ), + ( + "dual_relative", + "test-instruction-relative-place", + _step( + "move_relative", + "E5", + "object-alpha", + target=_selector("scene_ref", reference="target-alpha"), + relation="behind", + terminal_behavior="hold", + ), + [ + _binding("move_relative.object", "aerogel_fixture_7"), + _binding("move_relative.target", "plantain_marker"), + ], + ["CoordinatedPickment"], + "held_by_both_grippers", + ), + ], +) +def test_e5_symbolic_intent_reaches_the_seed_graph( + name: str, + instruction: str, + step: dict, + bindings: list[dict], + actions: list[str], + success: str, +) -> None: + grounded = interpret_and_ground_task_spec( + name, + instruction, + _open_scene(), + robot_profile="franka", + model="test-model", + caller=lambda **_kwargs: {"steps": [deepcopy(step)]}, + grounding_caller=_grounding_caller(*bindings), + ) + instance = grounded.task_spec["task_instances"][0] + graph = instantiate_seed_graph(grounded.task_spec, grounded.role_bindings) + + assert [node["atomic_action"] for node in graph["nodes"]] == actions + assert grounded.task_spec["success"]["terms"] == [ + {"type": success, "task_instance_id": instance["id"]} + ] + assert instance["params"].get("direction") == ( + "up" if name == "dual_lift" else step["direction"] + ) + if name == "dual_relative": + assert graph["task_groups"][0]["goal"]["reference_object"] == ( + "plantain_marker" + ) + assert graph["task_groups"][0]["goal"]["relation"] == "behind" + if name == "dual_move_place": + release_nodes = graph["nodes"][1:3] + assert {node["actor"]["arm"] for node in release_nodes} == { + "left_arm", + "right_arm", + } + assert len({node["sync_group"] for node in release_nodes}) == 1 diff --git a/tests/gen_sim/action_engine/test_graph_visualization.py b/tests/gen_sim/action_engine/test_graph_visualization.py new file mode 100644 index 000000000..efb1474ac --- /dev/null +++ b/tests/gen_sim/action_engine/test_graph_visualization.py @@ -0,0 +1,362 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from io import BytesIO + +from PIL import Image, ImageStat +import pytest + +from embodichain.gen_sim.action_engine.compiler import ( + compile_task_agent, + compile_task_agent_v2, +) +from embodichain.gen_sim.action_engine.domain import ( + EXECUTION_PROGRAM_SCHEMA, + MOTION_POLICY_VERSION, + TASK_AGENT_SCHEMA, + validate_execution_program, +) +from embodichain.gen_sim.action_engine.graph_visualization import ( + _RuntimeOverlay, + _dag_levels, + _dag_positions, + _dependency_pairs, + _graph_data, + render_seed_task_graph_png, + render_task_graph_png, +) + +_PNG_SIGNATURE = b"\x89PNG\r\n\x1a\n" + + +def _image(payload: bytes) -> Image.Image: + assert payload.startswith(_PNG_SIGNATURE) + image = Image.open(BytesIO(payload)).convert("RGB") + extrema = ImageStat.Stat(image).extrema + assert any(low != high for low, high in extrema) + return image + + +def _contains_color( + image: Image.Image, + color: str, + *, + minimum_pixels: int = 8, + tolerance: int = 4, +) -> bool: + target = tuple(bytes.fromhex(color.removeprefix("#"))) + matches = 0 + payload = image.tobytes() + for offset in range(0, len(payload), 3): + pixel = payload[offset : offset + 3] + if all( + abs(channel - expected) <= tolerance + for channel, expected in zip(pixel, target) + ): + matches += 1 + if matches >= minimum_pixels: + return True + return False + + +def _chain_program() -> dict[str, object]: + return compile_task_agent( + { + "schema_version": TASK_AGENT_SCHEMA, + "task": "unicode-λ-task", + "goal": "Pick up the cup and keep it hovering.", + "semantic_steps": [ + { + "id": "s01_hover", + "operator": "hold_hover", + "object": "cup", + "actor": {"mode": "required", "arm": "left_arm"}, + "goal": {}, + "depends_on": [], + } + ], + } + ) + + +def _action( + action_class: str, + arm: str | None, + target: str, +) -> dict[str, object]: + actor = {"mode": "auto"} if arm is None else {"mode": "required", "arm": arm} + return { + "atomic_action_class": action_class, + "actor": actor, + "control": "arm", + "target_binding": {"kind": "object", "object": target}, + "motion_policy": {"modifiers": []}, + } + + +def _fork_join_program() -> dict[str, object]: + program = { + "schema_version": EXECUTION_PROGRAM_SCHEMA, + "task": "fork_join_demo", + "goal_description": "Move two objects in parallel, then finish.", + "start": "v_start", + "goal": "v_goal", + "nodes": [ + {"id": "v_start", "semantic": "ready"}, + {"id": "v_left", "semantic": "left branch active"}, + {"id": "v_right", "semantic": "right branch active"}, + {"id": "v_join", "semantic": "branches complete"}, + {"id": "v_goal", "semantic": "task complete"}, + ], + "edges": [ + { + "id": "e_left_pick", + "source": "v_start", + "target": "v_left", + "semantic_step_id": "s_left", + "actions": [_action("PickUp", "left_arm", "left_object")], + "depends_on": [], + "resources": ["arm:left_arm"], + }, + { + "id": "e_right_pick", + "source": "v_start", + "target": "v_right", + "semantic_step_id": "s_right", + "actions": [_action("PickUp", "right_arm", "right_object")], + "depends_on": [], + "resources": ["arm:right_arm"], + }, + { + "id": "e_left_join", + "source": "v_left", + "target": "v_join", + "semantic_step_id": "s_left", + "actions": [_action("MoveHeldObject", "left_arm", "left_object")], + "depends_on": ["e_left_pick"], + "resources": ["arm:left_arm"], + }, + { + "id": "e_right_join", + "source": "v_right", + "target": "v_join", + "semantic_step_id": "s_right", + "actions": [_action("MoveHeldObject", "right_arm", "right_object")], + # Cross-branch dependency not implied by state continuity, so + # the renderer must draw a visible dashed dependency arrow. + "depends_on": ["e_right_pick", "e_left_pick"], + "resources": ["arm:right_arm"], + }, + { + "id": "e_finish", + "source": "v_join", + "target": "v_goal", + "semantic_step_id": "s_finish", + "actions": [_action("MoveJoints", None, "home")], + "depends_on": ["e_left_join", "e_right_join"], + "resources": ["arm:auto"], + }, + ], + "semantic_steps": [ + { + "id": "s_left", + "parent_step_id": "s_left", + "operator": "place_relative", + "object": "left_object", + "actor": {"mode": "required", "arm": "left_arm"}, + "goal": {"relation": "on"}, + "depends_on": [], + "postcondition": {"type": "semantic_goal"}, + "edge_ids": ["e_left_pick", "e_left_join"], + }, + { + "id": "s_right", + "parent_step_id": "s_right", + "operator": "place_relative", + "object": "right_object", + "actor": {"mode": "required", "arm": "right_arm"}, + "goal": {"relation": "on"}, + "depends_on": [], + "postcondition": {"type": "semantic_goal"}, + "edge_ids": ["e_right_pick", "e_right_join"], + }, + { + "id": "s_finish", + "parent_step_id": "s_finish", + "operator": "hold_hover", + "object": "home", + "actor": {"mode": "auto"}, + "goal": {}, + "depends_on": ["s_left", "s_right"], + "postcondition": {"type": "semantic_goal"}, + "edge_ids": ["e_finish"], + }, + ], + "allocation_groups": [ + { + "id": "g_parallel", + "semantic_step_ids": ["s_left", "s_right"], + "arm_constraint": "distinct_arms", + "execution_policy": "parallel_if_feasible", + "parallel_action_classes": ["PickUp"], + "workspace_policy": "shared_target_serial", + } + ], + "motion_policy_version": MOTION_POLICY_VERSION, + } + return validate_execution_program(program) + + +def test_seed_renderer_produces_a_compact_headless_png() -> None: + first = _image(render_seed_task_graph_png(_chain_program())) + second = _image(render_seed_task_graph_png(_chain_program())) + + assert first.size == second.size + assert first.width > first.height + assert first.height < 1_200 + + +def test_fork_join_layout_uses_actor_lanes_and_dependency_links() -> None: + program = _fork_join_program() + data = _graph_data(program, _RuntimeOverlay({}, {}, {})) + levels = _dag_levels(data.graph) + positions = _dag_positions( + data, + levels, + {"left": 2.6, "auto": 7.8, "right": 13.0}, + ) + + assert positions["v_left"][0] < 5.15 + assert positions["v_right"][0] > 10.45 + assert positions["v_start"][0] == pytest.approx(7.8) + assert positions["v_join"][0] == pytest.approx(7.8) + assert ("e_left_join", "e_finish") in _dependency_pairs(data) + assert ("e_right_join", "e_finish") in _dependency_pairs(data) + + image = _image(render_seed_task_graph_png(program)) + assert image.width > image.height + assert _contains_color(image, "#168A78") + assert _contains_color(image, "#D97706") + assert _contains_color(image, "#8A94A0") + + +def test_parallel_single_phase_edges_are_rendered_as_a_multigraph() -> None: + program = compile_task_agent( + { + "schema_version": TASK_AGENT_SCHEMA, + "task": "parallel_press", + "goal": "Press both independent buttons.", + "semantic_steps": [ + { + "id": "s_left", + "operator": "press", + "object": "left_button", + "actor": {"mode": "required", "arm": "left_arm"}, + "goal": {}, + "depends_on": [], + }, + { + "id": "s_right", + "operator": "press", + "object": "right_button", + "actor": {"mode": "required", "arm": "right_arm"}, + "goal": {}, + "depends_on": [], + }, + ], + } + ) + assert {(edge["source"], edge["target"]) for edge in program["edges"]} == { + ("v0_start", "v_goal") + } + + image = _image(render_seed_task_graph_png(program)) + + assert _contains_color(image, "#168A78") + assert _contains_color(image, "#D97706") + + +def test_runtime_renderer_overlays_observed_statuses() -> None: + program = _fork_join_program() + runtime = { + **program, + "runtime": { + "schema_version": "action_engine_runtime_record_v1", + "status": "failed", + "events": [ + { + "event": "edge", + "edge_id": "e_left_pick", + "arm": "left_arm", + "status": "executed", + }, + { + "event": "edge", + "edge_id": "e_right_pick", + "arm": "right_arm", + "status": "failed", + }, + ], + }, + } + + image = _image(render_task_graph_png(runtime)) + + assert _contains_color(image, "#25834B") + assert _contains_color(image, "#C43E3E") + + +def test_runtime_renderer_accepts_v2_seed_graph_envelope() -> None: + task_agent = { + "schema_version": TASK_AGENT_SCHEMA, + "task": "v2_runtime_overlay", + "goal": "Hold the cup.", + "semantic_steps": [ + { + "id": "hold", + "operator": "hold_hover", + "object": "cup", + "actor": {"mode": "required", "arm": "left_arm"}, + "goal": {}, + "depends_on": [], + } + ], + } + seed = compile_task_agent_v2(task_agent) + document = { + **seed, + "runtime": { + "schema_version": "action_engine_runtime_record_v2", + "status": "success", + "events": [], + }, + } + + image = _image(render_task_graph_png(document)) + + assert _contains_color(image, "#25834B") + + +def test_runtime_record_without_program_is_rejected() -> None: + with pytest.raises(ValueError, match="do not contain graph topology"): + render_task_graph_png( + { + "schema_version": "action_engine_runtime_record_v1", + "events": [], + } + ) diff --git a/tests/gen_sim/action_engine/test_gripper_profiles.py b/tests/gen_sim/action_engine/test_gripper_profiles.py new file mode 100644 index 000000000..bb5f9f17a --- /dev/null +++ b/tests/gen_sim/action_engine/test_gripper_profiles.py @@ -0,0 +1,129 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest + +from embodichain.gen_sim.action_engine.gripper_profiles import ( + GripperModel, + get_gripper_profile, +) + + +def test_gripper_models_are_strictly_validated() -> None: + assert get_gripper_profile("pgi").model is GripperModel.PGI + assert get_gripper_profile("robotiq").model is GripperModel.ROBOTIQ + + for invalid in ("", "PGI", "robotiq ", "unknown", None): + with pytest.raises((TypeError, ValueError), match="pgi.*robotiq"): + get_gripper_profile(invalid) # type: ignore[arg-type] + + +def test_pgi_profile_owns_asset_control_mimic_tcp_and_grasp_geometry() -> None: + profile = get_gripper_profile("pgi") + + assert profile.asset_path == "DH_PGI_140_80/DH_PGI_140_80.urdf" + assert profile.control_joint_names("left") == ("left_gripper_finger1_joint_1",) + assert profile.mimic_joint_names("left") == ("left_gripper_finger2_joint_1",) + assert profile.simulated_joint_names("left") == ( + "left_gripper_finger1_joint_1", + "left_gripper_finger2_joint_1", + ) + assert profile.open_positions == (0.0,) + assert profile.close_positions == (0.04,) + assert profile.control_limits == ((0.0, 0.04),) + assert profile.tcp_transform == ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.121), + (0.0, 0.0, 0.0, 1.0), + ) + assert profile.grasp_model.model_id == "dh_pgi_140_80" + assert profile.grasp_model.max_opening_width == pytest.approx(0.100) + assert profile.grasp_model.finger_length == pytest.approx(0.10) + assert profile.grasp_model.opening_margin == pytest.approx(0.03) + assert profile.release_open_fraction_tolerance == pytest.approx(0.03) + + +def test_robotiq_profile_separates_commanded_mimics_from_state_joint() -> None: + profile = get_gripper_profile("robotiq") + + assert profile.asset_path == ("Robotiq/robotiq_arg2f_140/robotiq_arg2f_140.urdf") + assert profile.control_joint_names("left") == ( + "left_finger_joint", + "left_inner_knuckle_joint", + "left_inner_finger_joint", + "left_right_outer_knuckle_joint", + "left_right_inner_knuckle_joint", + "left_right_inner_finger_joint", + ) + assert profile.control_joint_names("right") == ( + "right_finger_joint", + "right_left_inner_knuckle_joint", + "right_left_inner_finger_joint", + "right_outer_knuckle_joint", + "right_inner_knuckle_joint", + "right_inner_finger_joint", + ) + assert profile.mimic_joint_names("left") == ( + "left_inner_knuckle_joint", + "left_inner_finger_joint", + "left_right_outer_knuckle_joint", + "left_right_inner_knuckle_joint", + "left_right_inner_finger_joint", + ) + assert profile.state_joint_names("left") == ("left_finger_joint",) + assert profile.state_joint_names("right") == ("right_finger_joint",) + assert profile.state_joint_indices("left") == (0,) + assert profile.release_open_fraction_tolerance == pytest.approx(0.03) + assert profile.state_joint_indices("right") == (0,) + assert set(profile.state_joint_names("left")).isdisjoint( + profile.mimic_joint_names("left") + ) + assert set(profile.state_joint_names("right")).isdisjoint( + profile.mimic_joint_names("right") + ) + assert profile.open_positions == (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + assert profile.close_positions == (0.7, -0.7, 0.7, -0.7, -0.7, 0.7) + assert profile.tcp_transform == ( + (0.0, -1.0, 0.0, 0.0), + (1.0, 0.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.2), + (0.0, 0.0, 0.0, 1.0), + ) + assert profile.grasp_model.model_id == "robotiq_arg2f_140" + assert profile.grasp_model.max_opening_width == pytest.approx(0.15) + assert profile.grasp_model.finger_length == pytest.approx(0.13) + assert profile.grasp_model.opening_margin == pytest.approx(0.01) + + +def test_profile_manifest_records_tcp_frame_and_transform_conventions() -> None: + profile = get_gripper_profile("pgi") + + manifest = profile.runtime_manifest( + tcp_parent_frames={"left": "left_ee_link", "right": "right_ee_link"} + ) + + assert manifest["model"] == "pgi" + assert manifest["tcp"]["parent_frames"] == { + "left": "left_ee_link", + "right": "right_ee_link", + } + assert manifest["tcp"]["transform_direction"] == "parent_link_to_tcp" + assert manifest["tcp"]["matrix_layout"] == "row_major_homogeneous_4x4" + assert manifest["tcp"]["quaternion_order"] == "not_applicable" + assert manifest["grasp_model"]["model_id"] == "dh_pgi_140_80" diff --git a/tests/gen_sim/action_engine/test_orientation.py b/tests/gen_sim/action_engine/test_orientation.py new file mode 100644 index 000000000..e30e9567d --- /dev/null +++ b/tests/gen_sim/action_engine/test_orientation.py @@ -0,0 +1,205 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest + +from embodichain.gen_sim.action_engine.orientation import ( + AlignAxisConstraint, + MatchRotationConstraint, + compile_orientation_constraint, +) +from embodichain.gen_sim.action_engine.compiler import compile_task_agent +from embodichain.gen_sim.action_engine.protocol import ( + TASK_AGENT_SCHEMA, + TASK_SPEC_SCHEMA, +) +from embodichain.gen_sim.action_engine.tasks import instantiate_seed_graph + + +def test_unspecified_orientation_has_no_hard_constraint() -> None: + constraint = compile_orientation_constraint({}) + + assert constraint.terms == () + assert constraint.planning_preference == "minimize_rotation_from_current" + assert not constraint.requires_reference + + +def test_explicit_preserve_compiles_to_rotation_match() -> None: + constraint = compile_orientation_constraint({"orientation_goal": "preserve"}) + + assert constraint.terms == (MatchRotationConstraint(reference="step_start"),) + assert constraint.requires_reference + + +def test_match_rotation_requires_an_explicit_serialized_term() -> None: + constraint = compile_orientation_constraint( + { + "orientation_constraint": { + "terms": [ + { + "type": "match_rotation", + "reference": "target_pose", + } + ] + } + } + ) + + assert constraint.terms == (MatchRotationConstraint(reference="target_pose"),) + assert not constraint.allows_yaw_search + + +def test_upright_compiles_to_directed_axis_when_requested() -> None: + constraint = compile_orientation_constraint( + { + "orientation_goal": "upright", + "upright_local_axis": "z", + "orientation_directed": True, + } + ) + + assert constraint.terms == ( + AlignAxisConstraint( + local_axis="z", + target_axis="world_up", + directed=True, + ), + ) + + +def test_upright_rejects_non_boolean_directed_flag() -> None: + with pytest.raises(ValueError, match="orientation_directed must be a boolean"): + compile_orientation_constraint( + { + "orientation_goal": "upright", + "orientation_directed": "false", + } + ) + + +def test_legacy_long_axis_upright_remains_undirected() -> None: + constraint = compile_orientation_constraint( + { + "orientation_goal": "upright", + "upright_local_axis": "long_axis", + } + ) + + assert constraint.terms == ( + AlignAxisConstraint( + local_axis="long_axis", + target_axis="world_up", + directed=False, + ), + ) + assert constraint.allows_yaw_search + + +def test_lay_flat_compiles_to_short_axis_alignment_with_free_yaw() -> None: + constraint = compile_orientation_constraint({"orientation_goal": "lay_flat"}) + + assert constraint.terms == ( + AlignAxisConstraint( + local_axis="short_axis", + target_axis="world_up", + directed=False, + ), + ) + assert constraint.allows_yaw_search + + +def test_hold_hover_without_orientation_request_has_no_rotation_match() -> None: + compiled = compile_task_agent( + { + "schema_version": TASK_AGENT_SCHEMA, + "task": "hold", + "goal": "Hold the can above its initial position.", + "semantic_steps": [ + { + "id": "hold", + "operator": "hold_hover", + "object": "can", + "actor": {"mode": "required", "arm": "left_arm"}, + "goal": {}, + "depends_on": [], + } + ], + } + ) + + goal = compiled["semantic_steps"][0]["goal"] + assert goal["orientation_goal"] == "none" + assert compile_orientation_constraint(goal).terms == () + + +def test_serialized_constraint_keeps_term_local_tolerance() -> None: + constraint = compile_orientation_constraint( + { + "orientation_constraint": { + "terms": [ + { + "type": "align_axis", + "local_axis": "z", + "target_axis": "world_up", + "directed": True, + "tolerance": 0.1, + "scope": "terminal", + } + ] + } + } + ) + + assert constraint.terms == ( + AlignAxisConstraint( + local_axis="z", + target_axis="world_up", + directed=True, + tolerance=0.1, + ), + ) + + +def test_new_placement_without_orientation_request_has_no_hard_constraint() -> None: + task = { + "schema_version": TASK_SPEC_SCHEMA, + "task_id": "place_can", + "level": "L1", + "instruction": "Place the can beside the box.", + "reasoning_type": "none", + "task_instances": [ + { + "id": "place", + "task_type": "E1", + "params": { + "object_role": "can", + "target_role": "box", + "relation": "left_of", + }, + "depends_on": [], + "role": "primary", + } + ], + "success": {"type": "semantic_goal"}, + "oracle": {}, + "metadata": {}, + } + + graph = instantiate_seed_graph(task, {"can": "can", "box": "box"}) + + assert graph["task_groups"][0]["goal"]["orientation_goal"] == "none" diff --git a/tests/gen_sim/action_engine/test_solver_profiles.py b/tests/gen_sim/action_engine/test_solver_profiles.py new file mode 100644 index 000000000..04c1d5cc6 --- /dev/null +++ b/tests/gen_sim/action_engine/test_solver_profiles.py @@ -0,0 +1,42 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest + +from embodichain.gen_sim.action_engine.solver_profiles import ( + IK_SOLVER_MODES, + resolve_ik_solver_mode, +) + + +def test_auto_solver_preserves_current_robot_family_defaults() -> None: + assert IK_SOLVER_MODES == ("auto", "ur", "pytorch") + assert resolve_ik_solver_mode("auto", "dual_ur10") == "ur" + assert resolve_ik_solver_mode("auto", "dual_ur5") == "ur" + assert resolve_ik_solver_mode("auto", "dual_franka") == "pytorch" + + +def test_explicit_solver_mode_is_strict_and_profile_compatible() -> None: + assert resolve_ik_solver_mode("pytorch", "dual_ur10") == "pytorch" + assert resolve_ik_solver_mode("ur", "dual_ur10") == "ur" + + with pytest.raises(ValueError, match="Franka.*URSolver"): + resolve_ik_solver_mode("ur", "dual_franka") + for invalid in ("", "UR", "torch", None): + with pytest.raises((TypeError, ValueError), match="auto.*ur.*pytorch"): + resolve_ik_solver_mode(invalid, "dual_ur10") # type: ignore[arg-type]