diff --git a/agent_context/topics/simulation-system/simulation-system.md b/agent_context/topics/simulation-system/simulation-system.md index f62c19bdd..817bbf91c 100644 --- a/agent_context/topics/simulation-system/simulation-system.md +++ b/agent_context/topics/simulation-system/simulation-system.md @@ -207,6 +207,12 @@ affect FastRT/OfflineRT windows; hybrid and offscreen cameras derive internal size from their own output and quality. DexSim initializes DLSS lazily on a rendered frame, so config tests do not qualify GPU/NGX support. +On legacy engines without `dexsim.DLSSConfig` (including 0.4.3), render +configuration validates the DLSS settings but warns and skips their native +conversion. Renderer selection, sampling, OptiX denoising and tone mapping +still apply. This fallback does not enable DLSS on the legacy engine or change +the direct `DLSSCfg.to_dexsim_cfg()` API's native-support requirement. + `gym/utils/gym_utils.py:config_to_cfg()` decodes task `render_cfg.dlss` mappings into `DLSSCfg` before constructing `RenderCfg`. DLSS switches require booleans; ratio/exposure settings require real numbers, excluding booleans. diff --git a/docs/source/api_reference/embodichain/embodichain.gen_sim.task_engine.rst b/docs/source/api_reference/embodichain/embodichain.gen_sim.task_engine.rst new file mode 100644 index 000000000..f54d042c4 --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.gen_sim.task_engine.rst @@ -0,0 +1,132 @@ +embodichain.gen_sim.task_engine +================================ + +The Task Engine turns instructions and generated-scene evidence into immutable +semantic task graphs. Its execution workflow delegates Semantic Calls to the +canonical Task Program runtime; it does not ground atomic goals or issue robot +commands itself. + +Public facade +------------- + +.. automodule:: embodichain.gen_sim.task_engine + :members: + :imported-members: + :no-index: + +Interpretation and planning contracts +------------------------------------- + +.. automodule:: embodichain.gen_sim.task_engine.agent + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.cli + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.config + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.contracts + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.interpretation + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.ontology + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.state_machine + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.workflow_contracts + :members: + :no-index: + +Scene orchestration +------------------- + +These modules preserve canonical scene identity while separating generated +authoring data, conservative planning evidence, and live simulator bindings. + +.. automodule:: embodichain.gen_sim.task_engine.orchestration + :members: + :imported-members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.orchestration.artifacts + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.orchestration.contracts + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.orchestration.coordinator + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.orchestration.legacy_scene + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.orchestration.scene_adapter + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.orchestration.scene_source + :members: + :no-index: + +Scene analysis and inspection +----------------------------- + +.. automodule:: embodichain.gen_sim.task_engine.scene + :members: + :imported-members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.scene.conservative_graph + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.scene.contracts + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.scene.feasibility + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.scene.final_inspection + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.scene.scene_engine_v1 + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.scene_backend + :members: + :no-index: + +Execution workflow +------------------ + +The workflow owns run directories, bounded orchestration attempts, and +tensor-free reports. Physical execution remains behind the configured Task +Program subprocess boundary. + +.. automodule:: embodichain.gen_sim.task_engine.run_directory + :members: + :no-index: + +.. automodule:: embodichain.gen_sim.task_engine.workflow + :members: + :no-index: diff --git a/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst b/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst index a190393b6..eb85abaac 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst @@ -13,6 +13,7 @@ embodichain.lab.task_program.compiler CompiledArticulationJointPositionValidator CompiledBarrier + CompiledObjectNearRelativeTargetValidator CompiledObjectNearTargetValidator CompiledParallelBlock CompiledParallelBranch @@ -26,4 +27,3 @@ embodichain.lab.task_program.compiler CompiledTaskProgramValidator TaskProgramCompileError TaskProgramCompiler - diff --git a/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst index 42de8f47e..871c7d7e3 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst @@ -24,6 +24,7 @@ embodichain.lab.task_program BarrierCfg WaitStablePostCfg ObjectNearTargetValidatorCfg + ObjectNearRelativeTargetValidatorCfg ArticulationJointPositionValidatorCfg TaskProgramCompiler CompiledTaskProgram @@ -98,6 +99,9 @@ Schema .. autoclass:: ObjectNearTargetValidatorCfg :members: +.. autoclass:: ObjectNearRelativeTargetValidatorCfg + :members: + .. autoclass:: ArticulationJointPositionValidatorCfg :members: diff --git a/docs/source/api_reference/index.rst b/docs/source/api_reference/index.rst index 0e4c88def..cff58c95f 100644 --- a/docs/source/api_reference/index.rst +++ b/docs/source/api_reference/index.rst @@ -83,4 +83,5 @@ documentation. CI runs this same checker after style checks and before tests. .. toctree:: :maxdepth: 1 + embodichain/embodichain.gen_sim.task_engine public_api diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 253938d7a..50f1637cf 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -228,6 +228,125 @@ embodichain.gen_sim.gradio_ui.gradio_app main +embodichain.gen_sim.task_engine.orchestration.grounding +-------------------------------------------------------- + +Task-conditioned grounding selects canonical scene UIDs from a redacted +semantic inventory. It does not construct physical goals or action +invocations. + +.. currentmodule:: embodichain.gen_sim.task_engine.orchestration.grounding + +.. autosummary:: + + GroundingCaller + GroundingResult + ground_scene_references + +embodichain.gen_sim.task_engine.orchestration.scene_assets +----------------------------------------------------------- + +Generated GLB assets are normalized into renderer-safe runtime geometry while +the source files and their provenance remain unchanged. + +.. currentmodule:: embodichain.gen_sim.task_engine.orchestration.scene_assets + +.. autosummary:: + + normalize_scene_assets + +embodichain.gen_sim.task_engine.orchestration.scene_inventory +-------------------------------------------------------------- + +The structural inventory preserves source semantics and validates only +explicit task/scene compatibility. It deliberately avoids language matching +and physical goal grounding. + +.. currentmodule:: embodichain.gen_sim.task_engine.orchestration.scene_inventory + +.. autosummary:: + + SceneEntity + SceneInventory + validate_source_compatibility + validate_target_compatibility + +embodichain.gen_sim.task_engine.orchestration.source_scene +----------------------------------------------------------- + +Source-scene utilities resolve supported exports and normalize their stable +identities, paths, transforms, and conservative physics metadata for Task +Engine planning and simulator loading. + +.. currentmodule:: embodichain.gen_sim.task_engine.orchestration.source_scene + +.. autosummary:: + + PreparedScene + ResolvedSceneSource + is_prompt2scene_export + prepare_scene + resolve_gym_config_path + resolve_source_scene + +embodichain.gen_sim.task_engine.reporting +------------------------------------------ + +Tensor-free Task Engine reports retain the canonical Task Program runtime +result and its per-environment semantic outcomes without reconstructing +physical execution state. + +.. currentmodule:: embodichain.gen_sim.task_engine.reporting + +.. autosummary:: + + EXECUTION_REPORT_FILENAME + TASK_PROGRAM_EXECUTION_REPORT_SCHEMA + TaskProgramExecutionReport + validate_execution_report + write_execution_report + +embodichain.gen_sim.task_engine.semantic_graph +----------------------------------------------- + +Semantic task graphs are immutable, JSON-safe planning artifacts whose nodes +contain canonical Semantic Calls and exclude grounded action data. + +.. currentmodule:: embodichain.gen_sim.task_engine.semantic_graph + +.. autosummary:: + + SEMANTIC_TASK_GRAPH_SCHEMA + SemanticTaskGraph + semantic_task_graph_hash + validate_semantic_task_graph + +embodichain.gen_sim.task_engine.semantic_planner +------------------------------------------------- + +The semantic planner lowers validated Task Engine candidates and canonical +scene bindings into provider-free semantic task graphs. + +.. currentmodule:: embodichain.gen_sim.task_engine.semantic_planner + +.. autosummary:: + + SemanticTaskPlanner + UnsupportedSemanticCapabilityError + +embodichain.gen_sim.task_engine.task_program_bundle +---------------------------------------------------- + +Bundle generation materializes and preflights a fingerprint-bound configured +Task Program deployment from a semantic task graph and prepared scene. + +.. currentmodule:: embodichain.gen_sim.task_engine.task_program_bundle + +.. autosummary:: + + TaskProgramBundlePaths + generate_task_program_bundle + embodichain.gen_sim.scene_engine.core.scene_edit_plan ----------------------------------------------------- @@ -238,6 +357,58 @@ embodichain.gen_sim.scene_engine.core.scene_edit_plan SceneEditOperation SceneEditPlan +embodichain.gen_sim.scene_engine.errors +--------------------------------------- + +.. currentmodule:: embodichain.gen_sim.scene_engine.errors + +Scene service failures preserve typed preparation and materialization errors +across the Task Engine boundary. + +.. autosummary:: + + SceneServiceError + +embodichain.gen_sim.scene_engine.pipeline +----------------------------------------- + +.. currentmodule:: embodichain.gen_sim.scene_engine.pipeline + +The public authoring boundary separates deterministic scene analysis from +side-effecting materialization for generated and edited scenes. + +.. autosummary:: + + SCENE_BLUEPRINT_SCHEMA + SCENE_EDIT_BLUEPRINT_SCHEMA + SceneBlueprintPackage + SceneEditBlueprintPackage + SceneMaterialization + analyze_edit + analyze_image + materialize_blueprint + materialize_edit + +embodichain.gen_sim.scene_engine.pipeline.api +--------------------------------------------- + +.. currentmodule:: embodichain.gen_sim.scene_engine.pipeline.api + +Versioned blueprint artifacts and analyze/materialize operations provide the +implementation-level Scene Engine authoring contract. + +.. autosummary:: + + SCENE_BLUEPRINT_SCHEMA + SCENE_EDIT_BLUEPRINT_SCHEMA + SceneBlueprintPackage + SceneEditBlueprintPackage + SceneMaterialization + analyze_edit + analyze_image + materialize_blueprint + materialize_edit + embodichain.gen_sim.scene_engine.pipeline.editing.scene_edit_asset_preparation ------------------------------------------------------------------------------- @@ -567,6 +738,7 @@ embodichain.lab.sim.atomic_actions.control GRASP_COMMAND JointPositionCommand OPEN_COMMAND + PARK_COMMAND embodichain.lab.sim.atomic_actions.core --------------------------------------- @@ -1248,6 +1420,16 @@ embodichain.lab.sim.motion.solvers.pink_solver PinkSolver PinkSolverCfg +embodichain.lab.sim.motion.solvers.pytorch_solver +------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.motion.solvers.pytorch_solver + +.. autosummary:: + + PytorchSolverCfg + PytorchSolver + embodichain.lab.sim.motion.solvers.srs_solver --------------------------------------------- diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 92c759099..36e7f2609 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -231,9 +231,10 @@ control-part adapter resolves current joint-backed endpoints through `Robot.control_parts`; custom adapters may instead return mobile, whole-body, or other runtime targets. -`MoveJoints` is intentionally `agent_visible=False`: it is useful for home, -recovery, calibration, and scripted postures, but is not exposed to an Action -Agent by default. +`MoveJoints` is also the canonical implementation for home, recovery, +calibration, and other embodiment-named postures. Semantic callers should +constrain those uses through a registered call and keep the named target in the +robot profile. ## Shared goal and configuration rules @@ -253,7 +254,11 @@ Explicit pose tensors use `(4, 4)` or `(B, 4, 4)`. Waypoint-capable fields in `EndEffectorPoseGoal` and `PlaceGoal` also accept `(B, N, 4, 4)`. `SceneEntityPose` resolves to the latest `(B, 4, 4)` pose from each `SceneSnapshot`, checks optional perception confidence, and registers that -entity as a recovery dependency. +entity as a recovery dependency. `world_displacement` keeps a translation in +the world frame after local composition. `world_orientation` replaces the +tracked entity's rotation before applying `relative_pose`, allowing a target to +track a moving reference position while retaining a grounded world-frame +orientation. | Skill / field | `SceneEntityPose` accepted | Automatic scene-motion replan | |---|---:|---:| @@ -379,7 +384,7 @@ than an EEF pose. | Motion | joint planning/interpolation from observed qpos; supports joint waypoints | | Completion | `JOINT_GOAL_REACHED` | | Effect | none | -| Agent visibility | hidden by default (`agent_visible=False`) | +| Agent visibility | visible | `target` accepts an explicit qpos tensor with shape `(control_dof,)`, `(B, control_dof)`, or `(B, N, control_dof)`, or a non-empty string resolved diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 298ff48b1..fad0c249f 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -352,7 +352,7 @@ instances to the engine's planning services: ```python engine = AtomicActionEngine(motion_generator, control_profiles=profiles) -# All eleven built-ins are immediately usable by stable skill ID. +# All twelve built-ins are immediately usable by stable skill ID. assert "move_end_effector" in engine.actions assert "pick_up" in engine.actions ``` diff --git a/embodichain/data/assets/obj_assets.py b/embodichain/data/assets/obj_assets.py index 403549138..169f0d946 100644 --- a/embodichain/data/assets/obj_assets.py +++ b/embodichain/data/assets/obj_assets.py @@ -291,7 +291,7 @@ class Drawer(EmbodiChainDataset): def __init__(self, data_root: str = None): data_descriptor = o3d.data.DataDescriptor( os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, obj_assets, "Drawer.zip"), - "3981636db1f4188146fce25d54084612", + "eba30c852074388c2e5b634b1ae37572", ) prefix = type(self).__name__ path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root diff --git a/embodichain/gen_sim/scene_engine/cli/start.py b/embodichain/gen_sim/scene_engine/cli/start.py index 6d46f32dc..5d0c752c6 100644 --- a/embodichain/gen_sim/scene_engine/cli/start.py +++ b/embodichain/gen_sim/scene_engine/cli/start.py @@ -18,6 +18,7 @@ import argparse from collections.abc import Sequence +import math from pathlib import Path from embodichain.gen_sim.scene_engine.pipeline.generate import generate_scene_from_image @@ -31,9 +32,12 @@ def cli_scene_engine( output_root: str | Path, *, edit_prompt: str | None = None, + scene_z_rotation_degrees: float = 0.0, ) -> None: """Generate a scene from an image, edit an export, or do both in sequence.""" resolved_output_root = Path(output_root).expanduser().resolve() + if not math.isfinite(scene_z_rotation_degrees): + raise ValueError("scene_z_rotation_degrees must be finite.") if edit_prompt is not None: edit_prompt = edit_prompt.strip() if not edit_prompt: @@ -60,6 +64,7 @@ def cli_scene_engine( generate_scene_from_image( image_path=resolved_image_path, output_root=resolved_output_root, + scene_z_rotation_degrees=scene_z_rotation_degrees, ) if edit_prompt is not None: edit_scene( @@ -93,9 +98,56 @@ def main(argv: Sequence[str] | None = None) -> None: default=None, help="Text instruction for editing an existing or newly generated output root", ) + parser.add_argument( + "--scene-z-rotation-degrees", + "--scene_z_rotation_degrees", + "--prompt2scene-scene-z-rotation-degrees", + "--prompt2scene_scene_z_rotation_degrees", + dest="scene_z_rotation_degrees", + type=float, + default=0.0, + help=( + "Final counterclockwise world-Z rotation applied to the complete " + "generated scene. Defaults to 0." + ), + ) + parser.add_argument( + "--target-body-scale-mode", + "--target_body_scale_mode", + choices=("preserve",), + default="preserve", + help=( + "Compatibility option for the direct-GLB Scene Engine path; source " + "scale is always preserved." + ), + ) + parser.add_argument( + "--prompt2scene-mesh-x-rotation-degrees", + "--prompt2scene_mesh_x_rotation_degrees", + type=_zero_mesh_x_rotation, + default=0.0, + help=( + "Compatibility option for direct GLB loading. It must remain 0; " + "DexSim performs the GLTF y-up conversion without a baked 90-degree fix." + ), + ) args = parser.parse_args(argv) - cli_scene_engine(args.image, args.output_root, edit_prompt=args.edit_prompt) + cli_scene_engine( + args.image, + args.output_root, + edit_prompt=args.edit_prompt, + scene_z_rotation_degrees=args.scene_z_rotation_degrees, + ) + + +def _zero_mesh_x_rotation(value: str) -> float: + rotation = float(value) + if not math.isfinite(rotation) or rotation != 0.0: + raise argparse.ArgumentTypeError( + "prompt2scene mesh X rotation must be 0 for direct GLB loading." + ) + return rotation if __name__ == "__main__": diff --git a/embodichain/gen_sim/scene_engine/core/scene_edit_plan.py b/embodichain/gen_sim/scene_engine/core/scene_edit_plan.py index a459eb523..03b87d718 100644 --- a/embodichain/gen_sim/scene_engine/core/scene_edit_plan.py +++ b/embodichain/gen_sim/scene_engine/core/scene_edit_plan.py @@ -32,7 +32,24 @@ @dataclass(frozen=True) class SceneEditOperation: - """One normalized edit operation produced from an LLM edit draft.""" + """Describe one normalized add, move, or delete operation. + + Attributes: + op: Operation kind. Add creates a new object, move repositions an + existing object, and delete removes an existing object. + object_id: Existing object ID for move/delete, or the generated ID for + an added object. + target_id: Optional existing scene object used as the spatial target. + relation: Spatial relation between the edited object and ``target_id``. + table_region: Optional named tabletop region. It is valid only when the + target is the table and the relation is ``"on"``. + category: Semantic category required for an added object. + name: Human-readable name required for an added object. + description: Generation prompt and semantic description required for + an added object. + pose_description: Optional free-form pose intent for an added or moved + object. + """ op: SceneEditOperationType object_id: str | None = None @@ -61,7 +78,22 @@ def to_dict(self) -> dict[str, object]: @dataclass class SceneEditPlan: - """Validated operations against one immutable pre-edit scene state.""" + """Validate edit operations against one pre-edit scene state. + + Construction validates every object reference and rejects conflicting + operations without mutating the supplied scene or scene graph. + + Attributes: + scene: Scene state that exists before the edit is applied. + scene_graph: Pre-edit support and spatial-relation graph. Its node IDs + must match the scene object IDs. + operations: Normalized operations in application order. + + Raises: + ValueError: If scene IDs are inconsistent, an operation has invalid + fields or references, edits conflict, or a deletion would orphan a + support descendant. + """ scene: Scene scene_graph: SceneGraph diff --git a/embodichain/gen_sim/scene_engine/errors.py b/embodichain/gen_sim/scene_engine/errors.py new file mode 100644 index 000000000..dd23d2aa9 --- /dev/null +++ b/embodichain/gen_sim/scene_engine/errors.py @@ -0,0 +1,23 @@ +# ---------------------------------------------------------------------------- +# 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 + +__all__ = ["SceneServiceError"] + + +class SceneServiceError(RuntimeError): + """A transient or remote Scene Engine service failure.""" diff --git a/embodichain/gen_sim/scene_engine/pipeline/__init__.py b/embodichain/gen_sim/scene_engine/pipeline/__init__.py index 015c41510..ecf448d22 100644 --- a/embodichain/gen_sim/scene_engine/pipeline/__init__.py +++ b/embodichain/gen_sim/scene_engine/pipeline/__init__.py @@ -16,4 +16,26 @@ from __future__ import annotations -__all__: list[str] = [] +from .api import ( + SCENE_BLUEPRINT_SCHEMA, + SCENE_EDIT_BLUEPRINT_SCHEMA, + SceneBlueprintPackage, + SceneEditBlueprintPackage, + SceneMaterialization, + analyze_edit, + analyze_image, + materialize_blueprint, + materialize_edit, +) + +__all__ = [ + "SCENE_BLUEPRINT_SCHEMA", + "SCENE_EDIT_BLUEPRINT_SCHEMA", + "SceneBlueprintPackage", + "SceneEditBlueprintPackage", + "SceneMaterialization", + "analyze_edit", + "analyze_image", + "materialize_blueprint", + "materialize_edit", +] diff --git a/embodichain/gen_sim/scene_engine/pipeline/api.py b/embodichain/gen_sim/scene_engine/pipeline/api.py new file mode 100644 index 000000000..aaddcad55 --- /dev/null +++ b/embodichain/gen_sim/scene_engine/pipeline/api.py @@ -0,0 +1,379 @@ +# ---------------------------------------------------------------------------- +# 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 stage boundaries for Scene Engine generation and editing.""" + +from __future__ import annotations + +from copy import deepcopy +from dataclasses import dataclass +import hashlib +import json +from pathlib import Path +from typing import Any, Final + +from embodichain.gen_sim.scene_engine.clients.articulated_generation import ( + ArticulatedGenerationClient, +) +from embodichain.gen_sim.scene_engine.clients.geometry_generation import ( + GeometryGenerationClient, +) +from embodichain.gen_sim.scene_engine.clients.image_generation import ( + ImageGenerationClient, +) +from embodichain.gen_sim.scene_engine.clients.image_segmentation import ( + ImageSegmentationClient, +) +from embodichain.gen_sim.scene_engine.core.scene import Scene +from embodichain.gen_sim.scene_engine.core.scene_edit_plan import SceneEditPlan +from embodichain.gen_sim.scene_engine.core.scene_graph import SceneGraph +from embodichain.gen_sim.scene_engine.llms.openai_compatible_client import ( + OpenAICompatibleVLM, +) +from embodichain.gen_sim.scene_engine.pipeline.editing.scene_edit_asset_preparation import ( + prepare_scene_edit_assets, +) +from embodichain.gen_sim.scene_engine.pipeline.editing.scene_edit_layout_generation import ( + edit_layout, +) +from embodichain.gen_sim.scene_engine.pipeline.editing.scene_edit_understanding import ( + understand_scene_edit, +) +from embodichain.gen_sim.scene_engine.pipeline.generation.scene_generation import ( + generate_scene_and_refine, +) +from embodichain.gen_sim.scene_engine.pipeline.generation.scene_understanding import ( + understand_scene, +) +from embodichain.gen_sim.scene_engine.pipeline.utils.scene_exporter import SceneExporter +from embodichain.gen_sim.scene_engine.pipeline.utils.scene_importer import ( + SceneExportImporter, +) +from embodichain.utils.logger import log_info + +__all__ = [ + "SCENE_BLUEPRINT_SCHEMA", + "SCENE_EDIT_BLUEPRINT_SCHEMA", + "SceneBlueprintPackage", + "SceneEditBlueprintPackage", + "SceneMaterialization", + "analyze_edit", + "analyze_image", + "materialize_blueprint", + "materialize_edit", +] + +SCENE_BLUEPRINT_SCHEMA: Final = "embodichain.scene-blueprint/v2" +SCENE_EDIT_BLUEPRINT_SCHEMA: Final = "embodichain.scene-edit-blueprint/v2" + + +@dataclass(frozen=True) +class SceneBlueprintPackage: + """In-process scene semantics plus their persisted audit document.""" + + schema_version: str + blueprint_id: str + image_path: Path + output_root: Path + manifest_path: Path + scene: Scene + scene_graph: SceneGraph + + def __post_init__(self) -> None: + if self.schema_version != SCENE_BLUEPRINT_SCHEMA: + raise ValueError( + "SceneBlueprintPackage schema_version must be " + f"{SCENE_BLUEPRINT_SCHEMA!r}." + ) + + +@dataclass(frozen=True) +class SceneEditBlueprintPackage: + """Validated edit intent before added assets and layout are materialized.""" + + schema_version: str + blueprint_id: str + edit_prompt: str + output_root: Path + manifest_path: Path + scene_edit_plan: SceneEditPlan + updated_scene_graph: SceneGraph + + def __post_init__(self) -> None: + if self.schema_version != SCENE_EDIT_BLUEPRINT_SCHEMA: + raise ValueError( + "SceneEditBlueprintPackage schema_version must be " + f"{SCENE_EDIT_BLUEPRINT_SCHEMA!r}." + ) + + +@dataclass(frozen=True) +class SceneMaterialization: + """One exported materialized scene revision.""" + + scene: Scene + scene_graph: SceneGraph + output_root: Path + scene_config_path: Path + + +def analyze_image( + image_path: str | Path, + output_root: str | Path, + *, + vlm_client: OpenAICompatibleVLM | None = None, + image_segmentation_client: ImageSegmentationClient | None = None, +) -> SceneBlueprintPackage: + """Understand an image and persist the pre-generation semantic blueprint.""" + resolved_image = Path(image_path).expanduser().resolve() + resolved_output = Path(output_root).expanduser().resolve() + resolved_output.mkdir(parents=True, exist_ok=True) + effective_vlm = vlm_client or OpenAICompatibleVLM.from_dotenv() + segmentation = image_segmentation_client or ImageSegmentationClient.from_dotenv() + owns_segmentation = image_segmentation_client is None + log_info("Starting Scene Understanding") + try: + segmentation.check_health() + scene, scene_graph = understand_scene( + scene=Scene(), + image_path=resolved_image, + output_root=resolved_output, + vlm_client=effective_vlm, + image_segmentation_client=segmentation, + ) + finally: + if owns_segmentation: + segmentation.close() + log_info("Completed Scene Understanding") + + payload = { + "schema_version": SCENE_BLUEPRINT_SCHEMA, + "image_path": resolved_image.as_posix(), + "scene": scene.to_dict(), + "scene_graph": scene_graph.to_dict(), + "artifacts": _artifact_records(resolved_output / "scene_understanding"), + } + blueprint_id = _canonical_hash(payload) + document = {**payload, "blueprint_id": blueprint_id} + manifest_path = resolved_output / "scene_blueprint.json" + _write_json(manifest_path, document) + return SceneBlueprintPackage( + schema_version=SCENE_BLUEPRINT_SCHEMA, + blueprint_id=blueprint_id, + image_path=resolved_image, + output_root=resolved_output, + manifest_path=manifest_path, + scene=scene, + scene_graph=scene_graph, + ) + + +def materialize_blueprint( + blueprint: SceneBlueprintPackage, + *, + vlm_client: OpenAICompatibleVLM | None = None, + geometry_generation_client: GeometryGenerationClient | None = None, + articulated_generation_client: ArticulatedGenerationClient | None = None, +) -> SceneMaterialization: + """Generate assets and layout for one image-derived blueprint.""" + scene = deepcopy(blueprint.scene) + scene_graph = deepcopy(blueprint.scene_graph) + effective_vlm = vlm_client or OpenAICompatibleVLM.from_dotenv() + geometry = geometry_generation_client or GeometryGenerationClient.from_dotenv() + has_articulated_objects = any(item.is_articulated for item in scene.objects) + articulated = articulated_generation_client if has_articulated_objects else None + owns_articulated = False + log_info("Starting Objects + Coarse Layout Generation") + try: + if has_articulated_objects and articulated is None: + articulated = ArticulatedGenerationClient.from_dotenv() + owns_articulated = True + geometry.check_health() + if articulated is not None: + articulated.check_health() + scene = generate_scene_and_refine( + image_path=blueprint.image_path, + output_root=blueprint.output_root, + scene=scene, + scene_graph=scene_graph, + geometry_generation_client=geometry, + vlm_client=effective_vlm, + articulated_generation_client=articulated, + ) + finally: + if owns_articulated and articulated is not None: + articulated.close() + if geometry_generation_client is None: + geometry.close() + log_info("Completed Objects + Coarse Layout Generation") + return _export_materialization( + scene=scene, + scene_graph=scene_graph, + output_root=blueprint.output_root, + ) + + +def analyze_edit( + *, + output_root: str | Path, + edit_prompt: str, + vlm_client: OpenAICompatibleVLM | None = None, +) -> SceneEditBlueprintPackage: + """Interpret and persist one edit against an already generated scene.""" + resolved_output = Path(output_root).expanduser().resolve() + normalized_prompt = str(edit_prompt).strip() + if not normalized_prompt: + raise ValueError("Edit prompt must not be empty.") + scene, scene_graph = SceneExportImporter( + output_root=resolved_output + ).import_scene_and_graph() + effective_vlm = vlm_client or OpenAICompatibleVLM.from_dotenv() + log_info("Starting Edit Understanding") + scene_edit_plan, updated_scene_graph = understand_scene_edit( + scene=scene, + scene_graph=scene_graph, + edit_prompt=normalized_prompt, + vlm_client=effective_vlm, + ) + log_info("Completed Edit Understanding") + payload = { + "schema_version": SCENE_EDIT_BLUEPRINT_SCHEMA, + "edit_prompt": normalized_prompt, + "scene_edit_plan": scene_edit_plan.to_dict(), + "updated_scene_graph": updated_scene_graph.to_dict(), + } + blueprint_id = _canonical_hash(payload) + manifest_path = resolved_output / "scene_edit" / "scene_edit_blueprint.json" + _write_json(manifest_path, {**payload, "blueprint_id": blueprint_id}) + return SceneEditBlueprintPackage( + schema_version=SCENE_EDIT_BLUEPRINT_SCHEMA, + blueprint_id=blueprint_id, + edit_prompt=normalized_prompt, + output_root=resolved_output, + manifest_path=manifest_path, + scene_edit_plan=scene_edit_plan, + updated_scene_graph=updated_scene_graph, + ) + + +def materialize_edit( + blueprint: SceneEditBlueprintPackage, + *, + vlm_client: OpenAICompatibleVLM | None = None, + image_generation_client: ImageGenerationClient | None = None, + geometry_generation_client: GeometryGenerationClient | None = None, + image_segmentation_client: ImageSegmentationClient | None = None, +) -> SceneMaterialization: + """Generate added assets, apply layout edits, and export the new revision.""" + scene_edit_plan = deepcopy(blueprint.scene_edit_plan) + updated_scene_graph = deepcopy(blueprint.updated_scene_graph) + effective_vlm = vlm_client or OpenAICompatibleVLM.from_dotenv() + image_generation = image_generation_client or ImageGenerationClient.from_dotenv() + geometry = geometry_generation_client or GeometryGenerationClient.from_dotenv() + segmentation = image_segmentation_client or ImageSegmentationClient.from_dotenv() + owned_clients = ( + (image_generation, image_generation_client is None), + (geometry, geometry_generation_client is None), + (segmentation, image_segmentation_client is None), + ) + log_info("Starting Objects Preparation") + try: + for client, _ in owned_clients: + client.check_health() + added_assets = prepare_scene_edit_assets( + scene_edit_plan=scene_edit_plan, + output_root=blueprint.output_root, + image_generation_client=image_generation, + geometry_generation_client=geometry, + image_segmentation_client=segmentation, + vlm_client=effective_vlm, + ) + finally: + for client, owned in owned_clients: + if owned: + client.close() + log_info("Completed Objects Preparation") + log_info("Starting Layout Generation") + scene = edit_layout( + scene=scene_edit_plan.scene, + scene_edit_plan=scene_edit_plan, + updated_scene_graph=updated_scene_graph, + added_assets=added_assets, + output_root=blueprint.output_root, + ) + log_info("Completed Layout Generation") + return _export_materialization( + scene=scene, + scene_graph=updated_scene_graph, + output_root=blueprint.output_root, + ) + + +def _export_materialization( + *, + scene: Scene, + scene_graph: SceneGraph, + output_root: Path, +) -> SceneMaterialization: + log_info("Starting Scene Export") + scene_config_path = SceneExporter( + scene=scene, + scene_graph=scene_graph, + output_root=output_root, + ).export() + log_info("Completed Scene Export") + return SceneMaterialization( + scene=scene, + scene_graph=scene_graph, + output_root=output_root, + scene_config_path=scene_config_path, + ) + + +def _artifact_records(root: Path) -> list[dict[str, Any]]: + if not root.is_dir(): + return [] + records = [] + for path in sorted(item for item in root.rglob("*") if item.is_file()): + records.append( + { + "path": path.resolve().as_posix(), + "sha256": hashlib.sha256(path.read_bytes()).hexdigest(), + "size": path.stat().st_size, + } + ) + return records + + +def _canonical_hash(value: Any) -> str: + payload = json.dumps( + value, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode("utf-8") + return hashlib.sha256(payload).hexdigest() + + +def _write_json(path: Path, value: Any) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + temporary = path.with_suffix(path.suffix + ".tmp") + temporary.write_text( + json.dumps(value, indent=2, ensure_ascii=False, allow_nan=False) + "\n", + encoding="utf-8", + ) + temporary.replace(path) diff --git a/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_asset_preparation.py b/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_asset_preparation.py index 670436e39..e16d385ba 100644 --- a/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_asset_preparation.py +++ b/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_asset_preparation.py @@ -70,7 +70,38 @@ def prepare_scene_edit_assets( image_segmentation_client: ImageSegmentationClient, vlm_client: OpenAICompatibleVLM | None = None, ) -> list[SceneObject]: - """Prepare and return SimReady assets required by add operations.""" + """Generate canonical SimReady assets for a scene edit's add operations. + + Move-only and delete-only plans return immediately without modifying an + existing asset-preparation directory. For add operations, the function + generates and segments one image per object, creates coarse geometry, + processes it into SimReady geometry, and resets the returned objects to + identity edit-time poses. + + Args: + scene_edit_plan: Validated edit plan whose add operations define the + objects to generate. + output_root: Scene Engine output root. Intermediate artifacts are + written below ``scene_editing/asset_preparation``. + image_generation_client: Client used to render object images from the + operation descriptions. + geometry_generation_client: Client used to create coarse GLB geometry + from each generated image and mask. + image_segmentation_client: Client used to isolate the generated object + in each image. + vlm_client: Optional VLM used by SimReady processing to estimate asset + scale and orientation. + + Returns: + Added ``SceneObject`` assets in edit-plan order, or an empty list when + the plan contains no add operations. + + Raises: + ValueError: If add metadata or generated image, mask, and geometry + mappings are incomplete or inconsistent. + FileNotFoundError: If geometry generation does not produce an expected + GLB file. + """ # Prepare descriptions for all newly added objects. added_asset_descriptions = _collect_added_asset_descriptions(scene_edit_plan) # Skip asset generation when the edit plan only moves or deletes existing objects. diff --git a/embodichain/gen_sim/scene_engine/pipeline/generate.py b/embodichain/gen_sim/scene_engine/pipeline/generate.py index 71155a40a..5f65660d7 100644 --- a/embodichain/gen_sim/scene_engine/pipeline/generate.py +++ b/embodichain/gen_sim/scene_engine/pipeline/generate.py @@ -41,13 +41,28 @@ generate_scene_and_refine, ) from embodichain.gen_sim.scene_engine.pipeline.utils.scene_exporter import SceneExporter +from embodichain.gen_sim.scene_engine.pipeline.utils.scene_layout_utils import ( + rotate_scene_z_up_world, +) def generate_scene_from_image( image_path: str | Path, output_root: str | Path, + *, + scene_z_rotation_degrees: float = 0.0, ) -> Scene: - """Generate the initial core scene state from an input image.""" + """Generate the initial core scene state from an input image. + + Args: + image_path: Source tabletop image. + output_root: Directory receiving intermediate and exported artifacts. + scene_z_rotation_degrees: Final counterclockwise world-z rotation applied + rigidly to the complete scene before export. + + Returns: + The final scene in the rotated export frame. + """ resolved_output_root = Path(output_root).expanduser().resolve() resolved_output_root.mkdir(parents=True, exist_ok=True) @@ -100,6 +115,10 @@ def generate_scene_from_image( # 3. Scene Export log_info("Starting Scene Export") + rotate_scene_z_up_world( + scene=scene, + rotation_degrees=scene_z_rotation_degrees, + ) scene_exporter = SceneExporter( scene=scene, scene_graph=scene_graph, diff --git a/embodichain/gen_sim/scene_engine/pipeline/utils/scene_layout_utils.py b/embodichain/gen_sim/scene_engine/pipeline/utils/scene_layout_utils.py index 855342920..4838e0c31 100644 --- a/embodichain/gen_sim/scene_engine/pipeline/utils/scene_layout_utils.py +++ b/embodichain/gen_sim/scene_engine/pipeline/utils/scene_layout_utils.py @@ -17,7 +17,9 @@ from __future__ import annotations import numpy as np +from scipy.spatial.transform import Rotation +from embodichain.gen_sim.scene_engine.core.scene import Scene from embodichain.gen_sim.scene_engine.core.scene_object import SceneObject from embodichain.gen_sim.scene_engine.pipeline.utils.scene_generation_utils import ( layout_object_to_transform_matrix, @@ -75,6 +77,70 @@ def translate_scene_object_y_up_by_z_up_delta( ] +def rotate_scene_z_up_world(*, scene: Scene, rotation_degrees: float) -> None: + """Rotate every final scene pose and support point about world z. + + The generated meshes remain unchanged. The same rigid world transform is + applied to positions, orientations, and persisted z-up XY support metadata + so the exported scene stays internally consistent. + + Args: + scene: Final generated scene whose object poses use the internal y-up frame. + rotation_degrees: Counterclockwise world-z rotation in degrees. + + Raises: + ValueError: If the rotation is not finite or a scene pose is incomplete. + """ + angle = float(rotation_degrees) + if not np.isfinite(angle): + raise ValueError("rotation_degrees must be finite.") + if angle % 360.0 == 0.0: + return + + world_rotation = np.eye(4) + world_rotation[:3, :3] = Rotation.from_euler("z", angle, degrees=True).as_matrix() + basis = y_up_to_z_up_matrix() + inverse_basis = np.linalg.inv(basis) + rotation_xy = world_rotation[:2, :2] + + for scene_object in scene.objects: + y_up_layout = scene_object_y_up_layout(scene_object) + z_up_transform = ( + basis @ layout_object_to_transform_matrix(y_up_layout) @ inverse_basis + ) + rotated_y_up_transform = inverse_basis @ world_rotation @ z_up_transform @ basis + rotated_layout = transform_matrix_to_layout_object( + scene_object.id, + rotated_y_up_transform, + ) + scene_object.pos = rotated_layout["pos"] + scene_object.rot = rotated_layout["rot"] + scene_object.scale = rotated_layout["scale"] + + if scene_object.center_xy is not None: + scene_object.center_xy = ( + rotation_xy + @ np.asarray( + two_floats(scene_object.center_xy, field_name="center_xy"), + dtype=float, + ) + ).tolist() + for field_name in ("support_contour_xy", "support_optimization_rect_xy"): + points = getattr(scene_object, field_name) + if points is None: + continue + setattr( + scene_object, + field_name, + [ + (rotation_xy @ np.asarray(two_floats(point, field_name=field_name))) + .astype(float) + .tolist() + for point in points + ], + ) + + def measure_scene_object_z_up_world_aabb( *, scene_object: SceneObject ) -> list[list[float]]: diff --git a/embodichain/gen_sim/task_engine/__init__.py b/embodichain/gen_sim/task_engine/__init__.py new file mode 100644 index 000000000..087983cc4 --- /dev/null +++ b/embodichain/gen_sim/task_engine/__init__.py @@ -0,0 +1,179 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Scene-independent task interpretation and protocol ownership.""" + +from __future__ import annotations + +from typing import Any + +from .agent import ( + TaskAgent, + TaskGenerationError, + derive_scene_request, + derive_success_spec, +) +from .contracts import ( + SCENE_REQUEST_SCHEMA, + SUCCESS_SPEC_SCHEMA, + TASK_CANDIDATE_SET_SCHEMA, + TASK_DRAFT_SCHEMA, + SceneRequest, + SuccessSpec, + TaskCandidate, + TaskCandidateSet, + TaskDraft, + canonical_hash, + validate_scene_request, + validate_success_spec, + validate_task_candidate, + validate_task_candidate_set, + validate_task_draft, +) +from .interpretation import ( + INSTRUCTION_INTENT_SCHEMA, + InstructionCaller, + InstructionDraftResult, + InstructionIntent, + interpret_instruction_draft, + validate_instruction_intent, +) +from .ontology import ( + RELATIONS, + TASK_CONTRACTS, + TERMINAL_BEHAVIORS, + TRANSPORT_DIRECTIONS, + TaskContract, + task_contract, + task_success_type, +) +from .config import ( + TASK_ENGINE_DEFAULTS_SCHEMA, + TaskEngineExecutionCfg, + TaskEnginePlanningCfg, + TaskEngineWorkflowCfg, + load_task_engine_config, +) +from .state_machine import ( + StageStatus, + TaskEngineState, + WorkflowStage, + complete_stage, + fail_stage, + initial_state, + replay_events, + skip_stage, + start_stage, +) +from .workflow_contracts import ( + TASK_RUN_REQUEST_SCHEMA, + SceneInputKind, + TaskRunRequest, + scene_input_kind, + validate_scene_history_root, + validate_scene_output_separation, + validate_task_run_request, +) + +__all__ = [ + "INSTRUCTION_INTENT_SCHEMA", + "InstructionCaller", + "InstructionDraftResult", + "InstructionIntent", + "RELATIONS", + "SCENE_REQUEST_SCHEMA", + "SUCCESS_SPEC_SCHEMA", + "SceneRequest", + "SuccessSpec", + "TASK_CANDIDATE_SET_SCHEMA", + "TASK_CONTRACTS", + "TASK_DRAFT_SCHEMA", + "TERMINAL_BEHAVIORS", + "TRANSPORT_DIRECTIONS", + "TaskAgent", + "TaskCandidate", + "TaskCandidateSet", + "TaskContract", + "TaskDraft", + "TaskGenerationError", + "TASK_RUN_REQUEST_SCHEMA", + "TASK_ENGINE_DEFAULTS_SCHEMA", + "SceneInputKind", + "SceneAnalysis", + "SceneEngineBackend", + "SceneRevision", + "StageStatus", + "TaskEngineState", + "TaskEngineExecutionCfg", + "TaskEnginePlanningCfg", + "TaskEngineWorkflowCfg", + "TaskEngineRunResult", + "TaskEngineWorkflow", + "TaskRunRequest", + "WorkflowStage", + "TASK_ENGINE_RUN_MANIFEST_SCHEMA", + "SubprocessActionExecutor", + "canonical_hash", + "derive_scene_request", + "derive_success_spec", + "complete_stage", + "fail_stage", + "initial_state", + "interpret_instruction_draft", + "load_task_engine_config", + "replay_events", + "task_contract", + "task_success_type", + "scene_input_kind", + "validate_scene_history_root", + "scene_blueprint_objects", + "skip_stage", + "start_stage", + "validate_instruction_intent", + "validate_scene_request", + "validate_scene_output_separation", + "validate_success_spec", + "validate_task_candidate", + "validate_task_candidate_set", + "validate_task_draft", + "validate_task_run_request", +] + +_SCENE_BACKEND_EXPORTS = { + "SceneAnalysis", + "SceneEngineBackend", + "SceneRevision", + "scene_blueprint_objects", +} +_WORKFLOW_EXPORTS = { + "TASK_ENGINE_RUN_MANIFEST_SCHEMA", + "SubprocessActionExecutor", + "TaskEngineRunResult", + "TaskEngineWorkflow", +} + + +def __getattr__(name: str) -> Any: + """Load orchestration entry points lazily to avoid engine import cycles.""" + if name in _SCENE_BACKEND_EXPORTS: + from . import scene_backend + + return getattr(scene_backend, name) + if name in _WORKFLOW_EXPORTS: + from . import workflow + + return getattr(workflow, name) + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") diff --git a/embodichain/gen_sim/task_engine/__main__.py b/embodichain/gen_sim/task_engine/__main__.py new file mode 100644 index 000000000..9e4f06dbe --- /dev/null +++ b/embodichain/gen_sim/task_engine/__main__.py @@ -0,0 +1,27 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Module entry point for Task Engine workflows.""" + +from __future__ import annotations + +from .cli import main + +__all__ = ["main"] + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/embodichain/gen_sim/task_engine/_bundle_runner.py b/embodichain/gen_sim/task_engine/_bundle_runner.py new file mode 100644 index 000000000..fbd17957a --- /dev/null +++ b/embodichain/gen_sim/task_engine/_bundle_runner.py @@ -0,0 +1,538 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Private subprocess boundary for canonical Task Program execution.""" + +from __future__ import annotations + +import argparse +from copy import deepcopy +import json +import os +from pathlib import Path +import random +import sys +from typing import Any, NoReturn, Sequence + +import numpy as np +import torch + +from embodichain.utils.utility import load_config + +from .orchestration.artifacts import STATIC_SCENE_MANIFEST_FILENAME +from .orchestration.scene_source import verify_scene_source_fingerprint +from .reporting import write_execution_report +from .semantic_graph import validate_semantic_task_graph + +__all__ = ["execute_bundle", "main"] + + +def main(argv: Sequence[str] | None = None) -> int: + """Parse the private runner protocol and execute one bundle.""" + parser = argparse.ArgumentParser( + prog="embodichain.gen_sim.task_engine._bundle_runner", + add_help=False, + ) + parser.add_argument("--bundle", required=True) + parser.add_argument("--execution-output", required=True) + protocol, forwarded = parser.parse_known_args(argv) + return execute_bundle( + protocol.bundle, + forwarded, + execution_output=protocol.execution_output, + ) + + +def execute_bundle( + bundle: str | Path, + forwarded: Sequence[str] = (), + *, + execution_output: str | Path | None = None, +) -> int: + """Execute one semantic bundle through the ordinary Gym Task Program bridge.""" + root = Path(bundle).expanduser().resolve() + if not root.is_dir(): + raise FileNotFoundError(f"Bundle directory does not exist: {root}") + output = ( + root / "execution" + if execution_output is None + else Path(execution_output).expanduser().resolve() + ) + output.mkdir(parents=True, exist_ok=True) + deployment_path = root / "task_program_deployment.yaml" + program_path = root / "task_program/program.yaml" + graph_path = root / "semantic_task_graph.json" + fingerprint_path = root / "integration_fingerprint.json" + for path in (deployment_path, program_path, graph_path, fingerprint_path): + if not path.is_file(): + raise FileNotFoundError(f"Bundle is missing required artifact: {path}") + _verify_source(root) + graph = validate_semantic_task_graph(_read_json(graph_path)) + fingerprint = _read_json(fingerprint_path) + deployment = _verify_integration_fingerprint( + root, deployment_path, graph, fingerprint + ) + _verify_program_projection(program_path, graph) + + args = _runner_parser().parse_args(list(forwarded)) + args.gym_config = deployment_path.as_posix() + args.record_trajectory = True + trajectory_root = output / "trajectory" + args.trajectory_save_dir = trajectory_root.as_posix() + random.seed(args.seed) + np.random.seed(args.seed) + torch.manual_seed(args.seed) + + result_metadata: dict[str, Any] | None = None + row_success = [False] * int(args.num_envs) + terminal_reasons = ["runtime_not_started"] * int(args.num_envs) + failure: dict[str, Any] | None = None + env: Any = None + try: + import gymnasium + + from embodichain.lab.gym.envs.demo import execute_demo_episode + from embodichain.lab.gym.utils.gym_utils import build_env_cfg_from_args + from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, + ) + + discover_task_packages() + execute_init_hooks() + from ._task_program.assembly import register_deployment + from embodichain.lab.task_program.language import load_task_program + + deployment_cfg = load_config(deployment_path) + register_deployment( + deployment, + environment_id=deployment_cfg["id"], + max_episode_steps=int(deployment_cfg["max_episode_steps"]), + ) + + def configure_environment(value: dict[str, Any]) -> None: + _configure_recording(value, output) + value.pop("task_program", None) + + env_cfg, gym_config, action_config = build_env_cfg_from_args( + args, + gym_config_modifier=configure_environment, + ) + env_cfg.task_program = load_task_program( + program_path, + integration=deployment.selection, + validation_context=deployment.integration.registration.catalog, + ) + deployment.integration.registration.catalog.preflight(env_cfg.task_program) + env = gymnasium.make(id=gym_config["id"], cfg=env_cfg, **action_config) + env.reset(seed=args.seed, options={"save_data": False}) + result = execute_demo_episode(env, episode_index=0, attempt_id=0) + result_metadata = result.to_metadata() + row_success = [bool(value) for value in result.success] + terminal_reasons = list(result.terminal_reasons) or [ + str(result.terminal_reason) + ] * len(row_success) + if result.completed and result.all_success: + env.reset() + else: + _preserve_failed_execution_recording( + env, + output, + num_envs=len(row_success), + ) + env.reset(options={"save_data": False}) + except Exception as exc: + failure = _exception_metadata(exc) + if env is not None: + try: + _preserve_failed_execution_recording( + env, + output, + num_envs=int(args.num_envs), + ) + except Exception as recording_error: + failure["recording_error"] = _exception_metadata(recording_error) + try: + env.reset(options={"save_data": False}) + except Exception as abort_error: + failure["abort_error"] = _exception_metadata(abort_error) + finally: + if env is not None: + try: + getattr(env, "unwrapped", env).close(exit_process=False) + except Exception as cleanup_error: + cleanup = { + "type": type(cleanup_error).__name__, + "message": str(cleanup_error), + } + if failure is None: + failure = cleanup + else: + failure["cleanup_error"] = cleanup + try: + from embodichain.lab.sim.sim_manager import SimulationManager + + SimulationManager.flush_cleanup_queue() + except Exception as cleanup_error: + cleanup = { + "type": type(cleanup_error).__name__, + "message": str(cleanup_error), + } + if failure is None: + failure = cleanup + else: + failure["simulation_cleanup_error"] = cleanup + + if len(row_success) != int(args.num_envs): + row_success = (row_success + [False] * int(args.num_envs))[: int(args.num_envs)] + if len(terminal_reasons) != len(row_success): + terminal_reasons = [ + ( + str(result_metadata.get("terminal_reason", "runtime_failed")) + if result_metadata is not None + else "runtime_failed" + ) + ] * len(row_success) + report = _build_execution_report( + graph, + result_metadata, + row_success=row_success, + terminal_reasons=terminal_reasons, + failure=failure, + trajectory_root=trajectory_root, + ) + write_execution_report(output, report) + _print_json(report) + return 0 if report["status"] == "succeeded" else 2 + + +def _build_execution_report( + graph: dict[str, Any], + runtime_result: dict[str, Any] | None, + *, + row_success: list[bool], + terminal_reasons: list[str], + failure: dict[str, Any] | None, + trajectory_root: Path, +) -> dict[str, Any]: + """Build a report without turning row-local failure into global failure.""" + semantic_success = _semantic_success_by_env( + graph, + runtime_result, + num_envs=len(row_success), + ) + return { + "schema_version": "task_program_execution_report/v1", + "status": "succeeded" if failure is None and all(row_success) else "failed", + "task_id": str(graph["task_id"]), + "semantic_call_count": len(graph["nodes"]), + "integration_fingerprint": str(graph["integration_fingerprint"]), + "record_dir": trajectory_root.as_posix(), + "environments": [ + { + "env_id": env_id, + "success": success and failure is None, + "terminal_reason": str(terminal_reasons[env_id]), + "semantic_success": semantic_success[env_id], + } + for env_id, success in enumerate(row_success) + ], + "runtime_result": deepcopy(runtime_result), + "failure": failure, + } + + +def _exception_metadata(exc: BaseException) -> dict[str, Any]: + """Preserve one exception and its explicit causal chain as JSON evidence.""" + if not isinstance(exc, BaseException): + raise TypeError("exc must be a BaseException.") + result = {"type": type(exc).__name__, "message": str(exc)} + causes: list[dict[str, str]] = [] + seen = {id(exc)} + current = exc + while len(causes) < 8: + next_error = current.__cause__ + if next_error is None and not current.__suppress_context__: + next_error = current.__context__ + if next_error is None or id(next_error) in seen: + break + seen.add(id(next_error)) + causes.append( + { + "type": type(next_error).__name__, + "message": str(next_error), + } + ) + current = next_error + if causes: + result["causes"] = causes + return result + + +def _preserve_failed_execution_recording( + env: Any, + output: Path, + *, + num_envs: int, +) -> None: + """Commit an audit copy of a failed attempt before the reset discards it. + + The common demo executor intentionally asks callers to discard invalid + episodes. Task Engine still needs a causal trajectory and camera artifact + for diagnosing a failed physical boundary, so this helper commits only to + the isolated execution directory and never to the training dataset. + """ + target = getattr(env, "unwrapped", env) + trajectory = getattr(target, "_traj_buffer", None) + trajectory_steps = getattr(target, "_traj_steps", None) + if trajectory is not None and trajectory_steps is not None: + active_env_ids = [ + env_id + for env_id in range(num_envs) + if int(trajectory_steps[env_id].item()) > 0 + ] + if active_env_ids: + trajectory_dir = output / "trajectory" + trajectory_dir.mkdir(parents=True, exist_ok=True) + target.save_trajectory( + trajectory_dir / "failed_attempt.pt", + env_ids=active_env_ids, + ) + + event_manager = getattr(target, "event_manager", None) + mode_cfgs = getattr(event_manager, "_mode_functor_cfgs", {}) + try: + from embodichain.lab.gym.envs.managers.record import record_camera_data + + for configured_functors in mode_cfgs.values(): + for functor_cfg in configured_functors: + if isinstance(functor_cfg.func, record_camera_data): + try: + # A first-step failure can precede the recording interval. + # Fetch the last rendered state without advancing physics. + functor_cfg.func(target, env_ids=None, **functor_cfg.params) + finally: + functor_cfg.func.save_and_clear() + except (AttributeError, TypeError, RuntimeError, OSError): + # The trajectory is the required audit artifact. Camera persistence is + # best effort because custom environments may not expose this manager. + return + + +def _verify_integration_fingerprint( + bundle: Path, + deployment_path: Path, + graph: dict[str, Any], + fingerprint: dict[str, Any], +) -> Any: + """Recompose provider-free integration identity before simulation starts.""" + from ._task_program.assembly import ADAPTER_CONTRACT, load_deployment + + if ( + fingerprint.get("schema_version") != "semantic_integration_fingerprint/v2" + or fingerprint.get("adapter_contract") != ADAPTER_CONTRACT + ): + raise ValueError( + "Incompatible GenSim bundle contract; regenerate the bundle against " + "the public 2620929c baseline." + ) + + deployment_cfg = load_config(deployment_path) + embodiment_cfg = load_config(bundle / "components/embodiment.yaml") + if type(deployment_cfg) is not dict or type(embodiment_cfg) is not dict: + raise ValueError("Configured deployment components must be exact mappings.") + task_program = deployment_cfg.get("task_program") + skill_profile = embodiment_cfg.get("skill_profile") + if type(task_program) is not dict or type(skill_profile) is not dict: + raise ValueError( + "Configured deployment must declare task_program and skill_profile." + ) + composed = load_deployment( + task_program=task_program, + skill_profile=skill_profile, + base_dir=bundle, + ) + if fingerprint.get("integration_id") != composed.integration_id: + raise ValueError( + "GenSim bundle integration identity does not match its declaration." + ) + expected = graph["integration_fingerprint"] + actual = composed.integration.integration_fingerprint + recorded = fingerprint.get("integration_fingerprint") + if expected != recorded or expected != actual: + raise ValueError( + "Semantic integration fingerprint drifted before execution: " + f"graph={expected!r}, recorded={recorded!r}, actual={actual!r}. " + "Regenerate the GenSim bundle against the current baseline contract." + ) + recorded_registration = fingerprint.get("registration_fingerprint") + actual_registration = composed.integration.registration.fingerprint + if recorded_registration != actual_registration: + raise ValueError( + "Semantic registration fingerprint drifted before execution: " + f"recorded={recorded_registration!r}, actual={actual_registration!r}." + ) + return composed + + +def _verify_program_projection( + program_path: Path, + graph: dict[str, Any], +) -> None: + """Require the executable program to be an exact projection of the graph.""" + program = load_config(program_path) + if type(program) is not dict: + raise ValueError("Task Program artifact must contain an exact mapping.") + if program.get("targets") != graph["targets"]: + raise ValueError("Task Program targets do not match SemanticTaskGraph targets.") + body = program.get("program") + if type(body) is not dict or body.get("kind") != "sequence": + raise ValueError("Generated Task Program must contain one sequence body.") + items = body.get("items") + if type(items) is not list or len(items) != len(graph["nodes"]): + raise ValueError( + "Task Program segment count does not match SemanticTaskGraph nodes." + ) + for index, (item, node) in enumerate(zip(items, graph["nodes"], strict=True)): + if type(item) is not dict or item.get("kind") != "segment": + raise ValueError(f"Task Program item {index} must be one segment.") + if item.get("name") != node["id"]: + raise ValueError( + f"Task Program item {index} does not match graph node ID " + f"{node['id']!r}." + ) + steps = item.get("steps") + if ( + type(steps) is not dict + or steps.get("kind") != "invoke" + or steps.get("call") != node["call"] + ): + raise ValueError( + f"Task Program segment {node['id']!r} is not an exact " + "SemanticTaskGraph call projection." + ) + + +def _semantic_success_by_env( + graph: dict[str, Any], + runtime_result: dict[str, Any] | None, + *, + num_envs: int, +) -> list[dict[str, bool]]: + """Project verified runtime segment outcomes onto immutable TaskGroups.""" + node_success = {str(node["id"]): [False] * num_envs for node in graph["nodes"]} + segments = ( + runtime_result.get("segments", []) if type(runtime_result) is dict else [] + ) + if type(segments) is list: + for segment in segments: + if type(segment) is not dict: + continue + name = segment.get("name") + if name not in node_success: + continue + successes = segment.get("successes") + active = segment.get("active") + if type(successes) is not list or len(successes) != num_envs: + continue + if type(active) is not list or len(active) != num_envs: + active = [True] * num_envs + node_success[str(name)] = [ + bool(is_active) and bool(success) + for is_active, success in zip(active, successes, strict=True) + ] + result: list[dict[str, bool]] = [] + for env_id in range(num_envs): + result.append( + { + str(group["id"]): all( + node_success[str(node_id)][env_id] for node_id in group["node_ids"] + ) + for group in graph["task_groups"] + } + ) + return result + + +def _runner_parser() -> argparse.ArgumentParser: + from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser + + parser = argparse.ArgumentParser(add_help=True) + add_env_launcher_args_to_parser(parser, require_gym_config=False) + parser.set_defaults(seed=0) + parser.add_argument( + "--failure-policy", choices=("stop", "continue"), default="stop" + ) + return parser + + +def _configure_recording(config: dict[str, Any], output: Path) -> None: + env_config = config.setdefault("env", {}) + events = env_config.setdefault("events", {}) + events["record_camera"] = { + "func": "record_camera_data", + "mode": "interval", + "interval_step": 5, + "params": { + "name": "task_program_audience_view", + "resolution": [640, 360], + "intrinsics": [280.0, 280.0, 320.0, 180.0], + "eye": [0.6, 0.0, 1.8], + "target": [0.0, 0.0, 0.75], + "up": [-1.0, 0.0, 0.0], + "save_path": (output / "videos").as_posix(), + }, + } + + +def _verify_source(bundle: Path) -> None: + static_manifest_path = bundle / STATIC_SCENE_MANIFEST_FILENAME + if not static_manifest_path.is_file(): + return + static_manifest = _read_json(static_manifest_path) + source = static_manifest.get("source", {}) + if isinstance(source, dict) and isinstance(source.get("source_fingerprint"), dict): + verify_scene_source_fingerprint(source["source_fingerprint"]) + + +def _read_json(path: Path) -> dict[str, Any]: + try: + value = json.loads(path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError) as exc: + raise ValueError(f"Unable to read JSON artifact {path}: {exc}") from exc + if type(value) is not dict: + raise ValueError(f"JSON artifact must contain an object: {path}") + return value + + +def _print_json(value: Any) -> None: + print(json.dumps(value, ensure_ascii=False, indent=2, allow_nan=False)) + + +def _module_entrypoint() -> NoReturn: + """Run the private protocol and bypass unsafe native interpreter teardown.""" + exit_code = main() + # DexSim owns native CUDA/Vulkan state whose interpreter-order teardown is + # unsafe after the explicit environment cleanup above. This private runner + # is already an isolated subprocess, so flush the published protocol output + # and use the same fast-exit boundary as the canonical simulation CLI. + sys.stdout.flush() + sys.stderr.flush() + os._exit(exit_code) + + +if __name__ == "__main__": + _module_entrypoint() diff --git a/embodichain/gen_sim/task_engine/_task_program/BASELINE_LIMITS.md b/embodichain/gen_sim/task_engine/_task_program/BASELINE_LIMITS.md new file mode 100644 index 000000000..8111fa0b6 --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/BASELINE_LIMITS.md @@ -0,0 +1,111 @@ +# Baseline Limits + +The public implementation is pinned to commit +`2620929c82132df130ebeb43e69a2eb96791cba0`. Task Engine must not patch or +subclass its execution Session/Bridge to conceal a missing behavior. + +## Registered Calls Do Not Inherit Phase Protection + +The public compiler grounds terminal effects for registered calls, but its +phase acquisition/release gates and in-flight held-object guards are generated +only for the exact built-in Pick, Place and HandOver call types. Selecting the +same Atomic Skill descriptor does not select the same protection contract. + +A CPU CLI probe uses the real SemanticCallCompiler, the real GenSim Pick +lowerer, and unchanged public CPU scene/robot fixtures. Both calls ground to +PickUp with a terminal effect monitor: + +| Semantic Call | Acquisition gates | In-flight held guards | +| --- | --- | --- | +| Built-in Pick | destination_acquired | destination_attached | +| gen_sim.pick.probe | none | none | + +Reproduce from the repository root: + +```bash +/home/dex/miniconda3/envs/embodichain040/bin/python -B \ + outputs/gen_sim/baseline_only_audit_JJ4DKu/registered_phase_protection_probe.py +``` + +The adjacent registered_phase_protection.log records the result. This is +grounding evidence, not a simulated dropped-object rollout. The probe checks +that public implementation files still match 2620929c before running. + +Affected task routes include constrained E2 and upright E1 acquisition, +registered relative/stack placement, coordinated E5, and effect-preserving +registered motion. Built-in HandOver retains its receiver acquisition gate +and source/destination guards. A failed terminal effect or post-policy still +blocks later calls; this does not prove interruption during the current call. + +SemanticLowering has no field for a registered phase gate or held guard. +Registered lowerers cannot replace curated semantics or return skill options; +InvokeCfg has no per-call policy selection. Task Engine must not fill this gap +by replacing the compiler, runner, Session or Bridge, modifying private +invocation state, or treating a terminal stability check as an in-flight guard. + +Retaining built-in calls where their declared inputs suffice is preferable, +but there is no built-in coordinated E5 call. Complete qualification therefore +requires a separate decision on the public extension contract. Normal physical +successes, including the frozen multi-seed runs, are not evidence that this +failure-path requirement is met. Migration of the missing protection is stopped +at this boundary; no private workaround is installed. + +## Initial Empty-Plan Recovery + +A CPU-only probe using the real AtomicActionEngine and ExecutionSession was +rerun after restoring the public files. It exercised three distinct cases: + +| Initial plan | Retry budget | Observed result | +| --- | --- | --- | +| Valid | 1 | A command is emitted normally | +| Empty failure | 0 | FAILED, with no command emitted | +| Empty failure, then valid | 1 | The second plan raises ValueError before execution | + +The third case reports: + +```text +Recovery replans must preserve endpoint tracking source fingerprints and projector routes; start a new invocation to change feedback ownership. +``` + +The empty first plan has not established tracking ownership, but the baseline +continuity check rejects the first valid replacement's tracking routes. +Evidence is retained in the workspace under +`outputs/gen_sim/baseline_only_audit_JJ4DKu/initial_empty_plan_recovery.log`. +The earlier standalone reproduction is in +`outputs/gen_sim/baseline_gate_2620929c/reproduce.md`. + +Passing a normal physical rollout does not prove this recovery case works. +No local Session override, private eligibility update, retry-budget reduction, +or hidden execution retry is used as a substitute for fixing the baseline. + +The frozen full task2_1 seed-1 run also reached this defect, without selecting +or restarting an execution attempt. Its first gen_sim.pick.step_01 plan had +320 filtered grasp proposals but no feasible pre-grasp candidate. A replan +then triggered the tracking-continuity ValueError. The public workflow recorded +semantic_call_failed, deactivated the row and did not run later calls. +The raw report is retained at +outputs/gen_sim/task2_1/20260906_180545/attempts/scene_0001/action_attempts/action_0001/execution_report.json. +The audit wrapper's generic error about a successful single attempt also covers +nonzero CLI exit codes; it must not be interpreted as evidence of a retry here. + +## Grasp Binding Port + +The baseline GraspGoal accepts an explicit grasp pose. Its PickUpOptions +contains downstream_object_target_poses and one shared (3,) world approach +direction. It has no release-clearance fields, object-axis approach weight, +or upper_half region semantics. Task-owned filtering now supplies that region +and release screening through the existing sampling interface, not new Goal +fields. Existing normal Pick look-ahead remains compiler-owned; the unused +custom registered-Pick future-target declaration was removed. + +The current CPU probe in outputs/gen_sim/baseline_grasp_mask_probe.py uses the +unchanged public PickUp planner with six- and seven-joint mock robot layouts. +It verifies mixed success/failure and all-failure candidate rows, no commands +for failed rows, and no planning-time TaskState mutation. This is interface +evidence, not physical qualification of those robot layouts. + +The baseline rejects skill_options returned by registered lowerers. Thus a +task's approach policy must be bound before execution, using task-scoped call +aliases and baseline presets. This does not recreate the withdrawn Bx3 +per-environment approach option. All final physical runs must use regenerated +bundles with the current adapter contract. diff --git a/embodichain/gen_sim/task_engine/_task_program/README.md b/embodichain/gen_sim/task_engine/_task_program/README.md new file mode 100644 index 000000000..cc8cc53db --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/README.md @@ -0,0 +1,138 @@ +# GenSim Task Program Integration + +This package owns GenSim declarations and provider assembly, not another +Task Program executor. The shared compiler, semantic runtime, execution +sessions, Gym bridge, and demo executor remain authoritative. + +The public implementation is exactly the `2620929c` baseline. E3, E5 and a +single-object E2 have passed fresh physical regressions against it. The full +can task and final multi-seed qualification remain separate gates. See +`BASELINE_LIMITS.md` for the independent recovery gap and the registered-call +phase-protection gap. The latter blocks complete qualification even when a +normal physical rollout succeeds; terminal acceptance is not in-flight safety. + +## Local Boundary + +- `assembly.py` composes the existing simulation integration with task-owned + named stability presets. Its factory only selects provider instances and + returns the exact public `TaskProgramEnvironmentAdapter`. +- `grasp_filter.py` wraps the existing parallel-jaw sampling service. Immutable + geometry rules filter unrestricted single-arm proposals by object-local + region and release clearance. Stock PickUp still chooses symmetric variants, + solves candidate IK, builds trajectories, and publishes measured effects. +- `stability.py` implements `SegmentPostPolicyPort`. It observes object poses + and object-to-endpoint drift while yielding ordinary target-qpos holds. + Only Gym consumes those commands and advances simulation. +- `align_held.py` binds a declared object axis using the latest scene + observation. It emits a normal `MoveHeldObject` goal with a minimal rotation; + it neither solves IK nor overwrites the runtime's measured grasp state. + The explicit `current_object_pose` selector changes orientation without + moving the object to a nominal scene position. E1 upright placement reuses + E2's constrained acquisition and staging recipe. E4 upright transfer aligns + on the source arm before HandOver and corrects on the receiver afterward. +- `stack_place.py` registers a separate stack-placement call and option preset. + It reuses the original Place goal/effect binding but does not inherit the low + table-placement TCP cap, which could otherwise eliminate the release retreat. +- `release_clearance.py` binds generated staging heights and a fingerprinted + withdrawal distance to the existing `MoveEndEffector` skill. The free hand + first clears vertically, then moves toward its own arm root before Park. + It refuses a live held attachment and never solves IK or applies commands. +- `motion.py` supplies a planning policy for multi-waypoint EEF approaches. + Cartesian samples are solved by the original motion generator; command + timing, execution, cancellation and recovery remain owned by the core. +- `constraints.json` is a closed, versioned, inert bundle artifact. The + integration fingerprint includes its complete contents and the program. + Unknown fields and old bundles without this artifact fail before execution. +- A policy records measurements and time-window results. It never modifies + `TaskState`, row eligibility, the program counter, recovery queues, or final + task success. The core bridge combines its result with the skill result. + +`wait_stable` retains its declared meaning: an entity must satisfy a named +stability preset. Upright and stacking presets require a contiguous stable +window. Held-object presets additionally fail on observed grasp slip or target +loss instead of restarting the observation window after a drop. + +Placement acceptance is also repeated at the final cleanup segment. A can +passing an early stable check must remain upright after hand clearance and +Park; a stack must remain supported for another complete stable window after +withdrawal. These are ordinary shared segment post-policies and validators, +not a separate task-result update path. +Stack stability measures translation and rotation of both objects against one +shared window. Movement of either object restarts the complete observation +window; a stationary upper object cannot conceal motion of its support. + +Coordinated placement additionally checks the scene-generated destination; +being stationary at a wrong location is not sufficient. Coordinated hold +checks that destination and both attachments. Upright HandOver hold checks +the requested axis and the receiving attachment without inventing a position +goal. Ordinary position validators, including Pour return targets, are also +rechecked after cleanup. Upright acquisition and staging use the canonical +`PreparedScene.table_top_z`, not a second required copy in AABB metadata. + +The planner's predicted holder map only selects declarations (for example, +omitting a duplicate Pick before a continuation HandOver). It is not runtime +ownership: the shared compiler and runtime still require measured attachment +evidence and block calls after failure. + +Baseline registered lowerers may not return action options. GenSim therefore +binds each constrained Pick's declared approach direction into a task-scoped +`gen_sim.pick.` policy alias before compilation. Directions come +from scene declarations and preceding planned alignment, not runtime option +mutation. The factory and lowerer identity classes specialize only this inert +call ID because the baseline requires class-level identities; they introduce +no new execution behavior or Atomic Skill implementation. + +Each invocation uses the baseline's single shared world direction. Geometry +filtering and stock IK remain row-local. A rejected or empty proposal row uses +the sampler's existing infinite-cost failure convention, never successful +padding or a private eligibility update. End-specific, best-grasp and paired +grasp protocols retain their baseline behavior; constrained Pick requires +unrestricted sampling without a fixed grasp or post-sampling rotation. + +The coordinated Robotiq recipe admits 5 mm contact pairs and uses the measured +65 mm pad extent around the configured TCP. The generic 130 mm conservative +envelope is retained for inclined single-arm grasps because the current +three-box collision approximation couples palm position to finger length. +Both service declarations are fingerprinted. Physical joint limits, hand +commands, and contact/holding acceptance thresholds are unchanged. + +Stacking from a low handover grasp includes explicit staging, free-arm park, +upper regrasp and final placement calls. The shared Task Program observes every +transition. This is a compiled recipe, not a private retry/state machine. + +## Current Migration Status + +`configured.py` owns task service decoding and delegates public option decoding +directly to the baseline. It does not inject extra fields into public Options. +`services.py` owns immutable Pick, MoveHeldObject, relative Place and coordinated +transport routes and lowerers. Baseline component composition, common scalar +decoders, Park and Pour are reused without adding fields to the shared YAML +schema. The shared configured service modules and official pour-water format +have been restored to the baseline. + +The public language/compiler/validator stack has also been restored: GenSim +upright and stability requirements now use the existing named post-policy port, +not new public validator types. All shared Atomic Skill and compiler changes +have been withdrawn. MoveHeldObject now binds one exact target; alternative +declarations are rejected instead of being silently discarded. Public +HandOver owns its original release sequence without an extra settle phase. +Simulation-factory internals used during assembly are isolated compatibility +dependencies; no Session or Bridge method is replaced. + +The fingerprint manifest uses `semantic_integration_fingerprint/v2` with the +explicit `gen_sim.task_program/2620929c/v3` adapter contract. Old bundles are +rejected before component loading and must be regenerated. +Version 3 adds the support object's stable window and a final failed-attempt +camera fetch before reset. The v2 physical runs are retained as historical +evidence, not qualification of this revised acceptance behavior. + +E1-E5 are the Task Engine execution scope. E6-E9 routing is rejected before +semantic graph generation and bundle asset writing. Their task-generation, +joint-target extraction and integration branches have been removed. Physical +background articulations can still be loaded without exposing those task calls. +This package does not claim that arbitrary scenes or robots have been physically +qualified. + +Physical acceptance requires the user's complete can-stacking and original +tray-holding CLI runs, with measured stable end states and recorded failures. +Provider-level CLI probes are necessary but do not substitute for these runs. diff --git a/embodichain/gen_sim/task_engine/_task_program/__init__.py b/embodichain/gen_sim/task_engine/_task_program/__init__.py new file mode 100644 index 000000000..a9eb6ef90 --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/__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. +# ---------------------------------------------------------------------------- + +"""Task Engine-owned declarations and services for the shared Task Program runtime.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/embodichain/gen_sim/task_engine/_task_program/align_held.py b/embodichain/gen_sim/task_engine/_task_program/align_held.py new file mode 100644 index 000000000..4179fbadd --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/align_held.py @@ -0,0 +1,237 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Bind an observed held-object axis to an existing pose-motion skill.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +from typing import Any, ClassVar + +import torch + +from embodichain.lab.sim.atomic_actions.primitives.move_held_object import ( + HeldObjectPoseGoal, + MoveHeldObject, + MoveHeldObjectOptions, +) +from embodichain.lab.task_program.compiler.lowering import ( + RegisteredSemanticLowerer, + SemanticLowering, +) +from embodichain.lab.task_program.semantics import ( + RegisteredSemanticCall, + SceneObjectRef, + SemanticCallDescriptor, + SkillPolicyPreset, +) +from embodichain.utils.math import axis_angle_to_rotation_matrix + +__all__: list[str] = [] + +ALIGN_HELD_CALL = "gen_sim.align_held" + + +class _AlignHeldLowerer(RegisteredSemanticLowerer): + call_id: ClassVar[str] = ALIGN_HELD_CALL + target_descriptor = MoveHeldObject.descriptor() + preserves_symbolic_state: ClassVar[bool] = True + + def __init__(self, routes: tuple[tuple[Any, ...], ...], robot: Any) -> None: + self._routes = { + (obj, target, preserve): (axis, position) + for obj, target, preserve, axis, position in routes + } + self._robot = robot + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: Any, + bound: Any, + option_template: Any, + ) -> SemanticLowering: + if type(option_template) is not MoveHeldObjectOptions: + raise TypeError("Held alignment requires MoveHeldObjectOptions.") + args = dict(call.arguments) + if ( + set(args) != {"object", "target", "preserve_yaw"} + or type(args["preserve_yaw"]) is not bool + ): + raise ValueError( + "Held alignment requires references and a declared yaw policy." + ) + axis, position = self._routes[ + (args["object"], args["target"], args["preserve_yaw"]) + ] + resource = bound.binding.resources["primary"] + key = resource.endpoints["motion"].task_state_key + held = context.task.get_held_object(key) + if held is None or held.semantics.entity_id != args["object"]: + raise ValueError("Held alignment requires the declared object to be held.") + observed = context.scene.entities[args["object"]] + if observed.confidence <= 0: + raise ValueError("Held alignment requires a current object observation.") + pose = observed.pose.clone() + if pose.shape == (4, 4): + pose = pose.unsqueeze(0).expand(context.batch_size, -1, -1).clone() + source = torch.nn.functional.normalize( + pose[:, :3, :3] @ pose.new_tensor(axis), dim=-1 + ) + target = torch.zeros_like(source) + target[:, 2] = 1 + cross = torch.cross(source, target, dim=-1) + sine = torch.linalg.vector_norm(cross, dim=-1) + cosine = (source * target).sum(-1).clamp(-1, 1) + rotation_axis = cross / sine.clamp_min(1e-8)[:, None] + # Opposite axes need a deterministic perpendicular, not an arbitrary yaw. + basis = torch.eye(3, device=pose.device, dtype=pose.dtype)[ + source.abs().argmin(-1) + ] + perpendicular = torch.nn.functional.normalize( + torch.cross(source, basis, dim=-1), dim=-1 + ) + rotation_axis = torch.where( + (sine < 1e-8)[:, None], perpendicular, rotation_axis + ) + correction = axis_angle_to_rotation_matrix( + rotation_axis * torch.atan2(sine, cosine)[:, None] + ) + pose[:, :3, :3] = correction @ pose[:, :3, :3] + if position is not None: + pose[:, :3, 3] = pose.new_tensor(position) + if args["preserve_yaw"]: + return SemanticLowering(goal=HeldObjectPoseGoal(pose)) + # Upright leaves yaw free. Prefer a TCP approaching from its arm root + # rather than forcing the wrist to point back toward the robot base. + motion = resource.endpoints["motion"].runtime_target + part = motion.control_part + root_name = self._robot.cfg.solver_cfg[part].root_link_name + root_pose = self._robot.get_link_pose( + link_name=root_name, + env_ids=context.env_ids.tolist(), + to_matrix=True, + ).to(device=pose.device, dtype=pose.dtype) + toward_object = pose[:, :2, 3] - root_pose[..., :2, 3] + object_to_eef = held.object_to_eef.to(device=pose.device, dtype=pose.dtype) + approach = (pose[:, :3, :3] @ object_to_eef[..., :3, :3])[:, :2, 2] + flip = (torch.linalg.vector_norm(approach, dim=-1) > 0.1) & ( + (approach * toward_object).sum(-1) < 0 + ) + half_turn = torch.diag(pose.new_tensor([-1.0, -1.0, 1.0])) + pose[:, :3, :3] = torch.where( + flip[:, None, None], + half_turn @ pose[:, :3, :3], + pose[:, :3, :3], + ) + return SemanticLowering(goal=HeldObjectPoseGoal(pose)) + + +@dataclass(frozen=True, slots=True) +class _AlignHeldFactory: + call_id: ClassVar[str] = ALIGN_HELD_CALL + revision: ClassVar[str] = "3" + target_descriptor = MoveHeldObject.descriptor() + routes: tuple[ + tuple[str, str, bool, tuple[float, ...], tuple[float, ...] | None], ... + ] + + def create( + self, *, simulation: Any, robot: Any, scene_registry: Any, engine: Any + ) -> _AlignHeldLowerer: + if engine.robot is not robot: + raise ValueError("Held alignment must bind the factory's robot.") + for obj, _, _, _, _ in self.routes: + scene_registry.resolve(obj, expected_type=SceneObjectRef) + return _AlignHeldLowerer(self.routes, robot) + + +def with_held_alignment( + registration: Any, *, program: dict[str, Any], constraints: Any +) -> Any: + axes = { + cfg.entity: cfg.local_axis + for cfg in constraints.values() + if cfg.local_axis is not None + } + routes = {} + for item in program["program"]["items"]: + call = item["steps"]["call"] + if call.get("call_id") != ALIGN_HELD_CALL: + continue + args = call["arguments"] + obj, target = args["object"], args["target"] + preserve = args.get("preserve_yaw") + if type(preserve) is not bool: + raise ValueError( + "Held alignment needs an explicit preserve_yaw policy; regenerate the bundle." + ) + values = ( + None + if target == "current_object_pose" + else program["targets"][target]["values"] + ) + if (values is not None and len(values) != 1) or axes.get(obj) is None: + raise ValueError( + "Held alignment needs an exact staging target and declared axis." + ) + routes[(obj, target, preserve)] = ( + obj, + target, + preserve, + axes[obj], + None if values is None else tuple(values[0]["position"]), + ) + if not routes: + return registration + factory = _AlignHeldFactory(tuple(routes.values())) + catalog = registration.call_catalog.with_descriptor( + SemanticCallDescriptor( + call_id=ALIGN_HELD_CALL, + spec_type=RegisteredSemanticCall, + target_descriptor=factory.target_descriptor, + ) + ) + presets = [] + for preset in registration.robot_profile_binding.presets: + options = dict(preset.action_option_templates) + options[ALIGN_HELD_CALL] = MoveHeldObjectOptions() + presets.append( + SkillPolicyPreset( + preset_id=preset.preset_id, + required_planner=preset.required_planner, + action_option_templates=options, + effect_monitors=preset.effect_monitors, + effect_assurance=preset.effect_assurance, + motion_policy=preset.motion_policy, + tracking_policy=preset.tracking_policy, + recovery_policy=preset.recovery_policy, + workflow_recovery_policy=preset.workflow_recovery_policy, + runner_cfg=preset.runner_cfg, + ) + ) + return replace( + registration, + call_catalog=catalog, + robot_profile_binding=replace( + registration.robot_profile_binding, presets=tuple(presets) + ), + registered_semantic_lowerer_factories=( + *registration.registered_semantic_lowerer_factories, + factory, + ), + ) diff --git a/embodichain/gen_sim/task_engine/_task_program/assembly.py b/embodichain/gen_sim/task_engine/_task_program/assembly.py new file mode 100644 index 000000000..534064f0c --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/assembly.py @@ -0,0 +1,256 @@ +# ---------------------------------------------------------------------------- +# 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-owned composition around the unchanged simulation runtime.""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, replace +from pathlib import Path +from typing import Any + +from embodichain.lab.task_program.integrations.environment import ( + TaskProgramEnvironmentAdapter, +) +from embodichain.lab.task_program.integrations.simulation.environment import ( + SimulationTaskProgramFactory, +) +from embodichain.utils.utility import load_config + +from ..contracts import canonical_hash +from .stability import StabilityConstraint, TaskStabilityPort +from .configured import compose_deployment +from .grasp_filter import GRASP_FILTER_REVISION, install_grasp_filters + +__all__: list[str] = [] + +ADAPTER_CONTRACT = "gen_sim.task_program/2620929c/v3" + + +class _TaskFactory(SimulationTaskProgramFactory): + """Select task-owned observation services, not a different executor.""" + + def __init__( + self, *args: Any, constraints: dict[str, StabilityConstraint], **kwargs: Any + ) -> None: + super().__init__(*args, **kwargs) + self._task_post_port = TaskStabilityPort( + self.segment_policy_port, + self._simulation, + self._robot, + self.task_program_registration.scene_binding, + constraints, + step_dt=self.step_dt, + ) + + def registration_owned_segment_policy_ports(self) -> tuple[Any, Any]: + return self._task_post_port, self.segment_policy_port + + +@dataclass(frozen=True, slots=True) +class TaskAdapterFactory: + """Immutable identity and lazy service construction for a GenSim bundle.""" + + registration: Any + integration_fingerprint: str + constraints: tuple[tuple[str, StabilityConstraint], ...] + grasp_factories: tuple[tuple[str, Any], ...] + cartesian_approaches: bool = False + + def create_adapter(self, environment: Any) -> TaskProgramEnvironmentAdapter: + """Return the exact shared adapter; no Session or Bridge is overridden.""" + self.registration.assert_unchanged() + motion_factory = None + if self.cartesian_approaches: + from embodichain.lab.sim.motion.motion_generator import MotionGenCfg + from embodichain.lab.sim.motion.planners import ToppraPlannerCfg + from .motion import ApproachMotionGenerator + + motion_factory = lambda: ApproachMotionGenerator( + MotionGenCfg( + planner_cfg=ToppraPlannerCfg(robot_uid=environment.robot.uid) + ) + ) + factory = _TaskFactory( + environment.sim, + environment.robot, + self.registration, + step_dt=environment.step_dt, + motion_generator_factory=motion_factory, + grasp_pose_generators=install_grasp_filters( + self.registration, + environment.sim, + {name: create() for name, create in self.grasp_factories}, + ), + constraints=dict(self.constraints), + ) + return factory.create_adapter() + + +def load_deployment( + *, task_program: object, skill_profile: object, base_dir: str | Path +) -> Any: + """Compose the current integration plus an explicitly versioned local contract.""" + base = compose_deployment( + task_program=task_program, + skill_profile=skill_profile, + base_dir=base_dir, + ) + path = Path(base_dir) / "task_program" / "constraints.json" + if not path.is_file(): + raise ValueError( + "GenSim bundle has no task constraints; regenerate the bundle." + ) + payload = load_config(path) + if ( + type(payload) is not dict + or set(payload) != {"schema_version", "presets"} + or payload["schema_version"] != "gen_sim_task_constraints/v1" + ): + raise ValueError( + "Unsupported GenSim task constraint format; regenerate the bundle." + ) + presets = payload["presets"] + if type(presets) is not dict or any( + type(name) is not str or not name.startswith("gen_sim.") or name != name.strip() + for name in presets + ): + raise ValueError("Task stability presets must have exact gen_sim.* names.") + constraints = { + name: StabilityConstraint.decode(cfg) for name, cfg in presets.items() + } + settle_presets = dict(base.integration.registration.settle_presets) + if set(settle_presets) & set(constraints): + raise ValueError("Task stability presets cannot replace core settling presets.") + for name in constraints: + settle_presets[name] = settle_presets["rigid_object"].snapshot() + registration = replace(base.integration.registration, settle_presets=settle_presets) + program = load_config(base.program_path) + from .align_held import with_held_alignment + + registration = with_held_alignment( + registration, program=program, constraints=constraints + ) + from .release_clearance import CLEAR_RELEASED_CALL, with_release_clearance + + registration = with_release_clearance(registration, program=program) + if any(cfg.kind == "stack" for cfg in constraints.values()): + from .stack_place import with_stack_placement + + registration = with_stack_placement( + registration, + targets=frozenset( + (cfg.entity, cfg.reference) + for cfg in constraints.values() + if cfg.kind == "stack" + ), + ) + grasp_factories = base.integration.grasp_factories + # The pad envelope is needed for thin-object coordinated grasps. Retain + # the conservative full-finger envelope for inclined single-arm grasps: + # this three-box model couples palm placement to the finger length. + # The configured Robotiq grasp command closes its pads to zero gap. + # Its reference 1 cm sampling cutoff excludes the original tray's + # measured 5-8 mm contacts. The URDF pad collision mesh spans 65 mm + # about the configured 0.2 m TCP, not the proxy's symmetric 130 mm. + # Keep palm/width/thickness checks and all physical commands unchanged. + coordinated = any( + item.get("steps", {}).get("call", {}).get("call_id") + in {"simulation.coordinated_hold", "simulation.coordinated_transport"} + for item in program["program"]["items"] + ) + grasp_factories = tuple( + ( + name, + ( + replace( + factory, + min_opening_width=( + min(factory.min_opening_width, 0.005) + if coordinated + else factory.min_opening_width + ), + finger_length=0.065 if coordinated else factory.finger_length, + ) + if factory.model_id == "robotiq_arg2f_140" + else factory + ), + ) + for name, factory in grasp_factories + ) + cartesian_approaches = any( + item.get("steps", {}).get("call", {}).get("kind") == "hand_over" + or item.get("steps", {}).get("call", {}).get("call_id") == CLEAR_RELEASED_CALL + for item in program["program"]["items"] + ) + fingerprint = canonical_hash( + { + "adapter_contract": ADAPTER_CONTRACT, + "grasp_filter_revision": GRASP_FILTER_REVISION, + "core_integration": base.integration.integration_fingerprint, + "registration": registration.fingerprint, + "task_constraints": payload, + "program": program, + "cartesian_approaches": cartesian_approaches, + "grasp_pose_generators": { + name: asdict(factory) for name, factory in grasp_factories + }, + } + ) + adapter = TaskAdapterFactory( + registration, + fingerprint, + tuple(constraints.items()), + grasp_factories, + cartesian_approaches, + ) + integration = replace( + base.integration, + registration=registration, + adapter_factory=adapter, + integration_fingerprint=fingerprint, + ) + return replace(base, integration=integration) + + +def register_deployment( + deployment: Any, *, environment_id: str, max_episode_steps: int +) -> None: + """Use the common environment and registry with a task-owned adapter factory.""" + from embodichain.lab.gym.envs import EmbodiedEnv + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + get_env_spec, + register_env_function, + ) + + if environment_id in REGISTERED_ENVS: + previous = get_env_spec(environment_id) + if ( + previous.cls is not EmbodiedEnv + or previous.task_program_adapter_factory.integration_fingerprint + != deployment.integration.integration_fingerprint + ): + raise ValueError( + "GenSim environment ID is already bound to another integration." + ) + return + register_env_function( + EmbodiedEnv, + environment_id, + max_episode_steps=max_episode_steps, + task_program_adapter_factory=deployment.integration.adapter_factory, + ) diff --git a/embodichain/gen_sim/task_engine/_task_program/configured.py b/embodichain/gen_sim/task_engine/_task_program/configured.py new file mode 100644 index 000000000..82da0b443 --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/configured.py @@ -0,0 +1,439 @@ +# ---------------------------------------------------------------------------- +# 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 Engine-owned E1-E5 service decoding and deployment composition.""" + +from __future__ import annotations + +from copy import deepcopy +from dataclasses import asdict, dataclass +from pathlib import Path +from typing import Any + +from embodichain.lab.task_program.integrations._configured_composition import ( + _compose_integration_payload, + _resolve_task_program_components, +) +from embodichain.lab.task_program.integrations.configured import ( + _mapping, + _identifier, + _identifier_tuple, + _sequence, + _finite_tuple, + _real, + _decode_scene, + _decode_robot_profile, + _decode_runtime_services, + _decode_registered_lowerer as _decode_shared_lowerer, +) +from embodichain.lab.task_program.integrations.catalog import ( + SimulationTaskProgramRegistration, +) +from embodichain.lab.task_program.language import TaskProgramIntegrationCfg +from embodichain.lab.task_program.semantics import ( + SemanticCallDescriptor, + RegisteredSemanticCall, + builtin_semantic_call_catalog, +) +from ..contracts import canonical_hash +from .services import ( + _AbsolutePoseTarget, + _SceneEntityTarget, + _PickRoute, + make_pick_factory, + _MoveHeldObjectRoute, + _MoveHeldObjectLowererFactory, + _CoordinatedTransportRoute, + _CoordinatedTransportLowererFactory, + _CoordinatedHoldLowererFactory, + _RelativePlaceRoute, + _RelativePlaceLowererFactory, +) + +__all__: list[str] = [] + + +def _decode_goal_pose( + value: object, *, path: str +) -> _AbsolutePoseTarget | _SceneEntityTarget: + """Decode one closed absolute or scene-relative target declaration.""" + common = _mapping( + value, + path=path, + required=frozenset({"kind"}), + optional=frozenset( + {"position", "quaternion_wxyz", "entity_id", "relative_pose"} + ), + ) + kind = _identifier(common["kind"], path=f"{path}.kind") + if kind == "pose": + config = _mapping( + value, + path=path, + required=frozenset({"kind", "position", "quaternion_wxyz"}), + ) + return _AbsolutePoseTarget( + _finite_tuple( + config["position"], path=f"{path}.position", expected_length=3 + ), + _finite_tuple( + config["quaternion_wxyz"], + path=f"{path}.quaternion_wxyz", + expected_length=4, + ), + ) + if kind == "scene_entity": + config = _mapping( + value, + path=path, + required=frozenset({"kind", "entity_id"}), + optional=frozenset({"relative_pose"}), + ) + return _SceneEntityTarget( + _identifier(config["entity_id"], path=f"{path}.entity_id"), + relative_pose=( + None + if "relative_pose" not in config + else _finite_tuple( + config["relative_pose"], + path=f"{path}.relative_pose", + expected_length=16, + ) + ), + ) + raise ValueError( + f"Unsupported {path}.kind {kind!r}; expected pose or scene_entity." + ) + + +def decode_task_lowerer(value: object, *, path: str) -> Any: + """Decode only the E1-E5 task-owned routes or established shared services.""" + if type(value) is not dict: + raise TypeError(f"{path} must be a mapping.") + kind = _identifier(value.get("kind"), path=f"{path}.kind") + if kind == "pick": + config = _mapping( + value, + path=path, + required=frozenset({"kind", "routes"}), + optional=frozenset({"call_id"}), + ) + call_id = _identifier( + config.get("call_id", "simulation.pick"), path=f"{path}.call_id" + ) + if call_id != "simulation.pick" and not call_id.startswith("gen_sim.pick."): + raise ValueError("Task Pick aliases must use the gen_sim.pick namespace.") + routes: list[_PickRoute] = [] + for index, raw in enumerate(_sequence(config["routes"], path=f"{path}.routes")): + route_path = f"{path}.routes[{index}]" + route = _mapping( + raw, + path=route_path, + required=frozenset({"object_id", "target_id"}), + optional=frozenset( + { + "release_clearance_object_pose", + "release_clearance_plane_z", + "release_clearance_safety_margin", + "grasp_region", + } + ), + ) + routes.append( + _PickRoute( + object_id=_identifier( + route["object_id"], path=f"{route_path}.object_id" + ), + target_id=_identifier( + route["target_id"], path=f"{route_path}.target_id" + ), + release_clearance_object_pose=( + None + if "release_clearance_object_pose" not in route + else _decode_goal_pose( + route["release_clearance_object_pose"], + path=f"{route_path}.release_clearance_object_pose", + ) + ), + release_clearance_plane_z=( + None + if "release_clearance_plane_z" not in route + else _real( + route["release_clearance_plane_z"], + path=f"{route_path}.release_clearance_plane_z", + ) + ), + release_clearance_safety_margin=_real( + route.get("release_clearance_safety_margin", 0.0), + path=f"{route_path}.release_clearance_safety_margin", + minimum=0.0, + ), + grasp_region=( + None + if "grasp_region" not in route + else _identifier( + route["grasp_region"], path=f"{route_path}.grasp_region" + ) + ), + ) + ) + return make_pick_factory(tuple(routes), call_id) + if kind == "move_held_object": + config = _mapping(value, path=path, required=frozenset({"kind", "routes"})) + routes: list[_MoveHeldObjectRoute] = [] + for index, raw in enumerate(_sequence(config["routes"], path=f"{path}.routes")): + route_path = f"{path}.routes[{index}]" + route = _mapping( + raw, + path=route_path, + required=frozenset({"object_id", "target_id", "pose"}), + ) + routes.append( + _MoveHeldObjectRoute( + object_id=_identifier( + route["object_id"], path=f"{route_path}.object_id" + ), + target_id=_identifier( + route["target_id"], path=f"{route_path}.target_id" + ), + pose=_decode_goal_pose(route["pose"], path=f"{route_path}.pose"), + ) + ) + return _MoveHeldObjectLowererFactory(tuple(routes)) + if kind in {"coordinated_transport", "coordinated_hold"}: + config = _mapping( + value, + path=path, + required=frozenset({"kind", "routes"}), + ) + routes: list[_CoordinatedTransportRoute] = [] + for index, route_value in enumerate( + _sequence(config["routes"], path=f"{path}.routes") + ): + route_path = f"{path}.routes[{index}]" + route = _mapping( + route_value, + path=route_path, + required=frozenset({"object_id", "target_id"}), + optional=frozenset( + { + "reference_entity_id", + "relative_pose", + "world_displacement", + } + ), + ) + has_reference = "reference_entity_id" in route + has_pose = "relative_pose" in route + has_displacement = "world_displacement" in route + if has_reference != has_pose or has_reference == has_displacement: + raise ValueError( + f"{route_path} must declare exactly one of " + "reference_entity_id with relative_pose or " + "world_displacement." + ) + routes.append( + _CoordinatedTransportRoute( + object_id=_identifier( + route["object_id"], path=f"{route_path}.object_id" + ), + target_id=_identifier( + route["target_id"], path=f"{route_path}.target_id" + ), + reference_entity_id=( + _identifier( + route["reference_entity_id"], + path=f"{route_path}.reference_entity_id", + ) + if has_reference + else None + ), + relative_pose=( + _finite_tuple( + route["relative_pose"], + path=f"{route_path}.relative_pose", + expected_length=16, + ) + if has_pose + else None + ), + world_displacement=( + _finite_tuple( + route["world_displacement"], + path=f"{route_path}.world_displacement", + expected_length=3, + ) + if has_displacement + else None + ), + ) + ) + factory_type = ( + _CoordinatedTransportLowererFactory + if kind == "coordinated_transport" + else _CoordinatedHoldLowererFactory + ) + return factory_type(routes=tuple(routes)) + if kind == "place_relative": + config = _mapping( + value, + path=path, + required=frozenset({"kind", "routes"}), + ) + routes: list[_RelativePlaceRoute] = [] + for index, route_value in enumerate( + _sequence(config["routes"], path=f"{path}.routes") + ): + route_path = f"{path}.routes[{index}]" + route = _mapping( + route_value, + path=route_path, + required=frozenset( + { + "object_id", + "reference_entity_id", + "relation", + "world_displacement", + } + ), + ) + routes.append( + _RelativePlaceRoute( + object_id=_identifier( + route["object_id"], + path=f"{route_path}.object_id", + ), + reference_entity_id=_identifier( + route["reference_entity_id"], + path=f"{route_path}.reference_entity_id", + ), + relation=_identifier( + route["relation"], + path=f"{route_path}.relation", + ), + world_displacement=_finite_tuple( + route["world_displacement"], + path=f"{route_path}.world_displacement", + expected_length=3, + ), + ) + ) + return _RelativePlaceLowererFactory(routes=tuple(routes)) + if kind in {"park", "pour", "axis_align"}: + return _decode_shared_lowerer(value, path=path) + raise ValueError( + f"{path}: unsupported Task Engine service {kind!r}; expected E1-E5 services." + ) + + +@dataclass(frozen=True, slots=True) +class TaskIntegration: + registration: SimulationTaskProgramRegistration + integration_fingerprint: str + grasp_factories: tuple[tuple[str, Any], ...] + adapter_factory: Any = None + + +@dataclass(frozen=True, slots=True) +class TaskDeployment: + integration_id: str + program_id: str + program_path: Path + selection: TaskProgramIntegrationCfg + integration: TaskIntegration + scene_binding: dict[str, Any] + + +def compose_deployment( + *, task_program: object, skill_profile: object, base_dir: str | Path +) -> TaskDeployment: + """Reuse component contracts while owning E1-E5 service decoding locally.""" + program_path, task, policy = _resolve_task_program_components( + task_program, + base_dir=Path(base_dir).expanduser(), + ) + skill_profile = _mapping( + skill_profile, + path="embodiment component.skill_profile", + required=frozenset( + {"contract_id", "profile_id", "resources", "command_presets"} + ), + optional=frozenset({"runtime_services"}), + ) + scene = _mapping( + task["scene_binding"], + path="task integration.scene_binding", + required=frozenset({"contract_id", "registry_id"}), + optional=frozenset( + {"rigid_objects", "articulations", "links", "collision_world_mode"} + ), + ) + payload = _compose_integration_payload( + task=task, policy=policy, skill_profile=skill_profile, scene=scene + ) + raw_services = deepcopy(payload.get("runtime_services", {})) + raw_lowerers = raw_services.pop("registered_semantic_lowerers", ()) + lowerers = tuple( + decode_task_lowerer( + value, path=f"runtime_services.registered_semantic_lowerers[{i}]" + ) + for i, value in enumerate( + _sequence(raw_lowerers, path="registered_semantic_lowerers") + ) + ) + services = _decode_runtime_services(raw_services) + call_catalog = builtin_semantic_call_catalog() + for factory in lowerers: + call_catalog = call_catalog.with_descriptor( + SemanticCallDescriptor( + call_id=factory.call_id, + spec_type=RegisteredSemanticCall, + target_descriptor=factory.target_descriptor, + ) + ) + registration = SimulationTaskProgramRegistration( + scene_binding=_decode_scene(payload["scene"]), + robot_profile_binding=_decode_robot_profile(payload["robot_profile"]), + call_catalog=call_catalog, + handover_pose_providers=services.handover_pose_providers, + control_part_evidence_factory=services.control_part_evidence, + registered_semantic_lowerer_factories=lowerers, + ) + fingerprint = canonical_hash( + { + "local_decoder_revision": 1, + "registration": registration.fingerprint, + "grasp_pose_generators": { + key: asdict(value) for key, value in services.grasp_pose_generators + }, + } + ) + return TaskDeployment( + integration_id=_identifier(task["integration_id"], path="integration_id"), + program_id=_identifier(task["program_id"], path="program_id"), + program_path=program_path, + selection=TaskProgramIntegrationCfg( + robot_profile=registration.robot_profile_binding.profile_id, + scene_registry=registration.scene_binding.registry_id, + runtime_preset=_identifier( + policy["preset_id"], path="execution_policy.preset_id" + ), + ), + integration=TaskIntegration( + registration, fingerprint, services.grasp_pose_generators + ), + scene_binding=deepcopy(dict(scene)), + ) diff --git a/embodichain/gen_sim/task_engine/_task_program/grasp_filter.py b/embodichain/gen_sim/task_engine/_task_program/grasp_filter.py new file mode 100644 index 000000000..5f8756f0c --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/grasp_filter.py @@ -0,0 +1,264 @@ +# ---------------------------------------------------------------------------- +# 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-owned grasp filtering through the baseline sampling-service interface.""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +from typing import Any + +import torch + +from embodichain.toolkits.graspkit import ParallelJawGraspPoseGenerator +from embodichain.lab.task_program.semantics import ( + GRASP_AFFORDANCE_CAPABILITY, + SceneObjectRef, +) +from embodichain.utils import logger +from embodichain.utils.math import pose_inv + +__all__: list[str] = [] + +GRASP_FILTER_REVISION = 1 + + +def geometry_key(vertices: torch.Tensor, triangles: torch.Tensor) -> str: + """Match immutable local geometry, not object world poses or mutable call state.""" + digest = hashlib.sha256() + for value, dtype in ((vertices, torch.float32), (triangles, torch.int64)): + data = value.detach().to(device="cpu", dtype=dtype).contiguous() + digest.update(str(tuple(data.shape)).encode("ascii")) + digest.update(data.numpy().tobytes()) + return digest.hexdigest() + + +@dataclass(frozen=True, slots=True) +class GraspRule: + object_id: str + target_id: str + geometry: str + local_axis: tuple[float, float, float] + midpoint: float + upper_half: bool + release_pose: tuple[float, ...] | None + plane_z: float | None + margin: float + + +def accepted_candidates( + poses: torch.Tensor, + object_pose: torch.Tensor, + rule: GraspRule, + model: Any, +) -> torch.Tensor: + """Check each candidate in the object frame and at its declared release pose.""" + object_to_grasp = pose_inv(object_pose) @ poses + accepted = torch.isfinite(poses).all(dim=-1).all(dim=-1) + if rule.upper_half: + projected = object_to_grasp[:, :3, 3] @ poses.new_tensor(rule.local_axis) + accepted &= projected > rule.midpoint + if rule.release_pose is not None: + final_grasp = ( + poses.new_tensor(rule.release_pose).reshape(4, 4) @ object_to_grasp + ) + opening = float(model.max_opening_width) + corners = poses.new_tensor( + [ + [side * opening + dx, dy, dz] + for side in (-1.0, 1.0) + for dx in (-0.5 * model.finger_thickness, 0.5 * model.finger_thickness) + for dy in (-0.5 * model.finger_width, 0.5 * model.finger_width) + for dz in (-0.5 * model.finger_length, 0.5 * model.finger_length) + ] + ) + world = ( + corners @ final_grasp[:, :3, :3].transpose(-1, -2) + + final_grasp[:, None, :3, 3] + ) + assert rule.plane_z is not None + accepted &= world[..., 2].amin(-1) > rule.plane_z + rule.margin + return accepted + + +class TaskGraspPoseGenerator(ParallelJawGraspPoseGenerator): + """Filter unrestricted single-arm proposals; preserve other generator protocols. + + Rules are fixed at assembly. End-specific HandOver/stack proposals and dual + grasps retain their own baseline selection contracts. This provider owns no + execution cursor, held relation, eligibility state, or recovery policy. + """ + + def __init__( + self, delegate: ParallelJawGraspPoseGenerator, rules: tuple[GraspRule, ...] + ) -> None: + super().__init__(delegate.gripper_model) + self._delegate = delegate + self._rules = rules + keys = {rule.geometry for rule in rules} + self._by_geometry = { + key: tuple(rule for rule in rules if rule.geometry == key) for key in keys + } + + def require_rule( + self, + object_id: str, + target_id: str, + vertices: torch.Tensor, + triangles: torch.Tensor, + ) -> None: + key = geometry_key(vertices, triangles) + if not any( + rule.object_id == object_id + and rule.target_id == target_id + and rule.geometry == key + for rule in self._rules + ): + raise ValueError("Constrained Pick has no matching immutable grasp filter.") + + def get_valid_grasp_poses( + self, + *, + mesh_vertices: torch.Tensor, + mesh_triangles: torch.Tensor, + obj_poses: torch.Tensor, + approach_direction: torch.Tensor, + obj_longest_axis: torch.Tensor | None = None, + is_positive_part: bool | torch.Tensor = True, + ) -> list[tuple[torch.Tensor, torch.Tensor]]: + results = self._delegate.get_valid_grasp_poses( + mesh_vertices=mesh_vertices, + mesh_triangles=mesh_triangles, + obj_poses=obj_poses, + approach_direction=approach_direction, + obj_longest_axis=obj_longest_axis, + is_positive_part=is_positive_part, + ) + if obj_longest_axis is not None: + return results + rules = self._by_geometry.get(geometry_key(mesh_vertices, mesh_triangles), ()) + if not rules: + return results + if obj_poses.shape != (len(results), 4, 4): + raise ValueError("Grasp filter rows must match the observed object poses.") + filtered = [] + counts = [] + for row, (poses, costs) in enumerate(results): + if ( + poses.ndim != 3 + or poses.shape[1:] != (4, 4) + or costs.shape != poses.shape[:1] + ): + raise ValueError( + "Grasp proposals must have matching pose and cost rows." + ) + costs = costs.to(poses.device) + accepted = torch.isfinite(costs) + for rule in rules: + accepted &= accepted_candidates( + poses, obj_poses[row].to(poses), rule, self.gripper_model + ) + counts.append(int(accepted.sum())) + if accepted.any(): + filtered.append((poses[accepted].clone(), costs[accepted].clone())) + else: + # The baseline sampler represents a failed row by infinite cost. + # A finite padding pose is never eligible for a command or effect. + padding = ( + poses[:1] if poses.shape[0] else obj_poses[row : row + 1].to(poses) + ) + filtered.append((padding.clone(), poses.new_full((1,), torch.inf))) + logger.log_info(f"GenSim grasp filter candidate counts: {counts}.") + return filtered + + def get_best_grasp_poses( + self, **kwargs: Any + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Retain the baseline articulation-service protocol, not used by GenSim Pick.""" + return self._delegate.get_best_grasp_poses(**kwargs) + + def get_dual_arm_valid_grasp_poses(self, **kwargs: Any) -> Any: + """Coordinated grasps have a separate, unchanged paired-contact contract.""" + return self._delegate.get_dual_arm_valid_grasp_poses(**kwargs) + + +def install_grasp_filters( + registration: Any, simulation: Any, generators: dict[str, Any] +) -> dict[str, Any]: + """Assemble immutable geometry rules through the existing public scene binding.""" + routes = tuple( + route + for factory in registration.registered_semantic_lowerer_factories + if factory.call_id == "simulation.pick" + or factory.call_id.startswith("gen_sim.pick.") + for route in factory.routes + if route.grasp_region is not None + or route.release_clearance_object_pose is not None + ) + if not routes: + return generators + registry = registration.scene_binding.build(simulation) + registration.validate_scene_registry(registry) + rules = [] + for route in routes: + ref = registry.resolve(route.object_id, expected_type=SceneObjectRef) + grasp = registry.resolve_affordance(ref, capability=GRASP_AFFORDANCE_CAPABILITY) + sem = registry.object_semantics(ref, affordance=grasp) + affordance = sem.affordance + axis = getattr(affordance, "internal_axis", None) + if route.grasp_region is not None and axis is None: + raise ValueError("Upper-half filtering requires a declared local axis.") + axis = torch.tensor([0.0, 0.0, 1.0]) if axis is None else axis.detach().cpu() + axis = torch.nn.functional.normalize(axis, dim=-1) + projection = affordance.mesh_vertices.detach().cpu() @ axis + release = route.release_clearance_object_pose + if release is not None and not callable(getattr(release, "to_matrix", None)): + raise ValueError( + "Grasp filtering requires an explicit absolute release pose." + ) + rules.append( + GraspRule( + route.object_id, + route.target_id, + geometry_key(affordance.mesh_vertices, affordance.mesh_triangles), + tuple(float(x) for x in axis), + float((projection.amin() + projection.amax()) * 0.5), + route.grasp_region == "upper_half", + ( + None + if release is None + else tuple(float(x) for x in release.to_matrix().reshape(-1)) + ), + route.release_clearance_plane_z, + route.release_clearance_safety_margin, + ) + ) + for preset in registration.robot_profile_binding.presets: + for options in preset.action_option_templates.values(): + if getattr(options, "rotate_upright", None) is not None: + raise ValueError( + "Post-sampling grasp rotation is incompatible with fixed grasp filters." + ) + for generator in generators.values(): + if not isinstance(generator, ParallelJawGraspPoseGenerator): + raise TypeError( + "Task grasp filtering requires calibrated parallel-jaw generators." + ) + return { + name: TaskGraspPoseGenerator(generator, tuple(rules)) + for name, generator in generators.items() + } diff --git a/embodichain/gen_sim/task_engine/_task_program/motion.py b/embodichain/gen_sim/task_engine/_task_program/motion.py new file mode 100644 index 000000000..fde2b0206 --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/motion.py @@ -0,0 +1,100 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Cartesian approach sampling as a task-owned motion-planning policy.""" + +from __future__ import annotations + +from dataclasses import replace + +import torch + +from embodichain.lab.sim.motion.motion_generator import ( + MotionGenOptions, + MotionGenerator, +) +from embodichain.lab.sim.motion.planners.utils import ( + MoveType, + PlanResult, + PlanState, + interpolate_xpos_batched, +) + +__all__: list[str] = [] + + +def _cartesian_samples( + start: torch.Tensor, + targets: list[PlanState], + count: int, +) -> list[PlanState]: + """Include every supplied waypoint without changing the output time budget.""" + if count - 1 < len(targets): + raise ValueError("Cartesian approach sampling needs one sample per target.") + result: list[PlanState] = [] + previous = start + remaining = count - 1 + for index, target in enumerate(targets): + assert target.xpos is not None + pose = target.xpos.to(device=start.device, dtype=start.dtype) + if pose.ndim == 2: + pose = pose.unsqueeze(0).expand(start.shape[0], -1, -1) + if pose.shape != start.shape: + raise ValueError("Cartesian approach targets must match the start batch.") + intervals = remaining // (len(targets) - index) + interpolated = interpolate_xpos_batched(previous, pose, intervals + 1) + result.extend( + PlanState(move_type=MoveType.EEF_MOVE, xpos=interpolated[:, point]) + for point in range(1, intervals + 1) + ) + previous = pose + remaining -= intervals + return result + + +class ApproachMotionGenerator(MotionGenerator): + """Keep multi-waypoint EEF approaches Cartesian; the core solves every sample.""" + + def generate( + self, + target_states: list[PlanState], + options: MotionGenOptions | None = None, + ) -> PlanResult: + if ( + options is not None + and options.strategy == "ik_interp" + and not options.preserve_cartesian_samples + and len(target_states) > 1 + and all(state.move_type is MoveType.EEF_MOVE for state in target_states) + ): + if ( + options.start_qpos is None + or options.control_part is None + or options.sample_count is None + ): + raise ValueError( + "Approach planning requires a bound start and sample count." + ) + start = self.robot.compute_fk( + qpos=options.start_qpos, + name=options.control_part, + to_matrix=True, + ) + target_states = _cartesian_samples( + start, target_states, options.sample_count + ) + options = replace(options, preserve_cartesian_samples=True, is_linear=True) + return super().generate(target_states, options=options) diff --git a/embodichain/gen_sim/task_engine/_task_program/release_clearance.py b/embodichain/gen_sim/task_engine/_task_program/release_clearance.py new file mode 100644 index 000000000..533c6187d --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/release_clearance.py @@ -0,0 +1,209 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Bind a released hand's declared clearance route to the shared Cartesian skill.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import math +from typing import Any, ClassVar + +import torch + +from embodichain.lab.sim.atomic_actions import ( + EndEffectorPoseGoal, + MoveEndEffector, + MoveEndEffectorOptions, +) +from embodichain.lab.task_program.compiler.lowering import ( + RegisteredSemanticLowerer, + SemanticLowering, +) +from embodichain.lab.task_program.semantics import ( + RegisteredSemanticCall, + SceneObjectRef, + SemanticCallDescriptor, + SkillPolicyPreset, +) + +__all__: list[str] = [] + +CLEAR_RELEASED_CALL = "gen_sim.clear_released" + + +def _clearance_poses( + current: torch.Tensor, + root: torch.Tensor, + *, + height: float, + retreat: float, +) -> torch.Tensor: + """First clear vertically, then move toward the owning arm's root.""" + raised = current.clone() + raised[:, 2, 3] = torch.clamp_min(raised[:, 2, 3], height) + direction = root[:, :2, 3] - current[:, :2, 3] + direction = torch.nn.functional.normalize(direction, dim=-1) + withdrawn = raised.clone() + withdrawn[:, :2, 3] += retreat * direction + return torch.stack((raised, withdrawn), dim=1) + + +class _ClearReleasedLowerer(RegisteredSemanticLowerer): + call_id: ClassVar[str] = CLEAR_RELEASED_CALL + target_descriptor = MoveEndEffector.descriptor() + preserves_symbolic_state: ClassVar[bool] = True + + def __init__( + self, + routes: tuple[tuple[str, str, float], ...], + robot: Any, + *, + retreat_distance: float, + ) -> None: + self._routes = {(obj, target): height for obj, target, height in routes} + self._robot = robot + self._retreat = retreat_distance + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: Any, + bound: Any, + option_template: Any, + ) -> SemanticLowering: + if type(option_template) is not MoveEndEffectorOptions: + raise TypeError("Released-hand clearance requires MoveEndEffectorOptions.") + arguments = dict(call.arguments) + if set(arguments) != {"object", "target"}: + raise ValueError("Released-hand clearance requires only object and target.") + selector = (arguments["object"], arguments["target"]) + if selector not in self._routes: + raise ValueError("Released-hand clearance has no declared route.") + endpoint = bound.binding.resources["primary"].endpoints["motion"] + held = context.task.get_held_object(endpoint.task_state_key) + if held is not None and held.active_mask.any(): + raise ValueError("Released-hand clearance cannot move a held object.") + motion = endpoint.runtime_target + current = self._robot.compute_fk( + qpos=context.robot.qpos[:, list(motion.joint_ids)], + name=motion.control_part, + to_matrix=True, + ) + root = self._robot.get_link_pose( + link_name=self._robot.cfg.solver_cfg[motion.control_part].root_link_name, + env_ids=context.env_ids.tolist(), + to_matrix=True, + ).to(current) + return SemanticLowering( + goal=EndEffectorPoseGoal( + _clearance_poses( + current, + root, + height=self._routes[selector], + retreat=self._retreat, + ) + ) + ) + + +@dataclass(frozen=True, slots=True) +class _ClearReleasedFactory: + call_id: ClassVar[str] = CLEAR_RELEASED_CALL + revision: ClassVar[str] = "1" + target_descriptor = MoveEndEffector.descriptor() + routes: tuple[tuple[str, str, float], ...] + retreat_distance: float = 0.10 + + def create( + self, *, simulation: Any, robot: Any, scene_registry: Any, engine: Any + ) -> _ClearReleasedLowerer: + if engine.robot is not robot: + raise ValueError("Released-hand clearance must bind the factory's robot.") + for obj, _, _ in self.routes: + scene_registry.resolve(obj, expected_type=SceneObjectRef) + return _ClearReleasedLowerer( + self.routes, robot, retreat_distance=self.retreat_distance + ) + + +def with_release_clearance(registration: Any, *, program: dict[str, Any]) -> Any: + """Register only generated clearance routes, with no execution overrides.""" + routes = {} + for item in program["program"]["items"]: + call = item["steps"]["call"] + if call.get("call_id") != CLEAR_RELEASED_CALL: + continue + args = call["arguments"] + if set(args) != {"object", "target"}: + raise ValueError("Released-hand clearance requires only object and target.") + values = program["targets"][args["target"]]["values"] + if len(values) != 1: + raise ValueError( + "Released-hand clearance requires one exact staging target." + ) + height = values[0]["position"][2] + if ( + isinstance(height, bool) + or not isinstance(height, (float, int)) + or not math.isfinite(height) + ): + raise ValueError("Released-hand clearance height must be finite.") + routes[(args["object"], args["target"])] = ( + args["object"], + args["target"], + float(height), + ) + if not routes: + return registration + factory = _ClearReleasedFactory(tuple(routes.values())) + catalog = registration.call_catalog.with_descriptor( + SemanticCallDescriptor( + call_id=CLEAR_RELEASED_CALL, + spec_type=RegisteredSemanticCall, + target_descriptor=factory.target_descriptor, + ) + ) + presets = [] + for preset in registration.robot_profile_binding.presets: + options = dict(preset.action_option_templates) + options[CLEAR_RELEASED_CALL] = MoveEndEffectorOptions() + presets.append( + SkillPolicyPreset( + preset_id=preset.preset_id, + required_planner=preset.required_planner, + action_option_templates=options, + effect_monitors=preset.effect_monitors, + effect_assurance=preset.effect_assurance, + motion_policy=preset.motion_policy, + tracking_policy=preset.tracking_policy, + recovery_policy=preset.recovery_policy, + workflow_recovery_policy=preset.workflow_recovery_policy, + runner_cfg=preset.runner_cfg, + ) + ) + return replace( + registration, + call_catalog=catalog, + robot_profile_binding=replace( + registration.robot_profile_binding, presets=tuple(presets) + ), + registered_semantic_lowerer_factories=( + *registration.registered_semantic_lowerer_factories, + factory, + ), + ) diff --git a/embodichain/gen_sim/task_engine/_task_program/services.py b/embodichain/gen_sim/task_engine/_task_program/services.py new file mode 100644 index 000000000..f87e97ade --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/services.py @@ -0,0 +1,877 @@ +# ---------------------------------------------------------------------------- +# 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-owned immutable route declarations and skill binding services.""" + +from __future__ import annotations +from dataclasses import dataclass, make_dataclass +import hashlib +import math +from typing import Any, ClassVar +import torch +from embodichain.lab.task_program.integrations.extensions import ( + RegisteredSemanticLowererFactory, +) +from embodichain.lab.sim.atomic_actions import ( + ActionOptions, + AtomicActionEngine, + CoordinatedPickGoal, + CoordinatedPickment, + CoordinatedPickmentOptions, + HeldObjectPoseGoal, + MoveHeldObject, + MoveHeldObjectOptions, + ObjectSemantics, + Place, + PlanningContext, + SceneEntityPose, + SkillDescriptor, + GraspGoal, + PickUp, + PickUpOptions, +) +from embodichain.lab.task_program.semantics import ( + BoundSemanticCall, + GRASP_AFFORDANCE_CAPABILITY, + HeldObjectRelation, + RegisteredSemanticCall, + SceneObjectRef, + SceneRegistry, + SemanticEffectKind, + SemanticPose, +) +from embodichain.lab.task_program.compiler.lowering import ( + RegisteredHeldObjectEffect, + RegisteredSemanticLowerer, + RegisteredSemanticEffect, + SemanticLowering, + SemanticObjectTarget, +) + +from embodichain.lab.task_program.integrations._configured_services import ( + _CoordinatedTransportRoute, + _RelativePlaceLowerer, + _coordinated_transport_route, + _identifier, + _pose, + _world_displacement, +) + +from .grasp_filter import TaskGraspPoseGenerator + +__all__: list[str] = [] + +_COORDINATED_TRANSPORT_CALL_ID = "simulation.coordinated_transport" + + +_MOVE_HELD_OBJECT_CALL_ID = "simulation.move_held_object" + + +_PLACE_RELATIVE_CALL_ID = "simulation.place_relative" + + +_COORDINATED_HOLD_CALL_ID = "simulation.coordinated_hold" + + +_PICK_CALL_ID = "simulation.pick" + + +def _point( + value: tuple[float, float, float], + *, + field_name: str, +) -> tuple[float, float, float]: + """Validate one finite local-frame point.""" + if type(value) is not tuple or len(value) != 3: + raise TypeError(f"{field_name} must be an exact three-value tuple.") + normalized = tuple(float(item) for item in value) + if not all(math.isfinite(item) for item in normalized): + raise ValueError(f"{field_name} must contain three finite values.") + return normalized + + +@dataclass(frozen=True, slots=True) +class _SceneEntityTarget: + """Deeply immutable scene-relative target stored in an integration.""" + + entity_id: str + relative_pose: tuple[float, ...] | None = None + + def __post_init__(self) -> None: + _identifier(self.entity_id, field_name="entity_id") + if self.relative_pose is not None: + object.__setattr__(self, "relative_pose", _pose(self.relative_pose)) + + def snapshot(self) -> _SceneEntityTarget: + """Return the immutable declaration, which owns no runtime tensors.""" + return self + + +@dataclass(frozen=True, slots=True) +class _AbsolutePoseTarget: + """Tensor-free absolute pose stored in a fingerprinted declaration.""" + + position: tuple[float, float, float] + quaternion_wxyz: tuple[float, float, float, float] + + def __post_init__(self) -> None: + object.__setattr__( + self, "position", _point(self.position, field_name="position") + ) + quaternion = self.quaternion_wxyz + if ( + type(quaternion) is not tuple + or len(quaternion) != 4 + or any( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(value) + for value in quaternion + ) + ): + raise ValueError("quaternion_wxyz must contain four finite real values.") + norm = math.hypot(*quaternion) + if norm <= 1.0e-8: + raise ValueError("quaternion_wxyz must be non-zero.") + object.__setattr__( + self, "quaternion_wxyz", tuple(value / norm for value in quaternion) + ) + + def snapshot(self) -> _AbsolutePoseTarget: + """Return the deeply immutable target declaration.""" + return self + + def to_matrix(self) -> torch.Tensor: + """Create an independently owned runtime pose.""" + return SemanticPose(self.position, self.quaternion_wxyz).to_matrix() + + +def _configured_goal_pose( + value: _AbsolutePoseTarget | _SceneEntityTarget, +) -> torch.Tensor | SceneEntityPose: + """Convert an inert configured pose without observing or choosing a target.""" + if type(value) is _AbsolutePoseTarget: + return value.to_matrix() + if type(value) is _SceneEntityTarget: + return SceneEntityPose( + value.entity_id, + relative_pose=( + None + if value.relative_pose is None + else torch.tensor(value.relative_pose, dtype=torch.float32).reshape( + 4, 4 + ) + ), + ) + raise TypeError( + "Configured targets must be _AbsolutePoseTarget or _SceneEntityTarget." + ) + + +@dataclass(frozen=True, slots=True) +class _PickRoute: + """One named set of target-dependent pickup constraints.""" + + object_id: str + target_id: str + release_clearance_object_pose: _AbsolutePoseTarget | _SceneEntityTarget | None = ( + None + ) + release_clearance_plane_z: float | None = None + release_clearance_safety_margin: float = 0.0 + grasp_region: str | None = None + + def __post_init__(self) -> None: + _identifier(self.object_id, field_name="object_id") + _identifier(self.target_id, field_name="target_id") + if (self.release_clearance_object_pose is None) != ( + self.release_clearance_plane_z is None + ): + raise ValueError( + "Release-clearance pose and plane must be provided together." + ) + if self.release_clearance_object_pose is not None: + _configured_goal_pose(self.release_clearance_object_pose) + object.__setattr__( + self, + "release_clearance_object_pose", + self.release_clearance_object_pose.snapshot(), + ) + for name in ("release_clearance_plane_z", "release_clearance_safety_margin"): + value = getattr(self, name) + if value is not None and ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(value) + ): + raise ValueError(f"{name} must be a finite real number.") + if self.release_clearance_safety_margin < 0.0: + raise ValueError("release_clearance_safety_margin must be non-negative.") + if self.grasp_region not in {None, "upper_half"}: + raise ValueError("Task grasp_region must be upper_half or omitted.") + if ( + self.release_clearance_object_pose is not None + and type(self.release_clearance_object_pose) is not _AbsolutePoseTarget + ): + raise ValueError( + "Task release-clearance filtering requires an absolute pose." + ) + + +class _PickLowerer(RegisteredSemanticLowerer): + """Bind configured target constraints to the shared pickup goal.""" + + call_id: ClassVar[str] = _PICK_CALL_ID + target_descriptor: ClassVar[SkillDescriptor] = PickUp.descriptor() + effect_contract_kind: ClassVar[SemanticEffectKind] = SemanticEffectKind.ATTACH + + def __init__( + self, + routes: tuple[_PickRoute, ...], + semantics: tuple[ObjectSemantics, ...], + grasp_generators: Any = None, + call_id: str | None = None, + ) -> None: + self._routes = {(route.object_id, route.target_id): route for route in routes} + self._semantics = {item.entity_id: item for item in semantics} + if call_id is not None: + self.call_id = call_id + self._grasp_generators = ( + {} if grasp_generators is None else dict(grasp_generators) + ) + if not routes or len(self._routes) != len(routes): + raise ValueError("Configured Pick routes must be non-empty and unique.") + if len(self._semantics) != len(semantics) or any( + route.object_id not in self._semantics for route in routes + ): + raise ValueError( + "Configured Pick routes require unique matching object semantics." + ) + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: ActionOptions, + ) -> SemanticLowering: + """Bind task declarations to baseline goal/options and the installed sampler.""" + if type(option_template) is not PickUpOptions: + raise TypeError("Configured Pick requires exact PickUpOptions.") + arguments = dict(call.arguments) + if set(arguments) != {"object", "target"}: + raise ValueError( + "Configured Pick arguments must contain only object and target." + ) + key = (arguments["object"], arguments["target"]) + route = self._routes.get(key) + if route is None: + raise ValueError(f"Configured Pick has no route {key!r}.") + semantics = self._semantics[route.object_id] + if ( + route.grasp_region is not None + or route.release_clearance_object_pose is not None + ): + if ( + option_template.pick_object_part != "center" + or option_template.rotate_upright is not None + or option_template.fixed_object_to_eef is not None + ): + raise ValueError( + "Constrained Pick requires unrotated, unrestricted baseline sampling." + ) + target = ( + bound.binding.resources["primary"].endpoints["grasp"].runtime_target + ) + sampler = self._grasp_generators.get(target.target_id) + if not isinstance(sampler, TaskGraspPoseGenerator): + raise ValueError( + "Constrained Pick requires the task-owned grasp filter." + ) + sampler.require_rule( + route.object_id, + route.target_id, + semantics.affordance.mesh_vertices, + semantics.affordance.mesh_triangles, + ) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + registered_effect=RegisteredSemanticEffect( + effect_kind=SemanticEffectKind.ATTACH, + held_objects=( + RegisteredHeldObjectEffect( + expectation_id="primary", + relation=HeldObjectRelation.ATTACHED, + object_id=route.object_id, + slot_id="primary", + ), + ), + ), + ) + + +@dataclass(frozen=True, slots=True) +class _PickLowererFactory(RegisteredSemanticLowererFactory): + """Resolve canonical objects for configured pickup targets.""" + + revision: ClassVar[str] = "2" + target_descriptor: ClassVar[SkillDescriptor] = PickUp.descriptor() + routes: tuple[_PickRoute, ...] + call_id: ClassVar[str] = _PICK_CALL_ID + lowerer_type: ClassVar[type[_PickLowerer]] = _PickLowerer + + def __post_init__(self) -> None: + if ( + type(self.routes) is not tuple + or not self.routes + or not all(type(route) is _PickRoute for route in self.routes) + ): + raise TypeError("routes must be a non-empty tuple of _PickRoute values.") + if len({(route.object_id, route.target_id) for route in self.routes}) != len( + self.routes + ): + raise ValueError("Configured Pick routes must be unique.") + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> RegisteredSemanticLowerer: + """Resolve grasp semantics without choosing grasp regions or directions.""" + del simulation + if engine.robot is not robot: + raise ValueError("Configured Pick requires the engine's exact robot.") + semantics = [] + for object_id in dict.fromkeys(route.object_id for route in self.routes): + ref = scene_registry.resolve(object_id, expected_type=SceneObjectRef) + grasp = scene_registry.resolve_affordance( + ref, capability=GRASP_AFFORDANCE_CAPABILITY + ) + semantics.append(scene_registry.object_semantics(ref, affordance=grasp)) + return self.lowerer_type( + self.routes, + tuple(semantics), + engine.grasp_pose_generators, + ) + + +def make_pick_factory( + routes: tuple[_PickRoute, ...], call_id: str +) -> _PickLowererFactory: + """Bind a task policy alias to the baseline's class-level factory identity. + + Only an inert identifier is specialized; the lowerer implementation and + target Atomic Skill remain unchanged. No executable code comes from data. + """ + if call_id == _PICK_CALL_ID: + return _PickLowererFactory(routes) + if not call_id.startswith("gen_sim.pick."): + raise ValueError("Task Pick aliases must use the gen_sim.pick namespace.") + suffix = hashlib.sha256(call_id.encode("utf-8")).hexdigest()[:16] + lowerer_type = type( + f"_TaskPickLowerer_{suffix}", + (_PickLowerer,), + {"call_id": call_id, "__module__": __name__, "__slots__": ()}, + ) + factory_type = make_dataclass( + f"_TaskPickFactory_{suffix}", + [], + bases=(_PickLowererFactory,), + namespace={ + "call_id": call_id, + "lowerer_type": lowerer_type, + "__module__": __name__, + }, + frozen=True, + slots=True, + ) + return factory_type(routes) + + +@dataclass(frozen=True, slots=True) +class _MoveHeldObjectRoute: + """One exact declared object target.""" + + object_id: str + target_id: str + pose: _AbsolutePoseTarget | _SceneEntityTarget + + def __post_init__(self) -> None: + _identifier(self.object_id, field_name="object_id") + _identifier(self.target_id, field_name="target_id") + _configured_goal_pose(self.pose) + object.__setattr__(self, "pose", self.pose.snapshot()) + + +class _MoveHeldObjectLowerer(RegisteredSemanticLowerer): + """Bind absolute or scene-relative targets to the shared transport goal.""" + + call_id: ClassVar[str] = _MOVE_HELD_OBJECT_CALL_ID + target_descriptor: ClassVar[SkillDescriptor] = MoveHeldObject.descriptor() + preserves_symbolic_state: ClassVar[bool] = True + + def __init__(self, routes: tuple[_MoveHeldObjectRoute, ...]) -> None: + self._routes = {route.target_id: route for route in routes} + if not routes or len(self._routes) != len(routes): + raise ValueError("MoveHeldObject targets must be non-empty and unique.") + + def _route(self, call: RegisteredSemanticCall) -> _MoveHeldObjectRoute: + arguments = dict(call.arguments) + if "target" not in arguments or set(arguments) - { + "target", + "object", + "reference", + }: + raise ValueError("MoveHeldObject arguments must select a declared target.") + route = self._routes.get(arguments["target"]) + if route is None or arguments.get("object", route.object_id) != route.object_id: + raise ValueError( + "MoveHeldObject call does not match a configured object-target route." + ) + if "reference" in arguments and ( + type(route.pose) is not _SceneEntityTarget + or arguments["reference"] != route.pose.entity_id + ): + raise ValueError( + "MoveHeldObject reference does not match the configured target." + ) + return route + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: ActionOptions, + ) -> SemanticLowering: + """Bind an exact target without observing the robot or solving IK.""" + del context, bound + if type(option_template) is not MoveHeldObjectOptions: + raise TypeError( + "Configured transport requires exact MoveHeldObjectOptions." + ) + route = self._route(call) + return SemanticLowering( + goal=HeldObjectPoseGoal(_configured_goal_pose(route.pose)) + ) + + def pick_lookahead_targets( + self, + call: RegisteredSemanticCall, + *, + picked_object: SceneObjectRef, + bound: BoundSemanticCall, + previous_target: SemanticObjectTarget | None, + ) -> tuple[SemanticObjectTarget, ...] | None: + """Conservatively screen the nominal target, which is always permitted.""" + del bound, previous_target + route = self._route(call) + if picked_object.entity_id != route.object_id: + return None + return ( + SemanticObjectTarget( + pose=( + SemanticPose(route.pose.position, route.pose.quaternion_wxyz) + if type(route.pose) is _AbsolutePoseTarget + else _configured_goal_pose(route.pose) + ) + ), + ) + + +@dataclass(frozen=True, slots=True) +class _MoveHeldObjectLowererFactory(RegisteredSemanticLowererFactory): + """Validate canonical references for configured transport goals.""" + + call_id: ClassVar[str] = _MOVE_HELD_OBJECT_CALL_ID + revision: ClassVar[str] = "2" + target_descriptor: ClassVar[SkillDescriptor] = MoveHeldObject.descriptor() + routes: tuple[_MoveHeldObjectRoute, ...] + + def __post_init__(self) -> None: + if ( + type(self.routes) is not tuple + or not self.routes + or not all(type(route) is _MoveHeldObjectRoute for route in self.routes) + ): + raise TypeError( + "routes must be a non-empty tuple of _MoveHeldObjectRoute values." + ) + if len({route.target_id for route in self.routes}) != len(self.routes): + raise ValueError("MoveHeldObject target IDs must be unique.") + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> RegisteredSemanticLowerer: + """Retain only inert target declarations, never a live robot.""" + del simulation + if engine.robot is not robot: + raise ValueError("MoveHeldObject requires the engine's exact robot.") + for route in self.routes: + scene_registry.resolve(route.object_id, expected_type=SceneObjectRef) + if type(route.pose) is _SceneEntityTarget: + scene_registry.lookup(route.pose.entity_id) + return _MoveHeldObjectLowerer(self.routes) + + +@dataclass(frozen=True, slots=True) +class _RelativePlaceRoute: + """One configured object relation expressed in the world frame.""" + + object_id: str + reference_entity_id: str + relation: str + world_displacement: tuple[float, float, float] + + def __post_init__(self) -> None: + for field_name in ("object_id", "reference_entity_id", "relation"): + object.__setattr__( + self, + field_name, + _identifier(getattr(self, field_name), field_name=field_name), + ) + if self.relation not in { + "front_left_of", + "front_right_of", + "back_left_of", + "back_right_of", + "above", + "behind", + "front_of", + "left_of", + "on", + "right_of", + }: + raise ValueError( + "Relative Place relation must be one of above, behind, front_of, " + "left_of, on, right_of, or a front/back left/right diagonal." + ) + object.__setattr__( + self, + "world_displacement", + _world_displacement(self.world_displacement), + ) + + @property + def selector(self) -> tuple[str, str, str]: + """Return the semantic arguments selecting this immutable route.""" + return self.object_id, self.reference_entity_id, self.relation + + +@dataclass(frozen=True, slots=True) +class _RelativePlaceLowererFactory(RegisteredSemanticLowererFactory): + """Create fresh relative-placement lowerers from canonical scene refs.""" + + call_id: ClassVar[str] = _PLACE_RELATIVE_CALL_ID + revision: ClassVar[str] = "1" + target_descriptor: ClassVar[SkillDescriptor] = Place.descriptor() + + routes: tuple[_RelativePlaceRoute, ...] + + def __post_init__(self) -> None: + if type(self.routes) is not tuple or not self.routes: + raise ValueError("Relative Place routes must be a non-empty exact tuple.") + if not all(type(route) is _RelativePlaceRoute for route in self.routes): + raise TypeError("Relative Place routes must be _RelativePlaceRoute values.") + if len({route.selector for route in self.routes}) != len(self.routes): + raise ValueError("Relative Place routes must be unique.") + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> RegisteredSemanticLowerer: + """Canonicalize all object/reference IDs before constructing a lowerer.""" + del simulation + if engine.robot is not robot: + raise ValueError( + "Relative Place lowerer requires the engine's exact robot." + ) + routes: list[_RelativePlaceRoute] = [] + for route in self.routes: + object_ref = scene_registry.resolve( + route.object_id, + expected_type=SceneObjectRef, + ) + reference_ref = scene_registry.resolve( + route.reference_entity_id, + expected_type=SceneObjectRef, + ) + routes.append( + _RelativePlaceRoute( + object_id=object_ref.entity_id, + reference_entity_id=reference_ref.entity_id, + relation=route.relation, + world_displacement=route.world_displacement, + ) + ) + return _RelativePlaceLowerer(tuple(routes)) + + +class _CoordinatedTransportLowerer(RegisteredSemanticLowerer): + """Lower one configured dual-arm object transport and release route.""" + + call_id: ClassVar[str] = _COORDINATED_TRANSPORT_CALL_ID + target_descriptor: ClassVar[SkillDescriptor] = CoordinatedPickment.descriptor() + effect_contract_kind: ClassVar[SemanticEffectKind] = SemanticEffectKind.RELEASE + release: ClassVar[bool] = True + + def __init__( + self, + routes: tuple[ + _CoordinatedTransportRoute | tuple[str, str, str, tuple[float, ...]], + ..., + ], + semantics: tuple[ObjectSemantics, ...], + ) -> None: + if len(routes) != len(semantics): + raise ValueError( + "Coordinated transport routes and semantics must have equal length." + ) + normalized = tuple( + _coordinated_transport_route(route, index=index) + for index, route in enumerate(routes) + ) + self._routes = { + (route.object_id, route.target_id): (route, object_semantics) + for route, object_semantics in zip(normalized, semantics, strict=True) + } + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: ActionOptions, + ) -> SemanticLowering: + """Construct one late-bound coordinated object target.""" + del bound + if type(option_template) is not CoordinatedPickmentOptions: + raise TypeError( + "Configured coordinated transport requires an exact " + "CoordinatedPickmentOptions template." + ) + if option_template.release is not self.release: + raise ValueError( + f"Configured {self.call_id} requires release={self.release}." + ) + arguments = dict(call.arguments) + if set(arguments) not in ( + {"object", "target"}, + {"object", "target", "world_displacement"}, + ): + raise ValueError( + f"{self.call_id} arguments must contain object, target, and an " + "optional world_displacement." + ) + route = (arguments["object"], arguments["target"]) + resolved = self._routes.get(route) + if resolved is None: + raise ValueError( + f"{self.call_id} does not declare object-target route {route!r}." + ) + route_cfg, semantics = resolved + if "world_displacement" in arguments: + declared = tuple(float(value) for value in arguments["world_displacement"]) + if declared != route_cfg.world_displacement: + raise ValueError( + f"{self.call_id} world_displacement does not match its " + "configured route." + ) + if route_cfg.world_displacement is not None: + try: + observed = context.scene.entities[route_cfg.object_id] + except KeyError as exc: + raise KeyError( + "Coordinated transport world displacement references " + f"unobserved object {route_cfg.object_id!r}." + ) from exc + if observed.confidence <= 0.0: + raise ValueError( + "Coordinated transport requires positive observation " + f"confidence for {route_cfg.object_id!r}." + ) + object_pose = observed.pose.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if object_pose.dim() == 2: + object_pose = object_pose.unsqueeze(0).expand( + context.batch_size, + -1, + -1, + ) + object_target_pose: torch.Tensor | SceneEntityPose = object_pose.clone() + displacement = torch.tensor( + route_cfg.world_displacement, + dtype=object_target_pose.dtype, + device=object_target_pose.device, + ) + object_target_pose[:, :3, 3] += displacement + else: + assert route_cfg.reference_entity_id is not None + assert route_cfg.relative_pose is not None + object_target_pose = SceneEntityPose( + route_cfg.reference_entity_id, + relative_pose=torch.tensor( + route_cfg.relative_pose, + dtype=torch.float32, + ).reshape(4, 4), + ) + return SemanticLowering( + goal=CoordinatedPickGoal( + semantics=semantics, + object_target_pose=object_target_pose, + ), + registered_effect=RegisteredSemanticEffect( + effect_kind=( + SemanticEffectKind.RELEASE + if self.release + else SemanticEffectKind.ATTACH + ), + held_objects=tuple( + RegisteredHeldObjectEffect( + expectation_id=slot_id, + relation=( + HeldObjectRelation.DETACHED + if self.release + else HeldObjectRelation.ATTACHED + ), + object_id=route_cfg.object_id, + slot_id=slot_id, + allow_missing_detached_baseline=self.release, + ) + for slot_id in ("left", "right") + ), + ), + ) + + +class _CoordinatedHoldLowerer(_CoordinatedTransportLowerer): + """Lower one coordinated transport that retains both verified grasps.""" + + call_id: ClassVar[str] = _COORDINATED_HOLD_CALL_ID + effect_contract_kind: ClassVar[SemanticEffectKind] = SemanticEffectKind.ATTACH + release: ClassVar[bool] = False + + +@dataclass(frozen=True, slots=True) +class _CoordinatedTransportLowererFactory(RegisteredSemanticLowererFactory): + """Create configured dual-arm transport routes from canonical scene refs.""" + + call_id: ClassVar[str] = _COORDINATED_TRANSPORT_CALL_ID + revision: ClassVar[str] = "1" + target_descriptor: ClassVar[SkillDescriptor] = CoordinatedPickment.descriptor() + lowerer_type: ClassVar[type[_CoordinatedTransportLowerer]] = ( + _CoordinatedTransportLowerer + ) + + routes: tuple[ + _CoordinatedTransportRoute | tuple[str, str, str, tuple[float, ...]], + ..., + ] + + def __post_init__(self) -> None: + if type(self.routes) is not tuple or not self.routes: + raise ValueError( + "Coordinated transport routes must be a non-empty exact tuple." + ) + normalized = [ + _coordinated_transport_route(route, index=index) + for index, route in enumerate(self.routes) + ] + selectors = [(route.object_id, route.target_id) for route in normalized] + if len(set(selectors)) != len(selectors): + raise ValueError("Coordinated transport routes must be unique.") + object.__setattr__(self, "routes", tuple(normalized)) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> RegisteredSemanticLowerer: + """Resolve grasp semantics and validate all live target references.""" + del simulation + if engine.robot is not robot: + raise ValueError( + "Coordinated transport lowerer requires the engine's exact robot." + ) + canonical_routes: list[_CoordinatedTransportRoute] = [] + semantics: list[ObjectSemantics] = [] + for route in self.routes: + object_ref = scene_registry.resolve( + route.object_id, + expected_type=SceneObjectRef, + ) + grasp_ref = scene_registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + if route.world_displacement is not None: + canonical_routes.append( + _CoordinatedTransportRoute( + object_id=object_ref.entity_id, + target_id=route.target_id, + world_displacement=route.world_displacement, + ) + ) + else: + assert route.reference_entity_id is not None + assert route.relative_pose is not None + reference = scene_registry.lookup(route.reference_entity_id) + canonical_routes.append( + _CoordinatedTransportRoute( + object_id=object_ref.entity_id, + target_id=route.target_id, + reference_entity_id=reference.ref.entity_id, + relative_pose=route.relative_pose, + ) + ) + semantics.append( + scene_registry.object_semantics( + object_ref, + affordance=grasp_ref, + ) + ) + return self.lowerer_type( + tuple(canonical_routes), + tuple(semantics), + ) + + +@dataclass(frozen=True, slots=True) +class _CoordinatedHoldLowererFactory(_CoordinatedTransportLowererFactory): + """Create configured coordinated hold routes.""" + + call_id: ClassVar[str] = _COORDINATED_HOLD_CALL_ID + lowerer_type: ClassVar[type[_CoordinatedTransportLowerer]] = _CoordinatedHoldLowerer diff --git a/embodichain/gen_sim/task_engine/_task_program/stability.py b/embodichain/gen_sim/task_engine/_task_program/stability.py new file mode 100644 index 000000000..f35c37ac8 --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/stability.py @@ -0,0 +1,394 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Named stability services; the shared bridge owns all execution decisions.""" + +from __future__ import annotations + +from collections.abc import Iterator, Mapping +from copy import deepcopy +from dataclasses import dataclass, fields +import math +from typing import Any + +import torch + +from embodichain.lab.task_program.compiler.program import CompiledPostPolicy +from embodichain.utils.math import pose_inv + +__all__: list[str] = [] + + +@dataclass(frozen=True, slots=True) +class StabilityConstraint: + """Immutable, fingerprinted definition of one task-owned stability preset.""" + + entity: str + kind: str + duration: float = 3.0 + timeout: float = 12.0 + reference: str | None = None + local_axis: tuple[float, float, float] | None = None + reference_axis: tuple[float, float, float] | None = None + displacement: tuple[float, float, float] | None = None + target_position: tuple[float, float, float] | None = None + reference_half_extents: tuple[float, float] | None = None + object_bottom: float = 0.0 + reference_top: float = 0.0 + position_tolerance: float = 0.04 + minimum_alignment: float = 0.8660254037844386 + support_tolerance: float = 0.01 + translation_drift: float = 0.02 + rotation_drift: float = math.pi / 18.0 + motion_parts: tuple[str, ...] = () + + def __post_init__(self) -> None: + if self.kind not in {"upright", "stack", "hold", "placement"}: + raise ValueError(f"Unsupported task stability kind {self.kind!r}.") + for name in ("entity", "reference"): + value = getattr(self, name) + if (name == "entity" or value is not None) and ( + type(value) is not str or not value or value != value.strip() + ): + raise ValueError(f"{name} must be an exact non-empty identifier.") + for name in ( + "duration", + "timeout", + "position_tolerance", + "support_tolerance", + "translation_drift", + "rotation_drift", + ): + value = getattr(self, name) + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(value) + or value <= 0 + ): + raise ValueError(f"{name} must be finite and positive.") + if self.timeout < self.duration: + raise ValueError("Stability timeout cannot precede its duration.") + for name in ("object_bottom", "reference_top", "minimum_alignment"): + value = getattr(self, name) + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(value) + ): + raise ValueError(f"{name} must be finite.") + if not 0 <= self.minimum_alignment <= 1: + raise ValueError("minimum_alignment must be in [0, 1].") + for name in ( + "local_axis", + "reference_axis", + "displacement", + "target_position", + "reference_half_extents", + ): + value = getattr(self, name) + if value is None: + continue + size = 2 if name == "reference_half_extents" else 3 + if ( + not isinstance(value, (tuple, list)) + or len(value) != size + or any( + isinstance(x, bool) + or not isinstance(x, (int, float)) + or not math.isfinite(x) + for x in value + ) + ): + raise ValueError(f"{name} must contain {size} finite numbers.") + vector = tuple(float(x) for x in value) + if name.endswith("axis"): + norm = math.sqrt(sum(x * x for x in vector)) + if norm <= 1e-8: + raise ValueError(f"{name} cannot be zero.") + vector = tuple(x / norm for x in vector) + if name == "reference_half_extents" and min(vector) <= 0: + raise ValueError("Support half extents must be positive.") + object.__setattr__(self, name, vector) + if not isinstance(self.motion_parts, (tuple, list)) or any( + type(x) is not str or not x or x != x.strip() for x in self.motion_parts + ): + raise ValueError("motion_parts must contain control-part identifiers.") + object.__setattr__(self, "motion_parts", tuple(self.motion_parts)) + if self.kind == "hold" and ( + not self.motion_parts + or (self.target_position is None and self.local_axis is None) + ): + raise ValueError( + "Held stability requires motion parts and a target position or axis." + ) + if self.kind == "placement" and ( + self.target_position is None + and (self.reference is None or self.displacement is None) + ): + raise ValueError("Placement stability requires an explicit target.") + if self.target_position is not None and self.displacement is not None: + raise ValueError( + "Task stability cannot combine absolute and relative targets." + ) + if self.kind == "stack" and ( + self.reference is None + or self.reference_half_extents is None + or self.local_axis is None + or self.reference_axis is None + ): + raise ValueError("Stack stability requires support geometry and both axes.") + if self.kind == "upright" and self.local_axis is None: + raise ValueError("Upright stability requires its object-local axis.") + if (self.reference is None) != ( + self.displacement is None + ) and self.kind != "stack": + raise ValueError("Relative targets require a reference and displacement.") + + @classmethod + def decode(cls, value: object) -> StabilityConstraint: + """Reject undeclared payload fields before constructing any live service.""" + if type(value) is not dict or set(value) - {item.name for item in fields(cls)}: + raise ValueError("Task stability constraint has unknown fields.") + return cls(**value) + + +class TaskStabilityPort: + """Observe named stable conditions and return masks to the unmodified bridge. + + Only measurements and per-policy results are stored here. Task state, + row eligibility, recovery, cancellation and completion belong to the core. + """ + + def __init__( + self, + delegate: Any, + simulation: Any, + robot: Any, + scene_binding: Any, + constraints: Mapping[str, StabilityConstraint], + *, + step_dt: float, + ) -> None: + if not math.isfinite(step_dt) or step_dt <= 0: + raise ValueError("step_dt must be finite and positive.") + self._delegate = delegate + self._robot = robot + self._dt = float(step_dt) + self._constraints = dict(constraints) + self._objects = { + binding.entity_id: simulation.get_rigid_object(binding.simulation_uid) + for binding in scene_binding.rigid_objects + } + for cfg in self._constraints.values(): + for entity in (cfg.entity, cfg.reference): + if entity is not None and ( + entity not in self._objects or self._objects[entity] is None + ): + raise ValueError(f"Task stability entity {entity!r} is not bound.") + self._results: dict[int, torch.Tensor] = {} + self._metadata: dict[int, dict[str, Any]] = {} + + def validate_policy(self, policy: Any, *, segment: Any) -> None: + """Validate declared identity and membership without reading live state.""" + if type(policy) is not CompiledPostPolicy: + raise TypeError("Expected an exact CompiledPostPolicy.") + if policy.cfg.preset not in self._constraints: + self._delegate.validate_policy(policy, segment=segment) + return + if not any(item is policy for item in segment.post_policies): + raise ValueError("Task stability policy does not belong to this segment.") + cfg = self._constraints[policy.cfg.preset] + if policy.cfg.kind != "wait_stable" or policy.entity.entity_id != cfg.entity: + raise ValueError("Task stability declaration and compiled policy disagree.") + + def _pose(self, entity: str) -> torch.Tensor: + pose = self._objects[entity].get_local_pose(to_matrix=True) + if ( + not isinstance(pose, torch.Tensor) + or pose.ndim != 3 + or pose.shape[-2:] != (4, 4) + ): + raise ValueError("Object poses must have shape (num_envs, 4, 4).") + return pose.clone() + + @staticmethod + def _drift( + pose: torch.Tensor, reference: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor]: + translation = torch.linalg.vector_norm( + pose[:, :3, 3] - reference[:, :3, 3], dim=-1 + ) + rotation = reference[:, :3, :3].transpose(-1, -2) @ pose[:, :3, :3] + cosine = ((rotation.diagonal(dim1=-2, dim2=-1).sum(-1) - 1) / 2).clamp(-1, 1) + return translation, torch.acos(cosine) + + def _attachments( + self, pose: torch.Tensor, parts: tuple[str, ...] + ) -> tuple[torch.Tensor, ...]: + qpos = self._robot.get_qpos() + return tuple( + pose_inv(pose) + @ self._robot.compute_fk( + qpos=qpos[:, self._robot.get_joint_ids(name=part)], + name=part, + to_matrix=True, + ) + for part in parts + ) + + def actions( + self, + policy: Any, + *, + segment: Any, + active_mask: torch.Tensor, + ) -> Iterator[torch.Tensor]: + """Yield target-qpos holds; only Gym may consume and apply them.""" + self.validate_policy(policy, segment=segment) + if policy.cfg.preset not in self._constraints: + yield from self._delegate.actions( + policy, segment=segment, active_mask=active_mask + ) + return + cfg = self._constraints[policy.cfg.preset] + initial = self._pose(cfg.entity) + if active_mask.dtype != torch.bool or active_mask.shape != initial.shape[:1]: + raise ValueError( + "Task stability active rows must match the observed batch." + ) + active = active_mask.clone() + needed = math.ceil(cfg.duration / self._dt) + maximum = math.ceil(cfg.timeout / self._dt) + consecutive = torch.zeros_like(active, dtype=torch.long) + anchor = initial.clone() + reference_anchor = self._pose(cfg.reference) if cfg.kind == "stack" else None + initial_attachments = self._attachments(initial, cfg.motion_parts) + failed = torch.zeros_like(active) + for elapsed in range(maximum + 1): + pose = self._pose(cfg.entity) + valid = torch.isfinite(pose).all(dim=-1).all(dim=-1) + measurements: dict[str, Any] = {} + if cfg.local_axis is not None: + alignment = (pose[:, :3, :3] @ pose.new_tensor(cfg.local_axis))[:, 2] + valid &= alignment >= cfg.minimum_alignment + measurements["alignment"] = alignment.tolist() + reference = self._pose(cfg.reference) if cfg.reference is not None else None + target = None + if cfg.target_position is not None: + target = pose.new_tensor(cfg.target_position) + elif reference is not None and cfg.displacement is not None: + target = reference[:, :3, 3] + pose.new_tensor(cfg.displacement) + if target is not None: + error = torch.linalg.vector_norm(pose[:, :3, 3] - target, dim=-1) + valid &= error <= cfg.position_tolerance + measurements["position_error"] = error.tolist() + if cfg.kind == "stack": + assert ( + reference is not None + and reference_anchor is not None + and cfg.reference_axis is not None + ) + alignment = ( + reference[:, :3, :3] @ pose.new_tensor(cfg.reference_axis) + )[:, 2] + gap = ( + pose[:, 2, 3] + + cfg.object_bottom + - reference[:, 2, 3] + - cfg.reference_top + ) + delta = pose[:, :2, 3] - reference[:, :2, 3] + valid &= (alignment >= cfg.minimum_alignment) & ( + gap.abs() <= cfg.support_tolerance + ) + valid &= ( + delta.abs() <= pose.new_tensor(cfg.reference_half_extents) + ).all(-1) + reference_translation, reference_rotation = self._drift( + reference, reference_anchor + ) + valid &= (reference_translation <= cfg.translation_drift) & ( + reference_rotation <= cfg.rotation_drift + ) + measurements.update( + support_gap=gap.tolist(), + reference_alignment=alignment.tolist(), + reference_translation_drift=reference_translation.tolist(), + reference_rotation_drift=reference_rotation.tolist(), + ) + translation, rotation = self._drift(pose, anchor) + valid &= (translation <= cfg.translation_drift) & ( + rotation <= cfg.rotation_drift + ) + for current, original in zip( + self._attachments(pose, cfg.motion_parts), + initial_attachments, + strict=True, + ): + slip, turn = self._drift(current, original) + valid &= (slip <= cfg.translation_drift) & (turn <= cfg.rotation_drift) + measurements.setdefault("attachment_translation", []).append( + slip.tolist() + ) + if cfg.kind == "hold": + failed |= active & ~valid + else: + anchor = torch.where(valid[:, None, None], anchor, pose) + if reference_anchor is not None: + # Both objects must share the same uninterrupted stable window. + reference_anchor = torch.where( + valid[:, None, None], reference_anchor, reference + ) + if elapsed: + consecutive = torch.where(valid & active & ~failed, consecutive + 1, 0) + accepted = active & (consecutive >= needed) & ~failed + self._results[id(policy)] = accepted.clone() + self._metadata[id(policy)] = { + "kind": cfg.kind, + "preset": policy.cfg.preset, + "elapsed_seconds": elapsed * self._dt, + "required_seconds": cfg.duration, + "stable_seconds": (consecutive * self._dt).tolist(), + "accepted_mask": accepted.tolist(), + "failed_mask": failed.tolist(), + "measurements": measurements, + } + if not (active & ~accepted & ~failed).any() or elapsed == maximum: + return + targets = self._robot.get_qpos(target=True) + observed = self._robot.get_qpos() + if targets.shape != observed.shape or not torch.isfinite(targets).all(): + raise ValueError( + "Target-qpos holds require finite full robot commands." + ) + yield torch.where( + active[:, None] & ~failed[:, None], targets, observed + ).clone() + + def post_policy_result(self, policy: Any, *, segment: Any) -> torch.Tensor: + """Publish measurements; the core decides execution eligibility.""" + if policy.cfg.preset not in self._constraints: + return self._delegate.post_policy_result(policy, segment=segment) + self.validate_policy(policy, segment=segment) + return self._results[id(policy)].clone() + + def post_policy_metadata(self, policy: Any, *, segment: Any) -> Mapping[str, Any]: + """Expose independently owned physical measurements.""" + if policy.cfg.preset not in self._constraints: + return self._delegate.post_policy_metadata(policy, segment=segment) + self.validate_policy(policy, segment=segment) + return deepcopy(self._metadata[id(policy)]) diff --git a/embodichain/gen_sim/task_engine/_task_program/stack_place.py b/embodichain/gen_sim/task_engine/_task_program/stack_place.py new file mode 100644 index 000000000..1dfc96093 --- /dev/null +++ b/embodichain/gen_sim/task_engine/_task_program/stack_place.py @@ -0,0 +1,246 @@ +# ---------------------------------------------------------------------------- +# 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-owned stack placement identity with a distinct, declared option preset.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import math +from typing import Any, ClassVar + +import torch + +from embodichain.lab.sim.atomic_actions.goals import resolve_pose_goal +from embodichain.lab.sim.atomic_actions.primitives.place import PlaceGoal +from embodichain.lab.task_program.compiler.lowering import SemanticLowering + +from .services import ( + _PickLowerer, + _PickLowererFactory, + _PickRoute, + _RelativePlaceLowerer, + _RelativePlaceLowererFactory, +) +from embodichain.lab.task_program.semantics import ( + RegisteredSemanticCall, + SemanticCallDescriptor, + SkillPolicyPreset, +) + +__all__: list[str] = [] + +STACK_PLACE_CALL = "gen_sim.stack_place" +STACK_PICK_CALL = "gen_sim.stack_pick" +_RELATIVE_PLACE_CALL = "simulation.place_relative" + + +class _StackPickLowerer(_PickLowerer): + call_id: ClassVar[str] = STACK_PICK_CALL + + +@dataclass(frozen=True, slots=True) +class _StackPickFactory(_PickLowererFactory): + call_id: ClassVar[str] = STACK_PICK_CALL + + def create(self, **kwargs: Any) -> _StackPickLowerer: + canonical = _PickLowererFactory.create(self, **kwargs) + return _StackPickLowerer( + tuple(canonical._routes.values()), tuple(canonical._semantics.values()) + ) + + +class _StackPlaceLowerer(_RelativePlaceLowerer): + """Reuse the original goal/effect binding without overriding action options.""" + + call_id: ClassVar[str] = STACK_PLACE_CALL + + def __init__( + self, + routes: tuple[Any, ...], + *, + approach: float, + release: float, + nominal: float, + ) -> None: + super().__init__(routes) + self._approach = approach + self._release = release + self._nominal = nominal + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: Any, + bound: Any, + option_template: Any, + ) -> SemanticLowering: + result = super().lower( + call, context=context, bound=bound, option_template=option_template + ) + # Freeze this observed, stable support for a precision descent. The + # final task policy still checks the actually observed support relation. + release = resolve_pose_goal( + result.goal.xpos, context, name="stack_release" + ).clone() + if release.ndim == 2: + release = release.unsqueeze(0).expand(context.batch_size, -1, -1).clone() + approach = release.clone() + approach[:, 2, 3] += self._approach - self._nominal + release[:, 2, 3] += self._release - self._nominal + return replace( + result, goal=PlaceGoal(xpos=torch.stack((approach, release), dim=1)) + ) + + def pick_lookahead_targets( + self, call: RegisteredSemanticCall, **kwargs: Any + ) -> Any: + targets = super().pick_lookahead_targets(call, **kwargs) + if targets is None: + return None + return tuple( + replace( + target, + pose=replace( + target.pose, + world_displacement=( + target.pose.world_displacement + + target.pose.world_displacement.new_tensor( + [0.0, 0.0, self._release - self._nominal] + ) + ), + ), + ) + for target in targets + ) + + +@dataclass(frozen=True, slots=True) +class _StackPlaceFactory(_RelativePlaceLowererFactory): + """Register a separate task preset while retaining canonical route validation.""" + + call_id: ClassVar[str] = STACK_PLACE_CALL + revision: ClassVar[str] = "2" + approach_clearance: float = 0.02 + release_clearance: float = 0.003 + nominal_clearance: float = 0.01 + + def __post_init__(self) -> None: + _RelativePlaceLowererFactory.__post_init__(self) + values = ( + self.approach_clearance, + self.release_clearance, + self.nominal_clearance, + ) + if any( + isinstance(v, bool) + or not isinstance(v, (int, float)) + or not math.isfinite(v) + or v <= 0 + for v in values + ): + raise ValueError("Stack clearances must be finite positive lengths.") + if self.release_clearance >= self.approach_clearance: + raise ValueError("Stack approach must be above its release target.") + + def create(self, **kwargs: Any) -> _StackPlaceLowerer: + canonical = _RelativePlaceLowererFactory.create(self, **kwargs) + return _StackPlaceLowerer( + tuple(canonical._routes.values()), + approach=self.approach_clearance, + release=self.release_clearance, + nominal=self.nominal_clearance, + ) + + +def with_stack_placement( + registration: Any, + *, + targets: frozenset[tuple[str, str]], +) -> Any: + """Give stacking a full retract, independently of low table-placement caps.""" + relative_factory = next( + factory + for factory in registration.registered_semantic_lowerer_factories + if factory.call_id == _RELATIVE_PLACE_CALL + ) + factory = _StackPlaceFactory( + tuple( + route + for route in relative_factory.routes + if (route.object_id, route.reference_entity_id) in targets + and route.relation in {"on", "above"} + ) + ) + catalog = registration.call_catalog.with_descriptor( + SemanticCallDescriptor( + call_id=STACK_PLACE_CALL, + spec_type=RegisteredSemanticCall, + target_descriptor=factory.target_descriptor, + ) + ) + pick_factory = _StackPickFactory( + tuple(_PickRoute(obj, target) for obj, target in sorted(targets)) + ) + catalog = catalog.with_descriptor( + SemanticCallDescriptor( + call_id=STACK_PICK_CALL, + spec_type=RegisteredSemanticCall, + target_descriptor=pick_factory.target_descriptor, + ) + ) + presets = [] + for preset in registration.robot_profile_binding.presets: + options = dict(preset.action_option_templates) + monitors = dict(preset.effect_monitors) + if "pick" in options: + options[STACK_PICK_CALL] = replace(options["pick"], pick_object_part="top") + if "pick" in monitors: + monitors[STACK_PICK_CALL] = monitors["pick"] + if _RELATIVE_PLACE_CALL in options: + options[STACK_PLACE_CALL] = replace( + options[_RELATIVE_PLACE_CALL], + max_approach_retract_z=None, + hand_interp_steps=40, + ) + if _RELATIVE_PLACE_CALL in monitors: + monitors[STACK_PLACE_CALL] = monitors[_RELATIVE_PLACE_CALL] + presets.append( + SkillPolicyPreset( + preset_id=preset.preset_id, + required_planner=preset.required_planner, + action_option_templates=options, + effect_monitors=monitors, + effect_assurance=preset.effect_assurance, + motion_policy=preset.motion_policy, + tracking_policy=preset.tracking_policy, + recovery_policy=preset.recovery_policy, + workflow_recovery_policy=preset.workflow_recovery_policy, + runner_cfg=preset.runner_cfg, + ) + ) + profile = replace(registration.robot_profile_binding, presets=tuple(presets)) + return replace( + registration, + robot_profile_binding=profile, + call_catalog=catalog, + registered_semantic_lowerer_factories=( + *registration.registered_semantic_lowerer_factories, + factory, + pick_factory, + ), + ) diff --git a/embodichain/gen_sim/task_engine/agent.py b/embodichain/gen_sim/task_engine/agent.py new file mode 100644 index 000000000..1d183e9d2 --- /dev/null +++ b/embodichain/gen_sim/task_engine/agent.py @@ -0,0 +1,289 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Scene-independent semantic candidate generation for Task Engine.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping +from concurrent.futures import ThreadPoolExecutor, as_completed +from copy import deepcopy +from dataclasses import dataclass +from typing import Any + +from .contracts import ( + SCENE_REQUEST_SCHEMA, + SUCCESS_SPEC_SCHEMA, + TASK_CANDIDATE_SET_SCHEMA, + TASK_DRAFT_SCHEMA, + TaskCandidate, + TaskCandidateSet, + canonical_hash, + validate_task_candidate, + validate_task_candidate_set, +) +from .interpretation import ( + InstructionCaller, + InstructionDraftResult, + interpret_instruction_draft, + validate_instruction_intent, +) +from .ontology import TASK_CONTRACTS, task_success_type + +__all__ = [ + "TaskAgent", + "TaskGenerationError", + "derive_scene_request", + "derive_success_spec", +] + +DraftInterpreter = Callable[..., InstructionDraftResult] + + +class TaskGenerationError(ValueError): + """Raised when every independently generated candidate fails validation.""" + + +@dataclass(frozen=True) +class _CandidateAttempt: + index: int + result: InstructionDraftResult | None = None + error: str = "" + + +class TaskAgent: + """Generate, validate, normalize, and vote on independent task drafts.""" + + def __init__( + self, + *, + caller: InstructionCaller | None = None, + interpreter: DraftInterpreter = interpret_instruction_draft, + ) -> None: + self._caller = caller + self._interpreter = interpreter + + def generate( + self, + task_id: str, + instruction: str, + model: str | None = None, + candidate_count: int = 3, + ) -> TaskCandidateSet: + """Generate candidates concurrently and retain votes after deduplication.""" + 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.") + if ( + isinstance(candidate_count, bool) + or not isinstance(candidate_count, int) + or candidate_count < 1 + ): + raise ValueError("candidate_count must be a positive integer.") + + attempts: list[_CandidateAttempt] = [] + with ThreadPoolExecutor( + max_workers=candidate_count, + thread_name_prefix="task-agent", + ) as executor: + futures = { + executor.submit( + self._interpreter, + normalized_instruction, + model=model, + caller=self._caller, + ): index + for index in range(candidate_count) + } + for future in as_completed(futures): + index = futures[future] + try: + attempts.append( + _CandidateAttempt(index=index, result=future.result()) + ) + except Exception as error: # Each candidate is an isolated vote. + attempts.append( + _CandidateAttempt( + index=index, + error=f"candidate_{index + 1:02d}: {type(error).__name__}: {error}", + ) + ) + attempts.sort(key=lambda item: item.index) + errors = [item.error for item in attempts if item.result is None] + unique: dict[str, TaskCandidate] = {} + valid_response_count = 0 + for attempt in attempts: + if attempt.result is None: + continue + assert attempt.result is not None + try: + canonical_intent = _canonicalize_intent(attempt.result.intent) + draft = { + "schema_version": TASK_DRAFT_SCHEMA, + "task_id": normalized_task_id, + "instruction": normalized_instruction, + "steps": canonical_intent["steps"], + } + semantic_hash = canonical_hash(draft["steps"]) + candidate_id = f"candidate_{len(unique) + 1:02d}" + candidate = validate_task_candidate( + { + "candidate_id": candidate_id, + "draft": draft, + "scene_request": derive_scene_request(draft), + "success_spec": derive_success_spec(draft), + "semantic_hash": semantic_hash, + "vote_count": 1, + "attempts": attempt.result.attempts, + "normalizations": deepcopy(list(attempt.result.normalizations)), + } + ) + existing = unique.get(semantic_hash) + if existing is not None: + existing["vote_count"] += 1 + existing["attempts"] = max( + existing["attempts"], attempt.result.attempts + ) + existing["normalizations"].extend(candidate["normalizations"]) + else: + unique[semantic_hash] = candidate + valid_response_count += 1 + except Exception as error: # Post-processing failures stay candidate-local. + errors.append( + f"candidate_{attempt.index + 1:02d}: " + f"{type(error).__name__}: {error}" + ) + + if not unique: + raise TaskGenerationError( + "All Task Agent candidates failed validation: " + "; ".join(errors) + ) + + return validate_task_candidate_set( + { + "schema_version": TASK_CANDIDATE_SET_SCHEMA, + "task_id": normalized_task_id, + "instruction": normalized_instruction, + "candidates": list(unique.values()), + "requested_candidate_count": candidate_count, + "valid_response_count": valid_response_count, + "errors": errors, + } + ) + + +def derive_scene_request(draft: Mapping[str, Any]) -> dict[str, Any]: + """Derive structural scene constraints without classifying reference text.""" + from .contracts import validate_scene_request, validate_task_draft + + normalized = validate_task_draft(draft) + references: list[dict[str, Any]] = [] + for step in normalized["steps"]: + task_type = str(step["task_type"]) + contract = TASK_CONTRACTS[task_type] + for role in ("object", "target"): + selector = step[role] + if selector["kind"] != "scene_ref": + continue + if role == "object": + structure = contract.source_structure + affordances = sorted(contract.scene_affordances) + initial_state = {"orientation": "fallen"} if task_type == "E2" else {} + attributes: dict[str, Any] = {} + else: + structure = _target_structure(task_type, str(step["relation"])) + affordances = _target_affordances(task_type, str(step["relation"])) + initial_state = {} + attributes = {} + references.append( + { + "reference_id": f"{step['id']}.{role}", + "step_id": step["id"], + "role": role, + "reference": selector["reference"], + "quantifier": selector["quantifier"], + "count": selector["count"], + "source_structure": structure, + "affordances": affordances, + "initial_state": initial_state, + "attributes": attributes, + } + ) + if not references: + raise ValueError("A TaskDraft must contain at least one scene_ref selector.") + return validate_scene_request( + { + "schema_version": SCENE_REQUEST_SCHEMA, + "task_id": normalized["task_id"], + "references": references, + } + ) + + +def derive_success_spec(draft: Mapping[str, Any]) -> dict[str, Any]: + """Derive every success term exclusively from the E-task ontology.""" + from .contracts import validate_success_spec, validate_task_draft + + normalized = validate_task_draft(draft) + return validate_success_spec( + { + "schema_version": SUCCESS_SPEC_SCHEMA, + "task_id": normalized["task_id"], + "op": "all", + "terms": [ + { + "step_id": step["id"], + "type": task_success_type(step["task_type"], step), + } + for step in normalized["steps"] + ], + }, + draft=normalized, + ) + + +def _canonicalize_intent(intent: Mapping[str, Any]) -> dict[str, Any]: + """Remove arbitrary model step IDs while preserving the explicit DAG order.""" + normalized = validate_instruction_intent(intent) + id_map = { + step["id"]: f"step_{index + 1:02d}" + for index, step in enumerate(normalized["steps"]) + } + steps = deepcopy(normalized["steps"]) + for step in steps: + old_id = step["id"] + step["id"] = id_map[old_id] + step["depends_on"] = [id_map[item] for item in step["depends_on"]] + for selector_name in ("object", "target"): + selector = step[selector_name] + if selector["kind"] == "step_result": + selector["step_id"] = id_map[selector["step_id"]] + return validate_instruction_intent({"steps": steps}) + + +def _target_affordances(task_type: str, relation: str) -> list[str]: + if task_type == "E3" or relation == "inside": + return ["container"] + return [] + + +def _target_structure(task_type: str, relation: str) -> str: + if relation == "on": + return "physical_entity" + if task_type == "E3" or relation == "inside": + return "rigid_object" + return "spatial_reference" diff --git a/embodichain/gen_sim/task_engine/cli.py b/embodichain/gen_sim/task_engine/cli.py new file mode 100644 index 000000000..b4f12f003 --- /dev/null +++ b/embodichain/gen_sim/task_engine/cli.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. +# ---------------------------------------------------------------------------- + +"""Unified CLI for complete Task Engine workflows.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path +import sys +from typing import Any, Final, Sequence + +from .config import load_task_engine_config +from .orchestration.scene_adapter import SceneAdapter +from .run_directory import reserve_run_directory +from .workflow import SubprocessActionExecutor, TaskEngineWorkflow +from .workflow_contracts import ( + TASK_RUN_REQUEST_SCHEMA, + validate_scene_history_root, + validate_scene_output_separation, +) + +__all__ = ["build_parser", "main"] + + +_ROBOT_PROFILES = ( + "ur5", + "ur10", + "dual_ur5", + "dual_ur10", + "franka", + "dual_franka", +) +_MODES: Final = ("image", "image-edit", "scene", "scene-edit") + + +def build_parser() -> argparse.ArgumentParser: + """Build the Task Engine parser.""" + parser = argparse.ArgumentParser( + prog="embodichain task-engine", + description="Prepare, run, or complete one Scene and Action workflow.", + ) + subparsers = parser.add_subparsers(dest="command", required=True) + prepare_parser = subparsers.add_parser( + "prepare", help="Prepare a bundle without simulator execution." + ) + _add_workflow_arguments(prepare_parser) + run_all_parser = subparsers.add_parser( + "run-all", help="Prepare and execute one complete workflow." + ) + _add_workflow_arguments(run_all_parser) + _add_open_window_argument(run_all_parser) + run_parser = subparsers.add_parser( + "run", help="Execute an already prepared Task Engine bundle." + ) + run_parser.add_argument("--bundle", required=True) + run_parser.add_argument("--output-root", required=True) + run_parser.add_argument("--config", default=None) + run_parser.add_argument("--seed", type=int, default=0) + run_parser.add_argument("--num-envs", type=int, default=None) + run_parser.add_argument("--dataset-saving", action="store_true") + _add_open_window_argument(run_parser) + _add_failure_policy_argument(run_parser) + return parser + + +def _add_workflow_arguments(parser: argparse.ArgumentParser) -> None: + parser.add_argument("--mode", choices=_MODES, required=True) + parser.add_argument("--task-id", "--task_id", required=True) + instruction = parser.add_mutually_exclusive_group(required=True) + instruction.add_argument("--instruction") + instruction.add_argument("--task-file", "--task_file") + parser.add_argument("--image") + parser.add_argument("--scene") + parser.add_argument("--scene-edit", "--scene_edit", default=None) + parser.add_argument("--output-root", required=True) + parser.add_argument("--config", default=None) + parser.add_argument("--model", default=None) + parser.add_argument("--base-seed", type=int, default=0) + parser.add_argument( + "--dataset_saving", + action="store_true", + help="Opt in to the Gym project's dataset recorder during execution.", + ) + parser.add_argument( + "--robot-profile", + choices=_ROBOT_PROFILES, + default="franka", + ) + _add_failure_policy_argument(parser) + + +def _add_failure_policy_argument(parser: argparse.ArgumentParser) -> None: + parser.add_argument( + "--failure-policy", + choices=("stop", "continue"), + default="stop", + help=( + "Whether dependency failures stop affected downstream execution or " + "allow diagnostic continuation." + ), + ) + + +def _add_open_window_argument(parser: argparse.ArgumentParser) -> None: + parser.add_argument( + "--open-window", + action="store_true", + help="Open the native DexSim window during simulator execution.", + ) + + +def main(argv: Sequence[str] | None = None) -> int: + """Dispatch one Task Engine workflow command.""" + parser = build_parser() + arguments = list(sys.argv[1:] if argv is None else argv) + if arguments and arguments[0] not in { + "prepare", + "run", + "run-all", + "-h", + "--help", + }: + arguments.insert(0, "run-all") + args = parser.parse_args(arguments) + if args.command == "run": + return _run_prepared_bundle(args) + return _run_workflow( + args, + execute=args.command == "run-all", + parser=parser, + ) + + +def _run_workflow( + args: argparse.Namespace, + *, + execute: bool, + parser: argparse.ArgumentParser, +) -> int: + try: + image, scene, edit = _mode_inputs(args) + except ValueError as exc: + parser.error(str(exc)) + if scene is not None: + validate_scene_history_root(scene, args.output_root) + instruction = _instruction(args) + adapter = SceneAdapter(model=args.model, robot_profile=args.robot_profile) + workflow = TaskEngineWorkflow(scene_adapter=adapter) + workflow_cfg, planning_cfg, execution_cfg = load_task_engine_config(args.config) + with reserve_run_directory(args.output_root) as allocation: + if scene is not None: + validate_scene_output_separation(scene, allocation.path) + result = workflow.run( + { + "schema_version": TASK_RUN_REQUEST_SCHEMA, + "task_id": args.task_id, + "task_instruction": instruction, + "image_path": image, + "gym_project": scene, + "scene_edit_prompt": edit, + "output_dir": allocation.path.as_posix(), + }, + config_path=args.config, + workflow_cfg=workflow_cfg, + planning_cfg=planning_cfg, + execution_cfg=execution_cfg, + model=args.model, + base_seed=args.base_seed, + dataset_saving=args.dataset_saving, + failure_policy=args.failure_policy, + open_window=bool(getattr(args, "open_window", False)), + run_id=allocation.run_id, + created_at=allocation.created_at, + execute=execute, + ) + _print_json( + { + "run_id": allocation.run_id, + "status": result.status, + "failure_class": result.failure_class, + "output_dir": result.output_dir.as_posix(), + "manifest": result.manifest_path.as_posix(), + "final_bundle": ( + None if result.final_bundle is None else result.final_bundle.as_posix() + ), + "video_paths": _report_saved_videos(result.output_dir) if execute else [], + } + ) + accepted = result.succeeded if execute else result.status == "prepared" + return 0 if accepted else 2 + + +def _run_prepared_bundle(args: argparse.Namespace) -> int: + _, _, execution_cfg = load_task_engine_config(args.config) + num_envs = execution_cfg.num_envs if args.num_envs is None else int(args.num_envs) + if num_envs < 1: + raise ValueError("num_envs must be positive.") + with reserve_run_directory(args.output_root) as allocation: + report = SubprocessActionExecutor()( + args.bundle, + allocation.path, + seed=int(args.seed), + num_envs=num_envs, + dataset_saving=bool(args.dataset_saving), + failure_policy=args.failure_policy, + open_window=bool(args.open_window), + ) + environments = report.get("environments", ()) + successes = [ + bool(item.get("success")) for item in environments if isinstance(item, dict) + ] + accepted = ( + str(report.get("status")) not in {"rejected", "aborted"} + and len(successes) == num_envs + and sum(successes) >= execution_cfg.required_successes + ) + _print_json( + { + "run_id": allocation.run_id, + "status": "succeeded" if accepted else "failed", + "output_dir": allocation.path.as_posix(), + "execution_report": report, + "video_paths": _report_saved_videos(allocation.path), + } + ) + return 0 if accepted else 2 + + +def _report_saved_videos(output_dir: Path) -> list[str]: + """List recordings only after the current run has finished publishing.""" + try: + paths = sorted( + { + path.resolve().as_posix() + for pattern in ( + "videos/*.mp4", + "attempts/scene_*/action_attempts/action_*/videos/*.mp4", + ) + for path in output_dir.glob(pattern) + if path.is_file() + } + ) + except OSError as exc: + print(f"[Task Engine] Unable to list saved videos: {exc}", file=sys.stderr) + return [] + for path in paths: + print(f"[Task Engine] Video saved: {path}", file=sys.stderr) + if not paths: + print("[Task Engine] No video files generated for this run.", file=sys.stderr) + return paths + + +def _instruction(args: argparse.Namespace) -> str: + instruction = ( + str(args.instruction).strip() + if args.instruction is not None + else Path(args.task_file).expanduser().read_text(encoding="utf-8").strip() + ) + if not instruction: + raise ValueError("Task instruction must not be empty.") + return instruction + + +def _mode_inputs(args: argparse.Namespace) -> tuple[str | None, str | None, str | None]: + image = None if args.image is None else str(args.image).strip() + scene = None if args.scene is None else str(args.scene).strip() + edit = None if args.scene_edit is None else str(args.scene_edit).strip() + expected = { + "image": (True, False, False), + "image-edit": (True, False, True), + "scene": (False, True, False), + "scene-edit": (False, True, True), + }[args.mode] + actual = (bool(image), bool(scene), bool(edit)) + if actual != expected: + raise ValueError( + f"mode={args.mode!r} requires image/scene/edit={expected}, got {actual}." + ) + return image, scene, edit + + +def _print_json(value: Any) -> None: + print(json.dumps(value, ensure_ascii=False, indent=2, allow_nan=False)) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/embodichain/gen_sim/task_engine/config.py b/embodichain/gen_sim/task_engine/config.py new file mode 100644 index 000000000..decb08c98 --- /dev/null +++ b/embodichain/gen_sim/task_engine/config.py @@ -0,0 +1,191 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Configuration owned by Task Engine orchestration.""" + +from __future__ import annotations + +from collections.abc import Mapping +from importlib.resources import files +from pathlib import Path +from typing import Any, Final + +import yaml + +from embodichain.utils import configclass + +__all__ = [ + "TASK_ENGINE_DEFAULTS_SCHEMA", + "TaskEngineExecutionCfg", + "TaskEnginePlanningCfg", + "TaskEngineWorkflowCfg", + "load_task_engine_config", +] + +TASK_ENGINE_DEFAULTS_SCHEMA: Final = "embodichain.task-engine-defaults/v1" + + +@configclass +class TaskEngineExecutionCfg: + """Success policy for vectorized simulator execution.""" + + num_envs: int = 1 + success_policy: str = "any" + min_successful_envs: int = 1 + + def __post_init__(self) -> None: + if ( + isinstance(self.num_envs, bool) + or not isinstance(self.num_envs, int) + or self.num_envs < 1 + ): + raise ValueError("num_envs must be a positive integer.") + if self.success_policy not in {"any", "all", "at_least"}: + raise ValueError("success_policy must be any, all, or at_least.") + if ( + isinstance(self.min_successful_envs, bool) + or not isinstance(self.min_successful_envs, int) + or not 1 <= self.min_successful_envs <= self.num_envs + ): + raise ValueError("min_successful_envs must be in [1, num_envs].") + if self.success_policy == "any" and self.min_successful_envs != 1: + raise ValueError("success_policy=any requires min_successful_envs=1.") + if self.success_policy == "all" and self.min_successful_envs != self.num_envs: + raise ValueError( + "success_policy=all requires min_successful_envs=num_envs." + ) + + @property + def required_successes(self) -> int: + """Return the number of successful replicas required for acceptance.""" + if self.success_policy == "all": + return self.num_envs + if self.success_policy == "any": + return 1 + return self.min_successful_envs + + +@configclass +class TaskEngineWorkflowCfg: + """Conservative first-version orchestration limits. + + The packaged YAML owns retry limits so deployment testing can tune them + without changing the orchestration implementation. + """ + + max_parallel_workers: int = 2 + max_scene_attempts: int = 2 + max_action_attempts: int = 3 + + def __post_init__(self) -> None: + for field_name in ( + "max_parallel_workers", + "max_scene_attempts", + "max_action_attempts", + ): + value = getattr(self, field_name) + if isinstance(value, bool) or not isinstance(value, int) or value < 1: + raise ValueError(f"{field_name} must be a positive integer.") + + +@configclass +class TaskEnginePlanningCfg: + """Task interpretation and semantic graph generation defaults.""" + + candidate_count: int = 3 + planning_mode: str = "offline" + max_episodes: int = 1 + max_episode_steps: int = 8000 + + def __post_init__(self) -> None: + for field_name in ( + "candidate_count", + "max_episodes", + "max_episode_steps", + ): + value = getattr(self, field_name) + if isinstance(value, bool) or not isinstance(value, int) or value < 1: + raise ValueError(f"{field_name} must be a positive integer.") + if self.planning_mode not in {"offline", "ab"}: + raise ValueError("planning_mode must be offline or ab.") + + +def load_task_engine_config( + path: str | Path | None = None, +) -> tuple[ + TaskEngineWorkflowCfg, + TaskEnginePlanningCfg, + TaskEngineExecutionCfg, +]: + """Load strict Task Engine defaults from YAML. + + Args: + path: Optional override YAML. The packaged defaults are used when omitted. + + Returns: + Validated workflow, planning, and execution configurations. + + Raises: + TypeError: If a configuration section is not a mapping. + ValueError: If the YAML schema or fields are invalid. + """ + content = ( + Path(path).expanduser().resolve().read_text(encoding="utf-8") + if path is not None + else files(__package__).joinpath("defaults.yaml").read_text(encoding="utf-8") + ) + raw = yaml.safe_load(content) + if not isinstance(raw, Mapping): + raise TypeError("Task Engine configuration must be a mapping.") + expected = {"schema_version", "workflow", "planning", "execution"} + if set(raw) != expected: + raise ValueError("Task Engine configuration fields are invalid.") + if raw.get("schema_version") != TASK_ENGINE_DEFAULTS_SCHEMA: + raise ValueError("Task Engine configuration schema_version is invalid.") + workflow = _mapping(raw.get("workflow"), "workflow") + planning = _mapping(raw.get("planning"), "planning") + execution = _mapping(raw.get("execution"), "execution") + if set(workflow) != { + "max_parallel_workers", + "max_scene_attempts", + "max_action_attempts", + }: + raise ValueError("Task Engine workflow configuration fields are invalid.") + required_planning = { + "candidate_count", + "planning_mode", + "max_episodes", + "max_episode_steps", + } + if set(planning) != required_planning: + raise ValueError("Task Engine planning configuration fields are invalid.") + if set(execution) != { + "num_envs", + "success_policy", + "min_successful_envs", + }: + raise ValueError("Task Engine execution configuration fields are invalid.") + return ( + TaskEngineWorkflowCfg(**workflow), + TaskEnginePlanningCfg(**planning), + TaskEngineExecutionCfg(**execution), + ) + + +def _mapping(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, Mapping): + raise TypeError(f"Task Engine {field_name} configuration must be a mapping.") + return dict(value) diff --git a/embodichain/gen_sim/task_engine/contracts.py b/embodichain/gen_sim/task_engine/contracts.py new file mode 100644 index 000000000..237423e55 --- /dev/null +++ b/embodichain/gen_sim/task_engine/contracts.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. +# ---------------------------------------------------------------------------- + +"""Strict, JSON-safe public contracts owned by Task Engine.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +import hashlib +import json +from typing import Any, TypeAlias + +from .interpretation import validate_instruction_intent +from .ontology import TASK_CONTRACTS, task_success_type + +__all__ = [ + "SCENE_REQUEST_SCHEMA", + "SUCCESS_SPEC_SCHEMA", + "TASK_CANDIDATE_SET_SCHEMA", + "TASK_DRAFT_SCHEMA", + "SceneRequest", + "SuccessSpec", + "TaskCandidate", + "TaskCandidateSet", + "TaskDraft", + "canonical_hash", + "validate_scene_request", + "validate_success_spec", + "validate_task_candidate", + "validate_task_candidate_set", + "validate_task_draft", +] + +TASK_DRAFT_SCHEMA = "action_engine_task_draft_v1" +SCENE_REQUEST_SCHEMA = "action_engine_scene_request_v1" +SUCCESS_SPEC_SCHEMA = "action_engine_success_spec_v1" +TASK_CANDIDATE_SET_SCHEMA = "action_engine_task_candidate_set_v1" + +TaskDraft: TypeAlias = dict[str, Any] +SceneRequest: TypeAlias = dict[str, Any] +SuccessSpec: TypeAlias = dict[str, Any] +TaskCandidate: TypeAlias = dict[str, Any] +TaskCandidateSet: TypeAlias = dict[str, Any] + +_SUCCESS_TYPES = frozenset( + {contract.success_type for contract in TASK_CONTRACTS.values()} | {"semantic_goal"} +) +_DRAFT_KEYS = frozenset({"schema_version", "task_id", "instruction", "steps"}) +_SCENE_REQUEST_KEYS = frozenset({"schema_version", "task_id", "references"}) +_REFERENCE_KEYS = frozenset( + { + "reference_id", + "step_id", + "role", + "reference", + "quantifier", + "count", + "source_structure", + "affordances", + "initial_state", + "attributes", + } +) +_SUCCESS_KEYS = frozenset({"schema_version", "task_id", "op", "terms"}) +_SUCCESS_TERM_KEYS = frozenset({"step_id", "type"}) +_CANDIDATE_KEYS = frozenset( + { + "candidate_id", + "draft", + "scene_request", + "success_spec", + "semantic_hash", + "vote_count", + "attempts", + "normalizations", + } +) +_CANDIDATE_SET_KEYS = frozenset( + { + "schema_version", + "task_id", + "instruction", + "candidates", + "requested_candidate_count", + "valid_response_count", + "errors", + } +) + + +def canonical_hash(value: Any) -> str: + """Return the stable SHA-256 of one JSON-safe protocol value.""" + payload = json.dumps( + value, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode("utf-8") + return hashlib.sha256(payload).hexdigest() + + +def validate_task_draft(value: Mapping[str, Any]) -> TaskDraft: + result = _mapping(value, "TaskDraft") + _keys(result, _DRAFT_KEYS, "TaskDraft") + _schema(result, TASK_DRAFT_SCHEMA, "TaskDraft") + result["task_id"] = _nonempty(result.get("task_id"), "TaskDraft.task_id") + result["instruction"] = _nonempty( + result.get("instruction"), "TaskDraft.instruction" + ) + intent = validate_instruction_intent({"steps": result.get("steps")}) + result["steps"] = intent["steps"] + return result + + +def validate_scene_request(value: Mapping[str, Any]) -> SceneRequest: + result = _mapping(value, "SceneRequest") + _keys(result, _SCENE_REQUEST_KEYS, "SceneRequest") + _schema(result, SCENE_REQUEST_SCHEMA, "SceneRequest") + task_id = _nonempty(result.get("task_id"), "SceneRequest.task_id") + references: list[dict[str, Any]] = [] + for index, raw in enumerate( + _sequence(result.get("references"), "SceneRequest.references") + ): + context = f"SceneRequest.references[{index}]" + reference = _mapping(raw, context) + _keys(reference, _REFERENCE_KEYS, context) + for key in ("reference_id", "step_id", "role", "reference", "source_structure"): + reference[key] = _nonempty(reference.get(key), f"{context}.{key}") + reference["role"] = _enum( + reference["role"], {"object", "target"}, f"{context}.role" + ) + reference["quantifier"] = _enum( + reference.get("quantifier"), + {"one", "all", "count"}, + f"{context}.quantifier", + ) + reference["count"] = _integer( + reference.get("count"), f"{context}.count", minimum=0 + ) + if reference["quantifier"] in {"one", "all"} and reference["count"] != 0: + raise ValueError( + f"{context} quantifier={reference['quantifier']} requires count=0." + ) + if reference["quantifier"] == "count" and reference["count"] < 1: + raise ValueError(f"{context} quantifier=count requires count>=1.") + reference["affordances"] = _strings( + reference.get("affordances"), f"{context}.affordances" + ) + reference["initial_state"] = _mapping( + reference.get("initial_state"), f"{context}.initial_state" + ) + reference["attributes"] = _mapping( + reference.get("attributes"), f"{context}.attributes" + ) + references.append(reference) + _unique([item["reference_id"] for item in references], "SceneRequest reference IDs") + result["task_id"] = task_id + result["references"] = references + _json_safe(result, "SceneRequest") + return result + + +def validate_success_spec( + value: Mapping[str, Any], + *, + draft: Mapping[str, Any] | None = None, +) -> SuccessSpec: + result = _mapping(value, "SuccessSpec") + _keys(result, _SUCCESS_KEYS, "SuccessSpec") + _schema(result, SUCCESS_SPEC_SCHEMA, "SuccessSpec") + task_id = _nonempty(result.get("task_id"), "SuccessSpec.task_id") + if result.get("op") != "all": + raise ValueError("SuccessSpec.op must be 'all'.") + terms: list[dict[str, str]] = [] + for index, raw in enumerate(_sequence(result.get("terms"), "SuccessSpec.terms")): + context = f"SuccessSpec.terms[{index}]" + term = _mapping(raw, context) + _keys(term, _SUCCESS_TERM_KEYS, context) + terms.append( + { + "step_id": _nonempty(term.get("step_id"), f"{context}.step_id"), + "type": _enum(term.get("type"), set(_SUCCESS_TYPES), f"{context}.type"), + } + ) + if not terms: + raise ValueError("SuccessSpec.terms must not be empty.") + _unique([term["step_id"] for term in terms], "SuccessSpec step IDs") + if draft is not None: + normalized_draft = validate_task_draft(draft) + if normalized_draft["task_id"] != task_id: + raise ValueError("SuccessSpec.task_id must match TaskDraft.task_id.") + expected = [ + { + "step_id": step["id"], + "type": task_success_type(step["task_type"], step), + } + for step in normalized_draft["steps"] + ] + if terms != expected: + raise ValueError( + "SuccessSpec terms must be ordered, complete, and derived from " + "task_success_type." + ) + result["task_id"] = task_id + result["terms"] = terms + return result + + +def validate_task_candidate(value: Mapping[str, Any]) -> TaskCandidate: + result = _mapping(value, "TaskCandidate") + _keys(result, _CANDIDATE_KEYS, "TaskCandidate") + result["candidate_id"] = _nonempty( + result.get("candidate_id"), "TaskCandidate.candidate_id" + ) + result["draft"] = validate_task_draft(result.get("draft")) + result["scene_request"] = validate_scene_request(result.get("scene_request")) + result["success_spec"] = validate_success_spec( + result.get("success_spec"), draft=result["draft"] + ) + for name in ("scene_request", "success_spec"): + if result[name]["task_id"] != result["draft"]["task_id"]: + raise ValueError(f"TaskCandidate {name}.task_id must match its draft.") + from .agent import derive_scene_request + + if result["scene_request"] != derive_scene_request(result["draft"]): + raise ValueError( + "TaskCandidate.scene_request must be derived exactly from its draft." + ) + result["semantic_hash"] = _digest( + result.get("semantic_hash"), "TaskCandidate.semantic_hash" + ) + if result["semantic_hash"] != canonical_hash(result["draft"]["steps"]): + raise ValueError( + "TaskCandidate.semantic_hash does not match its canonical steps." + ) + result["vote_count"] = _integer( + result.get("vote_count"), "TaskCandidate.vote_count", minimum=1 + ) + result["attempts"] = _integer( + result.get("attempts"), "TaskCandidate.attempts", minimum=1, maximum=2 + ) + result["normalizations"] = _mapping_sequence( + result.get("normalizations"), "TaskCandidate.normalizations" + ) + return result + + +def validate_task_candidate_set(value: Mapping[str, Any]) -> TaskCandidateSet: + result = _mapping(value, "TaskCandidateSet") + _keys(result, _CANDIDATE_SET_KEYS, "TaskCandidateSet") + _schema(result, TASK_CANDIDATE_SET_SCHEMA, "TaskCandidateSet") + task_id = _nonempty(result.get("task_id"), "TaskCandidateSet.task_id") + instruction = _nonempty(result.get("instruction"), "TaskCandidateSet.instruction") + requested = _integer( + result.get("requested_candidate_count"), + "TaskCandidateSet.requested_candidate_count", + minimum=1, + ) + valid = _integer( + result.get("valid_response_count"), + "TaskCandidateSet.valid_response_count", + minimum=1, + maximum=requested, + ) + candidates = [ + validate_task_candidate(item) + for item in _sequence(result.get("candidates"), "TaskCandidateSet.candidates") + ] + if not candidates: + raise ValueError("TaskCandidateSet.candidates must not be empty.") + _unique([item["candidate_id"] for item in candidates], "TaskCandidate IDs") + _unique( + [item["semantic_hash"] for item in candidates], "TaskCandidate semantic hashes" + ) + if sum(item["vote_count"] for item in candidates) != valid: + raise ValueError( + "TaskCandidate vote_count values must sum to valid_response_count." + ) + for candidate in candidates: + if ( + candidate["draft"]["task_id"] != task_id + or candidate["draft"]["instruction"] != instruction + ): + raise ValueError("Every TaskCandidate draft must match its candidate set.") + errors = _strings(result.get("errors"), "TaskCandidateSet.errors", allow_empty=True) + if valid + len(errors) != requested: + raise ValueError( + "Valid responses plus errors must equal requested_candidate_count." + ) + result.update( + { + "task_id": task_id, + "instruction": instruction, + "requested_candidate_count": requested, + "valid_response_count": valid, + "candidates": candidates, + "errors": errors, + } + ) + return result + + +def _mapping(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)): + raise ValueError(f"{context} must be a list.") + return list(value) + + +def _keys( + value: Mapping[str, Any], expected: set[str] | frozenset[str], context: str +) -> None: + if set(value) != set(expected): + raise ValueError( + f"{context} requires exactly fields {sorted(expected)}; received {sorted(value)}." + ) + + +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}.") + + +def _string(value: Any, context: str) -> str: + if not isinstance(value, str): + raise ValueError(f"{context} must be a string.") + return value.strip() + + +def _nonempty(value: Any, context: str) -> str: + result = _string(value, context) + if not result: + raise ValueError(f"{context} must not be empty.") + return result + + +def _enum(value: Any, choices: set[str], context: str) -> str: + result = _string(value, context) + if result not in choices: + raise ValueError(f"{context} must be one of {sorted(choices)}.") + return result + + +def _integer( + value: Any, context: str, *, minimum: int, maximum: int | None = None +) -> int: + if ( + not isinstance(value, int) + or isinstance(value, bool) + or value < minimum + or (maximum is not None and value > maximum) + ): + raise ValueError(f"{context} must be an integer in the allowed range.") + return value + + +def _strings(value: Any, context: str, *, allow_empty: bool = False) -> list[str]: + result = [_string(item, context) for item in _sequence(value, context)] + if not allow_empty and any(not item for item in result): + raise ValueError(f"{context} values must not be empty.") + if len(result) != len(set(result)): + raise ValueError(f"{context} values must be unique.") + return result + + +def _mapping_sequence(value: Any, context: str) -> list[dict[str, Any]]: + result = [_mapping(item, context) for item in _sequence(value, context)] + _json_safe(result, context) + return result + + +def _digest(value: Any, context: str) -> str: + result = _string(value, context) + if len(result) != 64 or any( + character not in "0123456789abcdef" for character in result + ): + raise ValueError(f"{context} must be a lowercase SHA-256 digest.") + return result + + +def _unique(values: Sequence[str], context: str) -> None: + if len(values) != len(set(values)): + raise ValueError(f"{context} must be unique.") + + +def _json_safe(value: Any, context: str) -> None: + try: + json.dumps(value, allow_nan=False) + except (TypeError, ValueError) as error: + raise ValueError(f"{context} must be finite and JSON serializable.") from error diff --git a/embodichain/gen_sim/task_engine/defaults.yaml b/embodichain/gen_sim/task_engine/defaults.yaml new file mode 100644 index 000000000..7df1ce454 --- /dev/null +++ b/embodichain/gen_sim/task_engine/defaults.yaml @@ -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. +# ---------------------------------------------------------------------------- + +schema_version: embodichain.task-engine-defaults/v1 + +workflow: + max_parallel_workers: 2 + max_scene_attempts: 2 + max_action_attempts: 1 + +planning: + candidate_count: 3 + planning_mode: offline + max_episodes: 1 + max_episode_steps: 10000 + +execution: + num_envs: 1 + success_policy: any + min_successful_envs: 1 diff --git a/embodichain/gen_sim/task_engine/interpretation.py b/embodichain/gen_sim/task_engine/interpretation.py new file mode 100644 index 000000000..b16d09f60 --- /dev/null +++ b/embodichain/gen_sim/task_engine/interpretation.py @@ -0,0 +1,1210 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Scene-independent structured interpretation for Task Engine.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from copy import deepcopy +from dataclasses import dataclass +import json +import os +from pathlib import Path +import re +from time import perf_counter +from typing import Any, TypeAlias + +from .ontology import ( + RELATIONS, + TASK_CONTRACTS, + TERMINAL_BEHAVIORS, + TRANSPORT_DIRECTIONS, +) + +__all__ = [ + "INSTRUCTION_INTENT_SCHEMA", + "InstructionDraftResult", + "InstructionIntent", + "InstructionCaller", + "interpret_instruction_draft", + "validate_instruction_intent", +] + +InstructionCaller = Callable[..., Mapping[str, Any]] +InstructionIntent: TypeAlias = dict[str, Any] +TASK_TYPES = frozenset(TASK_CONTRACTS) + +_RELATIONS = RELATIONS +_ARMS = frozenset({"none", "auto", "left_arm", "right_arm"}) +_ORIENTATIONS = frozenset({"none", "preserve", "upright"}) +_TARGET_STATES = frozenset({"none", "open", "closed", "activated"}) +_LAYOUTS = frozenset({"none", "line"}) +_AXES = frozenset({"none", "world_x", "world_y"}) +_DIRECTIONS = TRANSPORT_DIRECTIONS +_TERMINAL_BEHAVIORS = TERMINAL_BEHAVIORS +_SELECTOR_KINDS = frozenset({"none", "scene_ref", "step_result"}) +_QUANTIFIERS = frozenset({"one", "all", "count"}) +_STEP_KEYS = frozenset( + { + "id", + "task_type", + "object", + "target", + "relation", + "required_arm", + "transfer_arm", + "receive_arm", + "orientation_goal", + "target_state", + "target_setting", + "layout", + "axis", + "direction", + "terminal_behavior", + "depends_on", + } +) +_INTENT_TASK_FIELD_REGISTRY = { + task_type: contract.applicable_intent_fields + for task_type, contract in TASK_CONTRACTS.items() +} +_INTENT_FIELD_DEFAULTS: dict[str, Any] = { + "target": None, + "relation": "none", + "required_arm": "none", + "transfer_arm": "none", + "receive_arm": "none", + "orientation_goal": "none", + "target_state": "none", + "target_setting": 0, + "layout": "none", + "axis": "none", + "direction": "none", + "terminal_behavior": "none", +} +_SELECTOR_KEYS = frozenset( + { + "kind", + "step_id", + "reference", + "quantifier", + "count", + } +) +_FORBIDDEN_FIELDS = frozenset( + { + "atomic_action", + "atomic_actions", + "atomicaction", + "coordinates", + "bbox", + "bboxes", + "grasp_pose", + "keypoint", + "keypoints", + "joint_positions", + "joints", + "pose", + "position", + "qpos", + "rotation", + "target_pose", + "translation", + "trajectory", + "waypoints", + } +) +# MiMo's OpenAI-compatible endpoint can spend the whole completion budget in +# hidden reasoning when the request leaves thinking enabled. A sparse final +# JSON object then looks like a schema failure to the deterministic verifier. +# Keep the budget bounded and turn reasoning off for the text interpretation +# call; the parser must return an auditable object rather than a thought trace. +_MIMO_MAX_COMPLETION_TOKENS = 4096 +_GEN_SIM_DIR = Path(__file__).resolve().parents[1] +_GEN_SIM_ENV_PATH = _GEN_SIM_DIR / ".env" +_GEN_CONFIG_PATH = _GEN_SIM_DIR / "simready_pipeline" / "configs" / "gen_config.json" + + +class _MissingRequiredTargetError(ValueError): + """Identify a validation failure that receives targeted repair guidance.""" + + +class _MissingRequiredObjectError(ValueError): + """Identify a missing manipulated-object selector for targeted repair.""" + + +@dataclass(frozen=True) +class InstructionDraftResult: + """One validated, scene-independent interpretation and its audit metadata.""" + + intent: InstructionIntent + model: str + attempts: int + latency_seconds: float + normalizations: tuple[dict[str, Any], ...] + + +# Object semantics remain open natural-language references until the dedicated +# scene-grounding phase resolves them. All other values are strict protocol +# enums; non-canonical model output is repaired by the model, never guessed by +# a local language alias table. + +_SELECTOR_SCHEMA = { + "type": "object", + "additionalProperties": False, + "required": sorted(_SELECTOR_KEYS), + "properties": { + "kind": {"type": "string", "enum": sorted(_SELECTOR_KINDS)}, + "step_id": {"type": "string"}, + "reference": {"type": "string"}, + "quantifier": {"type": "string", "enum": sorted(_QUANTIFIERS)}, + "count": {"type": "integer", "minimum": 0}, + }, +} + +_INTENT_OUTPUT_SCHEMA = { + "title": "ActionEngineInstructionIntent", + "type": "object", + "additionalProperties": False, + "required": ["steps"], + "properties": { + "steps": { + "type": "array", + "minItems": 1, + "items": { + "type": "object", + "additionalProperties": False, + "required": sorted(_STEP_KEYS), + "properties": { + "id": {"type": "string"}, + "task_type": {"type": "string", "enum": sorted(TASK_TYPES)}, + "object": _SELECTOR_SCHEMA, + "target": _SELECTOR_SCHEMA, + "relation": {"type": "string", "enum": sorted(_RELATIONS)}, + "required_arm": {"type": "string", "enum": sorted(_ARMS)}, + "transfer_arm": {"type": "string", "enum": sorted(_ARMS)}, + "receive_arm": {"type": "string", "enum": sorted(_ARMS)}, + "orientation_goal": { + "type": "string", + "enum": sorted(_ORIENTATIONS), + }, + "target_state": { + "type": "string", + "enum": sorted(_TARGET_STATES), + }, + "target_setting": {"type": "integer"}, + "layout": {"type": "string", "enum": sorted(_LAYOUTS)}, + "axis": {"type": "string", "enum": sorted(_AXES)}, + "direction": { + "type": "string", + "enum": sorted(_DIRECTIONS), + }, + "terminal_behavior": { + "type": "string", + "enum": sorted(_TERMINAL_BEHAVIORS), + }, + "depends_on": { + "type": "array", + "items": {"type": "string"}, + }, + }, + }, + } + }, +} + +# Keep a read-only-by-convention public copy for callers that need to configure +# a structured client. The schema is an input contract, not a persisted task +# graph; ``validate_instruction_intent`` remains the authoritative verifier. +INSTRUCTION_INTENT_SCHEMA = deepcopy(_INTENT_OUTPUT_SCHEMA) + + +def interpret_instruction_draft( + instruction: str, + *, + model: str | None = None, + caller: InstructionCaller | None = None, +) -> InstructionDraftResult: + """Interpret one instruction without reading or grounding a scene.""" + instruction_text = str(instruction).strip() + if not instruction_text: + raise ValueError("instruction must be non-empty.") + prompt = _instruction_prompt(instruction_text) + invoke = caller or _default_instruction_caller + # An injected caller owns its transport and does not need provider config. + selected_model = model if caller is not None else _instruction_model(model) + if caller is None and selected_model is None: + raise ValueError( + "A text LLM model is required through --llm-model, " + "ACTION_ENGINE_LLM_MODEL, or OPENAI_MODEL." + ) + 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 JSON was invalid. Return a corrected " + "JSON object only; do not repeat the sparse response. Every step " + "must contain all 16 step keys and every selector all 5 selector " + "keys. Keep semantic fields explicit: E4 requires transfer_arm " + "and receive_arm, and E1/E3 require target plus relation (unless " + "E1 layout=line). Use canonical defaults only for fields that do " + "not apply. Validation error: " + f"{first_error}\n" + "Copy this complete shape before filling values (shape only; do " + "not copy its values or step count):\n" + f"{json.dumps(_instruction_shape_example(), ensure_ascii=False, sort_keys=True)}\n" + "Selector kind rules:\n" + f"{_instruction_selector_rules()}" + f"{_instruction_repair_guidance(first_error)}" + ) + try: + response = invoke( + prompt=current_prompt, + schema=deepcopy(INSTRUCTION_INTENT_SCHEMA), + model=selected_model, + ) + normalized, normalizations = _normalize_instruction_intent_fields( + _coerce_instruction_response(response) + ) + intent = validate_instruction_intent(normalized) + return InstructionDraftResult( + intent=intent, + model=selected_model or "injected_caller", + attempts=attempt + 1, + latency_seconds=perf_counter() - started, + normalizations=tuple(normalizations), + ) + except (TypeError, ValueError) as error: + if attempt: + raise ValueError( + "Instruction intent failed validation after one repair: " f"{error}" + ) from error + first_error = error + raise AssertionError("unreachable") + + +def _normalize_instruction_intent_fields( + value: Mapping[str, Any], +) -> tuple[dict[str, Any], list[dict[str, Any]]]: + """Canonicalize defaults and uniquely constrained cross-step continuity. + + The strict public validator deliberately remains unchanged. This pass is + confined to the LLM boundary, where weak JSON-mode providers sometimes + copy a meaningful value into an inapplicable slot such as E4.required_arm. + Required scene facts and ambiguous arm assignments are never inferred here + and still fail closed. + """ + result = deepcopy(dict(value)) + raw_steps = result.get("steps") + if not isinstance(raw_steps, list): + return result, [] + changes: list[dict[str, Any]] = [] + for index, raw_step in enumerate(raw_steps): + if not isinstance(raw_step, dict) or set(raw_step) != _STEP_KEYS: + continue + task_type = raw_step.get("task_type") + applicable = _INTENT_TASK_FIELD_REGISTRY.get(task_type) + if applicable is None: + continue + for field, configured_default in _INTENT_FIELD_DEFAULTS.items(): + field_applies = field in applicable + if task_type == "E1" and field in {"target", "relation"}: + field_applies = raw_step.get("layout") != "line" + if task_type == "E1" and field == "axis": + field_applies = raw_step.get("layout") == "line" + if field_applies: + continue + default = ( + _empty_selector() + if field == "target" and configured_default is None + else deepcopy(configured_default) + ) + if raw_step[field] == default: + continue + previous = deepcopy(raw_step[field]) + raw_step[field] = default + changes.append( + { + "path": f"steps[{index}].{field}", + "from": previous, + "to": deepcopy(default), + "reason": f"inapplicable_for_{task_type}", + } + ) + target = raw_step.get("target") + if ( + task_type == "E5" + and isinstance(target, Mapping) + and target.get("kind") == "none" + and raw_step.get("relation") == "none" + and raw_step.get("direction") == "none" + and raw_step.get("terminal_behavior") == "hold" + ): + raw_step["direction"] = "up" + changes.append( + { + "path": f"steps[{index}].direction", + "from": "none", + "to": "up", + "reason": "e5_hold_defaults_to_lift", + } + ) + if task_type == "E4" and raw_step.get("terminal_behavior") == "none": + terminal = ( + "place" + if isinstance(target, Mapping) and target.get("kind") != "none" + else "hold" + ) + raw_step["terminal_behavior"] = terminal + changes.append( + { + "path": f"steps[{index}].terminal_behavior", + "from": "none", + "to": terminal, + "reason": "e4_terminal_inferred_from_own_target", + } + ) + return result, changes + + +def _empty_selector() -> dict[str, Any]: + """Return the canonical selector value for an inapplicable target.""" + return { + "kind": "none", + "step_id": "", + "reference": "", + "quantifier": "one", + "count": 0, + } + + +def validate_instruction_intent(value: Mapping[str, Any]) -> dict[str, Any]: + """Validate the private, non-graph instruction interpretation contract.""" + if not isinstance(value, Mapping): + raise TypeError("Instruction intent must be a mapping.") + _reject_forbidden_fields(value) + if set(value) != {"steps"}: + raise ValueError("Instruction intent may contain only 'steps'.") + raw_steps = value.get("steps") + if not isinstance(raw_steps, Sequence) or isinstance(raw_steps, (str, bytes)): + raise ValueError("Instruction intent steps must be a list.") + if not raw_steps: + raise ValueError("Instruction intent steps must not be empty.") + steps = [] + ids: set[str] = set() + dependencies: dict[str, list[str]] = {} + for index, raw in enumerate(raw_steps): + context = f"InstructionIntent.steps[{index}]" + if not isinstance(raw, Mapping): + raise ValueError(f"{context} must be a mapping.") + if set(raw) != _STEP_KEYS: + raise ValueError( + f"{context} requires exactly fields {sorted(_STEP_KEYS)}; " + f"received {sorted(raw)}." + ) + step = deepcopy(dict(raw)) + step_id = _nonempty(step["id"], f"{context}.id") + if step_id in ids: + raise ValueError(f"Duplicate instruction step ID {step_id!r}.") + ids.add(step_id) + step["id"] = step_id + step["task_type"] = _choice( + step["task_type"], TASK_TYPES, f"{context}.task_type" + ) + step["object"] = _validate_selector(step["object"], f"{context}.object") + step["target"] = _validate_selector(step["target"], f"{context}.target") + step["relation"] = _canonical_relation(step["relation"], f"{context}.relation") + for key in ("required_arm", "transfer_arm", "receive_arm"): + step[key] = _canonical_arm(step[key], f"{context}.{key}") + step["orientation_goal"] = _canonical_orientation( + step["orientation_goal"], f"{context}.orientation_goal" + ) + step["target_state"] = _choice( + step["target_state"], _TARGET_STATES, f"{context}.target_state" + ) + if isinstance(step["target_setting"], bool) or not isinstance( + step["target_setting"], int + ): + raise ValueError(f"{context}.target_setting must be an integer.") + step["layout"] = _choice(step["layout"], _LAYOUTS, f"{context}.layout") + step["axis"] = _choice(step["axis"], _AXES, f"{context}.axis") + step["direction"] = _choice( + step["direction"], _DIRECTIONS, f"{context}.direction" + ) + step["terminal_behavior"] = _choice( + step["terminal_behavior"], + _TERMINAL_BEHAVIORS, + f"{context}.terminal_behavior", + ) + raw_depends = step["depends_on"] + if not isinstance(raw_depends, Sequence) or isinstance( + raw_depends, (str, bytes) + ): + raise ValueError(f"{context}.depends_on must be a list.") + step["depends_on"] = [ + _nonempty(item, f"{context}.depends_on") for item in raw_depends + ] + if step_id in step["depends_on"]: + raise ValueError(f"{context}.depends_on cannot contain its own ID.") + dependencies[step_id] = step["depends_on"] + _validate_task_fields(step, context) + steps.append(step) + positions = {str(step["id"]): index for index, step in enumerate(steps)} + for index, step in enumerate(steps): + for selector_name in ("object", "target"): + selector = step[selector_name] + if selector["kind"] != "step_result": + continue + reference = str(selector["step_id"]) + if reference not in positions: + raise ValueError( + f"Instruction step {step['id']!r} {selector_name} references " + f"unknown step {reference!r}." + ) + if positions[reference] >= index: + raise ValueError( + f"Instruction step {step['id']!r} {selector_name} must reference " + f"a preceding step, not {reference!r}." + ) + for step_id, depends_on in dependencies.items(): + unknown = set(depends_on) - ids + if unknown: + raise ValueError( + f"Instruction step {step_id!r} has unknown dependencies " + f"{sorted(unknown)}." + ) + _validate_dag(dependencies) + return {"steps": steps} + + +def _validate_selector(value: Any, context: str) -> dict[str, Any]: + if not isinstance(value, Mapping): + raise ValueError(f"{context} must be a mapping.") + if set(value) != _SELECTOR_KEYS: + raise ValueError( + f"{context} requires exactly fields {sorted(_SELECTOR_KEYS)}; " + f"received {sorted(value)}." + ) + selector = deepcopy(dict(value)) + selector["kind"] = _choice(selector["kind"], _SELECTOR_KINDS, f"{context}.kind") + selector["step_id"] = _selector_string(selector["step_id"], f"{context}.step_id") + selector["reference"] = _selector_string( + selector["reference"], f"{context}.reference" + ) + selector["quantifier"] = _canonical_quantifier( + selector["quantifier"], f"{context}.quantifier" + ) + if isinstance(selector["count"], bool) or not isinstance(selector["count"], int): + raise ValueError(f"{context}.count must be an integer.") + if selector["count"] < 0: + raise ValueError(f"{context}.count must be non-negative.") + kind = selector["kind"] + if kind == "scene_ref" and not selector["reference"]: + raise ValueError(f"{context} scene_ref requires a reference.") + if kind == "step_result": + if not selector["step_id"]: + raise ValueError(f"{context} step_result requires step_id.") + if selector["reference"]: + raise ValueError( + f"{context} step_result may identify only a prior step_id." + ) + if selector["quantifier"] != "one" or selector["count"] != 0: + raise ValueError( + f"{context} step_result requires quantifier=one and count=0." + ) + if kind == "scene_ref" and selector["step_id"]: + raise ValueError(f"{context} scene_ref cannot carry step_id.") + if kind == "none" and (selector["step_id"] or selector["reference"]): + raise ValueError(f"{context} kind=none cannot carry constraints.") + if kind == "none" and (selector["quantifier"] != "one" or selector["count"] != 0): + raise ValueError(f"{context} kind=none requires quantifier=one and count=0.") + if selector["quantifier"] == "one" and selector["count"] != 0: + raise ValueError(f"{context} quantifier=one requires count=0.") + if selector["quantifier"] == "all" and selector["count"] != 0: + raise ValueError(f"{context} quantifier=all requires count=0.") + if selector["quantifier"] == "count" and selector["count"] < 1: + raise ValueError(f"{context} quantifier=count requires count>=1.") + return selector + + +def _validate_task_fields(step: Mapping[str, Any], context: str) -> None: + task_type = str(step["task_type"]) + if step["object"]["kind"] == "none": + raise _MissingRequiredObjectError( + f"{context} {task_type} requires an object selector." + ) + target_kind = str(step["target"]["kind"]) + if task_type not in {"E1", "E3", "E4", "E5"} and step["relation"] != "none": + raise ValueError(f"{context} {task_type} does not accept relation.") + if task_type == "E3" and step["relation"] != "above": + raise ValueError(f"{context} E3 relation must be above.") + target_setting = int(step["target_setting"]) + if task_type != "E8" and target_setting != 0: + raise ValueError(f"{context} target_setting is only valid for E8.") + if task_type != "E1" and step["axis"] != "none": + raise ValueError(f"{context} axis is only valid for E1 line arrangement.") + if task_type == "E1" and step["layout"] != "line" and step["axis"] != "none": + raise ValueError(f"{context} axis is only valid for E1 line arrangement.") + if task_type not in {"E6", "E7", "E9"} and step["target_state"] != "none": + raise ValueError(f"{context} target_state is not valid for {task_type}.") + if task_type != "E4" and step["transfer_arm"] != "none": + raise ValueError(f"{context} transfer_arm is only valid for E4.") + if task_type != "E4" and step["receive_arm"] != "none": + raise ValueError(f"{context} receive_arm is only valid for E4.") + orientation_goal = str(step["orientation_goal"]) + if task_type == "E2" and orientation_goal != "upright": + raise ValueError(f"{context} E2 orientation_goal must be upright.") + if task_type not in {"E1", "E2", "E4"} and orientation_goal != "none": + raise ValueError( + f"{context} orientation_goal is only valid for E1, E2, and E4." + ) + if task_type == "E1" and step["layout"] == "line": + if target_kind != "none": + raise ValueError(f"{context} E1 line arrangement cannot carry a target.") + if step["relation"] != "none": + raise ValueError(f"{context} E1 line arrangement cannot carry a relation.") + elif task_type in {"E1", "E3"}: + if target_kind == "none": + raise _MissingRequiredTargetError( + f"{context} {task_type} requires a target selector." + ) + if step["relation"] == "none" and task_type == "E3": + raise ValueError(f"{context} {task_type} requires a symbolic relation.") + elif task_type == "E4": + terminal = str(step["terminal_behavior"]) + effective_terminal = ( + "place" if terminal == "none" and target_kind != "none" else terminal + ) + if effective_terminal == "none": + effective_terminal = "hold" + if effective_terminal not in _TERMINAL_BEHAVIORS - {"none"}: + raise ValueError(f"{context} E4 requires terminal_behavior hold/place.") + if effective_terminal == "place": + if target_kind == "none" or step["relation"] == "none": + raise ValueError( + f"{context} E4 terminal_behavior=place requires target and relation." + ) + elif target_kind != "none" or step["relation"] != "none": + raise ValueError( + f"{context} E4 terminal_behavior=hold cannot carry target or relation." + ) + elif task_type == "E5": + direction = str(step["direction"]) + terminal = str(step["terminal_behavior"]) + if terminal not in _TERMINAL_BEHAVIORS - {"none"}: + raise ValueError(f"{context} E5 requires terminal_behavior hold/place.") + if target_kind == "none": + if step["relation"] != "none": + raise ValueError(f"{context} E5 relation requires a target selector.") + if direction == "none" and terminal != "place": + raise ValueError( + f"{context} E5 requires a direction or target relation." + ) + else: + if step["relation"] == "none": + raise ValueError(f"{context} E5 target requires a relation.") + if direction != "none": + raise ValueError( + f"{context} E5 target relation cannot also carry direction." + ) + elif target_kind != "none": + raise ValueError(f"{context} {task_type} does not accept a target selector.") + if task_type not in {"E4", "E5"}: + if step["direction"] != "none": + raise ValueError(f"{context} direction is only valid for E5.") + if step["terminal_behavior"] != "none": + raise ValueError(f"{context} terminal_behavior is only valid for E5.") + if task_type == "E4": + transfer = str(step["transfer_arm"]) + receive = str(step["receive_arm"]) + if transfer not in {"left_arm", "right_arm"} or receive not in { + "left_arm", + "right_arm", + }: + raise ValueError(f"{context} E4 requires two explicit arms.") + if transfer == receive: + raise ValueError(f"{context} E4 transfer and receive arms must differ.") + if step["required_arm"] not in {"none", "auto"}: + raise ValueError( + f"{context} E4 uses transfer_arm/receive_arm, not required_arm." + ) + if task_type in {"E1", "E2"} and step["required_arm"] == "none": + raise ValueError( + f"{context} {task_type} requires required_arm=auto/left_arm/right_arm." + ) + if task_type == "E5" and step["required_arm"] not in {"none", "auto"}: + raise ValueError(f"{context} E5 always uses both arms, not required_arm.") + if task_type == "E6" and step["target_state"] != "open": + raise ValueError(f"{context} E6 target_state must be open.") + if task_type == "E7" and step["target_state"] != "closed": + raise ValueError(f"{context} E7 target_state must be closed.") + if task_type == "E9" and step["target_state"] != "activated": + raise ValueError(f"{context} E9 target_state must be activated.") + if step["layout"] == "line" and task_type != "E1": + raise ValueError(f"{context} only E1 supports layout=line.") + + +def _instruction_prompt(instruction: str) -> str: + return ( + "Convert the user's explicit L1-L3 instruction into typed E1-E9 task " + "intent. Understand synonyms, ellipsis, and pronouns, but " + "do not invent missing objects. Use step_result for cross-step pronouns " + "and explicit references to the result of an earlier manipulation. Keep " + "an independently selected repeated noun as scene_ref; identical text " + "alone does not prove object identity. " + "Object directions are robot-relative; arm names are robot body sides. " + "Preserve each concrete object or target phrase from the instruction as " + "an open scene_ref.reference. Do not classify it or emit a scene UID. " + "Emit no AtomicAction, category label, affordance, coordinates, poses, " + "paths, or reasoning. Encode explicit ordering with depends_on; same-action set " + "members may remain independent. Use empty strings and 'none' for " + "inapplicable required fields. A request to retract the transfer arm " + "E4 owns the complete transfer. For a handover followed by placement in " + "the same user intent, emit one E4 with target, relation, and " + "terminal_behavior=place; do not emit a trailing E1. Use " + "terminal_behavior=hold only when the receiver should keep holding the " + "object. The exact output keys are steps -> id, " + "task_type, object, target, relation, required_arm, transfer_arm, " + "receive_arm, orientation_goal, target_state, target_setting, layout, " + "axis, direction, terminal_behavior, depends_on; each selector has kind, " + "step_id, reference, quantifier, count.\n\n" + "Use orientation_goal=none unless the instruction explicitly requests " + "upright orientation or preserving the original orientation. Spatial " + "placement and handover alone do not imply preserve. " + "Emptying, dumping, or pouring contents from one container into another " + "is exactly one E3 step: object selects the source container, target " + "selects the receiving container, and relation=above. Pickup and staging " + "are internal to that E3 step. " + "Opening or pulling out a drawer is E6 with object selecting that drawer " + "and target_state=open. Closing or pushing in a drawer is E7 with object " + "selecting that drawer and target_state=closed. " + f"Instruction:\n{instruction}\n\n" + f"E1-E9 catalog:\n{json.dumps(_intent_capability_catalog(), ensure_ascii=False, sort_keys=True)}\n\n" + "Shape-only complete JSON example (do not copy its step count or values; " + "copy every key, including keys whose value is none/empty/0):\n" + f"{json.dumps(_instruction_shape_example(), ensure_ascii=False, sort_keys=True)}\n\n" + "Selector kind rules (these are not extra output fields):\n" + f"{_instruction_selector_rules()}\n\n" + "For E5, use target+relation for moving an object relative to another " + "object, or direction for a small robot-relative move. When a request " + "combines lifting with a horizontal direction, preserve that horizontal " + "direction: 'carry forward and keep raised' uses direction=front and " + "terminal_behavior=hold. The hold recipe includes a raised terminal pose. " + "Use direction=up only for a dual-arm pick, lift, raise, or hold request " + "without an explicit horizontal direction or another target, " + "and terminal_behavior=hold. Use hold unless the instruction explicitly " + "says to put/release the object. For pick " + "and release at the original location, use direction=none and place. A dual-arm " + "pick/move/transport request uses E5. Final checklist: every step " + "has all 16 step keys; every object and target " + "has all 5 selector keys. For an inapplicable field use the canonical " + "default shown in the example, never omit the field. E4 must explicitly " + "state transfer_arm, receive_arm, and terminal_behavior. E1/E3 must explicitly state target " + "and relation (except E1 layout=line). For E1 and E2, copy an explicitly " + "named left/right arm into required_arm; use required_arm=auto only when " + "the instruction does not specify an arm, and never use none." + ) + + +def _instruction_shape_example() -> dict[str, Any]: + """Return a compact field-complete example for providers with weak schemas.""" + selector = { + "kind": "scene_ref", + "step_id": "", + "reference": "example object A", + "quantifier": "one", + "count": 0, + } + empty_selector = { + "kind": "none", + "step_id": "", + "reference": "", + "quantifier": "one", + "count": 0, + } + return { + "steps": [ + { + "id": "step_1", + "task_type": "E2", + "object": selector, + "target": empty_selector, + "relation": "none", + "required_arm": "auto", + "transfer_arm": "none", + "receive_arm": "none", + "orientation_goal": "upright", + "target_state": "none", + "target_setting": 0, + "layout": "none", + "axis": "none", + "direction": "none", + "terminal_behavior": "none", + "depends_on": [], + } + ] + } + + +def _instruction_selector_rules() -> str: + """Return the mutually exclusive selector encodings for model prompts.""" + step_result = { + "kind": "step_result", + "step_id": "step_1", + "reference": "", + "quantifier": "one", + "count": 0, + } + return ( + "- kind=none: step_id and reference are empty strings; " + "quantifier='one'; count=0.\n" + "- kind=scene_ref: step_id is empty and reference preserves the concrete " + "object phrase from the user's instruction. Repeated scene_ref text does " + "not establish cross-step identity.\n" + "- kind=step_result: use it only for a pronoun that means exactly one " + "object, or an explicit continuation of the result of an earlier " + "instruction step. Set step_id to that prior " + "step ID and set reference='', quantifier='one', count=0. Do not copy " + "the prior object's phrase into this selector. Replace step_1 in this " + f"complete shape with the actual prior step ID: {json.dumps(step_result, sort_keys=True)}\n" + "A step_result may identify only a prior step_id; it cannot carry any " + "other object constraint." + ) + + +def _instruction_repair_guidance(error: Exception) -> str: + """Add narrow semantic guidance for errors weak JSON-mode models repeat.""" + if "E4 transfer and receive arms must differ" in str(error): + return ( + "\nSame-arm handover repair rule: transfer_arm and receive_arm must " + "name different arms. Preserve the explicitly stated transfer arm. " + "When a later clause clearly continues with the handed object using " + "the other arm, use that arm as receive_arm. Resolve coreference from " + "the instruction semantics; identical scene_ref text alone does not " + "prove that two independently selected objects are the same.\n" + ) + if isinstance(error, _MissingRequiredObjectError): + return ( + "\nMissing-object repair rule: preserve the selected task_type and " + "set object to a scene_ref that preserves the explicit manipulated " + "object phrase from the instruction. For E6/E7 the drawer, door, or " + "other articulated part is the object selector; target remains none.\n" + ) + if not isinstance(error, _MissingRequiredTargetError): + return "" + if " E3 requires a target selector" in str(error): + return ( + "\nMissing-target repair rule for E3: keep task_type=E3. object is " + "the source container whose contents are poured, target is the " + "receiving container, and relation must be above. An explicit grab " + "is part of the same E3 task.\n" + ) + return ( + "\nMissing-target repair rule: for a non-line E1 placement, object is " + "the item being moved and target is the explicit reference object " + "after the spatial relation in the original instruction. For example, " + "in 'place it to the left of the striped pedestal', object is the earlier " + "step_result for 'it', while target selects the striped pedestal; target " + "must not use kind=none. Use target kind=step_result only when the " + "reference object itself is exactly the result of a prior step.\n" + ) + + +def _intent_capability_catalog() -> dict[str, dict[str, Any]]: + """Return the LLM's thin, import-safe E1-E9 capability view. + + Action Engine's online planning catalog also reports runtime availability + and therefore imports simulator action classes. Text interpretation only + needs symbolic E semantics and must remain testable before a simulator + backend is installed. + """ + return { + task_type: { + "semantics": contract.semantics, + "applicable_fields": sorted(_INTENT_TASK_FIELD_REGISTRY[task_type]), + } + for task_type, contract in TASK_CONTRACTS.items() + } + + +def _default_instruction_caller( + *, + prompt: str, + schema: Mapping[str, Any], + model: str | None, +) -> Mapping[str, Any]: + 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": (), + } + for key in ("base_url", "default_query"): + if settings[key]: + kwargs[key] = settings[key] + if _is_mimo_compatible(settings): + # MiMo documents ``thinking`` as a provider extension carried in the + # OpenAI client's extra body. Disabling it is important here: hidden + # reasoning can consume the completion and leave only id/object/type. + kwargs.update( + { + "max_completion_tokens": _MIMO_MAX_COMPLETION_TOKENS, + "extra_body": {"thinking": {"type": "disabled"}}, + } + ) + client = ChatOpenAI(**kwargs) + # The full schema remains in the prompt and the local validator is still + # authoritative even when the provider only offers JSON mode. + 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)}" + ) + response = structured.invoke( + [ + SystemMessage( + content=( + "Return only the requested structured JSON response. Never " + "return reasoning, coordinates, or AtomicAction nodes." + ) + ), + HumanMessage(content=schema_prompt), + ] + ) + return _coerce_instruction_response(response) + + +def _instruction_model(explicit: str | None) -> str | None: + if isinstance(explicit, str) and explicit.strip(): + return explicit.strip() + # Keep model selection separate from credential loading. Reading the local + # dotenv file is side-effect free and gives generation the documented + # priority without leaking credentials into TaskSpec metadata. + for name in ("TASK_ENGINE_LLM_MODEL", "ACTION_ENGINE_LLM_MODEL", "OPENAI_MODEL"): + for source in ( + os.environ, + _load_local_env(), + ): + value = source.get(name) + if isinstance(value, str) and value.strip(): + return value.strip() + return None + + +def _load_local_env() -> dict[str, str]: + """Read Task Engine model configuration without mutating the environment.""" + return _load_env_file(_GEN_SIM_ENV_PATH) + + +def _is_mimo_compatible(settings: Mapping[str, Any]) -> bool: + 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: + if not hasattr(client, "with_structured_output"): + return client + method = "json_mode" + try: + return client.with_structured_output(schema, method=method) + except (TypeError, ValueError): + if method == "json_mode" and hasattr(client, "bind"): + from langchain_core.output_parsers import JsonOutputParser + + return ( + client.bind(response_format={"type": "json_object"}) + | JsonOutputParser() + ) + return client.with_structured_output(schema) + + +def _load_llm_settings(*, model: str | None) -> dict[str, Any]: + local_env = _load_local_env() + config: dict[str, Any] = {} + if _GEN_CONFIG_PATH.is_file(): + raw = json.loads(_GEN_CONFIG_PATH.read_text(encoding="utf-8")) + if isinstance(raw, Mapping): + llm = raw.get("llm", {}) + if isinstance(llm, Mapping): + configured = llm.get("openai_compatible", {}) + if isinstance(configured, Mapping): + config = dict(configured) + 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, + "TASK_ENGINE_LLM_MODEL", + "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 Task Engine interpretation. Set it " + f"in the process environment or {_GEN_SIM_ENV_PATH}." + ) + if not selected_model: + raise ValueError( + "A text LLM model is required through model=, TASK_ENGINE_LLM_MODEL, " + f"OPENAI_MODEL, 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]: + 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: + 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 _choice(value: Any, allowed: set[str] | frozenset[str], context: str) -> str: + if not isinstance(value, str) or value not in allowed: + raise ValueError(f"{context} must be one of {sorted(allowed)}.") + return value + + +def _selector_string(value: Any, context: str) -> str: + if not isinstance(value, str): + raise ValueError(f"{context} must be a string.") + return value.strip() + + +def _canonical_quantifier(value: Any, context: str) -> str: + return _choice(value, _QUANTIFIERS, context) + + +def _canonical_arm(value: Any, context: str) -> str: + return _choice(value, _ARMS, context) + + +def _canonical_relation(value: Any, context: str) -> str: + return _choice(value, _RELATIONS, context) + + +def _canonical_orientation(value: Any, context: str) -> str: + return _choice(value, _ORIENTATIONS, context) + + +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 _topological_steps(steps: Sequence[Mapping[str, Any]]) -> list[dict[str, Any]]: + """Return a stable topological ordering for validated intent steps.""" + 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 = list(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.") + for step_id in ready: + ordered.append(by_id[step_id]) + pending.remove(step_id) + return ordered + + +def _coerce_instruction_response(response: Any) -> Mapping[str, Any]: + """Coerce common structured-client response wrappers without accepting prose.""" + 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"Instruction model output has unsupported type {type(content).__name__}." + ) + text = content.strip() + if text.startswith("```"): + lines = text.splitlines() + if lines: + lines = lines[1:] + if lines and lines[-1].strip().startswith("```"): + lines = lines[:-1] + text = "\n".join(lines).strip() + try: + parsed = json.loads(text) + except json.JSONDecodeError as exc: + raise ValueError(f"Instruction model output is not valid JSON: {exc}") from exc + if not isinstance(parsed, Mapping): + raise ValueError("Instruction model output must decode to a JSON object.") + return dict(parsed) + + +def _validate_dag(dependencies: Mapping[str, Sequence[str]]) -> None: + visiting: set[str] = set() + visited: set[str] = set() + + def visit(node: str) -> None: + if node in visiting: + raise ValueError("Instruction intent dependencies contain a cycle.") + if node in visited: + return + visiting.add(node) + for dependency in dependencies[node]: + visit(str(dependency)) + visiting.remove(node) + visited.add(node) + + for node in dependencies: + visit(node) + + +def _reject_forbidden_fields(value: Any) -> None: + if isinstance(value, Mapping): + forbidden = _FORBIDDEN_FIELDS & {str(key).strip().lower() for key in value} + if forbidden: + raise ValueError( + f"Instruction intent contains forbidden fields {sorted(forbidden)}." + ) + for item in value.values(): + _reject_forbidden_fields(item) + elif isinstance(value, Sequence) and not isinstance(value, (str, bytes)): + for item in value: + _reject_forbidden_fields(item) diff --git a/embodichain/gen_sim/task_engine/ontology.py b/embodichain/gen_sim/task_engine/ontology.py new file mode 100644 index 000000000..1f497b645 --- /dev/null +++ b/embodichain/gen_sim/task_engine/ontology.py @@ -0,0 +1,292 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Scene-independent semantic ontology for the canonical E1-E9 tasks.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +from types import MappingProxyType +from typing import Any + +__all__ = [ + "RELATIONS", + "TASK_CONTRACTS", + "TERMINAL_BEHAVIORS", + "TRANSPORT_DIRECTIONS", + "TaskContract", + "task_contract", + "task_success_type", +] + + +# These are protocol values consumed by executable planners. They are not a +# vocabulary for matching words in user instructions. +RELATIONS = frozenset( + { + "none", + "on", + "inside", + "above", + "left_of", + "right_of", + "front_of", + "behind", + "front_left_of", + "front_right_of", + "back_left_of", + "back_right_of", + } +) +TRANSPORT_DIRECTIONS = frozenset( + { + "none", + "world_x", + "world_y", + "front", + "back", + "left", + "right", + "front_left", + "front_right", + "back_left", + "back_right", + "up", + "down", + } +) +TERMINAL_BEHAVIORS = frozenset({"none", "hold", "place"}) +_RESOURCE_MODES = frozenset({"single_arm", "handover", "coordinated"}) + + +@dataclass(frozen=True, slots=True) +class TaskContract: + """One scene-independent semantic E-task contract.""" + + task_type: str + semantics: 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], ...] + + def __post_init__(self) -> None: + if not self.primary_role_field.endswith("_role"): + raise ValueError("primary_role_field must name one role parameter.") + if self.resource_mode not in _RESOURCE_MODES: + raise ValueError(f"Unknown task resource_mode {self.resource_mode!r}.") + if self.direct_payload_relations - RELATIONS: + raise ValueError("direct_payload_relations contain unknown relations.") + terminal_behaviors = [item[0] for item in self.terminal_success_types] + if len(terminal_behaviors) != len(set(terminal_behaviors)): + raise ValueError("terminal_success_types must use unique behaviors.") + if set(terminal_behaviors) - TERMINAL_BEHAVIORS: + raise ValueError("terminal_success_types contain unknown behaviors.") + + +def _contract( + task_type: str, + semantics: str, + applicable_intent_fields: frozenset[str], + source_structure: str, + required_affordances: frozenset[str], + success_type: str, + *, + scene_affordances: frozenset[str] | None = None, + primary_role_field: str = "object_role", + resource_mode: str = "single_arm", + moves_primary_object: bool = False, + accepts_direct_payloads: bool = False, + direct_payload_relations: frozenset[str] = frozenset(), + accepts_incoming_hold: bool = False, + terminal_success_types: tuple[tuple[str, str], ...] = (), +) -> TaskContract: + return TaskContract( + task_type=task_type, + semantics=semantics, + applicable_intent_fields=applicable_intent_fields, + source_structure=source_structure, + required_affordances=required_affordances, + success_type=success_type, + scene_affordances=scene_affordances or required_affordances, + primary_role_field=primary_role_field, + resource_mode=resource_mode, + moves_primary_object=moves_primary_object, + accepts_direct_payloads=accepts_direct_payloads, + direct_payload_relations=direct_payload_relations, + accepts_incoming_hold=accepts_incoming_hold, + terminal_success_types=terminal_success_types, + ) + + +TASK_CONTRACTS: Mapping[str, TaskContract] = MappingProxyType( + { + "E1": _contract( + "E1", + "Pick, move, and place one object at a symbolic relation.", + frozenset( + { + "target", + "relation", + "required_arm", + "orientation_goal", + "layout", + "axis", + } + ), + "rigid_object", + frozenset({"graspable", "placeable"}), + "semantic_goal", + moves_primary_object=True, + accepts_direct_payloads=True, + direct_payload_relations=frozenset({"on", "inside"}), + accepts_incoming_hold=True, + ), + "E2": _contract( + "E2", + "Make one fallen object upright and place it stably.", + frozenset({"required_arm", "orientation_goal"}), + "rigid_object", + frozenset({"graspable", "orientable"}), + "object_upright", + moves_primary_object=True, + accepts_incoming_hold=True, + ), + "E3": _contract( + "E3", + "Pick up a source container, execute a tilt-and-restore pour over " + "a fixed target container, then place and home.", + frozenset({"target", "relation", "required_arm"}), + "rigid_object", + frozenset({"graspable", "pourable"}), + "poured", + primary_role_field="source_role", + moves_primary_object=True, + accepts_incoming_hold=True, + ), + "E4": _contract( + "E4", + "Transfer one object between arms, then either leave the receiver " + "holding it safely or place it at a symbolic relation.", + frozenset( + { + "target", + "relation", + "transfer_arm", + "receive_arm", + "orientation_goal", + "terminal_behavior", + } + ), + "rigid_object", + frozenset({"graspable", "handover"}), + "handover_complete", + resource_mode="handover", + moves_primary_object=True, + accepts_incoming_hold=True, + terminal_success_types=( + ("hold", "handover_complete"), + ("place", "semantic_goal"), + ), + ), + "E5": _contract( + "E5", + "Use both arms to pick, move, and optionally release one shared rigid object.", + frozenset({"target", "relation", "direction", "terminal_behavior"}), + "rigid_object", + frozenset({"dual_graspable"}), + "held_by_both_grippers", + scene_affordances=frozenset({"dual_graspable", "rigid"}), + resource_mode="coordinated", + moves_primary_object=True, + accepts_direct_payloads=True, + terminal_success_types=( + ("hold", "held_by_both_grippers"), + ("place", "semantic_goal"), + ), + ), + "E6": _contract( + "E6", + "Pull an articulated part to its requested state.", + frozenset({"required_arm", "target_state"}), + "articulation", + frozenset({"pullable"}), + "articulation_joint_near", + scene_affordances=frozenset({"articulated", "pullable"}), + ), + "E7": _contract( + "E7", + "Push an articulated part to its requested state.", + frozenset({"required_arm", "target_state"}), + "articulation", + frozenset({"pushable"}), + "articulation_joint_near", + scene_affordances=frozenset({"articulated", "pushable"}), + ), + "E8": _contract( + "E8", + "Turn one knob to a requested setting.", + frozenset({"required_arm", "target_setting"}), + "articulation", + frozenset({"turnable"}), + "articulation_joint_near", + ), + "E9": _contract( + "E9", + "Press one button until its requested terminal state.", + frozenset({"required_arm", "target_state"}), + "articulation", + frozenset({"pressable"}), + "pressed", + ), + } +) + + +def task_contract(task_type: str) -> TaskContract: + """Return the canonical contract or reject an unknown E-task type.""" + try: + return TASK_CONTRACTS[str(task_type)] + except KeyError as exc: + raise ValueError(f"Unsupported task type {task_type!r}.") from exc + + +def task_success_type( + task_type: str, + params: Mapping[str, Any] | None = None, +) -> str: + """Resolve a TaskSpec success type, including E5's terminal behavior.""" + contract = task_contract(task_type) + if not contract.terminal_success_types: + return contract.success_type + terminal_behavior = str((params or {}).get("terminal_behavior", "hold")) + success_by_behavior = dict(contract.terminal_success_types) + try: + return success_by_behavior[terminal_behavior] + except KeyError as exc: + raise ValueError( + f"{contract.task_type} terminal_behavior must be one of " + f"{sorted(success_by_behavior)}." + ) from exc diff --git a/embodichain/gen_sim/task_engine/orchestration/__init__.py b/embodichain/gen_sim/task_engine/orchestration/__init__.py new file mode 100644 index 000000000..8768ce678 --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/__init__.py @@ -0,0 +1,100 @@ +# ---------------------------------------------------------------------------- +# 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-owned orchestration across task, scene, and Semantic Skill engines.""" + +from __future__ import annotations + +from typing import Any + +from .artifacts import ( + ArtifactTransaction, + CONSERVATIVE_SCENE_GRAPH_FILENAME, + TaskEngineArtifactPaths, + FEASIBILITY_REPORT_FILENAME, + PREPARATION_FAILURE_FILENAME, + STATIC_SCENE_MANIFEST_FILENAME, + task_engine_artifact_paths, + write_execution_report, + write_preparation_failure, +) +from .contracts import ( + BINDING_REPORT_SCHEMA, + ROLE_BINDINGS_SCHEMA, + SCENE_MANIFEST_SCHEMA, + BindingReport, + RoleBindings, + SceneManifest, +) +from .scene_adapter import ( + CandidateSelection, + SceneAdaptation, + SceneAdapter, + SceneAdapterProtocolError, +) +from .scene_source import ( + SceneSourceFingerprint, + SceneSourceRef, + fingerprint_scene_source, + verify_scene_source_fingerprint, +) +from .legacy_scene import ( + LEGACY_SCENE_CONVERSION_SCHEMA, + LegacySceneRevision, + convert_legacy_gym_project, + restore_locked_scene_entities, +) + +__all__ = [ + "ArtifactTransaction", + "CONSERVATIVE_SCENE_GRAPH_FILENAME", + "BINDING_REPORT_SCHEMA", + "BindingReport", + "TaskEngineArtifactPaths", + "TaskEngineCoordinator", + "FEASIBILITY_REPORT_FILENAME", + "PREPARATION_FAILURE_FILENAME", + "PreparationResult", + "ROLE_BINDINGS_SCHEMA", + "RoleBindings", + "SCENE_MANIFEST_SCHEMA", + "STATIC_SCENE_MANIFEST_FILENAME", + "SceneAdaptation", + "CandidateSelection", + "SceneAdapter", + "SceneAdapterProtocolError", + "SceneManifest", + "SceneSourceFingerprint", + "SceneSourceRef", + "task_engine_artifact_paths", + "fingerprint_scene_source", + "LEGACY_SCENE_CONVERSION_SCHEMA", + "LegacySceneRevision", + "convert_legacy_gym_project", + "restore_locked_scene_entities", + "verify_scene_source_fingerprint", + "write_execution_report", + "write_preparation_failure", +] + + +def __getattr__(name: str) -> Any: + """Load coordinator entry points lazily to avoid graph-contract cycles.""" + if name in {"PreparationResult", "TaskEngineCoordinator"}: + from . import coordinator + + return getattr(coordinator, name) + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") diff --git a/embodichain/gen_sim/task_engine/orchestration/artifacts.py b/embodichain/gen_sim/task_engine/orchestration/artifacts.py new file mode 100644 index 000000000..0409fc5eb --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/artifacts.py @@ -0,0 +1,325 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Transactional publication for Task Engine artifacts.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +import json +import os +from pathlib import Path +import shutil +import tempfile +from typing import Any + +from embodichain.gen_sim.task_engine.reporting import ( + EXECUTION_REPORT_FILENAME, + write_execution_report as _write_execution_report, +) +from embodichain.utils.utility import load_config, save_config + +__all__ = [ + "BINDING_REPORT_FILENAME", + "CONSERVATIVE_SCENE_GRAPH_FILENAME", + "EXECUTION_REPORT_FILENAME", + "FEASIBILITY_REPORT_FILENAME", + "FINAL_SCENE_INSPECTION_FILENAME", + "PREPARATION_FAILURE_FILENAME", + "ROLE_BINDINGS_FILENAME", + "SCENE_MANIFEST_FILENAME", + "STATIC_SCENE_MANIFEST_FILENAME", + "SUCCESS_SPEC_FILENAME", + "TASK_CANDIDATE_SET_FILENAME", + "TASK_DRAFT_FILENAME", + "SCENE_REQUEST_FILENAME", + "ArtifactTransaction", + "TaskEngineArtifactPaths", + "task_engine_artifact_paths", + "write_task_engine_artifacts", + "write_execution_report", + "write_preparation_failure", +] + + +TASK_CANDIDATE_SET_FILENAME = "task_candidate_set.json" +TASK_DRAFT_FILENAME = "task_draft.json" +SCENE_REQUEST_FILENAME = "scene_request.json" +SUCCESS_SPEC_FILENAME = "success_spec.json" +SCENE_MANIFEST_FILENAME = "scene_manifest.json" +STATIC_SCENE_MANIFEST_FILENAME = "static_scene_manifest.json" +CONSERVATIVE_SCENE_GRAPH_FILENAME = "conservative_scene_graph.json" +ROLE_BINDINGS_FILENAME = "role_bindings.json" +BINDING_REPORT_FILENAME = "binding_report.json" +FEASIBILITY_REPORT_FILENAME = "feasibility_report.json" +FINAL_SCENE_INSPECTION_FILENAME = "final_scene_inspection.json" +PREPARATION_FAILURE_FILENAME = "preparation_failure.json" + + +@dataclass(frozen=True) +class TaskEngineArtifactPaths: + """Canonical Task Engine paths rooted at one published bundle.""" + + root: Path + task_candidate_set: Path + task_draft: Path + scene_request: Path + success_spec: Path + scene_manifest: Path + static_scene_manifest: Path + conservative_scene_graph: Path + role_bindings: Path + binding_report: Path + feasibility_report: Path + final_scene_inspection: Path + preparation_failure: Path + execution_report: Path + + +def task_engine_artifact_paths( + output_dir: str | Path, +) -> TaskEngineArtifactPaths: + """Return all Task Engine paths without creating the directory.""" + root = Path(output_dir).expanduser().resolve() + return TaskEngineArtifactPaths( + root=root, + task_candidate_set=root / TASK_CANDIDATE_SET_FILENAME, + task_draft=root / TASK_DRAFT_FILENAME, + scene_request=root / SCENE_REQUEST_FILENAME, + success_spec=root / SUCCESS_SPEC_FILENAME, + scene_manifest=root / SCENE_MANIFEST_FILENAME, + static_scene_manifest=root / STATIC_SCENE_MANIFEST_FILENAME, + conservative_scene_graph=root / CONSERVATIVE_SCENE_GRAPH_FILENAME, + role_bindings=root / ROLE_BINDINGS_FILENAME, + binding_report=root / BINDING_REPORT_FILENAME, + feasibility_report=root / FEASIBILITY_REPORT_FILENAME, + final_scene_inspection=root / FINAL_SCENE_INSPECTION_FILENAME, + preparation_failure=root / PREPARATION_FAILURE_FILENAME, + execution_report=root / EXECUTION_REPORT_FILENAME, + ) + + +class ArtifactTransaction: + """Build a complete bundle beside its destination and publish it by rename.""" + + def __init__(self, output_dir: str | Path, *, overwrite: bool = False) -> None: + raw = Path(output_dir).expanduser() + self.output_dir = ( + (Path.cwd() / raw).resolve() if not raw.is_absolute() else raw.resolve() + ) + self.overwrite = bool(overwrite) + self.staging_dir: Path | None = None + self._committed = False + + def __enter__(self) -> "ArtifactTransaction": + destination = self.output_dir + destination.parent.mkdir(parents=True, exist_ok=True) + if destination.exists() and not self.overwrite: + raise FileExistsError( + f"Output directory already exists: {destination}. " + "Pass overwrite=True to replace it." + ) + self.staging_dir = Path( + tempfile.mkdtemp( + prefix=f".{destination.name}.staging-", + dir=destination.parent, + ) + ) + return self + + def commit(self) -> Path: + """Rewrite staging-local absolute paths, then atomically publish.""" + if self.staging_dir is None: + raise RuntimeError("ArtifactTransaction has not been entered.") + if self._committed: + raise RuntimeError("ArtifactTransaction has already been committed.") + staging = self.staging_dir + destination = self.output_dir + _relocate_artifact_paths(staging, destination) + + backup: Path | None = None + if destination.exists(): + if not self.overwrite: + raise FileExistsError( + f"Output directory already exists: {destination}." + ) + backup = Path( + tempfile.mkdtemp( + prefix=f".{destination.name}.backup-", + dir=destination.parent, + ) + ) + backup.rmdir() + os.replace(destination, backup) + try: + os.replace(staging, destination) + except BaseException: + if backup is not None and backup.exists() and not destination.exists(): + os.replace(backup, destination) + raise + else: + self._committed = True + self.staging_dir = None + if backup is not None: + _remove_path(backup) + _fsync_directory(destination.parent) + return destination + + def __exit__(self, exc_type: Any, exc: Any, traceback: Any) -> bool: + if self.staging_dir is not None and self.staging_dir.exists(): + shutil.rmtree(self.staging_dir) + return False + + +def write_task_engine_artifacts( + output_dir: str | Path, + *, + candidate_set: Mapping[str, Any], + scene_manifest: Mapping[str, Any] | None, + role_bindings: Mapping[str, Any] | None, + binding_report: Mapping[str, Any], + static_scene_manifest: Mapping[str, Any] | None = None, + conservative_scene_graph: Mapping[str, Any] | None = None, + feasibility_report: Mapping[str, Any] | None = None, + final_scene_inspection: Mapping[str, Any] | None = None, +) -> TaskEngineArtifactPaths: + """Write Task Engine protocols into an unpublished staging directory. + + An unsuccessful adaptation can omit SceneManifest and RoleBindings rather + than publishing protocol filenames whose payloads do not satisfy their + schemas. + """ + paths = task_engine_artifact_paths(output_dir) + paths.root.mkdir(parents=True, exist_ok=True) + _write_json(paths.task_candidate_set, candidate_set) + if scene_manifest is not None: + _write_json(paths.scene_manifest, scene_manifest) + if static_scene_manifest is not None: + _write_json(paths.static_scene_manifest, static_scene_manifest) + if conservative_scene_graph is not None: + _write_json(paths.conservative_scene_graph, conservative_scene_graph) + if role_bindings is not None: + _write_json(paths.role_bindings, role_bindings) + _write_json(paths.binding_report, binding_report) + if feasibility_report is not None: + _write_json(paths.feasibility_report, feasibility_report) + if final_scene_inspection is not None: + _write_json(paths.final_scene_inspection, final_scene_inspection) + + return paths + + +def write_execution_report(output_dir: str | Path, value: Any) -> Path: + """Publish a canonical Task Program execution report.""" + return _write_execution_report(output_dir, value) + + +def write_preparation_failure(output_dir: str | Path, value: Any) -> Path: + """Write a strict-JSON audit for a failed candidate planning transaction.""" + path = task_engine_artifact_paths(output_dir).preparation_failure + path.parent.mkdir(parents=True, exist_ok=True) + _write_json(path, value) + return path + + +def _write_json(path: Path, value: Any) -> None: + try: + payload = ( + json.dumps( + value, + ensure_ascii=False, + indent=2, + sort_keys=False, + allow_nan=False, + ) + + "\n" + ).encode("utf-8") + except (TypeError, ValueError) as exc: + raise ValueError(f"Artifact {path.name} is not strict JSON data.") from exc + temporary: Path | None = None + try: + with tempfile.NamedTemporaryFile( + mode="wb", + dir=path.parent, + prefix=f".{path.name}.", + suffix=".tmp", + delete=False, + ) as stream: + temporary = Path(stream.name) + stream.write(payload) + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, path) + temporary = None + finally: + if temporary is not None: + temporary.unlink(missing_ok=True) + + +def _relocate_artifact_paths(staging: Path, destination: Path) -> None: + """Replace staging-root absolute paths embedded in JSON and YAML artifacts.""" + source_prefix = staging.resolve().as_posix() + destination_prefix = destination.resolve().as_posix() + + def relocate(value: Any) -> Any: + if isinstance(value, str): + if value == source_prefix: + return destination_prefix + if value.startswith(source_prefix + "/"): + return destination_prefix + value[len(source_prefix) :] + return value + if isinstance(value, list): + return [relocate(item) for item in value] + if isinstance(value, dict): + return { + relocate(key) if isinstance(key, str) else key: relocate(item) + for key, item in value.items() + } + return value + + for path in staging.rglob("*.json"): + try: + value = json.loads(path.read_text(encoding="utf-8")) + except json.JSONDecodeError as exc: + raise ValueError(f"Generated artifact is invalid JSON: {path}") from exc + relocated = relocate(value) + if relocated != value: + _write_json(path, relocated) + for suffix in ("*.yaml", "*.yml"): + for path in staging.rglob(suffix): + value = load_config(path) + relocated = relocate(value) + if relocated != value: + save_config(path, relocated) + + +def _remove_path(path: Path) -> None: + if path.is_dir() and not path.is_symlink(): + shutil.rmtree(path) + else: + path.unlink(missing_ok=True) + + +def _fsync_directory(path: Path) -> None: + try: + descriptor = os.open(path, os.O_RDONLY) + except OSError: + return + try: + os.fsync(descriptor) + finally: + os.close(descriptor) diff --git a/embodichain/gen_sim/task_engine/orchestration/contracts.py b/embodichain/gen_sim/task_engine/orchestration/contracts.py new file mode 100644 index 000000000..74d180fd3 --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/contracts.py @@ -0,0 +1,447 @@ +# ---------------------------------------------------------------------------- +# 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 cross-engine contracts for scene binding and orchestration.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +import json +import math +from typing import Any, TypeAlias + +from embodichain.gen_sim.task_engine import ( + SCENE_REQUEST_SCHEMA, + SUCCESS_SPEC_SCHEMA, + TASK_CANDIDATE_SET_SCHEMA, + TASK_DRAFT_SCHEMA, + SceneRequest, + SuccessSpec, + TaskCandidate, + TaskCandidateSet, + TaskDraft, + canonical_hash, + validate_scene_request, + validate_success_spec, + validate_task_candidate, + validate_task_candidate_set, + validate_task_draft, +) + +__all__ = [ + "BINDING_REPORT_SCHEMA", + "ROLE_BINDINGS_SCHEMA", + "SCENE_MANIFEST_SCHEMA", + "SCENE_REQUEST_SCHEMA", + "SUCCESS_SPEC_SCHEMA", + "TASK_CANDIDATE_SET_SCHEMA", + "TASK_DRAFT_SCHEMA", + "BindingReport", + "RoleBindings", + "SceneManifest", + "SceneRequest", + "SuccessSpec", + "TaskCandidate", + "TaskCandidateSet", + "TaskDraft", + "canonical_hash", + "validate_binding_report", + "validate_role_bindings", + "validate_scene_manifest", + "validate_scene_request", + "validate_success_spec", + "validate_task_candidate", + "validate_task_candidate_set", + "validate_task_draft", +] + +SCENE_MANIFEST_SCHEMA = "action_engine_scene_manifest_v1" +ROLE_BINDINGS_SCHEMA = "action_engine_role_bindings_v1" +BINDING_REPORT_SCHEMA = "action_engine_binding_report_v1" +SceneManifest: TypeAlias = dict[str, Any] +RoleBindings: TypeAlias = dict[str, Any] +BindingReport: TypeAlias = dict[str, Any] + + +def validate_scene_manifest(value: Mapping[str, Any]) -> SceneManifest: + result = _mapping(value, "SceneManifest") + _keys( + result, + {"schema_version", "scene_id", "source_format", "robot_profile", "objects"}, + "SceneManifest", + ) + _schema(result, SCENE_MANIFEST_SCHEMA, "SceneManifest") + for key in ("scene_id", "source_format", "robot_profile"): + result[key] = _nonempty(result.get(key), f"SceneManifest.{key}") + object_keys = { + "uid", + "role", + "name", + "description", + "category", + "color", + "affordances", + "initial_state", + "attributes", + } + objects = [] + for index, raw in enumerate( + _sequence(result.get("objects"), "SceneManifest.objects") + ): + context = f"SceneManifest.objects[{index}]" + item = _mapping(raw, context) + _keys(item, object_keys, context) + item["uid"] = _nonempty(item.get("uid"), f"{context}.uid") + for key in ("role", "name", "description", "category"): + item[key] = _string(item.get(key), f"{context}.{key}") + if item.get("color") is not None: + item["color"] = _string(item.get("color"), f"{context}.color") + item["affordances"] = _strings( + item.get("affordances"), f"{context}.affordances" + ) + item["initial_state"] = _mapping( + item.get("initial_state"), f"{context}.initial_state" + ) + item["attributes"] = _mapping(item.get("attributes"), f"{context}.attributes") + objects.append(item) + _unique([item["uid"] for item in objects], "SceneManifest object UIDs") + result["objects"] = objects + _json_safe(result, "SceneManifest") + return result + + +def validate_role_bindings(value: Mapping[str, Any]) -> RoleBindings: + result = _mapping(value, "RoleBindings") + _keys( + result, + { + "schema_version", + "task_id", + "candidate_id", + "reference_bindings", + "role_bindings", + }, + "RoleBindings", + ) + _schema(result, ROLE_BINDINGS_SCHEMA, "RoleBindings") + for key in ("task_id", "candidate_id"): + result[key] = _nonempty(result.get(key), f"RoleBindings.{key}") + result["reference_bindings"] = _string_lists( + result.get("reference_bindings"), "RoleBindings.reference_bindings" + ) + if any(not uids for uids in result["reference_bindings"].values()): + raise ValueError("RoleBindings.reference_bindings values must not be empty.") + result["role_bindings"] = _string_map( + result.get("role_bindings"), "RoleBindings.role_bindings" + ) + return result + + +def validate_binding_report(value: Mapping[str, Any]) -> BindingReport: + result = _mapping(value, "BindingReport") + _keys( + result, + { + "schema_version", + "task_id", + "status", + "selected_candidate_id", + "selection_reason", + "candidates", + }, + "BindingReport", + ) + _schema(result, BINDING_REPORT_SCHEMA, "BindingReport") + result["task_id"] = _nonempty(result.get("task_id"), "BindingReport.task_id") + result["status"] = _enum( + result.get("status"), + {"bound", "ambiguous", "unsatisfied"}, + "BindingReport.status", + ) + result["selected_candidate_id"] = _string( + result.get("selected_candidate_id"), "BindingReport.selected_candidate_id" + ) + result["selection_reason"] = _string( + result.get("selection_reason"), "BindingReport.selection_reason" + ) + if result["status"] == "bound" and not result["selected_candidate_id"]: + raise ValueError("A bound BindingReport requires selected_candidate_id.") + candidate_keys = { + "candidate_id", + "semantic_hash", + "status", + "references", + "reasons", + } + reference_keys = { + "reference_id", + "status", + "confidence", + "candidate_uids", + "selected_uids", + "reasons", + } + candidates = [] + for index, raw in enumerate( + _sequence(result.get("candidates"), "BindingReport.candidates") + ): + context = f"BindingReport.candidates[{index}]" + candidate = _mapping(raw, context) + _keys(candidate, candidate_keys, context) + candidate["candidate_id"] = _nonempty( + candidate.get("candidate_id"), f"{context}.candidate_id" + ) + candidate["semantic_hash"] = _digest( + candidate.get("semantic_hash"), f"{context}.semantic_hash" + ) + candidate["status"] = _enum( + candidate.get("status"), + {"resolved", "ambiguous", "not_found", "incompatible"}, + f"{context}.status", + ) + references = [] + for ref_index, ref_raw in enumerate( + _sequence(candidate.get("references"), f"{context}.references") + ): + ref_context = f"{context}.references[{ref_index}]" + reference = _mapping(ref_raw, ref_context) + _keys(reference, reference_keys, ref_context) + reference["reference_id"] = _nonempty( + reference.get("reference_id"), f"{ref_context}.reference_id" + ) + reference["status"] = _enum( + reference.get("status"), + {"resolved", "ambiguous", "not_found", "incompatible"}, + f"{ref_context}.status", + ) + reference["confidence"] = _number( + reference.get("confidence"), + f"{ref_context}.confidence", + minimum=0.0, + maximum=1.0, + ) + reference["candidate_uids"] = _strings( + reference.get("candidate_uids"), + f"{ref_context}.candidate_uids", + allow_empty=True, + ) + reference["selected_uids"] = _strings( + reference.get("selected_uids"), + f"{ref_context}.selected_uids", + allow_empty=True, + ) + reference["reasons"] = _strings( + reference.get("reasons"), f"{ref_context}.reasons", allow_empty=True + ) + selected = set(reference["selected_uids"]) + candidates_for_reference = set(reference["candidate_uids"]) + if not selected <= candidates_for_reference: + raise ValueError( + f"{ref_context}.selected_uids must be a subset of candidate_uids." + ) + if reference["status"] == "resolved" and not selected: + raise ValueError( + f"{ref_context} status=resolved requires selected_uids." + ) + if reference["status"] != "resolved" and selected: + raise ValueError( + f"{ref_context} non-resolved status cannot select UIDs." + ) + if reference["status"] == "not_found" and candidates_for_reference: + raise ValueError( + f"{ref_context} status=not_found cannot carry candidate_uids." + ) + references.append(reference) + if not references: + raise ValueError(f"{context}.references must not be empty.") + _unique( + [item["reference_id"] for item in references], + f"{context} reference IDs", + ) + expected_status = _candidate_binding_status(references) + if candidate["status"] != expected_status: + raise ValueError( + f"{context}.status must be {expected_status!r} for its references." + ) + candidate["references"] = references + candidate["reasons"] = _strings( + candidate.get("reasons"), f"{context}.reasons", allow_empty=True + ) + candidates.append(candidate) + if not candidates: + raise ValueError("BindingReport.candidates must not be empty.") + _unique( + [item["candidate_id"] for item in candidates], "BindingReport candidate IDs" + ) + if result["selected_candidate_id"] and result["selected_candidate_id"] not in { + item["candidate_id"] for item in candidates + }: + raise ValueError("BindingReport.selected_candidate_id is unknown.") + if result["status"] != "bound" and result["selected_candidate_id"]: + raise ValueError( + "A non-bound BindingReport cannot carry selected_candidate_id." + ) + selected = next( + ( + candidate + for candidate in candidates + if candidate["candidate_id"] == result["selected_candidate_id"] + ), + None, + ) + if result["status"] == "bound" and ( + selected is None or selected["status"] != "resolved" + ): + raise ValueError( + "A bound BindingReport must select a resolved candidate audit." + ) + if result["status"] == "unsatisfied" and any( + candidate["status"] in {"resolved", "ambiguous"} for candidate in candidates + ): + raise ValueError( + "An unsatisfied BindingReport cannot contain resolved or ambiguous candidates." + ) + result["candidates"] = candidates + return result + + +def _mapping(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 _candidate_binding_status(references: Sequence[Mapping[str, Any]]) -> str: + statuses = {str(reference["status"]) for reference in references} + if statuses == {"resolved"}: + return "resolved" + if "ambiguous" in statuses: + return "ambiguous" + if "incompatible" in statuses: + return "incompatible" + return "not_found" + + +def _sequence(value: Any, context: str) -> list[Any]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)): + raise ValueError(f"{context} must be a list.") + return list(value) + + +def _keys( + value: Mapping[str, Any], expected: set[str] | frozenset[str], context: str +) -> None: + if set(value) != set(expected): + raise ValueError( + f"{context} requires exactly fields {sorted(expected)}; received {sorted(value)}." + ) + + +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}.") + + +def _string(value: Any, context: str) -> str: + if not isinstance(value, str): + raise ValueError(f"{context} must be a string.") + return value.strip() + + +def _nonempty(value: Any, context: str) -> str: + result = _string(value, context) + if not result: + raise ValueError(f"{context} must not be empty.") + return result + + +def _enum(value: Any, choices: set[str], context: str) -> str: + result = _string(value, context) + if result not in choices: + raise ValueError(f"{context} must be one of {sorted(choices)}.") + return result + + +def _integer( + value: Any, context: str, *, minimum: int, maximum: int | None = None +) -> int: + if ( + not isinstance(value, int) + or isinstance(value, bool) + or value < minimum + or (maximum is not None and value > maximum) + ): + raise ValueError(f"{context} must be an integer in the allowed range.") + return value + + +def _number(value: Any, context: str, *, minimum: float, maximum: float) -> float: + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(value) + or not minimum <= float(value) <= maximum + ): + raise ValueError( + f"{context} must be a finite number between {minimum} and {maximum}." + ) + return float(value) + + +def _strings(value: Any, context: str, *, allow_empty: bool = False) -> list[str]: + result = [_string(item, context) for item in _sequence(value, context)] + if not allow_empty and any(not item for item in result): + raise ValueError(f"{context} values must not be empty.") + if len(result) != len(set(result)): + raise ValueError(f"{context} values must be unique.") + return result + + +def _string_map(value: Any, context: str) -> dict[str, str]: + result = _mapping(value, context) + return { + _nonempty(key, context): _nonempty(item, context) + for key, item in result.items() + } + + +def _string_lists(value: Any, context: str) -> dict[str, list[str]]: + result = _mapping(value, context) + return { + _nonempty(key, context): _strings(item, context) for key, item in result.items() + } + + +def _digest(value: Any, context: str) -> str: + result = _string(value, context) + if len(result) != 64 or any( + character not in "0123456789abcdef" for character in result + ): + raise ValueError(f"{context} must be a lowercase SHA-256 digest.") + return result + + +def _unique(values: Sequence[str], context: str) -> None: + if len(values) != len(set(values)): + raise ValueError(f"{context} must be unique.") + + +def _json_safe(value: Any, context: str) -> None: + try: + json.dumps(value, allow_nan=False) + except (TypeError, ValueError) as error: + raise ValueError(f"{context} must be finite and JSON serializable.") from error diff --git a/embodichain/gen_sim/task_engine/orchestration/coordinator.py b/embodichain/gen_sim/task_engine/orchestration/coordinator.py new file mode 100644 index 000000000..b0868cb43 --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/coordinator.py @@ -0,0 +1,333 @@ +# ---------------------------------------------------------------------------- +# 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-owned preparation ending at the canonical Task Program boundary.""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass +import json +from pathlib import Path +from typing import Any + +from embodichain.gen_sim.task_engine import ( + TaskAgent, + TaskCandidateSet, + validate_scene_output_separation, + validate_task_candidate_set, +) +from embodichain.gen_sim.task_engine.semantic_graph import ( + SemanticTaskGraph, + semantic_task_graph_hash, +) +from embodichain.gen_sim.task_engine.semantic_planner import ( + SemanticTaskPlanner, + UnsupportedSemanticCapabilityError, +) +from embodichain.gen_sim.task_engine.task_program_bundle import ( + TaskProgramBundlePaths, + generate_task_program_bundle, +) + +from .artifacts import ( + ArtifactTransaction, + TaskEngineArtifactPaths, + task_engine_artifact_paths, + write_preparation_failure, + write_task_engine_artifacts, +) +from .scene_adapter import SceneAdaptation, SceneAdapter +from .scene_source import SceneSourceRef + +__all__ = ["PreparationResult", "TaskEngineCoordinator"] + +_PREPARATION_FAILURE_SCHEMA = "semantic_task_preparation_failure/v1" + + +@dataclass(frozen=True, slots=True) +class PreparationResult: + """Published result of one Task -> Scene -> Semantic Skill preparation.""" + + status: str + output_dir: Path + candidate_set: TaskCandidateSet + adaptation: SceneAdaptation + artifacts: TaskEngineArtifactPaths + semantic_task_graph: SemanticTaskGraph | None = None + generated_paths: TaskProgramBundlePaths | None = None + feasibility_report: dict[str, Any] | None = None + planning_attempts: tuple[dict[str, Any], ...] = () + unbound_action_plan: dict[str, Any] | None = None + + @property + def bound(self) -> bool: + """Return whether a fingerprint-bound Task Program bundle was published.""" + return self.status == "bound" + + @property + def selected_candidate_id(self) -> str | None: + """Return the selected candidate identity, if one was bound.""" + return self.adaptation.selected_candidate_id + + +class TaskEngineCoordinator: + """Interpret and bind tasks without owning physical action construction.""" + + def __init__( + self, + *, + task_agent: TaskAgent | None = None, + scene_adapter: SceneAdapter | None = None, + semantic_planner: SemanticTaskPlanner | None = None, + ) -> None: + self.task_agent = task_agent or TaskAgent() + self.scene_adapter = scene_adapter or SceneAdapter() + self.semantic_planner = semantic_planner or SemanticTaskPlanner() + + def prepare( + self, + task_id: str, + instruction: str, + source: SceneSourceRef | str | Path, + output_dir: str | Path, + *, + model: str | None = None, + candidate_count: int = 3, + overwrite: bool = False, + planning_mode: str = "offline", + gripper_model: str = "pgi", + ik_solver: str = "auto", + vlm_model: str | None = None, + max_episodes: int | None = None, + max_episode_steps: int | None = None, + planner_policy: Mapping[str, Any] | None = None, + randomize_scene: bool = False, + randomize_table_material: bool = False, + candidate_set: TaskCandidateSet | Mapping[str, Any] | None = None, + force_most_likely: bool = False, + final_inspection: Mapping[str, Any] | None = None, + unbound_action_plan: Mapping[str, Any] | None = None, + ) -> PreparationResult: + """Publish a graph and configured Task Program as one transaction. + + Robot part routing, action options, grounding, physical effects, and + command execution remain owned by the selected Task Program integration. + Legacy CLI keywords are accepted for one migration window but cannot + alter those lower-layer contracts. + """ + del ( + gripper_model, + ik_solver, + vlm_model, + planner_policy, + randomize_scene, + randomize_table_material, + ) + normalized_source = self._coerce_source(source) + validate_scene_output_separation(normalized_source.path, output_dir) + with ArtifactTransaction(output_dir, overwrite=overwrite) as transaction: + staging = transaction.staging_dir + assert staging is not None + if candidate_set is None: + normalized_candidates = self.task_agent.generate( + task_id, + instruction, + model=model, + candidate_count=candidate_count, + ) + else: + normalized_candidates = validate_task_candidate_set(candidate_set) + if normalized_candidates["task_id"] != str(task_id).strip(): + raise ValueError("TaskCandidateSet.task_id must match task_id.") + if normalized_candidates["instruction"] != str(instruction).strip(): + raise ValueError( + "TaskCandidateSet.instruction must match instruction." + ) + + adaptation_kwargs: dict[str, Any] = {"force_most_likely": force_most_likely} + if final_inspection is not None: + adaptation_kwargs["final_inspection"] = final_inspection + adaptation = self.scene_adapter.adapt( + normalized_candidates, + normalized_source, + **adaptation_kwargs, + ) + status = str(adaptation.binding_report["status"]) + self._write_audit_artifacts( + staging, + candidate_set=normalized_candidates, + adaptation=adaptation, + final_inspection=final_inspection, + ) + if status != "bound": + published = transaction.commit() + return PreparationResult( + status=status, + output_dir=published, + candidate_set=deepcopy(normalized_candidates), + adaptation=adaptation, + artifacts=task_engine_artifact_paths(published), + ) + + selected = adaptation.selected_candidate + role_bindings = adaptation.role_bindings + if selected is None or role_bindings is None: + raise ValueError( + "A bound SceneAdaptation must include a selected candidate and " + "RoleBindings." + ) + planning_attempt = { + "candidate_id": str(selected["candidate_id"]), + "planner_route": str(planning_mode), + "status": "running", + } + try: + graph = self.semantic_planner.plan( + selected, + role_bindings, + adaptation.prepared_scene.planner_objects, + planner_route=planning_mode, + ) + graph, generated = generate_task_program_bundle( + graph, + adaptation.prepared_scene, + staging, + robot_profile=str(adaptation.scene_manifest["robot_profile"]), + max_episodes=max_episodes, + max_episode_steps=max_episode_steps, + ) + except (TypeError, ValueError, UnsupportedSemanticCapabilityError) as exc: + planning_attempt["status"] = "failed" + planning_attempt["error"] = _error_record(exc) + write_preparation_failure( + staging, + { + "schema_version": _PREPARATION_FAILURE_SCHEMA, + "task_id": str(normalized_candidates["task_id"]), + "status": "unsupported_semantic_capability", + "selected_candidate_id": str(selected["candidate_id"]), + "attempts": [deepcopy(planning_attempt)], + }, + ) + published = transaction.commit() + return PreparationResult( + status="planning_failed", + output_dir=published, + candidate_set=deepcopy(normalized_candidates), + adaptation=adaptation, + artifacts=task_engine_artifact_paths(published), + planning_attempts=(deepcopy(planning_attempt),), + ) + + planning_attempt.update( + { + "status": "preflight_succeeded", + "semantic_task_graph_hash": semantic_task_graph_hash(graph), + "semantic_call_count": len(graph["nodes"]), + "integration_fingerprint": graph["integration_fingerprint"], + } + ) + _write_json(staging / "planner_report.json", planning_attempt) + self._write_selected_candidate_artifacts(staging, selected) + published = transaction.commit() + return PreparationResult( + status="bound", + output_dir=published, + candidate_set=deepcopy(normalized_candidates), + adaptation=adaptation, + artifacts=task_engine_artifact_paths(published), + semantic_task_graph=deepcopy(graph), + generated_paths=_published_paths(generated, published), + planning_attempts=(deepcopy(planning_attempt),), + unbound_action_plan=( + None + if unbound_action_plan is None + else deepcopy(dict(unbound_action_plan)) + ), + ) + + def _coerce_source(self, source: SceneSourceRef | str | Path) -> SceneSourceRef: + if isinstance(source, SceneSourceRef): + return source + return SceneSourceRef( + source, + robot_profile=self.scene_adapter.robot_profile, + ) + + @staticmethod + def _write_audit_artifacts( + output_dir: Path, + *, + candidate_set: TaskCandidateSet, + adaptation: SceneAdaptation, + final_inspection: Mapping[str, Any] | None, + ) -> None: + write_task_engine_artifacts( + output_dir, + candidate_set=candidate_set, + scene_manifest=( + adaptation.scene_manifest + if adaptation.binding_report["status"] == "bound" + else None + ), + role_bindings=adaptation.role_bindings, + binding_report=adaptation.binding_report, + static_scene_manifest=adaptation.static_scene_manifest, + conservative_scene_graph=adaptation.conservative_scene_graph, + final_scene_inspection=final_inspection, + ) + + @staticmethod + def _write_selected_candidate_artifacts( + output_dir: Path, + selected: Mapping[str, Any], + ) -> None: + _write_json(output_dir / "task_draft.json", selected["draft"]) + _write_json(output_dir / "scene_request.json", selected["scene_request"]) + _write_json(output_dir / "success_spec.json", selected["success_spec"]) + + +def _published_paths( + paths: TaskProgramBundlePaths, + published: Path, +) -> TaskProgramBundlePaths: + def target(path: Path) -> Path: + return published / path.relative_to(paths.root) + + return TaskProgramBundlePaths( + root=published, + deployment=target(paths.deployment), + program=target(paths.program), + integration=target(paths.integration), + scene=target(paths.scene), + embodiment=target(paths.embodiment), + execution_policy=target(paths.execution_policy), + semantic_task_graph=target(paths.semantic_task_graph), + integration_fingerprint=target(paths.integration_fingerprint), + ) + + +def _write_json(path: Path, value: Any) -> None: + path.write_text( + json.dumps(value, ensure_ascii=False, indent=2, allow_nan=False) + "\n", + encoding="utf-8", + ) + + +def _error_record(error: Exception) -> dict[str, str]: + return {"type": type(error).__name__, "message": str(error)} diff --git a/embodichain/gen_sim/task_engine/orchestration/grounding.py b/embodichain/gen_sim/task_engine/orchestration/grounding.py new file mode 100644 index 000000000..b0a1ad7ad --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/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 .scene_inventory 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": "TaskEngineSceneGrounding", + "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/task_engine/orchestration/legacy_scene.py b/embodichain/gen_sim/task_engine/orchestration/legacy_scene.py new file mode 100644 index 000000000..4f278f323 --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/legacy_scene.py @@ -0,0 +1,378 @@ +# ---------------------------------------------------------------------------- +# 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-only conversion of legacy Gym projects into editable scene revisions.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +from dataclasses import dataclass +import json +from pathlib import Path +import shutil +from typing import Any, Final + +import numpy as np +from scipy.spatial.transform import Rotation +import trimesh + +from .source_scene import prepare_scene, resolve_source_scene + +from .scene_source import ( + SceneSourceFingerprint, + fingerprint_scene_source, + verify_scene_source_fingerprint, +) + +__all__ = [ + "LEGACY_SCENE_CONVERSION_SCHEMA", + "LegacySceneRevision", + "convert_legacy_gym_project", + "restore_locked_scene_entities", +] + +LEGACY_SCENE_CONVERSION_SCHEMA: Final = "embodichain.legacy-scene-conversion/v1" +_CONVERSION_MANIFEST = "legacy_conversion.json" + + +@dataclass(frozen=True) +class LegacySceneRevision: + """A new editable revision derived without modifying its legacy source.""" + + output_root: Path + scene_config_path: Path + scene_graph_path: Path + manifest_path: Path + source_fingerprint: SceneSourceFingerprint + locked_entity_uids: tuple[str, ...] + + +def convert_legacy_gym_project( + source: str | Path, + output_root: str | Path, +) -> LegacySceneRevision: + """Convert a supported legacy Gym project into a Scene Engine revision. + + Args: + source: Legacy Gym project directory or explicit configuration path. + output_root: Empty destination owned by the new scene revision. + + Returns: + Paths and provenance for the converted revision. + + Raises: + ValueError: If the source is not legacy or the destination already exists. + FileNotFoundError: If a referenced source asset is missing. + """ + resolved = resolve_source_scene(source) + if resolved.source_format != "legacy_gym_config": + raise ValueError("Legacy conversion requires a legacy Gym configuration.") + destination = Path(output_root).expanduser().resolve() + if destination.exists(): + if not destination.is_dir() or any(destination.iterdir()): + raise ValueError("Legacy scene revision output_root must be empty.") + source_fingerprint = fingerprint_scene_source(source) + prepared = prepare_scene(source) + export_root = destination / "scene_export" + assets_root = export_root / "mesh_assets" + assets_root.mkdir(parents=True, exist_ok=True) + semantics = {str(item.get("uid")): item for item in prepared.planner_objects} + + background = [ + _editable_entry(item, semantics=semantics, assets_root=assets_root) + for item in prepared.background + ] + rigid_objects = [ + _editable_entry(item, semantics=semantics, assets_root=assets_root) + for item in prepared.rigid_objects + ] + articulations = [ + _locked_articulation( + item, + source_root=resolved.path.parent, + destination_root=export_root / "locked_assets", + ) + for item in prepared.articulations + ] + table = next((item for item in background if item.get("uid") == "table"), None) + if table is None: + raise ValueError("Legacy conversion requires one table support object.") + _measure_support_metadata(table, export_root=export_root) + for item in rigid_objects: + _measure_center(item, export_root=export_root) + + scene_config = { + "format": "embodichain.scene-export/v1", + "scene_id": f"legacy-revision-{source_fingerprint.config_sha256[:16]}", + "background": background, + "rigid_object": rigid_objects, + "articulation": articulations, + } + scene_config_path = export_root / "scene_config.json" + _write_json(scene_config_path, scene_config) + scene_graph = { + "nodes": [ + { + "object_id": "table", + "parent_id": None, + "parent_relation": None, + "table_region": None, + "orientation_state": None, + }, + *[ + { + "object_id": str(item["uid"]), + "parent_id": "table", + "parent_relation": "on", + "table_region": None, + "orientation_state": None, + } + for item in rigid_objects + ], + ], + "relations": [], + } + scene_graph_path = export_root / "scene_graph.json" + _write_json(scene_graph_path, scene_graph) + locked_uids = tuple( + sorted(str(item["uid"]) for item in [*background, *articulations]) + ) + manifest = { + "schema_version": LEGACY_SCENE_CONVERSION_SCHEMA, + "source": source_fingerprint.to_dict(), + "scene_config": scene_config_path.as_posix(), + "audit_hierarchy": "unknown", + "operational_hierarchy": "assumed_on_table", + "assumptions": [ + { + "uid": str(item["uid"]), + "relation": "on", + "parent_uid": "table", + "confidence": None, + "source": "operational_assumption", + } + for item in rigid_objects + ], + "locked_entity_uids": list(locked_uids), + "locked_articulations": deepcopy(articulations), + "locked_background": deepcopy( + [item for item in background if item.get("uid") != "table"] + ), + } + manifest_path = destination / _CONVERSION_MANIFEST + _write_json(manifest_path, manifest) + verify_scene_source_fingerprint(source_fingerprint.to_dict()) + return LegacySceneRevision( + output_root=destination, + scene_config_path=scene_config_path, + scene_graph_path=scene_graph_path, + manifest_path=manifest_path, + source_fingerprint=source_fingerprint, + locked_entity_uids=locked_uids, + ) + + +def restore_locked_scene_entities(revision_root: str | Path) -> Path: + """Restore collision-only legacy entities after Scene Engine export. + + Args: + revision_root: Converted revision root containing ``legacy_conversion.json``. + + Returns: + Updated scene configuration path. + + Raises: + FileNotFoundError: If the conversion manifest or scene config is absent. + ValueError: If a generated scene attempts to reuse a locked UID. + """ + root = Path(revision_root).expanduser().resolve() + manifest_path = root / _CONVERSION_MANIFEST + if not manifest_path.is_file(): + raise FileNotFoundError( + f"Legacy conversion manifest not found: {manifest_path}" + ) + manifest = _read_mapping(manifest_path) + if manifest.get("schema_version") != LEGACY_SCENE_CONVERSION_SCHEMA: + raise ValueError("Legacy conversion manifest schema is invalid.") + config_path = root / "scene_export" / "scene_config.json" + config = _read_mapping(config_path) + existing = { + str(item.get("uid")) + for section in ("background", "rigid_object", "articulation") + for item in config.get(section, ()) + if isinstance(item, Mapping) and item.get("uid") + } + for section, key in ( + ("background", "locked_background"), + ("articulation", "locked_articulations"), + ): + values = manifest.get(key, ()) + if not isinstance(values, Sequence) or isinstance(values, (str, bytes)): + raise TypeError(f"Legacy conversion manifest {key} must be a sequence.") + target = list(config.get(section, ())) + for raw in values: + item = deepcopy(dict(raw)) + uid = str(item.get("uid", "")) + if uid in existing: + raise ValueError(f"Generated scene reused locked entity UID {uid!r}.") + existing.add(uid) + target.append(item) + config[section] = target + _write_json(config_path, config) + verify_scene_source_fingerprint(manifest["source"]) + return config_path + + +def _editable_entry( + value: Mapping[str, Any], + *, + semantics: Mapping[str, Mapping[str, Any]], + assets_root: Path, +) -> dict[str, Any]: + item = deepcopy(dict(value)) + uid = str(item.get("uid", "")).strip() + if not uid: + raise ValueError("Converted scene entities require a UID.") + semantic = semantics.get(uid, {}) + for key in ("category", "name", "description"): + item[key] = str(semantic.get(key) or item.get(key) or uid) + shape = item.get("shape") + if not isinstance(shape, Mapping): + raise ValueError(f"Legacy scene entity {uid!r} has no supported shape.") + destination = assets_root / uid / f"{uid}.glb" + destination.parent.mkdir(parents=True, exist_ok=True) + _shape_to_glb(shape, destination) + item["shape"] = { + "shape_type": "Mesh", + "fpath": destination.relative_to(assets_root.parent).as_posix(), + "compute_uv": False, + } + item.setdefault("body_scale", [1.0, 1.0, 1.0]) + item.setdefault("init_pos", [0.0, 0.0, 0.0]) + item.setdefault("init_rot", [0.0, 0.0, 0.0]) + item.setdefault("attrs", {"mass": 1.0}) + item.setdefault("body_type", "kinematic" if uid == "table" else "dynamic") + item.setdefault("max_convex_hull_num", 1 if uid == "table" else 16) + return item + + +def _shape_to_glb(shape: Mapping[str, Any], destination: Path) -> None: + shape_type = str(shape.get("shape_type", "")) + if shape_type == "Mesh": + source = Path(str(shape.get("fpath", ""))).expanduser().resolve() + if not source.is_file(): + raise FileNotFoundError(f"Legacy mesh asset not found: {source}") + mesh = trimesh.load(source, force="scene") + elif shape_type == "Cube": + size = _vector(shape.get("size", [1.0, 1.0, 1.0]), length=3) + mesh = trimesh.Scene(trimesh.creation.box(extents=size)) + elif shape_type == "Sphere": + radius = float(shape.get("radius", 1.0)) + if not np.isfinite(radius) or radius <= 0.0: + raise ValueError("Legacy sphere radius must be positive and finite.") + mesh = trimesh.Scene(trimesh.creation.icosphere(radius=radius)) + else: + raise ValueError(f"Unsupported legacy shape_type {shape_type!r}.") + mesh.export(destination, file_type="glb") + + +def _locked_articulation( + value: Mapping[str, Any], + *, + source_root: Path, + destination_root: Path, +) -> dict[str, Any]: + item = deepcopy(dict(value)) + uid = str(item.get("uid", "")).strip() + raw = Path(str(item.get("fpath", ""))).expanduser() + source = raw.resolve() if raw.is_absolute() else (source_root / raw).resolve() + if not source.is_file(): + raise FileNotFoundError(f"Legacy articulation asset not found: {source}") + target_root = destination_root / uid + shutil.copytree(source.parent, target_root, dirs_exist_ok=True) + copied = target_root / source.name + item["fpath"] = copied.resolve().as_posix() + return item + + +def _measure_support_metadata(entry: dict[str, Any], *, export_root: Path) -> None: + bounds = _world_bounds(entry, export_root=export_root) + entry["support_surface_z"] = float(bounds[1, 2]) + rectangle = [ + [float(bounds[0, 0]), float(bounds[0, 1])], + [float(bounds[1, 0]), float(bounds[0, 1])], + [float(bounds[1, 0]), float(bounds[1, 1])], + [float(bounds[0, 0]), float(bounds[1, 1])], + ] + entry["support_contour_xy"] = rectangle + entry["support_optimization_rect_xy"] = deepcopy(rectangle) + entry["center_xy"] = [ + float((bounds[0, 0] + bounds[1, 0]) / 2.0), + float((bounds[0, 1] + bounds[1, 1]) / 2.0), + ] + + +def _measure_center(entry: dict[str, Any], *, export_root: Path) -> None: + bounds = _world_bounds(entry, export_root=export_root) + entry["center_xy"] = [ + float((bounds[0, 0] + bounds[1, 0]) / 2.0), + float((bounds[0, 1] + bounds[1, 1]) / 2.0), + ] + + +def _world_bounds(entry: Mapping[str, Any], *, export_root: Path) -> np.ndarray: + shape = dict(entry["shape"]) + mesh_path = (export_root / str(shape["fpath"])).resolve() + loaded = trimesh.load(mesh_path, force="scene") + mesh = loaded.to_geometry() + scale = np.asarray(_vector(entry.get("body_scale", [1.0] * 3), length=3)) + mesh.apply_scale(scale) + transform = np.eye(4) + transform[:3, :3] = Rotation.from_euler( + "XYZ", + _vector(entry.get("init_rot", [0.0] * 3), length=3), + degrees=True, + ).as_matrix() + transform[:3, 3] = _vector(entry.get("init_pos", [0.0] * 3), length=3) + mesh.apply_transform(transform) + return np.asarray(mesh.bounds, dtype=float) + + +def _vector(value: Any, *, length: int) -> list[float]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)): + raise TypeError("Legacy scene vector must be a sequence.") + result = [float(item) for item in value] + if len(result) != length or not np.all(np.isfinite(result)): + raise ValueError(f"Legacy scene vector must contain {length} finite values.") + return result + + +def _read_mapping(path: Path) -> dict[str, Any]: + if not path.is_file(): + raise FileNotFoundError(path) + value = json.loads(path.read_text(encoding="utf-8")) + if not isinstance(value, Mapping): + raise TypeError(f"JSON document must contain an object: {path}") + return dict(value) + + +def _write_json(path: Path, value: Any) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text( + json.dumps(value, ensure_ascii=False, indent=2, allow_nan=False) + "\n", + encoding="utf-8", + ) diff --git a/embodichain/gen_sim/task_engine/orchestration/scene_adapter.py b/embodichain/gen_sim/task_engine/orchestration/scene_adapter.py new file mode 100644 index 000000000..2104ee6d8 --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/scene_adapter.py @@ -0,0 +1,1050 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Bind Task Agent candidates to a redacted, authoritative scene inventory.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from copy import deepcopy +from dataclasses import dataclass, field +import hashlib +import json +from pathlib import Path +from typing import Any + +from embodichain.gen_sim.task_engine.orchestration.grounding import ( + GroundingCaller, + ground_scene_references, +) +from embodichain.gen_sim.task_engine.orchestration.scene_inventory import ( + SceneInventory, + validate_source_compatibility, + validate_target_compatibility, +) +from embodichain.gen_sim.task_engine.orchestration.source_scene import ( + PreparedScene, + prepare_scene, + resolve_source_scene, +) +from embodichain.gen_sim.task_engine import TaskCandidate, TaskCandidateSet +from embodichain.gen_sim.task_engine.interpretation import ( + _default_instruction_caller, +) +from embodichain.gen_sim.task_engine.scene import ( + ConservativeSceneGraph, + SceneEngineV1Adapter, + StaticSceneManifest, + build_conservative_scene_graph, + validate_static_scene_manifest, +) +from embodichain.gen_sim.task_engine.scene.final_inspection import ( + apply_final_inspection, + validate_final_scene_inspection, +) + +from .contracts import ( + BINDING_REPORT_SCHEMA, + ROLE_BINDINGS_SCHEMA, + SCENE_MANIFEST_SCHEMA, + BindingReport, + RoleBindings, + SceneManifest, + validate_binding_report, + validate_role_bindings, + validate_scene_manifest, + validate_task_candidate, + validate_task_candidate_set, +) +from .scene_source import ( + SceneSourceRef, + fingerprint_scene_source, + scene_revision_id, +) + +__all__ = [ + "Adjudicator", + "CandidateSelection", + "SceneAdaptation", + "SceneAdapter", + "SceneAdapterProtocolError", +] + + +Adjudicator = Callable[..., Mapping[str, Any]] + +_REDACTED_KEYS = frozenset( + { + "absolute_position", + "bbox", + "bboxes", + "bounding_box", + "center", + "centroid", + "coordinates", + "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", + } +) + + +class SceneAdapterProtocolError(ValueError): + """The grounding or adjudication transport violated its JSON protocol.""" + + +@dataclass(frozen=True) +class CandidateSelection: + """Candidate binding against semantic scene data before materialization.""" + + scene_manifest: SceneManifest + role_bindings: RoleBindings | None + binding_report: BindingReport + selected_candidate: TaskCandidate | None + candidate_bindings: dict[str, RoleBindings] = field(default_factory=dict) + + @property + def selected_candidate_id(self) -> str | None: + """Return the chosen candidate identifier, when one was bindable.""" + return ( + str(self.selected_candidate["candidate_id"]) + if self.selected_candidate is not None + else None + ) + + +@dataclass(frozen=True) +class SceneAdaptation: + """Complete Scene Adapter result, including the reusable prepared scene.""" + + scene_manifest: SceneManifest + role_bindings: RoleBindings | None + binding_report: BindingReport + selected_candidate: TaskCandidate | None + prepared_scene: PreparedScene + source_config_path: Path + conservative_scene_graph: ConservativeSceneGraph + static_scene_manifest: StaticSceneManifest | None = None + candidate_bindings: dict[str, RoleBindings] = field(default_factory=dict) + + @property + def selected_candidate_id(self) -> str | None: + return ( + str(self.selected_candidate["candidate_id"]) + if self.selected_candidate is not None + else None + ) + + @property + def reference_bindings(self) -> dict[str, list[str]]: + if self.role_bindings is None: + return {} + return deepcopy(self.role_bindings["reference_bindings"]) + + +class SceneAdapter: + """Adapt one existing or packaged scene to a set of task candidates.""" + + def __init__( + self, + *, + model: str | None = None, + grounding_caller: GroundingCaller | None = None, + adjudicator: Adjudicator | None = None, + robot_profile: str = "franka", + scene_engine_adapter: SceneEngineV1Adapter | None = None, + ) -> None: + self.model = model + self.grounding_caller = grounding_caller + self.adjudicator = adjudicator + self.robot_profile = robot_profile + self.scene_engine_adapter = scene_engine_adapter or SceneEngineV1Adapter() + + def adapt( + self, + candidate_set: TaskCandidateSet | Sequence[Mapping[str, Any]], + source: SceneSourceRef | str | Path, + *, + grounding_caller: GroundingCaller | None = None, + adjudicator: Adjudicator | None = None, + force_most_likely: bool = False, + final_inspection: Mapping[str, Any] | None = None, + ) -> SceneAdaptation: + """Ground all candidates, then deterministically choose a bindable one.""" + task_id, instruction, candidates = _coerce_candidates(candidate_set) + source_ref = self._resolve_source(source) + source_fingerprint = fingerprint_scene_source(source_ref) + prepared = prepare_scene( + source_ref.path, + z_rotation_degrees=source_ref.z_rotation_degrees, + body_scale_policy=source_ref.body_scale_policy, + body_scale=source_ref.body_scale, + ) + if final_inspection is not None: + normalized_inspection = validate_final_scene_inspection(final_inspection) + if normalized_inspection["scene_revision_id"] != scene_revision_id( + source_ref + ): + raise ValueError( + "FinalSceneInspection does not describe the adapted scene revision." + ) + prepared = apply_final_inspection(prepared, normalized_inspection) + inventory = SceneInventory( + prepared.planner_objects, + robot_profile=source_ref.robot_profile, + ) + resolved_source = resolve_source_scene(source_ref.path) + manifest = _build_manifest( + prepared, + inventory, + source_format=resolved_source.source_format, + ) + static_manifest = self.scene_engine_adapter.adapt_prepared_scene( + prepared, + source_format=resolved_source.source_format, + robot_profile=inventory.profile, + ) + static_manifest["source"]["source_fingerprint"] = source_fingerprint.to_dict() + static_manifest = validate_static_scene_manifest(static_manifest) + conservative_scene_graph = build_conservative_scene_graph( + prepared, + scene_id=static_manifest["scene_id"], + ) + if fingerprint_scene_source(source_ref) != source_fingerprint: + raise RuntimeError("Source Gym project changed while it was being adapted.") + + selection = self._select_candidates( + task_id, + instruction, + candidates, + manifest=manifest, + inventory=inventory, + scene_objects=prepared.planner_objects, + grounding_caller=grounding_caller, + adjudicator=adjudicator, + force_most_likely=force_most_likely, + ) + return SceneAdaptation( + scene_manifest=manifest, + role_bindings=selection.role_bindings, + binding_report=selection.binding_report, + selected_candidate=selection.selected_candidate, + prepared_scene=prepared, + source_config_path=prepared.source_config_path, + conservative_scene_graph=conservative_scene_graph, + static_scene_manifest=static_manifest, + candidate_bindings=selection.candidate_bindings, + ) + + def select_objects( + self, + candidate_set: TaskCandidateSet | Sequence[Mapping[str, Any]], + scene_objects: Sequence[Mapping[str, Any]], + *, + source_format: str = "embodichain.scene-blueprint/v2", + robot_profile: str | None = None, + grounding_caller: GroundingCaller | None = None, + adjudicator: Adjudicator | None = None, + force_most_likely: bool = False, + ) -> CandidateSelection: + """Bind candidates to semantic objects before assets are generated. + + Args: + candidate_set: Validated Task Engine candidate set. + scene_objects: Blueprint-level semantic object records. + source_format: Provenance label included in the semantic manifest. + robot_profile: Optional robot profile override. + grounding_caller: Optional structured grounding transport. + adjudicator: Optional candidate tie-breaker. + force_most_likely: Resolve ranked UID hypotheses instead of rejecting + low-confidence or ambiguous responses. + + Returns: + Audited candidate selection without requiring generated assets. + """ + task_id, instruction, candidates = _coerce_candidates(candidate_set) + inventory = SceneInventory( + scene_objects, + robot_profile=robot_profile or self.robot_profile, + ) + manifest = _build_semantic_manifest( + inventory, + source_format=source_format, + ) + return self._select_candidates( + task_id, + instruction, + candidates, + manifest=manifest, + inventory=inventory, + scene_objects=scene_objects, + grounding_caller=grounding_caller, + adjudicator=adjudicator, + force_most_likely=force_most_likely, + ) + + def _select_candidates( + self, + task_id: str, + instruction: str, + candidates: Sequence[TaskCandidate], + *, + manifest: SceneManifest, + inventory: SceneInventory, + scene_objects: Sequence[Mapping[str, Any]], + grounding_caller: GroundingCaller | None, + adjudicator: Adjudicator | None, + force_most_likely: bool, + ) -> CandidateSelection: + invoke = grounding_caller or self.grounding_caller + use_default_adjudicator = invoke is None + if invoke is None: + invoke = _default_grounding_caller() + choose = adjudicator or self.adjudicator + if choose is None and use_default_adjudicator: + choose = _default_adjudicator(model=self.model) + audits: list[dict[str, Any]] = [] + bindings_by_candidate: dict[str, dict[str, tuple[str, ...]]] = {} + for candidate in candidates: + audit, bindings = _ground_candidate( + candidate, + instruction=instruction, + inventory=inventory, + scene_objects=scene_objects, + model=self.model, + caller=invoke, + force_most_likely=force_most_likely, + ) + audits.append(audit) + if bindings is not None: + bindings_by_candidate[str(candidate["candidate_id"])] = bindings + + selected_id, status, reason = _select_candidate( + candidates, + audits, + manifest=manifest, + instruction=instruction, + adjudicator=choose, + ) + report = validate_binding_report( + { + "schema_version": BINDING_REPORT_SCHEMA, + "task_id": task_id, + "status": status, + "selected_candidate_id": selected_id or "", + "selection_reason": reason, + "candidates": audits, + } + ) + selected = next( + ( + deepcopy(candidate) + for candidate in candidates + if candidate["candidate_id"] == selected_id + ), + None, + ) + candidate_bindings = { + candidate_id: validate_role_bindings( + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": task_id, + "candidate_id": candidate_id, + "reference_bindings": { + key: list(value) for key, value in sorted(raw_bindings.items()) + }, + "role_bindings": {}, + } + ) + for candidate_id, raw_bindings in bindings_by_candidate.items() + } + role_bindings = None if selected_id is None else candidate_bindings[selected_id] + return CandidateSelection( + scene_manifest=manifest, + role_bindings=role_bindings, + binding_report=report, + selected_candidate=selected, + candidate_bindings=candidate_bindings, + ) + + def _resolve_source( + self, + source: SceneSourceRef | str | Path, + ) -> SceneSourceRef: + if isinstance(source, SceneSourceRef): + return source + return SceneSourceRef(source, robot_profile=self.robot_profile) + + +def _coerce_candidates( + value: TaskCandidateSet | Sequence[Mapping[str, Any]], +) -> tuple[str, str, list[TaskCandidate]]: + if isinstance(value, Mapping): + normalized = validate_task_candidate_set(value) + return ( + normalized["task_id"], + normalized["instruction"], + normalized["candidates"], + ) + if isinstance(value, Sequence) and not isinstance(value, (str, bytes)): + candidates = [validate_task_candidate(candidate) for candidate in value] + if not candidates: + raise ValueError("SceneAdapter requires at least one TaskCandidate.") + task_ids = {candidate["draft"]["task_id"] for candidate in candidates} + instructions = {candidate["draft"]["instruction"] for candidate in candidates} + if len(task_ids) != 1 or len(instructions) != 1: + raise ValueError("All TaskCandidates must describe the same task.") + return task_ids.pop(), instructions.pop(), candidates + raise TypeError("candidate_set must be a TaskCandidateSet or candidate sequence.") + + +def _default_grounding_caller() -> GroundingCaller: + # Keep provider setup lazy so package import and offline tests never load an + # LLM client. This is the same structured transport used by interpretation. + return _default_instruction_caller + + +def _default_adjudicator(*, model: str | None) -> Adjudicator: + caller = _default_grounding_caller() + + def adjudicate(**kwargs: Any) -> Mapping[str, Any]: + candidates = [ + { + key: deepcopy(candidate[key]) + for key in ( + "candidate_id", + "draft", + "scene_request", + "success_spec", + "vote_count", + ) + } + for candidate in kwargs["candidates"] + ] + allowed = [str(candidate["candidate_id"]) for candidate in candidates] + schema = { + "title": "ActionEngineTaskAdjudication", + "type": "object", + "additionalProperties": False, + "required": ["candidate_id"], + "properties": { + "candidate_id": {"type": "string", "enum": allowed}, + }, + } + prompt = ( + "Select exactly one already verified, fully bindable task candidate " + "that best matches the instruction and redacted scene manifest. Do " + "not alter a candidate or invent a new interpretation. Return only " + "candidate_id.\n\n" + f"Instruction:\n{kwargs['instruction']}\n\n" + "Candidates:\n" + f"{json.dumps(candidates, ensure_ascii=False, sort_keys=True)}\n\n" + "Redacted scene manifest:\n" + f"{json.dumps(kwargs['scene_manifest'], ensure_ascii=False, sort_keys=True)}" + ) + try: + return caller(prompt=prompt, schema=schema, model=model) + except (TypeError, ValueError) as exc: + raise SceneAdapterProtocolError( + f"Task adjudication returned invalid structured output: {exc}" + ) from exc + + return adjudicate + + +def _ground_candidate( + candidate: TaskCandidate, + *, + instruction: str, + inventory: SceneInventory, + scene_objects: Sequence[Mapping[str, Any]], + model: str | None, + caller: GroundingCaller, + force_most_likely: bool, +) -> tuple[dict[str, Any], dict[str, tuple[str, ...]] | None]: + responses: list[Any] = [] + + def audited_caller(**kwargs: Any) -> Mapping[str, Any]: + call_kwargs = dict(kwargs) + if force_most_likely: + call_kwargs["prompt"] = ( + f"{kwargs['prompt']}\n\nFINAL BINDING OVERRIDE: do not return " + "ambiguous merely because confidence is low. Choose the most " + "likely existing UID that satisfies the supplied structured " + "role, affordance, state, and attribute metadata. Return " + "candidate UIDs in descending likelihood order. Do not invent, " + "add, delete, move, or modify any scene object. Use not_found " + "when no structurally compatible existing object is plausible." + ) + response = caller(**call_kwargs) + responses.append(deepcopy(response)) + if force_most_likely: + return _force_most_likely_response(response, candidate=candidate) + return response + + candidate_id = str(candidate["candidate_id"]) + try: + result = ground_scene_references( + instruction=instruction, + intent=candidate["draft"], + inventory=inventory, + scene_objects=scene_objects, + model=model, + caller=audited_caller, + ) + except (TypeError, ValueError) as exc: + if responses: + audits = _audit_unresolved_response( + responses[-1], + candidate=candidate, + inventory=inventory, + error=str(exc), + ) + status = _candidate_status(audits) + return ( + _candidate_audit(candidate, status, audits, [str(exc)]), + None, + ) + raise SceneAdapterProtocolError( + f"Grounding candidate {candidate_id!r} failed before returning JSON: {exc}" + ) from exc + + raw_bindings = result.bindings + response_by_id = _response_bindings(responses[-1], candidate=candidate) + self_reference_reasons = _self_reference_reasons(candidate["draft"], raw_bindings) + reference_audits = [] + incompatible: set[str] = set() + request_by_id = { + str(request["reference_id"]): request + for request in candidate["scene_request"]["references"] + } + for reference_id, uids in raw_bindings.items(): + compatibility_reasons = _compatibility_reasons( + request_by_id[reference_id], + uids, + inventory=inventory, + draft=candidate["draft"], + ) + compatibility_reasons.extend(self_reference_reasons.get(reference_id, ())) + compatibility_reasons = sorted(set(compatibility_reasons)) + if compatibility_reasons: + incompatible.add(reference_id) + response = response_by_id[reference_id] + audit_reasons = list(compatibility_reasons) + if ( + force_most_likely + and response.get("status") == "ambiguous" + and response.get("uids") + ): + audit_reasons.append( + "Forced the highest-ranked structurally compatible UID from an " + "ambiguous low-confidence response." + ) + reference_audits.append( + { + "reference_id": reference_id, + "status": ("incompatible" if compatibility_reasons else "resolved"), + "confidence": float(response["confidence"]), + "candidate_uids": list(response["uids"]), + "selected_uids": ([] if compatibility_reasons else list(uids)), + "reasons": audit_reasons, + } + ) + if incompatible: + reasons = [ + f"Reference {reference_id!r} conflicts with authoritative scene semantics." + for reference_id in sorted(incompatible) + ] + return ( + _candidate_audit(candidate, "incompatible", reference_audits, reasons), + None, + ) + return _candidate_audit(candidate, "resolved", reference_audits, []), dict( + raw_bindings + ) + + +def _force_most_likely_response( + response: Mapping[str, Any], + *, + candidate: TaskCandidate, +) -> Mapping[str, Any]: + """Turn ranked low-confidence UID hypotheses into explicit selections.""" + if not isinstance(response, Mapping) or set(response) != {"bindings"}: + return response + requests = { + str(item["reference_id"]): item + for item in candidate["scene_request"]["references"] + } + raw_bindings = response.get("bindings") + if not isinstance(raw_bindings, Sequence) or isinstance(raw_bindings, (str, bytes)): + return response + result = deepcopy(dict(response)) + values = [] + for raw in raw_bindings: + if not isinstance(raw, Mapping): + return response + item = deepcopy(dict(raw)) + request = requests.get(str(item.get("reference_id", ""))) + uids = item.get("uids") + if ( + request is not None + and item.get("status") in {"resolved", "ambiguous"} + and isinstance(uids, Sequence) + and not isinstance(uids, (str, bytes)) + and uids + ): + quantifier = str(request["quantifier"]) + count = int(request["count"]) + if quantifier == "one": + item["uids"] = list(uids[:1]) + elif quantifier == "count": + item["uids"] = list(uids[:count]) + item["status"] = "resolved" + confidence = item.get("confidence") + if isinstance(confidence, (int, float)) and not isinstance( + confidence, bool + ): + item["confidence"] = max(0.5, float(confidence)) + values.append(item) + result["bindings"] = values + return result + + +def _response_bindings( + response: Any, + *, + candidate: TaskCandidate, +) -> dict[str, Mapping[str, Any]]: + expected = { + str(request["reference_id"]) + for request in candidate["scene_request"]["references"] + } + if not isinstance(response, Mapping) or set(response) != {"bindings"}: + raise SceneAdapterProtocolError( + "Grounding response must contain only bindings." + ) + values = response["bindings"] + if not isinstance(values, Sequence) or isinstance(values, (str, bytes)): + raise SceneAdapterProtocolError("Grounding response bindings must be a list.") + result: dict[str, Mapping[str, Any]] = {} + for raw in values: + if not isinstance(raw, Mapping): + raise SceneAdapterProtocolError( + "Every grounding binding must be a mapping." + ) + reference_id = raw.get("reference_id") + if not isinstance(reference_id, str) or reference_id not in expected: + raise SceneAdapterProtocolError( + "Grounding response contains an unknown reference ID." + ) + if reference_id in result: + raise SceneAdapterProtocolError( + "Grounding response contains duplicate reference IDs." + ) + result[reference_id] = raw + if set(result) != expected: + raise SceneAdapterProtocolError( + "Grounding response omitted requested reference IDs." + ) + return result + + +def _audit_unresolved_response( + response: Any, + *, + candidate: TaskCandidate, + inventory: SceneInventory, + error: str, +) -> list[dict[str, Any]]: + by_id = _response_bindings(response, candidate=candidate) + audits: list[dict[str, Any]] = [] + for request in candidate["scene_request"]["references"]: + reference_id = str(request["reference_id"]) + raw = by_id[reference_id] + if set(raw) != {"reference_id", "status", "uids", "confidence"}: + raise SceneAdapterProtocolError( + f"Grounding binding {reference_id!r} has unsupported fields." + ) + status = raw["status"] + if status not in {"resolved", "ambiguous", "not_found"}: + raise SceneAdapterProtocolError( + f"Grounding binding {reference_id!r} has invalid status." + ) + uids = raw["uids"] + confidence = raw["confidence"] + if ( + not isinstance(uids, Sequence) + or isinstance(uids, (str, bytes)) + or any( + not isinstance(uid, str) or uid not in inventory.by_uid for uid in uids + ) + or len(set(uids)) != len(uids) + ): + raise SceneAdapterProtocolError( + f"Grounding binding {reference_id!r} has invalid candidate UIDs." + ) + if ( + isinstance(confidence, bool) + or not isinstance(confidence, (int, float)) + or not 0.0 <= float(confidence) <= 1.0 + ): + raise SceneAdapterProtocolError( + f"Grounding binding {reference_id!r} has invalid confidence." + ) + audit_status = status + reasons: list[str] = [] + if status == "not_found" and uids: + raise SceneAdapterProtocolError( + f"Grounding binding {reference_id!r} status=not_found requires no UIDs." + ) + if status == "resolved": + audit_status = "incompatible" + reasons.append(error) + else: + reasons.append(f"Grounding returned status={status}.") + audits.append( + { + "reference_id": reference_id, + "status": audit_status, + "confidence": float(confidence), + "candidate_uids": list(uids), + "selected_uids": [], + "reasons": reasons, + } + ) + return audits + + +def _compatibility_reasons( + request: Mapping[str, Any], + uids: Sequence[str], + *, + inventory: SceneInventory, + draft: Mapping[str, Any], +) -> list[str]: + entities = [inventory.by_uid[uid] for uid in uids] + reasons: list[str] = [] + role = str(request["role"]) + step = next(item for item in draft["steps"] if item["id"] == request["step_id"]) + try: + if role == "object": + validate_source_compatibility(str(step["task_type"]), entities) + else: + for entity in entities: + validate_target_compatibility( + str(step["task_type"]), + entity, + relation=str(step["relation"]), + ) + except ValueError as exc: + reasons.append(str(exc)) + + expected_structure = str(request["source_structure"]) + for entity in entities: + # Source structure is strict for manipulated objects. Target structure + # is relation-dependent and is already checked by + # validate_target_compatibility; a table support surface must not be + # rejected merely because it is passive rather than a rigid object. + if role == "object": + if expected_structure == "articulation" and entity.role != "articulation": + reasons.append( + f"UID {entity.uid!r} is not an articulation as requested." + ) + if expected_structure in { + "rigid_object", + "movable", + } and entity.role not in { + "object", + "rigid_object", + }: + reasons.append( + f"UID {entity.uid!r} is not a movable rigid object as requested." + ) + required_affordances = set(request["affordances"]) + if entity.affordances: + missing = required_affordances - set(entity.affordances) + if missing: + reasons.append( + f"UID {entity.uid!r} explicitly lacks affordances {sorted(missing)}." + ) + for key, expected in request["initial_state"].items(): + if key in entity.initial_state and entity.initial_state[key] != expected: + reasons.append( + f"UID {entity.uid!r} state {key!r} conflicts with the request." + ) + for key, expected in request["attributes"].items(): + if key in entity.attributes and entity.attributes[key] != expected: + reasons.append( + f"UID {entity.uid!r} attribute {key!r} conflicts with the request." + ) + return sorted(set(reasons)) + + +def _self_reference_reasons( + draft: Mapping[str, Any], + bindings: Mapping[str, Sequence[str]], +) -> dict[str, list[str]]: + """Reject object/target identity overlap, including step_result selectors.""" + objects_by_step: dict[str, tuple[str, ...]] = {} + reasons: dict[str, list[str]] = {} + for step in draft["steps"]: + step_id = str(step["id"]) + object_uids = _selector_uids( + step["object"], + reference_id=f"{step_id}.object", + bindings=bindings, + objects_by_step=objects_by_step, + ) + target_uids = _selector_uids( + step["target"], + reference_id=f"{step_id}.target", + bindings=bindings, + objects_by_step=objects_by_step, + ) + overlap = sorted(set(object_uids) & set(target_uids)) + if overlap: + reason = ( + f"Grounding step {step_id!r} uses the same UID as object and " + f"target: {overlap}." + ) + for role in ("object", "target"): + selector = step[role] + if selector["kind"] == "scene_ref": + reasons.setdefault(f"{step_id}.{role}", []).append(reason) + objects_by_step[step_id] = object_uids + return reasons + + +def _selector_uids( + selector: Mapping[str, Any], + *, + reference_id: str, + bindings: Mapping[str, Sequence[str]], + objects_by_step: Mapping[str, tuple[str, ...]], +) -> tuple[str, ...]: + kind = str(selector["kind"]) + if kind == "scene_ref": + return tuple(str(uid) for uid in bindings[reference_id]) + if kind == "step_result": + return objects_by_step[str(selector["step_id"])] + return () + + +def _candidate_audit( + candidate: TaskCandidate, + status: str, + references: Sequence[Mapping[str, Any]], + reasons: Sequence[str], +) -> dict[str, Any]: + return { + "candidate_id": candidate["candidate_id"], + "semantic_hash": candidate["semantic_hash"], + "status": status, + "references": [deepcopy(dict(reference)) for reference in references], + "reasons": list(reasons), + } + + +def _candidate_status(references: Sequence[Mapping[str, Any]]) -> str: + statuses = {str(reference["status"]) for reference in references} + if statuses == {"resolved"}: + return "resolved" + if "ambiguous" in statuses: + return "ambiguous" + if "incompatible" in statuses: + return "incompatible" + return "not_found" + + +def _select_candidate( + candidates: Sequence[TaskCandidate], + audits: Sequence[Mapping[str, Any]], + *, + manifest: SceneManifest, + instruction: str, + adjudicator: Adjudicator | None, +) -> tuple[str | None, str, str]: + audit_by_id = {str(audit["candidate_id"]): audit for audit in audits} + bound = [ + candidate + for candidate in candidates + if audit_by_id[str(candidate["candidate_id"])]["status"] == "resolved" + ] + majority = [candidate for candidate in bound if int(candidate["vote_count"]) >= 2] + if len(majority) == 1: + return str(majority[0]["candidate_id"]), "bound", "majority_bindable" + if not majority and len(bound) == 1: + return str(bound[0]["candidate_id"]), "bound", "unique_bindable" + + choices = majority if majority else bound + if len(choices) > 1: + if adjudicator is None: + return None, "ambiguous", "multiple_conflicting_bindable_candidates" + raw = adjudicator( + instruction=instruction, + candidates=deepcopy(list(choices)), + scene_manifest=deepcopy(manifest), + ) + if not isinstance(raw, Mapping) or set(raw) != {"candidate_id"}: + raise SceneAdapterProtocolError( + "Adjudicator response must contain only candidate_id." + ) + selected_id = raw["candidate_id"] + allowed = {str(candidate["candidate_id"]) for candidate in choices} + if not isinstance(selected_id, str) or selected_id not in allowed: + raise SceneAdapterProtocolError( + "Adjudicator must select the candidate_id of a verified bindable candidate." + ) + return selected_id, "bound", "adjudicated_bindable" + if any(audit["status"] == "ambiguous" for audit in audits): + return None, "ambiguous", "no_fully_bound_candidate" + return None, "unsatisfied", "no_fully_bound_candidate" + + +def _build_manifest( + prepared: PreparedScene, + inventory: SceneInventory, + *, + source_format: str, +) -> SceneManifest: + objects = [ + { + "uid": entity.uid, + "role": entity.role, + "name": entity.name, + "description": entity.description, + "category": entity.category, + "color": entity.color, + "affordances": sorted(entity.affordances), + "initial_state": _redact_semantics(entity.initial_state), + "attributes": _redact_semantics(entity.attributes), + } + for entity in sorted(inventory.entities, key=lambda item: item.uid) + ] + scene_id = _canonical_hash( + { + "source_format": source_format, + "objects": objects, + "asset_hashes": prepared.asset_hashes, + "rotation": prepared.z_rotation_degrees, + "xy_translation": list(prepared.source_scene_xy_translation), + "body_scale_policy": prepared.body_scale_policy, + "body_scale": prepared.body_scale, + } + ) + return validate_scene_manifest( + { + "schema_version": SCENE_MANIFEST_SCHEMA, + "scene_id": scene_id, + "source_format": source_format, + "robot_profile": inventory.profile, + "objects": objects, + } + ) + + +def _build_semantic_manifest( + inventory: SceneInventory, + *, + source_format: str, +) -> SceneManifest: + objects = [ + { + "uid": entity.uid, + "role": entity.role, + "name": entity.name, + "description": entity.description, + "category": entity.category, + "color": entity.color, + "affordances": sorted(entity.affordances), + "initial_state": _redact_semantics(entity.initial_state), + "attributes": _redact_semantics(entity.attributes), + } + for entity in sorted(inventory.entities, key=lambda item: item.uid) + ] + return validate_scene_manifest( + { + "schema_version": SCENE_MANIFEST_SCHEMA, + "scene_id": _canonical_hash( + {"source_format": source_format, "objects": objects} + ), + "source_format": source_format, + "robot_profile": inventory.profile, + "objects": objects, + } + ) + + +def _redact_semantics(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_semantics(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)): + simple = [item for item in child if isinstance(item, (str, bool))] + if len(simple) == len(child): + result[name] = simple + return result + + +def _canonical_hash(value: Any) -> str: + payload = json.dumps( + value, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode("utf-8") + return hashlib.sha256(payload).hexdigest() diff --git a/embodichain/gen_sim/task_engine/orchestration/scene_assets.py b/embodichain/gen_sim/task_engine/orchestration/scene_assets.py new file mode 100644 index 000000000..66563bea5 --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/scene_assets.py @@ -0,0 +1,210 @@ +# ---------------------------------------------------------------------------- +# 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 generated-scene meshes for the Task Program runtime.""" + +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 .source_scene import PreparedScene + +__all__ = ["normalize_scene_assets"] + +_POLICY = "action_engine_glb_geometry_v3" + + +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. + + Args: + scene: Prepared scene whose runtime GLB assets should be normalized. + output_dir: Root directory for content-addressed normalized assets. + + Returns: + A copy of the prepared scene that references normalized runtime meshes + and records their hashes and provenance. + """ + 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) + if _has_inconsistent_shading_normals(mesh): + # DexSim rejects ray-traced meshes when a smoothed vertex normal + # points away from an adjacent triangle. Split only affected + # meshes; preserving safe topology keeps grasp sampling stable. + mesh.unmerge_vertices() + 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 _has_inconsistent_shading_normals(mesh: Any) -> bool: + """Return whether any face corner normal points away from its triangle.""" + vertices = np.asarray(mesh.vertices, dtype=np.float64) + faces = np.asarray(mesh.faces) + if faces.size == 0: + return False + triangles = vertices[faces] + crosses = np.cross( + triangles[:, 1] - triangles[:, 0], + triangles[:, 2] - triangles[:, 0], + ) + face_lengths = np.linalg.norm(crosses, axis=1) + face_normals = np.zeros_like(crosses) + np.divide( + crosses, + face_lengths[:, None], + out=face_normals, + where=face_lengths[:, None] > 0.0, + ) + vertex_normals = np.zeros((len(vertices), 3), dtype=np.float64) + np.add.at( + vertex_normals, + faces.reshape(-1), + np.repeat(face_normals, 3, axis=0), + ) + vertex_lengths = np.linalg.norm(vertex_normals, axis=1) + np.divide( + vertex_normals, + vertex_lengths[:, None], + out=vertex_normals, + where=vertex_lengths[:, None] > 0.0, + ) + corner_normals = vertex_normals[faces] + if not np.isfinite(face_normals).all() or not np.isfinite(corner_normals).all(): + return True + alignments = np.einsum("fci,fi->fc", corner_normals, face_normals) + return bool(np.any(alignments < 0.0)) + + +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/task_engine/orchestration/scene_inventory.py b/embodichain/gen_sim/task_engine/orchestration/scene_inventory.py new file mode 100644 index 000000000..cdb449b02 --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/scene_inventory.py @@ -0,0 +1,265 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Structural scene inventory used by Task Engine semantic binding.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass, field +from typing import Any + +from embodichain.gen_sim.task_engine.ontology import task_contract + +__all__ = [ + "SceneEntity", + "SceneInventory", + "validate_source_compatibility", + "validate_target_compatibility", +] + + +@dataclass(frozen=True) +class SceneEntity: + """One scene entity with source semantics preserved verbatim. + + Attributes: + uid: Stable runtime identity used by semantic calls. + role: Structural scene role such as ``rigid_object`` or ``background``. + name: Source-provided display name. + description: Source-provided semantic description. + category: Open-world source category label. + color: Optional source-provided color label. + position: Initial world position used only for relative-side scoring. + affordances: Explicit source affordances; an empty set remains unknown. + initial_state: Source-provided initial semantic state. + attributes: Additional source semantic attributes. + source_uid: Original identity before runtime normalization. + """ + + 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: + """Index structural scene facts without natural-language matching rules. + + Args: + scene_objects: Source scene objects with canonical runtime identities. + robot_profile: Non-empty profile identifier retained for audit context. + + Raises: + ValueError: If identities are missing or duplicated, the profile is + empty, or the scene has no interactive object. + """ + + _PASSIVE_ROLES = frozenset( + { + "background", + "camera", + "light", + "robot", + "sensor", + "support_surface", + "table", + } + ) + + def __init__( + self, + scene_objects: Sequence[Mapping[str, Any]], + *, + robot_profile: str, + ) -> None: + profile = str(robot_profile).strip().lower().replace("-", "_") + if not profile: + raise ValueError("robot_profile must be non-empty.") + self.profile = 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, ...]: + """Return interaction entities accepted as source candidates.""" + return self.interactive + + def left_score(self, entity: SceneEntity) -> float: + """Return robot-relative lateral score; positive values are left.""" + return -entity.position[1] + + +def validate_source_compatibility( + task_type: str, + objects: Sequence[SceneEntity], +) -> None: + """Validate source structure and explicitly declared affordances. + + Args: + task_type: Canonical E-task identifier. + objects: Bound source entities to validate. + + Raises: + ValueError: If ``task_type`` is unknown, or a source has incompatible + structure or contradicts the task's required affordances. + """ + 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 structural or explicitly declared target contradictions. + + Args: + task_type: Canonical E-task identifier. + target: Bound target entity, or ``None`` when the call has no target. + relation: Canonical spatial relation requested by the task. + + Raises: + ValueError: If containment lacks a target or the target explicitly + contradicts the required container structure or affordance. + """ + if relation == "on" and target is not None: + 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 _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() + attributes = raw.get("attributes", {}) + if not isinstance(attributes, Mapping): + raise ValueError(f"Scene object {uid!r} attributes must be a mapping.") + raw_color = raw.get("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( + value + for item in raw_affordances + if ( + value := ( + str(item.get("type", item.get("name", ""))).strip().lower() + if isinstance(item, Mapping) + else str(item).strip().lower() + ) + ) + ) + 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/task_engine/orchestration/scene_source.py b/embodichain/gen_sim/task_engine/orchestration/scene_source.py new file mode 100644 index 000000000..cc8e40d68 --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/scene_source.py @@ -0,0 +1,279 @@ +# ---------------------------------------------------------------------------- +# 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-only references and integrity checks for existing Gym projects.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +import hashlib +import json +import os +from pathlib import Path +from typing import Any +from urllib.parse import unquote, urlparse +import xml.etree.ElementTree as ET + +from embodichain.data import get_data_path +from .source_scene import resolve_source_scene + +__all__ = [ + "SceneSourceFingerprint", + "SceneSourceRef", + "fingerprint_scene_source", + "scene_revision_id", + "verify_scene_source_fingerprint", +] + +_SCENE_SECTIONS = ("background", "rigid_object", "articulation") + + +@dataclass(frozen=True) +class SceneSourceRef: + """Reference an existing scene without copying or owning its files.""" + + path: Path | str + robot_profile: str = "franka" + z_rotation_degrees: float | None = None + body_scale_policy: str = "preserve" + body_scale: tuple[float, float, float] = (1.0, 1.0, 1.0) + + def __post_init__(self) -> None: + object.__setattr__(self, "path", Path(self.path).expanduser()) + + +@dataclass(frozen=True) +class SceneSourceFingerprint: + """Content evidence for one externally owned scene source.""" + + source_format: str + config_path: Path + config_sha256: str + asset_sha256: dict[str, str] + + def to_dict(self) -> dict[str, Any]: + """Return a JSON-safe audit view.""" + return { + "source_format": self.source_format, + "config_path": self.config_path.as_posix(), + "config_sha256": self.config_sha256, + "asset_sha256": dict(sorted(self.asset_sha256.items())), + } + + +def fingerprint_scene_source( + source: SceneSourceRef | str | Path, +) -> SceneSourceFingerprint: + """Hash a source config and referenced assets without copying either.""" + source_path = source.path if isinstance(source, SceneSourceRef) else source + resolved = resolve_source_scene(source_path) + config_bytes = resolved.path.read_bytes() + try: + config = json.loads(config_bytes) + except json.JSONDecodeError as exc: + raise ValueError(f"Scene config is not valid JSON: {resolved.path}") from exc + if not isinstance(config, Mapping): + raise ValueError(f"Scene config must contain an object: {resolved.path}") + + asset_hashes: dict[str, str] = {} + for section in _SCENE_SECTIONS: + entries = config.get(section, ()) + if not isinstance(entries, Sequence) or isinstance(entries, (str, bytes)): + continue + for index, entry in enumerate(entries): + if not isinstance(entry, Mapping): + continue + references: list[tuple[str, Any]] = [] + shape = entry.get("shape") + if isinstance(shape, Mapping) and shape.get("fpath"): + references.append(("shape.fpath", shape["fpath"])) + if section == "articulation" and entry.get("fpath"): + references.append(("fpath", entry["fpath"])) + for field_name, reference in references: + asset_path = Path(str(reference)).expanduser() + if not asset_path.is_absolute(): + asset_path = resolved.path.parent / asset_path + asset_path = asset_path.resolve() + if not asset_path.is_file() and not Path(str(reference)).is_absolute(): + asset_path = ( + Path(get_data_path(str(reference))).expanduser().resolve() + ) + if not asset_path.is_file(): + raise FileNotFoundError( + f"Scene asset does not exist: {asset_path} " + f"({section}[{index}].{field_name})." + ) + for dependency in _asset_dependency_files(asset_path): + asset_hashes[dependency.as_posix()] = _sha256( + dependency.read_bytes() + ) + return SceneSourceFingerprint( + source_format=resolved.source_format, + config_path=resolved.path, + config_sha256=_sha256(config_bytes), + asset_sha256=asset_hashes, + ) + + +def verify_scene_source_fingerprint(expected: Mapping[str, Any]) -> None: + """Raise when an externally owned source changed after preparation.""" + required = {"source_format", "config_path", "config_sha256", "asset_sha256"} + if set(expected) != required: + raise ValueError("Scene source fingerprint fields are invalid.") + actual = fingerprint_scene_source(str(expected["config_path"])).to_dict() + normalized = { + "source_format": str(expected["source_format"]), + "config_path": Path(str(expected["config_path"])).resolve().as_posix(), + "config_sha256": str(expected["config_sha256"]), + "asset_sha256": dict(expected["asset_sha256"]), + } + if actual != normalized: + raise RuntimeError( + "Source Gym project changed after Task Engine preparation; " + "prepare a new bundle before running it." + ) + + +def scene_revision_id(source: SceneSourceRef | str | Path) -> str: + """Return a location-independent content identity for one scene revision. + + Volatile exporter IDs and absolute asset paths are excluded. Referenced + asset content remains part of the identity through SHA-256 placeholders. + + Args: + source: Scene project, configuration path, or Task Engine source reference. + + Returns: + Stable SHA-256 identity of scene semantics and referenced asset content. + """ + source_path = source.path if isinstance(source, SceneSourceRef) else source + resolved = resolve_source_scene(source_path) + try: + config = json.loads(resolved.path.read_text(encoding="utf-8")) + except json.JSONDecodeError as exc: + raise ValueError(f"Scene config is not valid JSON: {resolved.path}") from exc + if not isinstance(config, Mapping): + raise ValueError(f"Scene config must contain an object: {resolved.path}") + normalized = _normalize_revision_value( + dict(config), + config_root=resolved.path.parent, + ) + normalized.pop("scene_id", None) + payload = json.dumps( + normalized, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode("utf-8") + return _sha256(payload) + + +def _normalize_revision_value(value: Any, *, config_root: Path) -> Any: + if isinstance(value, Mapping): + result = { + str(key): _normalize_revision_value(item, config_root=config_root) + for key, item in value.items() + } + for key in ("fpath",): + raw = result.get(key) + if not isinstance(raw, str) or not raw: + continue + path = Path(raw).expanduser() + if not path.is_absolute(): + path = config_root / path + path = path.resolve() + if path.is_file(): + files = _asset_dependency_files(path) + result[key] = { + "sha256": _sha256(path.read_bytes()), + "dependency_sha256": { + Path( + os.path.relpath(dependency, start=path.parent) + ).as_posix(): (_sha256(dependency.read_bytes())) + for dependency in files + if dependency != path + }, + } + return result + if isinstance(value, Sequence) and not isinstance(value, (str, bytes)): + return [ + _normalize_revision_value(item, config_root=config_root) for item in value + ] + return value + + +def _asset_dependency_files(asset_path: Path) -> tuple[Path, ...]: + """Return one asset and every local XML-declared dependency transitively.""" + pending = [asset_path.resolve()] + visited: set[Path] = set() + while pending: + path = pending.pop() + if path in visited: + continue + if not path.is_file(): + raise FileNotFoundError(f"Scene asset dependency does not exist: {path}") + visited.add(path) + if path.suffix.lower() not in {".urdf", ".xml", ".mjcf", ".xacro"}: + continue + try: + root = ET.parse(path).getroot() + except ET.ParseError: + # Opaque articulation assets remain valid direct dependencies even + # when their extension suggests XML. + continue + for element in root.iter(): + tag = element.tag.rsplit("}", maxsplit=1)[-1] + if tag not in {"mesh", "texture", "include"}: + continue + for attribute in ("filename", "file", "url"): + reference = element.attrib.get(attribute) + if reference: + pending.append(_resolve_asset_reference(path, reference)) + return tuple(sorted(visited)) + + +def _resolve_asset_reference(owner: Path, reference: str) -> Path: + """Resolve a local filesystem or ROS package URI without global state.""" + parsed = urlparse(reference) + if parsed.scheme in {"http", "https", "data"}: + raise ValueError( + f"Remote scene asset dependencies cannot be integrity-hashed: {reference}" + ) + if parsed.scheme == "file": + return Path(unquote(parsed.path)).expanduser().resolve() + if parsed.scheme == "package": + package_name = parsed.netloc + relative = Path(unquote(parsed.path.lstrip("/"))) + candidates = [ + ancestor / package_name / relative + for ancestor in (owner.parent, *owner.parents) + ] + candidates.append(owner.parent / relative) + for candidate in candidates: + if candidate.is_file(): + return candidate.resolve() + raise FileNotFoundError( + f"Unable to resolve package asset {reference!r} from {owner}." + ) + if parsed.scheme: + raise ValueError(f"Unsupported scene asset URI scheme: {reference}") + return (owner.parent / unquote(reference)).expanduser().resolve() + + +def _sha256(value: bytes) -> str: + return hashlib.sha256(value).hexdigest() diff --git a/embodichain/gen_sim/task_engine/orchestration/source_scene.py b/embodichain/gen_sim/task_engine/orchestration/source_scene.py new file mode 100644 index 000000000..98e5e8918 --- /dev/null +++ b/embodichain/gen_sim/task_engine/orchestration/source_scene.py @@ -0,0 +1,741 @@ +# ---------------------------------------------------------------------------- +# 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 fills missing physics values +without overwriting explicitly authored physical properties. +""" + +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 + +__all__ = [ + "PreparedScene", + "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_.-]+") + +_SCENE_DEFAULTS = { + "prompt2scene_z_rotation_degrees": -90.0, + "body_scale_policy": "preserve", + "body_scale": (1.0, 1.0, 1.0), +} +_BACKGROUND_POLICY = { + "mass": 10.0, + "static_friction": 0.95, + "dynamic_friction": 0.9, + "restitution": 0.01, + "max_convex_hull_num": 1, +} +_RIGID_POLICY = { + "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", +} +_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 PreparedScene: + """A source scene normalized for planning and simulator loading. + + Attributes: + source_config_path: Absolute source configuration path. + scene_dir: Directory against which source assets were resolved. + planner_objects: Semantic object view used during task planning. + background: Normalized simulator background configurations. + rigid_objects: Normalized simulator rigid-object configurations. + articulations: Normalized simulator articulation configurations. + uid_map: Mapping from source identities to canonical runtime identities. + table_top_z: Estimated tabletop height when it can be derived. + z_rotation_degrees: World-frame rotation applied to source poses. + body_scale_policy: Applied source-scale policy. + body_scale: Requested scale vector. + asset_hashes: Runtime-identity to source-asset digest mapping. + source_scene_xy_translation: World translation applied before rotation. + asset_provenance: Optional normalized-asset audit entries. + """ + + 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], ...] = () + + +@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 source config through the compatibility API name. + + Args: + gym_project: Task root, export directory, or explicit configuration path. + + Returns: + Absolute path selected by :func:`resolve_source_scene`. + + Raises: + FileNotFoundError: If no supported source configuration exists. + ValueError: If source discovery is ambiguous or unsupported. + """ + return resolve_source_scene(gym_project).path + + +def is_prompt2scene_export(gym_project: str | Path) -> bool: + """Return whether the input has Prompt2Scene export provenance. + + Args: + gym_project: Task root, export directory, or explicit configuration path. + + Returns: + ``True`` when a supported source carries Prompt2Scene provenance; + otherwise ``False``, including invalid or missing paths. + """ + 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. + + Args: + gym_project: Task root, export directory, or explicit configuration path. + z_rotation_degrees: Optional world-frame rotation override. Prompt2Scene + inputs use the canonical rotation when this value is omitted. + source_scene_xy_translation: Optional two-value world translation. An + explicit robot scene otherwise centers itself on its table anchor. + body_scale_policy: One of ``preserve``, ``multiply``, or ``absolute``. + body_scale: Positive three-value scale consumed by the selected policy. + + Returns: + Canonically identified planner and simulator views of the source scene. + + Raises: + FileNotFoundError: If source discovery or a referenced asset fails. + ValueError: If the source, transform, scale, identities, or scene + structure is invalid. + """ + 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 = ( + [deepcopy(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"] = {**_BACKGROUND_ATTRS, **source_attrs} + result["body_type"] = "kinematic" + result["max_convex_hull_num"] = int(_BACKGROUND_POLICY["max_convex_hull_num"]) + else: + # Imported physical properties are authoritative; defaults only fill gaps. + result["attrs"] = {**_RIGID_ATTRS, **source_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/task_engine/reporting.py b/embodichain/gen_sim/task_engine/reporting.py new file mode 100644 index 000000000..4327b01e9 --- /dev/null +++ b/embodichain/gen_sim/task_engine/reporting.py @@ -0,0 +1,157 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Tensor-free reports preserving canonical Task Program runtime evidence.""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +import json +from pathlib import Path +from typing import Any, Final, TypeAlias + +__all__ = [ + "EXECUTION_REPORT_FILENAME", + "TASK_PROGRAM_EXECUTION_REPORT_SCHEMA", + "TaskProgramExecutionReport", + "validate_execution_report", + "write_execution_report", +] + +EXECUTION_REPORT_FILENAME: Final = "execution_report.json" +TASK_PROGRAM_EXECUTION_REPORT_SCHEMA: Final = "task_program_execution_report/v1" +TaskProgramExecutionReport: TypeAlias = dict[str, Any] + + +def validate_execution_report(value: Mapping[str, Any]) -> TaskProgramExecutionReport: + """Validate the stable Task Engine view without reclassifying runtime truth. + + Args: + value: JSON-compatible execution-report mapping. + + Returns: + A detached normalized report. + + Raises: + TypeError: If the report or one of its typed fields has the wrong type. + ValueError: If required fields, schema identity, or values are invalid. + """ + try: + report = json.loads(json.dumps(value, ensure_ascii=False, allow_nan=False)) + except (TypeError, ValueError) as exc: + raise TypeError( + "Execution report must contain only finite JSON values." + ) from exc + if type(report) is not dict: + raise TypeError("Execution report must be an exact mapping.") + required = { + "schema_version", + "status", + "task_id", + "semantic_call_count", + "integration_fingerprint", + "record_dir", + "environments", + "runtime_result", + "failure", + } + if set(report) != required: + raise ValueError( + "Execution report fields are invalid; " + f"missing={sorted(required - set(report))}, " + f"unexpected={sorted(set(report) - required)}." + ) + if report["schema_version"] != TASK_PROGRAM_EXECUTION_REPORT_SCHEMA: + raise ValueError("Execution report schema_version is unsupported.") + if report["status"] not in {"succeeded", "failed", "rejected", "aborted"}: + raise ValueError("Execution report status is invalid.") + if not isinstance(report["task_id"], str) or not report["task_id"].strip(): + raise ValueError("Execution report task_id must be non-empty.") + if ( + type(report["semantic_call_count"]) is not int + or report["semantic_call_count"] < 0 + ): + raise ValueError("Execution report semantic_call_count must be non-negative.") + fingerprint = report["integration_fingerprint"] + if not isinstance(fingerprint, str) or len(fingerprint) != 64: + raise ValueError("Execution report integration_fingerprint is invalid.") + if not isinstance(report["record_dir"], str) or not report["record_dir"]: + raise ValueError("Execution report record_dir must be a non-empty path.") + environments = report["environments"] + if type(environments) is not list or not environments: + raise ValueError("Execution report environments must be a non-empty list.") + for index, environment in enumerate(environments): + if type(environment) is not dict: + raise TypeError( + f"Execution report environments[{index}] must be a mapping." + ) + expected = {"env_id", "success", "terminal_reason", "semantic_success"} + if set(environment) != expected: + raise ValueError( + f"Execution report environments[{index}] fields are invalid." + ) + if type(environment["env_id"]) is not int or environment["env_id"] < 0: + raise ValueError( + f"Execution report environments[{index}].env_id is invalid." + ) + if type(environment["success"]) is not bool: + raise TypeError( + f"Execution report environments[{index}].success must be bool." + ) + if not isinstance(environment["terminal_reason"], str): + raise TypeError( + f"Execution report environments[{index}].terminal_reason must be string." + ) + semantics = environment["semantic_success"] + if type(semantics) is not dict or any( + type(item) is not bool for item in semantics.values() + ): + raise TypeError( + f"Execution report environments[{index}].semantic_success is invalid." + ) + if report["failure"] is not None and type(report["failure"]) is not dict: + raise TypeError("Execution report failure must be a mapping or None.") + return deepcopy(report) + + +def write_execution_report( + output_dir: str | Path, + value: Mapping[str, Any], +) -> Path: + """Write one validated execution report. + + Args: + output_dir: Directory that receives ``execution_report.json``. + value: JSON-compatible execution-report mapping. + + Returns: + Absolute path to the written report. + + Raises: + TypeError: If the report contains an invalid typed field. + ValueError: If the report violates its schema. + OSError: If the destination cannot be created or written. + """ + report = validate_execution_report(value) + root = Path(output_dir).expanduser().resolve() + root.mkdir(parents=True, exist_ok=True) + path = root / EXECUTION_REPORT_FILENAME + path.write_text( + json.dumps(report, ensure_ascii=False, indent=2, allow_nan=False) + "\n", + encoding="utf-8", + ) + return path diff --git a/embodichain/gen_sim/task_engine/run_directory.py b/embodichain/gen_sim/task_engine/run_directory.py new file mode 100644 index 000000000..0092cffc0 --- /dev/null +++ b/embodichain/gen_sim/task_engine/run_directory.py @@ -0,0 +1,87 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Collision-safe allocation of human-readable Task Engine run directories.""" + +from __future__ import annotations + +from contextlib import contextmanager +from dataclasses import dataclass +from datetime import datetime +from pathlib import Path +from typing import Iterator + +__all__ = ["RunDirectory", "reserve_run_directory"] + + +@dataclass(frozen=True) +class RunDirectory: + """One reserved run identifier and its not-yet-published destination.""" + + run_id: str + output_root: Path + path: Path + created_at: datetime + + +@contextmanager +def reserve_run_directory( + output_root: str | Path, + *, + now: datetime | None = None, +) -> Iterator[RunDirectory]: + """Reserve a timestamped child name without creating its destination. + + Args: + output_root: Persistent task-history directory. + now: Optional timezone-aware timestamp used by deterministic tests. + + Yields: + A run directory allocation safe to publish through ArtifactTransaction. + """ + created_at = now or datetime.now().astimezone() + if created_at.tzinfo is None or created_at.utcoffset() is None: + raise ValueError("Task Engine run timestamps must include a timezone.") + root = Path(output_root).expanduser().resolve() + root.mkdir(parents=True, exist_ok=True) + if not root.is_dir(): + raise NotADirectoryError(root) + + base = created_at.strftime("%Y%m%d_%H%M%S") + for collision_index in range(10_000): + run_id = base if collision_index == 0 else f"{base}_{collision_index:02d}" + destination = root / run_id + reservation = root / f".{run_id}.reserve" + if destination.exists(): + continue + try: + reservation.mkdir() + except FileExistsError: + continue + if destination.exists(): + reservation.rmdir() + continue + try: + yield RunDirectory( + run_id=run_id, + output_root=root, + path=destination, + created_at=created_at, + ) + finally: + reservation.rmdir() + return + raise RuntimeError("Unable to reserve a Task Engine run directory.") diff --git a/embodichain/gen_sim/task_engine/scene/__init__.py b/embodichain/gen_sim/task_engine/scene/__init__.py new file mode 100644 index 000000000..40a218b57 --- /dev/null +++ b/embodichain/gen_sim/task_engine/scene/__init__.py @@ -0,0 +1,53 @@ +# ---------------------------------------------------------------------------- +# 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 Engine ownership of scene adaptation and static feasibility.""" + +from __future__ import annotations + +from .contracts import ( + ASSESSMENT_STATUSES, + FEASIBILITY_REPORT_SCHEMA, + STATIC_SCENE_MANIFEST_SCHEMA, + FeasibilityReport, + StaticSceneManifest, + validate_feasibility_report, + validate_static_scene_manifest, +) +from .feasibility import FeasibilityBroker +from .scene_engine_v1 import SceneEngineV1Adapter +from .conservative_graph import ( + CONSERVATIVE_SCENE_GRAPH_SCHEMA, + ConservativeSceneGraph, + build_conservative_scene_graph, + validate_conservative_scene_graph, +) + +__all__ = [ + "ASSESSMENT_STATUSES", + "CONSERVATIVE_SCENE_GRAPH_SCHEMA", + "FEASIBILITY_REPORT_SCHEMA", + "STATIC_SCENE_MANIFEST_SCHEMA", + "FeasibilityBroker", + "FeasibilityReport", + "SceneEngineV1Adapter", + "StaticSceneManifest", + "ConservativeSceneGraph", + "build_conservative_scene_graph", + "validate_feasibility_report", + "validate_static_scene_manifest", + "validate_conservative_scene_graph", +] diff --git a/embodichain/gen_sim/task_engine/scene/conservative_graph.py b/embodichain/gen_sim/task_engine/scene/conservative_graph.py new file mode 100644 index 000000000..ac3f2a634 --- /dev/null +++ b/embodichain/gen_sim/task_engine/scene/conservative_graph.py @@ -0,0 +1,247 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Conservative hierarchy evidence for imported scenes.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +import json +from pathlib import Path +from typing import Any, Final, TypeAlias + +__all__ = [ + "CONSERVATIVE_SCENE_GRAPH_SCHEMA", + "ConservativeSceneGraph", + "build_conservative_scene_graph", + "validate_conservative_scene_graph", +] + +CONSERVATIVE_SCENE_GRAPH_SCHEMA: Final = "embodichain.conservative-scene-graph/v1" +ConservativeSceneGraph: TypeAlias = dict[str, Any] + + +def build_conservative_scene_graph( + prepared_scene: Any, + *, + scene_id: str, +) -> ConservativeSceneGraph: + """Use exported hierarchy when available and mark every gap as unknown.""" + source_path = Path(getattr(prepared_scene, "source_config_path")).resolve() + uid_map = dict(getattr(prepared_scene, "uid_map", {}) or {}) + exported = _read_exported_graph(source_path.with_name("scene_graph.json")) + operational_assumptions = _legacy_operational_assumption_uids(source_path) + exported_nodes = { + str(node.get("object_id")): node + for node in exported.get("nodes", ()) + if isinstance(node, Mapping) and node.get("object_id") + } + + nodes: list[dict[str, Any]] = [] + for raw in getattr(prepared_scene, "planner_objects"): + uid = str(raw.get("uid", "")) + source_uid = str(raw.get("source_uid", uid)) + known = exported_nodes.get(source_uid) or exported_nodes.get(uid) + attributes = raw.get("attributes", {}) + final_support = ( + attributes.get("final_support") if isinstance(attributes, Mapping) else None + ) + initial_state = raw.get("initial_state", {}) + final_orientation = ( + initial_state.get("orientation") + if isinstance(initial_state, Mapping) + else None + ) + if uid == "table": + node = { + "uid": uid, + "parent_uid": None, + "parent_relation": "root", + "orientation": "unknown", + "source": "structural_root", + } + elif isinstance(final_support, Mapping): + relation = str(final_support.get("relation", "unknown")) + parent_uid = final_support.get("parent_uid", "unknown") + node = { + "uid": uid, + "parent_uid": ( + str(parent_uid) + if isinstance(parent_uid, str) and parent_uid + else "unknown" + ), + "parent_relation": relation if relation == "on" else "unknown", + "orientation": ( + "standing" + if final_orientation == "upright" + else ("lying" if final_orientation == "fallen" else "unknown") + ), + "source": "final_inspection", + } + elif ( + known is None + or uid in operational_assumptions + or source_uid in operational_assumptions + ): + node = { + "uid": uid, + "parent_uid": "unknown", + "parent_relation": "unknown", + "orientation": "unknown", + "source": "conservative_import", + } + else: + raw_parent = known.get("parent_id") + parent_uid = ( + uid_map.get(str(raw_parent), str(raw_parent)) + if raw_parent is not None + else "unknown" + ) + relation = known.get("parent_relation") + orientation = known.get("orientation_state") + node = { + "uid": uid, + "parent_uid": parent_uid, + "parent_relation": relation if relation == "on" else "unknown", + "orientation": ( + orientation if orientation in {"standing", "lying"} else "unknown" + ), + "source": "scene_graph", + } + nodes.append(node) + + relations = [] + for raw in exported.get("relations", ()): + if not isinstance(raw, Mapping): + continue + source_uid = uid_map.get(str(raw.get("source_id")), str(raw.get("source_id"))) + target_uid = uid_map.get(str(raw.get("target_id")), str(raw.get("target_id"))) + relation = str(raw.get("relation", "")) + if source_uid and target_uid and relation: + relations.append( + { + "source_uid": source_uid, + "relation": relation, + "target_uid": target_uid, + "source": "scene_graph", + } + ) + return validate_conservative_scene_graph( + { + "schema_version": CONSERVATIVE_SCENE_GRAPH_SCHEMA, + "scene_id": str(scene_id), + "nodes": nodes, + "relations": relations, + } + ) + + +def _legacy_operational_assumption_uids(source_path: Path) -> set[str]: + manifest_path = source_path.parent.parent / "legacy_conversion.json" + if not manifest_path.is_file(): + return set() + try: + value = json.loads(manifest_path.read_text(encoding="utf-8")) + except json.JSONDecodeError as exc: + raise ValueError( + f"Legacy conversion manifest is invalid JSON: {manifest_path}" + ) from exc + if not isinstance(value, Mapping): + raise ValueError("Legacy conversion manifest must contain an object.") + assumptions = value.get("assumptions", ()) + if not isinstance(assumptions, Sequence) or isinstance(assumptions, (str, bytes)): + raise ValueError("Legacy conversion assumptions must be a sequence.") + return { + str(item["uid"]) + for item in assumptions + if isinstance(item, Mapping) and isinstance(item.get("uid"), str) + } + + +def validate_conservative_scene_graph( + value: Mapping[str, Any], +) -> ConservativeSceneGraph: + """Validate and detach one conservative graph.""" + if not isinstance(value, Mapping): + raise TypeError("ConservativeSceneGraph must be a mapping.") + result = deepcopy(dict(value)) + expected = {"schema_version", "scene_id", "nodes", "relations"} + if set(result) != expected: + raise ValueError("ConservativeSceneGraph fields are invalid.") + if result.get("schema_version") != CONSERVATIVE_SCENE_GRAPH_SCHEMA: + raise ValueError("ConservativeSceneGraph schema version is invalid.") + if not isinstance(result.get("scene_id"), str) or not result["scene_id"]: + raise ValueError("ConservativeSceneGraph.scene_id must not be empty.") + nodes = _sequence(result.get("nodes"), "nodes") + normalized_nodes = [] + for index, raw in enumerate(nodes): + if not isinstance(raw, Mapping): + raise TypeError(f"ConservativeSceneGraph.nodes[{index}] must be a mapping.") + node = dict(raw) + if set(node) != { + "uid", + "parent_uid", + "parent_relation", + "orientation", + "source", + }: + raise ValueError( + f"ConservativeSceneGraph.nodes[{index}] fields are invalid." + ) + if not isinstance(node["uid"], str) or not node["uid"]: + raise ValueError(f"ConservativeSceneGraph.nodes[{index}].uid is invalid.") + if node["parent_uid"] is not None and not isinstance(node["parent_uid"], str): + raise TypeError( + f"ConservativeSceneGraph.nodes[{index}].parent_uid is invalid." + ) + if node["parent_relation"] not in {"root", "on", "unknown"}: + raise ValueError( + f"ConservativeSceneGraph.nodes[{index}].parent_relation is invalid." + ) + if node["orientation"] not in {"standing", "lying", "unknown"}: + raise ValueError( + f"ConservativeSceneGraph.nodes[{index}].orientation is invalid." + ) + if not isinstance(node["source"], str) or not node["source"]: + raise ValueError( + f"ConservativeSceneGraph.nodes[{index}].source is invalid." + ) + normalized_nodes.append(node) + if len({node["uid"] for node in normalized_nodes}) != len(normalized_nodes): + raise ValueError("ConservativeSceneGraph node UIDs must be unique.") + result["nodes"] = normalized_nodes + result["relations"] = [ + dict(item) for item in _sequence(result.get("relations"), "relations") + ] + json.dumps(result, allow_nan=False) + return result + + +def _read_exported_graph(path: Path) -> dict[str, Any]: + if not path.is_file(): + return {} + try: + value = json.loads(path.read_text(encoding="utf-8")) + except json.JSONDecodeError as exc: + raise ValueError(f"Scene graph is not valid JSON: {path}") from exc + return dict(value) if isinstance(value, Mapping) else {} + + +def _sequence(value: Any, field_name: str) -> list[Any]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)): + raise TypeError(f"ConservativeSceneGraph.{field_name} must be a sequence.") + return list(value) diff --git a/embodichain/gen_sim/task_engine/scene/contracts.py b/embodichain/gen_sim/task_engine/scene/contracts.py new file mode 100644 index 000000000..974864785 --- /dev/null +++ b/embodichain/gen_sim/task_engine/scene/contracts.py @@ -0,0 +1,304 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""JSON contracts owned by the Scene Engine anti-corruption boundary.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +import json +import math +from typing import Any, TypeAlias + +__all__ = [ + "ASSESSMENT_STATUSES", + "FEASIBILITY_REPORT_SCHEMA", + "REMEDIATION_CLASSES", + "STATIC_SCENE_MANIFEST_SCHEMA", + "FeasibilityReport", + "StaticSceneManifest", + "validate_feasibility_report", + "validate_static_scene_manifest", +] + + +STATIC_SCENE_MANIFEST_SCHEMA = "embodichain.static-scene-manifest/v1" +FEASIBILITY_REPORT_SCHEMA = "embodichain.scene-action-feasibility/v2" +ASSESSMENT_STATUSES = frozenset({"proven", "runtime_probe", "unknown", "contradicted"}) +REMEDIATION_CLASSES = frozenset( + {"none", "scene_remediable", "action_capability", "input_conflict", "terminal"} +) +_EVIDENCE_STATUSES = frozenset({"declared", "inferred", "verified", "contradicted"}) + +StaticSceneManifest: TypeAlias = dict[str, Any] +FeasibilityReport: TypeAlias = dict[str, Any] + + +def validate_static_scene_manifest(value: Mapping[str, Any]) -> StaticSceneManifest: + """Validate and detach one static scene manifest.""" + result = _mapping(value, "StaticSceneManifest") + _exact_keys( + result, + { + "schema_version", + "scene_id", + "source_format", + "robot_profile", + "source", + "adapter_capabilities", + "objects", + }, + "StaticSceneManifest", + ) + _schema(result, STATIC_SCENE_MANIFEST_SCHEMA, "StaticSceneManifest") + for key in ("scene_id", "source_format", "robot_profile"): + result[key] = _nonempty(result.get(key), f"StaticSceneManifest.{key}") + result["source"] = _mapping(result.get("source"), "StaticSceneManifest.source") + result["adapter_capabilities"] = _bool_mapping( + result.get("adapter_capabilities"), + "StaticSceneManifest.adapter_capabilities", + ) + + objects: list[dict[str, Any]] = [] + for index, raw in enumerate(_sequence(result.get("objects"), "objects")): + context = f"StaticSceneManifest.objects[{index}]" + item = _mapping(raw, context) + _exact_keys( + item, + { + "uid", + "source_uid", + "role", + "name", + "description", + "category", + "color", + "geometry", + "initial_pose", + "physics", + "articulation", + "affordances", + "initial_state", + "attributes", + "provenance", + }, + context, + ) + item["uid"] = _nonempty(item.get("uid"), f"{context}.uid") + item["source_uid"] = _string(item.get("source_uid"), f"{context}.source_uid") + item["role"] = _nonempty(item.get("role"), f"{context}.role") + for key in ("name", "description", "category"): + item[key] = _string(item.get(key), f"{context}.{key}") + color = item.get("color") + if color is not None: + color = _string(color, f"{context}.color") + item["color"] = color + for key in ( + "geometry", + "initial_pose", + "physics", + "articulation", + "initial_state", + "attributes", + "provenance", + ): + item[key] = _mapping(item.get(key), f"{context}.{key}") + item["affordances"] = [ + _validate_affordance(evidence, f"{context}.affordances[{evidence_index}]") + for evidence_index, evidence in enumerate( + _sequence(item.get("affordances"), f"{context}.affordances") + ) + ] + objects.append(item) + uids = [item["uid"] for item in objects] + if len(set(uids)) != len(uids): + raise ValueError("StaticSceneManifest object UIDs must be unique.") + result["objects"] = objects + _json_safe(result, "StaticSceneManifest") + return result + + +def validate_feasibility_report(value: Mapping[str, Any]) -> FeasibilityReport: + """Validate and detach one scene/action feasibility report.""" + result = _mapping(value, "FeasibilityReport") + _exact_keys( + result, + { + "schema_version", + "task_id", + "candidate_id", + "scene_id", + "status", + "remediation_class", + "checks", + "blockers", + "summary", + }, + "FeasibilityReport", + ) + _schema(result, FEASIBILITY_REPORT_SCHEMA, "FeasibilityReport") + for key in ("task_id", "candidate_id", "scene_id"): + result[key] = _nonempty(result.get(key), f"FeasibilityReport.{key}") + result["status"] = _status(result.get("status"), "FeasibilityReport.status") + remediation_class = result.get("remediation_class") + if remediation_class not in REMEDIATION_CLASSES: + raise ValueError( + "FeasibilityReport.remediation_class must be one of " + f"{sorted(REMEDIATION_CLASSES)}." + ) + result["remediation_class"] = str(remediation_class) + if result["status"] != "contradicted" and remediation_class != "none": + raise ValueError( + "A non-contradicted FeasibilityReport requires remediation_class=none." + ) + checks: list[dict[str, Any]] = [] + for index, raw in enumerate(_sequence(result.get("checks"), "checks")): + context = f"FeasibilityReport.checks[{index}]" + item = _mapping(raw, context) + _exact_keys( + item, + {"kind", "subject", "status", "reason", "evidence"}, + context, + ) + item["kind"] = _nonempty(item.get("kind"), f"{context}.kind") + item["subject"] = _nonempty(item.get("subject"), f"{context}.subject") + item["status"] = _status(item.get("status"), f"{context}.status") + item["reason"] = _nonempty(item.get("reason"), f"{context}.reason") + item["evidence"] = _mapping(item.get("evidence"), f"{context}.evidence") + checks.append(item) + result["checks"] = checks + blockers = _sequence(result.get("blockers"), "FeasibilityReport.blockers") + if any(not isinstance(item, str) or not item for item in blockers): + raise ValueError("FeasibilityReport.blockers must contain non-empty strings.") + result["blockers"] = list(blockers) + summary = _mapping(result.get("summary"), "FeasibilityReport.summary") + expected = set(ASSESSMENT_STATUSES) + if set(summary) != expected: + raise ValueError( + "FeasibilityReport.summary must count every assessment status." + ) + if any( + isinstance(value, bool) or not isinstance(value, int) or value < 0 + for value in summary.values() + ): + raise ValueError( + "FeasibilityReport.summary counts must be non-negative integers." + ) + if sum(summary.values()) != len(checks): + raise ValueError("FeasibilityReport.summary must match the check count.") + result["summary"] = dict(summary) + _json_safe(result, "FeasibilityReport") + return result + + +def _validate_affordance(value: Any, context: str) -> dict[str, Any]: + item = _mapping(value, context) + _exact_keys( + item, + { + "type", + "status", + "confidence", + "source", + "link_uid", + "frame", + "parameters", + }, + context, + ) + item["type"] = _nonempty(item.get("type"), f"{context}.type") + status = item.get("status") + if status not in _EVIDENCE_STATUSES: + raise ValueError( + f"{context}.status must be one of {sorted(_EVIDENCE_STATUSES)}." + ) + confidence = item.get("confidence") + if confidence is not None: + if ( + isinstance(confidence, bool) + or not isinstance(confidence, (int, float)) + or not math.isfinite(float(confidence)) + or not 0.0 <= float(confidence) <= 1.0 + ): + raise ValueError(f"{context}.confidence must be null or in [0, 1].") + confidence = float(confidence) + item["confidence"] = confidence + item["source"] = _nonempty(item.get("source"), f"{context}.source") + item["link_uid"] = _string(item.get("link_uid"), f"{context}.link_uid") + item["frame"] = _mapping(item.get("frame"), f"{context}.frame") + item["parameters"] = _mapping(item.get("parameters"), f"{context}.parameters") + return item + + +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)): + raise TypeError(f"{context} must be a sequence.") + return list(value) + + +def _exact_keys(value: Mapping[str, Any], expected: set[str], context: str) -> None: + if set(value) != expected: + missing = sorted(expected - set(value)) + extra = sorted(set(value) - expected) + raise ValueError(f"{context} fields differ; missing={missing}, extra={extra}.") + + +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}.") + + +def _nonempty(value: Any, context: str) -> str: + result = _string(value, context).strip() + if not result: + raise ValueError(f"{context} must not be empty.") + return result + + +def _string(value: Any, context: str) -> str: + if not isinstance(value, str): + raise TypeError(f"{context} must be a string.") + return value + + +def _status(value: Any, context: str) -> str: + if value not in ASSESSMENT_STATUSES: + raise ValueError(f"{context} must be one of {sorted(ASSESSMENT_STATUSES)}.") + return str(value) + + +def _bool_mapping(value: Any, context: str) -> dict[str, bool]: + result = _mapping(value, context) + if any( + not isinstance(key, str) or not isinstance(item, bool) + for key, item in result.items() + ): + raise TypeError(f"{context} must map strings to booleans.") + return result + + +def _json_safe(value: Any, context: str) -> None: + try: + json.dumps(value, allow_nan=False) + except (TypeError, ValueError) as exc: + raise ValueError(f"{context} must contain strict JSON data.") from exc diff --git a/embodichain/gen_sim/task_engine/scene/feasibility.py b/embodichain/gen_sim/task_engine/scene/feasibility.py new file mode 100644 index 000000000..db2a7a69d --- /dev/null +++ b/embodichain/gen_sim/task_engine/scene/feasibility.py @@ -0,0 +1,726 @@ +# ---------------------------------------------------------------------------- +# 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 task, scene, robot, and action-capability intersection.""" + +from __future__ import annotations + +from collections import Counter +from collections.abc import Mapping, Sequence +import math +from typing import Any + +from ..ontology import task_contract + +from .contracts import ( + ASSESSMENT_STATUSES, + FEASIBILITY_REPORT_SCHEMA, + FeasibilityReport, + validate_feasibility_report, + validate_static_scene_manifest, +) + +__all__ = ["FeasibilityBroker"] + + +_STATUS_PRIORITY = { + "proven": 0, + "runtime_probe": 1, + "unknown": 2, + "contradicted": 3, +} + + +class FeasibilityBroker: + """Produce an auditable compatibility report without repairing inputs.""" + + def assess( + self, + candidate: Mapping[str, Any], + role_bindings: Mapping[str, Any], + scene_manifest: Mapping[str, Any], + *, + capability_catalog: Mapping[str, Mapping[str, Any]], + task_actions: Mapping[str, Sequence[str]], + ) -> FeasibilityReport: + """Assess one grounded candidate against static and runtime capabilities.""" + manifest = validate_static_scene_manifest(scene_manifest) + draft = _mapping(candidate.get("draft"), "candidate.draft") + scene_request = _mapping( + candidate.get("scene_request"), "candidate.scene_request" + ) + bindings = role_bindings.get("reference_bindings", role_bindings) + bindings = _mapping(bindings, "role_bindings.reference_bindings") + objects = {item["uid"]: item for item in manifest["objects"]} + steps = { + str(item["id"]): item + for item in _sequence(draft.get("steps"), "candidate.draft.steps") + } + checks: list[dict[str, Any]] = [] + + for step_id, step in steps.items(): + task_type = str(step.get("task_type", "")) + actions = task_actions.get(task_type) + if not actions: + checks.append( + _check( + "task_capability", + step_id, + "contradicted", + f"Task type {task_type!r} has no registered action recipe.", + ) + ) + continue + for action_name in actions: + capability = capability_catalog.get(str(action_name)) + if capability is None: + checks.append( + _check( + "atomic_capability", + f"{step_id}:{action_name}", + "contradicted", + f"AtomicAction {action_name!r} is not registered.", + ) + ) + elif not bool(capability.get("runtime_available", False)): + checks.append( + _check( + "atomic_capability", + f"{step_id}:{action_name}", + "contradicted", + str( + capability.get("unavailable_reason") + or "Action is planning-only." + ), + evidence={"action": str(action_name)}, + ) + ) + else: + checks.append( + _check( + "atomic_capability", + f"{step_id}:{action_name}", + "proven", + "AtomicAction is registered and executable.", + evidence={"action": str(action_name)}, + ) + ) + if task_type == "E8": + reference_id = f"{step_id}.object" + raw_uids = bindings.get(reference_id, ()) + if not isinstance(raw_uids, Sequence) or isinstance( + raw_uids, (str, bytes) + ): + raw_uids = () + setting_maps = [] + for uid in raw_uids: + entity = objects.get(str(uid), {}) + attributes = entity.get("attributes", {}) + joint_settings = ( + attributes.get("joint_settings", {}) + if isinstance(attributes, Mapping) + else {} + ) + if isinstance(joint_settings, Mapping): + setting_maps.extend( + list(values) + for values in joint_settings.values() + if isinstance(values, Sequence) + and not isinstance(values, (str, bytes)) + and values + ) + for evidence in entity.get("affordances", ()): + if str(evidence.get("type")) != "turnable": + continue + parameters = evidence.get("parameters", {}) + values = ( + parameters.get("setting_values") + if isinstance(parameters, Mapping) + else None + ) + if ( + isinstance(values, Sequence) + and not isinstance(values, (str, bytes)) + and values + ): + setting_maps.append(list(values)) + checks.append( + _check( + "setting_mapping", + step_id, + "proven" if len(setting_maps) == 1 else "contradicted", + ( + "One explicit knob setting-to-angle map is available." + if len(setting_maps) == 1 + else "E8 requires exactly one explicit setting_values " + "map; ordinal knob settings cannot be inferred from " + "joint limits." + ), + evidence={"setting_map_count": len(setting_maps)}, + ) + ) + + for request in _sequence( + scene_request.get("references"), "candidate.scene_request.references" + ): + reference_id = str(request.get("reference_id", "")) + raw_uids = bindings.get(reference_id, ()) + if not isinstance(raw_uids, Sequence) or isinstance(raw_uids, (str, bytes)): + raw_uids = () + uids = [str(uid) for uid in raw_uids] + if not uids: + checks.append( + _check( + "binding", + reference_id, + "contradicted", + "Reference has no grounded scene entity.", + ) + ) + continue + for uid in uids: + entity = objects.get(uid) + if entity is None: + checks.append( + _check( + "binding", + f"{reference_id}:{uid}", + "contradicted", + "Binding references an entity absent from the static manifest.", + ) + ) + continue + checks.extend(self._entity_checks(request, entity, reference_id)) + + checks.extend(self._workspace_checks(steps, bindings, objects)) + + statuses = Counter(check["status"] for check in checks) + status = max( + (check["status"] for check in checks), + key=_STATUS_PRIORITY.__getitem__, + default="unknown", + ) + blockers = sorted( + { + f"{check['subject']}: {check['reason']}" + for check in checks + if check["status"] == "contradicted" + } + ) + return validate_feasibility_report( + { + "schema_version": FEASIBILITY_REPORT_SCHEMA, + "task_id": str(draft.get("task_id", "")), + "candidate_id": str(candidate.get("candidate_id", "")), + "scene_id": manifest["scene_id"], + "status": status, + "remediation_class": _remediation_class(checks), + "checks": checks, + "blockers": blockers, + "summary": { + name: int(statuses.get(name, 0)) + for name in sorted(ASSESSMENT_STATUSES) + }, + } + ) + + def _entity_checks( + self, + request: Mapping[str, Any], + entity: Mapping[str, Any], + reference_id: str, + ) -> list[dict[str, Any]]: + uid = str(entity["uid"]) + subject = f"{reference_id}:{uid}" + checks = [self._structure_check(request, entity, subject)] + evidence_by_type: dict[str, list[Mapping[str, Any]]] = {} + for evidence in entity["affordances"]: + evidence_by_type.setdefault(str(evidence["type"]), []).append(evidence) + for affordance in request.get("affordances", ()): + name = str(affordance) + checks.append( + self._affordance_check(name, evidence_by_type.get(name, ()), subject) + ) + for field_name in ("initial_state", "attributes"): + required = request.get(field_name, {}) + actual = entity.get(field_name, {}) + if isinstance(required, Mapping) and isinstance(actual, Mapping): + for key, expected in required.items(): + if key not in actual: + status = "unknown" + reason = f"Required {field_name} field {key!r} is not declared." + elif actual[key] != expected: + status = "contradicted" + reason = f"Required {field_name} field {key!r} conflicts with the scene." + else: + status = "proven" + reason = f"Required {field_name} field {key!r} matches." + checks.append( + _check( + field_name, + subject, + status, + reason, + evidence={"field": str(key)}, + ) + ) + if str(request.get("role")) == "object": + checks.append( + _check( + "runtime_reachability", + subject, + "runtime_probe", + "Reachability, collision, and grasp geometry require live planning.", + ) + ) + if ( + str(request.get("role")) == "target" + and str(request.get("source_structure")) == "physical_entity" + ): + checks.append( + _check( + "placement_support", + subject, + "runtime_probe", + "Support depends on the payload, candidate pose, live geometry, " + "and post-release stability.", + evidence={ + "runtime_obligations": [ + "placement_candidates", + "object_supported_by", + "stable_for", + "final_support_revalidation", + ] + }, + ) + ) + return checks + + def _workspace_checks( + self, + steps: Mapping[str, Mapping[str, Any]], + bindings: Mapping[str, Any], + objects: Mapping[str, Mapping[str, Any]], + ) -> list[dict[str, Any]]: + """Defer arm-side compatibility to the live robot frame.""" + checks: list[dict[str, Any]] = [] + object_uids_by_step: dict[str, tuple[str, ...]] = {} + phases: list[dict[str, Any]] = [] + for step_id, step in steps.items(): + object_uids = _step_selector_uids( + step_id, + "object", + step.get("object"), + bindings, + object_uids_by_step, + ) + object_uids_by_step[step_id] = object_uids + target_uids = _step_selector_uids( + step_id, + "target", + step.get("target"), + bindings, + object_uids_by_step, + ) + task_type = str(step.get("task_type", "")) + contract = task_contract(task_type) + required_arm = str(step.get("required_arm", "auto")) + if contract.resource_mode == "handover": + required_arm = str(step.get("transfer_arm", "none")) + if required_arm in {"left_arm", "right_arm"}: + for uid in object_uids: + entity = objects.get(uid) + position = ( + entity.get("initial_pose", {}).get("position", ()) + if isinstance(entity, Mapping) + and isinstance(entity.get("initial_pose"), Mapping) + else () + ) + if ( + not isinstance(position, Sequence) + or isinstance(position, (str, bytes, bytearray)) + or len(position) < 2 + ): + continue + checks.append( + _check( + "arm_layout_risk", + f"{step_id}:{uid}", + "runtime_probe", + "Arm-side compatibility requires live left/right arm-base " + "poses and workspace geometry.", + evidence={ + "required_arm": required_arm, + "object_world_position": [ + float(position[0]), + float(position[1]), + ], + "arm_side_frame": "live_robot", + "mismatch_risk": None, + "geometry_certificate": False, + }, + ) + ) + + phases.extend( + _workflow_phases( + step_id, + object_uids, + target_uids, + resource_mode=contract.resource_mode, + moves_primary_object=contract.moves_primary_object, + transfer_arm=str(step.get("transfer_arm", "none")), + receive_arm=str(step.get("receive_arm", "none")), + ) + ) + if phases: + checks.append( + _check( + "task_workspace", + "task_workflow", + "runtime_probe", + "Scene layout must satisfy pickup, transfer, placement, and " + "safety-clearance phases across the complete task workflow.", + evidence={ + "arm_side_frame": "live_robot", + "phases": phases, + "geometry_certificate": False, + }, + ) + ) + return checks + + @staticmethod + def _structure_check( + request: Mapping[str, Any], + entity: Mapping[str, Any], + subject: str, + ) -> dict[str, Any]: + expected = str(request.get("source_structure", "")) + role = str(entity.get("role", "")) + if expected in {"scene_entity", "spatial_reference"}: + if role in {"camera", "light", "robot", "sensor"}: + return _check( + "structure", + subject, + "contradicted", + f"Scene entity role {role!r} cannot be a spatial action target.", + evidence={"static_pose": _has_static_pose(entity)}, + ) + if not _has_static_pose(entity): + return _check( + "structure", + subject, + "unknown", + "Static scene evidence does not provide a finite spatial pose.", + evidence={"static_pose": False}, + ) + if role == "articulation": + return _check( + "structure", + subject, + "runtime_probe", + "Articulation has a static pose, but live spatial target lookup " + "must be validated at runtime.", + evidence={ + "static_pose": True, + "runtime_entity_kind": "articulation", + }, + ) + has_runtime_body = bool(entity.get("physics")) + if ( + role + in { + "background", + "object", + "rigid_object", + "support_surface", + "table", + } + and has_runtime_body + ): + return _check( + "structure", + subject, + "proven", + "Scene entity has a static pose and a rigid runtime body.", + evidence={ + "static_pose": True, + "runtime_entity_kind": "rigid_object", + }, + ) + return _check( + "structure", + subject, + "runtime_probe", + "Scene entity has a static pose, but its live target interface is " + "not proven by the static manifest.", + evidence={"static_pose": True, "runtime_entity_kind": "unknown"}, + ) + if expected == "physical_entity": + geometry = entity.get("geometry", {}) + shape = geometry.get("shape", {}) if isinstance(geometry, Mapping) else {} + asset_sha256 = ( + geometry.get("asset_sha256", "") + if isinstance(geometry, Mapping) + else "" + ) + physics = entity.get("physics", {}) + articulation = entity.get("articulation", {}) + has_physical_geometry = bool(shape) or bool(asset_sha256) + has_runtime_body = bool(physics) or bool(articulation) + if role in {"camera", "light", "sensor"}: + return _check( + "structure", + subject, + "contradicted", + f"Scene entity role {role!r} is not a physical collision body.", + evidence={"physical_geometry": False, "runtime_body": False}, + ) + if role == "articulation" or bool(articulation): + return _check( + "structure", + subject, + "contradicted", + "Placement on an articulation requires a link-level target " + "interface that the current runtime does not provide.", + evidence={ + "physical_geometry": has_physical_geometry, + "runtime_body": bool(articulation), + "runtime_entity_kind": "articulation", + "runtime_target_interface": False, + }, + ) + if has_physical_geometry and has_runtime_body: + return _check( + "structure", + subject, + "proven", + "Scene entity has physical geometry and a runtime body.", + evidence={"physical_geometry": True, "runtime_body": True}, + ) + return _check( + "structure", + subject, + "unknown", + "Static scene evidence does not prove physical geometry and a " + "runtime body required for placement.", + evidence={ + "physical_geometry": has_physical_geometry, + "runtime_body": has_runtime_body, + }, + ) + accepted_by_structure = { + "articulation": {"articulation"}, + "rigid_object": {"object", "rigid_object"}, + "movable": {"object", "rigid_object"}, + "support_surface": {"background", "support_surface", "table"}, + } + accepted = accepted_by_structure.get(expected) + if accepted is None: + return _check( + "structure", + subject, + "unknown", + f"Structure contract {expected!r} is not recognized by the broker.", + evidence={"scene_role": role}, + ) + if role in accepted: + return _check( + "structure", + subject, + "proven", + f"Scene role {role!r} satisfies structure {expected!r}.", + ) + return _check( + "structure", + subject, + "contradicted", + f"Scene role {role!r} does not satisfy structure {expected!r}.", + ) + + @staticmethod + def _affordance_check( + name: str, + evidence: Sequence[Mapping[str, Any]], + subject: str, + ) -> dict[str, Any]: + if not evidence: + return _check( + "affordance", + subject, + "unknown", + f"Affordance {name!r} has no evidence.", + evidence={"affordance": name}, + ) + statuses = {str(item.get("status")) for item in evidence} + if statuses == {"contradicted"}: + status = "contradicted" + reason = f"Affordance {name!r} is explicitly contradicted." + elif "verified" in statuses: + status = "proven" + reason = f"Affordance {name!r} has verified evidence." + else: + status = "runtime_probe" + reason = ( + f"Affordance {name!r} is declared but requires physical validation." + ) + return _check( + "affordance", + subject, + status, + reason, + evidence={ + "affordance": name, + "sources": sorted({str(item.get("source")) for item in evidence}), + }, + ) + + +def _has_static_pose(entity: Mapping[str, Any]) -> bool: + pose = entity.get("initial_pose") + if not isinstance(pose, Mapping): + return False + position = pose.get("position") + if ( + not isinstance(position, Sequence) + or isinstance(position, (str, bytes, bytearray)) + or len(position) != 3 + ): + return False + return all( + not isinstance(value, bool) + and isinstance(value, (int, float)) + and math.isfinite(float(value)) + for value in position + ) + + +def _step_selector_uids( + step_id: str, + role: str, + selector: Any, + bindings: Mapping[str, Any], + object_uids_by_step: Mapping[str, tuple[str, ...]], +) -> tuple[str, ...]: + """Resolve direct and prior-step selectors for static workspace advice.""" + raw = bindings.get(f"{step_id}.{role}", ()) + if isinstance(raw, Sequence) and not isinstance(raw, (str, bytes, bytearray)): + direct = tuple(str(uid) for uid in raw if str(uid)) + if direct: + return direct + if not isinstance(selector, Mapping) or selector.get("kind") != "step_result": + return () + source_step = str(selector.get("step_id", "")) + return tuple(object_uids_by_step.get(source_step, ())) + + +def _workflow_phases( + step_id: str, + object_uids: Sequence[str], + target_uids: Sequence[str], + *, + resource_mode: str, + moves_primary_object: bool, + transfer_arm: str, + receive_arm: str, +) -> list[dict[str, Any]]: + """Describe whole-task layout anchors without inventing geometry bounds.""" + phases: list[dict[str, Any]] = [] + if object_uids: + phases.append( + { + "step_id": step_id, + "phase": "pickup", + "object_uids": list(object_uids), + } + ) + if resource_mode == "handover": + phases.append( + { + "step_id": step_id, + "phase": "handover_shared_workspace", + "object_uids": list(object_uids), + "transfer_arm": transfer_arm, + "receive_arm": receive_arm, + } + ) + if target_uids: + phases.append( + { + "step_id": step_id, + "phase": "target_interaction", + "object_uids": list(object_uids), + "target_uids": list(target_uids), + } + ) + if moves_primary_object: + phases.append( + { + "step_id": step_id, + "phase": "safety_clearance", + "object_uids": list(object_uids), + } + ) + return phases + + +def _check( + kind: str, + subject: str, + status: str, + reason: str, + *, + evidence: Mapping[str, Any] | None = None, +) -> dict[str, Any]: + return { + "kind": kind, + "subject": subject, + "status": status, + "reason": reason, + "evidence": dict(evidence or {}), + } + + +def _remediation_class(checks: Sequence[Mapping[str, Any]]) -> str: + """Classify contradictions by the subsystem capable of changing them.""" + contradicted = [check for check in checks if check.get("status") == "contradicted"] + if not contradicted: + return "none" + kinds = {str(check.get("kind", "")) for check in contradicted} + if kinds.intersection({"task_capability", "atomic_capability"}): + return "action_capability" + # A new materialization seed can change observed pose/orientation, but it + # cannot change task semantics, entity roles, bindings, or declared affordances. + if kinds <= {"initial_state"}: + return "scene_remediable" + if kinds.intersection({"binding", "structure", "affordance", "attributes"}): + return "input_conflict" + return "terminal" + + +def _mapping(value: Any, context: str) -> dict[str, Any]: + if not isinstance(value, Mapping): + raise TypeError(f"{context} must be a mapping.") + return dict(value) + + +def _sequence(value: Any, context: str) -> list[Mapping[str, Any]]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)): + raise TypeError(f"{context} must be a sequence.") + if any(not isinstance(item, Mapping) for item in value): + raise TypeError(f"{context} must contain mappings.") + return list(value) diff --git a/embodichain/gen_sim/task_engine/scene/final_inspection.py b/embodichain/gen_sim/task_engine/scene/final_inspection.py new file mode 100644 index 000000000..4956fedf8 --- /dev/null +++ b/embodichain/gen_sim/task_engine/scene/final_inspection.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. +# ---------------------------------------------------------------------------- + +"""Geometry-derived evidence from one completed scene revision.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +from dataclasses import replace +import json +from pathlib import Path +from typing import Any, Final, TypeAlias + +import numpy as np +from scipy.spatial.transform import Rotation +import trimesh + +from embodichain.gen_sim.task_engine.orchestration.source_scene import ( + PreparedScene, + prepare_scene, + resolve_source_scene, +) + +__all__ = [ + "FINAL_SCENE_INSPECTION_SCHEMA", + "FinalSceneInspection", + "apply_final_inspection", + "inspect_final_scene", + "validate_final_scene_inspection", +] + +FINAL_SCENE_INSPECTION_SCHEMA: Final = "embodichain.final-scene-inspection/v1" +FinalSceneInspection: TypeAlias = dict[str, Any] + +_Y_UP_TO_Z_UP = np.array( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]], + dtype=float, +) + + +def inspect_final_scene( + source: str | Path, + *, + revision_id: str, + contact_tolerance_m: float = 0.03, +) -> FinalSceneInspection: + """Measure final AABBs, orientation, and support from exported geometry. + + Args: + source: Completed scene project or configuration path. + revision_id: Content identity already assigned to the completed revision. + contact_tolerance_m: Maximum support-surface contact gap in meters. + + Returns: + Strict geometry-derived final inspection document. + """ + tolerance = float(contact_tolerance_m) + if not np.isfinite(tolerance) or tolerance <= 0.0: + raise ValueError("contact_tolerance_m must be positive and finite.") + normalized_revision_id = str(revision_id) + if len(normalized_revision_id) != 64: + raise ValueError("revision_id must be a SHA-256 hexadecimal digest.") + try: + int(normalized_revision_id, 16) + except ValueError as exc: + raise ValueError("revision_id must be a SHA-256 hexadecimal digest.") from exc + resolved = resolve_source_scene(source) + prepared = prepare_scene(source) + runtime = { + str(item.get("uid")): item + for item in ( + *prepared.background, + *prepared.rigid_objects, + *prepared.articulations, + ) + if isinstance(item, Mapping) and item.get("uid") + } + measured: dict[str, dict[str, Any]] = {} + for raw in prepared.planner_objects: + uid = str(raw.get("uid", "")) + role = str(raw.get("role", "")) + geometry = _measure_geometry( + runtime.get(uid, raw), + convert_y_up=resolved.is_prompt2scene, + ) + measured[uid] = { + "uid": uid, + "role": role, + "orientation": _orientation(geometry), + "support": { + "parent_uid": None if uid == "table" else "unknown", + "relation": "root" if uid == "table" else "unknown", + "confidence": 1.0 if uid == "table" else None, + "gap_m": None, + "xy_overlap_ratio": None, + }, + "world_aabb": ( + None + if geometry is None + else { + "min": geometry["bounds"][0].tolist(), + "max": geometry["bounds"][1].tolist(), + } + ), + "evidence": { + "source": "final_geometry" if geometry is not None else "unmeasured", + "method": "world_aabb_and_dominant_axis", + }, + } + + for uid, item in measured.items(): + child_geometry = _geometry_from_record(item) + if uid == "table" or child_geometry is None: + continue + support = _support_for( + uid, + child_geometry, + measured, + tolerance=tolerance, + ) + if support is not None: + item["support"] = support + + return validate_final_scene_inspection( + { + "schema_version": FINAL_SCENE_INSPECTION_SCHEMA, + "scene_revision_id": normalized_revision_id, + "source_config_path": prepared.source_config_path.as_posix(), + "contact_tolerance_m": tolerance, + "objects": [measured[uid] for uid in sorted(measured)], + } + ) + + +def apply_final_inspection( + prepared_scene: PreparedScene, + inspection: Mapping[str, Any], +) -> PreparedScene: + """Return a detached PreparedScene enriched with measured final evidence. + + Args: + prepared_scene: Normalized scene to enrich without mutation. + inspection: Validated or raw final inspection mapping. + + Returns: + Prepared scene whose semantic state reflects measured final geometry. + """ + normalized = validate_final_scene_inspection(inspection) + by_uid = {str(item["uid"]): item for item in normalized["objects"]} + planner_objects = [] + for raw in prepared_scene.planner_objects: + item = deepcopy(raw) + evidence = by_uid.get(str(item.get("uid"))) + if evidence is not None: + initial_state = deepcopy(dict(item.get("initial_state", {}))) + initial_state.pop("orientation", None) + if evidence["orientation"] == "standing": + initial_state["orientation"] = "upright" + elif evidence["orientation"] == "lying": + initial_state["orientation"] = "fallen" + attributes = deepcopy(dict(item.get("attributes", {}))) + attributes["final_support"] = deepcopy(evidence["support"]) + attributes["final_world_aabb"] = deepcopy(evidence["world_aabb"]) + item["initial_state"] = initial_state + item["attributes"] = attributes + planner_objects.append(item) + return replace(prepared_scene, planner_objects=tuple(planner_objects)) + + +def validate_final_scene_inspection( + value: Mapping[str, Any], +) -> FinalSceneInspection: + """Validate and detach one final scene inspection document. + + Args: + value: Inspection mapping to validate. + + Returns: + Detached, normalized inspection document. + """ + if not isinstance(value, Mapping): + raise TypeError("FinalSceneInspection must be a mapping.") + result = deepcopy(dict(value)) + expected = { + "schema_version", + "scene_revision_id", + "source_config_path", + "contact_tolerance_m", + "objects", + } + if set(result) != expected: + raise ValueError("FinalSceneInspection fields are invalid.") + if result.get("schema_version") != FINAL_SCENE_INSPECTION_SCHEMA: + raise ValueError("FinalSceneInspection schema version is invalid.") + revision_id = result.get("scene_revision_id") + if not isinstance(revision_id, str) or len(revision_id) != 64: + raise ValueError("FinalSceneInspection.scene_revision_id is invalid.") + try: + int(revision_id, 16) + except ValueError as exc: + raise ValueError("FinalSceneInspection.scene_revision_id is invalid.") from exc + source_path = result.get("source_config_path") + if not isinstance(source_path, str) or not source_path: + raise ValueError("FinalSceneInspection.source_config_path is invalid.") + tolerance = result.get("contact_tolerance_m") + if ( + isinstance(tolerance, bool) + or not isinstance(tolerance, (int, float)) + or not np.isfinite(float(tolerance)) + or float(tolerance) <= 0.0 + ): + raise ValueError("FinalSceneInspection.contact_tolerance_m is invalid.") + objects = result.get("objects") + if not isinstance(objects, Sequence) or isinstance(objects, (str, bytes)): + raise TypeError("FinalSceneInspection.objects must be a sequence.") + normalized = [_validate_object(item, index) for index, item in enumerate(objects)] + if len({item["uid"] for item in normalized}) != len(normalized): + raise ValueError("FinalSceneInspection object UIDs must be unique.") + result["objects"] = normalized + result["contact_tolerance_m"] = float(tolerance) + json.dumps(result, ensure_ascii=False, allow_nan=False) + return result + + +def _validate_object(value: Any, index: int) -> dict[str, Any]: + if not isinstance(value, Mapping): + raise TypeError(f"FinalSceneInspection.objects[{index}] must be a mapping.") + item = deepcopy(dict(value)) + expected = {"uid", "role", "orientation", "support", "world_aabb", "evidence"} + if set(item) != expected: + raise ValueError(f"FinalSceneInspection.objects[{index}] fields are invalid.") + if not isinstance(item["uid"], str) or not item["uid"]: + raise ValueError(f"FinalSceneInspection.objects[{index}].uid is invalid.") + if not isinstance(item["role"], str) or not item["role"]: + raise ValueError(f"FinalSceneInspection.objects[{index}].role is invalid.") + if item["orientation"] not in {"standing", "lying", "unknown"}: + raise ValueError( + f"FinalSceneInspection.objects[{index}].orientation is invalid." + ) + if not isinstance(item["support"], Mapping) or not isinstance( + item["evidence"], Mapping + ): + raise TypeError("FinalSceneInspection support and evidence must be mappings.") + support = deepcopy(dict(item["support"])) + if set(support) != { + "parent_uid", + "relation", + "confidence", + "gap_m", + "xy_overlap_ratio", + }: + raise ValueError("FinalSceneInspection support fields are invalid.") + if support["parent_uid"] is not None and not isinstance(support["parent_uid"], str): + raise TypeError("FinalSceneInspection support parent_uid is invalid.") + if support["relation"] not in {"root", "on", "unknown"}: + raise ValueError("FinalSceneInspection support relation is invalid.") + for field_name in ("confidence", "gap_m", "xy_overlap_ratio"): + field_value = support[field_name] + if field_value is not None and ( + isinstance(field_value, bool) + or not isinstance(field_value, (int, float)) + or not np.isfinite(float(field_value)) + ): + raise ValueError(f"FinalSceneInspection support {field_name} is invalid.") + if ( + support["confidence"] is not None + and not 0.0 <= float(support["confidence"]) <= 1.0 + ): + raise ValueError("FinalSceneInspection support confidence is invalid.") + if ( + support["xy_overlap_ratio"] is not None + and not 0.0 <= float(support["xy_overlap_ratio"]) <= 1.0 + 1.0e-6 + ): + raise ValueError("FinalSceneInspection support overlap is invalid.") + item["support"] = support + aabb = item["world_aabb"] + if aabb is not None: + if not isinstance(aabb, Mapping) or set(aabb) != {"min", "max"}: + raise ValueError("FinalSceneInspection world_aabb is invalid.") + if aabb["min"] is None or aabb["max"] is None: + raise ValueError("FinalSceneInspection world_aabb vectors are invalid.") + minimum = _vector(aabb["min"], default=(0.0, 0.0, 0.0)) + maximum = _vector(aabb["max"], default=(0.0, 0.0, 0.0)) + if np.any(np.asarray(maximum) < np.asarray(minimum)): + raise ValueError("FinalSceneInspection world_aabb bounds are inverted.") + item["world_aabb"] = {"min": minimum, "max": maximum} + evidence = deepcopy(dict(item["evidence"])) + if set(evidence) != {"source", "method"} or any( + not isinstance(evidence[key], str) or not evidence[key] for key in evidence + ): + raise ValueError("FinalSceneInspection evidence is invalid.") + item["evidence"] = evidence + return item + + +def _measure_geometry( + entry: Mapping[str, Any], + *, + convert_y_up: bool, +) -> dict[str, Any] | None: + shape = entry.get("shape") + if not isinstance(shape, Mapping): + return None + shape_type = str(shape.get("shape_type", "")) + if shape_type == "Mesh": + path = Path(str(shape.get("fpath", ""))).expanduser().resolve() + if not path.is_file(): + return None + loaded = trimesh.load(path, force="scene") + mesh = loaded.to_geometry() + elif shape_type == "Cube": + mesh = trimesh.creation.box( + extents=_vector(shape.get("size"), default=(1, 1, 1)) + ) + elif shape_type == "Sphere": + radius = float(shape.get("radius", 1.0)) + mesh = trimesh.creation.icosphere(radius=radius) + else: + return None + scale = np.asarray(_vector(entry.get("body_scale"), default=(1, 1, 1))) + mesh.apply_scale(scale) + local_extents = np.asarray(mesh.extents, dtype=float) + conversion = _Y_UP_TO_Z_UP if convert_y_up else np.eye(3) + rotation = Rotation.from_euler( + "XYZ", + _vector(entry.get("init_rot"), default=(0, 0, 0)), + degrees=True, + ).as_matrix() + transform = np.eye(4) + transform[:3, :3] = rotation @ conversion + transform[:3, 3] = _vector(entry.get("init_pos"), default=(0, 0, 0)) + mesh.apply_transform(transform) + return { + "bounds": np.asarray(mesh.bounds, dtype=float), + "local_extents": local_extents, + "axis_transform": transform[:3, :3], + "shape_type": shape_type, + } + + +def _orientation(geometry: Mapping[str, Any] | None) -> str: + if geometry is None or geometry["shape_type"] == "Sphere": + return "unknown" + extents = np.asarray(geometry["local_extents"], dtype=float) + ordered = np.sort(extents) + if ordered[-1] <= 0.0 or ordered[-1] / max(ordered[-2], 1.0e-9) < 1.2: + return "unknown" + dominant = int(np.argmax(extents)) + axis = np.asarray(geometry["axis_transform"], dtype=float)[:, dominant] + vertical = abs(float(axis[2])) / max(float(np.linalg.norm(axis)), 1.0e-9) + if vertical >= 0.75: + return "standing" + if vertical <= 0.35: + return "lying" + return "unknown" + + +def _geometry_from_record(item: Mapping[str, Any]) -> np.ndarray | None: + aabb = item.get("world_aabb") + if not isinstance(aabb, Mapping): + return None + return np.asarray([aabb["min"], aabb["max"]], dtype=float) + + +def _support_for( + uid: str, + child: np.ndarray, + objects: Mapping[str, Mapping[str, Any]], + *, + tolerance: float, +) -> dict[str, Any] | None: + child_bottom = float(child[0, 2]) + child_area = max( + float((child[1, 0] - child[0, 0]) * (child[1, 1] - child[0, 1])), + 1.0e-9, + ) + candidates = [] + for parent_uid, parent_item in objects.items(): + if parent_uid == uid: + continue + parent = _geometry_from_record(parent_item) + if parent is None: + continue + overlap_x = max( + 0.0, min(child[1, 0], parent[1, 0]) - max(child[0, 0], parent[0, 0]) + ) + overlap_y = max( + 0.0, min(child[1, 1], parent[1, 1]) - max(child[0, 1], parent[0, 1]) + ) + overlap_ratio = float(overlap_x * overlap_y / child_area) + gap = child_bottom - float(parent[1, 2]) + if overlap_ratio >= 0.1 and -tolerance <= gap <= tolerance: + candidates.append((overlap_ratio, -abs(gap), parent_uid, gap)) + if not candidates: + return None + overlap_ratio, _, parent_uid, gap = max(candidates) + confidence = min(1.0, overlap_ratio * max(0.0, 1.0 - abs(gap) / tolerance)) + return { + "parent_uid": parent_uid, + "relation": "on", + "confidence": float(confidence), + "gap_m": float(gap), + "xy_overlap_ratio": float(overlap_ratio), + } + + +def _vector(value: Any, *, default: tuple[float, float, float]) -> list[float]: + raw = default if value is None else value + if not isinstance(raw, Sequence) or isinstance(raw, (str, bytes)): + raise TypeError("Scene geometry vectors must be sequences.") + result = [float(item) for item in raw] + if len(result) != 3 or not np.all(np.isfinite(result)): + raise ValueError("Scene geometry vectors must contain three finite values.") + return result diff --git a/embodichain/gen_sim/task_engine/scene/scene_engine_v1.py b/embodichain/gen_sim/task_engine/scene/scene_engine_v1.py new file mode 100644 index 000000000..af0c4a47c --- /dev/null +++ b/embodichain/gen_sim/task_engine/scene/scene_engine_v1.py @@ -0,0 +1,242 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Adapt existing Scene Engine exports without changing their source schema.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +import hashlib +import json +from pathlib import Path +from typing import Any + +from .contracts import ( + STATIC_SCENE_MANIFEST_SCHEMA, + StaticSceneManifest, + validate_static_scene_manifest, +) + +__all__ = ["SceneEngineV1Adapter"] + + +class SceneEngineV1Adapter: + """Convert a normalized Scene Engine v1 export to the neutral manifest.""" + + def adapt_prepared_scene( + self, + prepared_scene: Any, + *, + source_format: str, + robot_profile: str, + ) -> StaticSceneManifest: + """Adapt the existing prepared-scene view through a duck-typed boundary.""" + planner_objects = tuple(getattr(prepared_scene, "planner_objects")) + runtime_objects = ( + tuple(getattr(prepared_scene, "background", ())) + + tuple(getattr(prepared_scene, "rigid_objects", ())) + + tuple(getattr(prepared_scene, "articulations", ())) + ) + runtime_by_uid = { + str(item.get("uid")): item + for item in runtime_objects + if isinstance(item, Mapping) and item.get("uid") + } + asset_hashes = dict(getattr(prepared_scene, "asset_hashes", {}) or {}) + objects = [ + self._object_manifest( + raw, + runtime=runtime_by_uid.get(str(raw.get("uid")), {}), + asset_sha256=str(asset_hashes.get(str(raw.get("uid")), "")), + ) + for raw in planner_objects + ] + identity = { + "source_format": str(source_format), + "robot_profile": str(robot_profile), + "objects": [_identity_object(item) for item in objects], + } + source_path = Path(getattr(prepared_scene, "source_config_path")) + return validate_static_scene_manifest( + { + "schema_version": STATIC_SCENE_MANIFEST_SCHEMA, + "scene_id": _canonical_hash(identity), + "source_format": str(source_format), + "robot_profile": str(robot_profile), + "source": { + "adapter": f"{type(self).__module__}.{type(self).__qualname__}", + "config_path": source_path.expanduser().resolve().as_posix(), + "config_sha256": _file_hash(source_path), + "asset_hashes": asset_hashes, + }, + "adapter_capabilities": { + "task_conditioned_generation": False, + "structured_affordances": any( + bool(item["affordances"]) for item in objects + ), + "articulation_instances": any( + item["role"] == "articulation" for item in objects + ), + "runtime_scene_observation": False, + }, + "objects": objects, + } + ) + + def _object_manifest( + self, + raw: Mapping[str, Any], + *, + runtime: Mapping[str, Any], + asset_sha256: str, + ) -> dict[str, Any]: + uid = str(raw.get("uid", "")).strip() + role = str(raw.get("role", "")).strip() + shape = raw.get("shape", runtime.get("shape", {})) + shape = deepcopy(dict(shape)) if isinstance(shape, Mapping) else {} + physics_keys = ("attrs", "body_type", "max_convex_hull_num") + physics = { + key: deepcopy(runtime[key]) for key in physics_keys if key in runtime + } + articulation = deepcopy(dict(runtime)) if role == "articulation" else {} + affordances = _affordance_evidence(raw.get("affordances", ())) + if role in {"background", "table", "support_surface"}: + affordances = _with_structural_evidence( + affordances, + "support_surface", + ) + if role in {"object", "rigid_object"}: + affordances = _with_structural_evidence(affordances, "rigid") + return { + "uid": uid, + "source_uid": str(raw.get("source_uid", "")), + "role": role, + "name": str(raw.get("name", "")), + "description": str(raw.get("description", "")), + "category": str(raw.get("category", "")), + "color": raw.get("color") if isinstance(raw.get("color"), str) else None, + "geometry": { + "shape": shape, + "asset_sha256": asset_sha256, + }, + "initial_pose": { + "position": deepcopy(list(raw.get("init_pos", ()))), + "rotation": deepcopy(list(raw.get("init_rot", ()))), + "scale": deepcopy(list(raw.get("body_scale", ()))), + }, + "physics": physics, + "articulation": articulation, + "affordances": affordances, + "initial_state": _mapping_or_empty(raw.get("initial_state")), + "attributes": _mapping_or_empty(raw.get("attributes")), + "provenance": { + "semantic_source": "scene_export", + "geometry_source": "prepared_scene", + "physics_source": "prepared_scene_runtime", + }, + } + + +def _affordance_evidence(value: Any) -> list[dict[str, Any]]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)): + return [] + result: list[dict[str, Any]] = [] + for raw in value: + if isinstance(raw, str) and raw.strip(): + result.append(_evidence(raw.strip(), status="declared")) + continue + if not isinstance(raw, Mapping): + continue + affordance_type = str(raw.get("type", raw.get("name", ""))).strip() + if not affordance_type: + continue + status = str(raw.get("status", "declared")) + result.append( + { + "type": affordance_type, + "status": status, + "confidence": raw.get("confidence"), + "source": str(raw.get("source", "scene_export")), + "link_uid": str(raw.get("link_uid", "")), + "frame": _mapping_or_empty(raw.get("frame")), + "parameters": _mapping_or_empty(raw.get("parameters")), + } + ) + return sorted(result, key=lambda item: (item["type"], item["source"])) + + +def _with_structural_evidence( + evidence: list[dict[str, Any]], affordance_type: str +) -> list[dict[str, Any]]: + if any(item["type"] == affordance_type for item in evidence): + return evidence + return sorted( + [ + *evidence, + _evidence(affordance_type, status="verified", source="adapter_structure"), + ], + key=lambda item: (item["type"], item["source"]), + ) + + +def _evidence( + affordance_type: str, + *, + status: str, + source: str = "scene_export", +) -> dict[str, Any]: + return { + "type": affordance_type, + "status": status, + "confidence": None, + "source": source, + "link_uid": "", + "frame": {}, + "parameters": {}, + } + + +def _mapping_or_empty(value: Any) -> dict[str, Any]: + return deepcopy(dict(value)) if isinstance(value, Mapping) else {} + + +def _identity_object(value: Mapping[str, Any]) -> dict[str, Any]: + result = deepcopy(dict(value)) + geometry = result.get("geometry") + if isinstance(geometry, dict): + shape = geometry.get("shape") + if isinstance(shape, dict) and geometry.get("asset_sha256"): + shape.pop("fpath", None) + return result + + +def _canonical_hash(value: Any) -> str: + payload = json.dumps( + value, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode("utf-8") + return hashlib.sha256(payload).hexdigest() + + +def _file_hash(path: Path) -> str: + resolved = path.expanduser().resolve() + if not resolved.is_file(): + return "" + return hashlib.sha256(resolved.read_bytes()).hexdigest() diff --git a/embodichain/gen_sim/task_engine/scene_backend.py b/embodichain/gen_sim/task_engine/scene_backend.py new file mode 100644 index 000000000..c6de15592 --- /dev/null +++ b/embodichain/gen_sim/task_engine/scene_backend.py @@ -0,0 +1,475 @@ +# ---------------------------------------------------------------------------- +# 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-owned adapter for Scene Engine analysis, revisions, and edits.""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass, replace +from pathlib import Path +import json +import shutil +from typing import Any + +from embodichain.gen_sim.task_engine.orchestration.source_scene import ( + resolve_source_scene, +) +from embodichain.gen_sim.scene_engine.pipeline import ( + SCENE_BLUEPRINT_SCHEMA, + SceneBlueprintPackage, + SceneMaterialization, + analyze_edit, + analyze_image, + materialize_blueprint, + materialize_edit, +) + +from .orchestration.legacy_scene import ( + convert_legacy_gym_project, + restore_locked_scene_entities, +) +from .orchestration.scene_adapter import CandidateSelection, SceneAdapter +from .orchestration.scene_source import ( + SceneSourceFingerprint, + fingerprint_scene_source, + scene_revision_id, + verify_scene_source_fingerprint, +) +from .scene.final_inspection import FinalSceneInspection, inspect_final_scene +from .workflow_contracts import TaskRunRequest, scene_input_kind + +__all__ = [ + "SceneRemediableError", + "SceneAnalysis", + "SceneEngineBackend", + "SceneRevision", + "scene_blueprint_objects", +] + +_LOCKED_SCENE_MANIFEST = "locked_scene_entities.json" + + +class SceneRemediableError(RuntimeError): + """A Scene output failure that permits a fresh materialization attempt.""" + + +@dataclass(frozen=True) +class SceneAnalysis: + """Scene semantics available before asset materialization.""" + + input_kind: str + source: Path + blueprint: SceneBlueprintPackage | None + source_fingerprint: SceneSourceFingerprint | None + + +@dataclass(frozen=True) +class SceneRevision: + """One immutable scene source selected for final Action preparation.""" + + source: Path + output_root: Path | None + revision_id: str + seed: int + edit_plan: dict[str, Any] | None + source_fingerprint: SceneSourceFingerprint | None + + +class SceneEngineBackend: + """Expose Scene Engine stages without giving it workflow ownership.""" + + def analyze( + self, + request: TaskRunRequest, + output_root: str | Path, + ) -> SceneAnalysis: + """Analyze an image or fingerprint an existing read-only project. + + Args: + request: Validated Task Engine run request. + output_root: Directory for image-understanding artifacts. + + Returns: + Scene semantics and immutable source provenance. + """ + root = Path(output_root).expanduser().resolve() + if scene_input_kind(request) == "image": + image_path = Path(str(request["image_path"])).resolve() + blueprint = analyze_image(image_path, root) + return SceneAnalysis( + input_kind="image", + source=image_path, + blueprint=blueprint, + source_fingerprint=None, + ) + source = Path(str(request["gym_project"])).resolve() + return SceneAnalysis( + input_kind="gym_project", + source=source, + blueprint=None, + source_fingerprint=fingerprint_scene_source(source), + ) + + def select( + self, + analysis: SceneAnalysis, + candidate_set: Mapping[str, Any], + scene_adapter: SceneAdapter, + *, + force_most_likely: bool, + ) -> CandidateSelection: + """Select a task candidate from blueprint or existing-scene semantics. + + Args: + analysis: Pre-materialization scene analysis. + candidate_set: Task candidates to ground and vote. + scene_adapter: Task-owned semantic binding adapter. + force_most_likely: Whether ranked UID hypotheses must be resolved. + + Returns: + Audited initial candidate selection. + """ + if analysis.blueprint is not None: + return scene_adapter.select_objects( + candidate_set, + scene_blueprint_objects(analysis.blueprint), + source_format=analysis.blueprint.schema_version, + force_most_likely=force_most_likely, + ) + adaptation = scene_adapter.adapt( + candidate_set, + analysis.source, + force_most_likely=force_most_likely, + ) + return CandidateSelection( + scene_manifest=adaptation.scene_manifest, + role_bindings=adaptation.role_bindings, + binding_report=adaptation.binding_report, + selected_candidate=adaptation.selected_candidate, + candidate_bindings=adaptation.candidate_bindings, + ) + + def materialize( + self, + analysis: SceneAnalysis, + request: TaskRunRequest, + output_root: str | Path, + *, + seed: int, + ) -> SceneRevision: + """Produce a new revision, or return the untouched existing source. + + Args: + analysis: Pre-materialization scene analysis. + request: Validated Task Engine run request. + output_root: Fresh directory for this scene attempt. + seed: Attempt seed recorded for recovery audit. + + Returns: + Final scene source for binding and Action Engine generation. + """ + root = Path(output_root).expanduser().resolve() + edit_prompt = request["scene_edit_prompt"] + if analysis.input_kind == "image": + assert analysis.blueprint is not None + root.mkdir(parents=True, exist_ok=False) + blueprint = replace(analysis.blueprint, output_root=root) + materialization = materialize_blueprint(blueprint) + edit_plan = None + if edit_prompt is not None: + edit_blueprint = analyze_edit( + output_root=root, + edit_prompt=str(edit_prompt), + ) + edit_plan = edit_blueprint.scene_edit_plan.to_dict() + materialization = materialize_edit(edit_blueprint) + revision = _revision(materialization, seed=seed, edit_plan=edit_plan) + _write_revision_audit( + root, + revision_id=revision.revision_id, + seed=seed, + edit_plan=edit_plan, + ) + return revision + + fingerprint = analysis.source_fingerprint + assert fingerprint is not None + if edit_prompt is None: + verify_scene_source_fingerprint(fingerprint.to_dict()) + return SceneRevision( + source=analysis.source, + output_root=None, + revision_id=scene_revision_id(analysis.source), + seed=seed, + edit_plan=None, + source_fingerprint=fingerprint, + ) + + resolved = resolve_source_scene(analysis.source) + if resolved.source_format == "legacy_gym_config": + converted = convert_legacy_gym_project(analysis.source, root) + editable_root = converted.output_root + else: + editable_root = _copy_scene_export_revision(resolved.path, root) + edit_blueprint = analyze_edit( + output_root=editable_root, + edit_prompt=str(edit_prompt), + ) + edit_plan = edit_blueprint.scene_edit_plan.to_dict() + materialization = materialize_edit(edit_blueprint) + if resolved.source_format == "legacy_gym_config": + restore_locked_scene_entities(editable_root) + else: + _restore_scene_export_locked_entities(editable_root) + verify_scene_source_fingerprint(fingerprint.to_dict()) + _write_revision_audit( + editable_root, + revision_id=scene_revision_id(materialization.scene_config_path), + seed=seed, + edit_plan=edit_plan, + source_fingerprint=fingerprint, + ) + return SceneRevision( + source=materialization.scene_config_path, + output_root=editable_root, + revision_id=scene_revision_id(materialization.scene_config_path), + seed=seed, + edit_plan=edit_plan, + source_fingerprint=fingerprint, + ) + + def inspect( + self, + revision: SceneRevision, + output_path: str | Path, + ) -> FinalSceneInspection: + """Inspect final geometry and publish support/orientation evidence. + + Args: + revision: Completed immutable scene revision. + output_path: JSON path receiving the inspection document. + + Returns: + Validated final scene inspection. + """ + try: + actual_revision_id = scene_revision_id(revision.source) + except (OSError, TypeError, ValueError) as exc: + raise SceneRemediableError( + f"Final scene content could not be hashed: {exc}" + ) from exc + if actual_revision_id != revision.revision_id: + raise RuntimeError("Final scene changed before geometry inspection.") + try: + inspection = inspect_final_scene( + revision.source, + revision_id=actual_revision_id, + ) + except (OSError, TypeError, ValueError) as exc: + raise SceneRemediableError( + f"Final scene assets could not be inspected: {exc}" + ) from exc + path = Path(output_path).expanduser().resolve() + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text( + json.dumps(inspection, ensure_ascii=False, indent=2, allow_nan=False) + + "\n", + encoding="utf-8", + ) + return inspection + + +def scene_blueprint_objects(blueprint: SceneBlueprintPackage) -> list[dict[str, Any]]: + """Convert image semantics into the redacted grounding inventory shape. + + Args: + blueprint: Scene Engine image-understanding package. + + Returns: + Semantic objects with unknown physical fields represented conservatively. + """ + if blueprint.schema_version != SCENE_BLUEPRINT_SCHEMA: + raise ValueError( + "Unsupported Scene Blueprint schema_version " + f"{blueprint.schema_version!r}; expected {SCENE_BLUEPRINT_SCHEMA!r}." + ) + result = [] + for item in blueprint.scene.objects: + result.append( + { + "uid": item.id, + "source_uid": item.id, + "role": "table" if item.kind == "table" else "rigid_object", + "name": item.name, + "description": item.description, + "category": item.category, + "color": None, + "init_pos": [0.0, 0.0, 0.0], + "affordances": [], + # Free-form pose descriptions are authoring intent, not measured + # orientation evidence. Final inspection publishes physical state. + "initial_state": {}, + "attributes": {}, + } + ) + return result + + +def _copy_scene_export_revision(source_config: Path, output_root: Path) -> Path: + if output_root.exists(): + if not output_root.is_dir() or any(output_root.iterdir()): + raise ValueError("Scene revision output_root must be empty.") + source_root = source_config.parent + destination = output_root / "scene_export" + shutil.copytree(source_root, destination) + config_path = destination / "scene_config.json" + config = _read_json_mapping(config_path) + background = list(config.get("background", ())) + rigid_objects = list(config.get("rigid_object", ())) + articulations = list(config.get("articulation", ())) + editable_rigid = [ + item + for item in rigid_objects + if isinstance(item, Mapping) and _scene_editable_rigid(item) + ] + locked_rigid = [item for item in rigid_objects if item not in editable_rigid] + table = [ + item + for item in background + if isinstance(item, Mapping) and item.get("uid") == "table" + ] + if len(table) != 1: + raise ValueError("Scene export revision requires exactly one table.") + locked = { + "schema_version": "embodichain.locked-scene-entities/v1", + "background": [item for item in background if item not in table], + "rigid_object": locked_rigid, + "articulation": articulations, + } + config["background"] = table + config["rigid_object"] = editable_rigid + config["articulation"] = [] + _write_json_mapping(config_path, config) + _write_json_mapping(output_root / _LOCKED_SCENE_MANIFEST, locked) + graph_path = destination / "scene_graph.json" + if graph_path.is_file(): + graph = _read_json_mapping(graph_path) + editable_uids = {str(item.get("uid")) for item in [*table, *editable_rigid]} + graph["nodes"] = [ + item + for item in graph.get("nodes", ()) + if isinstance(item, Mapping) and item.get("object_id") in editable_uids + ] + graph["relations"] = [ + item + for item in graph.get("relations", ()) + if isinstance(item, Mapping) + and item.get("source_id") in editable_uids + and item.get("target_id") in editable_uids + ] + _write_json_mapping(graph_path, graph) + return output_root + + +def _restore_scene_export_locked_entities(output_root: Path) -> None: + manifest = _read_json_mapping(output_root / _LOCKED_SCENE_MANIFEST) + if manifest.get("schema_version") != "embodichain.locked-scene-entities/v1": + raise ValueError("Locked scene entity manifest schema is invalid.") + config_path = output_root / "scene_export" / "scene_config.json" + config = _read_json_mapping(config_path) + existing = { + str(item.get("uid")) + for section in ("background", "rigid_object", "articulation") + for item in config.get(section, ()) + if isinstance(item, Mapping) and item.get("uid") + } + for section in ("background", "rigid_object", "articulation"): + target = list(config.get(section, ())) + for raw in manifest.get(section, ()): + item = deepcopy(dict(raw)) + uid = str(item.get("uid", "")) + if not uid or uid in existing: + raise ValueError(f"Scene edit reused locked entity UID {uid!r}.") + target.append(item) + existing.add(uid) + config[section] = target + _write_json_mapping(config_path, config) + + +def _scene_editable_rigid(value: Mapping[str, Any]) -> bool: + shape = value.get("shape") + return ( + isinstance(shape, Mapping) + and shape.get("shape_type") == "Mesh" + and isinstance(shape.get("fpath"), str) + and bool(shape["fpath"]) + ) + + +def _read_json_mapping(path: Path) -> dict[str, Any]: + value = json.loads(path.read_text(encoding="utf-8")) + if not isinstance(value, Mapping): + raise TypeError(f"JSON artifact must contain an object: {path}") + return dict(value) + + +def _write_json_mapping(path: Path, value: Mapping[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text( + json.dumps(value, ensure_ascii=False, indent=2, allow_nan=False) + "\n", + encoding="utf-8", + ) + + +def _revision( + value: SceneMaterialization, + *, + seed: int, + edit_plan: dict[str, Any] | None, +) -> SceneRevision: + return SceneRevision( + source=value.scene_config_path, + output_root=value.output_root, + revision_id=scene_revision_id(value.scene_config_path), + seed=seed, + edit_plan=edit_plan, + source_fingerprint=None, + ) + + +def _write_revision_audit( + output_root: Path, + *, + revision_id: str, + seed: int, + edit_plan: Mapping[str, Any] | None, + source_fingerprint: SceneSourceFingerprint | None = None, +) -> None: + payload = { + "schema_version": "embodichain.scene-revision-attempt/v1", + "revision_id": revision_id, + "seed": int(seed), + "edit_plan": None if edit_plan is None else dict(edit_plan), + "source_fingerprint": ( + None if source_fingerprint is None else source_fingerprint.to_dict() + ), + } + (output_root / "scene_revision_attempt.json").write_text( + json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False) + "\n", + encoding="utf-8", + ) diff --git a/embodichain/gen_sim/task_engine/semantic_graph.py b/embodichain/gen_sim/task_engine/semantic_graph.py new file mode 100644 index 000000000..219469d65 --- /dev/null +++ b/embodichain/gen_sim/task_engine/semantic_graph.py @@ -0,0 +1,330 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Immutable, execution-free task graphs built from canonical Semantic Calls.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +import json +import re +from typing import Any, Final, TypeAlias + +from embodichain.lab.task_program.language import decode_task_program + +from .contracts import canonical_hash + +__all__ = [ + "SEMANTIC_TASK_GRAPH_SCHEMA", + "SemanticTaskGraph", + "semantic_task_graph_hash", + "validate_semantic_task_graph", +] + +SEMANTIC_TASK_GRAPH_SCHEMA: Final = "semantic_task_graph/v1" +SemanticTaskGraph: TypeAlias = dict[str, Any] + +_GRAPH_KEYS = frozenset( + { + "schema_version", + "task_id", + "instruction", + "planner_route", + "integration_fingerprint", + "targets", + "nodes", + "task_groups", + "success", + } +) +_NODE_KEYS = frozenset( + { + "id", + "call", + "depends_on", + "task_instance_id", + "task_type", + "role", + } +) +_GROUP_KEYS = frozenset({"id", "task_type", "node_ids", "depends_on", "success"}) +_FORBIDDEN_KEYS = frozenset( + { + "action", + "action_invocation", + "action_options", + "atomic_action", + "atomic_goal", + "command", + "command_frame", + "control_part", + "controller", + "eef_pose", + "goal", + "grasp_pose", + "held_object", + "motion_policy", + "planner_backend", + "qpos", + "resource_claim", + "robot_part", + "solver", + "trajectory", + "waypoint", + } +) +_FINGERPRINT = re.compile(r"^[0-9a-f]{64}$") + + +def validate_semantic_task_graph(value: Mapping[str, Any]) -> SemanticTaskGraph: + """Validate and detach one provider-free semantic task graph. + + Every node payload is decoded through the Task Program language decoder. + Consequently Task Engine cannot silently grow a second Semantic Call JSON + dialect while planning remains independent from grounded robot actions. + + Args: + value: JSON-compatible task graph mapping. + + Returns: + A detached, normalized graph dictionary. + + Raises: + TypeError: If the graph is not an exact JSON mapping. + ValueError: If topology, schema, or a Semantic Call is invalid. + """ + graph = _json_snapshot(value) + _exact_keys(graph, _GRAPH_KEYS, "SemanticTaskGraph") + if graph["schema_version"] != SEMANTIC_TASK_GRAPH_SCHEMA: + raise ValueError( + "SemanticTaskGraph.schema_version must be " + f"{SEMANTIC_TASK_GRAPH_SCHEMA!r}." + ) + for field in ("task_id", "instruction", "planner_route"): + graph[field] = _nonempty(graph[field], f"SemanticTaskGraph.{field}") + fingerprint = _nonempty( + graph["integration_fingerprint"], + "SemanticTaskGraph.integration_fingerprint", + ) + if _FINGERPRINT.fullmatch(fingerprint) is None: + raise ValueError( + "SemanticTaskGraph.integration_fingerprint must be a lowercase " + "SHA-256 digest." + ) + + targets = graph["targets"] + if type(targets) is not dict: + raise TypeError("SemanticTaskGraph.targets must be an exact mapping.") + _reject_execution_data(targets, path="SemanticTaskGraph.targets") + + raw_nodes = graph["nodes"] + if type(raw_nodes) is not list or not raw_nodes: + raise ValueError("SemanticTaskGraph.nodes must be a non-empty list.") + nodes: list[dict[str, Any]] = [] + node_ids: set[str] = set() + for index, raw in enumerate(raw_nodes): + context = f"SemanticTaskGraph.nodes[{index}]" + if type(raw) is not dict: + raise TypeError(f"{context} must be an exact mapping.") + _exact_keys(raw, _NODE_KEYS, context) + node = deepcopy(raw) + for field in ("id", "task_instance_id", "task_type", "role"): + node[field] = _nonempty(node[field], f"{context}.{field}") + if node["id"] in node_ids: + raise ValueError(f"Duplicate SemanticTaskGraph node ID {node['id']!r}.") + node_ids.add(node["id"]) + node["depends_on"] = _string_list(node["depends_on"], f"{context}.depends_on") + if type(node["call"]) is not dict: + raise TypeError(f"{context}.call must be an exact mapping.") + _reject_execution_data(node["call"], path=f"{context}.call") + nodes.append(node) + + _validate_dependencies(nodes, node_ids, owner="SemanticTaskGraph.nodes") + _decode_calls(targets, [node["call"] for node in nodes]) + + raw_groups = graph["task_groups"] + if type(raw_groups) is not list or not raw_groups: + raise ValueError("SemanticTaskGraph.task_groups must be a non-empty list.") + groups: list[dict[str, Any]] = [] + group_ids: set[str] = set() + assigned_nodes: set[str] = set() + for index, raw in enumerate(raw_groups): + context = f"SemanticTaskGraph.task_groups[{index}]" + if type(raw) is not dict: + raise TypeError(f"{context} must be an exact mapping.") + _exact_keys(raw, _GROUP_KEYS, context) + group = deepcopy(raw) + for field in ("id", "task_type"): + group[field] = _nonempty(group[field], f"{context}.{field}") + if group["id"] in group_ids: + raise ValueError(f"Duplicate TaskGroup ID {group['id']!r}.") + group_ids.add(group["id"]) + group["node_ids"] = _string_list(group["node_ids"], f"{context}.node_ids") + if not group["node_ids"]: + raise ValueError(f"{context}.node_ids must not be empty.") + unknown = sorted(set(group["node_ids"]) - node_ids) + if unknown: + raise ValueError(f"{context} references unknown node IDs {unknown}.") + overlap = assigned_nodes.intersection(group["node_ids"]) + if overlap: + raise ValueError( + f"SemanticTaskGraph nodes belong to multiple groups: {sorted(overlap)}." + ) + assigned_nodes.update(group["node_ids"]) + group["depends_on"] = _string_list(group["depends_on"], f"{context}.depends_on") + if type(group["success"]) is not dict: + raise TypeError(f"{context}.success must be an exact mapping.") + _reject_execution_data(group["success"], path=f"{context}.success") + groups.append(group) + + if assigned_nodes != node_ids: + raise ValueError( + "Every SemanticTaskGraph node must belong to exactly one TaskGroup; " + f"unassigned={sorted(node_ids - assigned_nodes)}." + ) + _validate_dependencies(groups, group_ids, owner="SemanticTaskGraph.task_groups") + node_group = { + node_id: group["id"] for group in groups for node_id in group["node_ids"] + } + for node in nodes: + if node_group[node["id"]] != node["task_instance_id"]: + raise ValueError( + f"Node {node['id']!r} task_instance_id does not match TaskGroup membership." + ) + if type(graph["success"]) is not dict: + raise TypeError("SemanticTaskGraph.success must be an exact mapping.") + _reject_execution_data(graph["success"], path="SemanticTaskGraph.success") + + graph["nodes"] = nodes + graph["task_groups"] = groups + return graph + + +def semantic_task_graph_hash(value: Mapping[str, Any]) -> str: + """Return the deterministic content hash of a valid semantic task graph. + + Args: + value: JSON-compatible semantic task graph. + + Returns: + Lowercase SHA-256 content digest. + + Raises: + TypeError: If the graph is not an exact JSON mapping. + ValueError: If graph topology, schema, or a Semantic Call is invalid. + """ + return canonical_hash(validate_semantic_task_graph(value)) + + +def _decode_calls( + targets: Mapping[str, Any], calls: Sequence[Mapping[str, Any]] +) -> None: + items = [{"kind": "invoke", "call": deepcopy(dict(call))} for call in calls] + decode_task_program( + { + "program_id": "semantic_task_graph_validation", + "integration": { + "robot_profile": "provider_free_profile", + "scene_registry": "provider_free_scene", + "runtime_preset": "provider_free_runtime", + }, + "targets": deepcopy(dict(targets)), + "program": {"kind": "sequence", "items": items}, + } + ) + + +def _validate_dependencies( + items: Sequence[Mapping[str, Any]], + known_ids: set[str], + *, + owner: str, +) -> None: + positions = {str(item["id"]): index for index, item in enumerate(items)} + for item in items: + item_id = str(item["id"]) + dependencies = list(item["depends_on"]) + unknown = sorted(set(dependencies) - known_ids) + if unknown: + raise ValueError(f"{owner} {item_id!r} has unknown dependencies {unknown}.") + if item_id in dependencies: + raise ValueError(f"{owner} {item_id!r} cannot depend on itself.") + later = [ + dependency + for dependency in dependencies + if positions[dependency] >= positions[item_id] + ] + if later: + raise ValueError( + f"{owner} must be topologically ordered; {item_id!r} depends on {later}." + ) + + +def _reject_execution_data(value: Any, *, path: str) -> None: + if type(value) is dict: + for key, child in value.items(): + normalized = str(key).strip().lower().replace("-", "_") + if normalized in _FORBIDDEN_KEYS: + raise ValueError( + f"{path}.{key} is grounded execution data and is forbidden." + ) + _reject_execution_data(child, path=f"{path}.{key}") + elif type(value) is list: + for index, child in enumerate(value): + _reject_execution_data(child, path=f"{path}[{index}]") + + +def _json_snapshot(value: Mapping[str, Any]) -> dict[str, Any]: + if type(value) is not dict: + raise TypeError("SemanticTaskGraph must be an exact mapping.") + try: + payload = json.dumps(value, ensure_ascii=False, allow_nan=False) + decoded = json.loads(payload) + except (TypeError, ValueError) as exc: + raise TypeError( + "SemanticTaskGraph must contain only finite JSON values." + ) from exc + assert type(decoded) is dict + return decoded + + +def _exact_keys( + value: Mapping[str, Any], expected: frozenset[str], context: str +) -> None: + actual = set(value) + if actual != expected: + raise ValueError( + f"{context} fields must be exactly {sorted(expected)}; " + f"missing={sorted(expected - actual)}, unexpected={sorted(actual - expected)}." + ) + + +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 _string_list(value: Any, context: str) -> list[str]: + if type(value) is not list: + raise TypeError(f"{context} must be a list.") + result = [ + _nonempty(item, f"{context}[{index}]") for index, item in enumerate(value) + ] + if len(result) != len(set(result)): + raise ValueError(f"{context} must not contain duplicates.") + return result diff --git a/embodichain/gen_sim/task_engine/semantic_planner.py b/embodichain/gen_sim/task_engine/semantic_planner.py new file mode 100644 index 000000000..e1acb92a5 --- /dev/null +++ b/embodichain/gen_sim/task_engine/semantic_planner.py @@ -0,0 +1,913 @@ +# ---------------------------------------------------------------------------- +# 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 TaskCandidate lowering to provider-free Semantic Calls.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +import math +from typing import Any, Final + +from .contracts import TaskCandidate, validate_task_candidate +from .orchestration.contracts import RoleBindings, validate_role_bindings +from .semantic_graph import SemanticTaskGraph, validate_semantic_task_graph + +__all__ = ["SemanticTaskPlanner", "UnsupportedSemanticCapabilityError"] + +_COORDINATED_TRANSPORT_CALL_ID: Final = "simulation.coordinated_transport" +_COORDINATED_HOLD_CALL_ID: Final = "simulation.coordinated_hold" +_PARK_CALL_ID: Final = "simulation.park" +_PLACE_RELATIVE_CALL_ID: Final = "simulation.place_relative" +_STACK_PLACE_CALL_ID: Final = "gen_sim.stack_place" +_STACK_PICK_CALL_ID: Final = "gen_sim.stack_pick" +_MOVE_HELD_OBJECT_CALL_ID: Final = "simulation.move_held_object" +_ALIGN_HELD_CALL_ID: Final = "gen_sim.align_held" +_CLEAR_RELEASED_CALL_ID: Final = "gen_sim.clear_released" +_PICK_CALL_ID: Final = "simulation.pick" +_POUR_CALL_ID: Final = "simulation.pour" + + +class UnsupportedSemanticCapabilityError(ValueError): + """Raised when a TaskSpec has no executable canonical Semantic Call route.""" + + +class SemanticTaskPlanner: + """Lower Task Engine ontology steps into one immutable semantic task DAG. + + Args: + lateral_relation_distance: World-frame offset used for ``right_of``. + front_relation_distance: World-frame offset used for ``front_of``. + + Raises: + ValueError: If either relation distance is not positive. + """ + + def __init__( + self, + *, + lateral_relation_distance: float = 0.10, + front_relation_distance: float = 0.18, + ) -> None: + for field_name, value in ( + ("lateral_relation_distance", lateral_relation_distance), + ("front_relation_distance", front_relation_distance), + ): + if not isinstance(value, (int, float)) or value <= 0: + raise ValueError(f"{field_name} must be positive.") + self.lateral_relation_distance = float(lateral_relation_distance) + self.front_relation_distance = float(front_relation_distance) + + def plan( + self, + candidate: Mapping[str, Any], + role_bindings: Mapping[str, Any], + scene_objects: Sequence[Mapping[str, Any]], + *, + planner_route: str = "offline", + integration_fingerprint: str = "0" * 64, + ) -> SemanticTaskGraph: + """Build one semantic graph without importing or materializing actions. + + Args: + candidate: Valid selected TaskCandidate. + role_bindings: Canonical scene IDs selected by Scene Adapter. + scene_objects: Provider-free normalized scene metadata. + planner_route: Candidate route provenance. + integration_fingerprint: Exact integration fingerprint, or the + all-zero placeholder used before bundle preflight. + + Returns: + A validated ``semantic_task_graph/v1`` value. + """ + selected: TaskCandidate = validate_task_candidate(candidate) + bindings: RoleBindings = validate_role_bindings(role_bindings) + if bindings["task_id"] != selected["draft"]["task_id"]: + raise ValueError("RoleBindings.task_id must match the TaskCandidate.") + if bindings["candidate_id"] != selected["candidate_id"]: + raise ValueError("RoleBindings.candidate_id must match the TaskCandidate.") + + objects = { + str(item.get("runtime_uid", item.get("uid", ""))): deepcopy(dict(item)) + for item in scene_objects + if str(item.get("runtime_uid", item.get("uid", ""))).strip() + } + steps = selected["draft"]["steps"] + steps_by_id = {str(step["id"]): step for step in steps} + result_objects: dict[str, str] = {} + held_by: dict[str, str] = {} + upright_objects: set[str] = set() + upright_staging_targets: dict[str, str] = {} + targets: dict[str, Any] = {} + nodes: list[dict[str, Any]] = [] + groups: list[dict[str, Any]] = [] + group_terminal: dict[str, str] = {} + + for step in steps: + step_id = str(step["id"]) + task_type = str(step["task_type"]) + if task_type not in {"E1", "E2", "E3", "E4", "E5"}: + raise UnsupportedSemanticCapabilityError( + f"Task Engine currently supports only E1-E5, not {task_type}." + ) + object_id = self._resolve_step_entity( + step, + role="object", + bindings=bindings, + result_objects=result_objects, + steps_by_id=steps_by_id, + ) + calls: list[dict[str, Any]] + cleanup_resources: tuple[str, ...] = () + if task_type == "E2": + if str(step.get("orientation_goal")) != "upright": + raise UnsupportedSemanticCapabilityError( + f"Step {step_id!r} E2 currently requires " + "orientation_goal='upright'." + ) + requested = str(step.get("required_arm", "auto")) + resource = ( + held_by.get(object_id) or self._nearest_resource(object_id, objects) + if requested in {"auto", "none"} + else _resource(requested, field="required_arm") + ) + target_name = f"{step_id}_upright_target" + staging_target_name = f"{step_id}_upright_staging_target" + upright_position = self._upright_target_position( + object_id, + objects, + ) + targets.update(_upright_targets(step_id, upright_position)) + calls = [] + if held_by.get(object_id) != resource: + if object_id in held_by: + raise UnsupportedSemanticCapabilityError( + f"Step {step_id!r} requires {resource!r} while " + f"{object_id!r} is held by {held_by[object_id]!r}." + ) + calls.append( + { + "kind": "registered", + "call_id": _PICK_CALL_ID, + "arguments": { + "object": object_id, + "target": target_name, + }, + "resources": {"primary": resource}, + } + ) + calls.extend( + ( + { + "kind": "registered", + "call_id": _ALIGN_HELD_CALL_ID, + "arguments": { + "object": object_id, + "target": staging_target_name, + "preserve_yaw": False, + }, + "resources": {"primary": resource}, + }, + { + "kind": "registered", + "call_id": _PLACE_RELATIVE_CALL_ID, + "arguments": { + "object": object_id, + "reference": "table", + "relation": "on", + }, + "resources": {"primary": resource}, + }, + ) + ) + calls.insert( + -1, + { + "kind": "registered", + "call_id": _ALIGN_HELD_CALL_ID, + "arguments": { + "object": object_id, + "target": staging_target_name, + "preserve_yaw": True, + }, + "resources": {"primary": resource}, + }, + ) + held_by.pop(object_id, None) + objects[object_id]["init_pos"] = upright_position + upright_objects.add(object_id) + upright_staging_targets[object_id] = staging_target_name + cleanup_resources = (resource,) + elif task_type == "E3": + target_id = self._resolve_step_entity( + step, + role="target", + bindings=bindings, + result_objects=result_objects, + steps_by_id=steps_by_id, + ) + requested = str(step.get("required_arm", "auto")) + resource = ( + held_by.get(object_id) or self._nearest_resource(object_id, objects) + if requested in {"auto", "none"} + else _resource(requested, field="required_arm") + ) + calls = [] + if held_by.get(object_id) != resource: + if object_id in held_by: + raise UnsupportedSemanticCapabilityError( + f"Step {step_id!r} Pour requires {resource!r} while " + f"{object_id!r} is held by {held_by[object_id]!r}." + ) + calls.append( + { + "kind": "pick", + "object": object_id, + "resources": {"primary": resource}, + } + ) + pour_target = f"{step_id}_pour_target" + return_target = f"{step_id}_return_target" + targets[return_target] = { + "kind": "cyclic_pose", + "values": [ + { + "position": [ + float(value) for value in objects[object_id]["init_pos"] + ], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + } + ], + } + calls.extend( + ( + { + "kind": "registered", + "call_id": _MOVE_HELD_OBJECT_CALL_ID, + "arguments": { + "object": object_id, + "target": pour_target, + "reference": target_id, + }, + "resources": {"primary": resource}, + }, + { + "kind": "registered", + "call_id": _POUR_CALL_ID, + "arguments": {"object": object_id}, + "resources": {"primary": resource}, + }, + { + "kind": "place", + "object": object_id, + "at": {"kind": "target_ref", "target": return_target}, + "resources": {"primary": resource}, + }, + ) + ) + held_by.pop(object_id, None) + cleanup_resources = (resource,) + elif task_type == "E4": + source = _resource(step.get("transfer_arm"), field="transfer_arm") + destination = _resource(step.get("receive_arm"), field="receive_arm") + calls = [] + if held_by.get(object_id) != source: + if object_id in held_by: + raise UnsupportedSemanticCapabilityError( + f"Step {step_id!r} HandOver requires {source!r} but " + f"{object_id!r} is held by {held_by[object_id]!r}." + ) + calls.append( + { + "kind": "pick", + "object": object_id, + "resources": {"primary": source}, + } + ) + calls.append( + { + "kind": "hand_over", + "object": object_id, + "resources": {"source": source, "destination": destination}, + } + ) + held_by[object_id] = destination + # HandOver already retreats the source; do not delay a held receiver. + cleanup_resources = () + if str(step.get("terminal_behavior")) == "place": + target_id = self._resolve_step_entity( + step, + role="target", + bindings=bindings, + result_objects=result_objects, + steps_by_id=steps_by_id, + ) + place_call = self._placement_call( + step_id=step_id, + object_id=object_id, + target_id=target_id, + relation=str(step.get("relation", "none")), + resource=destination, + ) + calls.append(place_call) + held_by.pop(object_id, None) + cleanup_resources = (source, destination) + elif task_type == "E1": + target_id = self._resolve_step_entity( + step, + role="target", + bindings=bindings, + result_objects=result_objects, + steps_by_id=steps_by_id, + ) + requested = str(step.get("required_arm", "auto")) + resource = ( + held_by.get(object_id) or self._nearest_resource(object_id, objects) + if requested in {"auto", "none"} + else _resource(requested, field="required_arm") + ) + relation = str(step.get("relation", "none")) + calls = [] + if ( + held_by.get(object_id) == resource + and relation in {"on", "above"} + and object_id in upright_objects + and target_id in upright_objects + ): + # A low receiving grasp can intersect the support during + # stacking. Expose the regrasp as ordinary semantic calls. + calls.append( + self._placement_call( + step_id=step_id, + object_id=object_id, + target_id="table", + relation="on", + resource=resource, + ) + ) + held_by.pop(object_id) + other_resource = "left" if resource == "right" else "right" + if other_resource not in held_by.values(): + # Clear the former source hand only after the receiver + # has safely staged its object, not during a live hold. + calls.append(_park_call(other_resource)) + if held_by.get(object_id) != resource: + if object_id in held_by: + raise UnsupportedSemanticCapabilityError( + f"Step {step_id!r} requires {resource!r} while {object_id!r} " + f"is held by {held_by[object_id]!r}; add an explicit hand_over." + ) + calls.append( + { + "kind": "pick", + "object": object_id, + "resources": {"primary": resource}, + } + ) + place_call = self._placement_call( + step_id=step_id, + object_id=object_id, + target_id=target_id, + relation=relation, + resource=resource, + ) + if ( + relation in {"on", "above"} + and object_id in upright_objects + and target_id in upright_objects + ): + if calls and calls[-1]["kind"] == "pick": + calls[-1] = { + "kind": "registered", + "call_id": _STACK_PICK_CALL_ID, + "arguments": {"object": object_id, "target": target_id}, + "resources": {"primary": resource}, + } + place_call["call_id"] = _STACK_PLACE_CALL_ID + calls.append( + { + "kind": "registered", + "call_id": _ALIGN_HELD_CALL_ID, + "arguments": { + "object": object_id, + "target": upright_staging_targets[object_id], + "preserve_yaw": True, + }, + "resources": {"primary": resource}, + } + ) + calls.append(place_call) + held_by.pop(object_id, None) + cleanup_resources = (resource,) + elif task_type == "E5": + terminal_behavior = str(step.get("terminal_behavior")) + target_reference = step.get("target", {}) + target_id = None + if isinstance(target_reference, Mapping) and target_reference.get( + "kind" + ) not in {None, "none"}: + target_id = self._resolve_step_entity( + step, + role="target", + bindings=bindings, + result_objects=result_objects, + steps_by_id=steps_by_id, + ) + displacement = self._transport_world_displacement( + step, + object_id=object_id, + target_id=target_id, + objects=objects, + ) + calls = [ + { + "kind": "registered", + "call_id": ( + _COORDINATED_TRANSPORT_CALL_ID + if terminal_behavior == "place" + else _COORDINATED_HOLD_CALL_ID + ), + "arguments": { + "object": object_id, + "target": f"{step_id}_coordinated_target", + "world_displacement": displacement, + }, + "resources": {"left": "left", "right": "right"}, + } + ] + if terminal_behavior == "place": + held_by.pop(object_id, None) + cleanup_resources = ("left", "right") + else: + held_by[object_id] = "coordinated" + else: + raise UnsupportedSemanticCapabilityError( + f"Task type {task_type!r} has no phase-one Semantic Call route." + ) + + requested_upright = ( + task_type in {"E1", "E4"} and step.get("orientation_goal") == "upright" + ) + if requested_upright: + orientation_resource = resource if task_type == "E1" else destination + position = self._upright_target_position(object_id, objects) + targets.update(_upright_targets(step_id, position)) + staging_target = f"{step_id}_upright_staging_target" + upright_staging_targets[object_id] = staging_target + if task_type == "E4": + handover_index = next( + i for i, call in enumerate(calls) if call["kind"] == "hand_over" + ) + calls.insert( + handover_index, + { + "kind": "registered", + "call_id": _ALIGN_HELD_CALL_ID, + "arguments": { + "object": object_id, + "target": "current_object_pose", + "preserve_yaw": False, + }, + "resources": {"primary": source}, + }, + ) + if not any( + call.get("call_id") == _ALIGN_HELD_CALL_ID + and call.get("resources", {}).get("primary") == orientation_resource + for call in calls + ): + if task_type == "E1": + for index, call in enumerate(calls): + if call["kind"] == "pick": + calls[index] = { + "kind": "registered", + "call_id": _PICK_CALL_ID, + "arguments": { + "object": object_id, + "target": f"{step_id}_upright_target", + }, + "resources": {"primary": orientation_resource}, + } + calls.insert( + -1, + { + "kind": "registered", + "call_id": _ALIGN_HELD_CALL_ID, + "arguments": { + "object": object_id, + "target": staging_target, + "preserve_yaw": False, + }, + "resources": {"primary": orientation_resource}, + }, + ) + alignment = { + "kind": "registered", + "call_id": _ALIGN_HELD_CALL_ID, + "arguments": { + "object": object_id, + "target": "current_object_pose", + "preserve_yaw": True, + }, + "resources": {"primary": orientation_resource}, + } + if cleanup_resources: + calls.insert(-1, alignment) + else: + calls.append(alignment) + upright_objects.add(object_id) + call_roles = [(call, "primary") for call in calls] + if cleanup_resources and ( + task_type == "E2" + or requested_upright + or calls[-1].get("call_id") == _STACK_PLACE_CALL_ID + ): + call_roles.append( + ( + { + "kind": "registered", + "call_id": _CLEAR_RELEASED_CALL_ID, + "arguments": { + "object": object_id, + "target": upright_staging_targets[object_id], + }, + "resources": { + "primary": ( + orientation_resource + if requested_upright + else resource + ) + }, + }, + "cleanup", + ) + ) + call_roles.extend( + (_park_call(resource), "cleanup") for resource in cleanup_resources + ) + dependencies = [str(value) for value in step["depends_on"]] + first_dependencies = [group_terminal[value] for value in dependencies] + group_node_ids: list[str] = [] + previous: str | None = None + for call_index, (call, role) in enumerate(call_roles, start=1): + node_id = f"{step_id}__call_{call_index:02d}" + node_dependencies = ( + [previous] if previous is not None else first_dependencies + ) + nodes.append( + { + "id": node_id, + "call": call, + "depends_on": node_dependencies, + "task_instance_id": step_id, + "task_type": task_type, + "role": role, + } + ) + group_node_ids.append(node_id) + previous = node_id + assert previous is not None + group_terminal[step_id] = previous + groups.append( + { + "id": step_id, + "task_type": task_type, + "node_ids": group_node_ids, + "depends_on": dependencies, + "success": _step_success(selected["success_spec"], step_id), + } + ) + result_objects[step_id] = object_id + + return validate_semantic_task_graph( + { + "schema_version": "semantic_task_graph/v1", + "task_id": selected["draft"]["task_id"], + "instruction": selected["draft"]["instruction"], + "planner_route": str(planner_route), + "integration_fingerprint": str(integration_fingerprint), + "targets": targets, + "nodes": nodes, + "task_groups": groups, + "success": { + "kind": "all_task_groups", + "source": deepcopy(selected["success_spec"]), + }, + } + ) + + def _resolve_step_entity( + self, + step: Mapping[str, Any], + *, + role: str, + bindings: RoleBindings, + result_objects: Mapping[str, str], + steps_by_id: Mapping[str, Mapping[str, Any]], + ) -> str: + reference = step[role] + kind = str(reference["kind"]) + if kind == "scene_ref": + reference_id = f"{step['id']}.{role}" + values = bindings["reference_bindings"].get(reference_id) + if not values or len(values) != 1: + raise ValueError( + f"{reference_id} must resolve to exactly one scene entity." + ) + return str(values[0]) + if kind == "step_result": + source_step = str(reference["step_id"]) + if source_step not in steps_by_id or source_step not in result_objects: + raise ValueError( + f"Step {step['id']!r} references unavailable result {source_step!r}." + ) + return result_objects[source_step] + raise UnsupportedSemanticCapabilityError( + f"Step {step['id']!r} {role} reference kind {kind!r} is unsupported." + ) + + def _nearest_resource( + self, + object_id: str, + objects: Mapping[str, Mapping[str, Any]], + ) -> str: + item = objects.get(object_id) + if item is None: + raise ValueError(f"Scene metadata is missing object {object_id!r}.") + position = item.get("init_pos") + if not isinstance(position, Sequence) or len(position) != 3: + raise ValueError(f"Scene object {object_id!r} has no three-value init_pos.") + # The canonical dual-Franka embodiment faces world -X: its right arm + # base is on +Y and its left arm base is on -Y. + return "right" if float(position[1]) >= 0.0 else "left" + + def _relation_world_offset( + self, + relation: str, + *, + object_id: str, + target_id: str, + objects: Mapping[str, Mapping[str, Any]], + ) -> list[float]: + """Return a world-frame relation displacement including placement height.""" + target = objects.get(target_id) + obj = objects.get(object_id) + if target is None or obj is None: + raise ValueError( + f"Scene metadata is missing relation participants {object_id!r}, {target_id!r}." + ) + target_position = target.get("init_pos") + object_position = obj.get("init_pos") + if ( + not isinstance(target_position, Sequence) + or len(target_position) != 3 + or not isinstance(object_position, Sequence) + or len(object_position) != 3 + ): + raise ValueError( + "Relation participants require three-value init_pos fields." + ) + directions = { + "left_of": (0.0, -self.lateral_relation_distance), + "right_of": (0.0, self.lateral_relation_distance), + "front_of": (-self.front_relation_distance, 0.0), + "behind": (self.front_relation_distance, 0.0), + "front_left_of": ( + -self.front_relation_distance, + -self.lateral_relation_distance, + ), + "front_right_of": ( + -self.front_relation_distance, + self.lateral_relation_distance, + ), + "back_left_of": ( + self.front_relation_distance, + -self.lateral_relation_distance, + ), + "back_right_of": ( + self.front_relation_distance, + self.lateral_relation_distance, + ), + } + try: + x_offset, y_offset = directions[relation] + except KeyError as exc: + raise ValueError(f"Unsupported directional relation {relation!r}.") from exc + return [ + x_offset, + y_offset, + float(object_position[2]) - float(target_position[2]), + ] + + def _placement_call( + self, + *, + step_id: str, + object_id: str, + target_id: str, + relation: str, + resource: str, + ) -> dict[str, Any]: + """Build one canonical built-in or live-relative Place call.""" + if relation == "inside": + return { + "kind": "place", + "object": object_id, + "inside": _inside_affordance(target_id, object_id), + "resources": {"primary": resource}, + } + if relation not in { + "on", + "above", + "left_of", + "right_of", + "front_of", + "behind", + "front_left_of", + "front_right_of", + "back_left_of", + "back_right_of", + }: + raise UnsupportedSemanticCapabilityError( + f"Step {step_id!r} relation {relation!r} has no canonical " + "Semantic Call route." + ) + return { + "kind": "registered", + "call_id": _PLACE_RELATIVE_CALL_ID, + "arguments": { + "object": object_id, + "reference": target_id, + "relation": relation, + }, + "resources": {"primary": resource}, + } + + def _transport_world_displacement( + self, + step: Mapping[str, Any], + *, + object_id: str, + target_id: str | None, + objects: Mapping[str, Mapping[str, Any]], + ) -> list[float]: + """Resolve an E5 direction or target relation to one world displacement.""" + if target_id is not None: + offset = self._relation_world_offset( + str(step.get("relation", "none")), + object_id=object_id, + target_id=target_id, + objects=objects, + ) + return [ + float(objects[target_id]["init_pos"][index]) + + offset[index] + - float(objects[object_id]["init_pos"][index]) + for index in range(3) + ] + distance = 0.14 + bounds = objects[object_id].get("attributes", {}).get("final_world_aabb") + if isinstance(bounds, Mapping) and "min" in bounds and "max" in bounds: + horizontal_extent = max( + float(bounds["max"][axis]) - float(bounds["min"][axis]) + for axis in (0, 1) + ) + distance = min(distance, max(0.05, horizontal_extent * 0.3)) + vectors = { + "world_x": (1.0, 0.0, 0.0), + "world_y": (0.0, 1.0, 0.0), + "front": (-1.0, 0.0, 0.0), + "back": (1.0, 0.0, 0.0), + "left": (0.0, -1.0, 0.0), + "right": (0.0, 1.0, 0.0), + "front_left": (-1.0, -1.0, 0.0), + "front_right": (-1.0, 1.0, 0.0), + "back_left": (1.0, -1.0, 0.0), + "back_right": (1.0, 1.0, 0.0), + "up": (0.0, 0.0, 1.0), + "down": (0.0, 0.0, -1.0), + } + direction = str(step.get("direction", "none")) + try: + vector = vectors[direction] + except KeyError as exc: + raise UnsupportedSemanticCapabilityError( + f"E5 direction {direction!r} has no coordinated transport route." + ) from exc + magnitude = math.sqrt(sum(component * component for component in vector)) + return [distance * component / magnitude for component in vector] + + @staticmethod + def _upright_target_position( + object_id: str, + objects: Mapping[str, Mapping[str, Any]], + ) -> list[float]: + """Place an upright object at its original XY and measured support height.""" + item = objects.get(object_id) + if item is None: + raise ValueError(f"Scene metadata is missing object {object_id!r}.") + position = item.get("init_pos") + if not isinstance(position, Sequence) or len(position) != 3: + raise ValueError(f"Scene object {object_id!r} has no three-value init_pos.") + result = [float(value) for value in position] + attributes = item.get("attributes", {}) + object_aabb = ( + attributes.get("final_world_aabb") + if isinstance(attributes, Mapping) + else None + ) + table = objects.get("table") + table_attributes = table.get("attributes", {}) if table is not None else {} + table_aabb = ( + table_attributes.get("final_world_aabb") + if isinstance(table_attributes, Mapping) + else None + ) + if isinstance(object_aabb, Mapping) and isinstance(table_aabb, Mapping): + object_min = object_aabb.get("min") + object_max = object_aabb.get("max") + table_max = table_aabb.get("max") + if ( + isinstance(object_min, Sequence) + and len(object_min) == 3 + and isinstance(object_max, Sequence) + and len(object_max) == 3 + and isinstance(table_max, Sequence) + and len(table_max) == 3 + ): + longest_extent = max( + float(maximum) - float(minimum) + for minimum, maximum in zip(object_min, object_max, strict=True) + ) + result[2] = float(table_max[2]) + 0.01 + longest_extent / 2.0 + return result + + +def _resource(value: Any, *, field: str) -> str: + normalized = str(value).strip() + mapping = { + "left": "left", + "left_arm": "left", + "right": "right", + "right_arm": "right", + } + if normalized not in mapping: + raise UnsupportedSemanticCapabilityError( + f"{field}={normalized!r} is not a dual-Franka semantic resource." + ) + return mapping[normalized] + + +def _upright_targets(step_id: str, position: list[float]) -> dict[str, Any]: + """Share the upright acquisition recipe; bundle geometry refines its heights.""" + return { + f"{step_id}_{suffix}": { + "kind": "cyclic_pose", + "values": [ + {"position": list(position), "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0]} + ], + } + for suffix in ("upright_target", "upright_staging_target") + } + + +def _park_call(resource: str) -> dict[str, Any]: + """Build one payload-free semantic cleanup call for a logical resource.""" + return { + "kind": "registered", + "call_id": _PARK_CALL_ID, + "arguments": {}, + "resources": {"primary": resource}, + } + + +def _inside_affordance(container_id: str, object_id: str) -> str: + return f"inside__{container_id}__{object_id}" + + +def _step_success(success_spec: Mapping[str, Any], step_id: str) -> dict[str, Any]: + term = next( + ( + deepcopy(dict(item)) + for item in success_spec["terms"] + if item["step_id"] == step_id + ), + None, + ) + if term is None: + raise ValueError(f"SuccessSpec has no term for TaskGroup {step_id!r}.") + return {"kind": "semantic_task_term", **term} diff --git a/embodichain/gen_sim/task_engine/state_machine.py b/embodichain/gen_sim/task_engine/state_machine.py new file mode 100644 index 000000000..ffdaaba15 --- /dev/null +++ b/embodichain/gen_sim/task_engine/state_machine.py @@ -0,0 +1,294 @@ +# ---------------------------------------------------------------------------- +# 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, replayable state transitions for cross-engine orchestration.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from copy import deepcopy +from dataclasses import dataclass, field +from enum import Enum +from types import MappingProxyType +from typing import Any + +from .workflow_contracts import TaskRunRequest, validate_task_run_request + +__all__ = [ + "StageStatus", + "TaskEngineState", + "WorkflowStage", + "complete_stage", + "fail_stage", + "initial_state", + "replay_events", + "skip_stage", + "start_stage", +] + + +class WorkflowStage(str, Enum): + """Stable stages shared by all four supported input combinations.""" + + INPUT = "input" + TASK_CANDIDATES = "task_candidates" + SCENE_PREPARATION = "scene_preparation" + SCENE_EDIT = "scene_edit" + CANDIDATE_SELECTION = "candidate_selection" + SCENE_FINALIZATION = "scene_finalization" + UNBOUND_ACTION = "unbound_action" + FINAL_INSPECTION = "final_inspection" + FINAL_BINDING = "final_binding" + STATIC_FEASIBILITY = "static_feasibility" + GROUNDED_ACTION = "grounded_action" + EXECUTION = "execution" + + +class StageStatus(str, Enum): + """Lifecycle of one independently schedulable workflow stage.""" + + PENDING = "pending" + RUNNING = "running" + SUCCEEDED = "succeeded" + FAILED = "failed" + SKIPPED = "skipped" + + +_DEPENDENCIES: dict[WorkflowStage, frozenset[WorkflowStage]] = { + WorkflowStage.INPUT: frozenset(), + WorkflowStage.TASK_CANDIDATES: frozenset({WorkflowStage.INPUT}), + WorkflowStage.SCENE_PREPARATION: frozenset({WorkflowStage.INPUT}), + WorkflowStage.SCENE_EDIT: frozenset({WorkflowStage.SCENE_PREPARATION}), + WorkflowStage.CANDIDATE_SELECTION: frozenset( + { + WorkflowStage.TASK_CANDIDATES, + WorkflowStage.SCENE_PREPARATION, + } + ), + WorkflowStage.SCENE_FINALIZATION: frozenset( + {WorkflowStage.CANDIDATE_SELECTION, WorkflowStage.SCENE_EDIT} + ), + WorkflowStage.UNBOUND_ACTION: frozenset({WorkflowStage.CANDIDATE_SELECTION}), + WorkflowStage.FINAL_INSPECTION: frozenset({WorkflowStage.SCENE_FINALIZATION}), + WorkflowStage.FINAL_BINDING: frozenset( + {WorkflowStage.FINAL_INSPECTION, WorkflowStage.UNBOUND_ACTION} + ), + WorkflowStage.STATIC_FEASIBILITY: frozenset({WorkflowStage.FINAL_BINDING}), + WorkflowStage.GROUNDED_ACTION: frozenset({WorkflowStage.STATIC_FEASIBILITY}), + WorkflowStage.EXECUTION: frozenset({WorkflowStage.GROUNDED_ACTION}), +} + +_SKIPPABLE_STAGES = frozenset({WorkflowStage.SCENE_EDIT}) + + +@dataclass(frozen=True) +class TaskEngineState: + """Immutable state snapshot plus an append-only transition audit.""" + + request: Mapping[str, Any] + stages: Mapping[WorkflowStage, StageStatus] + events: tuple[Mapping[str, Any], ...] = field(default_factory=tuple) + + def __post_init__(self) -> None: + object.__setattr__( + self, + "request", + MappingProxyType(deepcopy(dict(self.request))), + ) + object.__setattr__( + self, + "stages", + MappingProxyType(dict(self.stages)), + ) + object.__setattr__( + self, + "events", + tuple(MappingProxyType(deepcopy(dict(event))) for event in self.events), + ) + + @property + def terminal(self) -> bool: + """Return whether execution succeeded or any stage failed.""" + return ( + self.stages[WorkflowStage.EXECUTION] == StageStatus.SUCCEEDED + or StageStatus.FAILED in self.stages.values() + ) + + def to_dict(self) -> dict[str, Any]: + """Return one JSON-safe audit snapshot.""" + return { + "request": deepcopy(dict(self.request)), + "stages": { + stage.value: self.stages[stage].value for stage in WorkflowStage + }, + "events": deepcopy([dict(event) for event in self.events]), + } + + +def initial_state(request: TaskRunRequest) -> TaskEngineState: + """Create a validated state with the optional edit stage resolved.""" + normalized = validate_task_run_request(request) + stages = {stage: StageStatus.PENDING for stage in WorkflowStage} + stages[WorkflowStage.INPUT] = StageStatus.SUCCEEDED + events = ( + { + "sequence": 1, + "stage": WorkflowStage.INPUT.value, + "from": StageStatus.PENDING.value, + "to": StageStatus.SUCCEEDED.value, + }, + ) + state = TaskEngineState(request=normalized, stages=stages, events=events) + if normalized["scene_edit_prompt"] is None: + state = skip_stage(state, WorkflowStage.SCENE_EDIT) + return state + + +def start_stage(state: TaskEngineState, stage: WorkflowStage) -> TaskEngineState: + """Start a pending stage only after every dependency has completed.""" + if state.terminal: + raise ValueError("A terminal TaskEngineState cannot start another stage.") + if state.stages[stage] != StageStatus.PENDING: + raise ValueError(f"Stage {stage.value!r} is not pending.") + incomplete = [ + dependency.value + for dependency in _DEPENDENCIES[stage] + if state.stages[dependency] not in {StageStatus.SUCCEEDED, StageStatus.SKIPPED} + ] + if incomplete: + raise ValueError( + f"Stage {stage.value!r} has incomplete dependencies: {incomplete}." + ) + return _transition(state, stage, StageStatus.RUNNING) + + +def complete_stage(state: TaskEngineState, stage: WorkflowStage) -> TaskEngineState: + """Complete one running stage.""" + if state.stages[stage] != StageStatus.RUNNING: + raise ValueError(f"Stage {stage.value!r} is not running.") + return _transition(state, stage, StageStatus.SUCCEEDED) + + +def fail_stage( + state: TaskEngineState, + stage: WorkflowStage, + *, + reason: str, +) -> TaskEngineState: + """Fail a stage, including a later retry of a previously successful stage.""" + if state.terminal: + raise ValueError("A terminal TaskEngineState cannot fail another stage.") + if state.stages[stage] not in { + StageStatus.PENDING, + StageStatus.RUNNING, + StageStatus.SUCCEEDED, + }: + raise ValueError(f"Stage {stage.value!r} cannot be failed now.") + normalized_reason = str(reason).strip() + if not normalized_reason: + raise ValueError("A failed stage requires a non-empty reason.") + return _transition( + state, + stage, + StageStatus.FAILED, + details={"reason": normalized_reason}, + ) + + +def skip_stage(state: TaskEngineState, stage: WorkflowStage) -> TaskEngineState: + """Skip one optional pending stage.""" + if stage not in _SKIPPABLE_STAGES: + raise ValueError("Only the optional scene_edit stage can be skipped.") + if state.stages[stage] != StageStatus.PENDING: + raise ValueError(f"Stage {stage.value!r} is not pending.") + return _transition(state, stage, StageStatus.SKIPPED) + + +def replay_events( + request: TaskRunRequest, + events: Sequence[Mapping[str, Any]], +) -> TaskEngineState: + """Rebuild a state by validating and applying its transition audit. + + Args: + request: Original workflow request used to create the state. + events: Complete ordered event audit to validate and replay. + + Returns: + The immutable state reconstructed from the supplied audit. + + Raises: + TypeError: If the audit is not a sequence of event mappings. + ValueError: If any event is missing, altered, or not a valid transition. + """ + if not isinstance(events, Sequence) or isinstance(events, (str, bytes)): + raise TypeError("Task Engine events must be a sequence of mappings.") + recorded = [] + for event in events: + if not isinstance(event, Mapping): + raise TypeError("Each Task Engine event must be a mapping.") + recorded.append(deepcopy(dict(event))) + + state = initial_state(request) + initial_events = [dict(event) for event in state.events] + if recorded[: len(initial_events)] != initial_events: + raise ValueError("Replay event does not match the canonical initial state.") + + for expected in recorded[len(initial_events) :]: + try: + stage = WorkflowStage(expected["stage"]) + target = StageStatus(expected["to"]) + if target == StageStatus.RUNNING: + replayed = start_stage(state, stage) + elif target == StageStatus.SUCCEEDED: + replayed = complete_stage(state, stage) + elif target == StageStatus.FAILED: + replayed = fail_stage(state, stage, reason=expected["reason"]) + elif target == StageStatus.SKIPPED: + replayed = skip_stage(state, stage) + else: + raise ValueError(f"Unsupported replay target: {target.value!r}.") + except (KeyError, TypeError, ValueError) as exc: + raise ValueError("Replay event does not match a valid transition.") from exc + if dict(replayed.events[-1]) != expected: + raise ValueError("Replay event does not match the generated transition.") + state = replayed + return state + + +def _transition( + state: TaskEngineState, + stage: WorkflowStage, + status: StageStatus, + *, + details: dict[str, Any] | None = None, +) -> TaskEngineState: + previous = state.stages[stage] + stages = dict(state.stages) + stages[stage] = status + event = { + "sequence": len(state.events) + 1, + "stage": stage.value, + "from": previous.value, + "to": status.value, + } + if details: + event.update(deepcopy(details)) + return TaskEngineState( + request=dict(state.request), + stages=stages, + events=(*state.events, event), + ) diff --git a/embodichain/gen_sim/task_engine/task_program_bundle.py b/embodichain/gen_sim/task_engine/task_program_bundle.py new file mode 100644 index 000000000..f5142788b --- /dev/null +++ b/embodichain/gen_sim/task_engine/task_program_bundle.py @@ -0,0 +1,1894 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Materialize one SemanticTaskGraph as a configured Task Program bundle.""" + +from __future__ import annotations + +from copy import deepcopy +from dataclasses import dataclass +import json +import math +from pathlib import Path +from typing import Any, Final + +import numpy as np + +from embodichain.lab.task_program.language import load_task_program +from embodichain.utils.utility import load_config, save_config + +from .semantic_graph import SemanticTaskGraph, validate_semantic_task_graph +from ._task_program.assembly import ADAPTER_CONTRACT, load_deployment + +__all__ = ["TaskProgramBundlePaths", "generate_task_program_bundle"] + +_EMBODIMENT_COMPONENTS: Final = { + "dual_franka": "dual_franka_robotiq_arg2f_140.yaml", + "dual_franka_robotiq_arg2f_140": "dual_franka_robotiq_arg2f_140.yaml", +} + +# The generated ``move forward`` task intent has no metric distance. Phase one +# supports only the dual-Franka embodiment, whose two-arm top-down tray grasp +# retains a reachable continuation over 0.14 m. Keep this semantic route +# target in the integration builder: the Atomic Action must execute the exact +# grounded goal and must not silently clamp an unreachable caller request. +_DUAL_FRANKA_COORDINATED_TRANSPORT_DISTANCE: Final = 0.14 +_DUAL_FRANKA_HANDOVER_CLEARANCE: Final = 0.193 +_DUAL_FRANKA_TABLE_MOUNT_OFFSET: Final = 0.35 +_DUAL_FRANKA_PLACE_TCP_CLEARANCE: Final = 0.22 +_LATERAL_RELATION_DISTANCE: Final = 0.10 +_FRONT_RELATION_DISTANCE: Final = 0.18 +# Leave enough free space around a placed object's support reference for the +# configured parallel-jaw fingers to close during a later semantic Pick. A +# tall object aligned by E2 needs the larger margin because its side grasp +# sweeps the finger length through the support plane. Both routes still come +# from scene geometry rather than task-owned robot poses. +_RELATION_CLEARANCE: Final = 0.02 +_AXIS_ALIGNED_RELATION_CLEARANCE: Final = 0.04 +_PLACEMENT_CLEARANCE: Final = 0.01 +_RELATIVE_POSITION_TOLERANCE: Final = 0.04 +_AXIS_ALIGN_CALL_ID: Final = "simulation.axis_align" +_COORDINATED_TRANSPORT_CALL_ID: Final = "simulation.coordinated_transport" +_PARK_CALL_ID: Final = "simulation.park" +_PLACE_RELATIVE_CALL_ID: Final = "simulation.place_relative" +_STACK_PLACE_CALL_ID: Final = "gen_sim.stack_place" +_STACK_PICK_CALL_ID: Final = "gen_sim.stack_pick" + + +_MOVE_HELD_OBJECT_CALL_ID: Final = "simulation.move_held_object" +_ALIGN_HELD_CALL_ID: Final = "gen_sim.align_held" +_CLEAR_RELEASED_CALL_ID: Final = "gen_sim.clear_released" + + +_PICK_CALL_ID: Final = "simulation.pick" + +_POUR_CALL_ID: Final = "simulation.pour" + +_COORDINATED_HOLD_CALL_ID: Final = "simulation.coordinated_hold" + + +_UPRIGHT_RELEASE_CLEARANCE: Final = 0.04 + +_SLENDER_UPRIGHT_RELEASE_CLEARANCE: Final = 0.01 + +_UPRIGHT_STAGING_CLEARANCE: Final = 0.20 + +_E2_RELEASE_SAFETY_MARGIN: Final = 0.02 + + +@dataclass(frozen=True, slots=True) +class TaskProgramBundlePaths: + """Files composing one portable configured Task Program deployment. + + Attributes: + root: Bundle root directory. + deployment: Runnable Gym deployment configuration. + program: Embodiment-independent Task Program source. + integration: Scene and runtime-service integration configuration. + scene: Physical scene component. + embodiment: Robot, sensor, and skill-profile component. + execution_policy: Canonical runtime execution-policy component. + semantic_task_graph: Immutable source semantic graph. + integration_fingerprint: Composed integration identity artifact. + """ + + root: Path + deployment: Path + program: Path + integration: Path + scene: Path + embodiment: Path + execution_policy: Path + semantic_task_graph: Path + integration_fingerprint: Path + + +def generate_task_program_bundle( + graph: SemanticTaskGraph, + prepared_scene: Any, + output_dir: str | Path, + *, + robot_profile: str, + max_episodes: int | None = None, + max_episode_steps: int | None = None, +) -> tuple[SemanticTaskGraph, TaskProgramBundlePaths]: + """Write, compose, and provider-free preflight one semantic deployment. + + Args: + graph: Provider-free semantic task graph. Its provisional fingerprint + is replaced with the exact composed integration fingerprint. + prepared_scene: Scene Adapter output with physical and planner views. + output_dir: Fresh bundle staging directory. + robot_profile: Task Engine robot-profile selector. Phase one supports + only the canonical dual-Franka embodiment. + max_episodes: Optional Gym episode limit. + max_episode_steps: Optional Gym step limit. + + Returns: + Final fingerprint-bound graph and all generated paths. + + Raises: + ValueError: If the robot profile is unsupported or graph/scene + integration cannot be composed and preflighted. + """ + selected_graph = validate_semantic_task_graph(graph) + unsupported = sorted( + {node["task_type"] for node in selected_graph["nodes"]} + - {"E1", "E2", "E3", "E4", "E5"} + ) + if unsupported: + raise ValueError(f"Task Engine supports only E1-E5, not {unsupported}.") + normalized_profile = str(robot_profile).strip() + try: + embodiment_filename = _EMBODIMENT_COMPONENTS[normalized_profile] + except KeyError as exc: + raise ValueError( + "Phase-one semantic bundle generation supports only dual_franka; " + f"received robot_profile={normalized_profile!r}." + ) from exc + root = Path(output_dir).expanduser().resolve() + root.mkdir(parents=True, exist_ok=True) + component_root = root / "components" + task_program_root = root / "task_program" + component_root.mkdir(parents=True, exist_ok=True) + task_program_root.mkdir(parents=True, exist_ok=True) + + from embodichain.gen_sim.task_engine.orchestration.scene_assets import ( + normalize_scene_assets, + ) + + scene = normalize_scene_assets(prepared_scene, root) + selected_graph = _refine_upright_targets(selected_graph, scene) + selected_graph = _refine_coordinated_targets(selected_graph, scene) + for node in selected_graph["nodes"]: + if node["call"].get("call_id") == _PICK_CALL_ID: + node["call"]["call_id"] = f"gen_sim.pick.{node['task_instance_id']}" + paths = TaskProgramBundlePaths( + root=root, + deployment=root / "task_program_deployment.yaml", + program=task_program_root / "program.yaml", + integration=task_program_root / "integration.yaml", + scene=component_root / "scene.yaml", + embodiment=component_root / "embodiment.yaml", + execution_policy=component_root / "execution_policy.yaml", + semantic_task_graph=root / "semantic_task_graph.json", + integration_fingerprint=root / "integration_fingerprint.json", + ) + project_root = Path(__file__).resolve().parents[3] + embodiment_source = ( + project_root + / "embodichain_tasks/configs/components/embodiments" + / embodiment_filename + ) + policy_source = ( + project_root + / "embodichain_tasks/configs/components/execution_policies" + / "dual_arm_trajectory_verified.yaml" + ) + embodiment_payload = load_config(embodiment_source) + _bind_embodiment_to_scene(embodiment_payload, table_top_z=scene.table_top_z) + save_config(paths.embodiment, embodiment_payload) + policy_payload = load_config(policy_source) + # Centimetre-scale object goals need a settled arm before the core captures + # the next measured grasp transform. Grasp endpoints use effect evidence, + # not this arm-only terminal tracking metric. + policy_payload["tracking"]["terminal_max_abs_error"] = 0.05 + policy_payload["tracking"]["consecutive_acceptances"] = 5 + policy_payload["tracking"]["terminal_settle_timeout"] = 3.0 + if any(node["call"]["kind"] == "hand_over" for node in selected_graph["nodes"]): + # Preserve 44 frames each for transfer and receiver approach after the + # generated hand-close, hold, release-settle, and retreat allocations. + policy_payload["motion"]["sample_count"] = 180 + save_config(paths.execution_policy, policy_payload) + + program_id = _program_identifier(selected_graph["task_id"]) + scene_contract = f"{program_id}_scene_v1" + stability = _task_stability_payload(selected_graph, scene, embodiment_payload) + save_config( + paths.program, + _program_payload( + selected_graph, + program_id, + scene, + stability_presets=set(stability["presets"]), + ), + ) + _write_json( + task_program_root / "constraints.json", + stability, + ) + save_config( + paths.integration, + _integration_payload( + selected_graph, + scene, + program_id=program_id, + scene_contract=scene_contract, + ), + ) + save_config(paths.scene, _scene_payload(scene, program_id=program_id)) + save_config( + paths.deployment, + { + "id": f"GenSimTaskProgram-{program_id}-v1", + "max_episodes": int(max_episodes or 1), + "max_episode_steps": int( + max_episode_steps + or ( + 8000 + if any( + cfg["kind"] == "stack" for cfg in stability["presets"].values() + ) + else 6000 + ) + ), + "num_envs": 1, + "arena_space": 2.5, + "physics_config": {"enable_ccd": True}, + "env": { + "sim_steps_per_control": 4, + "events": { + "settle_objects_on_reset": { + "func": "wait_for_dynamic_objects_to_settle", + "mode": "reset", + "params": { + "entity_cfgs": [ + {"uid": str(item["uid"])} + for item in scene.rigid_objects + ], + "min_steps": 10, + # Tall generated objects may need several seconds + # to finish a final low-energy roll after import. + "max_steps": 600, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise", + }, + } + }, + "extensions": {}, + }, + "scene": {"component": "components/scene.yaml"}, + "embodiment": {"component": "components/embodiment.yaml"}, + "task_program": { + "program": "task_program/program.yaml", + "integration": "task_program/integration.yaml", + "execution_policy": "components/execution_policy.yaml", + }, + }, + ) + + embodiment = load_config(paths.embodiment) + deployment = load_deployment( + task_program=load_config(paths.deployment)["task_program"], + skill_profile=embodiment["skill_profile"], + base_dir=root, + ) + fingerprint = deployment.integration.integration_fingerprint + selected_graph["integration_fingerprint"] = fingerprint + selected_graph = validate_semantic_task_graph(selected_graph) + _write_json(paths.semantic_task_graph, selected_graph) + _write_json( + paths.integration_fingerprint, + { + "schema_version": "semantic_integration_fingerprint/v2", + "adapter_contract": ADAPTER_CONTRACT, + "integration_id": deployment.integration_id, + "integration_fingerprint": fingerprint, + "registration_fingerprint": deployment.integration.registration.fingerprint, + }, + ) + + program = load_task_program( + paths.program, + integration=deployment.selection, + validation_context=deployment.integration.registration.catalog, + ) + deployment.integration.registration.catalog.preflight(program) + return selected_graph, paths + + +def _refine_coordinated_targets( + graph: SemanticTaskGraph, scene: Any +) -> SemanticTaskGraph: + """Keep a carried object above its support at the declared terminal pose.""" + result = deepcopy(graph) + objects = {str(item["runtime_uid"]): item for item in scene.planner_objects} + for node in result["nodes"]: + call = node["call"] + if call["kind"] != "registered" or call["call_id"] != _COORDINATED_HOLD_CALL_ID: + continue + displacement = list(call["arguments"]["world_displacement"]) + if abs(float(displacement[2])) > 1e-8: + continue + source = objects[str(call["arguments"]["object"])] + bottom, top = _vertical_mesh_bounds(source, axis_aligned=False) + displacement[2] = max(0.05, min(0.10, 0.5 * (top - bottom) + 0.03)) + call["arguments"]["world_displacement"] = displacement + return validate_semantic_task_graph(result) + + +def _task_stability_payload( + graph: SemanticTaskGraph, + scene: Any, + embodiment: dict[str, Any], +) -> dict[str, Any]: + """Declare task conditions without adding public validator or runtime types.""" + objects = {str(item["runtime_uid"]): item for item in scene.planner_objects} + upright = { + str(node["call"]["arguments"]["object"]) + for node in graph["nodes"] + if node["call"]["kind"] == "registered" + and ( + node["task_type"] == "E2" or node["call"]["call_id"] == _ALIGN_HELD_CALL_ID + ) + and "object" in node["call"]["arguments"] + } + oriented_groups = { + node["task_instance_id"] + for node in graph["nodes"] + if node["task_type"] in {"E1", "E4"} + and node["call"].get("call_id") == _ALIGN_HELD_CALL_ID + and node["call"]["arguments"].get("target") == "current_object_pose" + } + terminal_nodes = {group["node_ids"][-1] for group in graph["task_groups"]} + routes = { + (r["object_id"], r["reference_entity_id"], r["relation"]): r + for r in _relative_place_route_payloads(graph, scene, settled=True) + } + motion_parts = { + resource["resource_id"]: next( + endpoint["control_part"] + for endpoint in resource["endpoints"] + if endpoint["endpoint_id"] == "motion" + ) + for resource in embodiment["skill_profile"]["resources"] + } + presets: dict[str, Any] = {} + for node in graph["nodes"]: + call = node["call"] + if call["kind"] == "place" and node.get("task_instance_id") in oriented_groups: + object_id = str(call["object"]) + presets[f"gen_sim.{node['id']}.stable"] = { + "kind": "upright", + "entity": object_id, + "local_axis": _longest_local_axis(objects[object_id]), + } + if call["kind"] != "registered": + continue + arguments = call["arguments"] + if call["call_id"] in {_PLACE_RELATIVE_CALL_ID, _STACK_PLACE_CALL_ID}: + object_id = str(arguments["object"]) + reference_id = str(arguments["reference"]) + if ( + node["task_type"] == "E2" + or node.get("task_instance_id") in oriented_groups + or ( + object_id in upright + and reference_id == "table" + and arguments["relation"] in {"on", "above"} + ) + ): + route = routes[(object_id, reference_id, arguments["relation"])] + presets[f"gen_sim.{node['id']}.stable"] = { + "kind": "upright", + "entity": object_id, + "local_axis": _longest_local_axis(objects[object_id]), + "reference": reference_id, + "displacement": route["world_displacement"], + } + elif ( + arguments["relation"] in {"on", "above"} + and object_id in upright + and reference_id in upright + ): + bottom, _ = _vertical_mesh_bounds(objects[object_id], axis_aligned=True) + _, top = _vertical_mesh_bounds(objects[reference_id], axis_aligned=True) + presets[f"gen_sim.{node['id']}.stable"] = { + "kind": "stack", + "entity": object_id, + "reference": reference_id, + "local_axis": _longest_local_axis(objects[object_id]), + "reference_axis": _longest_local_axis(objects[reference_id]), + "object_bottom": bottom, + "reference_top": top, + "reference_half_extents": [ + max( + 0.001, + _horizontal_half_extent( + objects[reference_id], + world_axis=axis, + axis_aligned=True, + ) + - 0.002, + ) + for axis in (0, 1) + ], + "minimum_alignment": math.cos(math.pi / 18.0), + } + elif ( + call["call_id"] == _ALIGN_HELD_CALL_ID + and node["task_type"] == "E4" + and node["id"] in terminal_nodes + ): + object_id = str(arguments["object"]) + presets[f"gen_sim.{node['id']}.stable"] = { + "kind": "hold", + "entity": object_id, + "local_axis": _longest_local_axis(objects[object_id]), + "motion_parts": [motion_parts[call["resources"]["primary"]]], + } + elif call["call_id"] in { + _COORDINATED_HOLD_CALL_ID, + _COORDINATED_TRANSPORT_CALL_ID, + }: + object_id = str(arguments["object"]) + position = list(_position(objects[object_id])) + support = objects[object_id].get("attributes", {}).get("final_support", {}) + if support.get("parent_uid") == "table" and scene.table_top_z is not None: + # The skill offsets the live resting pose, not the import hover gap. + bottom, _ = _vertical_mesh_bounds( + objects[object_id], axis_aligned=False + ) + position[2] = float(scene.table_top_z) - bottom + displacement = arguments["world_displacement"] + holding = call["call_id"] == _COORDINATED_HOLD_CALL_ID + presets[f"gen_sim.{node['id']}.stable"] = { + "kind": "hold" if holding else "placement", + "entity": object_id, + "target_position": [ + position[i] + float(displacement[i]) for i in range(3) + ], + "motion_parts": ( + [ + motion_parts[call["resources"][slot]] + for slot in ("left", "right") + ] + if holding + else [] + ), + "position_tolerance": 0.02 if holding else _RELATIVE_POSITION_TOLERANCE, + } + return {"schema_version": "gen_sim_task_constraints/v1", "presets": presets} + + +def _program_payload( + graph: SemanticTaskGraph, + program_id: str, + scene: Any | None = None, + *, + stability_presets: set[str] | None = None, +) -> dict[str, Any]: + axis_by_object = ( + {} + if scene is None + else { + str(item["runtime_uid"]): _longest_local_axis(item) + for item in scene.planner_objects + } + ) + relative_routes = ( + {} + if scene is None + else { + (route["object_id"], route["reference_entity_id"], route["relation"]): route + for route in _relative_place_route_payloads(graph, scene, settled=True) + } + ) + items = [ + _program_node( + node, + relative_routes=relative_routes, + axis_by_object=axis_by_object, + task_stability=f"gen_sim.{node['id']}.stable" + in (stability_presets or set()), + ) + for node in graph["nodes"] + ] + by_name = {item["name"]: item for item in items} + for group in graph["task_groups"]: + terminal = by_name[group["node_ids"][-1]] + for node_id in reversed(group["node_ids"]): + accepted = by_name[node_id] + policies = [ + policy + for policy in accepted.get("post", ()) + if policy["preset"].startswith("gen_sim.") + ] + if not policies: + policies = list(accepted.get("post", ())) + validators = accepted.get("validators", ()) + if not policies and not validators: + continue + if accepted is not terminal: + # Cleanup is still physical work; it can invalidate placement. + terminal.setdefault("post", []).extend(deepcopy(policies)) + terminal.setdefault("validators", []).extend(deepcopy(validators)) + break + return { + "program_id": program_id, + "targets": deepcopy(graph["targets"]), + "program": {"kind": "sequence", "items": items}, + } + + +def _program_node( + node: dict[str, Any], + *, + relative_routes: dict[tuple[str, str, str], dict[str, Any]] | None = None, + axis_by_object: dict[str, list[float]] | None = None, + task_stability: bool = False, +) -> dict[str, Any]: + """Materialize one task node as one canonical runtime segment.""" + call = deepcopy(node["call"]) + segment: dict[str, Any] = { + "kind": "segment", + "name": str(node["id"]), + "steps": {"kind": "invoke", "call": call}, + } + settle_entities: list[str] = [] + settle_preset = "rigid_object" + if call["kind"] == "place": + inside = call.get("inside") + if inside is not None: + parts = str(inside).split("__") + if len(parts) != 3 or parts[0] != "inside": + raise ValueError(f"Unsupported generated inside affordance {inside!r}.") + settle_preset = "contained_rigid_object" + settle_entities.append(str(call["object"])) + elif call["kind"] == "registered" and call["call_id"] in { + _COORDINATED_TRANSPORT_CALL_ID, + _PLACE_RELATIVE_CALL_ID, + _STACK_PLACE_CALL_ID, + }: + if call["call_id"] == _COORDINATED_TRANSPORT_CALL_ID: + settle_preset = "transported_rigid_object" + settle_entities.append(str(call["arguments"]["object"])) + if settle_entities: + segment["post"] = [ + { + "kind": "wait_stable", + "entity": settle_entity, + "preset": settle_preset, + } + for settle_entity in dict.fromkeys(settle_entities) + ] + if call["kind"] == "registered" and call["call_id"] in { + _PLACE_RELATIVE_CALL_ID, + _STACK_PLACE_CALL_ID, + }: + arguments = call["arguments"] + selector = ( + str(arguments["object"]), + str(arguments["reference"]), + str(arguments["relation"]), + ) + try: + route = (relative_routes or {})[selector] + except KeyError as exc: + raise ValueError( + f"Relative placement has no generated route for {selector!r}." + ) from exc + segment["validators"] = [ + { + "kind": "object_near_relative_target", + "object": route["object_id"], + "reference": route["reference_entity_id"], + "displacement": deepcopy(route["world_displacement"]), + "position_tolerance": _RELATIVE_POSITION_TOLERANCE, + } + ] + if ( + not task_stability + and node.get("task_type") == "E2" + and ( + call["kind"] == "place" + or ( + call["kind"] == "registered" + and call["call_id"] == _PLACE_RELATIVE_CALL_ID + ) + ) + ): + raise ValueError("E2 release requires a task-owned upright stability preset.") + if node.get("task_type") == "E3" and call["kind"] == "place": + target = call.get("at") + if type(target) is not dict or set(target) != {"kind", "target"}: + raise ValueError("Generated E3 Place requires one exact return target.") + segment["validators"] = [ + { + "kind": "object_near_target", + "object": str(call["object"]), + "target": str(target["target"]), + "position_tolerance": 0.06, + } + ] + if task_stability: + arguments = call.get("arguments", {}) + entity = call.get("object", arguments.get("object")) + segment.setdefault("post", []).append( + { + "kind": "wait_stable", + "entity": entity, + "preset": f"gen_sim.{node['id']}.stable", + } + ) + return segment + + +def _integration_payload( + graph: SemanticTaskGraph, + scene: Any, + *, + program_id: str, + scene_contract: str, +) -> dict[str, Any]: + scene_objects = {str(item["runtime_uid"]): item for item in scene.planner_objects} + referenced_objects: set[str] = set() + inside_routes: list[tuple[str, str, str]] = [] + on_routes: list[tuple[str, str, str]] = [] + coordinated_routes: list[tuple[str, str, tuple[float, float, float]]] = [] + coordinated_hold_routes: list[tuple[str, str, tuple[float, float, float]]] = [] + move_held_routes: list[dict[str, Any]] = [] + upright_move_objects: set[str] = set() + pick_routes: dict[str, list[dict[str, Any]]] = {} + pick_options: dict[str, dict[str, Any]] = {} + axis_align_objects: set[str] = set() + pour_objects: set[str] = set() + relative_lowerer_routes = _relative_place_route_payloads(graph, scene) + has_relative_place = False + has_park_call = False + for node in graph["nodes"]: + call = node["call"] + if call["kind"] in {"pick", "place", "hand_over"}: + referenced_objects.add(str(call["object"])) + if call["kind"] == "place" and "inside" in call: + affordance = str(call["inside"]) + parts = affordance.split("__") + if len(parts) != 3 or parts[0] != "inside": + raise ValueError( + f"Unsupported generated inside affordance {affordance!r}." + ) + container_id, object_id = parts[1], parts[2] + referenced_objects.add(container_id) + inside_routes.append((affordance, container_id, object_id)) + if call["kind"] == "place" and "on" in call: + affordance = str(call["on"]) + parts = affordance.split("__") + if len(parts) != 3 or parts[0] != "on": + raise ValueError(f"Unsupported generated on affordance {affordance!r}.") + support_id, object_id = parts[1], parts[2] + referenced_objects.add(support_id) + on_routes.append((affordance, support_id, object_id)) + if call["kind"] == "registered" and call["call_id"] in { + "simulation.coordinated_transport", + _COORDINATED_HOLD_CALL_ID, + }: + arguments = call["arguments"] + object_id = str(arguments["object"]) + referenced_objects.add(object_id) + displacement = tuple( + float(value) + for value in arguments.get( + "world_displacement", + (-_DUAL_FRANKA_COORDINATED_TRANSPORT_DISTANCE, 0.0, 0.0), + ) + ) + route = (object_id, str(arguments["target"]), displacement) + if call["call_id"] == _COORDINATED_HOLD_CALL_ID: + coordinated_hold_routes.append(route) + else: + coordinated_routes.append(route) + elif call["kind"] == "registered" and ( + call["call_id"] == _PICK_CALL_ID + or call["call_id"].startswith("gen_sim.pick.") + ): + arguments = call["arguments"] + object_id = str(arguments["object"]) + final_target_id = str(arguments["target"]) + final_pose = _single_target_pose(graph, final_target_id) + table_top = scene.table_top_z + if table_top is None or not math.isfinite(float(table_top)): + raise ValueError( + "Upright pickup requires the scene's measured table_top_z." + ) + referenced_objects.add(object_id) + referenced_objects.add("table") + call_id = call["call_id"] + source_axis = _longest_local_axis(scene_objects[object_id]) + if object_id in upright_move_objects | axis_align_objects: + world_axis = np.array([0.0, 0.0, 1.0]) + else: + from scipy.spatial.transform import Rotation + + world_axis = Rotation.from_euler( + "XYZ", + scene_objects[object_id].get("init_rot", [0.0, 0.0, 0.0]), + degrees=True, + ).apply(source_axis) + approach = np.array([0.0, 0.0, -1.0]) - world_axis + length = np.linalg.norm(approach) + approach = ( + np.array([0.0, 0.0, -1.0]) if length <= 1e-6 else approach / length + ) + options = { + "kind": "pick_up", + "pre_grasp_distance": 0.08, + "grasp_settle_steps": 16, + "pick_object_part": "center", + "approach_direction": approach.tolist(), + } + if call_id in pick_options and pick_options[call_id] != options: + raise ValueError( + "Distinct Pick policies require task-scoped aliases; regenerate the bundle." + ) + pick_options[call_id] = options + pick_routes.setdefault(call_id, []).append( + { + "object_id": object_id, + "target_id": final_target_id, + # E2 permits all declared world-yaw alternatives. Requiring + # the nominal yaw here would reject feasible upright grasps. + # Transport checks live reachability; yaw does not change + # the fingertip heights used by release-clearance screening. + "release_clearance_object_pose": {"kind": "pose", **final_pose}, + "release_clearance_plane_z": float(table_top), + "release_clearance_safety_margin": _E2_RELEASE_SAFETY_MARGIN, + "grasp_region": "upper_half", + } + ) + elif call["kind"] == "registered" and call["call_id"] == _AXIS_ALIGN_CALL_ID: + object_id = str(call["arguments"]["object"]) + referenced_objects.add(object_id) + axis_align_objects.add(object_id) + elif call["kind"] == "registered" and call["call_id"] in { + _PLACE_RELATIVE_CALL_ID, + _STACK_PLACE_CALL_ID, + }: + arguments = call["arguments"] + referenced_objects.add(str(arguments["object"])) + referenced_objects.add(str(arguments["reference"])) + has_relative_place = True + elif ( + call["kind"] == "registered" + and call["call_id"] == _MOVE_HELD_OBJECT_CALL_ID + ): + arguments = call["arguments"] + object_id = str(arguments["object"]) + target_id = str(arguments["target"]) + referenced_objects.add(object_id) + if node["task_type"] == "E2": + upright_move_objects.add(object_id) + pose = _single_target_pose(graph, target_id) + move_held_routes.append( + { + "object_id": object_id, + "target_id": target_id, + "pose": {"kind": "pose", **pose}, + } + ) + else: + reference = str(arguments["reference"]) + referenced_objects.add(reference) + move_held_routes.append( + { + "object_id": object_id, + "target_id": target_id, + "pose": { + "kind": "scene_entity", + "entity_id": reference, + "relative_pose": _translation_pose(0.05, -0.10, 0.125), + }, + } + ) + elif call["kind"] == "registered" and call["call_id"] == _POUR_CALL_ID: + object_id = str(call["arguments"]["object"]) + referenced_objects.add(object_id) + pour_objects.add(object_id) + elif call["kind"] == "registered" and call["call_id"] == _ALIGN_HELD_CALL_ID: + referenced_objects.add(str(call["arguments"]["object"])) + upright_move_objects.add(str(call["arguments"]["object"])) + elif call["kind"] == "registered" and call["call_id"] in { + _STACK_PICK_CALL_ID, + _CLEAR_RELEASED_CALL_ID, + }: + referenced_objects.add(str(call["arguments"]["object"])) + elif call["kind"] == "registered" and call["call_id"] == "simulation.park": + has_park_call = True + elif call["kind"] == "registered": + raise ValueError( + f"Unsupported generated registered call {call['call_id']!r}." + ) + + rigid_bindings: list[dict[str, Any]] = [] + for entity_id in sorted(referenced_objects | {"table"}): + source = scene_objects.get(entity_id) + if source is None: + raise ValueError( + f"Semantic graph references missing scene entity {entity_id!r}." + ) + affordances: list[dict[str, Any]] = [] + if str(source["role"]) == "rigid_object": + grasp_affordance = { + "entity_id": f"{entity_id}_grasp", + "kind": "antipodal_grasp", + } + if entity_id in axis_align_objects | pour_objects | upright_move_objects: + grasp_affordance["internal_axis"] = _longest_local_axis(source) + affordances.append(grasp_affordance) + for affordance_id, container_id, object_id in inside_routes: + if container_id != entity_id: + continue + lateral = 0.06 if "apple" in object_id else -0.06 + affordances.append( + { + "entity_id": affordance_id, + "kind": "container", + "native_name": affordance_id, + "object_target_pose": _translation_pose(lateral, 0.0, 0.008), + "release_clearance": 0.12, + } + ) + for affordance_id, support_id, object_id in on_routes: + if support_id != entity_id: + continue + child = scene_objects.get(object_id) + if child is None: + raise ValueError( + f"On relation references missing scene entity {object_id!r}." + ) + affordances.append( + { + "entity_id": affordance_id, + "kind": "support_surface", + "native_name": affordance_id, + "object_target_pose": _support_target_pose( + source, + child, + axis_aligned=entity_id + in (axis_align_objects | upright_move_objects), + ), + } + ) + rigid_bindings.append( + { + "entity_id": entity_id, + "simulation_uid": entity_id, + "dynamics": ( + "dynamic" if str(source["role"]) == "rigid_object" else "static" + ), + "semantic_type": str(source.get("category") or entity_id), + "affordances": affordances, + } + ) + + def coordinated_lowerer_routes( + routes: list[tuple[str, str, tuple[float, float, float]]], + ) -> list[dict[str, Any]]: + return [ + { + "object_id": object_id, + "target_id": target_id, + "world_displacement": list(displacement), + } + for object_id, target_id, displacement in routes + ] + + lowerer_routes = coordinated_lowerer_routes(coordinated_routes) + hold_lowerer_routes = coordinated_lowerer_routes(coordinated_hold_routes) + if len(pour_objects) > 1: + raise ValueError("One generated bundle currently supports one Pour object.") + return { + "integration_id": f"{program_id}_integration_v1", + "program_id": program_id, + "requires": { + "scene_contract": scene_contract, + "embodiment_contract": "dual_arm_parallel_gripper", + }, + "scene_binding": { + "contract_id": scene_contract, + "registry_id": f"{program_id}_scene_registry", + "rigid_objects": rigid_bindings, + "articulations": [], + "links": [], + }, + "profile": { + "defaults": { + "pick_up": {"primary": "left"}, + "place": {"primary": "left"}, + "hand_over": {"source": "left", "destination": "right"}, + "axis_align": {"primary": "left"}, + "coordinated_pickment": {"left": "left", "right": "right"}, + }, + "action_options": { + "pick": { + "kind": "pick_up", + "pick_object_part": "center", + "pre_grasp_distance": 0.15, + "lift_height": 0.16, + "approach_alignment_max_angle": 0.10, + "hand_interp_steps": 5, + "grasp_settle_steps": ( + 16 + if any( + node["call"]["kind"] == "hand_over" + for node in graph["nodes"] + ) + else 0 + ), + "grasp_commit_fraction": 1.0, + }, + "place": { + "kind": "place", + "hand_interp_steps": 12, + "release_settle_steps": 60, + "lift_height": 0.18, + "cartesian_waypoint_count": 2, + "preserve_current_object_orientation": True, + }, + "hand_over": { + "kind": "hand_over", + "pre_grasp_distance": 0.15, + "lift_height": 0.0, + "hand_interp_steps": 16, + "hold_steps": 8, + "retreat_steps": 36, + "retreat_distance": 0.10, + "receive_pick_object_part": "bottom", + "release_at_target": False, + "arm_selection": "bound", + }, + **pick_options, + **( + { + _PLACE_RELATIVE_CALL_ID: { + "kind": "place", + "hand_interp_steps": 12, + "release_settle_steps": 60, + "lift_height": 0.10, + "max_approach_retract_z": ( + float(scene.table_top_z) + + _DUAL_FRANKA_PLACE_TCP_CLEARANCE + ), + "cartesian_waypoint_count": 2, + "preserve_current_object_orientation": True, + } + } + if has_relative_place + else {} + ), + **( + {_MOVE_HELD_OBJECT_CALL_ID: {"kind": "move_held_object"}} + if move_held_routes + else {} + ), + **( + {_POUR_CALL_ID: {"kind": "pour", "rotate_angle": -1.0471975512}} + if pour_objects + else {} + ), + **( + { + _COORDINATED_HOLD_CALL_ID: { + "kind": "coordinated_pickment", + "object_motion_keyframes": 8, + "pre_grasp_distance": 0.10, + "lift_height": 0.08, + "hand_interp_steps": 10, + "hold_steps": 4, + "release": False, + "approach_direction": [0.0, 0.0, -1.0], + "left_to_right_arm_direction": [0.0, 1.0, 0.0], + "middle_empty_ratio": 0.4, + } + } + if hold_lowerer_routes + else {} + ), + **( + { + _AXIS_ALIGN_CALL_ID: { + "kind": "axis_align", + "pre_grasp_distance": 0.15, + "lift_height": 0.16, + "hand_interp_steps": 5, + "grasp_settle_steps": 0, + "grasp_commit_fraction": 1.0, + "target_axis": [0.0, 0.0, 1.0], + } + } + if axis_align_objects + else {} + ), + **( + {"simulation.park": {"kind": "move_joints"}} + if has_park_call + else {} + ), + **( + { + "simulation.coordinated_transport": { + "kind": "coordinated_pickment", + "object_motion_keyframes": 8, + "pre_grasp_distance": 0.10, + "lift_height": 0.08, + "hand_interp_steps": 10, + "hold_steps": 4, + "release": True, + "release_steps": 10, + "retreat_distance": 0.08, + "retreat_steps": 12, + "approach_direction": [0.0, 0.0, -1.0], + "left_to_right_arm_direction": [0.0, 1.0, 0.0], + "middle_empty_ratio": 0.4, + } + } + if lowerer_routes + else {} + ), + }, + # Effect truth stays in Semantic Skill. The generated task + # planner only selects the canonical built-in monitor for the + # curated effectful calls; it never computes a held relation or + # synthesizes a successful postcondition itself. + "effect_monitors": { + semantic_id: { + "monitor_id": "builtin.composite_effect", + "revision": "1", + "params": { + "consecutive_samples": 3, + # Generated cube/apple meshes admit equivalent grasp + # orientations. Translation and the gripper/contact + # clause remain strict physical checks; orientation + # is intentionally relaxed for this calibrated scene. + "attached_translation_threshold": 0.06, + "attached_rotation_threshold": 3.0, + "detached_translation_threshold": 0.08, + "detached_rotation_threshold": 3.141592653589793, + }, + } + for semantic_id in ( + "pick", + "place", + "hand_over", + "simulation.coordinated_transport", + _COORDINATED_HOLD_CALL_ID, + _AXIS_ALIGN_CALL_ID, + *pick_routes, + _PLACE_RELATIVE_CALL_ID, + ) + if any( + node["call"].get("kind") == semantic_id + or node["call"].get("call_id") == semantic_id + for node in graph["nodes"] + ) + }, + "grounding_providers": {"hand_over": "simulation.configured_handover_pose"}, + }, + "runtime_services": { + "handover_pose_providers": [ + { + "kind": "configured_pose", + # Keep the exchange above the tray rim. This is a + # semantic object-space staging target; the Atomic Action + # derives all arm/EEF poses from it at runtime. + "final_position": [ + 0.0, + -0.08, + _handover_position_z(scene.table_top_z), + ], + "final_quaternion_wxyz": [ + 0.7071067812, + 0.7071067812, + 0.0, + 0.0, + ], + } + ], + "registered_semantic_lowerers": [ + *[ + {"kind": "pick", "call_id": call_id, "routes": routes} + for call_id, routes in pick_routes.items() + ], + *( + [{"kind": "place_relative", "routes": relative_lowerer_routes}] + if has_relative_place + else [] + ), + *( + [{"kind": "move_held_object", "routes": move_held_routes}] + if move_held_routes + else [] + ), + *( + [ + { + "kind": "pour", + "object_id": next(iter(pour_objects)), + } + ] + if pour_objects + else [] + ), + *( + [ + { + "kind": "axis_align", + "object_ids": sorted(axis_align_objects), + } + ] + if axis_align_objects + else [] + ), + *([{"kind": "park"}] if has_park_call else []), + *( + [{"kind": "coordinated_hold", "routes": hold_lowerer_routes}] + if hold_lowerer_routes + else [] + ), + *( + [{"kind": "coordinated_transport", "routes": lowerer_routes}] + if lowerer_routes + else [] + ), + ], + }, + } + + +def _scene_payload(scene: Any, *, program_id: str) -> dict[str, Any]: + articulations = [deepcopy(value) for value in scene.articulations] + for articulation in articulations: + for semantic_only_key in ( + "affordances", + "attributes", + "category", + "description", + "initial_state", + "is_articulated", + "name", + "proxy_body_scale", + "proxy_glb_fpath", + ): + articulation.pop(semantic_only_key, None) + suffix = Path(str(articulation.get("fpath", ""))).suffix.lower() + if suffix in {".usd", ".usda", ".usdc"}: + # pytorch-kinematics accepts URDF XML only. These task skills use + # DexSim's native link poses/geometry and do not need a PK chain. + articulation["build_pk_chain"] = False + return { + "scene_id": f"{program_id}_generated_scene", + "simulation": { + "light": { + "direct": [ + { + "uid": "main_light", + "color": [0.6, 0.6, 0.6], + "intensity": 30.0, + "init_pos": [0.5, 0.0, 3.0], + } + ] + }, + "background": [deepcopy(value) for value in scene.background], + "rigid_object": [deepcopy(value) for value in scene.rigid_objects], + "rigid_object_group": [], + "articulation": articulations, + }, + } + + +def _bind_embodiment_to_scene( + embodiment: dict[str, Any], + *, + table_top_z: float | None, +) -> None: + """Bind the generated deployment's robot mount to the current tabletop.""" + if table_top_z is None or not math.isfinite(float(table_top_z)): + raise ValueError("Dual-Franka deployment requires a derived tabletop height.") + simulation = embodiment.get("simulation") + if not isinstance(simulation, dict): + raise ValueError("Embodiment component has no simulation mapping.") + init_pos = simulation.get("init_pos") + if not isinstance(init_pos, list) or len(init_pos) != 3: + raise ValueError("Embodiment simulation.init_pos must contain three values.") + simulation["init_pos"] = [ + float(init_pos[0]), + float(init_pos[1]), + float(table_top_z) - _DUAL_FRANKA_TABLE_MOUNT_OFFSET, + ] + + +def _relative_place_route_payloads( + graph: SemanticTaskGraph, + scene: Any, + *, + settled: bool = False, +) -> list[dict[str, Any]]: + """Project release targets or their expected post-settle support positions.""" + axis_align_objects: set[str] = set() + selectors: set[tuple[str, str, str]] = set() + stack_selectors: set[tuple[str, str, str]] = set() + upright_targets: dict[tuple[str, str, str], str] = {} + for node in graph["nodes"]: + call = node["call"] + if call["kind"] != "registered": + continue + if call["call_id"] in {_AXIS_ALIGN_CALL_ID, _ALIGN_HELD_CALL_ID} or ( + node.get("task_type") == "E2" + and call["call_id"] == _MOVE_HELD_OBJECT_CALL_ID + ): + axis_align_objects.add(str(call["arguments"]["object"])) + elif call["call_id"] in {_PLACE_RELATIVE_CALL_ID, _STACK_PLACE_CALL_ID}: + arguments = call["arguments"] + if call["call_id"] == _STACK_PLACE_CALL_ID: + stack_selectors.add( + ( + str(arguments["object"]), + str(arguments["reference"]), + str(arguments["relation"]), + ) + ) + if node.get("task_type") == "E2": + upright_targets[ + ( + str(arguments["object"]), + str(arguments["reference"]), + str(arguments["relation"]), + ) + ] = f"{node['task_instance_id']}_upright_target" + selectors.add( + ( + str(arguments["object"]), + str(arguments["reference"]), + str(arguments["relation"]), + ) + ) + scene_objects = {str(item["runtime_uid"]): item for item in scene.planner_objects} + routes: list[dict[str, Any]] = [] + for selector in sorted(selectors): + object_id, reference_id, relation = selector + if selector in upright_targets: + target = _single_target_pose(graph, upright_targets[selector]) + reference_position = _position(scene_objects[reference_id]) + displacement = [ + float(target["position"][index]) - reference_position[index] + for index in range(3) + ] + if settled: + # Release clearance is a command target, not a resting height. + displacement[2] -= _upright_release_clearance(scene_objects[object_id]) + else: + displacement = _relative_world_displacement( + relation, + object_id=object_id, + reference_id=reference_id, + scene_objects=scene_objects, + axis_align_objects=axis_align_objects, + table_top_z=scene.table_top_z, + ) + if settled and selector in stack_selectors: + displacement[2] -= _PLACEMENT_CLEARANCE + routes.append( + { + "object_id": object_id, + "reference_entity_id": reference_id, + "relation": relation, + "world_displacement": displacement, + } + ) + return routes + + +def _relative_world_displacement( + relation: str, + *, + object_id: str, + reference_id: str, + scene_objects: dict[str, Any], + axis_align_objects: set[str], + table_top_z: float | None, +) -> list[float]: + """Derive one world-frame semantic relation from trusted scene geometry.""" + try: + obj = scene_objects[object_id] + reference = scene_objects[reference_id] + except KeyError as exc: + raise ValueError( + f"Relative placement references missing scene entity {exc.args[0]!r}." + ) from exc + if relation in {"on", "above"}: + object_bottom, _ = _vertical_mesh_bounds( + obj, + axis_aligned=object_id in axis_align_objects, + ) + if reference_id == "table": + if table_top_z is None: + raise ValueError( + "Relative placement on the table requires a derived tabletop height." + ) + reference_position = _position(reference) + reference_top = float(table_top_z) - reference_position[2] + object_position = _position(obj) + x_offset = object_position[0] - reference_position[0] + y_offset = object_position[1] - reference_position[1] + else: + _, reference_top = _vertical_mesh_bounds( + reference, + axis_aligned=reference_id in axis_align_objects, + ) + x_offset = 0.0 + y_offset = 0.0 + return [ + x_offset, + y_offset, + reference_top - object_bottom + _PLACEMENT_CLEARANCE, + ] + + diagonal_relations = { + "front_left_of": ("front_of", "left_of"), + "front_right_of": ("front_of", "right_of"), + "back_left_of": ("behind", "left_of"), + "back_right_of": ("behind", "right_of"), + } + if relation in diagonal_relations: + components = [ + _relative_world_displacement( + component, + object_id=object_id, + reference_id=reference_id, + scene_objects=scene_objects, + axis_align_objects=axis_align_objects, + table_top_z=table_top_z, + ) + for component in diagonal_relations[relation] + ] + return [components[0][0], components[1][1], components[0][2]] + + object_support_z = _support_origin_z( + obj, + axis_aligned=object_id in axis_align_objects, + table_top_z=table_top_z, + ) + reference_support_z = _support_origin_z( + reference, + axis_aligned=reference_id in axis_align_objects, + table_top_z=table_top_z, + ) + displacement = [ + 0.0, + 0.0, + object_support_z - reference_support_z + _PLACEMENT_CLEARANCE, + ] + if relation == "right_of": + displacement[1] = _horizontal_relation_distance( + obj, + reference, + world_axis=1, + object_axis_aligned=object_id in axis_align_objects, + reference_axis_aligned=reference_id in axis_align_objects, + minimum=_LATERAL_RELATION_DISTANCE, + ) + elif relation == "left_of": + displacement[1] = -_horizontal_relation_distance( + obj, + reference, + world_axis=1, + object_axis_aligned=object_id in axis_align_objects, + reference_axis_aligned=reference_id in axis_align_objects, + minimum=_LATERAL_RELATION_DISTANCE, + ) + elif relation == "front_of": + displacement[0] = -_horizontal_relation_distance( + obj, + reference, + world_axis=0, + object_axis_aligned=object_id in axis_align_objects, + reference_axis_aligned=reference_id in axis_align_objects, + minimum=_FRONT_RELATION_DISTANCE, + ) + elif relation == "behind": + displacement[0] = _horizontal_relation_distance( + obj, + reference, + world_axis=0, + object_axis_aligned=object_id in axis_align_objects, + reference_axis_aligned=reference_id in axis_align_objects, + minimum=_FRONT_RELATION_DISTANCE, + ) + else: + raise ValueError(f"Unsupported relative placement relation {relation!r}.") + return displacement + + +def _horizontal_relation_distance( + obj: dict[str, Any], + reference: dict[str, Any], + *, + world_axis: int, + object_axis_aligned: bool, + reference_axis_aligned: bool, + minimum: float, +) -> float: + """Return geometry-aware center separation for one planar relation.""" + return max( + minimum, + _horizontal_half_extent( + obj, + world_axis=world_axis, + axis_aligned=object_axis_aligned, + ) + + _horizontal_half_extent( + reference, + world_axis=world_axis, + axis_aligned=reference_axis_aligned, + ) + + ( + _AXIS_ALIGNED_RELATION_CLEARANCE + if object_axis_aligned + else _RELATION_CLEARANCE + ), + ) + + +def _horizontal_half_extent( + source: dict[str, Any], + *, + world_axis: int, + axis_aligned: bool, +) -> float: + """Return a conservative horizontal half extent in the intended pose.""" + vertices = _mesh_vertices(source) + if axis_aligned: + dominant_axis = int(np.argmax(np.ptp(vertices, axis=0))) + horizontal_extents = np.delete(np.ptp(vertices, axis=0), dominant_axis) + return 0.5 * float(horizontal_extents.max()) + from scipy.spatial.transform import Rotation + + world_vertices = Rotation.from_euler( + "XYZ", + source.get("init_rot", [0.0, 0.0, 0.0]), + degrees=True, + ).apply(vertices) + return 0.5 * float(np.ptp(world_vertices[:, world_axis])) + + +def _support_origin_z( + source: dict[str, Any], + *, + axis_aligned: bool, + table_top_z: float | None, +) -> float: + """Return the object-origin height when resting on the scene table.""" + if not axis_aligned: + return _position(source)[2] + if table_top_z is None: + raise ValueError("Axis-aligned placement requires a derived tabletop height.") + bottom, _ = _vertical_mesh_bounds(source, axis_aligned=True) + return float(table_top_z) - bottom + _PLACEMENT_CLEARANCE + + +def _dominant_local_axis(source: dict[str, Any]) -> list[float]: + """Return the unique major mesh axis in DexSim's object-local basis.""" + vertices = _mesh_vertices(source) + extents = np.ptp(vertices, axis=0) + order = np.argsort(extents) + major = int(order[-1]) + if extents[major] <= max(float(extents[order[-2]]) * 1.25, 1.0e-6): + raise ValueError( + f"Scene object {source.get('runtime_uid')!r} has no unique dominant " + "local axis for semantic upright alignment." + ) + axis = [0.0, 0.0, 0.0] + axis[major] = 1.0 + return axis + + +def _vertical_mesh_bounds( + source: dict[str, Any], + *, + axis_aligned: bool, +) -> tuple[float, float]: + """Return bottom/top offsets for the intended object orientation.""" + vertices = _mesh_vertices(source) + if axis_aligned: + axis = np.asarray(_dominant_local_axis(source), dtype=np.float64) + heights = vertices @ axis + else: + from scipy.spatial.transform import Rotation + + heights = Rotation.from_euler( + "XYZ", + source.get("init_rot", [0.0, 0.0, 0.0]), + degrees=True, + ).apply(vertices)[:, 2] + return float(heights.min()), float(heights.max()) + + +def _mesh_vertices(source: dict[str, Any]) -> np.ndarray: + """Load one configured mesh in DexSim's object-local coordinate basis.""" + shape = source.get("shape") + if not isinstance(shape, dict) or not shape.get("fpath"): + raise ValueError( + f"Scene object {source.get('runtime_uid')!r} has no mesh geometry." + ) + try: + import trimesh + + 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) + except Exception as exc: + raise ValueError( + f"Could not inspect scene mesh for {source.get('runtime_uid')!r}: {exc}" + ) from exc + if vertices.ndim != 2 or vertices.shape[1] != 3 or not vertices.size: + raise ValueError( + f"Scene object {source.get('runtime_uid')!r} has empty mesh geometry." + ) + # DexSim converts glTF's Y-up vertices into its Z-up object-local basis. + result = np.column_stack((vertices[:, 0], -vertices[:, 2], vertices[:, 1])) + scale = np.asarray(source.get("body_scale", [1.0, 1.0, 1.0]), dtype=np.float64) + if scale.shape != (3,) or not np.isfinite(scale).all(): + raise ValueError( + f"Scene object {source.get('runtime_uid')!r} has invalid body_scale." + ) + return result * scale + + +def _position(source: dict[str, Any]) -> tuple[float, float, float]: + """Return one finite source position.""" + value = source.get("init_pos") + if not isinstance(value, list) or len(value) != 3: + raise ValueError( + f"Scene object {source.get('runtime_uid')!r} has no three-value init_pos." + ) + result = tuple(float(item) for item in value) + if not all(math.isfinite(item) for item in result): + raise ValueError( + f"Scene object {source.get('runtime_uid')!r} has invalid init_pos." + ) + return result + + +def _handover_position_z(table_top_z: float | None) -> float: + """Place the dual-arm exchange at a scene-relative reachable clearance.""" + if table_top_z is None or not math.isfinite(float(table_top_z)): + raise ValueError("Hand-over grounding requires a derived tabletop height.") + return float(table_top_z) + _DUAL_FRANKA_HANDOVER_CLEARANCE + + +def _translation_pose(x: float, y: float, z: float) -> list[float]: + return [ + 1.0, + 0.0, + 0.0, + float(x), + 0.0, + 1.0, + 0.0, + float(y), + 0.0, + 0.0, + 1.0, + float(z), + 0.0, + 0.0, + 0.0, + 1.0, + ] + + +def _program_identifier(task_id: str) -> str: + normalized = "".join( + character if character.isalnum() or character == "_" else "_" + for character in str(task_id).strip().lower() + ).strip("_") + if not normalized: + raise ValueError("task_id does not contain a usable program identifier.") + return f"gen_sim_{normalized}" + + +def _write_json(path: Path, value: Any) -> None: + path.write_text( + json.dumps(value, ensure_ascii=False, indent=2, allow_nan=False) + "\n", + encoding="utf-8", + ) + + +def _single_target_pose( + graph: SemanticTaskGraph, + target_id: str, +) -> dict[str, Any]: + """Return one exact generated target pose.""" + target = graph["targets"].get(target_id) + if not isinstance(target, dict): + raise ValueError(f"Missing generated target {target_id!r}.") + values = target.get("values") + if not isinstance(values, list) or len(values) != 1: + raise ValueError(f"Generated target {target_id!r} must contain one pose.") + pose = values[0] + if ( + not isinstance(pose, dict) + or not isinstance(pose.get("position"), list) + or len(pose["position"]) != 3 + or not isinstance(pose.get("quaternion_wxyz"), list) + or len(pose["quaternion_wxyz"]) != 4 + ): + raise ValueError(f"Generated target {target_id!r} has an invalid pose.") + return pose + + +def _longest_local_axis(source: dict[str, Any]) -> list[float]: + """Return the longest axis in the simulator rigid-body local frame.""" + source_axis = _longest_source_axis(source) + shape = source.get("shape", {}) + path = shape.get("fpath") if isinstance(shape, dict) else None + if path and Path(str(path)).suffix.lower() in {".glb", ".gltf"}: + # glTF is Y-up. DexSim converts it to Z-up while importing the render + # mesh, so semantic axes must follow the same source-to-body mapping. + x_axis, y_axis, z_axis = source_axis + return [x_axis, -z_axis, y_axis] + return source_axis + + +def _longest_source_axis(source: dict[str, Any]) -> list[float]: + """Return the longest axis in the source mesh coordinate frame.""" + extents = _local_extents(source) + if extents is None: + return [0.0, 0.0, 1.0] + axis = [0.0, 0.0, 0.0] + axis[max(range(3), key=extents.__getitem__)] = 1.0 + return axis + + +def _local_extents(source: dict[str, Any]) -> list[float] | None: + """Return finite positive local geometry extents when available.""" + bounds = _local_bounds(source) + if bounds is None: + return None + return [maximum - minimum for minimum, maximum in zip(*bounds, strict=True)] + + +def _local_bounds(source: dict[str, Any]) -> tuple[list[float], list[float]] | None: + """Return finite local geometry bounds when available.""" + shape = source.get("shape", {}) + if not isinstance(shape, dict): + return None + if shape.get("shape_type") == "Cube": + size = shape.get("size") + if isinstance(size, list) and len(size) == 3: + extents = [float(value) for value in size] + if min(extents) <= 0.0: + return None + return ( + [-value / 2.0 for value in extents], + [value / 2.0 for value in extents], + ) + path = shape.get("fpath") + if not path: + return None + try: + import trimesh + + geometry = trimesh.load(str(path), force="scene").to_geometry() + bounds = [[float(value) for value in row] for row in geometry.bounds] + except (OSError, TypeError, ValueError): + return None + if ( + len(bounds) != 2 + or any(len(row) != 3 for row in bounds) + or any(maximum <= minimum for minimum, maximum in zip(*bounds, strict=True)) + ): + return None + return bounds[0], bounds[1] + + +def _upright_release_clearance(source: dict[str, Any]) -> float: + """Return the E2 recipe's drop distance above the final support surface.""" + extents = _local_extents(source) + if extents is None: + raise ValueError("Upright release requires finite object extents.") + ordered_extents = sorted(extents) + slenderness = ordered_extents[-1] / max(ordered_extents[-2], 1.0e-6) + return ( + _SLENDER_UPRIGHT_RELEASE_CLEARANCE + if slenderness >= 2.5 + else _UPRIGHT_RELEASE_CLEARANCE + ) + + +def _refine_upright_targets( + graph: SemanticTaskGraph, + scene: Any, +) -> SemanticTaskGraph: + """Use normalized mesh origins to place E2 objects on the measured table.""" + selected = deepcopy(graph) + objects = {str(item["runtime_uid"]): item for item in scene.planner_objects} + table = objects.get("table") + if table is None: + return selected + if scene.table_top_z is None: + return selected + table_top = float(scene.table_top_z) + target_routes: dict[str, tuple[str, str]] = {} + for node in selected["nodes"]: + call = node["call"] + if ( + node.get("task_type") not in {"E1", "E2", "E4"} + or call.get("kind") != "registered" + or call.get("call_id") + not in { + _MOVE_HELD_OBJECT_CALL_ID, + _PICK_CALL_ID, + _ALIGN_HELD_CALL_ID, + _CLEAR_RELEASED_CALL_ID, + } + ): + continue + arguments = call.get("arguments", {}) + object_id = str(arguments.get("object", "")) + target_id = str(arguments.get("target", "")) + resource = str(call.get("resources", {}).get("primary", "")) + target_routes[target_id] = (object_id, resource) + # Place owns the final descent; its release target still needs the + # same mesh-origin refinement when no separate final Move is emitted. + target_routes[f"{node['task_instance_id']}_upright_target"] = ( + object_id, + resource, + ) + for target_id, (object_id, resource) in target_routes.items(): + source = objects.get(object_id) + target = selected["targets"].get(target_id) if target_id is not None else None + bounds = _local_bounds(source) if source is not None else None + if bounds is None or not isinstance(target, dict): + continue + source_axis = _longest_source_axis(source) + axis = _longest_local_axis(source) + axis_index = source_axis.index(1.0) + local_minimum = bounds[0][axis_index] + values = target.get("values") + if not isinstance(values, list) or len(values) != 1: + continue + position = values[0].get("position") + if not isinstance(position, list) or len(position) != 3: + continue + clearance = _upright_release_clearance(source) + position[2] = table_top + clearance - local_minimum + if target_id.endswith("_upright_staging_target"): + position[2] += _UPRIGHT_STAGING_CLEARANCE + values[0]["quaternion_wxyz"] = _upright_target_quaternion( + source, + axis, + world_yaw=(math.pi if resource == "right" else 0.0), + ) + return validate_semantic_task_graph(selected) + + +def _upright_target_quaternion( + source: dict[str, Any], + local_axis: list[float], + *, + world_yaw: float, +) -> list[float]: + """Rotate the current object frame so its selected positive axis is world +Z.""" + import torch + + from embodichain.utils.math import ( + axis_angle_to_rotation_matrix, + matrix_from_quat, + quat_from_euler_xyz, + quat_from_matrix, + ) + + init_rot = source.get("init_rot") + if not isinstance(init_rot, list) or len(init_rot) != 3: + raise ValueError( + f"E2 object {source.get('runtime_uid')!r} requires three-value init_rot." + ) + angles = torch.deg2rad(torch.tensor(init_rot, dtype=torch.float32)) + quaternion = quat_from_euler_xyz(angles[0], angles[1], angles[2]) + rotation = matrix_from_quat(quaternion) + axis = torch.tensor(local_axis, dtype=torch.float32) + source_axis = torch.nn.functional.normalize(rotation @ axis, dim=0) + target_axis = torch.tensor([0.0, 0.0, 1.0], dtype=torch.float32) + cross = torch.linalg.cross(source_axis, target_axis) + dot = torch.clamp(torch.dot(source_axis, target_axis), -1.0, 1.0) + cross_norm = torch.linalg.vector_norm(cross) + if float(cross_norm) <= 1.0e-6: + if float(dot) >= 0.0: + delta = torch.eye(3, dtype=torch.float32) + else: + basis = torch.eye(3, dtype=torch.float32)[ + torch.argmin(torch.abs(source_axis)) + ] + perpendicular = torch.nn.functional.normalize( + torch.linalg.cross(source_axis, basis), dim=0 + ) + delta = axis_angle_to_rotation_matrix(perpendicular * math.pi) + else: + rotation_axis = cross / cross_norm + angle = torch.atan2(cross_norm, dot) + delta = axis_angle_to_rotation_matrix(rotation_axis * angle) + target_rotation = delta @ rotation + if world_yaw: + target_rotation = ( + axis_angle_to_rotation_matrix( + torch.tensor([0.0, 0.0, world_yaw], dtype=torch.float32) + ) + @ target_rotation + ) + target_quaternion = quat_from_matrix(target_rotation) + return [float(value) for value in target_quaternion] + + +def _support_target_pose( + support: dict[str, Any], + child: dict[str, Any], + *, + axis_aligned: bool, +) -> list[float]: + """Build a conservative object-center target above a support entity.""" + support_extents = _local_extents(support) or [0.1, 0.1, 0.1] + child_extents = _local_extents(child) or [0.05, 0.05, 0.05] + child_half_height = max(child_extents) / 2.0 + translation = [0.0, 0.0, 0.0] + if axis_aligned: + normal = _longest_local_axis(support) + support_bounds = _local_bounds(support) + source_axis = _longest_source_axis(support) + source_axis_index = source_axis.index(1.0) + support_top = ( + support_extents[source_axis_index] / 2.0 + if support_bounds is None + else support_bounds[1][source_axis_index] + ) + distance = support_top + child_half_height + 0.01 + translation = [component * distance for component in normal] + else: + attributes = support.get("attributes", {}) + aabb = ( + attributes.get("final_world_aabb") if isinstance(attributes, dict) else None + ) + position = support.get("init_pos", [0.0, 0.0, 0.0]) + if isinstance(aabb, dict) and isinstance(aabb.get("max"), list): + surface_height = float(aabb["max"][2]) - float(position[2]) + else: + surface_height = support_extents[2] / 2.0 + translation[2] = surface_height + child_half_height + 0.01 + return _translation_pose(*translation) diff --git a/embodichain/gen_sim/task_engine/workflow.py b/embodichain/gen_sim/task_engine/workflow.py new file mode 100644 index 000000000..3fdcda234 --- /dev/null +++ b/embodichain/gen_sim/task_engine/workflow.py @@ -0,0 +1,1358 @@ +# ---------------------------------------------------------------------------- +# 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 Task Engine workflow with bounded, fully audited recovery.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from concurrent.futures import ThreadPoolExecutor +from copy import deepcopy +from dataclasses import dataclass, replace +from datetime import datetime +import json +from pathlib import Path +import shutil +import subprocess +import sys +from typing import Any, Final + +from embodichain.gen_sim.task_engine.reporting import ( + EXECUTION_REPORT_FILENAME, + validate_execution_report, +) +from embodichain.gen_sim.scene_engine.errors import SceneServiceError + +from .agent import TaskAgent +from .config import ( + TaskEngineExecutionCfg, + TaskEnginePlanningCfg, + TaskEngineWorkflowCfg, + load_task_engine_config, +) +from .contracts import canonical_hash +from .orchestration.artifacts import ArtifactTransaction +from .orchestration.coordinator import PreparationResult, TaskEngineCoordinator +from .orchestration.scene_adapter import ( + CandidateSelection, + SceneAdapter, + SceneAdapterProtocolError, +) +from .orchestration.scene_source import SceneSourceRef +from .semantic_planner import UnsupportedSemanticCapabilityError +from .scene_backend import ( + SceneAnalysis, + SceneEngineBackend, + SceneRemediableError, + SceneRevision, +) +from .state_machine import ( + TaskEngineState, + WorkflowStage, + complete_stage, + fail_stage, + initial_state, + start_stage, +) +from .workflow_contracts import TaskRunRequest, validate_task_run_request + +__all__ = [ + "TASK_ENGINE_RUN_MANIFEST_SCHEMA", + "ActionExecutor", + "SubprocessActionExecutor", + "TaskEngineRunResult", + "TaskEngineWorkflow", +] + +TASK_ENGINE_RUN_MANIFEST_SCHEMA: Final = "embodichain.task-engine-run/v1" +ActionExecutor = Callable[..., Mapping[str, Any]] + + +@dataclass(frozen=True) +class TaskEngineRunResult: + """Published outcome of one isolated cross-engine workflow run.""" + + status: str + output_dir: Path + manifest_path: Path + state_path: Path + final_bundle: Path | None + failure_class: str | None = None + + @property + def succeeded(self) -> bool: + """Return whether real simulator execution met the configured policy.""" + return self.status == "succeeded" + + +class SubprocessActionExecutor: + """Execute a prepared bundle through Task Engine's private runner.""" + + def __call__( + self, + bundle: str | Path, + output_root: str | Path, + *, + seed: int, + num_envs: int, + dataset_saving: bool = False, + failure_policy: str = "stop", + open_window: bool = False, + ) -> Mapping[str, Any]: + """Run one simulator attempt and preserve its report and trajectory. + + Args: + bundle: Prepared Task Program bundle. + output_root: Fresh directory for this execution attempt. + seed: Simulator random seed. + num_envs: Number of vectorized scene replicas. + dataset_saving: Whether to enable the Gym project's dataset recorder. + failure_policy: Whether failed dependencies stop or permit downstream + diagnostic execution. + open_window: Whether to open the native DexSim execution window. + + Returns: + Validated Task Program execution report. + """ + bundle_root = Path(bundle).expanduser().resolve() + attempt_root = Path(output_root).expanduser().resolve() + if failure_policy not in {"stop", "continue"}: + raise ValueError("failure_policy must be 'stop' or 'continue'.") + if not isinstance(open_window, bool): + raise TypeError("open_window must be a boolean.") + attempt_root.mkdir(parents=True, exist_ok=False) + command = [ + sys.executable, + "-u", + "-m", + "embodichain.gen_sim.task_engine._bundle_runner", + "--bundle", + bundle_root.as_posix(), + "--execution-output", + attempt_root.as_posix(), + "--num_envs", + str(num_envs), + "--seed", + str(seed), + ] + if not open_window: + command.extend(["--headless", "--renderer", "fast-rt"]) + if not dataset_saving: + command.append("--filter_dataset_saving") + command.extend(["--failure-policy", failure_policy]) + log_path = attempt_root / "action.log" + print( + "[Task Engine] Starting " + f"{attempt_root.name}: seed={seed}, num_envs={num_envs}, " + f"dataset_saving={dataset_saving}, open_window={open_window}, " + f"failure_policy={failure_policy}", + flush=True, + ) + completed = _run_streaming_process(command, log_path) + print( + f"[Task Engine] Completed {attempt_root.name}: " + f"returncode={completed.returncode}", + flush=True, + ) + report_path = attempt_root / EXECUTION_REPORT_FILENAME + process_record = { + "command": command, + "returncode": completed.returncode, + "combined_log": log_path.name, + "stdout": completed.stdout, + "stderr": completed.stderr, + } + _write_json(attempt_root / "process.json", process_record) + if not report_path.is_file(): + raise RuntimeError( + "Task Program execution did not publish execution_report.json; " + f"returncode={completed.returncode}." + ) + report = validate_execution_report(_read_json(report_path)) + trajectory_copy = _copy_trajectory_record(report, attempt_root) + if report["semantic_call_count"] > 0 and trajectory_copy is None: + raise RuntimeError( + "Task Program report did not expose a readable trajectory record." + ) + _write_json( + attempt_root / "execution_attempt.json", + { + "seed": seed, + "num_envs": num_envs, + "dataset_saving": dataset_saving, + "failure_policy": failure_policy, + "returncode": completed.returncode, + "trajectory_copy": trajectory_copy, + "report": report, + }, + ) + return report + + +def _run_streaming_process( + command: list[str], + log_path: str | Path, +) -> subprocess.CompletedProcess[str]: + """Run a child while teeing its combined output to the terminal and disk. + + Args: + command: Argument vector passed directly to the child process. + log_path: File receiving the exact combined stdout and stderr bytes. + + Returns: + Completed process metadata with a decoded copy of the combined output. + """ + resolved_log = Path(log_path).expanduser().resolve() + resolved_log.parent.mkdir(parents=True, exist_ok=True) + captured = bytearray() + process: subprocess.Popen[bytes] | None = None + try: + with resolved_log.open("wb") as log_stream: + process = subprocess.Popen( + command, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + bufsize=0, + ) + assert process.stdout is not None + while True: + chunk = process.stdout.read(64 * 1024) + if not chunk: + break + captured.extend(chunk) + log_stream.write(chunk) + log_stream.flush() + _write_terminal_chunk(chunk) + returncode = process.wait() + except BaseException: + if process is not None and process.poll() is None: + process.terminate() + try: + process.wait(timeout=5) + except subprocess.TimeoutExpired: + process.kill() + process.wait() + raise + output = captured.decode("utf-8", errors="replace") + return subprocess.CompletedProcess( + args=command, + returncode=returncode, + stdout=output, + stderr="", + ) + + +def _write_terminal_chunk(chunk: bytes) -> None: + """Best-effort write of raw child output to the parent terminal.""" + try: + stream = getattr(sys.stdout, "buffer", None) + if stream is not None: + stream.write(chunk) + stream.flush() + return + sys.stdout.write(chunk.decode("utf-8", errors="replace")) + sys.stdout.flush() + except (BrokenPipeError, OSError, ValueError): + return + + +class TaskEngineWorkflow: + """Run Scene and Action work concurrently under Task Engine ownership.""" + + def __init__( + self, + *, + task_agent: TaskAgent | None = None, + scene_adapter: SceneAdapter | None = None, + coordinator: TaskEngineCoordinator | None = None, + scene_backend: SceneEngineBackend | None = None, + action_executor: ActionExecutor | None = None, + ) -> None: + self.task_agent = task_agent or TaskAgent() + self.scene_adapter = scene_adapter or SceneAdapter() + self.coordinator = coordinator or TaskEngineCoordinator( + task_agent=self.task_agent, + scene_adapter=self.scene_adapter, + ) + self.scene_backend = scene_backend or SceneEngineBackend() + self.action_executor = action_executor or SubprocessActionExecutor() + + def run( + self, + request: TaskRunRequest | Mapping[str, Any], + *, + workflow_cfg: TaskEngineWorkflowCfg | None = None, + planning_cfg: TaskEnginePlanningCfg | None = None, + execution_cfg: TaskEngineExecutionCfg | None = None, + config_path: str | Path | None = None, + model: str | None = None, + base_seed: int = 0, + dataset_saving: bool = False, + failure_policy: str = "stop", + open_window: bool = False, + run_id: str | None = None, + created_at: datetime | None = None, + overwrite: bool = False, + execute: bool = True, + ) -> TaskEngineRunResult: + """Run all stages and publish success only after simulator acceptance. + + Args: + request: One of the four image/project plus optional-edit inputs. + workflow_cfg: Optional retry and concurrency configuration. + planning_cfg: Optional interpretation and bundle generation defaults. + execution_cfg: Optional vectorized success policy. + config_path: YAML used for omitted workflow or execution config. + model: Optional Task and grounding model override. + base_seed: First audited scene and action attempt seed. + dataset_saving: Whether Action attempts may initialize dataset recording. + failure_policy: Whether failed dependencies stop or permit downstream + diagnostic execution. + open_window: Whether simulator attempts open the native DexSim window. + run_id: Optional externally allocated run identifier. + created_at: Optional timezone-aware run creation timestamp. + overwrite: Whether to atomically replace an existing run directory. + execute: Whether to execute the prepared bundle in the simulator. + + Returns: + Published run status, manifest, state audit, and final bundle path. + """ + normalized = validate_task_run_request(request) + if not isinstance(dataset_saving, bool): + raise TypeError("dataset_saving must be a boolean.") + if not isinstance(open_window, bool): + raise TypeError("open_window must be a boolean.") + if failure_policy not in {"stop", "continue"}: + raise ValueError("failure_policy must be 'stop' or 'continue'.") + if workflow_cfg is None or planning_cfg is None or execution_cfg is None: + loaded_workflow, loaded_planning, loaded_execution = ( + load_task_engine_config(config_path) + ) + workflow_cfg = workflow_cfg or loaded_workflow + planning_cfg = planning_cfg or loaded_planning + execution_cfg = execution_cfg or loaded_execution + effective_candidate_count = planning_cfg.candidate_count + effective_run_id = str(run_id or Path(normalized["output_dir"]).name).strip() + if not effective_run_id or Path(effective_run_id).name != effective_run_id: + raise ValueError("run_id must be one non-empty path component.") + effective_created_at = created_at or datetime.now().astimezone() + if ( + effective_created_at.tzinfo is None + or effective_created_at.utcoffset() is None + ): + raise ValueError("created_at must include a timezone.") + run_metadata = { + "run_id": effective_run_id, + "created_at": effective_created_at.isoformat(), + "dataset_saving": bool(dataset_saving), + "failure_policy": str(failure_policy), + } + state = initial_state(normalized) + attempts: list[dict[str, Any]] = [] + output_dir = Path(normalized["output_dir"]) + + with ArtifactTransaction(output_dir, overwrite=overwrite) as transaction: + staging = transaction.staging_dir + assert staging is not None + analysis_root = staging / "scene_analysis" + state = start_stage(state, WorkflowStage.TASK_CANDIDATES) + state = start_stage(state, WorkflowStage.SCENE_PREPARATION) + with ThreadPoolExecutor( + max_workers=workflow_cfg.max_parallel_workers, + thread_name_prefix="task-engine-input", + ) as executor: + candidate_future = executor.submit( + self.task_agent.generate, + normalized["task_id"], + normalized["task_instruction"], + model, + effective_candidate_count, + ) + analysis_future = executor.submit( + self.scene_backend.analyze, + normalized, + analysis_root, + ) + try: + candidate_set = candidate_future.result() + except Exception as exc: + analysis_future.cancel() + state = fail_stage( + state, + WorkflowStage.TASK_CANDIDATES, + reason=str(exc), + ) + return self._publish( + transaction, + staging, + normalized, + workflow_cfg, + planning_cfg, + execution_cfg, + run_metadata, + state, + attempts, + status="failed", + failure_class="task_generation", + ) + state = complete_stage(state, WorkflowStage.TASK_CANDIDATES) + try: + analysis = analysis_future.result() + except Exception as exc: + state = fail_stage( + state, + WorkflowStage.SCENE_PREPARATION, + reason=str(exc), + ) + return self._publish( + transaction, + staging, + normalized, + workflow_cfg, + planning_cfg, + execution_cfg, + run_metadata, + state, + attempts, + status="failed", + failure_class="scene_analysis", + ) + state = complete_stage(state, WorkflowStage.SCENE_PREPARATION) + + state = start_stage(state, WorkflowStage.CANDIDATE_SELECTION) + try: + selection = self.scene_backend.select( + analysis, + candidate_set, + self.scene_adapter, + force_most_likely=True, + ) + except SceneAdapterProtocolError as exc: + state = fail_stage( + state, + WorkflowStage.CANDIDATE_SELECTION, + reason=str(exc), + ) + return self._publish( + transaction, + staging, + normalized, + workflow_cfg, + planning_cfg, + execution_cfg, + run_metadata, + state, + attempts, + status="failed", + failure_class="candidate_selection", + ) + except Exception as exc: + state = fail_stage( + state, + WorkflowStage.CANDIDATE_SELECTION, + reason=str(exc), + ) + return self._publish( + transaction, + staging, + normalized, + workflow_cfg, + planning_cfg, + execution_cfg, + run_metadata, + state, + attempts, + status="failed", + failure_class="internal_error", + ) + _write_json( + staging / "initial_binding_report.json", selection.binding_report + ) + if selection.selected_candidate is None: + if normalized["scene_edit_prompt"] is None: + state = fail_stage( + state, + WorkflowStage.CANDIDATE_SELECTION, + reason=str(selection.binding_report["selection_reason"]), + ) + return self._publish( + transaction, + staging, + normalized, + workflow_cfg, + planning_cfg, + execution_cfg, + run_metadata, + state, + attempts, + status="input_conflict", + failure_class="unbound_scene_reference", + ) + provisional = _highest_vote_candidate(candidate_set) + selection = replace( + selection, + selected_candidate=deepcopy(provisional), + ) + _write_json( + staging / "provisional_candidate.json", + { + "candidate_id": provisional["candidate_id"], + "reason": "explicit_scene_edit_may_materialize_missing_reference", + "binding_status": selection.binding_report["status"], + }, + ) + state = complete_stage(state, WorkflowStage.CANDIDATE_SELECTION) + + state = start_stage(state, WorkflowStage.UNBOUND_ACTION) + if normalized["scene_edit_prompt"] is not None: + state = start_stage(state, WorkflowStage.SCENE_EDIT) + else: + state = start_stage(state, WorkflowStage.SCENE_FINALIZATION) + + unbound_plan: Mapping[str, Any] | None = None + unbound_failures: list[dict[str, Any]] = [] + unbound_error: Exception | None = None + scene_error: Exception | None = None + inspection_error = False + preparation_error: Exception | None = None + preparation: PreparationResult | None = None + scene_attempt_limit = ( + 1 + if analysis.input_kind == "gym_project" + and normalized["scene_edit_prompt"] is None + else workflow_cfg.max_scene_attempts + ) + for scene_index in range(1, scene_attempt_limit + 1): + inspection_error = False + scene_seed = int(base_seed) + scene_index - 1 + attempt_root = staging / "attempts" / f"scene_{scene_index:04d}" + attempt_root.mkdir(parents=True) + attempt = { + "scene_attempt": scene_index, + "scene_seed": scene_seed, + "status": "running", + "scene_revision": None, + "final_inspection": None, + "unbound_action_plan": None, + "final_unbound_action_plan": None, + "unbound_transition": None, + "unbound_failures": [], + "preparation": None, + "planning_attempts": [], + "action_attempts": [], + "parallel_errors": [], + "error": None, + } + attempts.append(attempt) + revision: SceneRevision | None = None + try: + if unbound_plan is None: + with ThreadPoolExecutor( + max_workers=workflow_cfg.max_parallel_workers, + thread_name_prefix="task-engine-parallel", + ) as executor: + scene_future = executor.submit( + self.scene_backend.materialize, + analysis, + normalized, + attempt_root / "scene_revision", + seed=scene_seed, + ) + draft_future = executor.submit( + self._draft_with_fallback, + candidate_set, + selection, + ) + try: + revision = scene_future.result() + except Exception as exc: + scene_error = exc + revision = None + try: + unbound_plan, unbound_failures = draft_future.result() + except Exception as exc: + unbound_error = exc + if unbound_error is not None: + raise unbound_error + state = complete_stage(state, WorkflowStage.UNBOUND_ACTION) + if scene_error is not None: + raise scene_error + assert revision is not None + else: + revision = self.scene_backend.materialize( + analysis, + normalized, + attempt_root / "scene_revision", + seed=scene_seed, + ) + scene_error = None + except Exception as exc: + if revision is not None: + attempt["scene_revision"] = _revision_record(revision) + state = _complete_materialized_scene( + state, + has_edit=normalized["scene_edit_prompt"] is not None, + ) + if unbound_error is not None: + attempt["status"] = "unbound_action_failed" + attempt["error"] = _error_record(unbound_error) + if scene_error is not None: + attempt["parallel_errors"].append( + { + "branch": "scene", + **_error_record(scene_error), + } + ) + _write_json(attempt_root / "attempt.json", attempt) + break + scene_error = exc + if unbound_plan is not None: + attempt["unbound_action_plan"] = deepcopy(dict(unbound_plan)) + attempt["unbound_failures"] = deepcopy(unbound_failures) + _write_json( + attempt_root / "unbound_action_plan.json", unbound_plan + ) + attempt["status"] = "scene_failed" + attempt["error"] = _error_record(exc) + _write_json(attempt_root / "attempt.json", attempt) + if ( + scene_index < scene_attempt_limit + and _is_scene_remediable_error(exc) + ): + continue + break + + attempt["scene_revision"] = _revision_record(revision) + attempt["unbound_action_plan"] = deepcopy(dict(unbound_plan)) + attempt["unbound_failures"] = deepcopy(unbound_failures) + _write_json(attempt_root / "unbound_action_plan.json", unbound_plan) + state = _complete_materialized_scene( + state, + has_edit=normalized["scene_edit_prompt"] is not None, + ) + if state.stages[WorkflowStage.FINAL_INSPECTION].value == "pending": + state = start_stage(state, WorkflowStage.FINAL_INSPECTION) + try: + final_inspection = self.scene_backend.inspect( + revision, + attempt_root / "final_scene_inspection.json", + ) + except Exception as exc: + scene_error = exc + inspection_error = True + attempt["status"] = "scene_inspection_failed" + attempt["error"] = _error_record(exc) + _write_json(attempt_root / "attempt.json", attempt) + if ( + scene_index < scene_attempt_limit + and _is_scene_remediable_error(exc) + ): + continue + break + attempt["final_inspection"] = deepcopy(dict(final_inspection)) + if state.stages[WorkflowStage.FINAL_INSPECTION].value == "running": + state = complete_stage(state, WorkflowStage.FINAL_INSPECTION) + + bundle_root = attempt_root / "bundle" + try: + preparation = self.coordinator.prepare( + normalized["task_id"], + normalized["task_instruction"], + SceneSourceRef( + revision.source, + robot_profile=self.scene_adapter.robot_profile, + ), + bundle_root, + model=model, + candidate_count=effective_candidate_count, + planning_mode=planning_cfg.planning_mode, + max_episodes=planning_cfg.max_episodes, + max_episode_steps=planning_cfg.max_episode_steps, + candidate_set=candidate_set, + force_most_likely=True, + final_inspection=final_inspection, + unbound_action_plan=unbound_plan, + ) + except Exception as exc: + preparation_error = exc + attempt["status"] = "preparation_error" + attempt["error"] = _error_record(exc) + _write_json(attempt_root / "attempt.json", attempt) + break + attempt["preparation"] = preparation.status + attempt["planning_attempts"] = deepcopy( + list(preparation.planning_attempts) + ) + if preparation.status == "bound": + attempt["status"] = "prepared" + _write_json(attempt_root / "attempt.json", attempt) + break + attempt["status"] = "preparation_failed" + attempt["error"] = { + "type": "PreparationFailure", + "message": preparation.status, + } + _write_json(attempt_root / "attempt.json", attempt) + if not _scene_remediable( + preparation, + analysis=analysis, + request=normalized, + ): + break + + if preparation is None or preparation.status != "bound": + failure_class = ( + "action_capability" + if unbound_error is not None + or isinstance(preparation_error, UnsupportedSemanticCapabilityError) + else ( + "preparation_error" + if preparation_error is not None + else _preparation_failure_class( + preparation, + scene_error=scene_error, + analysis=analysis, + request=normalized, + ) + ) + ) + failed_stage = ( + WorkflowStage.FINAL_INSPECTION + if inspection_error + else ( + WorkflowStage.UNBOUND_ACTION + if unbound_error is not None + else _failure_stage(failure_class, normalized) + ) + ) + if state.stages[failed_stage].value in { + "pending", + "running", + "succeeded", + }: + state = fail_stage( + state, + failed_stage, + reason=( + str(unbound_error) + if unbound_error is not None + else ( + str(preparation_error) + if preparation_error is not None + else ( + str(scene_error) + if scene_error is not None + else ( + preparation.status + if preparation is not None + else failure_class + ) + ) + ) + ), + ) + return self._publish( + transaction, + staging, + normalized, + workflow_cfg, + planning_cfg, + execution_cfg, + run_metadata, + state, + attempts, + status=( + "input_conflict" + if failure_class == "input_conflict" + else "failed" + ), + failure_class=failure_class, + ) + + final_candidate_id = preparation.selected_candidate_id + if not isinstance(final_candidate_id, str) or not final_candidate_id: + raise ValueError( + "A bound preparation must select one non-empty candidate ID." + ) + selected_attempt = attempts[-1] + final_unbound = getattr(preparation, "unbound_action_plan", None) + if ( + final_unbound is None + and final_candidate_id != unbound_plan["candidate_id"] + ): + final_candidate = next( + item + for item in candidate_set["candidates"] + if item["candidate_id"] == final_candidate_id + ) + final_unbound = _semantic_draft(final_candidate) + elif final_unbound is None: + final_unbound = unbound_plan + if str(final_unbound.get("candidate_id")) != final_candidate_id: + raise ValueError( + "Final UnboundActionPlan candidate does not match preparation." + ) + selected_attempt["final_unbound_action_plan"] = deepcopy( + dict(final_unbound) + ) + selected_attempt["unbound_transition"] = { + "initial_candidate_id": str(unbound_plan["candidate_id"]), + "initial_hash": canonical_hash(unbound_plan), + "final_candidate_id": final_candidate_id, + "final_hash": canonical_hash(final_unbound), + "changed": final_unbound != unbound_plan, + } + _write_json( + preparation.output_dir.parent / "final_unbound_action_plan.json", + final_unbound, + ) + _write_json( + preparation.output_dir.parent / "attempt.json", + selected_attempt, + ) + + for stage in ( + WorkflowStage.FINAL_BINDING, + WorkflowStage.STATIC_FEASIBILITY, + WorkflowStage.GROUNDED_ACTION, + ): + state = start_stage(state, stage) + state = complete_stage(state, stage) + if not execute: + final_root = staging / "final" + final_bundle = final_root / "bundle" + final_root.mkdir() + shutil.copytree(preparation.output_dir, final_bundle) + selected_attempt["status"] = "prepared" + _write_json( + preparation.output_dir.parent / "attempt.json", + selected_attempt, + ) + _write_json( + final_root / "selection.json", + { + "scene_attempt": selected_attempt["scene_attempt"], + "candidate_id": final_candidate_id, + "action_attempt": None, + "execution_report": None, + }, + ) + return self._publish( + transaction, + staging, + normalized, + workflow_cfg, + planning_cfg, + execution_cfg, + run_metadata, + state, + attempts, + status="prepared", + failure_class=None, + final_bundle=final_bundle, + ) + state = start_stage(state, WorkflowStage.EXECUTION) + + successful_report: Mapping[str, Any] | None = None + successful_action_root: Path | None = None + success_terms = _bundle_success_terms(preparation.output_dir) + for action_index in range(1, workflow_cfg.max_action_attempts + 1): + action_seed = int(base_seed) + action_index - 1 + action_root = ( + preparation.output_dir.parent + / "action_attempts" + / f"action_{action_index:04d}" + ) + action_record: dict[str, Any] = { + "action_attempt": action_index, + "seed": action_seed, + "status": "running", + "successful_environments": 0, + "required_successes": execution_cfg.required_successes, + "error": None, + } + try: + execution_options = { + "seed": action_seed, + "num_envs": execution_cfg.num_envs, + "dataset_saving": bool(dataset_saving), + "failure_policy": failure_policy, + } + if open_window: + execution_options["open_window"] = True + report = self.action_executor( + preparation.output_dir, + action_root, + **execution_options, + ) + successes = _environment_successes( + report, + required_semantic_steps=success_terms, + ) + if len(successes) != execution_cfg.num_envs: + raise ValueError( + "Execution report environment count does not match " + "TaskEngineExecutionCfg.num_envs." + ) + action_record["successful_environments"] = sum(successes) + accepted = ( + str(report.get("status")) not in {"rejected", "aborted"} + and sum(successes) >= execution_cfg.required_successes + ) + action_record["status"] = "succeeded" if accepted else "failed" + _write_json(action_root / "task_engine_attempt.json", action_record) + selected_attempt["action_attempts"].append(deepcopy(action_record)) + if accepted: + successful_report = deepcopy(dict(report)) + successful_action_root = action_root + break + except Exception as exc: + action_record["status"] = "failed" + action_record["error"] = _error_record(exc) + action_root.mkdir(parents=True, exist_ok=True) + _write_json(action_root / "task_engine_attempt.json", action_record) + selected_attempt["action_attempts"].append(deepcopy(action_record)) + _write_json( + preparation.output_dir.parent / "attempt.json", selected_attempt + ) + + if successful_report is None: + selected_attempt["status"] = "execution_failed" + _write_json( + preparation.output_dir.parent / "attempt.json", selected_attempt + ) + state = fail_stage( + state, + WorkflowStage.EXECUTION, + reason="All bounded Task Program execution attempts failed.", + ) + return self._publish( + transaction, + staging, + normalized, + workflow_cfg, + planning_cfg, + execution_cfg, + run_metadata, + state, + attempts, + status="failed", + failure_class="action_execution", + ) + + selected_attempt["status"] = "succeeded" + _write_json( + preparation.output_dir.parent / "attempt.json", selected_attempt + ) + state = complete_stage(state, WorkflowStage.EXECUTION) + final_root = staging / "final" + final_bundle = final_root / "bundle" + final_root.mkdir() + shutil.copytree(preparation.output_dir, final_bundle) + _write_json( + final_root / "selection.json", + { + "scene_attempt": selected_attempt["scene_attempt"], + "candidate_id": final_candidate_id, + "action_attempt": int(successful_action_root.name.split("_")[-1]), + "execution_report": successful_report, + "success_spec_steps": list(success_terms), + }, + ) + return self._publish( + transaction, + staging, + normalized, + workflow_cfg, + planning_cfg, + execution_cfg, + run_metadata, + state, + attempts, + status="succeeded", + failure_class=None, + final_bundle=final_bundle, + ) + + def _draft_with_fallback( + self, + candidate_set: Mapping[str, Any], + selection: CandidateSelection, + ) -> tuple[Mapping[str, Any], list[dict[str, Any]]]: + selected_id = selection.selected_candidate_id + resolved_ids = { + str(item["candidate_id"]) + for item in selection.binding_report["candidates"] + if item["status"] == "resolved" + } + ordered = [selected_id] + [ + str(item["candidate_id"]) + for item in candidate_set["candidates"] + if item["candidate_id"] != selected_id + and item["candidate_id"] in resolved_ids + ] + failures = [] + for candidate_id in ordered: + candidate = next( + item + for item in candidate_set["candidates"] + if item["candidate_id"] == candidate_id + ) + try: + return _semantic_draft(candidate), failures + except UnsupportedSemanticCapabilityError: + raise + except (TypeError, ValueError) as exc: + failures.append( + { + "candidate_id": candidate_id, + "stage": "unbound_action", + "draft": deepcopy(candidate["draft"]), + "error": _error_record(exc), + } + ) + raise ValueError( + "No selected task candidate can be represented by Semantic Skill." + ) + + @staticmethod + def _publish( + transaction: ArtifactTransaction, + staging: Path, + request: Mapping[str, Any], + workflow_cfg: TaskEngineWorkflowCfg, + planning_cfg: TaskEnginePlanningCfg, + execution_cfg: TaskEngineExecutionCfg, + run_metadata: Mapping[str, Any], + state: TaskEngineState, + attempts: Sequence[Mapping[str, Any]], + *, + status: str, + failure_class: str | None, + final_bundle: Path | None = None, + ) -> TaskEngineRunResult: + state_path = staging / "workflow_state.json" + manifest_path = staging / "run_manifest.json" + _write_json(state_path, state.to_dict()) + _write_json( + manifest_path, + { + "schema_version": TASK_ENGINE_RUN_MANIFEST_SCHEMA, + "run_id": run_metadata["run_id"], + "created_at": run_metadata["created_at"], + "output_root": Path(request["output_dir"]).parent.as_posix(), + "run_dir": Path(request["output_dir"]).as_posix(), + "status": status, + "failure_class": failure_class, + "request": deepcopy(dict(request)), + "configuration": { + "workflow": { + "max_parallel_workers": workflow_cfg.max_parallel_workers, + "max_scene_attempts": workflow_cfg.max_scene_attempts, + "max_action_attempts": workflow_cfg.max_action_attempts, + }, + "planning": { + "candidate_count": planning_cfg.candidate_count, + "planning_mode": planning_cfg.planning_mode, + "max_episodes": planning_cfg.max_episodes, + "max_episode_steps": planning_cfg.max_episode_steps, + }, + "execution": { + "num_envs": execution_cfg.num_envs, + "success_policy": execution_cfg.success_policy, + "min_successful_envs": execution_cfg.min_successful_envs, + "dataset_saving": bool(run_metadata["dataset_saving"]), + "failure_policy": str(run_metadata["failure_policy"]), + }, + }, + "attempts": deepcopy(list(attempts)), + "final_bundle": ( + None if final_bundle is None else final_bundle.as_posix() + ), + }, + ) + published = transaction.commit() + return TaskEngineRunResult( + status=status, + output_dir=published, + manifest_path=published / manifest_path.name, + state_path=published / state_path.name, + final_bundle=( + None if final_bundle is None else published / "final" / "bundle" + ), + failure_class=failure_class, + ) + + +def _complete_materialized_scene( + state: TaskEngineState, + *, + has_edit: bool, +) -> TaskEngineState: + if has_edit and state.stages[WorkflowStage.SCENE_EDIT].value == "running": + state = complete_stage(state, WorkflowStage.SCENE_EDIT) + state = start_stage(state, WorkflowStage.SCENE_FINALIZATION) + if state.stages[WorkflowStage.SCENE_FINALIZATION].value == "running": + state = complete_stage(state, WorkflowStage.SCENE_FINALIZATION) + return state + + +def _scene_remediable( + preparation: PreparationResult, + *, + analysis: SceneAnalysis, + request: Mapping[str, Any], +) -> bool: + if preparation.status != "infeasible": + return False + report = preparation.feasibility_report + if not isinstance(report, Mapping) or report.get("remediation_class") != ( + "scene_remediable" + ): + return False + if analysis.input_kind == "image": + return True + return request["scene_edit_prompt"] is not None + + +def _is_scene_remediable_error(error: Exception) -> bool: + """Return whether one typed Scene failure may create a new attempt.""" + return isinstance(error, (SceneRemediableError, SceneServiceError)) + + +def _preparation_failure_class( + preparation: PreparationResult | None, + *, + scene_error: Exception | None, + analysis: SceneAnalysis, + request: Mapping[str, Any], +) -> str: + if scene_error is not None: + return "scene_materialization" + if preparation is None: + return "scene_materialization" + if preparation.status == "planning_failed": + return "action_capability" + if preparation.status in {"ambiguous", "unsatisfied"}: + return "input_conflict" + if preparation.status == "infeasible": + report = preparation.feasibility_report + remediation = ( + str(report.get("remediation_class")) + if isinstance(report, Mapping) + else "terminal" + ) + if remediation == "action_capability": + return "action_capability" + if remediation == "input_conflict": + return "input_conflict" + if remediation != "scene_remediable": + return "terminal_feasibility" + if ( + analysis.input_kind == "gym_project" + and request["scene_edit_prompt"] is None + ): + return "read_only_scene_infeasible" + return "scene_infeasible" + return "preparation" + + +def _failure_stage( + failure_class: str, + request: Mapping[str, Any], +) -> WorkflowStage: + if failure_class == "action_capability": + return WorkflowStage.GROUNDED_ACTION + if failure_class == "preparation_error": + return WorkflowStage.FINAL_BINDING + if failure_class == "input_conflict": + return WorkflowStage.FINAL_BINDING + if failure_class in { + "scene_infeasible", + "read_only_scene_infeasible", + "terminal_feasibility", + }: + return WorkflowStage.STATIC_FEASIBILITY + if failure_class == "scene_materialization": + return ( + WorkflowStage.SCENE_EDIT + if request["scene_edit_prompt"] is not None + else WorkflowStage.SCENE_FINALIZATION + ) + return WorkflowStage.GROUNDED_ACTION + + +def _environment_successes( + report: Mapping[str, Any], + *, + required_semantic_steps: Sequence[str] = (), +) -> list[bool]: + environments = report.get("environments") + if not isinstance(environments, Sequence) or isinstance(environments, (str, bytes)): + raise ValueError("Execution report environments must be a sequence.") + values = [] + for item in environments: + if not isinstance(item, Mapping) or not isinstance(item.get("success"), bool): + raise ValueError("Every execution environment requires boolean success.") + success = bool(item["success"]) + if required_semantic_steps: + semantics = item.get("semantic_success") + if not isinstance(semantics, Mapping): + success = False + else: + success = success and all( + semantics.get(step_id) is True + for step_id in required_semantic_steps + ) + values.append(success) + if not values: + raise ValueError("Execution report must contain at least one environment.") + return values + + +def _bundle_success_terms(bundle: Path) -> tuple[str, ...]: + path = bundle / "success_spec.json" + if not path.is_file(): + return () + try: + value = _read_json(path) + terms = value.get("terms") + strict = isinstance(value.get("schema_version"), str) + if not isinstance(terms, Sequence) or isinstance(terms, (str, bytes)): + if strict: + raise ValueError("Task Program bundle has no valid SuccessSpec terms.") + return () + result = tuple( + str(item["step_id"]) + for item in terms + if isinstance(item, Mapping) and isinstance(item.get("step_id"), str) + ) + if len(result) != len(terms) or (strict and not result): + if strict: + raise ValueError("Task Program bundle SuccessSpec terms are invalid.") + return () + return result + except OSError: + return () + + +def _semantic_draft(candidate: Mapping[str, Any]) -> dict[str, Any]: + """Return a scene-independent call-coverage draft without physical actions.""" + draft = candidate.get("draft") + if not isinstance(draft, Mapping): + raise TypeError("TaskCandidate.draft must be a mapping.") + steps = draft.get("steps") + if not isinstance(steps, Sequence) or isinstance(steps, (str, bytes)): + raise TypeError("TaskDraft.steps must be a sequence.") + supported = {"E1", "E2", "E3", "E4", "E5"} + unsupported = sorted( + { + str(step.get("task_type")) + for step in steps + if isinstance(step, Mapping) and str(step.get("task_type")) not in supported + } + ) + if unsupported: + raise UnsupportedSemanticCapabilityError( + "Phase-one Semantic Skill planning does not support task types " + f"{unsupported}." + ) + return { + "schema_version": "semantic_task_plan_draft/v1", + "task_id": str(draft.get("task_id")), + "candidate_id": str(candidate.get("candidate_id")), + "steps": [ + { + "id": str(step["id"]), + "task_type": str(step["task_type"]), + "depends_on": [str(value) for value in step["depends_on"]], + } + for step in steps + if isinstance(step, Mapping) + ], + } + + +def _highest_vote_candidate(candidate_set: Mapping[str, Any]) -> Mapping[str, Any]: + candidates = candidate_set.get("candidates") + if not isinstance(candidates, Sequence) or isinstance(candidates, (str, bytes)): + raise TypeError("TaskCandidateSet.candidates must be a sequence.") + values = [item for item in candidates if isinstance(item, Mapping)] + if not values: + raise ValueError("TaskCandidateSet requires at least one candidate.") + return max( + values, + key=lambda item: ( + int(item.get("vote_count", 0)), + str(item.get("candidate_id", "")), + ), + ) + + +def _copy_trajectory_record(report: Mapping[str, Any], output_root: Path) -> str | None: + raw = report.get("record_dir") + if not isinstance(raw, str) or not raw: + return None + source = Path(raw).expanduser().resolve() + if not source.is_dir(): + return None + destination = output_root / "trajectory" + if source == destination or destination in source.parents: + return source.as_posix() + shutil.copytree(source, destination) + return destination.as_posix() + + +def _revision_record(revision: SceneRevision) -> dict[str, Any]: + return { + "source": revision.source.as_posix(), + "output_root": ( + None if revision.output_root is None else revision.output_root.as_posix() + ), + "revision_id": revision.revision_id, + "seed": revision.seed, + "edit_plan": deepcopy(revision.edit_plan), + "source_fingerprint": ( + None + if revision.source_fingerprint is None + else revision.source_fingerprint.to_dict() + ), + } + + +def _error_record(error: Exception) -> dict[str, str]: + return { + "type": type(error).__name__, + "failure_type": ( + "scene_remediable" if _is_scene_remediable_error(error) else "terminal" + ), + "message": str(error), + } + + +def _read_json(path: Path) -> dict[str, Any]: + value = json.loads(path.read_text(encoding="utf-8")) + if not isinstance(value, dict): + raise ValueError(f"JSON artifact must contain an object: {path}") + return value + + +def _write_json(path: Path, value: Any) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text( + json.dumps(value, ensure_ascii=False, indent=2, allow_nan=False) + "\n", + encoding="utf-8", + ) diff --git a/embodichain/gen_sim/task_engine/workflow_contracts.py b/embodichain/gen_sim/task_engine/workflow_contracts.py new file mode 100644 index 000000000..a6b61b04b --- /dev/null +++ b/embodichain/gen_sim/task_engine/workflow_contracts.py @@ -0,0 +1,179 @@ +# ---------------------------------------------------------------------------- +# 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 inputs for Task Engine cross-engine workflows.""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +import json +from pathlib import Path +from typing import Any, Final, Literal, TypeAlias + +__all__ = [ + "TASK_RUN_REQUEST_SCHEMA", + "SceneInputKind", + "TaskRunRequest", + "scene_input_kind", + "validate_scene_history_root", + "validate_scene_output_separation", + "validate_task_run_request", +] + +TASK_RUN_REQUEST_SCHEMA: Final = "embodichain.task-engine-run-request/v1" +TaskRunRequest: TypeAlias = dict[str, Any] +SceneInputKind = Literal["image", "gym_project"] + +_REQUEST_KEYS = frozenset( + { + "schema_version", + "task_id", + "task_instruction", + "image_path", + "gym_project", + "scene_edit_prompt", + "output_dir", + } +) + + +def validate_task_run_request(value: Mapping[str, Any]) -> TaskRunRequest: + """Validate and detach one Task Engine run request. + + Version 1 deliberately has no ``scene_generation_prompt``. Image workflows + use the image-only Scene Engine generation behavior and may apply one + optional edit after that initial scene has been generated. + """ + if not isinstance(value, Mapping): + raise TypeError("TaskRunRequest must be a mapping.") + result = deepcopy(dict(value)) + if set(result) != _REQUEST_KEYS: + missing = sorted(_REQUEST_KEYS - set(result)) + extra = sorted(set(result) - _REQUEST_KEYS) + raise ValueError( + f"TaskRunRequest fields differ; missing={missing}, extra={extra}." + ) + if result.get("schema_version") != TASK_RUN_REQUEST_SCHEMA: + raise ValueError( + "TaskRunRequest.schema_version must be " f"{TASK_RUN_REQUEST_SCHEMA!r}." + ) + result["task_id"] = _nonempty(result.get("task_id"), "task_id") + result["task_instruction"] = _nonempty( + result.get("task_instruction"), "task_instruction" + ) + result["output_dir"] = _path(result.get("output_dir"), "output_dir") + + image_path = _optional_path(result.get("image_path"), "image_path") + gym_project = _optional_path(result.get("gym_project"), "gym_project") + if (image_path is None) == (gym_project is None): + raise ValueError( + "TaskRunRequest requires exactly one of image_path or gym_project." + ) + result["image_path"] = image_path + result["gym_project"] = gym_project + if gym_project is not None: + validate_scene_output_separation(gym_project, result["output_dir"]) + + edit_prompt = result.get("scene_edit_prompt") + if edit_prompt is not None: + edit_prompt = _nonempty(edit_prompt, "scene_edit_prompt") + result["scene_edit_prompt"] = edit_prompt + _json_safe(result) + return result + + +def scene_input_kind(request: Mapping[str, Any]) -> SceneInputKind: + """Return the selected scene input kind after validating ``request``.""" + normalized = validate_task_run_request(request) + return "image" if normalized["image_path"] is not None else "gym_project" + + +def validate_scene_output_separation( + gym_project: str | Path, + output_dir: str | Path, +) -> None: + """Reject output paths that could replace or modify a read-only source. + + Args: + gym_project: Existing Gym project directory or configuration path. + output_dir: Transactional output directory for the Task Engine run. + + Raises: + ValueError: If either path contains the other or both paths are equal. + """ + source = Path(gym_project).expanduser().resolve() + output = Path(output_dir).expanduser().resolve() + if source == output or source in output.parents or output in source.parents: + raise ValueError( + "Task Engine output_dir and source Gym project must not overlap." + ) + + +def validate_scene_history_root( + gym_project: str | Path, + output_root: str | Path, +) -> None: + """Protect a source project before reserving a history-directory child. + + A prior run may live below the same history root because every new run is + published to a distinct timestamped child. The inverse remains unsafe: + creating the history root at or below the source project would write a + reservation and output artifacts into the read-only source tree. + + Args: + gym_project: Existing Gym project directory or configuration path. + output_root: Parent directory under which a new run will be reserved. + + Raises: + ValueError: If the history root is equal to or contained by the source + project boundary. + """ + source = Path(gym_project).expanduser().resolve() + protected_root = source.parent if source.is_file() else source + history_root = Path(output_root).expanduser().resolve() + if protected_root == history_root or protected_root in history_root.parents: + raise ValueError( + "Task Engine output_root must not be inside the read-only source " + "Gym project." + ) + + +def _path(value: Any, field_name: str) -> str: + text = _nonempty(value, field_name) + return Path(text).expanduser().resolve().as_posix() + + +def _optional_path(value: Any, field_name: str) -> str | None: + if value is None: + return None + return _path(value, field_name) + + +def _nonempty(value: Any, field_name: str) -> str: + if not isinstance(value, str): + raise TypeError(f"TaskRunRequest.{field_name} must be a string.") + result = value.strip() + if not result: + raise ValueError(f"TaskRunRequest.{field_name} must not be empty.") + return result + + +def _json_safe(value: Any) -> None: + try: + json.dumps(value, ensure_ascii=False, allow_nan=False) + except (TypeError, ValueError) as exc: + raise ValueError("TaskRunRequest must contain strict JSON data.") from exc diff --git a/embodichain/gen_sim/video_archive.py b/embodichain/gen_sim/video_archive.py new file mode 100644 index 000000000..706952d7b --- /dev/null +++ b/embodichain/gen_sim/video_archive.py @@ -0,0 +1,178 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Copy one completed GenSim recording to its task ID.""" + +from __future__ import annotations + +import argparse +from pathlib import Path +import shutil +import sys +from typing import Any, Sequence + +__all__: list[str] = [] + + +def _archive_task_recording(env: Any, task_id: str) -> Path | None: + """Archive the generated audience video from a completed GenSim task. + + Args: + env: Completed GenSim environment whose final reset flushed recording. + task_id: ID of the task that produced the recording. + + Returns: + Archived video path, or ``None`` when video recording is disabled. + + Raises: + RuntimeError: If configured recorders do not identify one task video. + ValueError: If the task ID can escape the video directory. + FileNotFoundError: If the expected source recording does not exist. + """ + manager = getattr(env.unwrapped, "event_manager", None) + mode_cfgs = getattr(manager, "_mode_functor_cfgs", {}) + recorders: list[Any] = [] + for configured in mode_cfgs.values(): + for functor_cfg in configured: + functor = getattr(functor_cfg, "func", None) + if getattr(type(functor), "__name__", "") in { + "record_camera_data", + "record_camera_data_async", + }: + recorders.append(functor) + if not recorders: + return None + + audience = [ + recorder + for recorder in recorders + if getattr(recorder, "_name", None) == "record_cam_audience_view" + ] + if len(audience) == 1: + recorder = audience[0] + elif len(recorders) == 1: + recorder = recorders[0] + else: + raise RuntimeError( + "GenSim task video archival found multiple camera recorders without " + "one audience recorder." + ) + recorder_name = str(getattr(recorder, "_name", "")).strip() + save_path = getattr(recorder, "_save_path", None) + if not recorder_name or not isinstance(save_path, (str, Path)): + raise RuntimeError( + f"Cannot archive video for task {task_id!r}: camera recorder does not " + "expose its output path and name." + ) + return _archive_task_video( + save_path, + source_stem=f"episode_0_{recorder_name}", + task_id=task_id, + ) + + +def _archive_task_video( + video_directory: str | Path, + *, + source_stem: str, + task_id: str, +) -> Path: + """Copy a completed recording to ``.``. + + Args: + video_directory: Directory containing the completed recording. + source_stem: Source file name without its video extension. + task_id: ID of the task that produced the recording. + + Returns: + Path to the archived recording. + + Raises: + ValueError: If the task ID can escape the video directory. + FileNotFoundError: If the expected source recording does not exist. + RuntimeError: If more than one source extension matches. + """ + _validate_task_id(task_id) + directory = Path(video_directory).expanduser().resolve() + source_prefix = f"{source_stem}." + candidates = ( + sorted( + path + for path in directory.iterdir() + if path.is_file() and path.name.startswith(source_prefix) + ) + if directory.is_dir() + else [] + ) + expected = directory / f"{source_stem}." + if not candidates: + raise FileNotFoundError( + f"Cannot archive video for task {task_id!r}: " + f"expected source video at {expected}." + ) + if len(candidates) != 1: + matches = ", ".join(path.as_posix() for path in candidates) + raise RuntimeError( + f"Cannot archive video for task {task_id!r}: expected exactly one " + f"source video at {expected}, found {matches}." + ) + + source = candidates[0] + extension = source.name[len(source_stem) :] + destination = directory / f"{task_id}{extension}" + if destination.exists() or destination.is_symlink(): + destination.unlink() + shutil.copy2(source, destination) + return destination + + +def _validate_task_id(task_id: str) -> None: + if ( + not isinstance(task_id, str) + or not task_id + or task_id in {".", ".."} + or "/" in task_id + or "\\" in task_id + or "\x00" in task_id + ): + raise ValueError( + f"Invalid task ID {task_id!r}: task IDs must be non-empty file names " + "without path separators." + ) + + +def _main(argv: Sequence[str] | None = None) -> int: + """Run task-video archival as a standalone GenSim command.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--video-directory", required=True) + parser.add_argument("--source-stem", required=True) + parser.add_argument("--task-id", required=True) + args = parser.parse_args(argv) + try: + destination = _archive_task_video( + args.video_directory, + source_stem=args.source_stem, + task_id=args.task_id, + ) + except (OSError, RuntimeError, ValueError) as exc: + print(f"error: {exc}", file=sys.stderr) + return 1 + print(destination) + return 0 + + +if __name__ == "__main__": + raise SystemExit(_main()) diff --git a/embodichain/lab/gym/envs/task_program/bridge.py b/embodichain/lab/gym/envs/task_program/bridge.py index 259c9669d..1133d8a78 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -482,6 +482,9 @@ class RuntimeCommandFrameEncoder: Args: qpos_provider: Full-qpos source aligned to a frame's explicit ``env_ids``. + hold_qpos_provider: Optional controller-target source used as the base + action for unaddressed joints. When omitted, the latest observed qpos + remains the compatibility fallback. transports: Optional additional transport encoders. The built-in joint-position encoder precedes them when enabled. include_joint_position: Whether to install the built-in joint-position @@ -493,6 +496,7 @@ def __init__( self, qpos_provider: CurrentQposProvider, *, + hold_qpos_provider: Callable[[torch.Tensor], torch.Tensor] | None = None, transports: Iterable[RuntimeTransportActionEncoder] = (), include_joint_position: bool = True, ) -> None: @@ -500,7 +504,10 @@ def __init__( raise TypeError("qpos_provider must implement CurrentQposProvider.") if type(include_joint_position) is not bool: raise TypeError("include_joint_position must be a bool.") + if hold_qpos_provider is not None and not callable(hold_qpos_provider): + raise TypeError("hold_qpos_provider must be callable or None.") self._qpos_provider = qpos_provider + self._hold_qpos_provider = hold_qpos_provider self._transports: dict[str, RuntimeTransportActionEncoder] = {} self._frozen = False if include_joint_position: @@ -609,9 +616,12 @@ def _validate_hold_target_types( def _base_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: """Capture and validate one owned full-qpos hold action.""" - qpos = self._qpos_provider.current_qpos(env_ids) + if self._hold_qpos_provider is None: + qpos = self._qpos_provider.current_qpos(env_ids) + else: + qpos = self._hold_qpos_provider(env_ids) if not isinstance(qpos, torch.Tensor): - raise TypeError("CurrentQposProvider.current_qpos() must return a tensor.") + raise TypeError("The full-qpos hold provider must return a tensor.") if qpos.dim() != 2 or qpos.shape[0] != env_ids.shape[0] or qpos.shape[1] == 0: raise ValueError( "Current qpos must have shape (batch_size, robot_dof) with non-zero DOF." @@ -846,6 +856,61 @@ class _SegmentLifecycle: post_policy_success: torch.Tensor | None = None +@dataclass(frozen=True, slots=True) +class _RuntimeProgress: + """Lightweight lifecycle view used between immutable audit boundaries.""" + + status: SemanticExecutionStatus + current_call_index: int | None + env_ids: torch.Tensor + wait_duration: float + + @property + def terminal(self) -> bool: + """Whether the runtime has reached a terminal audit boundary.""" + return self.status in { + SemanticExecutionStatus.COMPLETED, + SemanticExecutionStatus.FAILED, + SemanticExecutionStatus.CANCELLED, + } + + +def _runtime_progress( + value: SemanticExecutionResult | ParallelSemanticExecutionResult | Any, +) -> _RuntimeProgress: + """Read the small progress surface without traversing audit history.""" + status = getattr(value, "status", None) + if not isinstance(status, SemanticExecutionStatus): + raise TypeError("Runtime progress status must be a SemanticExecutionStatus.") + current_call_index = getattr(value, "current_call_index", None) + if current_call_index is not None and ( + type(current_call_index) is not int or current_call_index < 0 + ): + raise ValueError("Runtime progress call index must be non-negative or None.") + env_ids = getattr(value, "env_ids", None) + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError("Runtime progress env_ids must be non-empty int64 IDs.") + wait_duration = getattr(value, "wait_duration", None) + if ( + not isinstance(wait_duration, (int, float)) + or isinstance(wait_duration, bool) + or not math.isfinite(float(wait_duration)) + or float(wait_duration) < 0.0 + ): + raise ValueError("Runtime progress wait_duration must be non-negative.") + return _RuntimeProgress( + status=status, + current_call_index=current_call_index, + env_ids=env_ids.clone(), + wait_duration=float(wait_duration), + ) + + def _validate_runtime_result( result: SemanticExecutionResult | ParallelSemanticExecutionResult, ) -> SemanticExecutionResult | ParallelSemanticExecutionResult: @@ -1124,7 +1189,9 @@ def _decorate_action( action: Any, *, segment: Any, - result: SemanticExecutionResult | ParallelSemanticExecutionResult, + result: ( + SemanticExecutionResult | ParallelSemanticExecutionResult | _RuntimeProgress + ), action_kind: str | None = None, ) -> ControllerAction: """Own one action and attach stable program/runtime provenance.""" @@ -1179,6 +1246,7 @@ def _segment_actions( ) self._active_segment_id = segment_id result: SemanticExecutionResult | ParallelSemanticExecutionResult | None = None + progress: _RuntimeProgress | None = None segment_runtime: ( SequentialSemanticCallExecutorPort | ParallelSemanticExecutor ) = self._runtime @@ -1217,43 +1285,66 @@ def _segment_actions( execution_prefix_length=execution_prefix_length, ) ) + progress = _runtime_progress(result) + + def advance_runtime() -> None: + """Advance while avoiding a full history snapshot when supported.""" + nonlocal result, progress + lightweight_advance = ( + None if is_parallel else getattr(segment_runtime, "advance", None) + ) + if callable(lightweight_advance): + status = lightweight_advance() + if not isinstance(status, SemanticExecutionStatus): + raise TypeError( + "Runtime advance() must return SemanticExecutionStatus." + ) + progress = _runtime_progress(segment_runtime) + if progress.terminal: + result = _validate_runtime_result(segment_runtime.result) + progress = _runtime_progress(result) + return + result = _validate_runtime_result(segment_runtime.step()) + progress = _runtime_progress(result) while True: + assert progress is not None emitted = False while self._sink.pending_count: action = self._decorate_action( self._sink.pop(), segment=segment, - result=result, + result=progress, ) yield from self._yield_and_advance(action, lifecycle) emitted = True - if emitted and not result.terminal: + if emitted and not progress.terminal: # The result's wait duration was measured before the action # just consumed by Gym. Refresh it against the advanced # environment clock before deciding whether another hold is due. - result = _validate_runtime_result(segment_runtime.step()) + advance_runtime() continue - if result.terminal: + if progress.terminal: break - if result.wait_duration > 0.0: + if progress.wait_duration > 0.0: self._clock.steps_for_duration( - result.wait_duration, + progress.wait_duration, field_name="SemanticExecutionResult.wait_duration", ) hold = self._decorate_action( - self._sink.wait_hold(result.env_ids), + self._sink.wait_hold(progress.env_ids), segment=segment, - result=result, + result=progress, action_kind="runtime_wait_hold", ) yield from self._yield_and_advance(hold, lifecycle) - result = _validate_runtime_result(segment_runtime.step()) + advance_runtime() + assert result is not None self._record_runtime_result(lifecycle, result) self._retain_eligible_rows(result.success_mask) if is_parallel: diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index a00cbbe41..01cd0c0d2 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -37,8 +37,8 @@ from .affordance import ( Affordance, AntipodalAffordance, - AssembleAffordance, AxisAlignAffordance, + AssembleAffordance, InteractionPoints, OpenDoorAffordance, PressAffordance, @@ -58,6 +58,7 @@ GRASP_COMMAND, JointPositionCommand, OPEN_COMMAND, + PARK_COMMAND, ) from .core import AtomicAction, ObjectSemantics, SkillDescriptor from .effects import StateDelta @@ -262,11 +263,11 @@ "ArticulationAffordanceGeometry", "ArticulationGeometryProvider", "ArticulationJointGeometry", + "AxisAlignAffordance", "ArticulationJointState", "AssembleAffordance", "AssembleGoal", "AxisAlign", - "AxisAlignAffordance", "AxisAlignGoal", "AxisAlignOptions", "AtomicAction", @@ -360,6 +361,7 @@ "OPEN_COMMAND", "ObservationProvider", "ObservedArticulationJointState", + "PARK_COMMAND", "PickUp", "PickUpOptions", "Place", diff --git a/embodichain/lab/sim/atomic_actions/control.py b/embodichain/lab/sim/atomic_actions/control.py index 3af145d9c..d07b21479 100644 --- a/embodichain/lab/sim/atomic_actions/control.py +++ b/embodichain/lab/sim/atomic_actions/control.py @@ -31,6 +31,9 @@ GRASP_COMMAND = "grasp" """Conventional semantic command for an object-holding end effector.""" +PARK_COMMAND = "park" +"""Conventional semantic command for an embodiment-defined parked posture.""" + class ControlCommand(ABC): """Immutable-by-ownership command associated with one control part. @@ -282,4 +285,5 @@ def as_flat_mapping( "GRASP_COMMAND", "JointPositionCommand", "OPEN_COMMAND", + "PARK_COMMAND", ] diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index ef03d926d..8f1df7004 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -48,7 +48,7 @@ normalize_success_mask, ) from .policies import DynamicCollisionMode -from .requirements import SkillBindingContract +from .requirements import GRASP_CAPABILITY, SkillBindingContract from .runtime_commands import ( EndpointCommand, JointPositionPayload, @@ -717,6 +717,12 @@ def _tracking_sequence( for command in frame.commands: endpoints = endpoints_by_destination[command.destination_key] for endpoint in endpoints: + # Contact intentionally prevents grasping endpoints from + # reaching their unobstructed close setpoint. Attachment + # and release are accepted by typed effect evidence; joint + # tracking remains authoritative for motion endpoints. + if GRASP_CAPABILITY in endpoint.capabilities: + continue for channel_id in metrics_by_channel: channel = endpoint.tracking_channels.get(channel_id) if channel is None: diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 0af767e1a..62b2967df 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -1247,6 +1247,13 @@ def _install_plan( def _validate_phase_effect_gates(self, plan: ActionPlan) -> None: """Bind invocation-owned gates to non-initial named plan segments.""" + if not plan.plan_success.any(): + # A fully failed plan owns no executable phase boundary. Preserve + # its typed PlannerDiagnostics so ordinary retry/exhaustion logic + # can handle the failure; validating gate segment names against an + # empty failed-plan trajectory would replace that cause with a + # misleading preparation error. + return request = self._requests[self._invocation_index] for requirement in request.phase_effect_gates: if type(requirement) is not PhaseEffectGateRequirement: diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index e3fb94fc8..87f8e46d4 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -47,6 +47,12 @@ class SceneEntityPose: minimum_confidence: float = 0.0 """Minimum accepted perception confidence.""" + world_displacement: torch.Tensor | None = None + """Optional world-frame translation applied after ``relative_pose``.""" + + world_orientation: torch.Tensor | None = None + """Optional world-frame orientation applied before ``relative_pose``.""" + def __post_init__(self) -> None: if not isinstance(self.entity_id, str) or not self.entity_id.strip(): raise ValueError("entity_id must be a non-empty string.") @@ -57,6 +63,32 @@ def __post_init__(self) -> None: allow_waypoints=False, ) object.__setattr__(self, "relative_pose", self.relative_pose.clone()) + if self.world_displacement is not None: + displacement = self.world_displacement + if not isinstance(displacement, torch.Tensor): + raise TypeError("world_displacement must be a torch.Tensor or None.") + if displacement.dim() not in (1, 2) or displacement.shape[-1] != 3: + raise ValueError( + "world_displacement must have shape (3,) or (num_envs, 3)." + ) + if displacement.dim() == 2 and displacement.shape[0] == 0: + raise ValueError("world_displacement batches must not be empty.") + if not torch.isfinite(displacement).all(): + raise ValueError("world_displacement must contain finite values.") + object.__setattr__(self, "world_displacement", displacement.clone()) + if self.world_orientation is not None: + orientation = self.world_orientation + if not isinstance(orientation, torch.Tensor): + raise TypeError("world_orientation must be a torch.Tensor or None.") + if orientation.dim() not in (2, 3) or orientation.shape[-2:] != (3, 3): + raise ValueError( + "world_orientation must have shape (3, 3) or " "(num_envs, 3, 3)." + ) + if orientation.dim() == 3 and orientation.shape[0] == 0: + raise ValueError("world_orientation batches must not be empty.") + if not torch.isfinite(orientation).all(): + raise ValueError("world_orientation must contain finite values.") + object.__setattr__(self, "world_orientation", orientation.clone()) if not 0.0 <= self.minimum_confidence <= 1.0: raise ValueError("minimum_confidence must be in [0, 1].") @@ -70,6 +102,8 @@ def snapshot(self) -> SceneEntityPose: self.entity_id, relative_pose=self.relative_pose, minimum_confidence=self.minimum_confidence, + world_displacement=self.world_displacement, + world_orientation=self.world_orientation, ) @@ -154,14 +188,34 @@ def resolve_pose_goal( raise ValueError( f"Scene entity {value.entity_id!r} pose must match planning batch size." ) + if value.world_orientation is not None: + orientation = value.world_orientation.to(device=pose.device, dtype=pose.dtype) + if orientation.shape == (3, 3): + orientation = orientation.unsqueeze(0).expand(context.batch_size, -1, -1) + elif orientation.shape != (context.batch_size, 3, 3): + raise ValueError( + f"{name}.world_orientation must match planning batch size." + ) + pose = pose.clone() + pose[:, :3, :3] = orientation if value.relative_pose is None: - return pose.clone() - relative = value.relative_pose.to(device=pose.device, dtype=pose.dtype) - if relative.shape == (4, 4): - relative = relative.unsqueeze(0).expand(context.batch_size, -1, -1) - elif relative.shape != (context.batch_size, 4, 4): - raise ValueError(f"{name}.relative_pose must match planning batch size.") - return torch.bmm(pose, relative) + resolved = pose.clone() + else: + relative = value.relative_pose.to(device=pose.device, dtype=pose.dtype) + if relative.shape == (4, 4): + relative = relative.unsqueeze(0).expand(context.batch_size, -1, -1) + elif relative.shape != (context.batch_size, 4, 4): + raise ValueError(f"{name}.relative_pose must match planning batch size.") + resolved = torch.bmm(pose, relative) + if value.world_displacement is None: + return resolved + displacement = value.world_displacement.to(device=pose.device, dtype=pose.dtype) + if displacement.shape == (3,): + displacement = displacement.unsqueeze(0).expand(context.batch_size, -1) + elif displacement.shape != (context.batch_size, 3): + raise ValueError(f"{name}.world_displacement must match planning batch size.") + resolved[:, :3, 3] += displacement + return resolved def _resolve_object_pose( diff --git a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py b/embodichain/lab/sim/atomic_actions/primitives/axis_align.py index e8ba2ae5c..9fb31a293 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py +++ b/embodichain/lab/sim/atomic_actions/primitives/axis_align.py @@ -407,6 +407,19 @@ def _plan( coordinated_held_object_updates=coordinated_updates, ), segment_lengths=segment_lengths, + # Contact during close/manipulate intentionally moves the aligned + # object. Continue monitoring only through the pre-contact part of + # approach so that external target motion can still invalidate the + # plan without treating the action's own effect as goal drift. + scene_dependency_monitor_until={ + entity_id: max( + 1, + math.ceil( + segment_lengths["approach"] * options.grasp_commit_fraction + ), + ) + for entity_id in self._scene_dependencies(request) + }, ) def _resolve_grasp_pose( diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 160029c4c..7a88b3913 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import dataclass +import math from typing import ClassVar import torch @@ -53,6 +54,7 @@ normalize_success_mask, ) from embodichain.lab.sim.atomic_actions.requirements import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, DisjointResourceSlots, INVERSE_KINEMATICS_CAPABILITY, SkillBindingContract, @@ -133,6 +135,18 @@ class CoordinatedPickmentOptions(ActionOptions): hold_steps: int = 4 """Number of waypoints to hold the final object target pose.""" + release: bool = False + """Whether both hands open after reaching the shared object target pose.""" + + release_steps: int = 10 + """Number of waypoints used for the simultaneous hand-open segment.""" + + retreat_distance: float = 0.08 + """World-Z retreat distance after a coordinated release.""" + + retreat_steps: int = 12 + """Number of waypoints used for the simultaneous post-release retreat.""" + approach_direction: torch.Tensor = torch.tensor( [0.0, 0.0, -1.0], dtype=torch.float32 ) @@ -149,6 +163,9 @@ class CoordinatedPickmentOptions(ActionOptions): """Fraction of the object's left-to-right extent left grasp-free in the middle so the two grippers pinch opposite ends. Must be in ``[0, 1]``.""" + grasp_seed: int = 17_393 + """Deterministic seed isolated around coordinated grasp sampling.""" + def __post_init__(self) -> None: if self.object_motion_keyframes < 2: raise ValueError("object_motion_keyframes must be at least 2.") @@ -156,7 +173,16 @@ def __post_init__(self) -> None: raise ValueError("pre_grasp_distance must be non-negative.") if self.lift_height < 0.0: raise ValueError("lift_height must be non-negative.") - for name in ("hand_interp_steps", "hold_steps"): + if not isinstance(self.release, bool): + raise TypeError("release must be a bool.") + if self.retreat_distance < 0.0: + raise ValueError("retreat_distance must be non-negative.") + for name in ( + "hand_interp_steps", + "hold_steps", + "release_steps", + "retreat_steps", + ): if getattr(self, name) < 0: raise ValueError(f"{name} must be non-negative.") for name in ("approach_direction", "left_to_right_arm_direction"): @@ -170,6 +196,8 @@ def __post_init__(self) -> None: object.__setattr__(self, name, value.clone()) if not 0.0 <= self.middle_empty_ratio <= 1.0: raise ValueError("middle_empty_ratio must be in [0, 1].") + if type(self.grasp_seed) is not int or self.grasp_seed < 0: + raise ValueError("grasp_seed must be a non-negative integer.") @dataclass(frozen=True, slots=True, eq=False) @@ -356,11 +384,17 @@ class CoordinatedPickment( skill_id: ClassVar[str] = "coordinated_pickment" GoalType: ClassVar[type] = CoordinatedPickGoal OptionsType: ClassVar[type] = CoordinatedPickmentOptions + _MAX_REACHABILITY_CANDIDATES: ClassVar[int] = 32 binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=tuple( make_manipulation_slot( role, - motion_capabilities=frozenset({INVERSE_KINEMATICS_CAPABILITY}), + motion_capabilities=frozenset( + { + INVERSE_KINEMATICS_CAPABILITY, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + } + ), grasp_commands={ OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, @@ -498,6 +532,8 @@ def _resolve_target( context: PlanningContext, options: CoordinatedPickmentOptions, resources: _CoordinatedPickResources, + left_start_qpos: torch.Tensor, + right_start_qpos: torch.Tensor, ) -> tuple[ torch.Tensor, torch.Tensor, @@ -521,9 +557,14 @@ def _resolve_target( self._resolve_dual_arm_grasp_poses( target.semantics, object_initial_pose, + object_target_pose, options, resources.left_hand.target_id, resources.right_hand.target_id, + left_start_qpos, + right_start_qpos, + resources.left_arm.control_part, + resources.right_arm.control_part, ) ) left_object_to_eef = torch.bmm(pose_inv(object_initial_pose), left_grasp_xpos) @@ -557,19 +598,30 @@ def _resolve_dual_arm_grasp_poses( self, semantics: ObjectSemantics, object_poses: torch.Tensor, + object_target_poses: torch.Tensor, options: CoordinatedPickmentOptions, left_grasp_target_id: str, right_grasp_target_id: str, + left_start_qpos: torch.Tensor, + right_start_qpos: torch.Tensor, + left_control_part: str, + right_control_part: str, ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: """Sample left/right grasp poses from the target antipodal affordance. Args: semantics: Object semantics carrying an :class:`AntipodalAffordance`. object_poses: Object poses with shape ``(num_envs, 4, 4)``. + object_target_poses: Requested terminal object poses with shape + ``(num_envs, 4, 4)``. options: Coordinated pickment options carrying the dual-arm and approach directions used by the grasp-pose generator. left_grasp_target_id: Left grasp endpoint target ID. right_grasp_target_id: Right grasp endpoint target ID. + left_start_qpos: Current left-arm joint positions. + right_start_qpos: Current right-arm joint positions. + left_control_part: Bound left-arm control-part name. + right_control_part: Bound right-arm control-part name. Returns: ``(left_grasp_xpos, right_grasp_xpos, success_mask)``. The grasp poses @@ -582,11 +634,6 @@ def _resolve_dual_arm_grasp_poses( "CoordinatedPickment requires an AntipodalAffordance to sample " "dual-arm grasps." ) - num_envs = object_poses.shape[0] - identity = torch.eye(4, dtype=torch.float32, device=self.device) - left_grasp_xpos = identity.unsqueeze(0).repeat(num_envs, 1, 1) - right_grasp_xpos = identity.unsqueeze(0).repeat(num_envs, 1, 1) - success_mask = torch.zeros(num_envs, dtype=torch.bool, device=self.device) approach_direction = options.approach_direction.to( device=self.device, dtype=torch.float32 ) @@ -602,72 +649,546 @@ def _resolve_dual_arm_grasp_poses( right_generator = self.planning_services.grasp_pose_generator( right_grasp_target_id ) - if left_generator is not right_generator: - raise ValueError( - "CoordinatedPickment currently requires the left and right " - "grasp endpoints to share one grasp-pose generator instance." - ) - if not isinstance(left_generator, ParallelJawGraspPoseGenerator): + if not isinstance( + left_generator, ParallelJawGraspPoseGenerator + ) or not isinstance(right_generator, ParallelJawGraspPoseGenerator): raise TypeError( - "CoordinatedPickment requires a " "ParallelJawGraspPoseGenerator." + "CoordinatedPickment requires ParallelJawGraspPoseGenerator " + "services for both grasp endpoints." ) - dual_results = left_generator.get_dual_arm_valid_grasp_poses( - mesh_vertices=semantics.affordance.mesh_vertices, - mesh_triangles=semantics.affordance.mesh_triangles, - obj_poses=object_poses, - left_to_right_arm_direction=left_to_right_arm_direction, - approach_direction=approach_direction, - middle_empty_ratio=options.middle_empty_ratio, - ) - for env_idx, result in enumerate(dual_results): - if result is None: - logger.log_warning( - f"Failed to sample dual-arm grasps for environment {env_idx}." + if left_generator.gripper_model != right_generator.gripper_model: + raise ValueError( + "CoordinatedPickment requires matching left and right parallel-jaw " + "gripper geometry." + ) + partition_ratios = self._candidate_middle_empty_ratios( + semantics.affordance, + object_poses, + left_to_right_arm_direction, + base_ratio=options.middle_empty_ratio, + ) + approach_directions = self._candidate_approach_directions( + semantics.affordance, + object_poses, + left_to_right_arm_direction, + requested=approach_direction, + ) + identity = torch.eye(4, dtype=torch.float32, device=self.device).repeat( + self.num_envs, + 1, + 1, + ) + left_grasp_xpos = identity.clone() + right_grasp_xpos = identity.clone() + success_mask = torch.zeros( + self.num_envs, + dtype=torch.bool, + device=self.device, + ) + sampling_device = torch.device(self.device) + cuda_devices = ( + [ + ( + torch.cuda.current_device() + if sampling_device.index is None + else sampling_device.index ) - continue - left_grasp = self._select_best_grasp(result["left"]) - right_grasp = self._select_best_grasp(result["right"]) - if left_grasp is None or right_grasp is None: - logger.log_warning( - f"No valid left/right grasp for environment {env_idx}." + ] + if sampling_device.type == "cuda" + else [] + ) + selected_candidate: tuple[int, int] | None = None + for approach_index, candidate_approach in enumerate(approach_directions): + for partition_index, partition_ratio in enumerate(partition_ratios): + # GraspKit perturbs approach directions while building candidates. + # Isolate that randomness so retries and alternative partitions do + # not consume or depend on application-global RNG state. Reusing + # one seed also keeps the sampled surface realization fixed while + # geometry policies are the only variables under evaluation. + with torch.random.fork_rng(devices=cuda_devices): + torch.manual_seed(options.grasp_seed) + if cuda_devices: + torch.cuda.manual_seed_all(options.grasp_seed) + dual_results = left_generator.get_dual_arm_valid_grasp_poses( + mesh_vertices=semantics.affordance.mesh_vertices, + mesh_triangles=semantics.affordance.mesh_triangles, + obj_poses=object_poses, + left_to_right_arm_direction=left_to_right_arm_direction, + approach_direction=candidate_approach, + middle_empty_ratio=partition_ratio, + ) + candidate_left, left_success = self._select_reachable_arm_grasp( + dual_results, + role="left", + object_poses=object_poses, + object_target_poses=object_target_poses, + start_qpos=left_start_qpos, + control_part=left_control_part, + options=options, + log_failure=False, ) - continue - left_grasp_xpos[env_idx] = left_grasp.to( - device=self.device, dtype=torch.float32 + candidate_right, right_success = self._select_reachable_arm_grasp( + dual_results, + role="right", + object_poses=object_poses, + object_target_poses=object_target_poses, + start_qpos=right_start_qpos, + control_part=right_control_part, + options=options, + log_failure=False, + ) + selected = ~success_mask & left_success & right_success + left_grasp_xpos = torch.where( + selected[:, None, None], + candidate_left, + left_grasp_xpos, + ) + right_grasp_xpos = torch.where( + selected[:, None, None], + candidate_right, + right_grasp_xpos, + ) + success_mask |= selected + if bool(selected.any().item()) and selected_candidate is None: + selected_candidate = (approach_index, partition_index) + if success_mask.all(): + break + if success_mask.all(): + break + if not success_mask.all(): + failed = torch.nonzero(~success_mask, as_tuple=False).flatten().tolist() + logger.log_warning( + "No jointly reachable coordinated grasp for environment(s) " + f"{failed}; tried {len(approach_directions)} approach directions " + f"and middle-empty ratios {list(partition_ratios)}." ) - right_grasp_xpos[env_idx] = right_grasp.to( - device=self.device, dtype=torch.float32 + elif selected_candidate is not None: + approach_index, partition_index = selected_candidate + logger.log_info( + "Selected coordinated grasp candidate with approach direction " + f"{approach_directions[approach_index].detach().cpu().tolist()} " + f"and middle-empty ratio {partition_ratios[partition_index]}." ) - success_mask[env_idx] = True return left_grasp_xpos, right_grasp_xpos, success_mask @staticmethod - def _select_best_grasp(arm_result: dict) -> torch.Tensor | None: - """Return the lowest-cost grasp pose from one arm's sampler result. + def _candidate_middle_empty_ratios( + affordance: AntipodalAffordance, + object_poses: torch.Tensor, + left_to_right_arm_direction: torch.Tensor, + *, + base_ratio: float, + ) -> tuple[float, ...]: + """Rank deterministic dual-grasp partitions from live object geometry. + + The live-axis candidate respects the current object orientation. An + axis-aligned geometry candidate remains in the bounded search because + a rotated container can otherwise make the projected span too narrow + for both grippers even though its physical end regions remain usable. + """ + vertices = affordance.mesh_vertices + if ( + not isinstance(vertices, torch.Tensor) + or vertices.dim() != 2 + or vertices.shape[0] < 3 + or vertices.shape[1] != 3 + or not bool(torch.isfinite(vertices).all().item()) + ): + return (float(base_ratio),) + local_vertices = vertices.to( + device=object_poses.device, + dtype=object_poses.dtype, + ) + centered = local_vertices - local_vertices.mean(dim=0, keepdim=True) + covariance = ( + centered.transpose(0, 1) @ centered / float(local_vertices.shape[0]) + ) + eigenvalues, eigenvectors = torch.linalg.eigh(covariance) + second = float(eigenvalues[-2].clamp_min(1.0e-12).item()) + longest = float(eigenvalues[-1].clamp_min(1.0e-12).item()) + elongation_ratio = math.sqrt(longest / second) + elongation_confidence = min( + 1.0, + max(0.0, (elongation_ratio - 1.0) / 1.5), + ) + world_axes = torch.matmul(object_poses[:, :3, :3], eigenvectors) + principal_world = world_axes[:, :, -1] + principal_world = principal_world / torch.linalg.vector_norm( + principal_world, + dim=1, + keepdim=True, + ).clamp_min(1.0e-6) + arm_alignment = torch.abs( + torch.sum( + principal_world * left_to_right_arm_direction[None], + dim=1, + ) + ) + geometric_ratio = 0.25 + 0.45 * float(arm_alignment.mean().item()) + preferred_ratio = (1.0 - elongation_confidence) * float( + base_ratio + ) + elongation_confidence * geometric_ratio + axis_aligned_ratio = (1.0 - elongation_confidence) * float( + base_ratio + ) + elongation_confidence * 0.70 + ratios: list[float] = [] + for raw_ratio in ( + preferred_ratio, + axis_aligned_ratio, + float(base_ratio), + preferred_ratio - 0.15, + preferred_ratio + 0.15, + ): + ratio = min(0.90, max(0.05, raw_ratio)) + if not any(abs(ratio - existing) <= 1.0e-6 for existing in ratios): + ratios.append(ratio) + return tuple(ratios) - Args: - arm_result: One ``"left"``/``"right"`` result from the installed - parallel-jaw grasp-pose generator. + @staticmethod + def _candidate_approach_directions( + affordance: AntipodalAffordance, + object_poses: torch.Tensor, + left_to_right_arm_direction: torch.Tensor, + *, + requested: torch.Tensor, + ) -> tuple[torch.Tensor, ...]: + """Build a bounded world-frame approach search for coordinated grasps.""" + candidates: list[torch.Tensor] = [] + + def add(direction: torch.Tensor) -> None: + value = direction.to(device=object_poses.device, dtype=object_poses.dtype) + norm = torch.linalg.vector_norm(value) + if not bool(torch.isfinite(value).all().item()) or float(norm) <= 1.0e-6: + return + value = value / norm + if any( + float(torch.dot(value, existing).item()) >= 1.0 - 1.0e-5 + for existing in candidates + ): + return + candidates.append(value) + + down = requested.new_tensor([0.0, 0.0, -1.0]) + add(requested) + add(down) + horizontal_arm = left_to_right_arm_direction.clone() + horizontal_arm[2] = 0.0 + horizontal_norm = torch.linalg.vector_norm(horizontal_arm) + if float(horizontal_norm) > 1.0e-6: + horizontal_arm = horizontal_arm / horizontal_norm + robot_forward = torch.stack( + (-horizontal_arm[1], horizontal_arm[0], horizontal_arm.new_tensor(0.0)) + ) + add(robot_forward + down) + add(-robot_forward + down) + add(robot_forward) + add(-robot_forward) - Returns: - The selected ``(4, 4)`` grasp pose, or ``None`` when the sampler - reports no valid grasp for this arm. + vertices = affordance.mesh_vertices + if ( + isinstance(vertices, torch.Tensor) + and vertices.dim() == 2 + and vertices.shape[0] >= 3 + and vertices.shape[1] == 3 + and bool(torch.isfinite(vertices).all().item()) + ): + local_vertices = vertices.to( + device=object_poses.device, + dtype=object_poses.dtype, + ) + centered = local_vertices - local_vertices.mean(dim=0, keepdim=True) + covariance = ( + centered.transpose(0, 1) @ centered / float(local_vertices.shape[0]) + ) + _, eigenvectors = torch.linalg.eigh(covariance) + world_axes = torch.matmul(object_poses[:, :3, :3], eigenvectors) + for axis_index in range(3): + axes = world_axes[:, :, axis_index] + if float(torch.mean(torch.abs(axes[:, 2])).item()) > 0.75: + continue + reference = axes[0] + consistency = torch.abs(torch.matmul(axes, reference)) + if bool((consistency < 0.90).any().item()): + continue + add(reference + down) + add(-reference + down) + add(reference) + add(-reference) + break + return tuple(candidates) + + def _select_reachable_arm_grasp( + self, + dual_results: list[dict[str, dict[str, object]] | None], + *, + role: str, + object_poses: torch.Tensor, + object_target_poses: torch.Tensor, + start_qpos: torch.Tensor, + control_part: str, + options: CoordinatedPickmentOptions, + log_failure: bool = True, + ) -> tuple[torch.Tensor, torch.Tensor]: + """Select the lowest-cost candidate with a reachable transport route.""" + live_eef_pose = self.robot.compute_fk( + qpos=start_qpos, + name=control_part, + to_matrix=True, + ) + live_eef_pose = torch.as_tensor( + live_eef_pose, + dtype=torch.float32, + device=self.device, + ) + if live_eef_pose.shape != (self.num_envs, 4, 4): + raise ValueError( + f"Current {control_part} pose must have shape " + f"({self.num_envs}, 4, 4), got {tuple(live_eef_pose.shape)}." + ) + candidates, costs, sampled = self._ranked_arm_grasp_candidates( + dual_results, + role=role, + live_eef_pose=live_eef_pose, + ) + candidate_count = candidates.shape[1] + object_to_eef = torch.matmul( + pose_inv(object_poses)[:, None], + candidates, + ) + pre_grasp = candidates.clone() + pre_grasp[..., :3, 3] -= pre_grasp[..., :3, 2] * options.pre_grasp_distance + lifted_object = translate_pose_world( + object_poses, + torch.tensor( + [0.0, 0.0, options.lift_height], + dtype=object_poses.dtype, + device=self.device, + ), + ) + lifted_eef = torch.matmul(lifted_object[:, None], object_to_eef) + stages: list[tuple[str, torch.Tensor]] = [ + ("pre_grasp", pre_grasp), + ("grasp", candidates), + ("lift", lifted_eef), + ] + + # Screen the transport with the same continuation used by the final + # synchronized plan. Solving the terminal pose directly from the lift + # seed can reject a reachable route when a sparse IK sampler needs the + # intermediate solutions to stay on one joint-space branch. + transport_keyframes = self._interpolate_object_pose( + lifted_object, + object_target_poses, + max(2, options.object_motion_keyframes), + include_orientation=True, + ) + for keyframe_index in range(1, transport_keyframes.shape[1]): + stages.append( + ( + f"transport_{keyframe_index}", + torch.matmul( + transport_keyframes[:, keyframe_index, None], + object_to_eef, + ), + ) + ) + target_eef = stages[-1][1] + if options.release and options.retreat_distance > 0.0: + retreat_eef = target_eef.clone() + retreat_eef[..., 2, 3] += options.retreat_distance + stages.append(("retreat", retreat_eef)) + + seed = start_qpos[:, None, :].expand(-1, candidate_count, -1).clone() + feasible = sampled.clone() + stage_counts: dict[str, list[int]] = {} + for stage_name, stage_poses in stages: + result = self.robot.compute_batch_ik( + pose=stage_poses, + name=control_part, + joint_seed=seed, + ) + if type(result) is not tuple or len(result) != 2: + raise TypeError( + "CoordinatedPickment batch IK must return (success, qpos)." + ) + stage_success = torch.as_tensor( + result[0], + dtype=torch.bool, + device=self.device, + ) + stage_qpos = torch.as_tensor( + result[1], + dtype=torch.float32, + device=self.device, + ) + if stage_success.shape != sampled.shape: + raise ValueError( + f"Batch IK success for {control_part} {stage_name} must have " + f"shape {tuple(sampled.shape)}, got {tuple(stage_success.shape)}." + ) + if stage_qpos.shape != seed.shape: + raise ValueError( + f"Batch IK qpos for {control_part} {stage_name} must have " + f"shape {tuple(seed.shape)}, got {tuple(stage_qpos.shape)}." + ) + feasible &= stage_success + seed = torch.where(feasible[..., None], stage_qpos, seed) + stage_counts[stage_name] = feasible.sum(dim=1).tolist() + + feasible_costs = torch.where( + feasible, + costs, + torch.full_like(costs, torch.inf), + ) + best_cost, best_index = feasible_costs.min(dim=1) + success = torch.isfinite(best_cost) + env_index = torch.arange(candidates.shape[0], device=self.device) + selected = candidates[env_index, best_index] + if log_failure and not success.all(): + failed = torch.nonzero(~success, as_tuple=False).flatten().tolist() + logger.log_warning( + f"CoordinatedPickment {role} route screening failed for " + f"environment(s) {failed}: sampled={sampled.sum(dim=1).tolist()}, " + f"reachable={stage_counts}." + ) + return selected, success + + def _ranked_arm_grasp_candidates( + self, + dual_results: list[dict[str, dict[str, object]] | None], + *, + role: str, + live_eef_pose: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Pad wrist-canonicalized candidates in ascending total-cost order.""" + if role not in {"left", "right"}: + raise ValueError("Coordinated grasp role must be 'left' or 'right'.") + if live_eef_pose.shape != (len(dual_results), 4, 4): + raise ValueError( + "live_eef_pose must provide one 4x4 pose per dual-grasp result." + ) + ranked: list[tuple[torch.Tensor, torch.Tensor]] = [] + max_candidates = 0 + for row_index, result in enumerate(dual_results): + if result is None: + poses = torch.empty((0, 4, 4), device=self.device) + candidate_costs = torch.empty((0,), device=self.device) + else: + arm_result = result.get(role) + if not isinstance(arm_result, dict) or not arm_result.get( + "is_success", False + ): + poses = torch.empty((0, 4, 4), device=self.device) + candidate_costs = torch.empty((0,), device=self.device) + else: + poses = torch.as_tensor( + arm_result["grasp_poses"], + dtype=torch.float32, + device=self.device, + ) + candidate_costs = torch.as_tensor( + arm_result["total_cost"], + dtype=torch.float32, + device=self.device, + ).reshape(-1) + if poses.shape == (4, 4): + poses = poses.unsqueeze(0) + if poses.dim() != 3 or poses.shape[1:] != (4, 4): + raise ValueError( + f"Coordinated {role} grasp poses must have shape " + f"(N, 4, 4), got {tuple(poses.shape)}." + ) + if poses.shape[0] != candidate_costs.shape[0]: + raise ValueError( + f"Coordinated {role} grasp poses and costs must have " + "equal candidate counts." + ) + finite = torch.isfinite(candidate_costs) + poses, wrist_rotation_cost = self._canonicalize_parallel_jaw_poses( + poses[finite], + live_eef_pose[row_index], + ) + candidate_costs = ( + candidate_costs[finite] + wrist_rotation_cost / math.pi + ) + order = torch.argsort(candidate_costs) + poses = poses[order][: self._MAX_REACHABILITY_CANDIDATES] + candidate_costs = candidate_costs[order][ + : self._MAX_REACHABILITY_CANDIDATES + ] + ranked.append((poses, candidate_costs)) + max_candidates = max(max_candidates, poses.shape[0]) + + padded_count = max(1, max_candidates) + identity = torch.eye(4, dtype=torch.float32, device=self.device) + poses = identity.repeat(len(ranked), padded_count, 1, 1) + costs = torch.full( + (len(ranked), padded_count), + torch.inf, + dtype=torch.float32, + device=self.device, + ) + sampled = torch.zeros_like(costs, dtype=torch.bool) + for env_index, (env_poses, env_costs) in enumerate(ranked): + count = env_poses.shape[0] + if count == 0: + continue + poses[env_index, :count] = env_poses + costs[env_index, :count] = env_costs + sampled[env_index, :count] = True + return poses, costs, sampled + + @staticmethod + def _canonicalize_parallel_jaw_poses( + poses: torch.Tensor, + live_eef_pose: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + """Choose each grasp's local-z half-turn nearest the live wrist. + + A parallel-jaw grasp is physically unchanged by a 180-degree rotation + around its TCP z axis. Canonicalizing that symmetry before IK avoids + rejecting an otherwise reachable top-down grasp solely because the + sampler returned the opposite wrist roll. """ - if not arm_result.get("is_success", False): - return None - grasp_poses = arm_result["grasp_poses"].to(dtype=torch.float32) - costs = arm_result["total_cost"].to(dtype=torch.float32) - if grasp_poses.dim() == 2: - # The sampler returns a single eye(4) placeholder when it finds no - # valid pair; is_success should already cover this, but stay robust. - grasp_poses = grasp_poses.unsqueeze(0) - costs = costs.unsqueeze(0) - if grasp_poses.shape[0] == 0: - return None - best_idx = torch.argmin(costs) - if not torch.isfinite(costs[best_idx]): - return None - return grasp_poses[best_idx] + if poses.dim() != 3 or poses.shape[1:] != (4, 4): + raise ValueError("poses must have shape (N, 4, 4).") + if live_eef_pose.shape != (4, 4): + raise ValueError("live_eef_pose must have shape (4, 4).") + half_turn = torch.eye( + 4, + dtype=poses.dtype, + device=poses.device, + ) + half_turn[0, 0] = -1.0 + half_turn[1, 1] = -1.0 + alternatives = torch.matmul(poses, half_turn) + live_rotation = ( + live_eef_pose[:3, :3] + .unsqueeze(0) + .expand( + poses.shape[0], + -1, + -1, + ) + ) + + def rotation_distance(candidate: torch.Tensor) -> torch.Tensor: + relative = torch.matmul( + live_rotation.transpose(-1, -2), + candidate[:, :3, :3], + ) + cosine = ( + torch.diagonal(relative, dim1=-2, dim2=-1).sum(dim=-1) - 1.0 + ) * 0.5 + return torch.acos(torch.clamp(cosine, -1.0, 1.0)) + + original_cost = rotation_distance(poses) + alternative_cost = rotation_distance(alternatives) + use_alternative = alternative_cost < original_cost + return ( + torch.where(use_alternative[:, None, None], alternatives, poses), + torch.where(use_alternative, alternative_cost, original_cost), + ) def _compute_segment_lengths( self, sample_count: int, options: CoordinatedPickmentOptions @@ -675,14 +1196,16 @@ def _compute_segment_lengths( """Split the invocation sample budget across coordinated-pick segments.""" n_close = max(2, options.hand_interp_steps) n_hold = max(0, options.hold_steps) - n_motion = sample_count - n_close - n_hold + n_release = max(2, options.release_steps) if options.release else 0 + n_retreat = max(2, options.retreat_steps) if options.release else 0 + n_motion = sample_count - n_close - n_hold - n_release - n_retreat n_approach = n_motion // 3 n_lift = n_motion // 3 n_move = n_motion - n_approach - n_lift if min(n_approach, n_lift, n_move) < 2: raise ValueError( "Not enough waypoints for coordinated pickment. Please increase " - "sample_count or decrease hand_interp_steps/hold_steps." + "sample_count or decrease close/hold/release/retreat steps." ) return { "approach": n_approach, @@ -690,6 +1213,8 @@ def _compute_segment_lengths( "lift": n_lift, "move": n_move, "hold": n_hold, + "release": n_release, + "retreat": n_retreat, } def _compute_pre_grasp_xpos( @@ -881,6 +1406,9 @@ def _plan( "Coordinated dual-arm planning is not supported by the cuRobo backend." ) state = context + left_start_qpos, right_start_qpos = self._resolve_dual_arm_start( + state, resources + ) ( object_initial_pose, object_target_pose, @@ -890,7 +1418,14 @@ def _plan( right_target_xpos, held_states, grasp_success, - ) = self._resolve_target(target, context, options, resources) + ) = self._resolve_target( + target, + context, + options, + resources, + left_start_qpos, + right_start_qpos, + ) left_held_state, right_held_state = held_states if not grasp_success.any(): logger.log_warning("CoordinatedPickment failed to resolve dual-arm grasps.") @@ -899,9 +1434,6 @@ def _plan( context, message="Failed to resolve dual-arm grasps.", ) - left_start_qpos, right_start_qpos = self._resolve_dual_arm_start( - state, resources - ) segments = self._compute_segment_lengths( request.motion_policy.sample_count, options ) @@ -1036,6 +1568,63 @@ def _plan( resources=resources, ) + release_trajectory = torch.empty( + (self.num_envs, 0, self.robot_dof), + dtype=torch.float32, + device=self.device, + ) + retreat_trajectory = torch.empty( + (self.num_envs, 0, self.robot_dof), + dtype=torch.float32, + device=self.device, + ) + if options.release: + release_trajectory = self._assemble_segment( + state, + self._repeat_qpos(left_target_qpos, segments["release"]), + self._repeat_qpos(right_target_qpos, segments["release"]), + self._interpolate_qpos( + resources.left_hand_close_qpos, + resources.left_hand_open_qpos, + segments["release"], + ), + self._interpolate_qpos( + resources.right_hand_close_qpos, + resources.right_hand_open_qpos, + segments["release"], + ), + resources=resources, + ) + retreat_delta = torch.tensor( + [0.0, 0.0, options.retreat_distance], + dtype=torch.float32, + device=self.device, + ) + left_retreat_xpos = translate_pose_world(left_target_xpos, retreat_delta) + right_retreat_xpos = translate_pose_world(right_target_xpos, retreat_delta) + success_mask, left_retreat_traj = self._plan_masked_arm_trajectory( + resources.left_arm.control_part, + left_target_qpos, + left_retreat_xpos.unsqueeze(1), + segments["retreat"], + success_mask, + ) + success_mask, right_retreat_traj = self._plan_masked_arm_trajectory( + resources.right_arm.control_part, + right_target_qpos, + right_retreat_xpos.unsqueeze(1), + segments["retreat"], + success_mask, + ) + retreat_trajectory = self._assemble_segment( + state, + left_retreat_traj, + right_retreat_traj, + self._repeat_qpos(resources.left_hand_open_qpos, segments["retreat"]), + self._repeat_qpos(resources.right_hand_open_qpos, segments["retreat"]), + resources=resources, + ) + full = torch.cat( [ approach_trajectory, @@ -1043,6 +1632,8 @@ def _plan( lift_trajectory, move_trajectory, hold_trajectory, + release_trajectory, + retreat_trajectory, ], dim=1, ) @@ -1067,8 +1658,12 @@ def _plan( ), expected_effects=StateDelta( held_object_updates={ - resources.left_task_state_key: left_held_object, - resources.right_task_state_key: right_held_object, + resources.left_task_state_key: ( + None if options.release else left_held_object + ), + resources.right_task_state_key: ( + None if options.release else right_held_object + ), }, ), segment_lengths={ @@ -1077,7 +1672,28 @@ def _plan( "lift": lift_trajectory.shape[1], "move": move_trajectory.shape[1], "hold": hold_trajectory.shape[1], + "release": release_trajectory.shape[1], + "retreat": retreat_trajectory.shape[1], }, + # The approach has three evenly spaced keyframes: current, + # pre-grasp, and grasp. Contact may move the coordinated object + # during the pre-grasp-to-grasp leg, so stop treating that + # expected self-motion as an external scene revision once both + # grippers reach the pre-grasp keyframe. Independent late-bound + # destination dependencies remain monitored for transport. + scene_dependency_monitor_until=( + {} + if ( + target.object_initial_pose is not None + or target.semantics.entity_id is None + ) + else { + target.semantics.entity_id: max( + 1, + math.ceil(approach_trajectory.shape[1] / 2), + ) + } + ), ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 0919f6a56..73f6180dd 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -21,7 +21,7 @@ import math from collections.abc import Mapping from dataclasses import dataclass -from typing import ClassVar +from typing import ClassVar, Literal import torch @@ -104,13 +104,56 @@ class HandOverOptions(ActionOptions): hand_interp_steps: int = 10 """Waypoints used by every gripper open/close interpolation.""" + hold_steps: int = 4 + """Closed-hand waypoints used to settle a receiving grasp.""" + + retreat_steps: int = 24 + """Waypoints used while the source hand retreats after release.""" + + retreat_distance: float = 0.10 + """Distance retraced opposite the source TCP approach before lifting.""" + + receive_pick_object_part: Literal["center", "top", "bottom"] = "bottom" + """Object end selected by the receiving gripper for an existing hold.""" + + release_at_target: bool = True + """Whether the receiving hand places and releases after the transfer. + + When false, execution ends after the source hand opens and the receiving + resource remains the verified holder. This supports a semantic handover + followed by a later Place call without moving that workflow into Task Engine. + """ + + arm_selection: Literal["nearest", "bound"] = "nearest" + """How the transfer participant is selected. + + ``"nearest"`` preserves the low-level Atomic Action default for direct + callers. Semantic Task Program bindings should select ``"bound"`` so the + explicit ``source`` and ``destination`` resource slots are authoritative. + """ + def __post_init__(self) -> None: - for name in ("pre_grasp_distance", "lift_height"): + for name in ("pre_grasp_distance", "lift_height", "retreat_distance"): value = getattr(self, name) if not math.isfinite(value) or value < 0.0: raise ValueError(f"{name} must be finite and non-negative.") if self.hand_interp_steps < 1: raise ValueError("hand_interp_steps must be at least 1.") + for name in ("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.") + if self.retreat_steps < 2: + raise ValueError("retreat_steps must be at least 2.") + if self.receive_pick_object_part not in ("center", "top", "bottom"): + raise ValueError( + "receive_pick_object_part must be exactly 'center', 'top', or " + "'bottom'." + ) + if type(self.release_at_target) is not bool: + raise TypeError("release_at_target must be a bool.") + if self.arm_selection not in ("nearest", "bound"): + raise ValueError("arm_selection must be exactly 'nearest' or 'bound'.") @dataclass(frozen=True, slots=True, eq=False) @@ -146,7 +189,7 @@ class _DirectionalPlan: class HandOver(AtomicAction[HandOverGoal, HandOverOptions]): - """Pick an object with the nearer arm, hand it over, and place it. + """Pick an object with the nearer arm and transfer it to the other arm. For each environment, the action chooses the arm whose root link is closer to the observed object pose. It samples at most 1000 mesh-surface points and @@ -155,15 +198,21 @@ class HandOver(AtomicAction[HandOverGoal, HandOverOptions]): approaches point toward the object horizontally and tilt downward by 45 degrees. Otherwise both approaches are world-Z downward. - The first arm grasps the projected end of ``obj_longest_axis`` nearest its - current TCP; the receiving arm grasps the opposite end at the predicted - middle object pose. This keeps the two hands from selecting the same object - region regardless of whether a long object is standing or lying down. + For a free object, the first arm grasps the projected end of + ``obj_longest_axis`` nearest its current TCP. For an existing hold, that + source grasp is inherited from the preceding action and the receiving arm + uses the configured center, top, or bottom region at the predicted middle + object pose. A center request is filtered to the object's middle third; + top and bottom requests let Semantic Call look-ahead reserve the opposite + source end. After each grasp waypoint, subsequent EEF waypoints preserve that grasp - rotation and change translation only. In particular, placement first moves - strictly horizontally at the handover height and then lowers to the final - target pose before releasing the object. + rotation and change translation only. The released source TCP first + retraces its grasp approach before lifting. With + ``release_at_target=True``, the receiving arm additionally moves + horizontally at handover height, lowers to the final target pose, and + releases. Transfer-only mode stops with the receiving arm recorded as the + verified holder for a later Semantic Call. """ skill_id: ClassVar[str] = "hand_over" @@ -285,7 +334,7 @@ def _plan( request: ResolvedActionRequest[HandOverGoal, HandOverOptions], context: PlanningContext, ) -> ActionPlan: - """Plan the complete pick-up, handover, placement, and release.""" + """Plan pickup and transfer, with optional placement and release.""" goal = self.require_goal(request) options = request.skill_options resources = self._resolve_resources(request) @@ -299,6 +348,24 @@ def _plan( if not isinstance(goal.semantics.affordance, AntipodalAffordance): raise ValueError("HandOver requires an AntipodalAffordance.") + # A semantic handover is also the continuation point after an explicit + # Pick call. In that case the source attachment is already verified by + # TaskState and the action must transfer that attachment rather than + # silently attempting a second pickup. Keep the legacy unified route + # below for direct low-level callers that start with two free arms. + source_held = context.task.get_held_object(resources.first.task_state_key) + source_mask = context.task.held_object_mask(resources.first.task_state_key) + if source_held is not None and source_mask.any(): + if self._same_object(goal.semantics, source_held): + if options.release_at_target: + raise ValueError( + "HandOver cannot place an already-held object in the same " + "invocation; use a following Place call." + ) + return self._plan_existing_hold( + request, context, resources, source_held + ) + object_pose = _resolve_object_pose( goal.semantics, context, @@ -320,13 +387,23 @@ def _plan( ) first_root_pose = self._root_link_pose(resources.first.arm, context.env_ids) second_root_pose = self._root_link_pose(resources.second.arm, context.env_ids) - first_distance = torch.linalg.vector_norm( - object_pose[:, :3, 3] - first_root_pose[:, :3, 3], dim=1 - ) - second_distance = torch.linalg.vector_norm( - object_pose[:, :3, 3] - second_root_pose[:, :3, 3], dim=1 - ) - first_is_handover = first_distance <= second_distance + if options.arm_selection == "bound": + # Semantic bindings are authoritative: ``source`` acquires and + # ``destination`` receives. Do not silently invert an explicit + # request merely because the object starts nearer the other arm. + first_is_handover = torch.ones( + self.num_envs, + dtype=torch.bool, + device=self.device, + ) + else: + first_distance = torch.linalg.vector_norm( + object_pose[:, :3, 3] - first_root_pose[:, :3, 3], dim=1 + ) + second_distance = torch.linalg.vector_norm( + object_pose[:, :3, 3] - second_root_pose[:, :3, 3], dim=1 + ) + first_is_handover = first_distance <= second_distance # This unified action starts before pickup. Rows where either bound arm # already holds an object are therefore ineligible and remain at the @@ -465,12 +542,41 @@ def _plan( semantics=goal.semantics, object_to_eef=first_object_to_eef, grasp_xpos=first_grasp_xpos, + env_mask=(None if options.release_at_target else ~first_is_handover), ) second_candidate = HeldObjectState( semantics=goal.semantics, object_to_eef=second_object_to_eef, grasp_xpos=second_grasp_xpos, + env_mask=(None if options.release_at_target else first_is_handover), + ) + first_effect_candidate = HeldObjectState( + semantics=goal.semantics, + object_to_eef=first_object_to_eef, + grasp_xpos=first_grasp_xpos, + env_mask=eligible, + ) + second_effect_candidate = HeldObjectState( + semantics=goal.semantics, + object_to_eef=second_object_to_eef, + grasp_xpos=second_grasp_xpos, + env_mask=eligible, ) + if options.release_at_target: + terminal_updates = { + resources.first.task_state_key: None, + resources.second.task_state_key: None, + } + elif options.arm_selection == "bound": + terminal_updates = { + resources.first.task_state_key: None, + resources.second.task_state_key: second_effect_candidate, + } + else: + terminal_updates = { + resources.first.task_state_key: first_candidate, + resources.second.task_state_key: second_candidate, + } return self.build_plan( request, @@ -482,29 +588,499 @@ def _plan( step_dt=context.require_control_dt(), ), expected_effects=StateDelta( - held_object_updates={ - resources.first.task_state_key: None, - resources.second.task_state_key: None, - }, + held_object_updates=terminal_updates, ), effect_candidates=StateDelta( held_object_updates={ - resources.first.task_state_key: first_candidate, - resources.second.task_state_key: second_candidate, + resources.first.task_state_key: first_effect_candidate, + resources.second.task_state_key: second_effect_candidate, }, ), segment_lengths=segment_lengths, + # The object may move from contact as soon as the pickup gripper + # starts closing. Keep dynamic-target monitoring active through + # the approach, but do not classify expected pickup motion as an + # external scene revision. scene_dependency_monitor_until=( {} if goal.semantics.entity_id is None - else { - goal.semantics.entity_id: ( - segment_lengths["pickup_approach"] - + segment_lengths["pickup_close"] - ) + else {goal.semantics.entity_id: segment_lengths["pickup_approach"]} + ), + ) + + def _plan_existing_hold( + self, + request: ResolvedActionRequest[HandOverGoal, HandOverOptions], + context: PlanningContext, + resources: _HandOverResources, + held: HeldObjectState, + ) -> ActionPlan: + """Transfer a verified source attachment to the destination hand. + + This is the canonical continuation used by ``Pick -> HandOver``. It + deliberately lives in the Atomic Action so Task Engine never owns + grasp poses, hand timing, or a second physical execution loop. + """ + goal = self.require_goal(request) + options = request.skill_options + source_mask = context.task.exclusive_held_object_mask( + resources.first.task_state_key + ) + destination_mask = context.task.held_object_mask( + resources.second.task_state_key + ) + eligible = source_mask & ~destination_mask + self._report_waypoint_failure( + context, + "existing_source_attachment", + ~source_mask, + "source participant does not own the requested object", + ) + self._report_waypoint_failure( + context, + "destination_unoccupied", + destination_mask, + "destination participant already holds an object", + ) + if not eligible.any(): + return self.failed_plan( + request, + context, + message=( + "HandOver requires an exclusive source attachment and an " + "unoccupied destination." + ), + ) + + source_start_qpos = context.last_qpos[:, list(resources.first.arm.joint_ids)] + destination_start_qpos = context.last_qpos[ + :, list(resources.second.arm.joint_ids) + ] + source_object_to_eef = held.object_to_eef.to( + device=self.device, dtype=torch.float32 + ) + if source_object_to_eef.dim() == 2: + source_object_to_eef = source_object_to_eef.unsqueeze(0).expand( + self.num_envs, -1, -1 + ) + source_eef = self.robot.compute_fk( + qpos=source_start_qpos, + name=resources.first.arm.control_part, + to_matrix=True, + ) + current_object_pose = torch.bmm(source_eef, pose_inv(source_object_to_eef)) + + source_root = self._root_link_pose(resources.first.arm, context.env_ids) + destination_root = self._root_link_pose( + resources.second.arm, + context.env_ids, + ) + # A continuation transfer derives its shared-workspace coordinate from + # the two bound arm roots, just like the unified free-arm route. The + # configured final target supplies only a safe absolute height because + # this mode deliberately leaves the destination holding the object. + # This keeps both transfer directions reachable without task-owned arm + # poses or direction-specific provider constants. + exchange_pose = current_object_pose.clone() + target_pose = resolve_batched_pose( + resolve_pose_goal( + goal.target_pose, + context, + name="handover_exchange_pose", + ), + num_envs=self.num_envs, + device=self.device, + name="handover_exchange_pose", + ) + exchange_pose[:, 2, 3] = torch.maximum( + current_object_pose[:, 2, 3], + target_pose[:, 2, 3], + ) + exchange_pose = self._middle_object_pose( + exchange_pose, + source_root, + destination_root, + ) + exchange_pose[:, :3, :3] = current_object_pose[:, :3, :3] + + source_exchange_eef = torch.bmm(exchange_pose, source_object_to_eef) + destination_eef = self.robot.compute_fk( + qpos=destination_start_qpos, + name=resources.second.arm.control_part, + to_matrix=True, + ) + # Approach diagonally from the receiver's side of the embodiment. A + # top-down receiver places two bulky parallel grippers in the same + # vertical envelope and can squeeze the object out while the source + # opens. Root-to-root direction is stable, robot-generic role geometry + # and reproduces the successful inward approach for either transfer + # direction without embedding left/right names. + approach_direction, approach_direction_valid = ( + self._downward_diagonal_approach_direction( + destination_root[:, :3, 3], + source_root[:, :3, 3], + ) + ) + + affordance = goal.semantics.affordance + assert isinstance(affordance, AntipodalAffordance) + longest_axis = affordance.get_object_longest_axis( + exchange_pose, + max_points=self._SURFACE_POINT_COUNT, + ) + receive_center_axis: torch.Tensor | None = None + if options.receive_pick_object_part != "center": + local_axis = exchange_pose.new_tensor([0.0, 0.0, 1.0]) + receive_axis: torch.Tensor | None = torch.matmul( + exchange_pose[:, :3, :3], local_axis + ) + receive_positive = torch.full( + (self.num_envs,), + options.receive_pick_object_part == "top", + dtype=torch.bool, + device=self.device, + ) + else: + # Ask the grasp service for all collision-free candidates, then + # retain only candidates through the object's middle third. The + # grasp-service axis selector can choose only one outer end, so it + # cannot represent a true center grasp by itself. + receive_axis = None + receive_center_axis = longest_axis + receive_positive = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + destination_grasp, grasp_success = self._resolve_grasp( + affordance, + exchange_pose, + approach_direction, + resources.second.hand.target_id, + obj_longest_axis=receive_axis, + is_positive_part=receive_positive, + center_axis=receive_center_axis, + ) + destination_pre_grasp = translate_pose_world( + destination_grasp, + -destination_grasp[:, :3, 2] * options.pre_grasp_distance, + ) + destination_object_to_eef = torch.bmm( + pose_inv(exchange_pose), destination_grasp + ) + source_retreat_waypoints = self._source_retreat_waypoints( + source_exchange_eef, + destination_grasp, + source_fallback=source_eef, + destination_fallback=destination_eef, + retreat_distance=options.retreat_distance, + lift_height=options.lift_height, + ) + + lengths = self._compute_existing_hold_segment_lengths( + request.motion_policy.sample_count, + options, + ) + success = ( + normalize_success_mask( + grasp_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver receiving-grasp success", + ) + & approach_direction_valid + & eligible + ) + self._report_waypoint_failure( + context, + "receive_approach_direction", + eligible & ~approach_direction_valid, + "source and destination roots have no horizontal separation", + ) + self._report_waypoint_failure( + context, + "receive_grasp", + eligible & ~success, + "no finite receiving grasp was found", + ) + + phase_success, source_transfer = plan_named_arm_trajectory( + self.motion_generator, + resources.first.arm.control_part, + source_start_qpos, + source_exchange_eef.unsqueeze(1), + lengths["transfer"], + request.motion_policy, + context.control_dt, + ) + success &= normalize_success_mask( + phase_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver existing-hold source transfer success", + ) + source_hold_qpos = source_transfer[:, -1] + phase_success, destination_approach = plan_named_arm_trajectory( + self.motion_generator, + resources.second.arm.control_part, + destination_start_qpos, + torch.stack((destination_pre_grasp, destination_grasp), dim=1), + lengths["approach"], + request.motion_policy, + context.control_dt, + ) + success &= normalize_success_mask( + phase_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver existing-hold destination approach success", + ) + destination_hold_qpos = destination_approach[:, -1] + phase_success, source_retreat = plan_named_arm_trajectory( + self.motion_generator, + resources.first.arm.control_part, + source_hold_qpos, + source_retreat_waypoints, + lengths["retreat"], + request.motion_policy, + context.control_dt, + ) + success &= normalize_success_mask( + phase_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver existing-hold source retreat success", + ) + + segment_values: list[tuple[str, torch.Tensor]] = [ + ( + "transfer", + self._assemble_segment( + context, + source_transfer, + repeat_qpos(destination_start_qpos, lengths["transfer"]), + repeat_qpos(resources.first.hand_grasp_qpos, lengths["transfer"]), + repeat_qpos(resources.second.hand_open_qpos, lengths["transfer"]), + resources.first, + resources.second, + ), + ), + ( + "receive_approach", + self._assemble_segment( + context, + repeat_qpos(source_hold_qpos, lengths["approach"]), + destination_approach, + repeat_qpos(resources.first.hand_grasp_qpos, lengths["approach"]), + repeat_qpos(resources.second.hand_open_qpos, lengths["approach"]), + resources.first, + resources.second, + ), + ), + ( + "receive_close", + self._assemble_segment( + context, + repeat_qpos(source_hold_qpos, lengths["close"]), + repeat_qpos(destination_hold_qpos, lengths["close"]), + repeat_qpos(resources.first.hand_grasp_qpos, lengths["close"]), + interpolate_hand_qpos( + resources.second.hand_open_qpos, + resources.second.hand_grasp_qpos, + n_waypoints=lengths["close"], + ), + resources.first, + resources.second, + ), + ), + ] + if lengths["hold"]: + segment_values.append( + ( + "receive_hold", + self._assemble_segment( + context, + repeat_qpos(source_hold_qpos, lengths["hold"]), + repeat_qpos(destination_hold_qpos, lengths["hold"]), + repeat_qpos(resources.first.hand_grasp_qpos, lengths["hold"]), + repeat_qpos(resources.second.hand_grasp_qpos, lengths["hold"]), + resources.first, + resources.second, + ), + ) + ) + segment_values.extend( + ( + ( + "handover_release", + self._assemble_segment( + context, + repeat_qpos(source_hold_qpos, lengths["release"]), + repeat_qpos(destination_hold_qpos, lengths["release"]), + interpolate_hand_qpos( + resources.first.hand_grasp_qpos, + resources.first.hand_open_qpos, + n_waypoints=lengths["release"], + ), + repeat_qpos( + resources.second.hand_grasp_qpos, lengths["release"] + ), + resources.first, + resources.second, + ), + ), + ( + "source_retreat", + self._assemble_segment( + context, + source_retreat, + repeat_qpos(destination_hold_qpos, lengths["retreat"]), + repeat_qpos(resources.first.hand_open_qpos, lengths["retreat"]), + repeat_qpos( + resources.second.hand_grasp_qpos, lengths["retreat"] + ), + resources.first, + resources.second, + ), + ), + ) + ) + trajectory = torch.cat([value for _, value in segment_values], dim=1) + received = HeldObjectState( + semantics=held.semantics, + object_to_eef=destination_object_to_eef, + grasp_xpos=destination_grasp, + env_mask=eligible, + ) + 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.first.task_state_key: None, + resources.second.task_state_key: received, + } + ), + effect_candidates=StateDelta( + held_object_updates={ + resources.first.task_state_key: held, + resources.second.task_state_key: received, } ), + segment_lengths={name: value.shape[1] for name, value in segment_values}, + scene_dependency_monitor_until={ + entity_id: 0 for entity_id in self._scene_dependencies(request) + }, + ) + + @staticmethod + def _source_retreat_waypoints( + source_exchange_eef: torch.Tensor, + destination_grasp: torch.Tensor, + *, + source_fallback: torch.Tensor, + destination_fallback: torch.Tensor, + retreat_distance: float, + lift_height: float, + ) -> torch.Tensor: + """Retrace the source grasp approach before lifting its open TCP. + + Moving along the line between the two TCP origins is not generally a + valid withdrawal direction: for a top grasp it sweeps the open source + fingers sideways through the transferred object. TCP ``-z`` is the + inverse of the source grasp approach, so intermediate Cartesian + waypoints on that ray clear the fingers before adding world-up + clearance. Root separation remains only a degenerate-pose fallback. + """ + if source_exchange_eef.shape != destination_grasp.shape: + raise ValueError( + "Source exchange and destination grasp poses must have matching " + "shapes." + ) + if source_fallback.shape != source_exchange_eef.shape: + raise ValueError("Source fallback poses must match exchange poses.") + if destination_fallback.shape != destination_grasp.shape: + raise ValueError("Destination fallback poses must match grasp poses.") + + direction = -source_exchange_eef[:, :3, 2] + fallback = source_fallback[:, :3, 3] - destination_fallback[:, :3, 3] + fallback[:, 2] = 0.0 + direction_norm = torch.linalg.vector_norm(direction, dim=1, keepdim=True) + fallback_norm = torch.linalg.vector_norm(fallback, dim=1, keepdim=True) + direction = torch.where( + direction_norm > 1.0e-6, + direction, + torch.where( + fallback_norm > 1.0e-6, + fallback, + direction.new_tensor([1.0, 0.0, 0.0]).expand_as(direction), + ), ) + direction = direction / torch.linalg.vector_norm( + direction, dim=1, keepdim=True + ).clamp_min(1.0e-6) + + retreat_fractions = source_exchange_eef.new_tensor([1.0 / 3.0, 2.0 / 3.0, 1.0]) + retreat = source_exchange_eef[:, None].repeat(1, 3, 1, 1) + retreat[:, :, :3, 3] += ( + direction[:, None] * retreat_fractions[None, :, None] * retreat_distance + ) + lifted = retreat[:, -1:].repeat(1, 2, 1, 1) + lifted[:, :, 2, 3] += lifted.new_tensor([0.5, 1.0])[None] * lift_height + return torch.cat((retreat, lifted), dim=1) + + @staticmethod + def _same_object( + requested: object, + held: HeldObjectState, + ) -> bool: + """Return whether semantic object identity matches a held relation.""" + requested_entity_id = getattr(requested, "entity_id", None) + held_entity_id = held.semantics.entity_id + if requested_entity_id is not None and held_entity_id is not None: + return requested_entity_id == held_entity_id + requested_entity = getattr(requested, "entity", None) + held_entity = held.semantics.entity + if requested_entity is not None and held_entity is not None: + return requested_entity is held_entity + requested_label = getattr(requested, "label", None) + return bool(requested_label) and requested_label == held.semantics.label + + @staticmethod + def _compute_existing_hold_segment_lengths( + sample_count: int, + options: HandOverOptions, + ) -> dict[str, int]: + """Split one existing-hold transfer into motion and hand phases.""" + close = max(2, options.hand_interp_steps) + release = max(2, options.hand_interp_steps) + hold = options.hold_steps + retreat = max(2, options.retreat_steps) + reserved = close + release + hold + retreat + remaining = sample_count - reserved + if remaining < 4: + raise ValueError( + "Not enough HandOver waypoints for an existing held-object " + "transfer; increase sample_count or reduce hand phases." + ) + transfer = max(2, remaining // 2) + approach = remaining - transfer + if approach < 2: + raise ValueError( + "Not enough HandOver waypoints for the receiving approach." + ) + return { + "transfer": transfer, + "approach": approach, + "close": close, + "hold": hold, + "release": release, + "retreat": retreat, + } def _find_symmetric_nearest_xpos( self, target_xpos: torch.Tensor, reference_xpos: torch.Tensor @@ -665,22 +1241,30 @@ def _plan_direction( pose_inv(middle_object_pose), receive_grasp, ) - placed_object_pose = final_object_pose.clone() - placed_object_pose[:, :3, :3] = middle_object_pose[:, :3, :3] - above_object_pose = placed_object_pose.clone() - # Move to the target's horizontal coordinates while preserving the - # middle handover height exactly. The following target performs the - # only vertical motion and reaches the requested final object pose. - above_object_pose[:, 2, 3] = middle_object_pose[:, 2, 3] - lowering_direction_valid = ( - above_object_pose[:, 2, 3] - placed_object_pose[:, 2, 3] > 1.0e-6 - ) - receive_above_eef = torch.bmm(above_object_pose, receive_object_to_eef) - receive_final_eef = torch.bmm(placed_object_pose, receive_object_to_eef) - # Likewise, receiving-grasp through final lowering reuses the same EEF - # rotation and changes translation only. - receive_above_eef[:, :3, :3] = receive_grasp[:, :3, :3] - receive_final_eef[:, :3, :3] = receive_grasp[:, :3, :3] + lowering_direction_valid = torch.ones( + self.num_envs, + dtype=torch.bool, + device=self.device, + ) + receive_above_eef: torch.Tensor | None = None + receive_final_eef: torch.Tensor | None = None + if options.release_at_target: + placed_object_pose = final_object_pose.clone() + placed_object_pose[:, :3, :3] = middle_object_pose[:, :3, :3] + above_object_pose = placed_object_pose.clone() + # Move to the target's horizontal coordinates while preserving the + # middle handover height exactly. The following target performs the + # only vertical motion and reaches the requested final object pose. + above_object_pose[:, 2, 3] = middle_object_pose[:, 2, 3] + lowering_direction_valid = ( + above_object_pose[:, 2, 3] - placed_object_pose[:, 2, 3] > 1.0e-6 + ) + receive_above_eef = torch.bmm(above_object_pose, receive_object_to_eef) + receive_final_eef = torch.bmm(placed_object_pose, receive_object_to_eef) + # Likewise, receiving-grasp through final lowering reuses the same EEF + # rotation and changes translation only. + receive_above_eef[:, :3, :3] = receive_grasp[:, :3, :3] + receive_final_eef[:, :3, :3] = receive_grasp[:, :3, :3] self._report_waypoint_failure( context, "pickup_grasp", @@ -709,12 +1293,13 @@ def _plan_direction( "no finite grasp candidate on the opposite object end for arm " f"{receive.arm.control_part!r}", ) - self._report_waypoint_failure( - context, - "target_final", - active_mask & ~lowering_direction_valid, - "final target is not below the horizontal-transfer height", - ) + if options.release_at_target: + self._report_waypoint_failure( + context, + "target_final", + active_mask & ~lowering_direction_valid, + "final target is not below the horizontal-transfer height", + ) success = ( handover_direction_valid @@ -813,33 +1398,39 @@ def _plan_direction( success &= receive_approach_success receive_grasp_qpos = receive_approach[:, -1] - placement_targets = torch.stack([receive_above_eef, receive_final_eef], dim=1) - phase_success, receive_place = plan_named_arm_trajectory( - self.motion_generator, - receive.arm.control_part, - receive_grasp_qpos, - placement_targets, - segment_lengths["place"], - request.motion_policy, - context.control_dt, - ) - placement_success = normalize_success_mask( - phase_success, - num_envs=self.num_envs, - device=self.device, - name="HandOver placement success", - ) - self._report_phase_failure( - context, - phase_name="place", - waypoint_names=("target_above", "target_final"), - target_poses=placement_targets, - start_qpos=receive_grasp_qpos, - arm=receive.arm, - failed_mask=active_mask & ~placement_success, - ) - success &= placement_success - receive_final_qpos = receive_place[:, -1] + receive_place: torch.Tensor | None = None + receive_final_qpos: torch.Tensor | None = None + if options.release_at_target: + assert receive_above_eef is not None and receive_final_eef is not None + placement_targets = torch.stack( + [receive_above_eef, receive_final_eef], dim=1 + ) + phase_success, receive_place = plan_named_arm_trajectory( + self.motion_generator, + receive.arm.control_part, + receive_grasp_qpos, + placement_targets, + segment_lengths["place"], + request.motion_policy, + context.control_dt, + ) + placement_success = normalize_success_mask( + phase_success, + num_envs=self.num_envs, + device=self.device, + name="HandOver placement success", + ) + self._report_phase_failure( + context, + phase_name="place", + waypoint_names=("target_above", "target_final"), + target_poses=placement_targets, + start_qpos=receive_grasp_qpos, + arm=receive.arm, + failed_mask=active_mask & ~placement_success, + ) + success &= placement_success + receive_final_qpos = receive_place[:, -1] segments = [ self._assemble_segment( @@ -923,31 +1514,44 @@ def _plan_direction( handover, receive, ), - self._assemble_segment( - state, - repeat_qpos(handover_middle_qpos, segment_lengths["place"]), - receive_place, - repeat_qpos(handover.hand_open_qpos, segment_lengths["place"]), - repeat_qpos(receive.hand_grasp_qpos, segment_lengths["place"]), - handover, - receive, - ), - self._assemble_segment( - state, - repeat_qpos(handover_middle_qpos, segment_lengths["receive_release"]), - repeat_qpos(receive_final_qpos, segment_lengths["receive_release"]), - repeat_qpos( - handover.hand_open_qpos, segment_lengths["receive_release"] - ), - interpolate_hand_qpos( - receive.hand_grasp_qpos, - receive.hand_open_qpos, - n_waypoints=segment_lengths["receive_release"], - ), - handover, - receive, - ), ] + if options.release_at_target: + assert receive_place is not None and receive_final_qpos is not None + segments.extend( + ( + self._assemble_segment( + state, + repeat_qpos(handover_middle_qpos, segment_lengths["place"]), + receive_place, + repeat_qpos(handover.hand_open_qpos, segment_lengths["place"]), + repeat_qpos(receive.hand_grasp_qpos, segment_lengths["place"]), + handover, + receive, + ), + self._assemble_segment( + state, + repeat_qpos( + handover_middle_qpos, + segment_lengths["receive_release"], + ), + repeat_qpos( + receive_final_qpos, + segment_lengths["receive_release"], + ), + repeat_qpos( + handover.hand_open_qpos, + segment_lengths["receive_release"], + ), + interpolate_hand_qpos( + receive.hand_grasp_qpos, + receive.hand_open_qpos, + n_waypoints=segment_lengths["receive_release"], + ), + handover, + receive, + ), + ) + ) trajectory = torch.cat(segments, dim=1) actual_lengths = { name: segment.shape[1] @@ -1092,10 +1696,11 @@ def _resolve_grasp( approach_direction: torch.Tensor, grasp_target_id: str, *, - obj_longest_axis: torch.Tensor, + obj_longest_axis: torch.Tensor | None, is_positive_part: torch.Tensor, + center_axis: torch.Tensor | None = None, ) -> tuple[torch.Tensor, torch.Tensor]: - """Select the lowest-cost grasp on one projected end of the object.""" + """Select the lowest-cost grasp in the requested projected region.""" if object_pose.shape != (self.num_envs, 4, 4): raise ValueError( "HandOver grasp object_pose must have shape " @@ -1106,10 +1711,17 @@ def _resolve_grasp( "HandOver grasp approach_direction must have shape " f"({self.num_envs}, 3)." ) - if obj_longest_axis.shape != (self.num_envs, 3): + if obj_longest_axis is not None and obj_longest_axis.shape != ( + self.num_envs, + 3, + ): raise ValueError( f"HandOver obj_longest_axis must have shape ({self.num_envs}, 3)." ) + if center_axis is not None and center_axis.shape != (self.num_envs, 3): + raise ValueError( + f"HandOver center_axis must have shape ({self.num_envs}, 3)." + ) if is_positive_part.dtype != torch.bool or is_positive_part.shape != ( self.num_envs, ): @@ -1140,6 +1752,14 @@ def _resolve_grasp( 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) + if center_axis is not None: + candidates, costs = self._center_grasp_candidates( + affordance, + object_pose[env_index], + center_axis[env_index], + candidates, + costs, + ) if candidates.shape[0] == 0 or not torch.isfinite(costs).any(): continue finite_costs = torch.where( @@ -1151,6 +1771,38 @@ def _resolve_grasp( success[env_index] = True return poses, success + def _center_grasp_candidates( + self, + affordance: AntipodalAffordance, + object_pose: torch.Tensor, + center_axis: torch.Tensor, + candidates: torch.Tensor, + costs: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + """Keep grasp centers in the middle third of the object's long axis.""" + vertices = affordance.mesh_vertices + if vertices is None: + raise ValueError("Center HandOver grasp selection requires mesh vertices.") + axis = center_axis.to(device=self.device, dtype=torch.float32) + axis_norm = torch.linalg.vector_norm(axis) + if not torch.isfinite(axis).all() or axis_norm <= 1.0e-8: + raise ValueError("HandOver center_axis must be finite and non-zero.") + axis = axis / axis_norm + vertices = vertices.to(device=self.device, dtype=torch.float32) + world_vertices = ( + torch.matmul(vertices, object_pose[:3, :3].transpose(0, 1)) + + object_pose[:3, 3] + ) + projections = torch.matmul(world_vertices, axis) + span = projections.max() - projections.min() + if not torch.isfinite(span) or span <= 1.0e-8: + raise ValueError("Center HandOver grasp selection requires finite extent.") + lower = projections.min() + span / 3.0 + upper = projections.max() - span / 3.0 + candidate_projections = torch.matmul(candidates[:, :3, 3], axis) + middle = (candidate_projections >= lower) & (candidate_projections <= upper) + return candidates[middle], costs[middle] + @staticmethod def _downward_diagonal_approach_direction( start_position: torch.Tensor, @@ -1202,27 +1854,35 @@ def _compute_segment_lengths( sample_count: int, options: HandOverOptions, ) -> dict[str, int]: - """Split the sample budget across four arm and four hand phases.""" + """Split the sample budget across enabled arm and hand phases.""" hand_count = options.hand_interp_steps - motion_budget = sample_count - 4 * hand_count - if motion_budget < 8: + hand_phase_count = 4 if options.release_at_target else 3 + motion_phase_count = 4 if options.release_at_target else 3 + motion_budget = sample_count - hand_phase_count * hand_count + if motion_budget < 2 * motion_phase_count: raise ValueError( "Not enough HandOver waypoints. Increase sample_count or decrease " "hand_interp_steps." ) - motion_counts = [motion_budget // 4] * 4 - for index in range(motion_budget % 4): + motion_counts = [motion_budget // motion_phase_count] * motion_phase_count + for index in range(motion_budget % motion_phase_count): motion_counts[index] += 1 - return { + result = { "pickup_approach": motion_counts[0], "pickup_close": hand_count, "pickup_transport": motion_counts[1], "receive_approach": motion_counts[2], "receive_close": hand_count, "handover_release": hand_count, - "place": motion_counts[3], - "receive_release": hand_count, } + if options.release_at_target: + result.update( + { + "place": motion_counts[3], + "receive_release": hand_count, + } + ) + return result @staticmethod def _assemble_segment( diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index fe2d0e4ea..ac9716cdb 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -82,7 +82,6 @@ class MoveJoints(AtomicAction[JointPositionGoal, MoveJointsOptions]): skill_id: ClassVar[str] = "move_joints" GoalType: ClassVar[type] = JointPositionGoal OptionsType: ClassVar[type] = MoveJointsOptions - agent_visible: ClassVar[bool] = False binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( make_motion_slot( diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index 646d35a7b..445235f50 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -50,7 +50,6 @@ ObjectActionGoal, PoseGoalValue, _resolve_object_pose, - collect_scene_dependencies, resolve_pose_goal, validate_pose_goal, ) @@ -151,6 +150,9 @@ class PickUpOptions(ActionOptions): pre_grasp_distance: float = 0.15 """Distance to offset back from the grasp pose along the approach direction.""" + grasp_commit_fraction: float = 1.0 + """Approach fraction after which contact motion no longer invalidates grasp.""" + approach_direction: torch.Tensor = torch.tensor([0, 0, -1], dtype=torch.float32) """World-frame direction from the pre-grasp pose to the grasp pose.""" @@ -187,6 +189,12 @@ def __post_init__(self) -> None: raise ValueError("lift_height must be non-negative.") if self.pre_grasp_distance < 0.0: raise ValueError("pre_grasp_distance must be non-negative.") + if isinstance(self.grasp_commit_fraction, bool) or not isinstance( + self.grasp_commit_fraction, (int, float) + ): + raise TypeError("grasp_commit_fraction must be a real number.") + if not 0.0 < self.grasp_commit_fraction <= 1.0: + raise ValueError("grasp_commit_fraction must be in (0, 1].") if self.approach_direction.shape != (3,): raise ValueError("approach_direction must have shape (3,).") if not torch.isfinite(self.approach_direction).all(): @@ -267,16 +275,17 @@ def _scene_dependencies( self, request: ResolvedActionRequest[GraspGoal, PickUpOptions], ) -> tuple[str, ...]: - """Include the semantic object when it has a stable scene identity.""" + """Monitor only the object that the current invocation must acquire. + + Downstream object targets guide grasp selection for static look-ahead, + but they are grounded again at the next Semantic Call boundary. A + moving downstream destination must therefore not invalidate an active + pickup after a feasible grasp has already been selected. + """ dependencies = set(super()._scene_dependencies(request)) entity_id = request.goal.semantics.entity_id if entity_id is not None: dependencies.add(entity_id) - dependencies.update( - collect_scene_dependencies( - request.skill_options.downstream_object_target_poses - ) - ) return tuple(sorted(dependencies)) def _get_full_pickup_trajectory( @@ -533,14 +542,20 @@ def _plan( coordinated_held_object_updates=coordinated_updates, ), segment_lengths=segment_lengths, - # Once the approach is dispatched the object can move because of - # contact or grasping. That self-induced motion must not look like - # an external dynamic-goal update. - scene_dependency_monitor_until=( - {} - if sem.entity_id is None - else {sem.entity_id: segment_lengths["approach"]} - ), + # Once the approach is dispatched, contact can move the object and + # the selected downstream suffix can be grounded again after this + # semantic boundary. Neither expected pickup motion nor a later + # destination revision should invalidate an already acquired + # grasp during close/lift. + scene_dependency_monitor_until={ + entity_id: max( + 1, + math.ceil( + segment_lengths["approach"] * options.grasp_commit_fraction + ), + ) + for entity_id in self._scene_dependencies(request) + }, ) def _resolve_grasp_pose( diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index c75557903..a8386c79f 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -360,6 +360,14 @@ def _plan( "release": n_open + n_settle, "retract": n_back, }, + # Contact and release can move a dynamic destination (for example, + # a tray). Monitor late-bound targets through approach, then let + # the semantic effect/post-policy boundary observe the resulting + # physical state instead of treating expected contact as a new + # goal revision. + scene_dependency_monitor_until={ + entity_id: n_down for entity_id in self._scene_dependencies(request) + }, ) def _resolve_place_xpos( diff --git a/embodichain/lab/sim/cfg.py b/embodichain/lab/sim/cfg.py index 05781b49f..d779bfdcb 100644 --- a/embodichain/lab/sim/cfg.py +++ b/embodichain/lab/sim/cfg.py @@ -285,14 +285,24 @@ def to_dexsim_flags(self) -> Renderer: def apply_to_dexsim_config(self, world_config: dexsim.WorldConfig) -> None: """Apply rendering settings to a DexSim world configuration. + Engines without ``DLSSConfig`` retain the standard rendering path and + emit a warning that DLSS settings cannot be applied. + Args: world_config: DexSim world configuration to update in place. """ world_config.renderer = self.to_dexsim_flags() - world_config.dlss_config = self.dlss.to_dexsim_cfg( - window_width=world_config.win_config.width, - window_height=world_config.win_config.height, - ) + if hasattr(dexsim, "DLSSConfig"): + world_config.dlss_config = self.dlss.to_dexsim_cfg( + window_width=world_config.win_config.width, + window_height=world_config.win_config.height, + ) + else: + self.dlss.__post_init__() + logger.log_warning( + "This DexSim build has no DLSSConfig API (for example, 0.4.3). " + "Ignoring DLSS settings and using standard rendering." + ) world_config.raytrace_config.render_iterations_per_frame = self.spp world_config.raytrace_config.open_denoise = True world_config.raytrace_config.denoiser_type = DenoiserType.OPTIX diff --git a/embodichain/lab/sim/motion/solvers/pytorch_solver.py b/embodichain/lab/sim/motion/solvers/pytorch_solver.py index d1d257351..b8a801965 100644 --- a/embodichain/lab/sim/motion/solvers/pytorch_solver.py +++ b/embodichain/lab/sim/motion/solvers/pytorch_solver.py @@ -14,6 +14,8 @@ # limitations under the License. # ---------------------------------------------------------------------------- +from __future__ import annotations + import torch from typing import Union, Tuple, List, TYPE_CHECKING @@ -32,6 +34,8 @@ lazy_import_pytorch_kinematics, ) +__all__ = ["PytorchSolverCfg", "PytorchSolver"] + @configclass class PytorchSolverCfg(SolverCfg): @@ -382,9 +386,10 @@ def get_ik( tcp_xpos = torch.as_tensor( self.tcp_xpos, device=self.device, dtype=torch.float32 ) - tcp_xpos_inv = tcp_xpos.clone() - tcp_xpos_inv[:3, :3] = tcp_xpos_inv[:3, :3].T - tcp_xpos_inv[:3, 3] = -tcp_xpos_inv[:3, :3] @ tcp_xpos_inv[:3, 3] + # Do not transpose the rotation into its own overlapping tensor view. + # That corrupts non-symmetric rotations (for example the 90-degree TCP + # used by parallel grippers) before IK sees the link-frame target. + tcp_xpos_inv = torch.linalg.inv(tcp_xpos) target_xpos = target_xpos @ tcp_xpos_inv # Get joint limits and ensure shape matches dof diff --git a/embodichain/lab/task_program/__init__.py b/embodichain/lab/task_program/__init__.py index 9d5131107..5996595ea 100644 --- a/embodichain/lab/task_program/__init__.py +++ b/embodichain/lab/task_program/__init__.py @@ -33,6 +33,7 @@ TaskProgramIntegrationCfg, HandOverCfg, InvokeCfg, + ObjectNearRelativeTargetValidatorCfg, ObjectNearTargetValidatorCfg, ParallelCfg, PickCfg, @@ -81,6 +82,7 @@ "TaskProgramValidationError", "HandOverCfg", "InvokeCfg", + "ObjectNearRelativeTargetValidatorCfg", "ObjectNearTargetValidatorCfg", "ParallelCfg", "PickCfg", diff --git a/embodichain/lab/task_program/compiler/__init__.py b/embodichain/lab/task_program/compiler/__init__.py index 300f47a3e..e7c5890c2 100644 --- a/embodichain/lab/task_program/compiler/__init__.py +++ b/embodichain/lab/task_program/compiler/__init__.py @@ -21,6 +21,7 @@ from .program import ( CompiledArticulationJointPositionValidator, CompiledBarrier, + CompiledObjectNearRelativeTargetValidator, CompiledObjectNearTargetValidator, CompiledParallelBlock, CompiledParallelBranch, @@ -39,6 +40,7 @@ __all__ = [ "CompiledArticulationJointPositionValidator", "CompiledBarrier", + "CompiledObjectNearRelativeTargetValidator", "CompiledObjectNearTargetValidator", "CompiledParallelBlock", "CompiledParallelBranch", diff --git a/embodichain/lab/task_program/compiler/lowering.py b/embodichain/lab/task_program/compiler/lowering.py index 2b855d0f7..1f9b6993f 100644 --- a/embodichain/lab/task_program/compiler/lowering.py +++ b/embodichain/lab/task_program/compiler/lowering.py @@ -209,12 +209,20 @@ def ground( affordance: Affordance, context: PlanningContext, ) -> SceneEntityPose: - """Return the current container-relative target frame.""" - del context + """Return a collision-safe release frame above the final target.""" if type(affordance) is not ContainerAffordance: raise TypeError("affordance must be exactly ContainerAffordance.") + relative_pose = None + if affordance.release_clearance > 0.0: + relative_pose = torch.eye( + 4, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + relative_pose[2, 3] = affordance.release_clearance return SceneEntityPose( relation.affordance.entity_id, + relative_pose=relative_pose, minimum_confidence=affordance.minimum_confidence, ) @@ -291,8 +299,10 @@ class AnalyzedSemanticCall: opaque_symbolic_effect: bool = False effect_monitor_ref: EffectMonitorRef | None = None downstream_object_targets: tuple[SemanticObjectTarget, ...] = () + handover_source_pick_object_part: str | None = None requires_verified_held_object: bool = False requires_fresh_observation: bool = True + handover_from_held: bool = False def __post_init__(self) -> None: if type(self.index) is not int or self.index < 0: @@ -333,10 +343,20 @@ def __post_init__(self) -> None: "SemanticObjectTarget values." ) object.__setattr__(self, "downstream_object_targets", targets) + if self.handover_source_pick_object_part is not None and ( + type(self.handover_source_pick_object_part) is not str + or not self.handover_source_pick_object_part + ): + raise TypeError( + "handover_source_pick_object_part must be a non-empty string " + "or None." + ) if type(self.requires_verified_held_object) is not bool: raise TypeError("requires_verified_held_object must be a bool.") if type(self.requires_fresh_observation) is not bool: raise TypeError("requires_fresh_observation must be a bool.") + if type(self.handover_from_held) is not bool: + raise TypeError("handover_from_held must be a bool.") @property def call(self) -> SemanticCallSpec: @@ -415,6 +435,75 @@ def __post_init__(self) -> None: object.__setattr__(self, "effect_dependencies", dependencies) +@dataclass(frozen=True, slots=True) +class RegisteredHeldObjectEffect: + """One registered-call held relation awaiting endpoint evidence binding.""" + + expectation_id: str + relation: HeldObjectRelation + object_id: str + slot_id: str + allow_missing_detached_baseline: bool = False + + def __post_init__(self) -> None: + for field_name in ("expectation_id", "object_id", "slot_id"): + _validate_identifier( + getattr(self, field_name), + field_name=f"RegisteredHeldObjectEffect.{field_name}", + ) + if not isinstance(self.relation, HeldObjectRelation): + raise TypeError("relation must be a HeldObjectRelation.") + if type(self.allow_missing_detached_baseline) is not bool: + raise TypeError("allow_missing_detached_baseline must be a bool.") + if ( + self.allow_missing_detached_baseline + and self.relation is not HeldObjectRelation.DETACHED + ): + raise ValueError( + "Only detached relations may allow a missing verified baseline." + ) + + +@dataclass(frozen=True, slots=True) +class RegisteredSemanticEffect: + """Declarative registered-call effect grounded by the canonical compiler.""" + + effect_kind: SemanticEffectKind + held_objects: tuple[RegisteredHeldObjectEffect, ...] + + def __post_init__(self) -> None: + if not isinstance(self.effect_kind, SemanticEffectKind): + raise TypeError("effect_kind must be a SemanticEffectKind.") + held_objects = tuple(self.held_objects) + if not held_objects or not all( + type(value) is RegisteredHeldObjectEffect for value in held_objects + ): + raise TypeError( + "held_objects must contain RegisteredHeldObjectEffect values." + ) + if len({value.expectation_id for value in held_objects}) != len(held_objects): + raise ValueError("Registered effect expectation IDs must be unique.") + if len({value.slot_id for value in held_objects}) != len(held_objects): + raise ValueError("Registered effect resource slots must be unique.") + relations = {value.relation for value in held_objects} + if self.effect_kind is SemanticEffectKind.ATTACH and relations != { + HeldObjectRelation.ATTACHED + }: + raise ValueError("An attach contract requires attached relations.") + if self.effect_kind is SemanticEffectKind.RELEASE and relations != { + HeldObjectRelation.DETACHED + }: + raise ValueError("A release contract requires detached relations.") + if self.effect_kind is SemanticEffectKind.TRANSFER and relations != { + HeldObjectRelation.ATTACHED, + HeldObjectRelation.DETACHED, + }: + raise ValueError( + "A transfer contract requires attached and detached relations." + ) + object.__setattr__(self, "held_objects", held_objects) + + @dataclass(frozen=True, slots=True) class SemanticLowering: """Registered-lowerer output wrapped by compiler-owned invocation policy.""" @@ -424,6 +513,7 @@ class SemanticLowering: control_overrides: ActionControlOverrides = field( default_factory=ActionControlOverrides ) + registered_effect: RegisteredSemanticEffect | None = None def __post_init__(self) -> None: if self.skill_options is not None and not isinstance( @@ -432,6 +522,13 @@ def __post_init__(self) -> None: raise TypeError("skill_options must be an ActionOptions or None.") if type(self.control_overrides) is not ActionControlOverrides: raise TypeError("control_overrides must be exactly ActionControlOverrides.") + if ( + self.registered_effect is not None + and type(self.registered_effect) is not RegisteredSemanticEffect + ): + raise TypeError( + "registered_effect must be a RegisteredSemanticEffect or None." + ) class RegisteredSemanticLowerer(ABC): @@ -439,6 +536,16 @@ class RegisteredSemanticLowerer(ABC): call_id: ClassVar[str] target_descriptor: ClassVar[SkillDescriptor] + effect_contract_kind: ClassVar[SemanticEffectKind | None] = None + preserves_symbolic_state: ClassVar[bool] = False + """Whether the call leaves compiler-owned symbolic ``TaskState`` unchanged. + + The default is deliberately opaque. A lowerer may opt in only when it has + no registered effect contract and its execution cannot attach, detach, or + transfer a held object. This declaration does not make the call + transparent to geometric pickup look-ahead; that remains governed by + :meth:`pick_lookahead_targets`. + """ def pick_lookahead_targets( self, @@ -451,11 +558,13 @@ def pick_lookahead_targets( """Declare retained-object poses used to screen an earlier pickup. ``None`` keeps the registered call opaque and stops pickup look-ahead. - An exact tuple certifies that the call retains ``picked_object`` on its - bound ``primary`` resource; each item is an object pose that the - selected grasp must make reachable. ``previous_target`` is the latest - pose declared earlier in the retained chain. An empty tuple therefore - means retained attachment without an additional pose target. + Each item in an exact tuple is an object pose that the selected grasp + must make reachable. Calls without a release/transfer effect certify + that they retain ``picked_object`` on their bound ``primary`` resource; + a release/transfer call contributes its terminal target and ends the + chain. ``previous_target`` is the latest pose declared earlier in the + retained chain. An empty tuple therefore means retained attachment + without an additional pose target. Args: call: Registered semantic call being analyzed. @@ -826,6 +935,18 @@ def __init__( f"Lowerer {call_id!r} target_descriptor must exactly match " "the registered catalog target." ) + preserves_symbolic_state = getattr( + type(lowerer), "preserves_symbolic_state", None + ) + if type(preserves_symbolic_state) is not bool: + raise TypeError( + f"Lowerer {call_id!r} preserves_symbolic_state must be a bool." + ) + if preserves_symbolic_state and lowerer.effect_contract_kind is not None: + raise ValueError( + f"Lowerer {call_id!r} cannot both preserve symbolic state and " + "declare an effect contract." + ) lowerers[call_id] = lowerer if isinstance(relation_grounders, (str, bytes)): raise TypeError("relation_grounders must be an iterable of grounders.") @@ -1022,6 +1143,7 @@ def analyze( for index, bound in enumerate(bound_calls): call = bound.linked.call requires_held = type(call) is Place + handover_from_held = False if type(call) is Pick: previous = latest_holder.get(call.object.entity_id) if previous is not None: @@ -1058,29 +1180,63 @@ def analyze( latest_holder.pop(call.object.entity_id, None) elif type(call) is HandOver: previous = latest_holder.get(call.object.entity_id) - if previous is not None: - raise _diagnostic( - "invalid_object_state_flow", - (*path, index, "call", "object"), - f"Object {call.object.entity_id!r} is already acquired by " - f"call {previous[0]}; the unified HandOver action starts " - "before pickup and requires both candidate arms to be " - "unoccupied.", + source_resource = bound.binding.resource_ids["source"] + destination_resource = bound.binding.resource_ids["destination"] + if previous is None: + # The unified primitive owns pickup, transfer, placement, + # and (by default) release when it starts with two free + # resources. + effect_kind = SemanticEffectKind.RELEASE + else: + if previous[1] != source_resource: + raise _diagnostic( + "held_resource_mismatch", + (*path, index, "call", "resources", "source"), + f"HandOver source resource {source_resource!r} does not " + f"match the verified holder {previous[1]!r} from call " + f"{previous[0]}.", + (previous[1],), + ) + if source_resource == destination_resource: + raise _diagnostic( + "resource_alias", + (*path, index, "call", "resources"), + "HandOver source and destination resources must differ.", + ) + dependencies.append( + SemanticEffectDependency( + producer_index=previous[0], + consumer_index=index, + object=call.object, + ) + ) + # A prior Pick has already established the source + # attachment. The canonical primitive now performs only + # the physical transfer and leaves the destination held. + effect_kind = SemanticEffectKind.TRANSFER + handover_from_held = True + requires_held = True + latest_holder[call.object.entity_id] = ( + index, + destination_resource, ) - # The current primitive owns pickup, transfer, placement, and - # release. Its externally visible held-object postcondition is - # therefore that both candidate arms are detached. - effect_kind = SemanticEffectKind.RELEASE else: effect_kind = SemanticEffectKind.REGISTERED - # A registered extension has no declarative state-flow contract - # and therefore forms an opaque effect boundary. - latest_holder.clear() + lowerer = self._registered_lowerers[call.call_id] + if not lowerer.preserves_symbolic_state: + # Registered extensions are fail-closed unless their installed + # lowerer explicitly declares an effectless symbolic contract. + latest_holder.clear() downstream_targets = ( self._downstream_targets(index, bound_calls) if type(call) is Pick else () ) + handover_source_pick_object_part = ( + self._handover_source_pick_object_part(index, bound_calls) + if type(call) is Pick + else None + ) effect_monitor_ref = self._effect_monitor_ref( bound, path=(*path, index, "effect_monitor"), @@ -1099,7 +1255,9 @@ def analyze( opaque_symbolic_effect=opaque_symbolic_effect, effect_monitor_ref=effect_monitor_ref, downstream_object_targets=downstream_targets, + handover_source_pick_object_part=(handover_source_pick_object_part), requires_verified_held_object=requires_held, + handover_from_held=handover_from_held, ) ) return SemanticWorkflow._create( @@ -1120,10 +1278,10 @@ def _static_symbolic_writes( """Return exact provider-free ``TaskState`` keys for one linked call. Curated calls own these contracts. Registered calls remain an opaque - physical-effect boundary until their public descriptor grows an - explicit static-effect contract; lowering arguments are never guessed. - Conditional coordinated-held cleanup is likewise omitted because its - exact pair keys depend on the verified input ``TaskState``. + physical-effect boundary unless their installed lowerer explicitly + declares that it preserves symbolic state; lowering arguments are never + guessed. Conditional coordinated-held cleanup is likewise omitted + because its exact pair keys depend on verified input ``TaskState``. """ call = bound.linked.call if type(call) in (Pick, Place): @@ -1156,7 +1314,8 @@ def _static_symbolic_writes( False, ) if type(call) is RegisteredSemanticCall: - return frozenset(), True + lowerer = self._registered_lowerers[call.call_id] + return frozenset(), not lowerer.preserves_symbolic_state raise AssertionError(f"Unsupported linked call {type(call).__name__}.") @staticmethod @@ -1233,7 +1392,12 @@ def ground( self._assert_workflow_current(workflow, path=path) self._validate_context(context) eligible = self._normalize_eligible_mask(eligible_mask, context) - analyzed = workflow.calls[call_index] + analyzed = self._resolve_live_handover_mode( + workflow.calls[call_index], + context, + eligible, + path=(*path, call_index, "call"), + ) call = analyzed.call if type(call) is Pick: lowering = self._lower_pick(analyzed, context) @@ -1243,6 +1407,13 @@ def ground( lowering = self._lower_handover(analyzed, context, eligible, path=path) elif type(call) is RegisteredSemanticCall: lowering = self._lower_registered(analyzed, context, path=path) + self._validate_registered_held_effects( + analyzed, + lowering, + context, + eligible, + path=(*path, call_index, "call"), + ) else: # pragma: no cover - exact workflow construction prevents this raise AssertionError(f"Unsupported analyzed call {type(call).__name__}.") @@ -1267,6 +1438,7 @@ def ground( analyzed, invocation, context, + lowering=lowering, path=(*path, call_index, "effect"), ) effect_monitor: EffectMonitor | None = None @@ -1308,6 +1480,56 @@ def ground( eligible_mask=eligible, ) + def _resolve_live_handover_mode( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + eligible: torch.Tensor, + *, + path: tuple[PathPart, ...], + ) -> AnalyzedSemanticCall: + """Adopt a verified source hold when a call is analyzed in isolation. + + Task Program segment boundaries may analyze ``Pick`` and ``HandOver`` + as separate workflows even though verified ``TaskState`` flows between + them. Static analysis therefore cannot always infer transfer-only mode. + JIT grounding reconciles that mode from the fresh, verified state while + requiring every eligible row to agree. + """ + if type(analyzed.call) is not HandOver or analyzed.handover_from_held: + return analyzed + task_state_key = self._participant_task_state_key( + analyzed.bound, + slot_id="source", + path=(*path, "resources", "source"), + ) + held = context.task.get_held_object(task_state_key) + if held is None or held.semantics.entity_id != analyzed.call.object.entity_id: + return analyzed + assert held.env_mask is not None + held_eligible = eligible & held.env_mask + if not held_eligible.any(): + return analyzed + missing = eligible & ~held.env_mask + if missing.any(): + missing_env_ids = tuple( + str(value) + for value in context.env_ids[missing].detach().to("cpu").tolist() + ) + raise _diagnostic( + "inconsistent_handover_source_state", + path, + "HandOver cannot mix held and unheld source state across " + "eligible environments.", + missing_env_ids, + ) + return replace( + analyzed, + effect_kind=SemanticEffectKind.TRANSFER, + requires_verified_held_object=True, + handover_from_held=True, + ) + def _assert_current(self, *, path: tuple[PathPart, ...]) -> None: """Reject a compiler after the engine skill catalog changes.""" try: @@ -1410,15 +1632,25 @@ def _effect_monitor_ref( f"Verified preset {bound.preset.preset_id!r} requires an " f"effect monitor for curated call {semantic_id!r}.", ) + if type(bound.linked.call) is RegisteredSemanticCall: + lowerer = self._registered_lowerers.get(bound.linked.call.call_id) + if lowerer is not None and lowerer.effect_contract_kind is not None: + raise _diagnostic( + "missing_effect_monitor", + path, + f"Verified preset {bound.preset.preset_id!r} requires an " + f"effect monitor for registered call {semantic_id!r}.", + ) return None if type(bound.linked.call) is RegisteredSemanticCall: - raise _diagnostic( - "registered_effect_contract_not_installed", - path, - f"Registered semantic call {semantic_id!r} selects an effect " - "monitor but no declarative effect-contract grounder is " - "installed.", - ) + lowerer = self._registered_lowerers.get(bound.linked.call.call_id) + if lowerer is None or lowerer.effect_contract_kind is None: + raise _diagnostic( + "registered_effect_contract_not_installed", + path, + f"Registered semantic call {semantic_id!r} selects an effect " + "monitor but no declarative effect contract is installed.", + ) try: self._effect_monitor_registry.validate_ref(monitor_ref) except KeyError as exc: @@ -1479,6 +1711,11 @@ def _downstream_targets( "None." ) targets.extend(registered_targets) + if lowerer.effect_contract_kind in { + SemanticEffectKind.RELEASE, + SemanticEffectKind.TRANSFER, + }: + break continue call_object = getattr(call, "object", None) if type(call_object) is not SceneObjectRef or ( @@ -1510,6 +1747,36 @@ def _downstream_targets( break return tuple(targets) + def _handover_source_pick_object_part( + self, + pick_index: int, + bound_calls: list[BoundSemanticCall], + ) -> str | None: + """Choose a source grasp that leaves the next handover side exposed.""" + pick = bound_calls[pick_index].linked.call + assert type(pick) is Pick + object_id = pick.object.entity_id + for bound in bound_calls[pick_index + 1 :]: + call = bound.linked.call + call_object = getattr(call, "object", None) + if type(call_object) is not SceneObjectRef or ( + call_object.entity_id != object_id + ): + continue + if type(call) is not HandOver: + return None + handover_options = bound.preset.action_option_template(call.semantic_id) + if type(handover_options) is not HandOverOptions: + raise AssertionError( + "Linked handover call has a non-HandOverOptions template." + ) + return { + "bottom": "top", + "top": "bottom", + "center": "center", + }[handover_options.receive_pick_object_part] + return None + def _lower_pick( self, analyzed: AnalyzedSemanticCall, @@ -1540,6 +1807,10 @@ def _lower_pick( goal=GraspGoal(semantics=semantics), skill_options=replace( option_template, + pick_object_part=( + analyzed.handover_source_pick_object_part + or option_template.pick_object_part + ), downstream_object_target_poses=tuple(downstream_targets), ), ) @@ -1602,7 +1873,14 @@ def _lower_handover( """Lower the unified pickup-to-release handover atomic action.""" call = analyzed.call assert type(call) is HandOver - del eligible + if analyzed.handover_from_held: + self._require_held_object( + analyzed, + context, + eligible, + slot_id="source", + path=(*path, analyzed.index, "call", "object"), + ) grasp_ref = analyzed.bound.linked.affordances.get("receiver_grasp") if grasp_ref is None: raise AssertionError("Linked handover lacks receiver grasp affordance.") @@ -1627,9 +1905,12 @@ def _lower_handover( # bound arm roots, so only the provider's final pose is consumed. final_target = targets.final final = self._ground_object_target(final_target, context) + option_template = self._action_option_template(analyzed, HandOverOptions) + if analyzed.handover_from_held: + option_template = replace(option_template, release_at_target=False) return SemanticLowering( goal=HandOverGoal(semantics=semantics, target_pose=final), - skill_options=self._action_option_template(analyzed, HandOverOptions), + skill_options=option_template, ) def _lower_registered( @@ -1683,6 +1964,21 @@ def _lower_registered( f"Lowerer {call.call_id!r} must not return skill_options; " "the selected policy preset owns action options." ) + expected_effect_kind = lowerer.effect_contract_kind + actual_effect = lowering.registered_effect + if (expected_effect_kind is None) != (actual_effect is None): + raise TypeError( + f"Lowerer {call.call_id!r} effect contract does not match its " + "declared effect_contract_kind." + ) + if ( + expected_effect_kind is not None + and actual_effect is not None + and actual_effect.effect_kind is not expected_effect_kind + ): + raise TypeError( + f"Lowerer {call.call_id!r} produced an incompatible effect kind." + ) return replace(lowering, skill_options=deepcopy(option_template)) @staticmethod @@ -1711,6 +2007,7 @@ def _ground_effect_spec( invocation: ActionInvocation, context: PlanningContext, *, + lowering: SemanticLowering, path: tuple[PathPart, ...], ) -> SemanticEffectSpec | None: """Ground typed symbolic state and raw-evidence clauses.""" @@ -1719,6 +2016,7 @@ def _ground_effect_spec( call = analyzed.call state_expectations: list[EffectStateExpectation] = [] clauses: list[EffectClause] = [] + effect_kind = analyzed.effect_kind if type(call) is Pick: expectation, grounded_clauses = self._ground_held_effect( analyzed, @@ -1756,10 +2054,16 @@ def _ground_effect_spec( ) ) elif type(call) is HandOver: + source_relation = HeldObjectRelation.DETACHED + destination_relation = ( + HeldObjectRelation.ATTACHED + if analyzed.handover_from_held + else HeldObjectRelation.DETACHED + ) source, source_clauses = self._ground_held_effect( analyzed, expectation_id="source", - relation=HeldObjectRelation.DETACHED, + relation=source_relation, slot_id="source", object_id=call.object.entity_id, context=context, @@ -1769,7 +2073,7 @@ def _ground_effect_spec( destination, destination_clauses = self._ground_held_effect( analyzed, expectation_id="destination", - relation=HeldObjectRelation.DETACHED, + relation=destination_relation, slot_id="destination", object_id=call.object.entity_id, context=context, @@ -1778,11 +2082,36 @@ def _ground_effect_spec( ) state_expectations.extend((source, destination)) clauses.extend((*source_clauses, *destination_clauses)) + elif type(call) is RegisteredSemanticCall: + contract = lowering.registered_effect + if contract is None: + raise _diagnostic( + "registered_effect_contract_not_grounded", + path, + f"Registered semantic call {call.semantic_id!r} selected a " + "monitor but its lowerer produced no effect contract.", + ) + effect_kind = contract.effect_kind + for item in contract.held_objects: + expectation, grounded_clauses = self._ground_held_effect( + analyzed, + expectation_id=item.expectation_id, + relation=item.relation, + slot_id=item.slot_id, + object_id=item.object_id, + context=context, + path=(*path, "state_expectations", item.expectation_id), + allow_missing_detached_baseline=( + item.allow_missing_detached_baseline + ), + ) + state_expectations.append(expectation) + clauses.extend(grounded_clauses) else: # pragma: no cover - exact workflow construction prevents this raise AssertionError(f"Unsupported analyzed call {type(call).__name__}.") return SemanticEffectSpec( semantic_id=call.semantic_id, - effect_kind=analyzed.effect_kind, + effect_kind=effect_kind, skill_id=invocation.skill_id, invocation_id=invocation.invocation_id, invocation_revision=invocation.revision, @@ -1791,6 +2120,34 @@ def _ground_effect_spec( clauses=tuple(clauses), ) + def _validate_registered_held_effects( + self, + analyzed: AnalyzedSemanticCall, + lowering: SemanticLowering, + context: PlanningContext, + eligible: torch.Tensor, + *, + path: tuple[PathPart, ...], + ) -> None: + """Require verified input holds declared by a registered release.""" + contract = lowering.registered_effect + if contract is None: + return + for item in contract.held_objects: + if ( + item.relation is not HeldObjectRelation.DETACHED + or item.allow_missing_detached_baseline + ): + continue + self._require_held_object( + analyzed, + context, + eligible, + slot_id=item.slot_id, + object_id=item.object_id, + path=(*path, "registered_effect", item.expectation_id), + ) + def _ground_phase_effect_gates( self, analyzed: AnalyzedSemanticCall, @@ -1822,26 +2179,30 @@ def _ground_phase_effect_gates( ), ) elif type(call) is HandOver: - definitions = ( - ( - "source_acquired", - "pickup_transport", - "source", - HeldObjectRelation.ATTACHED, - ), - ( - "destination_acquired", - "handover_release", - "destination", - HeldObjectRelation.ATTACHED, - ), - ( - "source_released", - "place", - "source", - HeldObjectRelation.DETACHED, - ), - ) + if analyzed.handover_from_held: + definitions = ( + ( + "destination_acquired", + "handover_release", + "destination", + HeldObjectRelation.ATTACHED, + ), + ) + else: + definitions = ( + ( + "source_acquired", + "pickup_transport", + "source", + HeldObjectRelation.ATTACHED, + ), + ( + "destination_acquired", + "handover_release", + "destination", + HeldObjectRelation.ATTACHED, + ), + ) else: return () @@ -1902,6 +2263,19 @@ def _single_held_expectation_effect_spec( for clause in terminal_spec.clauses if clause.expectation_id == expectation_id ) + if relation is HeldObjectRelation.DETACHED: + # A release gate runs *before* the retract segment that creates + # geometric separation. Requiring the terminal pose-relation + # clause here deadlocks the action at the boundary: the gripper is + # open, but the endpoint cannot move away until this gate passes. + # Keep contact/constraint/force evidence at the phase boundary; + # the complete terminal monitor still verifies pose separation + # after retract before TaskState is committed. + clauses = tuple( + clause + for clause in clauses + if not isinstance(clause, PoseRelationClause) + ) if not clauses: raise ValueError( f"Held-object expectation {expectation_id!r} has no physical clauses." @@ -1989,24 +2363,49 @@ def _ground_held_object_guards( elif type(call) is HandOver: source = self._held_expectation(effect_spec, "source") destination = self._held_expectation(effect_spec, "destination") - definitions = ( - ( - "source_attached", - source.expectation_id, - ("pickup_transport", "receive_approach", "receive_close"), - HeldObjectGuardBaseline.PLANNED_EFFECT, - (source.task_state_key,), - True, - ), - ( - "destination_attached", - destination.expectation_id, - ("handover_release", "place"), - HeldObjectGuardBaseline.PLANNED_EFFECT, - (source.task_state_key, destination.task_state_key), - True, - ), - ) + if analyzed.handover_from_held: + definitions = ( + ( + "source_attached", + source.expectation_id, + ( + "transfer", + "receive_approach", + "receive_close", + "receive_hold", + ), + HeldObjectGuardBaseline.VERIFIED_TASK_STATE, + (source.task_state_key,), + True, + ), + ( + "destination_attached", + destination.expectation_id, + ("handover_release", "source_retreat"), + HeldObjectGuardBaseline.PLANNED_EFFECT, + (source.task_state_key, destination.task_state_key), + False, + ), + ) + else: + definitions = ( + ( + "source_attached", + source.expectation_id, + ("pickup_transport", "receive_approach", "receive_close"), + HeldObjectGuardBaseline.PLANNED_EFFECT, + (source.task_state_key,), + True, + ), + ( + "destination_attached", + destination.expectation_id, + ("handover_release", "place"), + HeldObjectGuardBaseline.PLANNED_EFFECT, + (source.task_state_key, destination.task_state_key), + True, + ), + ) else: return () @@ -2429,6 +2828,8 @@ def _compose_object_to_eef( object_target.entity_id, relative_pose=composed, minimum_confidence=object_target.minimum_confidence, + world_displacement=object_target.world_displacement, + world_orientation=object_target.world_orientation, ) def _target_with_observed_object_orientation( @@ -2469,6 +2870,13 @@ def _target_with_observed_object_orientation( target_pose[:, :3, :3] = observed_object_pose[:, :3, :3] if isinstance(object_target, torch.Tensor): return target_pose + if object_target.relative_pose is None: + return SceneEntityPose( + object_target.entity_id, + minimum_confidence=object_target.minimum_confidence, + world_displacement=object_target.world_displacement, + world_orientation=observed_object_pose[:, :3, :3], + ) parent_pose = resolve_pose_goal( SceneEntityPose( @@ -2492,6 +2900,7 @@ def _require_held_object( eligible: torch.Tensor, *, slot_id: str, + object_id: str | None = None, path: tuple[PathPart, ...], ) -> tuple[str, HeldObjectState]: """Resolve the logical participant key and verify held-object identity.""" @@ -2507,13 +2916,15 @@ def _require_held_object( task_state_key = endpoint.task_state_key assert isinstance(task_state_key, str) held = context.task.get_held_object(task_state_key) - call_object = getattr(analyzed.call, "object", None) - assert type(call_object) is SceneObjectRef - if held is None or held.semantics.entity_id != call_object.entity_id: + if object_id is None: + call_object = getattr(analyzed.call, "object", None) + assert type(call_object) is SceneObjectRef + object_id = call_object.entity_id + if held is None or held.semantics.entity_id != object_id: raise _diagnostic( "verified_held_object_required", path, - f"Call requires verified object {call_object.entity_id!r} held by " + f"Call requires verified object {object_id!r} held by " f"logical state key {task_state_key!r}.", ) assert held.env_mask is not None @@ -2526,7 +2937,7 @@ def _require_held_object( raise _diagnostic( "verified_held_object_required", path, - f"Object {call_object.entity_id!r} is not verified as held in " + f"Object {object_id!r} is not verified as held in " "every eligible environment.", missing_env_ids, ) diff --git a/embodichain/lab/task_program/compiler/program.py b/embodichain/lab/task_program/compiler/program.py index 57607bf02..fcdba3f8f 100644 --- a/embodichain/lab/task_program/compiler/program.py +++ b/embodichain/lab/task_program/compiler/program.py @@ -52,6 +52,7 @@ TaskProgramIntegrationCfg, HandOverCfg, InvokeCfg, + ObjectNearRelativeTargetValidatorCfg, ObjectNearTargetValidatorCfg, ParallelCfg, PickCfg, @@ -269,6 +270,39 @@ def __post_init__(self) -> None: object.__setattr__(self, "target_pose", self.target_pose.snapshot()) +@dataclass(frozen=True, slots=True) +class CompiledObjectNearRelativeTargetValidator: + """Owned validator with canonical object and live reference identities.""" + + cfg: ObjectNearRelativeTargetValidatorCfg + object: SceneObjectRef + reference: SceneObjectRef + source_path: ConfigPath + + def __post_init__(self) -> None: + if type(self.cfg) is not ObjectNearRelativeTargetValidatorCfg: + raise TypeError("cfg must be exactly ObjectNearRelativeTargetValidatorCfg.") + if type(self.object) is not SceneObjectRef: + raise TypeError("object must be exactly SceneObjectRef.") + if type(self.reference) is not SceneObjectRef: + raise TypeError("reference must be exactly SceneObjectRef.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + object.__setattr__( + self, + "cfg", + ObjectNearRelativeTargetValidatorCfg( + object=self.cfg.object, + reference=self.cfg.reference, + displacement=self.cfg.displacement, + position_tolerance=self.cfg.position_tolerance, + kind=self.cfg.kind, + ), + ) + object.__setattr__(self, "object", _copy_scene_ref(self.object)) + object.__setattr__(self, "reference", _copy_scene_ref(self.reference)) + + @dataclass(frozen=True, slots=True) class CompiledArticulationJointPositionValidator: """Owned joint-position validator with its canonical articulation.""" @@ -305,10 +339,13 @@ def __post_init__(self) -> None: CompiledTaskProgramValidator: TypeAlias = ( - CompiledObjectNearTargetValidator | CompiledArticulationJointPositionValidator + CompiledObjectNearTargetValidator + | CompiledObjectNearRelativeTargetValidator + | CompiledArticulationJointPositionValidator ) _COMPILED_VALIDATOR_TYPES = ( CompiledObjectNearTargetValidator, + CompiledObjectNearRelativeTargetValidator, CompiledArticulationJointPositionValidator, ) @@ -556,6 +593,14 @@ class _ObjectNearTargetValidatorTemplate: source_path: ConfigPath +@dataclass(frozen=True, slots=True) +class _ObjectNearRelativeTargetValidatorTemplate: + cfg: ObjectNearRelativeTargetValidatorCfg + object: SceneObjectRef + reference: SceneObjectRef + source_path: ConfigPath + + @dataclass(frozen=True, slots=True) class _ArticulationJointPositionValidatorTemplate: cfg: ArticulationJointPositionValidatorCfg @@ -564,7 +609,9 @@ class _ArticulationJointPositionValidatorTemplate: _ValidatorTemplate: TypeAlias = ( - _ObjectNearTargetValidatorTemplate | _ArticulationJointPositionValidatorTemplate + _ObjectNearTargetValidatorTemplate + | _ObjectNearRelativeTargetValidatorTemplate + | _ArticulationJointPositionValidatorTemplate ) @@ -925,6 +972,15 @@ def _iter_segments( source_path=validator.source_path, ) ) + elif type(validator) is _ObjectNearRelativeTargetValidatorTemplate: + validators.append( + CompiledObjectNearRelativeTargetValidator( + cfg=validator.cfg, + object=validator.object, + reference=validator.reference, + source_path=validator.source_path, + ) + ) elif type(validator) is _ArticulationJointPositionValidatorTemplate: validators.append( CompiledArticulationJointPositionValidator( @@ -1530,6 +1586,39 @@ def _compile_node( ) ) continue + if type(cfg) is ObjectNearRelativeTargetValidatorCfg: + if cfg.kind != "object_near_relative_target": + raise TaskProgramCompileError( + "unsupported_validator", + validator_path, + "ObjectNearRelativeTargetValidatorCfg must use kind " + "'object_near_relative_target'.", + ) + object_ref = self._resolve_scene( + cfg.object, + expected_types=(SceneObjectRef,), + path=(*validator_path, "object"), + ) + reference_ref = self._resolve_scene( + cfg.reference, + expected_types=(SceneObjectRef,), + path=(*validator_path, "reference"), + ) + validators.append( + _ObjectNearRelativeTargetValidatorTemplate( + cfg=ObjectNearRelativeTargetValidatorCfg( + object=cfg.object, + reference=cfg.reference, + displacement=cfg.displacement, + position_tolerance=cfg.position_tolerance, + kind=cfg.kind, + ), + object=object_ref, + reference=reference_ref, + source_path=validator_path, + ) + ) + continue if type(cfg) is ArticulationJointPositionValidatorCfg: if cfg.kind != "articulation_joint_position": raise TaskProgramCompileError( diff --git a/embodichain/lab/task_program/integrations/_configured_services.py b/embodichain/lab/task_program/integrations/_configured_services.py index 67d00a950..314eb26fa 100644 --- a/embodichain/lab/task_program/integrations/_configured_services.py +++ b/embodichain/lab/task_program/integrations/_configured_services.py @@ -34,11 +34,24 @@ ActionOptions, Affordance, AtomicActionEngine, + AxisAlign, AxisAlignAffordance, + AxisAlignGoal, + AxisAlignOptions, + CoordinatedPickGoal, + CoordinatedPickment, + CoordinatedPickmentOptions, HeldObjectPoseGoal, + JointPositionGoal, MoveHeldObject, MoveHeldObjectOptions, + MoveJoints, + MoveJointsOptions, ObjectSemantics, + PARK_COMMAND, + Place, + PlaceGoal, + PlaceOptions, PlanningContext, Pour, PourGoal, @@ -68,15 +81,19 @@ EffectEvidenceCollectionContext, EffectEvidenceProvider, GRASP_AFFORDANCE_CAPABILITY, + HeldObjectRelation, HeldObjectStateExpectation, RegisteredSemanticCall, SceneArticulationRef, SceneLinkRef, SceneObjectRef, SceneRegistry, + SemanticEffectKind, ) from embodichain.lab.task_program.compiler.lowering import ( + RegisteredHeldObjectEffect, RegisteredSemanticLowerer, + RegisteredSemanticEffect, SemanticLowering, SemanticObjectTarget, ) @@ -87,7 +104,11 @@ __all__: list[str] = [] _ARTICULATION_LINK_SLIDE_CALL_ID = "simulation.articulation_link_slide" +_AXIS_ALIGN_CALL_ID = "simulation.axis_align" +_COORDINATED_TRANSPORT_CALL_ID = "simulation.coordinated_transport" _MOVE_HELD_OBJECT_CALL_ID = "simulation.move_held_object" +_PARK_CALL_ID = "simulation.park" +_PLACE_RELATIVE_CALL_ID = "simulation.place_relative" _POUR_CALL_ID = "simulation.pour" _PUSH_OBJECT_CALL_ID = "simulation.push_object" _SLIDE_TARGET_POSE_MODES = frozenset({"live", "snapshot"}) @@ -146,6 +167,20 @@ def _pose(value: tuple[float, ...]) -> tuple[float, ...]: return normalized +def _world_displacement( + value: tuple[float, float, float], +) -> tuple[float, float, float]: + """Validate one finite non-zero world-frame displacement.""" + if type(value) is not tuple or len(value) != 3: + raise TypeError("world_displacement must be an exact three-value tuple.") + normalized = tuple(float(item) for item in value) + if not all(math.isfinite(item) for item in normalized): + raise ValueError("world_displacement must contain only finite values.") + if math.sqrt(sum(item * item for item in normalized)) <= 1.0e-6: + raise ValueError("world_displacement must be non-zero.") + return normalized + + def _slide_target_pose_mode(value: object) -> str: """Validate how a configured Slide resolves its target pose.""" if type(value) is not str or value not in _SLIDE_TARGET_POSE_MODES: @@ -155,6 +190,171 @@ def _slide_target_pose_mode(value: object) -> str: return value +class _ParkLowerer(RegisteredSemanticLowerer): + """Lower one resource-scoped semantic park request.""" + + call_id: ClassVar[str] = _PARK_CALL_ID + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + preserves_symbolic_state: ClassVar[bool] = True + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: ActionOptions, + ) -> SemanticLowering: + """Construct a named joint goal whose posture stays in the profile.""" + del context, bound + if type(option_template) is not MoveJointsOptions: + raise TypeError( + "Configured semantic park requires an exact " + "MoveJointsOptions template." + ) + if dict(call.arguments): + raise ValueError( + f"{self.call_id} arguments must be empty; the embodiment profile " + "owns the parked posture." + ) + return SemanticLowering(goal=JointPositionGoal(PARK_COMMAND)) + + +class _AxisAlignLowerer(RegisteredSemanticLowerer): + """Lower one configured object-upright request to ``AxisAlign``.""" + + call_id: ClassVar[str] = _AXIS_ALIGN_CALL_ID + target_descriptor: ClassVar[SkillDescriptor] = AxisAlign.descriptor() + effect_contract_kind: ClassVar[SemanticEffectKind] = SemanticEffectKind.ATTACH + + def __init__(self, semantics: tuple[ObjectSemantics, ...]) -> None: + if type(semantics) is not tuple or not semantics: + raise ValueError("AxisAlign semantics must be a non-empty exact tuple.") + self._semantics = { + value.entity_id: value + for value in semantics + if isinstance(value.entity_id, str) + } + if len(self._semantics) != len(semantics): + raise ValueError("AxisAlign semantics require unique scene entity IDs.") + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: ActionOptions, + ) -> SemanticLowering: + """Construct one typed axis-alignment goal and attach effect.""" + del context, bound + if type(option_template) is not AxisAlignOptions: + raise TypeError( + "Configured semantic axis alignment requires an exact " + "AxisAlignOptions template." + ) + arguments = dict(call.arguments) + if set(arguments) != {"object"}: + raise ValueError(f"{self.call_id} arguments must contain only 'object'.") + object_id = arguments["object"] + semantics = self._semantics.get(object_id) + if semantics is None: + raise ValueError(f"{self.call_id} does not declare object {object_id!r}.") + return SemanticLowering( + goal=AxisAlignGoal(semantics=semantics), + registered_effect=RegisteredSemanticEffect( + effect_kind=SemanticEffectKind.ATTACH, + held_objects=( + RegisteredHeldObjectEffect( + expectation_id="primary", + relation=HeldObjectRelation.ATTACHED, + object_id=object_id, + slot_id="primary", + ), + ), + ), + ) + + +@dataclass(frozen=True, slots=True) +class _AxisAlignLowererFactory(RegisteredSemanticLowererFactory): + """Create an axis-alignment lowerer from configured scene objects.""" + + call_id: ClassVar[str] = _AXIS_ALIGN_CALL_ID + revision: ClassVar[str] = "1" + target_descriptor: ClassVar[SkillDescriptor] = AxisAlign.descriptor() + + object_ids: tuple[str, ...] + + def __post_init__(self) -> None: + if type(self.object_ids) is not tuple or not self.object_ids: + raise ValueError("AxisAlign object_ids must be a non-empty exact tuple.") + normalized = tuple( + _identifier(value, field_name=f"object_ids[{index}]") + for index, value in enumerate(self.object_ids) + ) + if len(set(normalized)) != len(normalized): + raise ValueError("AxisAlign object_ids must be unique.") + object.__setattr__(self, "object_ids", normalized) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> RegisteredSemanticLowerer: + """Resolve exact axis-aware grasp semantics for every allowed object.""" + del simulation + if engine.robot is not robot: + raise ValueError("AxisAlign lowerer requires the engine's exact robot.") + semantics: list[ObjectSemantics] = [] + for object_id in self.object_ids: + object_ref = scene_registry.resolve( + object_id, + expected_type=SceneObjectRef, + ) + grasp_ref = scene_registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + object_semantics = scene_registry.object_semantics( + object_ref, + affordance=grasp_ref, + ) + if type(object_semantics.affordance) is not AxisAlignAffordance: + raise TypeError( + "Configured AxisAlign requires an AxisAlignAffordance grasp " + f"payload for {object_ref.entity_id!r}." + ) + semantics.append(object_semantics) + return _AxisAlignLowerer(tuple(semantics)) + + +@dataclass(frozen=True, slots=True) +class _ParkLowererFactory(RegisteredSemanticLowererFactory): + """Create the stateless profile-bound Park lowerer.""" + + call_id: ClassVar[str] = _PARK_CALL_ID + revision: ClassVar[str] = "1" + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> RegisteredSemanticLowerer: + """Validate runtime ownership and return one fresh Park lowerer.""" + del simulation, scene_registry + if engine.robot is not robot: + raise ValueError("Park lowerer requires the engine's exact robot.") + return _ParkLowerer() + + @dataclass(frozen=True, slots=True) class _AntipodalGraspPoseGeneratorFactory: """Own executable-free values and lazily create one fresh grasp service.""" @@ -167,7 +367,9 @@ class _AntipodalGraspPoseGeneratorFactory: finger_thickness: float palm_depth: float sample_count: int | None = None + approach_deviation_angle: float | None = None approach_direction_samples: int | None = None + max_candidates: int | None = None opening_margin: float | None = None point_sample_density: float | None = None filter_ground_collision: bool | None = None @@ -186,10 +388,14 @@ def __call__(self) -> GraspPoseGenerator: algorithm_kwargs: dict[str, object] = {} if self.sample_count is not None: algorithm_kwargs["sample_count"] = self.sample_count + if self.approach_deviation_angle is not None: + algorithm_kwargs["approach_deviation_angle"] = self.approach_deviation_angle if self.approach_direction_samples is not None: algorithm_kwargs["approach_direction_samples"] = ( self.approach_direction_samples ) + if self.max_candidates is not None: + algorithm_kwargs["max_candidates"] = self.max_candidates collision_kwargs: dict[str, object] = {} if self.opening_margin is not None: @@ -512,6 +718,547 @@ def create( ) +@dataclass(frozen=True, slots=True) +class _RelativePlaceRoute: + """One configured object relation expressed in the world frame.""" + + object_id: str + reference_entity_id: str + relation: str + world_displacement: tuple[float, float, float] + + def __post_init__(self) -> None: + for field_name in ("object_id", "reference_entity_id", "relation"): + object.__setattr__( + self, + field_name, + _identifier(getattr(self, field_name), field_name=field_name), + ) + if self.relation not in { + "above", + "behind", + "front_of", + "left_of", + "on", + "right_of", + }: + raise ValueError( + "Relative Place relation must be one of above, behind, front_of, " + "left_of, on, or right_of." + ) + object.__setattr__( + self, + "world_displacement", + _world_displacement(self.world_displacement), + ) + + @property + def selector(self) -> tuple[str, str, str]: + """Return the semantic arguments selecting this immutable route.""" + return self.object_id, self.reference_entity_id, self.relation + + +class _RelativePlaceLowerer(RegisteredSemanticLowerer): + """JIT-ground one object relation to the canonical ``Place`` skill.""" + + call_id: ClassVar[str] = _PLACE_RELATIVE_CALL_ID + target_descriptor: ClassVar[SkillDescriptor] = Place.descriptor() + effect_contract_kind: ClassVar[SemanticEffectKind] = SemanticEffectKind.RELEASE + + def __init__(self, routes: tuple[_RelativePlaceRoute, ...]) -> None: + if type(routes) is not tuple or not routes: + raise ValueError("Relative Place routes must be a non-empty exact tuple.") + self._routes = {route.selector: route for route in routes} + if len(self._routes) != len(routes): + raise ValueError("Relative Place routes must be unique.") + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: ActionOptions, + ) -> SemanticLowering: + """Resolve fresh object/reference poses and preserve the held grasp.""" + if type(option_template) is not PlaceOptions: + raise TypeError( + "Configured semantic relative placement requires an exact " + "PlaceOptions template." + ) + route = self._resolve_route(call) + + resource = bound.binding.resources.get("primary") + if resource is None: + raise ValueError("Relative Place requires a bound primary resource.") + motion_endpoint = resource.endpoints.get("motion") + if motion_endpoint is None: + raise ValueError("Relative Place requires a primary motion endpoint.") + task_state_key = motion_endpoint.task_state_key + if not isinstance(task_state_key, str): + raise TypeError("Relative Place motion endpoint has no task-state key.") + held = context.task.get_held_object(task_state_key) + if held is None or held.semantics.entity_id != route.object_id: + raise ValueError( + f"Relative Place requires verified object {route.object_id!r} " + f"held under task-state key {task_state_key!r}." + ) + + object_pose = self._observed_pose(context, route.object_id) + self._observed_pose(context, route.reference_entity_id) + displacement = torch.tensor( + route.world_displacement, + dtype=object_pose.dtype, + device=object_pose.device, + ) + object_to_eef = held.object_to_eef.to( + dtype=object_pose.dtype, + device=object_pose.device, + ) + if object_to_eef.shape == (4, 4): + object_to_eef = object_to_eef.unsqueeze(0).expand( + context.batch_size, + -1, + -1, + ) + if object_to_eef.shape != (context.batch_size, 4, 4): + raise ValueError( + "Relative Place held-object transform must match the planning batch." + ) + return SemanticLowering( + goal=PlaceGoal( + xpos=SceneEntityPose( + route.reference_entity_id, + relative_pose=object_to_eef, + world_displacement=displacement, + world_orientation=( + object_pose[:, :3, :3] + if option_template.preserve_current_object_orientation + else None + ), + ) + ), + registered_effect=RegisteredSemanticEffect( + effect_kind=SemanticEffectKind.RELEASE, + held_objects=( + RegisteredHeldObjectEffect( + expectation_id="primary", + relation=HeldObjectRelation.DETACHED, + object_id=route.object_id, + slot_id="primary", + ), + ), + ), + ) + + def pick_lookahead_targets( + self, + call: RegisteredSemanticCall, + *, + picked_object: SceneObjectRef, + bound: BoundSemanticCall, + previous_target: SemanticObjectTarget | None, + ) -> tuple[SemanticObjectTarget, ...] | None: + """Expose the live-relative release target to an earlier Pick.""" + del previous_target + route = self._resolve_route(call) + if picked_object.entity_id != route.object_id: + return None + options = bound.preset.action_option_template(call.semantic_id) + if type(options) is not PlaceOptions: + raise TypeError( + "Relative Place look-ahead requires an exact PlaceOptions template." + ) + return ( + SemanticObjectTarget( + pose=SceneEntityPose( + route.reference_entity_id, + world_displacement=torch.tensor( + route.world_displacement, + dtype=torch.float32, + ), + ), + preserve_current_object_orientation=( + options.preserve_current_object_orientation + ), + ), + ) + + def _resolve_route(self, call: RegisteredSemanticCall) -> _RelativePlaceRoute: + """Validate one call and return its exact configured route.""" + arguments = dict(call.arguments) + if set(arguments) != {"object", "reference", "relation"}: + raise ValueError( + f"{self.call_id} arguments must contain only 'object', " + "'reference', and 'relation'." + ) + selector = ( + arguments["object"], + arguments["reference"], + arguments["relation"], + ) + route = self._routes.get(selector) + if route is None: + raise ValueError( + f"{self.call_id} does not declare relation route {selector!r}." + ) + return route + + @staticmethod + def _observed_pose(context: PlanningContext, entity_id: str) -> torch.Tensor: + """Return one positive-confidence entity pose in the planning batch.""" + try: + observed = context.scene.entities[entity_id] + except KeyError as exc: + raise KeyError( + f"Relative Place references unobserved entity {entity_id!r}." + ) from exc + if observed.confidence <= 0.0: + raise ValueError( + f"Relative Place requires positive confidence for {entity_id!r}." + ) + pose = observed.pose.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if pose.shape == (4, 4): + return pose.unsqueeze(0).expand(context.batch_size, -1, -1).clone() + if pose.shape != (context.batch_size, 4, 4): + raise ValueError( + f"Relative Place entity {entity_id!r} pose must match the " + "planning batch." + ) + return pose.clone() + + +@dataclass(frozen=True, slots=True) +class _RelativePlaceLowererFactory(RegisteredSemanticLowererFactory): + """Create fresh relative-placement lowerers from canonical scene refs.""" + + call_id: ClassVar[str] = _PLACE_RELATIVE_CALL_ID + revision: ClassVar[str] = "1" + target_descriptor: ClassVar[SkillDescriptor] = Place.descriptor() + + routes: tuple[_RelativePlaceRoute, ...] + + def __post_init__(self) -> None: + if type(self.routes) is not tuple or not self.routes: + raise ValueError("Relative Place routes must be a non-empty exact tuple.") + if not all(type(route) is _RelativePlaceRoute for route in self.routes): + raise TypeError("Relative Place routes must be _RelativePlaceRoute values.") + if len({route.selector for route in self.routes}) != len(self.routes): + raise ValueError("Relative Place routes must be unique.") + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> RegisteredSemanticLowerer: + """Canonicalize all object/reference IDs before constructing a lowerer.""" + del simulation + if engine.robot is not robot: + raise ValueError( + "Relative Place lowerer requires the engine's exact robot." + ) + routes: list[_RelativePlaceRoute] = [] + for route in self.routes: + object_ref = scene_registry.resolve( + route.object_id, + expected_type=SceneObjectRef, + ) + reference_ref = scene_registry.resolve( + route.reference_entity_id, + expected_type=SceneObjectRef, + ) + routes.append( + _RelativePlaceRoute( + object_id=object_ref.entity_id, + reference_entity_id=reference_ref.entity_id, + relation=route.relation, + world_displacement=route.world_displacement, + ) + ) + return _RelativePlaceLowerer(tuple(routes)) + + +@dataclass(frozen=True, slots=True) +class _CoordinatedTransportRoute: + """One configured absolute target or fresh world-frame displacement.""" + + object_id: str + target_id: str + reference_entity_id: str | None = None + relative_pose: tuple[float, ...] | None = None + world_displacement: tuple[float, float, float] | None = None + + def __post_init__(self) -> None: + object.__setattr__( + self, + "object_id", + _identifier(self.object_id, field_name="object_id"), + ) + object.__setattr__( + self, + "target_id", + _identifier(self.target_id, field_name="target_id"), + ) + has_reference = self.reference_entity_id is not None + has_pose = self.relative_pose is not None + has_displacement = self.world_displacement is not None + if has_reference != has_pose or has_reference == has_displacement: + raise ValueError( + "Coordinated transport route must declare exactly one of " + "reference_entity_id with relative_pose or world_displacement." + ) + if has_reference: + assert self.reference_entity_id is not None + assert self.relative_pose is not None + object.__setattr__( + self, + "reference_entity_id", + _identifier( + self.reference_entity_id, + field_name="reference_entity_id", + ), + ) + object.__setattr__(self, "relative_pose", _pose(self.relative_pose)) + else: + assert self.world_displacement is not None + object.__setattr__( + self, + "world_displacement", + _world_displacement(self.world_displacement), + ) + + +def _coordinated_transport_route( + value: _CoordinatedTransportRoute | tuple[str, str, str, tuple[float, ...]], + *, + index: int, +) -> _CoordinatedTransportRoute: + """Normalize the legacy private tuple used by direct lowerer tests.""" + if type(value) is _CoordinatedTransportRoute: + return value + if type(value) is not tuple or len(value) != 4: + raise TypeError( + f"Coordinated transport routes[{index}] must be a route or an " + "exact four-value compatibility tuple." + ) + return _CoordinatedTransportRoute( + object_id=value[0], + target_id=value[1], + reference_entity_id=value[2], + relative_pose=value[3], + ) + + +class _CoordinatedTransportLowerer(RegisteredSemanticLowerer): + """Lower one configured dual-arm object transport and release route.""" + + call_id: ClassVar[str] = _COORDINATED_TRANSPORT_CALL_ID + target_descriptor: ClassVar[SkillDescriptor] = CoordinatedPickment.descriptor() + effect_contract_kind: ClassVar[SemanticEffectKind] = SemanticEffectKind.RELEASE + + def __init__( + self, + routes: tuple[ + _CoordinatedTransportRoute | tuple[str, str, str, tuple[float, ...]], + ..., + ], + semantics: tuple[ObjectSemantics, ...], + ) -> None: + if len(routes) != len(semantics): + raise ValueError( + "Coordinated transport routes and semantics must have equal length." + ) + normalized = tuple( + _coordinated_transport_route(route, index=index) + for index, route in enumerate(routes) + ) + self._routes = { + (route.object_id, route.target_id): (route, object_semantics) + for route, object_semantics in zip(normalized, semantics, strict=True) + } + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: ActionOptions, + ) -> SemanticLowering: + """Construct one late-bound coordinated object target.""" + del bound + if type(option_template) is not CoordinatedPickmentOptions: + raise TypeError( + "Configured coordinated transport requires an exact " + "CoordinatedPickmentOptions template." + ) + if not option_template.release: + raise ValueError( + "Configured coordinated transport must enable coordinated release." + ) + arguments = dict(call.arguments) + if set(arguments) != {"object", "target"}: + raise ValueError( + f"{self.call_id} arguments must contain only 'object' and 'target'." + ) + route = (arguments["object"], arguments["target"]) + resolved = self._routes.get(route) + if resolved is None: + raise ValueError( + f"{self.call_id} does not declare object-target route {route!r}." + ) + route_cfg, semantics = resolved + if route_cfg.world_displacement is not None: + try: + observed = context.scene.entities[route_cfg.object_id] + except KeyError as exc: + raise KeyError( + "Coordinated transport world displacement references " + f"unobserved object {route_cfg.object_id!r}." + ) from exc + if observed.confidence <= 0.0: + raise ValueError( + "Coordinated transport requires positive observation " + f"confidence for {route_cfg.object_id!r}." + ) + object_pose = observed.pose.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if object_pose.dim() == 2: + object_pose = object_pose.unsqueeze(0).expand( + context.batch_size, + -1, + -1, + ) + object_target_pose: torch.Tensor | SceneEntityPose = object_pose.clone() + displacement = torch.tensor( + route_cfg.world_displacement, + dtype=object_target_pose.dtype, + device=object_target_pose.device, + ) + object_target_pose[:, :3, 3] += displacement + else: + assert route_cfg.reference_entity_id is not None + assert route_cfg.relative_pose is not None + object_target_pose = SceneEntityPose( + route_cfg.reference_entity_id, + relative_pose=torch.tensor( + route_cfg.relative_pose, + dtype=torch.float32, + ).reshape(4, 4), + ) + return SemanticLowering( + goal=CoordinatedPickGoal( + semantics=semantics, + object_target_pose=object_target_pose, + ), + registered_effect=RegisteredSemanticEffect( + effect_kind=SemanticEffectKind.RELEASE, + held_objects=tuple( + RegisteredHeldObjectEffect( + expectation_id=slot_id, + relation=HeldObjectRelation.DETACHED, + object_id=route_cfg.object_id, + slot_id=slot_id, + allow_missing_detached_baseline=True, + ) + for slot_id in ("left", "right") + ), + ), + ) + + +@dataclass(frozen=True, slots=True) +class _CoordinatedTransportLowererFactory(RegisteredSemanticLowererFactory): + """Create configured dual-arm transport routes from canonical scene refs.""" + + call_id: ClassVar[str] = _COORDINATED_TRANSPORT_CALL_ID + revision: ClassVar[str] = "1" + target_descriptor: ClassVar[SkillDescriptor] = CoordinatedPickment.descriptor() + + routes: tuple[ + _CoordinatedTransportRoute | tuple[str, str, str, tuple[float, ...]], + ..., + ] + + def __post_init__(self) -> None: + if type(self.routes) is not tuple or not self.routes: + raise ValueError( + "Coordinated transport routes must be a non-empty exact tuple." + ) + normalized = [ + _coordinated_transport_route(route, index=index) + for index, route in enumerate(self.routes) + ] + selectors = [(route.object_id, route.target_id) for route in normalized] + if len(set(selectors)) != len(selectors): + raise ValueError("Coordinated transport routes must be unique.") + object.__setattr__(self, "routes", tuple(normalized)) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> RegisteredSemanticLowerer: + """Resolve grasp semantics and validate all live target references.""" + del simulation + if engine.robot is not robot: + raise ValueError( + "Coordinated transport lowerer requires the engine's exact robot." + ) + canonical_routes: list[_CoordinatedTransportRoute] = [] + semantics: list[ObjectSemantics] = [] + for route in self.routes: + object_ref = scene_registry.resolve( + route.object_id, + expected_type=SceneObjectRef, + ) + grasp_ref = scene_registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + if route.world_displacement is not None: + canonical_routes.append( + _CoordinatedTransportRoute( + object_id=object_ref.entity_id, + target_id=route.target_id, + world_displacement=route.world_displacement, + ) + ) + else: + assert route.reference_entity_id is not None + assert route.relative_pose is not None + reference = scene_registry.lookup(route.reference_entity_id) + canonical_routes.append( + _CoordinatedTransportRoute( + object_id=object_ref.entity_id, + target_id=route.target_id, + reference_entity_id=reference.ref.entity_id, + relative_pose=route.relative_pose, + ) + ) + semantics.append( + scene_registry.object_semantics( + object_ref, + affordance=grasp_ref, + ) + ) + return _CoordinatedTransportLowerer( + tuple(canonical_routes), + tuple(semantics), + ) + + class _PourLowerer(RegisteredSemanticLowerer): """Lower one configured held-object pouring request.""" diff --git a/embodichain/lab/task_program/integrations/catalog.py b/embodichain/lab/task_program/integrations/catalog.py index 5810e0a95..aa38a37b3 100644 --- a/embodichain/lab/task_program/integrations/catalog.py +++ b/embodichain/lab/task_program/integrations/catalog.py @@ -124,7 +124,13 @@ _CATALOG_FINGERPRINT_SCHEMA_VERSION = 4 _POST_POLICY_KINDS = frozenset({"wait_stable"}) -_VALIDATOR_KINDS = frozenset({"articulation_joint_position", "object_near_target"}) +_VALIDATOR_KINDS = frozenset( + { + "articulation_joint_position", + "object_near_relative_target", + "object_near_target", + } +) class IntegrationFingerprintMismatch(RuntimeError): diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index 2062c2a35..bd6c7a324 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -29,10 +29,16 @@ from ._configured_services import ( _AntipodalGraspPoseGeneratorFactory, _ArticulationLinkSlideLowererFactory, + _AxisAlignLowererFactory, + _CoordinatedTransportLowererFactory, + _CoordinatedTransportRoute, _JointPositionConstraintEvidenceProviderFactory, _MoveHeldObjectLowererFactory, + _ParkLowererFactory, _PourLowererFactory, _PushObjectLowererFactory, + _RelativePlaceLowererFactory, + _RelativePlaceRoute, ) from .catalog import ( SimulationTaskProgramRegistration, @@ -58,11 +64,14 @@ ) from embodichain.lab.sim.atomic_actions import ( ActionOptions, + AxisAlignOptions, + CoordinatedPickmentOptions, DynamicCollisionMode, ExecutionRunnerCfg, HandOverOptions, MotionPolicy, MoveHeldObjectOptions, + MoveJointsOptions, PickUpOptions, PlaceOptions, PourOptions, @@ -468,13 +477,19 @@ def _decode_placement_affordance( ), ) -> SupportSurfaceAffordanceBinding | ContainerAffordanceBinding: """Decode one configured placement affordance.""" + optional = { + "aliases", + "object_target_pose", + "minimum_confidence", + "is_default", + } + if binding_type is ContainerAffordanceBinding: + optional.add("release_clearance") config = _mapping( value, path=path, required=frozenset({"kind", "entity_id", "native_name"}), - optional=frozenset( - {"aliases", "object_target_pose", "minimum_confidence", "is_default"} - ), + optional=frozenset(optional), ) kind = _identifier(config["kind"], path=f"{path}.kind") if kind != expected_kind: @@ -507,6 +522,12 @@ def _decode_placement_affordance( path=f"{path}.object_target_pose", expected_length=16, ) + if binding_type is ContainerAffordanceBinding: + kwargs["release_clearance"] = _real( + config.get("release_clearance", 0.0), + path=f"{path}.release_clearance", + minimum=0.0, + ) return binding_type(**kwargs) @@ -536,6 +557,7 @@ def _decode_entity_affordance( "internal_axis", "object_target_pose", "minimum_confidence", + "release_clearance", "is_default", } ), @@ -755,12 +777,25 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: optional=frozenset( { "hand_interp_steps", + "hold_steps", + "release", + "release_at_target", + "arm_selection", + "release_steps", + "retreat_distance", + "retreat_steps", + "receive_pick_object_part", + "object_motion_keyframes", "grasp_settle_steps", "release_settle_steps", "pick_object_part", "lift_height", "pre_grasp_distance", + "grasp_commit_fraction", "approach_direction", + "left_to_right_arm_direction", + "middle_empty_ratio", + "grasp_seed", "approach_alignment_max_angle", "obj_upright_direction", "rotate_upright", @@ -782,11 +817,12 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: "support_frame_planar_contact_offset", "contact_frame_to_eef", "tool_calibrations", + "target_axis", } ), ) kind = _identifier(common["kind"], path=f"{path}.kind") - if kind == "pick_up": + if kind in {"axis_align", "pick_up"}: config = _mapping( value, path=path, @@ -798,12 +834,14 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: "pick_object_part", "lift_height", "pre_grasp_distance", + "grasp_commit_fraction", "approach_direction", "approach_alignment_max_angle", "obj_upright_direction", "rotate_upright", "grasp_frame_to_eef", "fixed_object_to_eef", + *(("target_axis",) if kind == "axis_align" else ()), } ), ) @@ -832,6 +870,13 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: path=f"{path}.{field_name}", minimum=0.0, ) + if "grasp_commit_fraction" in config: + kwargs["grasp_commit_fraction"] = _real( + config["grasp_commit_fraction"], + path=f"{path}.grasp_commit_fraction", + minimum=0.0, + maximum=1.0, + ) if "approach_direction" in config: import torch @@ -888,6 +933,19 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: ), dtype=torch.float32, ).reshape(4, 4) + if kind == "axis_align": + if "target_axis" in config: + import torch + + kwargs["target_axis"] = torch.tensor( + _finite_tuple( + config["target_axis"], + path=f"{path}.target_axis", + expected_length=3, + ), + dtype=torch.float32, + ) + return AxisAlignOptions(**kwargs) return PickUpOptions(**kwargs) if kind == "place": config = _mapping( @@ -948,6 +1006,82 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: required=frozenset({"kind"}), ) return MoveHeldObjectOptions() + if kind == "move_joints": + _mapping( + value, + path=path, + required=frozenset({"kind"}), + ) + return MoveJointsOptions() + if kind == "coordinated_pickment": + config = _mapping( + value, + path=path, + required=frozenset({"kind"}), + optional=frozenset( + { + "object_motion_keyframes", + "pre_grasp_distance", + "lift_height", + "hand_interp_steps", + "hold_steps", + "release", + "release_steps", + "retreat_distance", + "retreat_steps", + "approach_direction", + "left_to_right_arm_direction", + "middle_empty_ratio", + "grasp_seed", + } + ), + ) + kwargs: dict[str, object] = {} + for field_name in ( + "object_motion_keyframes", + "hand_interp_steps", + "hold_steps", + "release_steps", + "retreat_steps", + "grasp_seed", + ): + if field_name in config: + kwargs[field_name] = _integer( + config[field_name], + path=f"{path}.{field_name}", + minimum=(2 if field_name == "object_motion_keyframes" else 0), + ) + for field_name in ( + "pre_grasp_distance", + "lift_height", + "retreat_distance", + "middle_empty_ratio", + ): + if field_name in config: + kwargs[field_name] = _real( + config[field_name], + path=f"{path}.{field_name}", + minimum=0.0, + maximum=(1.0 if field_name == "middle_empty_ratio" else math.inf), + ) + if "release" in config: + kwargs["release"] = _boolean( + config["release"], + path=f"{path}.release", + ) + import torch + + for field_name in ("approach_direction", "left_to_right_arm_direction"): + if field_name in config: + kwargs[field_name] = torch.tensor( + _finite_tuple( + config[field_name], + path=f"{path}.{field_name}", + expected_length=3, + ), + dtype=torch.float32, + ) + return CoordinatedPickmentOptions(**kwargs) if kind == "pour": config = _mapping( value, @@ -1125,11 +1259,25 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: path=path, required=frozenset({"kind"}), optional=frozenset( - {"pre_grasp_distance", "lift_height", "hand_interp_steps"} + { + "pre_grasp_distance", + "lift_height", + "hand_interp_steps", + "hold_steps", + "retreat_distance", + "retreat_steps", + "receive_pick_object_part", + "release_at_target", + "arm_selection", + } ), ) kwargs = {} - for field_name in ("pre_grasp_distance", "lift_height"): + for field_name in ( + "pre_grasp_distance", + "lift_height", + "retreat_distance", + ): if field_name in config: kwargs[field_name] = _real( config[field_name], @@ -1142,11 +1290,34 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: path=f"{path}.hand_interp_steps", minimum=1, ) + for field_name in ("hold_steps", "retreat_steps"): + if field_name in config: + kwargs[field_name] = _integer( + config[field_name], + path=f"{path}.{field_name}", + minimum=0 if field_name == "hold_steps" else 2, + ) + if "receive_pick_object_part" in config: + kwargs["receive_pick_object_part"] = _identifier( + config["receive_pick_object_part"], + path=f"{path}.receive_pick_object_part", + ) + if "release_at_target" in config: + kwargs["release_at_target"] = _boolean( + config["release_at_target"], + path=f"{path}.release_at_target", + ) + if "arm_selection" in config: + kwargs["arm_selection"] = _identifier( + config["arm_selection"], + path=f"{path}.arm_selection", + ) return HandOverOptions(**kwargs) raise ValueError( f"Unsupported {path}.kind {kind!r}; supported kinds are " - "['hand_over', 'move_held_object', 'pick_up', 'place', 'pour', " - "'push_object', 'slide']." + "['coordinated_pickment', 'hand_over', 'move_held_object', " + "'move_joints', " + "'pick_up', 'place', 'pour', 'push_object', 'slide']." ) @@ -1593,7 +1764,9 @@ def _decode_grasp_generator( optional=frozenset( { "sample_count", + "approach_deviation_angle", "approach_direction_samples", + "max_candidates", "opening_margin", "point_sample_density", "filter_ground_collision", @@ -1628,6 +1801,15 @@ def _decode_grasp_generator( if "sample_count" in config else None ), + approach_deviation_angle=( + _real( + config["approach_deviation_angle"], + path=f"{path}.approach_deviation_angle", + minimum=0.0, + ) + if "approach_deviation_angle" in config + else None + ), approach_direction_samples=( _integer( config["approach_direction_samples"], @@ -1637,6 +1819,15 @@ def _decode_grasp_generator( if "approach_direction_samples" in config else None ), + max_candidates=( + _integer( + config["max_candidates"], + path=f"{path}.max_candidates", + minimum=1, + ) + if "max_candidates" in config + else None + ), opening_margin=( _real( config["opening_margin"], @@ -1715,9 +1906,13 @@ def _decode_registered_lowerer( path: str, ) -> ( _ArticulationLinkSlideLowererFactory + | _AxisAlignLowererFactory + | _CoordinatedTransportLowererFactory | _MoveHeldObjectLowererFactory + | _ParkLowererFactory | _PourLowererFactory | _PushObjectLowererFactory + | _RelativePlaceLowererFactory ): """Decode one allowlisted registered semantic lowerer factory.""" common = _mapping( @@ -1735,11 +1930,24 @@ def _decode_registered_lowerer( "reference_entity_id", "relative_pose", "object_id", + "object_ids", "routes", } ), ) kind = _identifier(common["kind"], path=f"{path}.kind") + if kind == "axis_align": + config = _mapping( + value, + path=path, + required=frozenset({"kind", "object_ids"}), + ) + return _AxisAlignLowererFactory( + object_ids=_identifier_tuple( + config["object_ids"], + path=f"{path}.object_ids", + ) + ) if kind == "articulation_link_slide": config = _mapping( value, @@ -1811,6 +2019,127 @@ def _decode_registered_lowerer( expected_length=16, ), ) + if kind == "coordinated_transport": + config = _mapping( + value, + path=path, + required=frozenset({"kind", "routes"}), + ) + routes: list[_CoordinatedTransportRoute] = [] + for index, route_value in enumerate( + _sequence(config["routes"], path=f"{path}.routes") + ): + route_path = f"{path}.routes[{index}]" + route = _mapping( + route_value, + path=route_path, + required=frozenset({"object_id", "target_id"}), + optional=frozenset( + { + "reference_entity_id", + "relative_pose", + "world_displacement", + } + ), + ) + has_reference = "reference_entity_id" in route + has_pose = "relative_pose" in route + has_displacement = "world_displacement" in route + if has_reference != has_pose or has_reference == has_displacement: + raise ValueError( + f"{route_path} must declare exactly one of " + "reference_entity_id with relative_pose or " + "world_displacement." + ) + routes.append( + _CoordinatedTransportRoute( + object_id=_identifier( + route["object_id"], path=f"{route_path}.object_id" + ), + target_id=_identifier( + route["target_id"], path=f"{route_path}.target_id" + ), + reference_entity_id=( + _identifier( + route["reference_entity_id"], + path=f"{route_path}.reference_entity_id", + ) + if has_reference + else None + ), + relative_pose=( + _finite_tuple( + route["relative_pose"], + path=f"{route_path}.relative_pose", + expected_length=16, + ) + if has_pose + else None + ), + world_displacement=( + _finite_tuple( + route["world_displacement"], + path=f"{route_path}.world_displacement", + expected_length=3, + ) + if has_displacement + else None + ), + ) + ) + return _CoordinatedTransportLowererFactory(routes=tuple(routes)) + if kind == "place_relative": + config = _mapping( + value, + path=path, + required=frozenset({"kind", "routes"}), + ) + routes: list[_RelativePlaceRoute] = [] + for index, route_value in enumerate( + _sequence(config["routes"], path=f"{path}.routes") + ): + route_path = f"{path}.routes[{index}]" + route = _mapping( + route_value, + path=route_path, + required=frozenset( + { + "object_id", + "reference_entity_id", + "relation", + "world_displacement", + } + ), + ) + routes.append( + _RelativePlaceRoute( + object_id=_identifier( + route["object_id"], + path=f"{route_path}.object_id", + ), + reference_entity_id=_identifier( + route["reference_entity_id"], + path=f"{route_path}.reference_entity_id", + ), + relation=_identifier( + route["relation"], + path=f"{route_path}.relation", + ), + world_displacement=_finite_tuple( + route["world_displacement"], + path=f"{route_path}.world_displacement", + expected_length=3, + ), + ) + ) + return _RelativePlaceLowererFactory(routes=tuple(routes)) + if kind == "park": + _mapping( + value, + path=path, + required=frozenset({"kind"}), + ) + return _ParkLowererFactory() if kind == "pour": config = _mapping( value, @@ -1851,7 +2180,8 @@ def _decode_registered_lowerer( return _PushObjectLowererFactory(routes=tuple(routes)) raise ValueError( f"Unsupported {path}.kind {kind!r}; supported kinds are " - "['articulation_link_slide', 'move_held_object', 'pour', 'push_object']." + "['articulation_link_slide', 'axis_align', 'coordinated_transport', " + "'move_held_object', 'park', 'place_relative', 'pour', 'push_object']." ) @@ -1913,9 +2243,13 @@ class _DecodedRuntimeServices: handover_pose_providers: tuple[ConfiguredHandOverPoseProvider, ...] = () registered_semantic_lowerers: tuple[ _ArticulationLinkSlideLowererFactory + | _AxisAlignLowererFactory + | _CoordinatedTransportLowererFactory | _MoveHeldObjectLowererFactory + | _ParkLowererFactory | _PourLowererFactory - | _PushObjectLowererFactory, + | _PushObjectLowererFactory + | _RelativePlaceLowererFactory, ..., ] = () control_part_evidence: _JointPositionConstraintEvidenceProviderFactory | None = None diff --git a/embodichain/lab/task_program/integrations/environment.py b/embodichain/lab/task_program/integrations/environment.py index 1254ae5d5..b7bcf3c67 100644 --- a/embodichain/lab/task_program/integrations/environment.py +++ b/embodichain/lab/task_program/integrations/environment.py @@ -709,6 +709,7 @@ def _assemble_execution_runtime( ) command_encoder = RuntimeCommandFrameEncoder( observation_provider, + hold_qpos_provider=getattr(observation_provider, "hold_qpos", None), transports=self._runtime_transports, include_joint_position=include_joint_position, ) diff --git a/embodichain/lab/task_program/integrations/simulation/bindings.py b/embodichain/lab/task_program/integrations/simulation/bindings.py index 9bf05c5a7..fb5c9d099 100644 --- a/embodichain/lab/task_program/integrations/simulation/bindings.py +++ b/embodichain/lab/task_program/integrations/simulation/bindings.py @@ -388,6 +388,8 @@ class ContainerAffordanceBinding: aliases: Optional non-authoritative lookup aliases. object_target_pose: Desired object pose relative to the parent. minimum_confidence: Minimum parent/affordance observation confidence. + release_clearance: Non-negative local-z release offset in metres. The + declared object target remains the final semantic target. is_default: Whether this is the parent's default ``Place(inside=...)`` frame. """ @@ -397,10 +399,18 @@ class ContainerAffordanceBinding: aliases: tuple[str, ...] = () object_target_pose: tuple[float, ...] = _IDENTITY_POSE minimum_confidence: float = 0.0 + release_clearance: float = 0.0 is_default: bool = False def __post_init__(self) -> None: _validate_placement_binding(self) + release_clearance = _finite( + self.release_clearance, + field_name="release_clearance", + ) + if release_clearance < 0.0: + raise ValueError("release_clearance must be non-negative.") + object.__setattr__(self, "release_clearance", release_clearance) @dataclass(frozen=True, slots=True) @@ -923,8 +933,15 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: aliases=binding.aliases, parent=parent, native_name=binding.native_name, - affordance=payload_type( - minimum_confidence=binding.minimum_confidence, + affordance=( + ContainerAffordance( + minimum_confidence=binding.minimum_confidence, + release_clearance=binding.release_clearance, + ) + if type(binding) is ContainerAffordanceBinding + else SupportSurfaceAffordance( + minimum_confidence=binding.minimum_confidence, + ) ), affordance_capabilities=frozenset({capability}), affordance_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, diff --git a/embodichain/lab/task_program/integrations/simulation/environment.py b/embodichain/lab/task_program/integrations/simulation/environment.py index 8b0cf5006..aa1b11f2b 100644 --- a/embodichain/lab/task_program/integrations/simulation/environment.py +++ b/embodichain/lab/task_program/integrations/simulation/environment.py @@ -361,6 +361,52 @@ def is_owned_by(self, owner_token: object) -> bool: """Return whether this provider belongs to one factory instance.""" return self._owner_token is owner_token + def hold_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + """Return controller targets that keep unaddressed robot joints fixed. + + Using measured qpos as a new target on every frame follows controller + error and lets an idle manipulator drift. The simulator's target qpos is + the authoritative position-control hold state across semantic calls. + """ + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError("env_ids must be a non-empty one-dimensional long tensor.") + if env_ids.device != self._env_ids.device: + raise ValueError("env_ids must share the simulation environment device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + row_by_id = { + int(env_id): row + for row, env_id in enumerate(self._env_ids.detach().cpu().tolist()) + } + try: + rows = [ + row_by_id[int(env_id)] for env_id in env_ids.detach().cpu().tolist() + ] + except KeyError as exc: + raise ValueError( + f"Environment ID {int(exc.args[0])} is absent from the simulation." + ) from exc + target_qpos = self._robot.get_qpos(target=True) + if ( + not isinstance(target_qpos, torch.Tensor) + or not target_qpos.is_floating_point() + or target_qpos.shape + != (self._env_ids.numel(), int(getattr(self._robot, "dof", 0))) + or target_qpos.device != self._env_ids.device + or not bool(torch.isfinite(target_qpos).all().item()) + ): + raise ValueError( + "robot.get_qpos(target=True) must return finite floating full-qpos " + "shape (num_envs, robot_dof) on the simulation device." + ) + index = torch.tensor(rows, dtype=torch.long, device=target_qpos.device) + return target_qpos.index_select(0, index).clone() + def _capture(self, task_state: TaskState) -> PlanningContext: """Capture one synchronized robot and scene observation.""" qpos = _full_robot_tensor(self._robot, "get_qpos", required=True) @@ -514,6 +560,7 @@ def __init__( simulation, robot, selected_scene_binding, + step_dt=self._step_dt, settle_presets=selected_settle_presets, env_ids=self._env_ids, ) diff --git a/embodichain/lab/task_program/integrations/simulation/policies.py b/embodichain/lab/task_program/integrations/simulation/policies.py index 03a30bd3b..dbde24771 100644 --- a/embodichain/lab/task_program/integrations/simulation/policies.py +++ b/embodichain/lab/task_program/integrations/simulation/policies.py @@ -45,6 +45,7 @@ from embodichain.lab.task_program.compiler import ( CompiledArticulationJointPositionValidator, + CompiledObjectNearRelativeTargetValidator, CompiledObjectNearTargetValidator, CompiledPostPolicy, CompiledTaskProgramSegment, @@ -66,6 +67,11 @@ class _SimulationSettleTarget: native_entity: Any +_POSE_DELTA_SETTLE_PRESETS = frozenset( + {"contained_rigid_object", "transported_rigid_object"} +) + + def default_simulation_settle_presets() -> Mapping[str, DynamicSettleMonitorCfg]: """Return independently owned built-in post-policy presets.""" return MappingProxyType( @@ -78,6 +84,22 @@ def default_simulation_settle_presets() -> Mapping[str, DynamicSettleMonitorCfg] check_interval_steps=2, required_stable_checks=3, ), + "contained_rigid_object": DynamicSettleMonitorCfg( + linear_velocity_threshold=0.03, + angular_velocity_threshold=1.0, + min_steps=10, + max_steps=240, + check_interval_steps=2, + required_stable_checks=3, + ), + "transported_rigid_object": DynamicSettleMonitorCfg( + linear_velocity_threshold=0.03, + angular_velocity_threshold=0.20, + min_steps=10, + max_steps=240, + check_interval_steps=2, + required_stable_checks=3, + ), "articulation": DynamicSettleMonitorCfg( linear_velocity_threshold=0.02, angular_velocity_threshold=0.10, @@ -107,8 +129,10 @@ class SimulationSegmentPolicyPort: robot: Live robot used to produce full target-qpos holds while the post-policy observes settling. scene_binding: Exact canonical-to-native scene declaration. + step_dt: Duration in seconds between environment control steps. settle_presets: Named settling policies. ``None`` installs the shared - ``rigid_object`` and ``articulation`` presets. + ``rigid_object``, ``contained_rigid_object``, + ``transported_rigid_object``, and ``articulation`` presets. env_ids: Optional stable logical row IDs. They describe correlation, not simulator row indices; simulator rows remain ordered exactly as returned by the robot and bound entities. @@ -124,6 +148,7 @@ def __init__( robot: Robot, scene_binding: SimulationSceneBinding, *, + step_dt: float, settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, env_ids: torch.Tensor | None = None, ) -> None: @@ -146,6 +171,13 @@ def __init__( raise ValueError("env_ids and robot qpos must share a device.") if torch.unique(env_ids).numel() != env_ids.numel(): raise ValueError("env_ids must contain unique values.") + if ( + isinstance(step_dt, bool) + or not isinstance(step_dt, (int, float)) + or not math.isfinite(float(step_dt)) + or float(step_dt) <= 0.0 + ): + raise ValueError("step_dt must be a finite positive real number.") selected_presets = ( default_simulation_settle_presets() @@ -174,6 +206,7 @@ def __init__( self._simulation = simulation self._robot = robot self._scene_binding = scene_binding + self._step_dt = float(step_dt) self._env_ids = env_ids.clone() self._row_indices = torch.arange( qpos.shape[0], @@ -264,6 +297,11 @@ def actions( preset = self._settle_presets[policy.cfg.preset] entity_id = policy.entity.entity_id target = self._settle_targets[entity_id] + use_pose_delta = ( + target.kind == "rigid_object" + and policy.cfg.preset in _POSE_DELTA_SETTLE_PRESETS + ) + measurement_source = "pose_delta" if use_pose_delta else "reported_velocity" result_key = id(policy) self._post_policy_results.pop(result_key, None) @@ -274,6 +312,7 @@ def actions( "kind": policy.cfg.kind, "entity_id": entity_id, "preset": policy.cfg.preset, + "measurement_source": measurement_source, "source_path": list(policy.source_path), "status": "skipped", "active_mask": active_mask.detach().cpu().tolist(), @@ -284,10 +323,22 @@ def actions( active_rows = self._row_indices[active_mask] monitor = DynamicSettleMonitor(preset, self._env_ids[active_mask]) + previous_pose: torch.Tensor | None = None elapsed_steps = 0 while True: + if use_pose_delta: + sample, previous_pose = self._measure_pose_delta_settle_target( + target, + row_indices=active_rows, + previous_pose=previous_pose, + ) + else: + sample = self._measure_reported_settle_target( + target, + row_indices=active_rows, + ) state = monitor.observe( - (self._measure_settle_target(target, row_indices=active_rows),), + (sample,), elapsed_steps=elapsed_steps, ) settled_mask = torch.zeros_like(active_mask) @@ -296,6 +347,7 @@ def actions( "kind": policy.cfg.kind, "entity_id": entity_id, "preset": policy.cfg.preset, + "measurement_source": measurement_source, "source_path": list(policy.source_path), "active_mask": active_mask.detach().cpu().tolist(), "status": ( @@ -363,6 +415,7 @@ def validate_validator( """Validate one validator against static bindings without observation.""" if type(validator) not in ( CompiledObjectNearTargetValidator, + CompiledObjectNearRelativeTargetValidator, CompiledArticulationJointPositionValidator, ): raise TypeError("validator must be an exact compiled validator.") @@ -380,6 +433,22 @@ def validate_validator( ) return + if type(validator) is CompiledObjectNearRelativeTargetValidator: + if validator.cfg.kind != "object_near_relative_target": + raise ValueError( + f"Unsupported compiled validator kind {validator.cfg.kind!r}." + ) + for role, entity_id in ( + ("object", validator.object.entity_id), + ("reference", validator.reference.entity_id), + ): + if entity_id not in self._rigid_objects: + raise KeyError( + f"Canonical validator {role} {entity_id!r} has no explicit " + "rigid-object binding." + ) + return + if validator.cfg.kind != "articulation_joint_position": raise ValueError( f"Unsupported compiled validator kind {validator.cfg.kind!r}." @@ -410,6 +479,8 @@ def validate(self, validator: Any, *, segment: Any) -> torch.Tensor: self.validate_validator(validator, segment=segment) if type(validator) is CompiledArticulationJointPositionValidator: return self._validate_articulation_joint_position(validator) + if type(validator) is CompiledObjectNearRelativeTargetValidator: + return self._validate_object_near_relative_target(validator) if type(validator) is not CompiledObjectNearTargetValidator: raise TypeError("validator must be an exact compiled validator.") entity_id = validator.object.entity_id @@ -454,6 +525,7 @@ def validator_metadata( """Return an owned JSON-safe trace for one completed validator.""" if type(validator) not in ( CompiledObjectNearTargetValidator, + CompiledObjectNearRelativeTargetValidator, CompiledArticulationJointPositionValidator, ): raise TypeError("validator must be an exact compiled validator.") @@ -463,6 +535,45 @@ def validator_metadata( raise RuntimeError("Validator metadata is unavailable before validation.") return deepcopy(metadata) + def _validate_object_near_relative_target( + self, + validator: CompiledObjectNearRelativeTargetValidator, + ) -> torch.Tensor: + """Compare one object with a fresh reference pose plus displacement.""" + object_id = validator.object.entity_id + reference_id = validator.reference.entity_id + object_pose = self._read_pose( + self._rigid_objects[object_id], + entity_id=object_id, + ) + reference_pose = self._read_pose( + self._rigid_objects[reference_id], + entity_id=reference_id, + ) + object_position = object_pose[:, :3, 3] + reference_position = reference_pose[:, :3, 3] + displacement = object_position.new_tensor(validator.cfg.displacement) + target_position = reference_position + displacement + error = torch.linalg.vector_norm(object_position - target_position, dim=1) + accepted = torch.isfinite(error) & ( + error <= float(validator.cfg.position_tolerance) + ) + self._validator_results[id(validator)] = { + "kind": validator.cfg.kind, + "object_id": object_id, + "reference_id": reference_id, + "source_path": list(validator.source_path), + "displacement": list(validator.cfg.displacement), + "position_tolerance": float(validator.cfg.position_tolerance), + "env_ids": self._env_ids.detach().cpu().tolist(), + "object_position": object_position.detach().cpu().tolist(), + "reference_position": reference_position.detach().cpu().tolist(), + "target_position": target_position.detach().cpu().tolist(), + "position_error": error.detach().cpu().tolist(), + "accepted_mask": accepted.detach().cpu().tolist(), + } + return accepted + def _validate_articulation_joint_position( self, validator: CompiledArticulationJointPositionValidator, @@ -753,13 +864,13 @@ def _require_native( ) return entity - def _measure_settle_target( + def _measure_reported_settle_target( self, target: _SimulationSettleTarget, *, row_indices: torch.Tensor, ) -> DynamicSettleSample: - """Measure physical bodies for explicitly selected simulator rows.""" + """Measure reported body velocities for selected simulator rows.""" if target.kind == "articulation": body_data = getattr(target.native_entity, "body_data", None) velocity = getattr(body_data, "body_link_vel", None) @@ -817,6 +928,69 @@ def _measure_settle_target( angular_speed=angular_speed.to(device=device), ) + def _measure_pose_delta_settle_target( + self, + target: _SimulationSettleTarget, + *, + row_indices: torch.Tensor, + previous_pose: torch.Tensor | None, + ) -> tuple[DynamicSettleSample, torch.Tensor]: + """Derive rigid-object speed from consecutive observed poses. + + Contact solvers can retain non-zero velocity-cache values for an object + that is geometrically stationary inside a container or after a + coordinated release. Pose deltas are therefore the canonical settling + evidence for contact-sensitive presets. The first observation is + deliberately unresolved because no temporal evidence exists yet. + """ + if target.kind != "rigid_object": + raise ValueError("Pose-delta settling requires a rigid-object target.") + pose = self._read_pose( + target.native_entity, + entity_id=target.canonical_id, + ) + pose = pose.index_select(0, row_indices.to(pose.device)) + row_count = row_indices.numel() + if previous_pose is None: + speed = torch.full( + (row_count, 1), + float("inf"), + dtype=pose.dtype, + device=pose.device, + ) + return ( + DynamicSettleSample( + entity_id=target.canonical_id, + linear_speed=speed.to(device=self._env_ids.device), + angular_speed=speed.to(device=self._env_ids.device), + ), + pose, + ) + if previous_pose.shape != pose.shape or previous_pose.device != pose.device: + raise ValueError( + "Consecutive rigid-object poses must have equal shape and device." + ) + + linear_speed = ( + torch.linalg.vector_norm( + pose[:, :3, 3] - previous_pose[:, :3, 3], + dim=-1, + ) + / self._step_dt + ) + relative_rotation = previous_pose[:, :3, :3].transpose(-1, -2) @ pose[:, :3, :3] + cosine = (relative_rotation.diagonal(dim1=-2, dim2=-1).sum(dim=-1) - 1.0) * 0.5 + angular_speed = torch.acos(cosine.clamp(min=-1.0, max=1.0)) / self._step_dt + device = self._env_ids.device + return ( + DynamicSettleSample( + entity_id=target.canonical_id, + linear_speed=linear_speed.unsqueeze(-1).to(device=device), + angular_speed=angular_speed.unsqueeze(-1).to(device=device), + ), + pose, + ) + @staticmethod def _articulation_joint_index( articulation: Any, diff --git a/embodichain/lab/task_program/language/__init__.py b/embodichain/lab/task_program/language/__init__.py index 282c4e714..5a59bece4 100644 --- a/embodichain/lab/task_program/language/__init__.py +++ b/embodichain/lab/task_program/language/__init__.py @@ -37,6 +37,7 @@ CyclicPoseTargetCfg, HandOverCfg, InvokeCfg, + ObjectNearRelativeTargetValidatorCfg, ObjectNearTargetValidatorCfg, ParallelCfg, PickCfg, @@ -60,6 +61,7 @@ "CyclicPoseTargetCfg", "HandOverCfg", "InvokeCfg", + "ObjectNearRelativeTargetValidatorCfg", "ObjectNearTargetValidatorCfg", "ParallelCfg", "PickCfg", diff --git a/embodichain/lab/task_program/language/decoder.py b/embodichain/lab/task_program/language/decoder.py index 8e156db02..a8caac8d2 100644 --- a/embodichain/lab/task_program/language/decoder.py +++ b/embodichain/lab/task_program/language/decoder.py @@ -33,6 +33,7 @@ TaskProgramIntegrationCfg, HandOverCfg, InvokeCfg, + ObjectNearRelativeTargetValidatorCfg, ObjectNearTargetValidatorCfg, ParallelCfg, PickCfg, @@ -640,7 +641,11 @@ def _decode_validator( kind = _expect_discriminator( mapping, path=path, - supported=("object_near_target", "articulation_joint_position"), + supported=( + "object_near_target", + "object_near_relative_target", + "articulation_joint_position", + ), ) if kind == "articulation_joint_position": _validate_fields( @@ -682,6 +687,58 @@ def _decode_validator( maximum_position=maximum, ) # type: ignore[return-value] + if kind == "object_near_relative_target": + _validate_fields( + mapping, + allowed=frozenset( + { + "kind", + "object", + "reference", + "displacement", + "position_tolerance", + } + ), + required=frozenset({"kind", "object", "reference", "displacement"}), + path=path, + ) + displacement = _expect_list( + mapping["displacement"], + path=(*path, "displacement"), + ) + if len(displacement) != 3: + raise _error( + "invalid_vector_shape", + (*path, "displacement"), + "displacement must contain exactly three numbers.", + ) + for index, number in enumerate(displacement): + if type(number) not in (int, float): + raise _error( + "invalid_number", + (*path, "displacement", index), + "Displacement components must be finite numbers, not bool values.", + ) + tolerance = mapping.get("position_tolerance", 0.03) + if type(tolerance) not in (int, float): + raise _error( + "invalid_number", + (*path, "position_tolerance"), + "position_tolerance must be a finite number, not bool.", + ) + return _construct( + ObjectNearRelativeTargetValidatorCfg, + path=path, + kind=kind, + object=_expect_identifier(mapping["object"], path=(*path, "object")), + reference=_expect_identifier( + mapping["reference"], + path=(*path, "reference"), + ), + displacement=tuple(displacement), + position_tolerance=tolerance, + ) # type: ignore[return-value] + _validate_fields( mapping, allowed=frozenset({"kind", "object", "target", "position_tolerance"}), @@ -1039,6 +1096,19 @@ def validate_task_program( role="object", path=(*validator_path, "object"), ) + elif type(validator) is ObjectNearRelativeTargetValidatorCfg: + _call_context( + context.validate_scene_reference, + validator.object, + role="object", + path=(*validator_path, "object"), + ) + _call_context( + context.validate_scene_reference, + validator.reference, + role="object", + path=(*validator_path, "reference"), + ) elif type(validator) is ArticulationJointPositionValidatorCfg: _call_context( context.validate_scene_reference, diff --git a/embodichain/lab/task_program/language/schema.py b/embodichain/lab/task_program/language/schema.py index e8055012a..c063ee846 100644 --- a/embodichain/lab/task_program/language/schema.py +++ b/embodichain/lab/task_program/language/schema.py @@ -439,6 +439,40 @@ def __post_init__(self) -> None: ) +@configclass +class ObjectNearRelativeTargetValidatorCfg: + """Validate an object's position relative to a freshly observed object.""" + + object: str = MISSING + reference: str = MISSING + displacement: tuple[float, float, float] = MISSING + position_tolerance: float = 0.03 + kind: str = "object_near_relative_target" + + def __post_init__(self) -> None: + """Validate scene identities, displacement, and tolerance.""" + _validate_identifier(self.object, field_name="object") + _validate_identifier(self.reference, field_name="reference") + if type(self.displacement) not in (list, tuple) or len(self.displacement) != 3: + raise ValueError("displacement must contain exactly three finite numbers.") + self.displacement = tuple( # type: ignore[assignment] + _validate_number(value, field_name=f"displacement[{index}]") + for index, value in enumerate(self.displacement) + ) + tolerance = _validate_number( + self.position_tolerance, + field_name="position_tolerance", + ) + if tolerance <= 0.0: + raise ValueError("position_tolerance must be positive.") + self.position_tolerance = tolerance + _validate_kind( + self.kind, + expected="object_near_relative_target", + field_name="kind", + ) + + @configclass class ArticulationJointPositionValidatorCfg: """Validate one articulation joint against an inclusive position interval.""" @@ -483,7 +517,9 @@ def __post_init__(self) -> None: ValidatorCfg: TypeAlias = ( - ObjectNearTargetValidatorCfg | ArticulationJointPositionValidatorCfg + ObjectNearTargetValidatorCfg + | ObjectNearRelativeTargetValidatorCfg + | ArticulationJointPositionValidatorCfg ) @@ -623,6 +659,7 @@ def __post_init__(self) -> None: _POST_POLICY_TYPES = (WaitStablePostCfg,) _VALIDATOR_TYPES = ( ObjectNearTargetValidatorCfg, + ObjectNearRelativeTargetValidatorCfg, ArticulationJointPositionValidatorCfg, ) _PROGRAM_NODE_TYPES = ( diff --git a/embodichain/lab/task_program/runtime/executor.py b/embodichain/lab/task_program/runtime/executor.py index 8a3904381..8cbfae7df 100644 --- a/embodichain/lab/task_program/runtime/executor.py +++ b/embodichain/lab/task_program/runtime/executor.py @@ -395,6 +395,21 @@ def status(self) -> SemanticExecutionStatus: """Return the current workflow status.""" return self._status + @property + def current_call_index(self) -> int | None: + """Return the active semantic-call index without copying audit history.""" + return self._current_call_index + + @property + def env_ids(self) -> torch.Tensor: + """Return an owned environment-ID view without copying audit history.""" + return self._env_ids.clone() + + @property + def wait_duration(self) -> float: + """Return the remaining runtime wait before another due cycle.""" + return self._wait_duration + @property def result(self) -> SemanticExecutionResult: """Return an immutable snapshot of the current workflow.""" @@ -463,8 +478,28 @@ def start( def step(self) -> SemanticExecutionResult: """Advance the current call by at most one due runner cycle.""" + self._advance_once() + return self.result + + def advance(self) -> SemanticExecutionStatus: + """Advance one due cycle without materializing the full audit snapshot. + + Long-running physical calls can collect hundreds of in-flight effect + traces. Consumers that only need lifecycle progress should use this + method and read :attr:`status`, :attr:`current_call_index`, + :attr:`env_ids`, and :attr:`wait_duration`; :attr:`result` remains the + explicit immutable audit boundary. + + Returns: + The workflow status after the due cycle. + """ + self._advance_once() + return self._status + + def _advance_once(self) -> None: + """Advance the current call without constructing a result snapshot.""" if self._status is not SemanticExecutionStatus.RUNNING: - return self.result + return runner = self._require_runner() grounded = self._require_grounded() monitor = grounded.effect_monitor @@ -472,6 +507,14 @@ def step(self) -> SemanticExecutionResult: verifier = self._effect_verifier elif grounded.analyzed.effect_assurance is EffectAssurance.PROJECTED: verifier = self._project_unverified_effect + elif ( + grounded.effect_spec is None + and runner.session.active_plan.expected_effects.is_empty + ): + # Verified assurance is vacuous for an explicitly effectless motion: + # tracking and transport acknowledgement still gate completion, and + # no symbolic or physical effect is projected into TaskState. + verifier = self._project_unverified_effect else: # pragma: no cover - compiler rejects this before execution raise RuntimeError( "A verified semantic call reached execution without an effect " @@ -499,9 +542,9 @@ def step(self) -> SemanticExecutionResult: "The atomic invocation requested a phase-effect gate, but the " "grounded semantic call did not install its monitor." ) - return self.result + return if runner_step.status is RunnerStatus.RUNNING: - return self.result + return recovery_item = self._active_recovery_item trigger = ( self._workflow_recovery_trigger() @@ -514,7 +557,6 @@ def step(self) -> SemanticExecutionResult: self._handle_original_call_finished(finished, trigger=trigger) else: self._handle_recovery_call_finished(recovery_item, finished) - return self.result def run( self, diff --git a/embodichain/lab/task_program/semantics/calls.py b/embodichain/lab/task_program/semantics/calls.py index 3935f74bc..2fc760ce4 100644 --- a/embodichain/lab/task_program/semantics/calls.py +++ b/embodichain/lab/task_program/semantics/calls.py @@ -436,11 +436,12 @@ class HandOver(SemanticCallSpec): """Pick up, transfer, and place an object with two robot resources. Args: - object: Authoritative scene-object reference. The object must not - already be held when the unified action starts. + object: Authoritative scene-object reference. When the source resource + already owns a verified attachment, the call continues from that + boundary and transfers the attachment to ``destination``. final_target: Optional final object-space delivery pose. - resources: Optional skill-local resource overrides. Select the second - candidate with the ``destination`` slot when needed. + resources: Optional skill-local resource overrides. ``source`` and + ``destination`` are authoritative for a continuation transfer. """ call_kind: ClassVar[str] = "hand_over" diff --git a/embodichain/lab/task_program/semantics/scene.py b/embodichain/lab/task_program/semantics/scene.py index ce50f3a6e..4c03f82e3 100644 --- a/embodichain/lab/task_program/semantics/scene.py +++ b/embodichain/lab/task_program/semantics/scene.py @@ -128,9 +128,15 @@ class ContainerAffordance(Affordance): Args: minimum_confidence: Minimum confidence accepted while resolving the late-bound target pose. + release_clearance: Non-negative distance in metres added along the + target frame's local positive z-axis while releasing the object. + The registered pose remains the final semantic object target; this + clearance keeps the end effector outside the container boundary + before the released object settles into that target. """ minimum_confidence: float = 0.0 + release_clearance: float = 0.0 def __post_init__(self) -> None: if isinstance(self.minimum_confidence, bool) or not isinstance( @@ -141,6 +147,14 @@ def __post_init__(self) -> None: self.minimum_confidence = float(self.minimum_confidence) if not 0.0 <= self.minimum_confidence <= 1.0: raise ValueError("minimum_confidence must be in [0, 1].") + if isinstance(self.release_clearance, bool) or not isinstance( + self.release_clearance, + (int, float), + ): + raise TypeError("release_clearance must be a number.") + self.release_clearance = float(self.release_clearance) + if not math.isfinite(self.release_clearance) or self.release_clearance < 0.0: + raise ValueError("release_clearance must be finite and non-negative.") def _validate_identifier(value: str, name: str) -> None: diff --git a/embodichain/toolkits/graspkit/pg_grasp/collision_checker.py b/embodichain/toolkits/graspkit/pg_grasp/collision_checker.py index 2747fa063..9bcfd5681 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/collision_checker.py +++ b/embodichain/toolkits/graspkit/pg_grasp/collision_checker.py @@ -27,7 +27,7 @@ import open3d as o3d from typing import List, Tuple, Union -from dexsim.kit.meshproc import convex_decomposition_coacd +from dexsim.kit.meshproc import convex_decomposition_vhacd from embodichain.compute.geometry._warp.convex_query import ( convex_signed_distance_kernel, @@ -39,6 +39,8 @@ __all__ = ["ConvexCollisionCheckerCfg", "ConvexCollisionChecker"] +_CONVEX_DECOMPOSITION_CACHE_TAG = "vhacd_v1" + @configclass class ConvexCollisionCheckerCfg: @@ -88,7 +90,10 @@ def __init__( self.cache_path = os.path.join( CONVEX_DECOMPOSITION_CACHE_DIR, - f"{mesh_hash}_{max_decomposition_hulls}.pkl", + ( + f"{mesh_hash}_{max_decomposition_hulls}_" + f"{_CONVEX_DECOMPOSITION_CACHE_TAG}.pkl" + ), ) if not os.path.isfile(self.cache_path): @@ -301,9 +306,11 @@ def _compute_plane_equations( mesh = o3d.t.geometry.TriangleMesh() mesh.vertex.positions = o3d.core.Tensor(vertices, dtype=o3d.core.Dtype.Float32) mesh.triangle.indices = o3d.core.Tensor(faces, dtype=o3d.core.Dtype.Int32) - is_success, out_mesh_list = convex_decomposition_coacd( + is_success, out_mesh_list = convex_decomposition_vhacd( mesh, max_convex_hull_num=max_decomposition_hulls ) + if not is_success or not out_mesh_list: + raise RuntimeError("V-HACD convex decomposition failed.") convex_vert_face_list = [] for out_mesh in out_mesh_list: verts = out_mesh.vertex.positions.numpy() diff --git a/embodichain_tasks/configs/components/embodiments/dual_franka_robotiq_arg2f_140.yaml b/embodichain_tasks/configs/components/embodiments/dual_franka_robotiq_arg2f_140.yaml new file mode 100644 index 000000000..fcf09823a --- /dev/null +++ b/embodichain_tasks/configs/components/embodiments/dual_franka_robotiq_arg2f_140.yaml @@ -0,0 +1,249 @@ +embodiment_id: dual_franka_robotiq_arg2f_140 + +simulation: + uid: DualFrankaPanda + urdf_cfg: + fname: dual_franka_robotiq_arg2f_140 + 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: Robotiq/robotiq_arg2f_140/robotiq_arg2f_140.urdf + - 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: Robotiq/robotiq_arg2f_140/robotiq_arg2f_140.urdf + init_pos: [-0.7, 0.0, 0.322894] + 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 + - 0.0 + - 0.0 + - 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: 50.0 + right_eef: 50.0 + damping: + left_arm: 1000.0 + right_arm: 1000.0 + left_eef: 5.0 + right_eef: 5.0 + max_effort: + left_arm: 10000.0 + right_arm: 10000.0 + left_eef: 500.0 + right_eef: 500.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_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_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_finger_joint + - right_left_inner_knuckle_joint + - right_left_inner_finger_joint + - right_outer_knuckle_joint + - right_inner_knuckle_joint + - right_inner_finger_joint + 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 + solver_cfg: + left_arm: + class_type: PytorchSolver + urdf_path: null + end_link_name: left_fr3_link8 + root_link_name: left_base + tcp: + - [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] + num_samples: 15 + right_arm: + class_type: PytorchSolver + urdf_path: null + end_link_name: right_fr3_link8 + root_link_name: right_base + tcp: + - [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] + num_samples: 15 + +sensor: + - sensor_type: Camera + uid: cam_high + 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] + +skill_profile: + contract_id: dual_arm_parallel_gripper + profile_id: dual_franka_robotiq_arg2f_140 + resources: + - resource_id: left + endpoints: + - endpoint_id: motion + control_part: left_arm + command_preset: left_arm_postures + capabilities: + - kinematics.inverse + - kinematics.batch_inverse + - motion.cartesian_pose + - motion.joint_position + - kinematics.forward + - endpoint_id: grasp + control_part: left_eef + capabilities: [interaction.grasp] + command_preset: left_parallel_gripper + - resource_id: right + endpoints: + - endpoint_id: motion + control_part: right_arm + command_preset: right_arm_postures + capabilities: + - kinematics.inverse + - kinematics.batch_inverse + - motion.cartesian_pose + - motion.joint_position + - kinematics.forward + - endpoint_id: grasp + control_part: right_eef + capabilities: [interaction.grasp] + command_preset: right_parallel_gripper + command_presets: + - preset_id: left_arm_postures + control_part: left_arm + commands: + park: [0.0, -0.569, 0.0, -2.81, 0.0, 3.037, 0.0] + - preset_id: right_arm_postures + control_part: right_arm + commands: + park: [0.0, -0.569, 0.0, -2.81, 0.0, 3.037, 0.0] + - preset_id: left_parallel_gripper + control_part: left_eef + commands: + open: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + grasp: [0.7, -0.7, 0.7, -0.7, -0.7, 0.7] + - preset_id: right_parallel_gripper + control_part: right_eef + commands: + open: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + grasp: [0.7, -0.7, 0.7, -0.7, -0.7, 0.7] + runtime_services: + grasp_pose_generators: + left_eef: + kind: antipodal_parallel_jaw + sample_count: 10000 + approach_deviation_angle: 0.3490658503988659 + approach_direction_samples: 4 + max_candidates: 500 + opening_margin: 0.02 + point_sample_density: 0.012 + filter_ground_collision: false + model: + 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 + right_eef: + kind: antipodal_parallel_jaw + sample_count: 10000 + approach_deviation_angle: 0.3490658503988659 + approach_direction_samples: 4 + max_candidates: 500 + opening_margin: 0.02 + point_sample_density: 0.012 + filter_ground_collision: false + model: + 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 + control_part_evidence: + kind: joint_position_constraint + control_parts: [left_eef, right_eef] + open_qpos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + minimum_displacement: 0.05 diff --git a/embodichain_tasks/configs/components/execution_policies/dual_arm_trajectory_open_loop.yaml b/embodichain_tasks/configs/components/execution_policies/dual_arm_trajectory_open_loop.yaml new file mode 100644 index 000000000..a93365cb8 --- /dev/null +++ b/embodichain_tasks/configs/components/execution_policies/dual_arm_trajectory_open_loop.yaml @@ -0,0 +1,32 @@ +policy_id: dual_arm_trajectory_open_loop_v1 +preset_id: trajectory + +requires: + embodiment_contract: dual_arm_parallel_gripper + +motion: + strategy: ik_interp + sample_count: 120 + dynamic_collision_mode: "off" + +tracking: + kind: timed + +recovery: + max_replans: 2 + max_action_retries: 2 + goal_translation_threshold: 0.05 + # Generated antipodal objects may roll in place while remaining inside the + # gripper's translational capture range. Translation remains the invalidation + # authority for this open-loop profile. + goal_rotation_threshold: 3.2 + +workflow_recovery: + max_recovery_attempts: 0 + +runner: + minimum_cycle_time: 0.0 + hold_on_completion: false + hold_during_effect_verification: false + +effect_assurance: projected diff --git a/embodichain_tasks/configs/components/execution_policies/dual_arm_trajectory_verified.yaml b/embodichain_tasks/configs/components/execution_policies/dual_arm_trajectory_verified.yaml new file mode 100644 index 000000000..76bf84379 --- /dev/null +++ b/embodichain_tasks/configs/components/execution_policies/dual_arm_trajectory_verified.yaml @@ -0,0 +1,58 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +policy_id: dual_arm_trajectory_verified_v1 +preset_id: trajectory_verified + +requires: + embodiment_contract: dual_arm_parallel_gripper + +motion: + strategy: ik_interp + # HandOver reserves explicit close, hold, release, and retreat phases. A + # 140-frame budget leaves the calibrated 44 frames each for held-object + # transfer and receiver approach instead of compressing either physical + # motion into an unsafe burst. + sample_count: 140 + dynamic_collision_mode: "off" + +tracking: + kind: joint_position + # The generated dual-Franka scene is driven through a 25 Hz joint-position + # target interface. These bounds retain a real feedback boundary while + # allowing the transient controller lag measured during close/lift. + in_flight_max_abs_error: 0.8 + terminal_max_abs_error: 0.25 + terminal_settle_timeout: 2.0 + consecutive_violations: 3 + consecutive_acceptances: 3 + grace_period: 0.2 + +recovery: + max_replans: 3 + max_action_retries: 1 + goal_translation_threshold: 0.05 + goal_rotation_threshold: 3.2 + +workflow_recovery: + max_recovery_attempts: 1 + +runner: + minimum_cycle_time: 0.04 + hold_on_completion: false + hold_during_effect_verification: false + +effect_assurance: verified diff --git a/embodichain_tasks/configs/tasks/classic_control/cart_pole/agents/grpo.json b/embodichain_tasks/configs/tasks/classic_control/cart_pole/agents/grpo.json index d44e26f67..de73223f8 100644 --- a/embodichain_tasks/configs/tasks/classic_control/cart_pole/agents/grpo.json +++ b/embodichain_tasks/configs/tasks/classic_control/cart_pole/agents/grpo.json @@ -87,4 +87,4 @@ "truncate_at_first_done": true } } -} +} \ No newline at end of file diff --git a/embodichain_tasks/configs/tasks/classic_control/cart_pole/agents/ppo.json b/embodichain_tasks/configs/tasks/classic_control/cart_pole/agents/ppo.json index e6598e818..a4b04beca 100644 --- a/embodichain_tasks/configs/tasks/classic_control/cart_pole/agents/ppo.json +++ b/embodichain_tasks/configs/tasks/classic_control/cart_pole/agents/ppo.json @@ -93,4 +93,4 @@ "max_grad_norm": 0.5 } } -} +} \ No newline at end of file diff --git a/embodichain_tasks/configs/tasks/manipulation/push_cube/agents/ppo.json b/embodichain_tasks/configs/tasks/manipulation/push_cube/agents/ppo.json index ae9adbbaa..399454e6d 100644 --- a/embodichain_tasks/configs/tasks/manipulation/push_cube/agents/ppo.json +++ b/embodichain_tasks/configs/tasks/manipulation/push_cube/agents/ppo.json @@ -76,4 +76,4 @@ "max_grad_norm": 0.5 } } -} +} \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index 299eaf49c..faece9731 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -25,7 +25,7 @@ dynamic = ["version"] # Core install dependencies (kept from requirements.txt). dependencies = [ - "dexsim_engine==0.5.0", + "dexsim_engine==0.4.3", "setuptools>=78.1.1", "gymnasium>=0.29.1", "langchain", diff --git a/setup.py b/setup.py index ac131d484..e8641bc83 100644 --- a/setup.py +++ b/setup.py @@ -134,6 +134,7 @@ def main(): package_data={ "embodichain": ["VERSION"], "embodichain.gen_sim.simready_pipeline.configs": ["*.json"], + "embodichain.gen_sim.task_engine": ["*.yaml"], "embodichain_tasks.configs": ["**/*.json", "**/*.yaml", "**/*.yml"], }, cmdclass=cmdclass, diff --git a/tests/gen_sim/__init__.py b/tests/gen_sim/__init__.py index 355d915ff..cdeead7b0 100644 --- a/tests/gen_sim/__init__.py +++ b/tests/gen_sim/__init__.py @@ -14,4 +14,8 @@ # limitations under the License. # ---------------------------------------------------------------------------- +"""Generative simulation tests.""" + from __future__ import annotations + +__all__: list[str] = [] diff --git a/tests/gen_sim/scene_engine/test_config.py b/tests/gen_sim/scene_engine/test_config.py index 3754210d9..96bebf4ad 100644 --- a/tests/gen_sim/scene_engine/test_config.py +++ b/tests/gen_sim/scene_engine/test_config.py @@ -60,6 +60,9 @@ def test_scene_engine_help_exposes_only_runtime_arguments( output = capsys.readouterr().out assert "--image" in output assert "--output_root" in output + assert "--prompt2scene_scene_z_rotation_degrees" in output + assert "--prompt2scene_mesh_x_rotation_degrees" in output + assert "--target_body_scale_mode" in output assert "gen_sim/.env" in output assert "--config" not in output @@ -70,23 +73,92 @@ def test_scene_engine_cli_forwards_validated_paths( ) -> None: image_path = tmp_path / "scene.png" image_path.write_bytes(b"png") - captured: dict[str, Path] = {} + captured: dict[str, object] = {} - def generate_scene(*, image_path: Path, output_root: Path) -> None: + def generate_scene( + *, + image_path: Path, + output_root: Path, + scene_z_rotation_degrees: float, + ) -> None: captured["image_path"] = image_path captured["output_root"] = output_root + captured["scene_z_rotation_degrees"] = scene_z_rotation_degrees monkeypatch.setattr(start, "generate_scene_from_image", generate_scene) output_root = tmp_path / "output" - start.cli_scene_engine(image_path, output_root) + start.cli_scene_engine( + image_path, + output_root, + scene_z_rotation_degrees=180.0, + ) assert captured == { "image_path": image_path.resolve(), "output_root": output_root.resolve(), + "scene_z_rotation_degrees": 180.0, } +def test_scene_engine_main_accepts_legacy_direct_glb_options( + monkeypatch: pytest.MonkeyPatch, + tmp_path: Path, +) -> None: + image_path = tmp_path / "scene.png" + image_path.write_bytes(b"png") + captured: dict[str, object] = {} + + def cli_scene_engine( + image: str, + output_root: str, + *, + edit_prompt: str | None, + scene_z_rotation_degrees: float, + ) -> None: + captured.update( + image=image, + output_root=output_root, + edit_prompt=edit_prompt, + scene_z_rotation_degrees=scene_z_rotation_degrees, + ) + + monkeypatch.setattr(start, "cli_scene_engine", cli_scene_engine) + + start.main( + [ + "--image", + str(image_path), + "--output_root", + str(tmp_path / "output"), + "--target_body_scale_mode", + "preserve", + "--prompt2scene_scene_z_rotation_degrees", + "180", + "--prompt2scene_mesh_x_rotation_degrees", + "0", + ] + ) + + assert captured["scene_z_rotation_degrees"] == 180.0 + + +def test_scene_engine_main_rejects_legacy_mesh_x_rotation( + tmp_path: Path, +) -> None: + with pytest.raises(SystemExit) as exc_info: + start.main( + [ + "--output_root", + str(tmp_path / "output"), + "--prompt2scene_mesh_x_rotation_degrees", + "90", + ] + ) + + assert exc_info.value.code == 2 + + def test_scene_engine_cli_edits_existing_output_without_an_image( monkeypatch: pytest.MonkeyPatch, tmp_path: Path, @@ -116,7 +188,12 @@ def test_scene_engine_cli_generates_then_edits_when_both_inputs_exist( image_path.write_bytes(b"png") call_order: list[str] = [] - def generate_scene(*, image_path: Path, output_root: Path) -> None: + def generate_scene( + *, + image_path: Path, + output_root: Path, + scene_z_rotation_degrees: float, + ) -> None: call_order.append("generate") def edit_scene(*, output_root: Path, edit_prompt: str) -> None: diff --git a/tests/gen_sim/scene_engine/test_pipeline_api.py b/tests/gen_sim/scene_engine/test_pipeline_api.py new file mode 100644 index 000000000..5bf79bc10 --- /dev/null +++ b/tests/gen_sim/scene_engine/test_pipeline_api.py @@ -0,0 +1,357 @@ +# ---------------------------------------------------------------------------- +# 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 +from pathlib import Path + +import pytest + +from embodichain.gen_sim.scene_engine.core.scene import Scene +from embodichain.gen_sim.scene_engine.core.scene_edit_plan import SceneEditPlan +from embodichain.gen_sim.scene_engine.core.scene_graph import ( + SceneGraph, + SceneGraphNode, +) +from embodichain.gen_sim.scene_engine.core.scene_object import SceneObject +from embodichain.gen_sim.scene_engine.pipeline import api + + +class _HealthyClient: + def __init__(self) -> None: + self.health_checks = 0 + + def check_health(self) -> None: + self.health_checks += 1 + + +class _OwnedClient(_HealthyClient): + def __init__(self) -> None: + super().__init__() + self.close_calls = 0 + + def close(self) -> None: + self.close_calls += 1 + + +def _materialization( + *, + scene: Scene, + scene_graph: SceneGraph, + output_root: Path, +) -> api.SceneMaterialization: + return api.SceneMaterialization( + scene=scene, + scene_graph=scene_graph, + output_root=output_root, + scene_config_path=output_root / "scene_export" / "scene_config.json", + ) + + +def _table_scene() -> tuple[Scene, SceneGraph]: + scene = Scene( + objects=[ + SceneObject( + id="table", + kind="table", + category="table", + name="table", + description="A work table.", + ) + ] + ) + graph = SceneGraph(nodes=[SceneGraphNode(object_id="table", parent_id=None)]) + return scene, graph + + +def test_analyze_image_persists_blueprint_and_artifact_hashes( + tmp_path: Path, + monkeypatch, +) -> None: + image_path = tmp_path / "input.png" + image_path.write_bytes(b"image") + scene, graph = _table_scene() + + def fake_understand_scene(**kwargs): + stage_root = Path(kwargs["output_root"]) / "scene_understanding" + stage_root.mkdir(parents=True) + (stage_root / "table-mask.png").write_bytes(b"mask") + return scene, graph + + monkeypatch.setattr(api, "understand_scene", fake_understand_scene) + segmentation = _HealthyClient() + package = api.analyze_image( + image_path, + tmp_path / "output", + vlm_client=object(), + image_segmentation_client=segmentation, + ) + document = json.loads(package.manifest_path.read_text(encoding="utf-8")) + + assert segmentation.health_checks == 1 + assert package.schema_version == api.SCENE_BLUEPRINT_SCHEMA + assert document["schema_version"] == "embodichain.scene-blueprint/v2" + assert document["blueprint_id"] == package.blueprint_id + assert document["scene_graph"] == graph.to_dict() + assert document["artifacts"][0]["path"].endswith("table-mask.png") + assert len(document["artifacts"][0]["sha256"]) == 64 + + +def test_analyze_edit_persists_post_edit_blueprint( + tmp_path: Path, + monkeypatch, +) -> None: + scene, graph = _table_scene() + plan = SceneEditPlan(scene=scene, scene_graph=graph, operations=[]) + + class FakeImporter: + def __init__(self, *, output_root: Path) -> None: + self.output_root = output_root + + def import_scene_and_graph(self): + return scene, graph + + monkeypatch.setattr(api, "SceneExportImporter", FakeImporter) + monkeypatch.setattr( + api, + "understand_scene_edit", + lambda **_: (plan, graph), + ) + package = api.analyze_edit( + output_root=tmp_path, + edit_prompt="Keep the scene unchanged.", + vlm_client=object(), + ) + document = json.loads(package.manifest_path.read_text(encoding="utf-8")) + + assert package.schema_version == api.SCENE_EDIT_BLUEPRINT_SCHEMA + assert document["schema_version"] == "embodichain.scene-edit-blueprint/v2" + assert document["blueprint_id"] == package.blueprint_id + assert document["scene_edit_plan"] == plan.to_dict() + assert document["updated_scene_graph"] == graph.to_dict() + + +def test_materialize_blueprint_does_not_mutate_audited_snapshot( + tmp_path: Path, + monkeypatch, +) -> None: + scene, graph = _table_scene() + manifest_path = tmp_path / "scene_blueprint.json" + manifest_path.write_text("audited blueprint\n", encoding="utf-8") + package = api.SceneBlueprintPackage( + schema_version=api.SCENE_BLUEPRINT_SCHEMA, + blueprint_id="blueprint", + image_path=tmp_path / "input.png", + output_root=tmp_path, + manifest_path=manifest_path, + scene=scene, + scene_graph=graph, + ) + original_scene = deepcopy(scene.to_dict()) + original_graph = deepcopy(graph.to_dict()) + + def fake_generate_scene_and_refine(**kwargs): + assert "seed" not in kwargs + assert kwargs["articulated_generation_client"] is None + assert kwargs["scene"] is not package.scene + assert kwargs["scene_graph"] is not package.scene_graph + kwargs["scene"].objects[0].name = "materialized table" + return kwargs["scene"] + + monkeypatch.setattr( + api, + "generate_scene_and_refine", + fake_generate_scene_and_refine, + ) + monkeypatch.setattr( + api, + "_export_materialization", + lambda *, scene, scene_graph, output_root: _materialization( + scene=scene, + scene_graph=scene_graph, + output_root=output_root, + ), + ) + + result = api.materialize_blueprint( + package, + vlm_client=object(), + geometry_generation_client=_HealthyClient(), + ) + + assert result.scene.objects[0].name == "materialized table" + assert package.scene.to_dict() == original_scene + assert package.scene_graph.to_dict() == original_graph + assert manifest_path.read_text(encoding="utf-8") == "audited blueprint\n" + + +def test_materialize_edit_does_not_mutate_audited_snapshot( + tmp_path: Path, + monkeypatch, +) -> None: + scene, graph = _table_scene() + plan = SceneEditPlan(scene=scene, scene_graph=graph, operations=[]) + manifest_path = tmp_path / "scene_edit_blueprint.json" + manifest_path.write_text("audited edit blueprint\n", encoding="utf-8") + package = api.SceneEditBlueprintPackage( + schema_version=api.SCENE_EDIT_BLUEPRINT_SCHEMA, + blueprint_id="edit-blueprint", + edit_prompt="Keep the scene unchanged.", + output_root=tmp_path, + manifest_path=manifest_path, + scene_edit_plan=plan, + updated_scene_graph=graph, + ) + original_plan = deepcopy(plan.to_dict()) + original_graph = deepcopy(graph.to_dict()) + + def fake_prepare_scene_edit_assets(**kwargs): + assert "seed" not in kwargs + return [] + + monkeypatch.setattr( + api, "prepare_scene_edit_assets", fake_prepare_scene_edit_assets + ) + + def fake_edit_layout(**kwargs): + assert kwargs["scene_edit_plan"] is not package.scene_edit_plan + assert kwargs["updated_scene_graph"] is not package.updated_scene_graph + kwargs["scene"].objects[0].name = "edited table" + return kwargs["scene"] + + monkeypatch.setattr(api, "edit_layout", fake_edit_layout) + monkeypatch.setattr( + api, + "_export_materialization", + lambda *, scene, scene_graph, output_root: _materialization( + scene=scene, + scene_graph=scene_graph, + output_root=output_root, + ), + ) + clients = [_HealthyClient(), _HealthyClient(), _HealthyClient()] + + result = api.materialize_edit( + package, + vlm_client=object(), + image_generation_client=clients[0], + geometry_generation_client=clients[1], + image_segmentation_client=clients[2], + ) + + assert result.scene.objects[0].name == "edited table" + assert package.scene_edit_plan.to_dict() == original_plan + assert package.updated_scene_graph.to_dict() == original_graph + assert manifest_path.read_text(encoding="utf-8") == "audited edit blueprint\n" + + +def test_scene_blueprint_package_rejects_v1_schema(tmp_path: Path) -> None: + scene, graph = _table_scene() + + with pytest.raises(ValueError, match="scene-blueprint/v2"): + api.SceneBlueprintPackage( + schema_version="embodichain.scene-blueprint/v1", + blueprint_id="legacy", + image_path=tmp_path / "input.png", + output_root=tmp_path, + manifest_path=tmp_path / "scene_blueprint.json", + scene=scene, + scene_graph=graph, + ) + + +def test_scene_edit_blueprint_package_rejects_v1_schema(tmp_path: Path) -> None: + scene, graph = _table_scene() + plan = SceneEditPlan(scene=scene, scene_graph=graph, operations=[]) + + with pytest.raises(ValueError, match="scene-edit-blueprint/v2"): + api.SceneEditBlueprintPackage( + schema_version="embodichain.scene-edit-blueprint/v1", + blueprint_id="legacy-edit", + edit_prompt="Keep the scene unchanged.", + output_root=tmp_path, + manifest_path=tmp_path / "scene_edit_blueprint.json", + scene_edit_plan=plan, + updated_scene_graph=graph, + ) + + +def test_materialize_blueprint_owns_articulated_client_lifecycle( + tmp_path: Path, + monkeypatch, +) -> None: + scene, graph = _table_scene() + scene.objects.append( + SceneObject( + id="microwave_001", + kind="asset", + category="microwave", + name="microwave", + description="An articulated microwave.", + is_articulated=True, + ) + ) + graph.nodes.append( + SceneGraphNode( + object_id="microwave_001", + parent_id="table", + parent_relation="on", + pose_description="Stand upright on its base.", + ) + ) + package = api.SceneBlueprintPackage( + schema_version=api.SCENE_BLUEPRINT_SCHEMA, + blueprint_id="articulated", + image_path=tmp_path / "input.png", + output_root=tmp_path, + manifest_path=tmp_path / "scene_blueprint.json", + scene=scene, + scene_graph=graph, + ) + articulated = _OwnedClient() + monkeypatch.setattr( + api.ArticulatedGenerationClient, + "from_dotenv", + lambda: articulated, + ) + + def fake_generate_scene_and_refine(**kwargs): + assert kwargs["articulated_generation_client"] is articulated + return kwargs["scene"] + + monkeypatch.setattr( + api, "generate_scene_and_refine", fake_generate_scene_and_refine + ) + monkeypatch.setattr( + api, + "_export_materialization", + lambda *, scene, scene_graph, output_root: _materialization( + scene=scene, + scene_graph=scene_graph, + output_root=output_root, + ), + ) + + api.materialize_blueprint( + package, + vlm_client=object(), + geometry_generation_client=_HealthyClient(), + ) + + assert articulated.health_checks == 1 + assert articulated.close_calls == 1 diff --git a/tests/gen_sim/scene_engine/test_scene_generation.py b/tests/gen_sim/scene_engine/test_scene_generation.py index b8c633884..584d8d214 100644 --- a/tests/gen_sim/scene_engine/test_scene_generation.py +++ b/tests/gen_sim/scene_engine/test_scene_generation.py @@ -49,6 +49,11 @@ layout_object_to_transform_matrix, transform_matrix_to_layout_object, ) +from embodichain.gen_sim.scene_engine.pipeline.utils.scene_layout_utils import ( + rotate_scene_z_up_world, + scene_object_y_up_layout, + y_up_to_z_up_matrix, +) def _z_up_rotation_from_y_up_layout(layout: dict[str, object]) -> np.ndarray: @@ -223,6 +228,75 @@ def test_visual_yaws_replace_coarse_rotations_but_preserve_positions() -> None: assert np.allclose(yawed_layout["pos"], [0.1, 0.2, 0.3]) +def test_rotate_scene_z_up_world_rotates_complete_scene_and_support_metadata() -> None: + scene = Scene( + objects=[ + SceneObject( + id="table", + kind="table", + category="table", + name="table", + description="table", + rot=[0.0, 0.0, 0.0], + pos=[0.0, 0.0, 0.0], + scale=[1.0, 1.0, 1.0], + center_xy=[1.0, 2.0], + support_contour_xy=[[1.0, 2.0], [-1.0, 2.0]], + support_optimization_rect_xy=[[1.0, 1.0], [-1.0, 1.0]], + ), + SceneObject( + id="book_001", + kind="asset", + category="book", + name="book", + description="book", + rot=[10.0, 20.0, 30.0], + pos=[1.0, 2.0, 3.0], + scale=[1.0, 2.0, 3.0], + center_xy=[3.0, 4.0], + ), + ] + ) + basis = y_up_to_z_up_matrix() + inverse_basis = np.linalg.inv(basis) + original_z_up_transforms = { + scene_object.id: ( + basis + @ layout_object_to_transform_matrix(scene_object_y_up_layout(scene_object)) + @ inverse_basis + ) + for scene_object in scene.objects + } + expected_world_rotation = np.eye(4) + expected_world_rotation[:3, :3] = Rotation.from_euler( + "z", 180.0, degrees=True + ).as_matrix() + + rotate_scene_z_up_world(scene=scene, rotation_degrees=180.0) + + for scene_object in scene.objects: + actual_z_up_transform = ( + basis + @ layout_object_to_transform_matrix(scene_object_y_up_layout(scene_object)) + @ inverse_basis + ) + assert np.allclose( + actual_z_up_transform, + expected_world_rotation @ original_z_up_transforms[scene_object.id], + ) + assert np.allclose(scene.table.center_xy, [-1.0, -2.0]) + assert np.allclose(scene.table.support_contour_xy, [[-1.0, -2.0], [1.0, -2.0]]) + assert np.allclose( + scene.table.support_optimization_rect_xy, + [[-1.0, -1.0], [1.0, -1.0]], + ) + + +def test_rotate_scene_z_up_world_rejects_non_finite_angle() -> None: + with pytest.raises(ValueError, match="rotation_degrees must be finite"): + rotate_scene_z_up_world(scene=Scene(), rotation_degrees=float("nan")) + + def test_articulated_usdcs_use_visible_rgba_in_scene_order( monkeypatch: pytest.MonkeyPatch, tmp_path: Path, diff --git a/tests/gen_sim/task_engine/__init__.py b/tests/gen_sim/task_engine/__init__.py new file mode 100644 index 000000000..b201491d8 --- /dev/null +++ b/tests/gen_sim/task_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. +# ---------------------------------------------------------------------------- + +"""Tests for Task Engine semantics and orchestration.""" + +from __future__ import annotations diff --git a/tests/gen_sim/task_engine/orchestration/__init__.py b/tests/gen_sim/task_engine/orchestration/__init__.py new file mode 100644 index 000000000..8256d7018 --- /dev/null +++ b/tests/gen_sim/task_engine/orchestration/__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. +# ---------------------------------------------------------------------------- + +"""Tests for Task Engine cross-engine orchestration.""" + +from __future__ import annotations diff --git a/tests/gen_sim/task_engine/orchestration/test_architecture.py b/tests/gen_sim/task_engine/orchestration/test_architecture.py new file mode 100644 index 000000000..dd3ab9d1f --- /dev/null +++ b/tests/gen_sim/task_engine/orchestration/test_architecture.py @@ -0,0 +1,201 @@ +# ---------------------------------------------------------------------------- +# 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 ast +from dataclasses import fields +from inspect import signature +from pathlib import Path + +import embodichain.gen_sim as gen_sim_package +from embodichain.gen_sim.task_engine import TaskAgent +from embodichain.gen_sim.task_engine import __main__ as task_engine_main +from embodichain.gen_sim.task_engine import cli as task_engine_cli +from embodichain.gen_sim.task_engine.orchestration.coordinator import ( + TaskEngineCoordinator, +) +from embodichain.gen_sim.task_engine.orchestration.artifacts import ( + TaskEngineArtifactPaths, + write_task_engine_artifacts, +) +from embodichain.gen_sim.task_engine.orchestration.scene_adapter import SceneAdapter + +_GEN_SIM_ROOT = Path(gen_sim_package.__file__).resolve().parent +_PURE_TASK_MODULES = ( + "agent.py", + "config.py", + "contracts.py", + "interpretation.py", + "ontology.py", + "state_machine.py", + "workflow_contracts.py", +) + + +def test_task_semantic_core_does_not_import_scene_action_or_orchestration() -> None: + forbidden = { + "embodichain.gen_sim.action_engine", + "embodichain.gen_sim.scene_engine", + "embodichain.gen_sim.task_engine.orchestration", + "embodichain.gen_sim.task_engine.scene", + } + offenders: list[str] = [] + for filename in _PURE_TASK_MODULES: + path = _GEN_SIM_ROOT / "task_engine" / filename + tree = ast.parse(path.read_text(encoding="utf-8"), filename=str(path)) + for node in ast.walk(tree): + if isinstance(node, ast.Import): + modules = [alias.name for alias in node.names] + elif isinstance(node, ast.ImportFrom): + modules = [node.module or ""] + else: + continue + if any( + module == prefix or module.startswith(prefix + ".") + for module in modules + for prefix in forbidden + ): + offenders.append(filename) + break + assert offenders == [] + + +def test_task_engine_does_not_import_atomic_execution_layer() -> None: + forbidden = "embodichain.lab.sim.atomic_actions" + execution_modules = {"engine", "execution", "runner", "simulation", "session"} + runtime_types = {"AtomicActionEngine", "ExecutionSession", "ExecutionRunner"} + offenders: list[str] = [] + task_root = _GEN_SIM_ROOT / "task_engine" + for path in task_root.rglob("*.py"): + tree = ast.parse(path.read_text(encoding="utf-8"), filename=str(path)) + for node in ast.walk(tree): + if isinstance(node, ast.Import): + modules = [alias.name for alias in node.names] + elif isinstance(node, ast.ImportFrom): + modules = [node.module or ""] + else: + continue + atomic_imports = [ + module + for module in modules + if module == forbidden or module.startswith(forbidden + ".") + ] + in_adapter = path.is_relative_to(task_root / "_task_program") + if atomic_imports and ( + not in_adapter + or any( + module.rsplit(".", 1)[-1] in execution_modules + for module in atomic_imports + ) + ): + offenders.append(path.relative_to(task_root).as_posix()) + break + # Goal types and factory annotations are allowed, private runtimes are not. + assert not any( + isinstance(node, ast.Call) + and isinstance(node.func, ast.Name) + and node.func.id in runtime_types + for node in ast.walk(tree) + ), path + assert offenders == [] + + +def test_configured_pick_and_transport_lowerers_only_bind_targets() -> None: + services = _GEN_SIM_ROOT / "task_engine/_task_program/services.py" + source = services.read_text(encoding="utf-8") + tree = ast.parse(source) + classes = {node.name: node for node in tree.body if isinstance(node, ast.ClassDef)} + for name in ("_PickLowerer", "_MoveHeldObjectLowerer"): + forbidden_calls = { + node.func.attr + for node in ast.walk(classes[name]) + if isinstance(node, ast.Call) + and isinstance(node.func, ast.Attribute) + and node.func.attr + in {"compute_ik", "compute_batch_ik", "plan", "generate", "step"} + } + assert forbidden_calls == set() + assert "simulation.pick_release_safe" not in source + assert "simulation.move_held_object_upright" not in source + + +def test_gensim_task_recipes_do_not_solve_live_ik() -> None: + for path in (_GEN_SIM_ROOT / "task_engine").rglob("*.py"): + tree = ast.parse(path.read_text(encoding="utf-8")) + assert not any( + isinstance(node, ast.Call) + and isinstance(node.func, ast.Attribute) + and node.func.attr in {"compute_ik", "compute_batch_ik"} + for node in ast.walk(tree) + ), path + + +def test_default_task_runtime_has_no_legacy_physical_executor() -> None: + task_root = _GEN_SIM_ROOT / "task_engine" + default_runtime = ( + task_root / "workflow.py", + task_root / "_bundle_runner.py", + task_root / "semantic_planner.py", + task_root / "task_program_bundle.py", + task_root / "orchestration/coordinator.py", + ) + forbidden_names = { + "ActionAgent", + "ActionGrounder", + "AtomicActionAdapter", + "ProgramExecutor", + } + offenders = { + path.relative_to(task_root).as_posix(): sorted( + forbidden_names.intersection( + node.id + for node in ast.walk( + ast.parse(path.read_text(encoding="utf-8"), filename=str(path)) + ) + if isinstance(node, ast.Name) + ) + ) + for path in default_runtime + } + assert {path: names for path, names in offenders.items() if names} == {} + + +def test_cross_engine_owners_are_explicit() -> None: + assert TaskAgent.__module__ == "embodichain.gen_sim.task_engine.agent" + assert SceneAdapter.__module__.startswith( + "embodichain.gen_sim.task_engine.orchestration" + ) + assert TaskEngineCoordinator.__module__.startswith( + "embodichain.gen_sim.task_engine.orchestration" + ) + + +def test_task_engine_owns_its_module_entry_point() -> None: + assert task_engine_main.main is task_engine_cli.main + + +def test_task_engine_artifacts_have_no_legacy_grounded_plan_boundary() -> None: + assert "grounded_task_plan" not in { + field.name for field in fields(TaskEngineArtifactPaths) + } + assert "grounded_task_plan" not in signature(write_task_engine_artifacts).parameters + + +def test_legacy_cross_engine_packages_are_deleted() -> None: + assert not any((_GEN_SIM_ROOT / "action_engine").rglob("*.py")) + assert not (_GEN_SIM_ROOT / "scene_bridge").exists() + assert not (_GEN_SIM_ROOT / "collaboration").exists() diff --git a/tests/gen_sim/task_engine/orchestration/test_coordinator_cli.py b/tests/gen_sim/task_engine/orchestration/test_coordinator_cli.py new file mode 100644 index 000000000..c490907b4 --- /dev/null +++ b/tests/gen_sim/task_engine/orchestration/test_coordinator_cli.py @@ -0,0 +1,1133 @@ +# ---------------------------------------------------------------------------- +# 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 +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from embodichain.gen_sim.task_engine import cli +from embodichain.gen_sim.task_engine import _bundle_runner as bundle_runner +from embodichain.gen_sim.task_engine.orchestration.artifacts import ( + ArtifactTransaction, +) +from embodichain.gen_sim.task_engine.orchestration.contracts import ( + BINDING_REPORT_SCHEMA, + ROLE_BINDINGS_SCHEMA, + SCENE_MANIFEST_SCHEMA, + SCENE_REQUEST_SCHEMA, + SUCCESS_SPEC_SCHEMA, + TASK_CANDIDATE_SET_SCHEMA, + TASK_DRAFT_SCHEMA, + canonical_hash, +) +from embodichain.gen_sim.task_engine.orchestration.coordinator import ( + TaskEngineCoordinator, +) +from embodichain.gen_sim.task_engine.orchestration.scene_adapter import ( + SceneAdaptation, +) +from embodichain.gen_sim.task_engine.orchestration.scene_source import SceneSourceRef +from embodichain.gen_sim.task_engine.orchestration.source_scene import PreparedScene +from embodichain.gen_sim.task_engine.semantic_planner import ( + UnsupportedSemanticCapabilityError, +) +from embodichain.gen_sim.task_engine.task_program_bundle import ( + TaskProgramBundlePaths, +) + +_UPRIGHT_CAN_INSTRUCTION = "test-instruction" + + +def _candidate_set() -> dict: + selector = { + "kind": "scene_ref", + "step_id": "", + "reference": "red can", + "quantifier": "one", + "count": 0, + } + none_selector = { + "kind": "none", + "step_id": "", + "reference": "", + "quantifier": "one", + "count": 0, + } + step = { + "id": "upright", + "task_type": "E2", + "object": selector, + "target": none_selector, + "relation": "none", + "required_arm": "auto", + "transfer_arm": "none", + "receive_arm": "none", + "orientation_goal": "upright", + "target_state": "none", + "target_setting": 0, + "layout": "none", + "axis": "none", + "direction": "none", + "terminal_behavior": "none", + "depends_on": [], + } + draft = { + "schema_version": TASK_DRAFT_SCHEMA, + "task_id": "upright_can", + "instruction": _UPRIGHT_CAN_INSTRUCTION, + "steps": [step], + } + candidate = { + "candidate_id": "candidate_01", + "draft": draft, + "scene_request": { + "schema_version": SCENE_REQUEST_SCHEMA, + "task_id": "upright_can", + "references": [ + { + "reference_id": "upright.object", + "step_id": "upright", + "role": "object", + "reference": "red can", + "quantifier": "one", + "count": 0, + "source_structure": "rigid_object", + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {}, + } + ], + }, + "success_spec": { + "schema_version": SUCCESS_SPEC_SCHEMA, + "task_id": "upright_can", + "op": "all", + "terms": [{"step_id": "upright", "type": "object_upright"}], + }, + "semantic_hash": canonical_hash([step]), + "vote_count": 1, + "attempts": 1, + "normalizations": [], + } + return { + "schema_version": TASK_CANDIDATE_SET_SCHEMA, + "task_id": "upright_can", + "instruction": _UPRIGHT_CAN_INSTRUCTION, + "candidates": [candidate], + "requested_candidate_count": 1, + "valid_response_count": 1, + "errors": [], + } + + +def _prepared_scene(tmp_path: Path) -> PreparedScene: + scene_path = tmp_path / "scene_config.json" + scene_path.write_text("{}", encoding="utf-8") + scene_object = { + "uid": "red_can", + "source_uid": "red_can", + "role": "rigid_object", + "name": "red can", + "description": "A red can.", + "category": "can", + "color": "red", + "position": [0.0, 0.0, 0.5], + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {}, + } + return PreparedScene( + source_config_path=scene_path, + scene_dir=tmp_path, + planner_objects=(scene_object,), + background=(), + rigid_objects=(), + articulations=(), + uid_map={"red_can": "red_can"}, + table_top_z=None, + z_rotation_degrees=0.0, + body_scale_policy="preserve", + body_scale=(1.0, 1.0, 1.0), + asset_hashes={}, + ) + + +def _adaptation(tmp_path: Path, *, status: str = "bound") -> SceneAdaptation: + candidates = _candidate_set() + candidate = candidates["candidates"][0] + selected_id = candidate["candidate_id"] if status == "bound" else "" + role_bindings = ( + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "upright_can", + "candidate_id": "candidate_01", + "reference_bindings": {"upright.object": ["red_can"]}, + "role_bindings": {}, + } + if status == "bound" + else None + ) + return SceneAdaptation( + scene_manifest={ + "schema_version": SCENE_MANIFEST_SCHEMA, + "scene_id": "scene", + "source_format": "test", + "robot_profile": "dual_franka", + "objects": [ + { + "uid": "red_can", + "role": "rigid_object", + "name": "red can", + "description": "A red can.", + "category": "can", + "color": "red", + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {}, + } + ], + }, + role_bindings=role_bindings, + binding_report={ + "schema_version": BINDING_REPORT_SCHEMA, + "task_id": "upright_can", + "status": status, + "selected_candidate_id": selected_id, + "selection_reason": "test", + "candidates": [ + { + "candidate_id": "candidate_01", + "semantic_hash": candidate["semantic_hash"], + "status": "resolved" if status == "bound" else status, + "references": [ + { + "reference_id": "upright.object", + "status": ( + "resolved" if status == "bound" else "ambiguous" + ), + "confidence": 1.0, + "candidate_uids": ["red_can"], + "selected_uids": (["red_can"] if status == "bound" else []), + "reasons": [], + } + ], + "reasons": [], + } + ], + }, + selected_candidate=deepcopy(candidate) if status == "bound" else None, + prepared_scene=_prepared_scene(tmp_path), + source_config_path=tmp_path / "scene_config.json", + conservative_scene_graph={ + "schema_version": "embodichain.conservative-scene-graph/v1", + "scene_id": "scene", + "nodes": [ + { + "uid": "red_can", + "parent_uid": "unknown", + "parent_relation": "unknown", + "orientation": "unknown", + "source": "test", + } + ], + "relations": [], + }, + ) + + +def test_artifact_transaction_rolls_back_and_preserves_existing_output( + tmp_path: Path, +) -> None: + output = tmp_path / "bundle" + output.mkdir() + (output / "kept.txt").write_text("old", encoding="utf-8") + + with pytest.raises(RuntimeError, match="fail"): + with ArtifactTransaction(output, overwrite=True) as transaction: + assert transaction.staging_dir is not None + (transaction.staging_dir / "partial.txt").write_text( + "partial", encoding="utf-8" + ) + raise RuntimeError("fail before commit") + + assert (output / "kept.txt").read_text(encoding="utf-8") == "old" + assert not (output / "partial.txt").exists() + + +def test_artifact_transaction_relocates_paths_in_json_mapping_keys( + tmp_path: Path, +) -> None: + output = tmp_path / "bundle" + with ArtifactTransaction(output) as transaction: + assert transaction.staging_dir is not None + staging = transaction.staging_dir.resolve().as_posix() + (transaction.staging_dir / "manifest.json").write_text( + json.dumps( + { + "config_path": f"{staging}/scene/scene_config.json", + "asset_sha256": {f"{staging}/scene/asset.usdc": "hash"}, + } + ), + encoding="utf-8", + ) + transaction.commit() + + manifest = json.loads((output / "manifest.json").read_text(encoding="utf-8")) + assert ( + manifest["config_path"] + == f"{output.resolve().as_posix()}/scene/scene_config.json" + ) + assert list(manifest["asset_sha256"]) == [ + f"{output.resolve().as_posix()}/scene/asset.usdc" + ] + + +def test_prepare_rejects_output_overlapping_read_only_source(tmp_path: Path) -> None: + source = tmp_path / "gym_project" + source.mkdir() + coordinator = TaskEngineCoordinator( + task_agent=object(), + scene_adapter=SimpleNamespace(robot_profile="franka"), + ) + + with pytest.raises(ValueError, match="must not overlap"): + coordinator.prepare( + "task", + "Pick up the object.", + source, + source / "task_run", + overwrite=True, + ) + + +def test_unbound_prepare_publishes_only_audit_artifacts(tmp_path: Path) -> None: + candidates = _candidate_set() + adaptation = _adaptation(tmp_path, status="ambiguous") + task_agent = SimpleNamespace(generate=lambda *args, **kwargs: candidates) + scene_adapter = SimpleNamespace( + robot_profile="franka", + adapt=lambda *args, **kwargs: adaptation, + ) + coordinator = TaskEngineCoordinator( + task_agent=task_agent, + scene_adapter=scene_adapter, + semantic_planner=SimpleNamespace( + plan=lambda *_args, **_kwargs: pytest.fail( + "Semantic Task Planner must not run" + ) + ), + ) + + result = coordinator.prepare( + "upright_can", + _UPRIGHT_CAN_INSTRUCTION, + tmp_path / "scene_config.json", + tmp_path / "bundle", + candidate_count=1, + ) + + assert result.status == "ambiguous" + assert (result.output_dir / "task_candidate_set.json").is_file() + assert (result.output_dir / "binding_report.json").is_file() + assert not (result.output_dir / "scene_manifest.json").exists() + assert not (result.output_dir / "role_bindings.json").exists() + assert not (result.output_dir / "grounded_task_plan.json").exists() + assert not (result.output_dir / "semantic_task_graph.json").exists() + assert not (result.output_dir / "task_program_deployment.yaml").exists() + + +def test_prepare_reuses_precomputed_candidates_without_rerunning_task_agent( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + adaptation = _adaptation(tmp_path, status="ambiguous") + task_agent = SimpleNamespace( + generate=lambda *args, **kwargs: pytest.fail("Task Agent must not rerun") + ) + coordinator = TaskEngineCoordinator( + task_agent=task_agent, + scene_adapter=SimpleNamespace( + robot_profile="franka", + adapt=lambda *args, **kwargs: adaptation, + ), + ) + + result = coordinator.prepare( + "upright_can", + _UPRIGHT_CAN_INSTRUCTION, + tmp_path / "scene_config.json", + tmp_path / "candidate-reuse", + candidate_set=candidates, + force_most_likely=True, + ) + + assert result.status == "ambiguous" + assert result.candidate_set == candidates + + +def test_prepare_inherits_adapter_robot_profile_for_raw_scene_path( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + adaptation = _adaptation(tmp_path, status="ambiguous") + captured: dict[str, object] = {} + + def adapt(_candidates, source, **_kwargs): + captured["source"] = source + return adaptation + + coordinator = TaskEngineCoordinator( + task_agent=SimpleNamespace( + generate=lambda *args, **kwargs: pytest.fail("Task Agent must not rerun") + ), + scene_adapter=SimpleNamespace(robot_profile="ur10", adapt=adapt), + ) + + result = coordinator.prepare( + "upright_can", + _UPRIGHT_CAN_INSTRUCTION, + tmp_path / "scene_config.json", + tmp_path / "ur10-bundle", + candidate_set=candidates, + ) + + assert result.status == "ambiguous" + assert isinstance(captured["source"], SceneSourceRef) + assert captured["source"].robot_profile == "ur10" + + +def test_bound_prepare_publishes_semantic_task_program_bundle( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + candidates = _candidate_set() + adaptation = _adaptation(tmp_path) + task_agent = SimpleNamespace(generate=lambda *args, **kwargs: candidates) + scene_adapter = SimpleNamespace( + robot_profile="franka", + adapt=lambda *args, **kwargs: adaptation, + ) + graph = { + "schema_version": "semantic_task_graph/v1", + "task_id": "upright_can", + "instruction": _UPRIGHT_CAN_INSTRUCTION, + "planner_route": "offline", + "integration_fingerprint": "0" * 64, + "targets": {}, + "nodes": [ + { + "id": "upright__call_01", + "call": { + "kind": "pick", + "object": "red_can", + "resources": {"primary": "left"}, + }, + "depends_on": [], + "task_instance_id": "upright", + "task_type": "E2", + "role": "primary", + } + ], + "task_groups": [ + { + "id": "upright", + "task_type": "E2", + "node_ids": ["upright__call_01"], + "depends_on": [], + "success": {"type": "object_upright"}, + } + ], + "success": {"kind": "all_task_groups"}, + } + generated_calls: list[dict[str, object]] = [] + + def generate_bundle(planned_graph, _scene, output, **kwargs): + generated_calls.append({"graph": planned_graph, **kwargs}) + root = Path(output) + paths = TaskProgramBundlePaths( + root=root, + deployment=root / "task_program_deployment.yaml", + program=root / "task_program/program.yaml", + integration=root / "task_program/integration.yaml", + scene=root / "components/scene.yaml", + embodiment=root / "components/embodiment.yaml", + execution_policy=root / "components/execution_policy.yaml", + semantic_task_graph=root / "semantic_task_graph.json", + integration_fingerprint=root / "integration_fingerprint.json", + ) + for path in ( + paths.deployment, + paths.program, + paths.integration, + paths.scene, + paths.embodiment, + paths.execution_policy, + paths.semantic_task_graph, + paths.integration_fingerprint, + ): + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text("{}\n", encoding="utf-8") + return deepcopy(planned_graph), paths + + monkeypatch.setattr( + "embodichain.gen_sim.task_engine.orchestration.coordinator.generate_task_program_bundle", + generate_bundle, + ) + + result = TaskEngineCoordinator( + task_agent=task_agent, + scene_adapter=scene_adapter, + semantic_planner=SimpleNamespace(plan=lambda *_args, **_kwargs: graph), + ).prepare( + "upright_can", + _UPRIGHT_CAN_INSTRUCTION, + tmp_path / "scene_config.json", + tmp_path / "bundle", + candidate_count=1, + ) + + assert result.bound + assert generated_calls == [ + { + "graph": graph, + "robot_profile": "dual_franka", + "max_episodes": None, + "max_episode_steps": None, + } + ] + assert result.semantic_task_graph == graph + assert (result.output_dir / "semantic_task_graph.json").is_file() + assert (result.output_dir / "task_program_deployment.yaml").is_file() + assert not (result.output_dir / "grounded_task_plan.json").exists() + assert not (result.output_dir / "seed_task_graph.json").exists() + + +def test_prepare_publishes_semantic_planning_failure_context( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + adaptation = _adaptation(tmp_path) + output = tmp_path / "failed-bundle" + output.mkdir() + (output / "stale.txt").write_text("old", encoding="utf-8") + + def unsupported(*_args, **_kwargs): + raise UnsupportedSemanticCapabilityError( + "Task type E2 has no phase-one Semantic Call route." + ) + + result = TaskEngineCoordinator( + task_agent=SimpleNamespace(generate=lambda *args, **kwargs: candidates), + scene_adapter=SimpleNamespace( + robot_profile="franka", + adapt=lambda *args, **kwargs: adaptation, + ), + semantic_planner=SimpleNamespace(plan=unsupported), + ).prepare( + "upright_can", + _UPRIGHT_CAN_INSTRUCTION, + tmp_path / "scene_config.json", + output, + candidate_count=1, + overwrite=True, + ) + + assert result.status == "planning_failed" + assert not result.bound + assert result.artifacts.preparation_failure.is_file() + assert not (result.output_dir / "stale.txt").exists() + failure = json.loads( + result.artifacts.preparation_failure.read_text(encoding="utf-8") + ) + assert failure["schema_version"] == "semantic_task_preparation_failure/v1" + assert failure["task_id"] == "upright_can" + assert failure["selected_candidate_id"] == "candidate_01" + assert failure["status"] == "unsupported_semantic_capability" + assert failure["attempts"] == [ + { + "candidate_id": "candidate_01", + "planner_route": "offline", + "status": "failed", + "error": { + "type": "UnsupportedSemanticCapabilityError", + "message": "Task type E2 has no phase-one Semantic Call route.", + }, + } + ] + + +def test_private_bundle_runner_forwards_arguments_without_leaking_sys_argv( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + bundle = tmp_path / "bundle" + bundle.mkdir() + execution_output = tmp_path / "execution" + captured: dict[str, object] = {} + + def fake_execute_bundle(path, forwarded, *, execution_output): + captured.update( + { + "path": path, + "forwarded": list(forwarded), + "execution_output": execution_output, + } + ) + return 7 + + monkeypatch.setattr(bundle_runner, "execute_bundle", fake_execute_bundle) + import sys + + original = sys.argv + assert ( + bundle_runner.main( + [ + "--bundle", + str(bundle), + "--execution-output", + str(execution_output), + "--seed", + "7", + ] + ) + == 7 + ) + + assert sys.argv is original + assert captured == { + "path": str(bundle), + "forwarded": ["--seed", "7"], + "execution_output": str(execution_output), + } + + +@pytest.mark.parametrize( + ("mode", "image", "scene", "edit"), + [ + ("image", "input.png", None, None), + ("image-edit", "input.png", None, "move the cup left"), + ("scene", None, "gym_project", None), + ("scene-edit", None, "gym_project", "move the cup left"), + ], +) +def test_unified_cli_accepts_exactly_four_modes( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], + mode: str, + image: str | None, + scene: str | None, + edit: str | None, +) -> None: + captured = {} + + class FakeWorkflow: + def __init__(self, **_kwargs) -> None: + pass + + def run(self, request, **kwargs): + captured["request"] = request + captured["kwargs"] = kwargs + output = Path(request["output_dir"]) + return SimpleNamespace( + status="succeeded", + succeeded=True, + failure_class=None, + output_dir=output, + manifest_path=output / "run_manifest.json", + final_bundle=output / "final" / "bundle", + ) + + monkeypatch.setattr(cli, "SceneAdapter", lambda **_kwargs: object()) + monkeypatch.setattr(cli, "TaskEngineWorkflow", FakeWorkflow) + arguments = [ + "--mode", + mode, + "--task-id", + "task", + "--instruction", + "place the cup", + "--output-root", + str(tmp_path / "history"), + "--base-seed", + "9", + ] + if image is not None: + arguments.extend(["--image", str(tmp_path / image)]) + if scene is not None: + arguments.extend(["--scene", str(tmp_path / scene)]) + if edit is not None: + arguments.extend(["--scene-edit", edit]) + if mode == "image": + arguments.append("--dataset_saving") + + assert cli.main(arguments) == 0 + + request = captured["request"] + assert request["image_path"] == (None if image is None else str(tmp_path / image)) + assert request["gym_project"] == (None if scene is None else str(tmp_path / scene)) + assert request["scene_edit_prompt"] == edit + assert captured["kwargs"]["base_seed"] == 9 + assert captured["kwargs"]["dataset_saving"] is (mode == "image") + assert captured["kwargs"]["failure_policy"] == "stop" + assert captured["kwargs"]["execute"] is True + payload = json.loads(capsys.readouterr().out) + assert payload["status"] == "succeeded" + assert payload["run_id"].replace("_", "").isdigit() + assert len(payload["run_id"]) == 15 + assert Path(payload["output_dir"]).parent == tmp_path / "history" + + +@pytest.mark.parametrize( + "lower_layer_option", + [ + ["--planner-mode", "ik_interp"], + ["--ik-solver", "pytorch"], + ["--show-grasp-poses"], + ], +) +def test_unified_cli_rejects_lower_layer_execution_options( + tmp_path: Path, + lower_layer_option: list[str], +) -> None: + with pytest.raises(SystemExit, match="2"): + cli.main( + [ + "prepare", + "--mode", + "image", + "--task-id", + "task", + "--instruction", + "place the cup", + "--image", + str(tmp_path / "input.png"), + "--output-root", + str(tmp_path / "history"), + *lower_layer_option, + ] + ) + + +def test_unified_cli_reuses_history_root_without_modifying_prior_scene( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + history = tmp_path / "task1008" + source = ( + history + / "20260820_105939" + / "attempts" + / "scene_0001" + / "scene_revision" + / "scene_export" + ) + source.mkdir(parents=True) + marker = source / "scene_config.json" + marker.write_text('{"source": "unchanged"}\n', encoding="utf-8") + captured = {} + + class FakeWorkflow: + def __init__(self, **_kwargs) -> None: + pass + + def run(self, request, **_kwargs): + captured["request"] = request + output = Path(request["output_dir"]) + return SimpleNamespace( + status="succeeded", + succeeded=True, + failure_class=None, + output_dir=output, + manifest_path=output / "run_manifest.json", + final_bundle=output / "final" / "bundle", + ) + + monkeypatch.setattr(cli, "SceneAdapter", lambda **_kwargs: object()) + monkeypatch.setattr(cli, "TaskEngineWorkflow", FakeWorkflow) + + assert ( + cli.main( + [ + "--mode", + "scene", + "--task-id", + "task1008", + "--scene", + str(source), + "--instruction", + "place the cup on the book", + "--output-root", + str(history), + ] + ) + == 0 + ) + + output_dir = Path(captured["request"]["output_dir"]) + assert output_dir.parent == history + assert output_dir != source + assert marker.read_text(encoding="utf-8") == '{"source": "unchanged"}\n' + assert list(history.glob(".*.reserve")) == [] + + +def test_unified_cli_rejects_history_root_inside_source_before_reservation( + tmp_path: Path, +) -> None: + source = tmp_path / "scene_export" + source.mkdir() + output_root = source / "new_runs" + + with pytest.raises(ValueError, match="read-only source"): + cli.main( + [ + "--mode", + "scene", + "--task-id", + "task", + "--scene", + str(source), + "--instruction", + "place the cup", + "--output-root", + str(output_root), + ] + ) + + assert not output_root.exists() + + +def test_unified_cli_rejects_mode_input_mismatch(tmp_path: Path) -> None: + with pytest.raises(SystemExit, match="2"): + cli.main( + [ + "--mode", + "image", + "--task-id", + "task", + "--instruction", + "place the cup", + "--image", + str(tmp_path / "input.png"), + "--scene", + str(tmp_path / "scene"), + "--output-root", + str(tmp_path / "history"), + ] + ) + + +def test_public_cli_exposes_prepare_run_and_run_all_modes() -> None: + parser = cli.build_parser() + help_text = parser.format_help() + assert "prepare" in help_text + assert "run-all" in help_text + assert "run" in help_text + assert "--overwrite" not in help_text + assert "--run-after-prepare" not in help_text + arguments = parser.parse_args( + [ + "prepare", + "--mode", + "image", + "--task-id", + "task", + "--instruction", + "place the cup", + "--image", + "input.png", + "--output-root", + "history", + "--dataset_saving", + ] + ) + assert arguments.command == "prepare" + assert arguments.dataset_saving is True + assert arguments.failure_policy == "stop" + assert not hasattr(arguments, "planner_mode") + assert not hasattr(arguments, "ik_solver") + assert not hasattr(arguments, "show_grasp_poses") + + +def test_run_all_cli_accepts_runtime_window_option() -> None: + arguments = cli.build_parser().parse_args( + [ + "run-all", + "--mode", + "scene", + "--task-id", + "task", + "--instruction", + "move the tray", + "--scene", + "scene", + "--output-root", + "history", + "--open-window", + ] + ) + + assert arguments.open_window is True + + +def test_run_all_cli_forwards_open_window( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + captured = {} + + class FakeWorkflow: + def __init__(self, **_kwargs) -> None: + pass + + def run(self, request, **kwargs): + captured.update(kwargs) + output = Path(request["output_dir"]) + return SimpleNamespace( + status="succeeded", + succeeded=True, + failure_class=None, + output_dir=output, + manifest_path=output / "run_manifest.json", + final_bundle=output / "final" / "bundle", + ) + + monkeypatch.setattr(cli, "SceneAdapter", lambda **_kwargs: object()) + monkeypatch.setattr(cli, "TaskEngineWorkflow", FakeWorkflow) + + result = cli.main( + [ + "run-all", + "--mode", + "image", + "--task-id", + "task", + "--instruction", + "place the cup", + "--image", + str(tmp_path / "input.png"), + "--output-root", + str(tmp_path / "history"), + "--open-window", + ] + ) + + assert result == 0 + assert captured["execute"] is True + assert captured["open_window"] is True + + +def test_prepare_cli_stops_before_simulator_execution( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], +) -> None: + captured = {} + + class FakeWorkflow: + def __init__(self, **_kwargs) -> None: + pass + + def run(self, request, **kwargs): + captured.update(kwargs) + output = Path(request["output_dir"]) + return SimpleNamespace( + status="prepared", + succeeded=False, + failure_class=None, + output_dir=output, + manifest_path=output / "run_manifest.json", + final_bundle=output / "final" / "bundle", + ) + + monkeypatch.setattr(cli, "SceneAdapter", lambda **_kwargs: object()) + monkeypatch.setattr(cli, "TaskEngineWorkflow", FakeWorkflow) + + result = cli.main( + [ + "prepare", + "--mode", + "image", + "--task-id", + "task", + "--instruction", + "place the cup", + "--image", + str(tmp_path / "input.png"), + "--output-root", + str(tmp_path / "history"), + ] + ) + + assert result == 0 + assert captured["execute"] is False + output = capsys.readouterr() + assert json.loads(output.out)["video_paths"] == [] + assert output.err == "" + + +@pytest.mark.parametrize("succeeded", [True, False]) +def test_run_all_cli_reports_published_videos_for_every_attempt( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], + succeeded: bool, +) -> None: + history = tmp_path / "history" + prior_video = history / "old_run" / "videos" / "previous.mp4" + prior_video.parent.mkdir(parents=True) + prior_video.write_bytes(b"previous video") + relative_paths = [ + Path("attempts/scene_0001/action_attempts/action_0001/videos/episode_0.mp4"), + Path("attempts/scene_0001/action_attempts/action_0002/videos/episode_0.mp4"), + Path("attempts/scene_0002/action_attempts/action_0001/videos/episode_0.mp4"), + ] + + class FakeWorkflow: + def __init__(self, **_kwargs) -> None: + pass + + def run(self, request, **_kwargs): + with ArtifactTransaction(request["output_dir"]) as transaction: + staging = transaction.staging_dir + assert staging is not None + for relative_path in reversed(relative_paths): + video = staging / relative_path + video.parent.mkdir(parents=True) + video.write_bytes(b"video") + (staging / "videos" / "directory.mp4").mkdir(parents=True) + (staging / "not_a_recording.mp4").write_bytes(b"unrelated asset") + published = transaction.commit() + assert not staging.exists() + return SimpleNamespace( + status="succeeded" if succeeded else "failed", + succeeded=succeeded, + failure_class=None if succeeded else "action_execution", + output_dir=published, + manifest_path=published / "run_manifest.json", + final_bundle=published / "final" / "bundle" if succeeded else None, + ) + + monkeypatch.setattr(cli, "SceneAdapter", lambda **_kwargs: object()) + monkeypatch.setattr(cli, "TaskEngineWorkflow", FakeWorkflow) + result = cli.main( + [ + "run-all", + "--mode", + "image", + "--task-id", + "task", + "--instruction", + "place the cup", + "--image", + str(tmp_path / "input.png"), + "--output-root", + str(history), + ] + ) + + assert result == (0 if succeeded else 2) + output = capsys.readouterr() + payload = json.loads(output.out) + expected = [ + (Path(payload["output_dir"]) / relative_path).as_posix() + for relative_path in relative_paths + ] + assert payload["video_paths"] == expected + assert all(Path(path).is_absolute() and Path(path).is_file() for path in expected) + assert output.err.splitlines() == [ + f"[Task Engine] Video saved: {path}" for path in expected + ] + assert ".staging-" not in output.out + output.err + + +@pytest.mark.parametrize("has_video", [True, False]) +def test_run_cli_executes_an_existing_bundle( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], + has_video: bool, +) -> None: + bundle = tmp_path / "bundle" + bundle.mkdir() + + class Executor: + def __call__(self, _bundle, output, **kwargs): + Path(output).mkdir() + if has_video: + videos = Path(output) / "videos" + videos.mkdir() + (videos / "episode_0.mp4").write_bytes(b"video") + assert kwargs["num_envs"] == 2 + assert kwargs["failure_policy"] == "continue" + assert kwargs["open_window"] is True + return { + "status": "failed", + "environments": [ + {"success": True}, + {"success": False}, + ], + } + + monkeypatch.setattr(cli, "SubprocessActionExecutor", Executor) + + result = cli.main( + [ + "run", + "--bundle", + str(bundle), + "--output-root", + str(tmp_path / "history"), + "--num-envs", + "2", + "--failure-policy", + "continue", + "--open-window", + ] + ) + + assert result == 0 + output = capsys.readouterr() + payload = json.loads(output.out) + if has_video: + expected = (Path(payload["output_dir"]) / "videos" / "episode_0.mp4").as_posix() + assert payload["video_paths"] == [expected] + assert output.err == f"[Task Engine] Video saved: {expected}\n" + else: + assert payload["video_paths"] == [] + assert output.err == "[Task Engine] No video files generated for this run.\n" + + +def test_video_listing_error_is_diagnostic_only( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], +) -> None: + def denied_glob(self, pattern): + raise PermissionError("recordings are unreadable") + + monkeypatch.setattr(Path, "glob", denied_glob) + assert cli._report_saved_videos(tmp_path) == [] + output = capsys.readouterr() + assert output.out == "" + assert output.err == ( + "[Task Engine] Unable to list saved videos: recordings are unreadable\n" + ) diff --git a/tests/gen_sim/task_engine/orchestration/test_grounding.py b/tests/gen_sim/task_engine/orchestration/test_grounding.py new file mode 100644 index 000000000..83c4ff013 --- /dev/null +++ b/tests/gen_sim/task_engine/orchestration/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.task_engine.orchestration.grounding import ( + ground_scene_references, +) +from embodichain.gen_sim.task_engine.orchestration.scene_inventory 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/task_engine/orchestration/test_legacy_scene.py b/tests/gen_sim/task_engine/orchestration/test_legacy_scene.py new file mode 100644 index 000000000..2a7fc146d --- /dev/null +++ b/tests/gen_sim/task_engine/orchestration/test_legacy_scene.py @@ -0,0 +1,164 @@ +# ---------------------------------------------------------------------------- +# 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 json +from pathlib import Path + +import trimesh + +from embodichain.gen_sim.task_engine.orchestration.source_scene import prepare_scene +from embodichain.gen_sim.task_engine.orchestration.legacy_scene import ( + convert_legacy_gym_project, + restore_locked_scene_entities, +) +from embodichain.gen_sim.task_engine.orchestration.scene_source import ( + fingerprint_scene_source, + scene_revision_id, +) +from embodichain.gen_sim.task_engine.scene import build_conservative_scene_graph + + +def _legacy_project(tmp_path: Path) -> Path: + project = tmp_path / "legacy" + assets = project / "assets" + assets.mkdir(parents=True) + trimesh.creation.box(extents=[1.0, 1.0, 0.1]).export( + assets / "table.glb", file_type="glb" + ) + trimesh.creation.cylinder(radius=0.03, height=0.12).export( + assets / "can.glb", file_type="glb" + ) + trimesh.creation.box(extents=[0.3, 0.2, 0.4]).export( + assets / "cabinet.glb", file_type="glb" + ) + (assets / "cabinet.urdf").write_text( + '' + '\n', + encoding="utf-8", + ) + config = { + "background": [ + { + "uid": "table_0", + "name": "table", + "description": "A work table.", + "category": "table", + "shape": {"shape_type": "Mesh", "fpath": "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": "can_0", + "name": "red can", + "description": "A red can.", + "category": "can", + "shape": {"shape_type": "Mesh", "fpath": "assets/can.glb"}, + "init_pos": [0.0, 0.1, 0.2], + "init_rot": [0.0, 0.0, 0.0], + "body_scale": [1.0, 1.0, 1.0], + } + ], + "articulation": [ + { + "uid": "cabinet_0", + "name": "cabinet", + "description": "A fixed cabinet.", + "category": "cabinet", + "fpath": "assets/cabinet.urdf", + "init_pos": [0.5, 0.0, 0.0], + "init_rot": [0.0, 0.0, 0.0], + "body_scale": [1.0, 1.0, 1.0], + } + ], + } + (project / "gym_config.json").write_text(json.dumps(config), encoding="utf-8") + return project + + +def test_legacy_conversion_is_read_only_and_restores_locked_articulation( + tmp_path: Path, +) -> None: + project = _legacy_project(tmp_path) + original = fingerprint_scene_source(project) + + revision = convert_legacy_gym_project(project, tmp_path / "revision") + converted = json.loads(revision.scene_config_path.read_text(encoding="utf-8")) + manifest = json.loads(revision.manifest_path.read_text(encoding="utf-8")) + + assert fingerprint_scene_source(project) == original + assert converted["format"] == "embodichain.scene-export/v1" + assert converted["background"][0]["uid"] == "table" + assert converted["rigid_object"][0]["uid"] == "can" + assert converted["articulation"][0]["uid"] == "cabinet" + assert manifest["audit_hierarchy"] == "unknown" + assert manifest["operational_hierarchy"] == "assumed_on_table" + assert set(revision.locked_entity_uids) == {"table", "cabinet"} + + converted["articulation"] = [] + revision.scene_config_path.write_text(json.dumps(converted), encoding="utf-8") + restore_locked_scene_entities(revision.output_root) + restored = json.loads(revision.scene_config_path.read_text(encoding="utf-8")) + + assert restored["articulation"][0]["uid"] == "cabinet" + assert Path(restored["articulation"][0]["fpath"]).is_file() + assert fingerprint_scene_source(project) == original + + +def test_legacy_conversion_separates_audit_and_operational_hierarchy( + tmp_path: Path, +) -> None: + revision = convert_legacy_gym_project( + _legacy_project(tmp_path), + tmp_path / "revision", + ) + + operational = json.loads(revision.scene_graph_path.read_text(encoding="utf-8")) + conservative = build_conservative_scene_graph( + prepare_scene(revision.scene_config_path), + scene_id="legacy-scene", + ) + + operational_can = next( + node for node in operational["nodes"] if node["object_id"] == "can" + ) + conservative_can = next( + node for node in conservative["nodes"] if node["uid"] == "can" + ) + assert operational_can["parent_id"] == "table" + assert operational_can["parent_relation"] == "on" + assert conservative_can["parent_uid"] == "unknown" + assert conservative_can["parent_relation"] == "unknown" + assert conservative_can["source"] == "conservative_import" + + +def test_scene_identity_covers_transitive_urdf_meshes(tmp_path: Path) -> None: + project = _legacy_project(tmp_path) + original_fingerprint = fingerprint_scene_source(project) + original_revision = scene_revision_id(project) + + trimesh.creation.box(extents=[0.6, 0.2, 0.4]).export( + project / "assets" / "cabinet.glb", + file_type="glb", + ) + + changed_fingerprint = fingerprint_scene_source(project) + assert changed_fingerprint.asset_sha256 != original_fingerprint.asset_sha256 + assert scene_revision_id(project) != original_revision diff --git a/tests/gen_sim/task_engine/orchestration/test_scene_adapter.py b/tests/gen_sim/task_engine/orchestration/test_scene_adapter.py new file mode 100644 index 000000000..2ae3b749f --- /dev/null +++ b/tests/gen_sim/task_engine/orchestration/test_scene_adapter.py @@ -0,0 +1,780 @@ +# ---------------------------------------------------------------------------- +# 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 +from pathlib import Path + +import pytest + +import embodichain.gen_sim.task_engine.orchestration.scene_adapter as scene_adapter_module +from embodichain.gen_sim.task_engine.contracts import ( + SCENE_REQUEST_SCHEMA, + SUCCESS_SPEC_SCHEMA, + TASK_CANDIDATE_SET_SCHEMA, + TASK_DRAFT_SCHEMA, + canonical_hash, +) +from embodichain.gen_sim.task_engine.orchestration.scene_adapter import ( + SceneAdapter, + SceneAdapterProtocolError, +) +from embodichain.gen_sim.task_engine.orchestration.scene_source import ( + SceneSourceRef, + fingerprint_scene_source, + verify_scene_source_fingerprint, +) +from embodichain.gen_sim.task_engine.agent import ( + derive_scene_request, + derive_success_spec, +) + +_UPRIGHT_CAN_INSTRUCTION = "test-instruction" + + +@pytest.fixture +def scene_export(tmp_path: Path) -> Path: + export = tmp_path / "scene_export" + assets = export / "meshes" + assets.mkdir(parents=True) + for name in ("table", "red_can", "blue_can"): + (assets / f"{name}.glb").write_bytes(f"mesh:{name}".encode()) + config = { + "format": "embodichain.scene-export/v1", + "scene_id": "2026-03-18T10:20:30Z", + "background": [ + { + "uid": "table", + "name": "table", + "description": "A work table.", + "category": "table", + "affordances": ["support_surface"], + "shape": {"shape_type": "Mesh", "fpath": "meshes/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": f"{color}_can", + "name": f"{color} can", + "description": f"A {color} soda can.", + "category": "can", + "attributes": { + "color": color, + "geometry": {"position": [1.0, 2.0, 3.0]}, + }, + "affordances": ["graspable", "orientable", "placeable"], + "initial_state": {"orientation": "fallen"}, + "shape": { + "shape_type": "Mesh", + "fpath": f"meshes/{color}_can.glb", + }, + "init_pos": [0.0, offset, 0.7], + "init_rot": [0.0, 0.0, 90.0], + "body_scale": [1.0, 1.0, 1.0], + } + for color, offset in (("red", 0.2), ("blue", -0.2)) + ], + } + (export / "scene_config.json").write_text(json.dumps(config), encoding="utf-8") + return export + + +def _legacy_gym_project(tmp_path: Path, filename: str) -> Path: + project = tmp_path / filename.removesuffix(".json") + assets = project / "assets" + assets.mkdir(parents=True) + for name in ("table.glb", "red_can.glb", "cabinet.urdf"): + (assets / name).write_bytes(f"asset:{name}".encode()) + config = { + "background": [ + { + "uid": "table_0", + "name": "table", + "description": "A work table.", + "category": "table", + "shape": {"shape_type": "Mesh", "fpath": "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": "red_can_0", + "name": "red can", + "description": "A red soda can.", + "category": "can", + "affordances": ["graspable", "orientable", "placeable"], + "initial_state": {"orientation": "fallen"}, + "shape": { + "shape_type": "Mesh", + "fpath": "assets/red_can.glb", + }, + "init_pos": [0.0, 0.2, 0.7], + "init_rot": [0.0, 0.0, 90.0], + "body_scale": [1.0, 1.0, 1.0], + } + ], + "articulation": [ + { + "uid": "cabinet_0", + "name": "cabinet", + "description": "A fixed articulated cabinet.", + "category": "cabinet", + "fpath": "assets/cabinet.urdf", + "init_pos": [0.4, 0.0, 0.0], + "init_rot": [0.0, 0.0, 0.0], + "body_scale": [1.0, 1.0, 1.0], + } + ], + } + (project / filename).write_text(json.dumps(config), encoding="utf-8") + return project + + +def _selector(reference: str) -> dict: + return { + "kind": "scene_ref", + "step_id": "", + "reference": reference, + "quantifier": "one", + "count": 0, + } + + +def _none_selector() -> dict: + return { + "kind": "none", + "step_id": "", + "reference": "", + "quantifier": "one", + "count": 0, + } + + +def _candidate(candidate_id: str, reference: str, *, votes: int = 1) -> dict: + step = { + "id": "upright", + "task_type": "E2", + "object": _selector(reference), + "target": _none_selector(), + "relation": "none", + "required_arm": "auto", + "transfer_arm": "none", + "receive_arm": "none", + "orientation_goal": "upright", + "target_state": "none", + "target_setting": 0, + "layout": "none", + "axis": "none", + "direction": "none", + "terminal_behavior": "none", + "depends_on": [], + } + draft = { + "schema_version": TASK_DRAFT_SCHEMA, + "task_id": "upright_can", + "instruction": _UPRIGHT_CAN_INSTRUCTION, + "steps": [step], + } + return { + "candidate_id": candidate_id, + "draft": draft, + "scene_request": { + "schema_version": SCENE_REQUEST_SCHEMA, + "task_id": "upright_can", + "references": [ + { + "reference_id": "upright.object", + "step_id": "upright", + "role": "object", + "reference": reference, + "quantifier": "one", + "count": 0, + "source_structure": "rigid_object", + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {}, + } + ], + }, + "success_spec": { + "schema_version": SUCCESS_SPEC_SCHEMA, + "task_id": "upright_can", + "op": "all", + "terms": [{"step_id": "upright", "type": "object_upright"}], + }, + "semantic_hash": canonical_hash([step]), + "vote_count": votes, + "attempts": 1, + "normalizations": [], + } + + +def _candidate_set(candidates: list[dict]) -> dict: + return { + "schema_version": TASK_CANDIDATE_SET_SCHEMA, + "task_id": "upright_can", + "instruction": _UPRIGHT_CAN_INSTRUCTION, + "candidates": candidates, + "requested_candidate_count": sum(item["vote_count"] for item in candidates), + "valid_response_count": sum(item["vote_count"] for item in candidates), + "errors": [], + } + + +def _placement_candidate(candidate_id: str = "place") -> dict: + candidate = _candidate(candidate_id, "red can") + step = candidate["draft"]["steps"][0] + step.update( + { + "task_type": "E1", + "target": _selector("table"), + "relation": "on", + "orientation_goal": "preserve", + } + ) + candidate["scene_request"]["references"] = [ + { + "reference_id": "upright.object", + "step_id": "upright", + "role": "object", + "reference": "red can", + "quantifier": "one", + "count": 0, + "source_structure": "rigid_object", + "affordances": ["graspable", "placeable"], + "initial_state": {}, + "attributes": {}, + }, + { + "reference_id": "upright.target", + "step_id": "upright", + "role": "target", + "reference": "table", + "quantifier": "one", + "count": 0, + "source_structure": "physical_entity", + "affordances": [], + "initial_state": {}, + "attributes": {}, + }, + ] + candidate["success_spec"]["terms"] = [ + {"step_id": "upright", "type": "semantic_goal"} + ] + candidate["semantic_hash"] = canonical_hash([step]) + return candidate + + +def _grounder(**kwargs) -> dict: + prompt = kwargs["prompt"] + uid = "blue_can" if '"reference": "blue can"' in prompt else "red_can" + return { + "bindings": [ + { + "reference_id": "upright.object", + "status": "resolved", + "uids": [uid], + "confidence": 0.95, + } + ] + } + + +def test_scene_source_fingerprint_reads_without_copying(scene_export: Path) -> None: + before = sorted(path.relative_to(scene_export) for path in scene_export.rglob("*")) + fingerprint = fingerprint_scene_source(SceneSourceRef(scene_export)) + after = sorted(path.relative_to(scene_export) for path in scene_export.rglob("*")) + + assert fingerprint.config_path == scene_export / "scene_config.json" + assert len(fingerprint.config_sha256) == 64 + assert len(fingerprint.asset_sha256) == 3 + assert after == before + + +@pytest.mark.parametrize("filename", ["gym_config.json", "gym_config_merged.json"]) +def test_scene_adapter_supports_legacy_gym_configs( + tmp_path: Path, + filename: str, +) -> None: + project = _legacy_gym_project(tmp_path, filename) + result = SceneAdapter(grounding_caller=_grounder).adapt( + _candidate_set([_candidate("legacy", "red can")]), + project, + ) + + assert result.selected_candidate_id == "legacy" + assert result.static_scene_manifest["source_format"] == "legacy_gym_config" + assert any( + item["role"] == "articulation" + for item in result.static_scene_manifest["objects"] + ) + assert ( + result.prepared_scene.articulations[0]["fpath"] + == (project / "assets" / "cabinet.urdf").resolve().as_posix() + ) + + +def test_scene_source_fingerprint_covers_articulation_fpath(tmp_path: Path) -> None: + project = _legacy_gym_project(tmp_path, "gym_config.json") + original = fingerprint_scene_source(project) + articulation_path = project / "assets" / "cabinet.urdf" + + articulation_path.write_bytes(b"changed articulation") + changed = fingerprint_scene_source(project) + + assert articulation_path.resolve().as_posix() in original.asset_sha256 + assert changed.asset_sha256 != original.asset_sha256 + with pytest.raises(RuntimeError, match="changed after Task Engine preparation"): + verify_scene_source_fingerprint(original.to_dict()) + + +def test_scene_adapter_selects_bindable_majority_and_redacts_manifest( + scene_export: Path, +) -> None: + red = _candidate("red-majority", "red can", votes=2) + blue = _candidate("blue-minority", "blue can") + result = SceneAdapter(grounding_caller=_grounder).adapt( + _candidate_set([red, blue]), + scene_export, + ) + + hierarchy_by_uid = { + node["uid"]: node for node in result.conservative_scene_graph["nodes"] + } + assert hierarchy_by_uid["red_can"]["parent_uid"] == "unknown" + assert hierarchy_by_uid["red_can"]["parent_relation"] == "unknown" + + assert result.binding_report["status"] == "bound" + assert result.binding_report["candidates"][0]["status"] == "resolved" + assert result.selected_candidate_id == "red-majority" + assert result.reference_bindings == {"upright.object": ["red_can"]} + assert result.role_bindings["role_bindings"] == {} + red_manifest = next( + item for item in result.scene_manifest["objects"] if item["uid"] == "red_can" + ) + assert "position" not in json.dumps(red_manifest) + assert ( + result.prepared_scene.source_config_path == scene_export / "scene_config.json" + ) + assert result.static_scene_manifest is not None + static_by_uid = { + item["uid"]: item for item in result.static_scene_manifest["objects"] + } + assert static_by_uid["red_can"]["physics"]["body_type"] == "dynamic" + assert static_by_uid["red_can"]["geometry"]["asset_sha256"] + + +def test_scene_adapter_returns_report_for_business_level_non_binding( + scene_export: Path, +) -> None: + candidate = _candidate("missing", "green can") + + def not_found(**_kwargs): + return { + "bindings": [ + { + "reference_id": "upright.object", + "status": "not_found", + "uids": [], + "confidence": 0.0, + } + ] + } + + result = SceneAdapter(grounding_caller=not_found).adapt( + _candidate_set([candidate]), + scene_export, + ) + + assert result.selected_candidate is None + assert result.role_bindings is None + assert result.binding_report["status"] == "unsatisfied" + assert ( + result.binding_report["candidates"][0]["references"][0]["status"] == "not_found" + ) + + +def test_semantic_blueprint_selection_forces_ranked_low_confidence_uid() -> None: + candidate = _candidate("likely", "the can") + scene_objects = [ + { + "uid": "table", + "role": "table", + "name": "table", + "description": "A work table.", + "category": "table", + "init_pos": [0.0, 0.0, 0.0], + "affordances": ["support_surface"], + "initial_state": {}, + "attributes": {}, + }, + *[ + { + "uid": f"{color}_can", + "role": "rigid_object", + "name": f"{color} can", + "description": f"A {color} can.", + "category": "can", + "init_pos": [0.0, offset, 0.1], + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {"color": color}, + } + for color, offset in (("red", -0.1), ("blue", 0.1)) + ], + ] + + def ambiguous(**_kwargs): + return { + "bindings": [ + { + "reference_id": "upright.object", + "status": "ambiguous", + "uids": ["red_can", "blue_can"], + "confidence": 0.2, + } + ] + } + + result = SceneAdapter(grounding_caller=ambiguous).select_objects( + _candidate_set([candidate]), + scene_objects, + force_most_likely=True, + ) + + assert result.selected_candidate_id == "likely" + assert result.role_bindings["reference_bindings"] == {"upright.object": ["red_can"]} + reference = result.binding_report["candidates"][0]["references"][0] + assert reference["confidence"] == 0.2 + assert reference["candidate_uids"] == ["red_can", "blue_can"] + assert reference["selected_uids"] == ["red_can"] + assert reference["reasons"] == [ + "Forced the highest-ranked structurally compatible UID from an " + "ambiguous low-confidence response." + ] + + +def test_scene_adapter_uses_unique_bindable_then_injected_adjudication( + scene_export: Path, +) -> None: + red = _candidate("red", "red can") + blue = _candidate("blue", "blue can") + + def one_missing(**kwargs): + if '"reference": "red can"' in kwargs["prompt"]: + return { + "bindings": [ + { + "reference_id": "upright.object", + "status": "not_found", + "uids": [], + "confidence": 0.0, + } + ] + } + return _grounder(**kwargs) + + unique = SceneAdapter(grounding_caller=one_missing).adapt( + _candidate_set([red, blue]), + scene_export, + ) + assert unique.selected_candidate_id == "blue" + assert unique.binding_report["selection_reason"] == "unique_bindable" + + ambiguous = SceneAdapter(grounding_caller=_grounder).adapt( + _candidate_set([red, blue]), + scene_export, + ) + assert ambiguous.binding_report["status"] == "ambiguous" + + adjudicated = SceneAdapter( + grounding_caller=_grounder, + adjudicator=lambda **_kwargs: {"candidate_id": "blue"}, + ).adapt(_candidate_set([red, blue]), scene_export) + assert adjudicated.selected_candidate_id == "blue" + assert adjudicated.binding_report["selection_reason"] == "adjudicated_bindable" + + +def test_scene_adapter_runs_one_default_structured_adjudication( + scene_export: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + adjudications = 0 + + def caller(**kwargs): + nonlocal adjudications + if kwargs["schema"]["title"] == "ActionEngineTaskAdjudication": + adjudications += 1 + return {"candidate_id": "blue"} + return _grounder(**kwargs) + + monkeypatch.setattr( + scene_adapter_module, "_default_grounding_caller", lambda: caller + ) + result = SceneAdapter().adapt( + _candidate_set([_candidate("red", "red can"), _candidate("blue", "blue can")]), + scene_export, + ) + + assert result.selected_candidate_id == "blue" + assert result.binding_report["selection_reason"] == "adjudicated_bindable" + assert adjudications == 1 + + +def test_scene_adapter_accepts_direct_source_and_rejects_bad_protocol( + scene_export: Path, +) -> None: + candidate = _candidate("red", "red can") + direct = SceneAdapter(grounding_caller=_grounder).adapt( + _candidate_set([candidate]), + scene_export, + ) + result = SceneAdapter(grounding_caller=_grounder).adapt( + _candidate_set([candidate]), + SceneSourceRef(scene_export), + ) + assert result.scene_manifest == direct.scene_manifest + assert result.role_bindings == direct.role_bindings + + with pytest.raises(SceneAdapterProtocolError, match="unsupported fields"): + SceneAdapter( + grounding_caller=lambda **_kwargs: { + "bindings": [ + { + "reference_id": "upright.object", + "status": "not_found", + "uids": [], + "confidence": 0.0, + "invented": True, + } + ] + } + ).adapt(_candidate_set([candidate]), scene_export) + + +def test_explicit_scene_semantic_conflict_is_incompatible( + scene_export: Path, +) -> None: + config_path = scene_export / "scene_config.json" + config = json.loads(config_path.read_text(encoding="utf-8")) + config["rigid_object"][0]["initial_state"]["orientation"] = "upright" + config_path.write_text(json.dumps(config), encoding="utf-8") + + result = SceneAdapter(grounding_caller=_grounder).adapt( + _candidate_set([_candidate("red", "red can")]), + scene_export, + ) + reference = result.binding_report["candidates"][0]["references"][0] + assert result.binding_report["status"] == "unsatisfied" + assert reference["status"] == "incompatible" + assert result.binding_report["candidates"][0]["status"] == "incompatible" + assert "state 'orientation' conflicts" in reference["reasons"][0] + + +def test_scene_adapter_accepts_passive_support_target_and_rejects_self_reference( + scene_export: Path, +) -> None: + candidate = _placement_candidate() + + def place_on_table(**_kwargs): + return { + "bindings": [ + { + "reference_id": "upright.object", + "status": "resolved", + "uids": ["red_can"], + "confidence": 0.95, + }, + { + "reference_id": "upright.target", + "status": "resolved", + "uids": ["table"], + "confidence": 0.95, + }, + ] + } + + bound = SceneAdapter(grounding_caller=place_on_table).adapt( + _candidate_set([candidate]), scene_export + ) + assert bound.binding_report["status"] == "bound" + assert bound.reference_bindings["upright.target"] == ["table"] + + def self_reference(**_kwargs): + response = place_on_table() + response["bindings"][1]["uids"] = ["red_can"] + return response + + incompatible = SceneAdapter(grounding_caller=self_reference).adapt( + _candidate_set([candidate]), scene_export + ) + assert incompatible.binding_report["status"] == "unsatisfied" + assert incompatible.binding_report["candidates"][0]["status"] == "incompatible" + + +def test_scene_adapter_enforces_count_cardinality_in_audit( + scene_export: Path, +) -> None: + candidate = _candidate("two", "cans") + selector = candidate["draft"]["steps"][0]["object"] + selector.update(quantifier="count", count=2) + request = candidate["scene_request"]["references"][0] + request.update(reference="cans", quantifier="count", count=2) + candidate["semantic_hash"] = canonical_hash(candidate["draft"]["steps"]) + + def one_only(**_kwargs): + return { + "bindings": [ + { + "reference_id": "upright.object", + "status": "resolved", + "uids": ["red_can"], + "confidence": 0.95, + } + ] + } + + result = SceneAdapter(grounding_caller=one_only).adapt( + _candidate_set([candidate]), scene_export + ) + + assert result.binding_report["status"] == "unsatisfied" + audit = result.binding_report["candidates"][0] + assert audit["status"] == "incompatible" + assert "requires exactly 2 UIDs" in audit["references"][0]["reasons"][0] + + +def test_scene_adapter_binds_all_matching_uids( + scene_export: Path, +) -> None: + candidate = _candidate("all", "all cans") + candidate["draft"]["steps"][0]["object"].update(quantifier="all") + candidate["scene_request"] = derive_scene_request(candidate["draft"]) + candidate["semantic_hash"] = canonical_hash(candidate["draft"]["steps"]) + + def all_cans(**_kwargs): + return { + "bindings": [ + { + "reference_id": "upright.object", + "status": "resolved", + "uids": ["red_can", "blue_can"], + "confidence": 0.95, + } + ] + } + + result = SceneAdapter(grounding_caller=all_cans).adapt( + _candidate_set([candidate]), scene_export + ) + + assert result.binding_report["status"] == "bound" + assert result.reference_bindings == {"upright.object": ["red_can", "blue_can"]} + assert result.candidate_bindings[candidate["candidate_id"]][ + "reference_bindings" + ] == {"upright.object": ["red_can", "blue_can"]} + + +def test_scene_adapter_rejects_step_result_object_matching_same_step_target( + scene_export: Path, +) -> None: + config_path = scene_export / "scene_config.json" + config = json.loads(config_path.read_text(encoding="utf-8")) + config["rigid_object"][0]["affordances"].append("support_surface") + config_path.write_text(json.dumps(config), encoding="utf-8") + + candidate = _candidate("self-reference", "red can") + second = deepcopy(candidate["draft"]["steps"][0]) + second.update( + { + "id": "place_again", + "task_type": "E1", + "object": { + "kind": "step_result", + "step_id": "upright", + "reference": "", + "quantifier": "one", + "count": 0, + }, + "target": _selector("red can"), + "relation": "on", + "orientation_goal": "preserve", + "depends_on": ["upright"], + } + ) + candidate["draft"]["steps"].append(second) + candidate["scene_request"] = derive_scene_request(candidate["draft"]) + candidate["success_spec"] = derive_success_spec(candidate["draft"]) + candidate["semantic_hash"] = canonical_hash(candidate["draft"]["steps"]) + + def same_uid(**_kwargs): + return { + "bindings": [ + { + "reference_id": "upright.object", + "status": "resolved", + "uids": ["red_can"], + "confidence": 0.95, + }, + { + "reference_id": "place_again.target", + "status": "resolved", + "uids": ["red_can"], + "confidence": 0.95, + }, + ] + } + + result = SceneAdapter(grounding_caller=same_uid).adapt( + _candidate_set([candidate]), scene_export + ) + + assert result.binding_report["status"] == "unsatisfied" + target_audit = result.binding_report["candidates"][0]["references"][1] + assert target_audit["status"] == "incompatible" + assert "same UID as object and target" in target_audit["reasons"][0] + + +def test_scene_source_fingerprint_covers_assets_and_config(scene_export: Path) -> None: + original = fingerprint_scene_source(scene_export) + + asset_path = scene_export / "meshes" / "red_can.glb" + asset_path.write_bytes(b"changed asset") + changed_asset = fingerprint_scene_source(scene_export) + assert changed_asset.asset_sha256 != original.asset_sha256 + + config_path = scene_export / "scene_config.json" + config = json.loads(config_path.read_text(encoding="utf-8")) + config["rigid_object"][0]["body_scale"] = [1.1, 1.0, 1.0] + config["rigid_object"][0]["physics"] = {"mass": 0.25} + config_path.write_text(json.dumps(config), encoding="utf-8") + changed_config = fingerprint_scene_source(scene_export) + assert changed_config.config_sha256 != changed_asset.config_sha256 + + +def test_scene_source_verification_rejects_later_mutation(scene_export: Path) -> None: + expected = fingerprint_scene_source(scene_export).to_dict() + (scene_export / "meshes" / "red_can.glb").write_bytes(b"changed later") + + with pytest.raises(RuntimeError, match="changed after Task Engine preparation"): + verify_scene_source_fingerprint(expected) diff --git a/tests/gen_sim/task_engine/orchestration/test_scene_assets.py b/tests/gen_sim/task_engine/orchestration/test_scene_assets.py new file mode 100644 index 000000000..4b5971b6f --- /dev/null +++ b/tests/gen_sim/task_engine/orchestration/test_scene_assets.py @@ -0,0 +1,86 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for Task Engine scene-asset normalization.""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +import pytest +import trimesh + +from embodichain.gen_sim.task_engine.orchestration import scene_assets as assets + + +def test_bake_glb_splits_face_corners_for_renderer_safe_normals( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + source = tmp_path / "source.glb" + destination = tmp_path / "baked.glb" + trimesh.creation.box().export(source, file_type="glb") + monkeypatch.setattr( + assets, + "_has_inconsistent_shading_normals", + lambda _mesh: True, + ) + + assets._bake_glb(source, destination, [1.0, 1.0, 1.0]) + + baked = trimesh.load(destination, force="scene", process=False) + geometry = tuple(baked.geometry.values()) + assert len(geometry) == 1 + assert len(geometry[0].vertices) == 3 * len(geometry[0].faces) + + +def test_bake_glb_preserves_safe_mesh_topology(tmp_path: Path) -> None: + source = tmp_path / "source.glb" + destination = tmp_path / "baked.glb" + trimesh.creation.box().export(source, file_type="glb") + source_scene = trimesh.load(source, force="scene", process=False) + source_mesh = tuple(source_scene.geometry.values())[0] + + assets._bake_glb(source, destination, [1.0, 1.0, 1.0]) + + baked = trimesh.load(destination, force="scene", process=False) + geometry = tuple(baked.geometry.values()) + assert len(geometry) == 1 + assert len(geometry[0].vertices) == len(source_mesh.vertices) + + +def test_inconsistent_shading_normals_detect_opposed_face_corner() -> None: + mesh = trimesh.Trimesh( + vertices=np.asarray( + [ + [0.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + ] + ), + faces=np.asarray( + [ + [0, 1, 2], + [0, 1, 2], + [0, 1, 2], + [0, 2, 1], + ] + ), + process=False, + ) + + assert assets._has_inconsistent_shading_normals(mesh) is True diff --git a/tests/gen_sim/task_engine/orchestration/test_source_scene.py b/tests/gen_sim/task_engine/orchestration/test_source_scene.py new file mode 100644 index 000000000..34119462c --- /dev/null +++ b/tests/gen_sim/task_engine/orchestration/test_source_scene.py @@ -0,0 +1,255 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for Task Engine source-scene normalization.""" + +from __future__ import annotations + +import json +from pathlib import Path + +import pytest + +from embodichain.gen_sim.task_engine.orchestration.source_scene import ( + prepare_scene, + resolve_gym_config_path, + resolve_source_scene, +) + + +@pytest.fixture +def gym_export(tmp_path: Path) -> Path: + """Create a minimal legacy Prompt2Scene export.""" + 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") + scene_state = export / "scene_state" + scene_state.mkdir() + (scene_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: + """Create a minimal canonical scene export.""" + export = tmp_path / "scene_export" + assets = export / "mesh_assets" + assets.mkdir(parents=True) + (assets / "table.glb").write_bytes(b"not-a-real-glb") + for uid in ("bottle_001", "bottle_002"): + (assets / f"{uid}.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 test_prepare_scene_normalizes_prompt2scene_export(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 + assert Path(scene.rigid_objects[0]["shape"]["fpath"]).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] + + +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_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_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_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_table(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) + + assert scene.source_scene_xy_translation == pytest.approx((-1.0, -2.0)) + assert scene.background[0]["init_pos"][:2] == pytest.approx([0.0, 0.0]) + assert scene.rigid_objects[0]["init_pos"][:2] == pytest.approx([0.2, 0.3]) + + +def test_scene_export_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) diff --git a/tests/gen_sim/task_engine/scene/__init__.py b/tests/gen_sim/task_engine/scene/__init__.py new file mode 100644 index 000000000..96ca57709 --- /dev/null +++ b/tests/gen_sim/task_engine/scene/__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. +# ---------------------------------------------------------------------------- + +"""Tests for Task Engine scene adaptation boundaries.""" + +from __future__ import annotations diff --git a/tests/gen_sim/task_engine/scene/test_final_inspection.py b/tests/gen_sim/task_engine/scene/test_final_inspection.py new file mode 100644 index 000000000..937a075d9 --- /dev/null +++ b/tests/gen_sim/task_engine/scene/test_final_inspection.py @@ -0,0 +1,118 @@ +# ---------------------------------------------------------------------------- +# 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 json +from pathlib import Path + +import numpy as np +import trimesh + +from embodichain.gen_sim.task_engine.orchestration.scene_source import ( + scene_revision_id, +) +from embodichain.gen_sim.task_engine.scene.final_inspection import ( + inspect_final_scene, +) + + +def _scene_export(root: Path, *, scene_id: str, can_rotation: list[float]) -> Path: + export = root / "scene_export" + assets = export / "mesh_assets" + assets.mkdir(parents=True) + trimesh.creation.box(extents=[1.0, 0.1, 1.0]).export( + assets / "table.glb", file_type="glb" + ) + can = trimesh.creation.cylinder(radius=0.04, height=0.2) + can.apply_transform( + trimesh.transformations.rotation_matrix(np.pi / 2.0, [1.0, 0.0, 0.0]) + ) + can.export(assets / "can.glb", file_type="glb") + config = { + "format": "embodichain.scene-export/v1", + "scene_id": scene_id, + "background": [ + { + "uid": "table", + "name": "table", + "description": "A support table.", + "category": "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": "can", + "name": "red can", + "description": "A red can.", + "category": "can", + "shape": {"shape_type": "Mesh", "fpath": "mesh_assets/can.glb"}, + "init_pos": [0.0, 0.0, 0.15], + "init_rot": can_rotation, + "body_scale": [1.0, 1.0, 1.0], + } + ], + } + path = export / "scene_config.json" + path.write_text(json.dumps(config), encoding="utf-8") + return path + + +def test_scene_revision_id_ignores_exporter_timestamp_and_location( + tmp_path: Path, +) -> None: + first = _scene_export( + tmp_path / "first", scene_id="scene-100", can_rotation=[0, 0, 0] + ) + second = _scene_export( + tmp_path / "second", scene_id="scene-200", can_rotation=[0, 0, 0] + ) + + assert scene_revision_id(first) == scene_revision_id(second) + + value = json.loads(second.read_text(encoding="utf-8")) + value["rigid_object"][0]["init_pos"][0] = 0.25 + second.write_text(json.dumps(value), encoding="utf-8") + assert scene_revision_id(first) != scene_revision_id(second) + + +def test_final_inspection_recomputes_support_and_orientation(tmp_path: Path) -> None: + source = _scene_export( + tmp_path / "standing", scene_id="scene", can_rotation=[0.0, 0.0, 0.0] + ) + + inspection = inspect_final_scene(source, revision_id=scene_revision_id(source)) + + can = next(item for item in inspection["objects"] if item["uid"] == "can") + assert can["orientation"] == "standing" + assert can["support"]["parent_uid"] == "table" + assert can["support"]["relation"] == "on" + assert can["support"]["xy_overlap_ratio"] > 0.9 + + +def test_final_inspection_detects_lying_rotation(tmp_path: Path) -> None: + source = _scene_export( + tmp_path / "lying", scene_id="scene", can_rotation=[90.0, 0.0, 0.0] + ) + + inspection = inspect_final_scene(source, revision_id=scene_revision_id(source)) + + can = next(item for item in inspection["objects"] if item["uid"] == "can") + assert can["orientation"] == "lying" diff --git a/tests/gen_sim/task_engine/scene/test_scene_boundary.py b/tests/gen_sim/task_engine/scene/test_scene_boundary.py new file mode 100644 index 000000000..5321f83dc --- /dev/null +++ b/tests/gen_sim/task_engine/scene/test_scene_boundary.py @@ -0,0 +1,575 @@ +# ---------------------------------------------------------------------------- +# 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 pathlib import Path +from types import SimpleNamespace + +import pytest + +from embodichain.gen_sim.task_engine.scene import ( + FeasibilityBroker, + SceneEngineV1Adapter, +) +from embodichain.gen_sim.task_engine.agent import derive_scene_request +from embodichain.gen_sim.task_engine.contracts import TASK_DRAFT_SCHEMA + + +def _prepared_scene(tmp_path: Path) -> SimpleNamespace: + table = { + "uid": "table", + "source_uid": "table_0", + "role": "background", + "name": "table", + "description": "A support table.", + "category": "table", + "color": "brown", + "shape": {"shape_type": "Mesh", "fpath": "/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], + "attributes": {}, + "initial_state": {}, + "affordances": [], + } + can = { + "uid": "red_can", + "source_uid": "red_can_0", + "role": "rigid_object", + "name": "red can", + "description": "A fallen red can.", + "category": "can", + "color": "red", + "shape": {"shape_type": "Mesh", "fpath": "/assets/can.glb"}, + "init_pos": [0.1, 0.0, 0.7], + "init_rot": [90.0, 0.0, 0.0], + "body_scale": [1.0, 1.0, 1.0], + "attributes": {}, + "initial_state": {"orientation": "fallen"}, + "affordances": ["graspable", "orientable", "placeable"], + } + runtime_table = { + "uid": "table", + "shape": table["shape"], + "attrs": {"mass": 10.0}, + "body_type": "kinematic", + } + runtime_can = { + "uid": "red_can", + "shape": can["shape"], + "attrs": {"mass": 0.1}, + "body_type": "dynamic", + } + return SimpleNamespace( + source_config_path=tmp_path / "scene_config.json", + planner_objects=(table, can), + background=(runtime_table,), + rigid_objects=(runtime_can,), + articulations=(), + asset_hashes={"table": "a" * 64, "red_can": "b" * 64}, + ) + + +def _candidate(task_type: str, affordances: list[str]) -> dict: + return { + "candidate_id": "candidate_01", + "draft": { + "task_id": "task", + "steps": [{"id": "step_01", "task_type": task_type}], + }, + "scene_request": { + "references": [ + { + "reference_id": "step_01.object", + "role": "object", + "source_structure": "rigid_object", + "affordances": affordances, + "initial_state": ( + {"orientation": "fallen"} if task_type == "E2" else {} + ), + "attributes": {}, + } + ] + }, + } + + +def _catalog(*, pour_available: bool = False) -> dict[str, dict]: + return { + name: {"runtime_available": True, "unavailable_reason": None} + for name in ("PickUp", "MoveHeldObject", "Place", "TurnKnob") + } | { + "Pour": { + "runtime_available": pour_available, + "unavailable_reason": None if pour_available else "Pour is planning-only.", + } + } + + +def _selector(kind: str, *, reference: str = "") -> dict[str, object]: + return { + "kind": kind, + "step_id": "", + "reference": reference, + "quantifier": "one", + "count": 0, + } + + +def _relation_candidate(relation: str) -> dict: + draft = { + "schema_version": TASK_DRAFT_SCHEMA, + "task_id": "place_relative", + "instruction": "place the can relative to the target", + "steps": [ + { + "id": "step_01", + "task_type": "E1", + "object": _selector("scene_ref", reference="red can"), + "target": _selector("scene_ref", reference="target"), + "relation": relation, + "required_arm": "auto", + "transfer_arm": "none", + "receive_arm": "none", + "orientation_goal": "preserve", + "target_state": "none", + "target_setting": 0, + "layout": "none", + "axis": "none", + "direction": "none", + "terminal_behavior": "none", + "depends_on": [], + } + ], + } + return { + "candidate_id": "candidate_01", + "draft": draft, + "scene_request": derive_scene_request(draft), + } + + +def _manifest_with_target_kinds(tmp_path: Path) -> dict: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + by_uid = {item["uid"]: item for item in manifest["objects"]} + by_uid["red_can"]["affordances"].append( + { + "type": "container", + "status": "declared", + "confidence": None, + "source": "test", + "link_uid": "", + "frame": {}, + "parameters": {}, + } + ) + articulation = deepcopy(by_uid["red_can"]) + articulation.update( + uid="cabinet", + source_uid="cabinet_0", + role="articulation", + name="cabinet", + category="cabinet", + physics={}, + articulation={"runtime_uid": "cabinet"}, + affordances=[], + ) + manifest["objects"].append(articulation) + return manifest + + +def test_scene_engine_v1_adapter_preserves_static_execution_evidence( + tmp_path: Path, +) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="embodichain.scene-export/v1", + robot_profile="dual_franka", + ) + + by_uid = {item["uid"]: item for item in manifest["objects"]} + assert manifest["adapter_capabilities"]["task_conditioned_generation"] is False + assert by_uid["red_can"]["geometry"]["asset_sha256"] == "b" * 64 + assert by_uid["red_can"]["physics"]["body_type"] == "dynamic" + assert {item["type"] for item in by_uid["table"]["affordances"]} == { + "support_surface" + } + assert ( + next( + item + for item in by_uid["red_can"]["affordances"] + if item["type"] == "graspable" + )["status"] + == "declared" + ) + + +def test_e2_feasibility_requires_runtime_probe_for_geometry(tmp_path: Path) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + report = FeasibilityBroker().assess( + _candidate("E2", ["graspable", "orientable"]), + {"step_01.object": ["red_can"]}, + manifest, + capability_catalog=_catalog(), + task_actions={"E2": ("PickUp", "MoveHeldObject", "Place")}, + ) + + assert report["status"] == "runtime_probe" + assert report["remediation_class"] == "none" + assert report["blockers"] == [] + assert report["summary"]["proven"] > 0 + assert report["summary"]["runtime_probe"] > 0 + + +def test_planning_only_action_is_reported_as_contradicted(tmp_path: Path) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + report = FeasibilityBroker().assess( + _candidate("E3", ["graspable", "pourable"]), + {"step_01.object": ["red_can"]}, + manifest, + capability_catalog=_catalog(), + task_actions={"E3": ("Pour",)}, + ) + + assert report["status"] == "contradicted" + assert report["remediation_class"] == "action_capability" + assert any("planning-only" in blocker for blocker in report["blockers"]) + + +def test_e3_does_not_require_runtime_content_observation_before_execution( + tmp_path: Path, +) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + report = FeasibilityBroker().assess( + _candidate("E3", ["graspable", "pourable"]), + {"step_01.object": ["red_can"]}, + manifest, + capability_catalog=_catalog(pour_available=True), + task_actions={"E3": ("PickUp", "MoveHeldObject", "Pour")}, + ) + + assert report["status"] != "contradicted" + assert all(check["kind"] != "content_observation" for check in report["checks"]) + + +def test_e8_requires_explicit_setting_to_angle_mapping(tmp_path: Path) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + report = FeasibilityBroker().assess( + _candidate("E8", ["turnable"]), + {"step_01.object": ["red_can"]}, + manifest, + capability_catalog=_catalog(), + task_actions={"E8": ("TurnKnob",)}, + ) + + assert any( + check["kind"] == "setting_mapping" and check["status"] == "contradicted" + for check in report["checks"] + ) + assert any("setting_values" in blocker for blocker in report["blockers"]) + + +def test_final_orientation_conflict_is_scene_remediable(tmp_path: Path) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + can = next(item for item in manifest["objects"] if item["uid"] == "red_can") + can["initial_state"]["orientation"] = "upright" + + report = FeasibilityBroker().assess( + _candidate("E2", ["graspable", "orientable"]), + {"step_01.object": ["red_can"]}, + manifest, + capability_catalog=_catalog(), + task_actions={"E2": ("PickUp", "MoveHeldObject", "Place")}, + ) + + assert report["status"] == "contradicted" + assert report["remediation_class"] == "scene_remediable" + + +def test_missing_affordance_remains_unknown_instead_of_becoming_supported( + tmp_path: Path, +) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + report = FeasibilityBroker().assess( + _candidate("E1", ["graspable", "liquid_safe"]), + {"step_01.object": ["red_can"]}, + manifest, + capability_catalog=_catalog(), + task_actions={"E1": ("PickUp", "MoveHeldObject", "Place")}, + ) + + assert report["status"] == "unknown" + assert any( + check["status"] == "unknown" and "liquid_safe" in check["reason"] + for check in report["checks"] + ) + + +def test_physical_object_can_be_a_runtime_support_without_support_affordance( + tmp_path: Path, +) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + candidate = _candidate("E1", ["graspable", "placeable"]) + candidate["draft"]["steps"][0].update( + target={"kind": "scene_ref"}, + relation="on", + ) + candidate["scene_request"]["references"].append( + { + "reference_id": "step_01.target", + "role": "target", + "source_structure": "physical_entity", + "affordances": [], + "initial_state": {}, + "attributes": {}, + } + ) + + report = FeasibilityBroker().assess( + candidate, + { + "step_01.object": ["red_can"], + "step_01.target": ["red_can"], + }, + manifest, + capability_catalog=_catalog(), + task_actions={"E1": ("PickUp", "MoveHeldObject", "Place")}, + ) + + structure = next( + check + for check in report["checks"] + if check["kind"] == "structure" and check["subject"] == "step_01.target:red_can" + ) + assert structure["status"] == "proven" + support_probe = next( + check for check in report["checks"] if check["kind"] == "placement_support" + ) + assert support_probe["status"] == "runtime_probe" + assert support_probe["evidence"]["runtime_obligations"] == [ + "placement_candidates", + "object_supported_by", + "stable_for", + "final_support_revalidation", + ] + assert report["blockers"] == [] + + +@pytest.mark.parametrize( + ("relation", "target_uid", "expected_structure", "expected_status"), + [ + ("on", "red_can", "physical_entity", "proven"), + ("on", "table", "physical_entity", "proven"), + ("on", "cabinet", "physical_entity", "contradicted"), + ("inside", "red_can", "rigid_object", "proven"), + ("inside", "table", "rigid_object", "contradicted"), + ("inside", "cabinet", "rigid_object", "contradicted"), + ("behind", "red_can", "spatial_reference", "proven"), + ("behind", "table", "spatial_reference", "proven"), + ("behind", "cabinet", "spatial_reference", "runtime_probe"), + ("front_of", "red_can", "spatial_reference", "proven"), + ("front_of", "table", "spatial_reference", "proven"), + ("front_of", "cabinet", "spatial_reference", "runtime_probe"), + ("left_of", "red_can", "spatial_reference", "proven"), + ("left_of", "table", "spatial_reference", "proven"), + ("left_of", "cabinet", "spatial_reference", "runtime_probe"), + ("right_of", "red_can", "spatial_reference", "proven"), + ("right_of", "table", "spatial_reference", "proven"), + ("right_of", "cabinet", "spatial_reference", "runtime_probe"), + ], +) +def test_relation_target_structure_matrix_uses_capability_semantics( + tmp_path: Path, + relation: str, + target_uid: str, + expected_structure: str, + expected_status: str, +) -> None: + candidate = _relation_candidate(relation) + target_request = next( + item + for item in candidate["scene_request"]["references"] + if item["role"] == "target" + ) + report = FeasibilityBroker().assess( + candidate, + { + "step_01.object": ["red_can"], + "step_01.target": [target_uid], + }, + _manifest_with_target_kinds(tmp_path), + capability_catalog=_catalog(), + task_actions={"E1": ("PickUp", "MoveHeldObject", "Place")}, + ) + + structure = next( + check + for check in report["checks"] + if check["kind"] == "structure" + and check["subject"] == f"step_01.target:{target_uid}" + ) + assert target_request["source_structure"] == expected_structure + assert structure["status"] == expected_status + + +def test_legacy_scene_entity_target_is_treated_as_an_abstract_structure( + tmp_path: Path, +) -> None: + candidate = _relation_candidate("behind") + target_request = next( + item + for item in candidate["scene_request"]["references"] + if item["role"] == "target" + ) + target_request["source_structure"] = "scene_entity" + + report = FeasibilityBroker().assess( + candidate, + { + "step_01.object": ["red_can"], + "step_01.target": ["red_can"], + }, + _manifest_with_target_kinds(tmp_path), + capability_catalog=_catalog(), + task_actions={"E1": ("PickUp", "MoveHeldObject", "Place")}, + ) + + structure = next( + check + for check in report["checks"] + if check["kind"] == "structure" and check["subject"] == "step_01.target:red_can" + ) + assert structure["status"] == "proven" + assert report["blockers"] == [] + + +def test_unknown_structure_contract_is_not_a_scene_contradiction( + tmp_path: Path, +) -> None: + candidate = _relation_candidate("behind") + target_request = next( + item + for item in candidate["scene_request"]["references"] + if item["role"] == "target" + ) + target_request["source_structure"] = "future_spatial_capability" + + report = FeasibilityBroker().assess( + candidate, + { + "step_01.object": ["red_can"], + "step_01.target": ["red_can"], + }, + _manifest_with_target_kinds(tmp_path), + capability_catalog=_catalog(), + task_actions={"E1": ("PickUp", "MoveHeldObject", "Place")}, + ) + + structure = next( + check + for check in report["checks"] + if check["kind"] == "structure" and check["subject"] == "step_01.target:red_can" + ) + assert structure["status"] == "unknown" + assert not any("future_spatial_capability" in item for item in report["blockers"]) + + +def test_required_arm_side_requires_the_live_robot_frame( + tmp_path: Path, +) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + red_can = next(item for item in manifest["objects"] if item["uid"] == "red_can") + red_can["initial_pose"]["position"][1] = -0.20 + candidate = _candidate("E2", ["graspable", "orientable"]) + candidate["draft"]["steps"][0]["required_arm"] = "right_arm" + + report = FeasibilityBroker().assess( + candidate, + {"step_01.object": ["red_can"]}, + manifest, + capability_catalog=_catalog(), + task_actions={"E2": ("PickUp", "MoveHeldObject", "Place")}, + ) + + probe = next( + check for check in report["checks"] if check["kind"] == "arm_layout_risk" + ) + assert probe["status"] == "runtime_probe" + assert probe["evidence"]["arm_side_frame"] == "live_robot" + assert probe["evidence"]["mismatch_risk"] is None + assert "expected_arm" not in probe["evidence"] + assert probe["evidence"]["geometry_certificate"] is False + assert report["blockers"] == [] + + +def test_workspace_report_covers_complete_task_phases(tmp_path: Path) -> None: + manifest = SceneEngineV1Adapter().adapt_prepared_scene( + _prepared_scene(tmp_path), + source_format="test", + robot_profile="dual_franka", + ) + report = FeasibilityBroker().assess( + _candidate("E2", ["graspable", "orientable"]), + {"step_01.object": ["red_can"]}, + manifest, + capability_catalog=_catalog(), + task_actions={"E2": ("PickUp", "MoveHeldObject", "Place")}, + ) + + workflow = next( + check for check in report["checks"] if check["kind"] == "task_workspace" + ) + phases = {item["phase"] for item in workflow["evidence"]["phases"]} + assert phases == {"pickup", "safety_clearance"} + assert workflow["status"] == "runtime_probe" diff --git a/tests/gen_sim/task_engine/test_agent.py b/tests/gen_sim/task_engine/test_agent.py new file mode 100644 index 000000000..e7505b658 --- /dev/null +++ b/tests/gen_sim/task_engine/test_agent.py @@ -0,0 +1,1059 @@ +# ---------------------------------------------------------------------------- +# 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 threading +from time import sleep + +import pytest + +from embodichain.gen_sim.task_engine.contracts import ( + SUCCESS_SPEC_SCHEMA, + TASK_DRAFT_SCHEMA, + validate_success_spec, + validate_task_candidate, + validate_task_draft, +) +from embodichain.gen_sim.task_engine.agent import ( + TaskAgent, + TaskGenerationError, + derive_scene_request, + derive_success_spec, +) +from embodichain.gen_sim.task_engine.interpretation import ( + InstructionDraftResult, + validate_instruction_intent, +) +from embodichain.gen_sim.task_engine.orchestration.contracts import ( + ROLE_BINDINGS_SCHEMA, +) +from embodichain.gen_sim.task_engine.semantic_planner import ( + SemanticTaskPlanner, + UnsupportedSemanticCapabilityError, +) + +_TEST_INSTRUCTION = "test-instruction" + + +def _selector(kind="none", *, step_id="", reference="", quantifier="one", count=0): + return { + "kind": kind, + "step_id": step_id, + "reference": reference, + "quantifier": quantifier, + "count": count, + } + + +def _step(step_id="orient", reference="purple can"): + return { + "id": step_id, + "task_type": "E2", + "object": _selector("scene_ref", reference=reference), + "target": _selector(), + "relation": "none", + "required_arm": "auto", + "transfer_arm": "none", + "receive_arm": "none", + "orientation_goal": "upright", + "target_state": "none", + "target_setting": 0, + "layout": "none", + "axis": "none", + "direction": "none", + "terminal_behavior": "none", + "depends_on": [], + } + + +def _result(step): + return InstructionDraftResult( + intent={"steps": [deepcopy(step)]}, + model="injected_caller", + attempts=1, + latency_seconds=0.01, + normalizations=(), + ) + + +def test_task_agent_generates_concurrently_deduplicates_and_counts_votes(): + barrier = threading.Barrier(3) + lock = threading.Lock() + assigned = 0 + + def interpreter(_instruction, **_kwargs): + nonlocal assigned + with lock: + index = assigned + assigned += 1 + barrier.wait(timeout=2) + sleep(0.01) + if index < 2: + return _result(_step(step_id=f"arbitrary_{index}")) + return _result(_step(step_id="different", reference="orange can")) + + result = TaskAgent(interpreter=interpreter).generate("task", _TEST_INSTRUCTION) + + assert result["requested_candidate_count"] == 3 + assert result["valid_response_count"] == 3 + assert len(result["candidates"]) == 2 + assert sorted(item["vote_count"] for item in result["candidates"]) == [1, 2] + assert {item["draft"]["steps"][0]["id"] for item in result["candidates"]} == { + "step_01" + } + + +def test_scene_request_and_success_are_deterministic_contract_derivations(): + draft = { + "schema_version": TASK_DRAFT_SCHEMA, + "task_id": "upright", + "instruction": _TEST_INSTRUCTION, + "steps": [_step(reference="all cans")], + } + draft["steps"][0]["object"].update(quantifier="all") + + request = derive_scene_request(draft) + success = derive_success_spec(draft) + + assert request["references"] == [ + { + "reference_id": "orient.object", + "step_id": "orient", + "role": "object", + "reference": "all cans", + "quantifier": "all", + "count": 0, + "source_structure": "rigid_object", + "affordances": ["graspable", "orientable"], + "initial_state": {"orientation": "fallen"}, + "attributes": {}, + } + ] + assert success["terms"] == [{"step_id": "orient", "type": "object_upright"}] + + +@pytest.mark.parametrize( + ("relation", "expected_structure", "expected_affordances"), + [ + ("on", "physical_entity", []), + ("inside", "rigid_object", ["container"]), + ("behind", "spatial_reference", []), + ("front_of", "spatial_reference", []), + ("left_of", "spatial_reference", []), + ("right_of", "spatial_reference", []), + ], +) +def test_target_requirements_describe_capabilities_not_concrete_roles( + relation: str, + expected_structure: str, + expected_affordances: list[str], +) -> None: + step = _step(step_id="place", reference="green can") + step.update( + task_type="E1", + target=_selector("scene_ref", reference="red can"), + relation=relation, + orientation_goal="preserve", + ) + draft = { + "schema_version": TASK_DRAFT_SCHEMA, + "task_id": "stack", + "instruction": _TEST_INSTRUCTION, + "steps": [step], + } + + request = derive_scene_request(draft) + + target = next( + reference + for reference in request["references"] + if reference["role"] == "target" + ) + assert target["source_structure"] == expected_structure + assert target["affordances"] == expected_affordances + + +def test_semantic_planner_rejects_implicit_multi_object_expansion(): + def interpreter(_instruction, **_kwargs): + step = _step(reference="all cans") + step["object"].update(quantifier="all") + return _result(step) + + candidate = TaskAgent(interpreter=interpreter).generate( + "upright", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + with pytest.raises(ValueError, match="must resolve to exactly one scene entity"): + SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "upright", + "candidate_id": candidate["candidate_id"], + "reference_bindings": {"step_01.object": ["can_a", "can_b"]}, + "role_bindings": {}, + }, + [ + {"uid": "can_a", "init_pos": [0.0, -0.2, 0.7]}, + {"uid": "can_b", "init_pos": [0.0, 0.2, 0.7]}, + ], + ) + + +@pytest.mark.parametrize("task_type", ("E1", "E2")) +def test_manipulation_intent_requires_an_explicit_or_auto_arm( + task_type: str, +) -> None: + """An executable single-arm task cannot retain the inapplicable sentinel.""" + step = _step() + step["task_type"] = task_type + step["required_arm"] = "none" + if task_type == "E1": + step["target"] = _selector("scene_ref", reference="tray") + step["relation"] = "on" + step["orientation_goal"] = "preserve" + + with pytest.raises(ValueError, match="requires required_arm"): + validate_instruction_intent({"steps": [step]}) + + +def test_semantic_planner_adds_profile_bound_cleanup_without_joint_data() -> None: + """Task-group cleanup stays a semantic call and never embeds robot qpos.""" + + def interpreter(_instruction, **_kwargs): + step = _step(step_id="place", reference="cube") + step.update( + task_type="E1", + target=_selector("scene_ref", reference="tray"), + relation="inside", + required_arm="left_arm", + orientation_goal="preserve", + ) + return _result(step) + + candidate = TaskAgent(interpreter=interpreter).generate( + "place_cube", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "place_cube", + "candidate_id": candidate["candidate_id"], + "reference_bindings": { + "step_01.object": ["cube"], + "step_01.target": ["tray"], + }, + "role_bindings": {}, + }, + [ + {"runtime_uid": "cube", "init_pos": [0.0, -0.2, 0.7]}, + {"runtime_uid": "tray", "init_pos": [0.0, 0.0, 0.7]}, + ], + ) + + assert [node["call"]["kind"] for node in graph["nodes"]] == [ + "pick", + "place", + "registered", + ] + cleanup = graph["nodes"][-1] + assert cleanup["role"] == "cleanup" + assert cleanup["call"] == { + "kind": "registered", + "call_id": "simulation.park", + "arguments": {}, + "resources": {"primary": "left"}, + } + assert "qpos" not in repr(graph).lower() + + +def test_semantic_planner_keeps_explicit_e2_arm_through_release() -> None: + """E2 remains semantic while the integration owns axis and motion details.""" + candidate = TaskAgent( + interpreter=lambda *_args, **_kwargs: _result( + {**_step(), "required_arm": "right_arm"} + ) + ).generate("upright", _TEST_INSTRUCTION, candidate_count=1)["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "upright", + "candidate_id": candidate["candidate_id"], + "reference_bindings": {"step_01.object": ["purple_can"]}, + "role_bindings": {}, + }, + [ + {"runtime_uid": "purple_can", "init_pos": [0.0, 0.2, 0.7]}, + {"runtime_uid": "table", "init_pos": [0.0, 0.0, 0.0]}, + ], + ) + + assert [node["call"].get("call_id") for node in graph["nodes"]] == [ + "simulation.pick", + "gen_sim.align_held", + "gen_sim.align_held", + "simulation.place_relative", + "gen_sim.clear_released", + "simulation.park", + ] + assert all( + node["call"]["resources"] == {"primary": "right"} for node in graph["nodes"] + ) + release = next( + node["call"] + for node in graph["nodes"] + if node["call"].get("call_id") == "simulation.place_relative" + ) + assert release["arguments"] == { + "object": "purple_can", + "reference": "table", + "relation": "on", + } + assert set(graph["targets"]) == { + "step_01_upright_target", + "step_01_upright_staging_target", + } + + +def test_semantic_planner_routes_handover_through_verified_pick_state() -> None: + """A transfer starts from a verified source attachment boundary.""" + step = _step(step_id="handover", reference="can") + step.update( + task_type="E4", + transfer_arm="left_arm", + receive_arm="right_arm", + orientation_goal="preserve", + terminal_behavior="hold", + ) + candidate = TaskAgent(interpreter=lambda *_args, **_kwargs: _result(step)).generate( + "handover", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "handover", + "candidate_id": candidate["candidate_id"], + "reference_bindings": {"step_01.object": ["can"]}, + "role_bindings": {}, + }, + [{"runtime_uid": "can", "init_pos": [0.0, -0.2, 0.7]}], + ) + + assert [node["call"] for node in graph["nodes"]] == [ + { + "kind": "pick", + "object": "can", + "resources": {"primary": "left"}, + }, + { + "kind": "hand_over", + "object": "can", + "resources": {"source": "left", "destination": "right"}, + }, + ] + + +@pytest.mark.parametrize( + "relation", + ("above", "behind", "front_of", "left_of", "on", "right_of"), +) +def test_semantic_planner_keeps_spatial_relations_late_bound(relation: str) -> None: + """Every supported E1 relation names entities instead of an initial pose.""" + step = _step(step_id="place", reference="can") + step.update( + task_type="E1", + target=_selector("scene_ref", reference="notebook"), + relation=relation, + required_arm="left_arm", + orientation_goal="preserve", + ) + candidate = TaskAgent(interpreter=lambda *_args, **_kwargs: _result(step)).generate( + "place", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "place", + "candidate_id": candidate["candidate_id"], + "reference_bindings": { + "step_01.object": ["can"], + "step_01.target": ["notebook"], + }, + "role_bindings": {}, + }, + [ + {"runtime_uid": "can", "init_pos": [0.0, -0.2, 0.7]}, + {"runtime_uid": "notebook", "init_pos": [0.0, 0.0, 0.7]}, + ], + ) + + assert graph["targets"] == {} + assert graph["nodes"][1]["call"] == { + "kind": "registered", + "call_id": "simulation.place_relative", + "arguments": { + "object": "can", + "reference": "notebook", + "relation": relation, + }, + "resources": {"primary": "left"}, + } + + +def test_draft_rejects_grounded_fields_and_task_agent_fails_closed(): + draft = { + "schema_version": TASK_DRAFT_SCHEMA, + "task_id": "bad", + "instruction": "bad", + "steps": [_step()], + } + draft["steps"][0]["object"]["uid"] = "scene_uid" + with pytest.raises(ValueError, match="forbidden|exactly fields"): + validate_task_draft(draft) + + def invalid(_instruction, **_kwargs): + raise ValueError("invalid draft after repair") + + with pytest.raises(TaskGenerationError, match="All Task Agent candidates"): + TaskAgent(interpreter=invalid).generate("bad", "bad") + + +def test_task_candidate_rejects_scene_constraints_not_derived_from_draft(): + candidate = TaskAgent( + interpreter=lambda *_args, **_kwargs: _result(_step()) + ).generate("upright", _TEST_INSTRUCTION, candidate_count=1)["candidates"][0] + candidate["scene_request"]["references"][0]["affordances"] = [] + + with pytest.raises(ValueError, match="derived exactly"): + validate_task_candidate(candidate) + + +def test_success_spec_rejects_types_outside_task_ontology(): + with pytest.raises(ValueError, match="must be one of"): + validate_success_spec( + { + "schema_version": SUCCESS_SPEC_SCHEMA, + "task_id": "bad_success", + "op": "all", + "terms": [{"step_id": "step_01", "type": "looks_good"}], + } + ) + + +def test_task_agent_isolates_invalid_interpreter_results(): + lock = threading.Lock() + calls = 0 + + def interpreter(_instruction, **_kwargs): + nonlocal calls + with lock: + index = calls + calls += 1 + if index == 0: + invalid = _step() + invalid["object"]["uid"] = "red_can" + return _result(invalid) + return _result(_step()) + + result = TaskAgent(interpreter=interpreter).generate( + "upright", _TEST_INSTRUCTION, candidate_count=2 + ) + + assert result["valid_response_count"] == 1 + assert len(result["errors"]) == 1 + assert len(result["candidates"]) == 1 + + +def test_semantic_planner_composes_e2_from_pick_move_and_move_joints() -> None: + """E2 separates verified pickup, upright transport, opening, and retreat.""" + + def interpreter(_instruction, **_kwargs): + return _result(_step(reference="purple can")) + + candidate = TaskAgent(interpreter=interpreter).generate( + "upright_can", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "upright_can", + "candidate_id": candidate["candidate_id"], + "reference_bindings": {"step_01.object": ["can"]}, + "role_bindings": {}, + }, + [ + { + "runtime_uid": "can", + "role": "rigid_object", + "init_pos": [0.1, -0.2, 0.76], + "attributes": { + "final_world_aabb": { + "min": [0.04, -0.26, 0.73], + "max": [0.16, -0.14, 0.79], + } + }, + }, + { + "runtime_uid": "table", + "role": "background", + "init_pos": [0.0, 0.0, 0.0], + "attributes": { + "final_world_aabb": { + "min": [-0.5, -0.5, 0.0], + "max": [0.5, 0.5, 0.72], + } + }, + }, + ], + ) + + assert [node["call"] for node in graph["nodes"]] == [ + { + "kind": "registered", + "call_id": "simulation.pick", + "arguments": {"object": "can", "target": "step_01_upright_target"}, + "resources": {"primary": "left"}, + }, + { + "kind": "registered", + "call_id": "gen_sim.align_held", + "arguments": { + "object": "can", + "target": "step_01_upright_staging_target", + "preserve_yaw": False, + }, + "resources": {"primary": "left"}, + }, + { + "kind": "registered", + "call_id": "gen_sim.align_held", + "arguments": { + "object": "can", + "target": "step_01_upright_staging_target", + "preserve_yaw": True, + }, + "resources": {"primary": "left"}, + }, + { + "kind": "registered", + "call_id": "simulation.place_relative", + "arguments": { + "object": "can", + "reference": "table", + "relation": "on", + }, + "resources": {"primary": "left"}, + }, + { + "kind": "registered", + "call_id": "gen_sim.clear_released", + "arguments": { + "object": "can", + "target": "step_01_upright_staging_target", + }, + "resources": {"primary": "left"}, + }, + { + "kind": "registered", + "call_id": "simulation.park", + "arguments": {}, + "resources": {"primary": "left"}, + }, + ] + assert graph["targets"]["step_01_upright_target"]["values"][0][ + "position" + ] == pytest.approx([0.1, -0.2, 0.79]) + assert graph["task_groups"][0]["success"]["type"] == "object_upright" + + +@pytest.mark.parametrize( + "relation", + [ + "left_of", + "right_of", + "front_of", + "behind", + "front_left_of", + "front_right_of", + "back_left_of", + "back_right_of", + ], +) +def test_semantic_planner_keeps_directional_e1_targets_live( + relation: str, +) -> None: + """Directional E1 calls retain their reference identity until execution.""" + + def interpreter(_instruction, **_kwargs): + step = _step(step_id="place", reference="cube") + step.update( + task_type="E1", + target=_selector("scene_ref", reference="bottle"), + relation=relation, + required_arm="left_arm", + orientation_goal="preserve", + ) + return _result(step) + + candidate = TaskAgent(interpreter=interpreter).generate( + "relative_place", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "relative_place", + "candidate_id": candidate["candidate_id"], + "reference_bindings": { + "step_01.object": ["cube"], + "step_01.target": ["bottle"], + }, + "role_bindings": {}, + }, + [ + {"runtime_uid": "cube", "init_pos": [0.0, 0.0, 0.8]}, + {"runtime_uid": "bottle", "init_pos": [0.2, 0.3, 0.7]}, + ], + ) + + call = graph["nodes"][1]["call"] + assert call["call_id"] == "simulation.place_relative" + assert call["arguments"] == { + "object": "cube", + "reference": "bottle", + "relation": relation, + } + assert graph["targets"] == {} + + +def test_semantic_planner_keeps_e1_support_relation_late_bound() -> None: + """Place-on selects trusted scene geometry rather than a frozen pose.""" + + def interpreter(_instruction, **_kwargs): + step = _step(step_id="stack", reference="apple") + step.update( + task_type="E1", + target=_selector("scene_ref", reference="can"), + relation="on", + required_arm="left_arm", + orientation_goal="preserve", + ) + return _result(step) + + candidate = TaskAgent(interpreter=interpreter).generate( + "stack_apple", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "stack_apple", + "candidate_id": candidate["candidate_id"], + "reference_bindings": { + "step_01.object": ["apple"], + "step_01.target": ["can"], + }, + "role_bindings": {}, + }, + [ + {"runtime_uid": "apple", "init_pos": [0.0, 0.0, 0.8]}, + {"runtime_uid": "can", "init_pos": [0.2, 0.3, 0.8]}, + ], + ) + + assert graph["nodes"][1]["call"] == { + "kind": "registered", + "call_id": "simulation.place_relative", + "arguments": {"object": "apple", "reference": "can", "relation": "on"}, + "resources": {"primary": "left"}, + } + + +def test_semantic_planner_composes_e3_from_existing_pour_skills() -> None: + """E3 emits only canonical calls while retaining the live target container.""" + + def interpreter(_instruction, **_kwargs): + step = _step(step_id="pour", reference="bottle") + step.update( + task_type="E3", + target=_selector("scene_ref", reference="cup"), + relation="above", + required_arm="right_arm", + orientation_goal="none", + ) + return _result(step) + + candidate = TaskAgent(interpreter=interpreter).generate( + "pour_water", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "pour_water", + "candidate_id": candidate["candidate_id"], + "reference_bindings": { + "step_01.object": ["bottle"], + "step_01.target": ["cup"], + }, + "role_bindings": {}, + }, + [ + {"runtime_uid": "bottle", "init_pos": [0.1, 0.2, 0.75]}, + {"runtime_uid": "cup", "init_pos": [0.0, 0.0, 0.75]}, + ], + ) + + assert [node["call"]["kind"] for node in graph["nodes"]] == [ + "pick", + "registered", + "registered", + "place", + "registered", + ] + assert graph["nodes"][1]["call"] == { + "kind": "registered", + "call_id": "simulation.move_held_object", + "arguments": { + "object": "bottle", + "target": "step_01_pour_target", + "reference": "cup", + }, + "resources": {"primary": "right"}, + } + assert graph["nodes"][2]["call"] == { + "kind": "registered", + "call_id": "simulation.pour", + "arguments": {"object": "bottle"}, + "resources": {"primary": "right"}, + } + assert graph["nodes"][3]["call"]["at"] == { + "kind": "target_ref", + "target": "step_01_return_target", + } + assert graph["task_groups"][0]["success"]["type"] == "poured" + + +@pytest.mark.parametrize( + ("task_type", "terminal"), [("E1", "place"), ("E4", "hold"), ("E4", "place")] +) +def test_explicit_upright_adds_alignment_on_the_final_holding_arm( + task_type: str, terminal: str +) -> None: + step = _step(reference="can") + step.update( + task_type=task_type, required_arm="right_arm" if task_type == "E1" else "none" + ) + if task_type == "E4": + step.update( + transfer_arm="left_arm", receive_arm="right_arm", terminal_behavior=terminal + ) + if terminal == "place": + step.update(target=_selector("scene_ref", reference="table"), relation="on") + candidate = TaskAgent(interpreter=lambda *_a, **_kw: _result(step)).generate( + "upright_goal", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "upright_goal", + "candidate_id": candidate["candidate_id"], + "reference_bindings": { + "step_01.object": ["can"], + **({"step_01.target": ["table"]} if terminal == "place" else {}), + }, + "role_bindings": {}, + }, + [ + {"runtime_uid": "can", "init_pos": [0.0, -0.2, 0.8]}, + {"runtime_uid": "table", "init_pos": [0.0, 0.0, 0.72]}, + ], + ) + calls = [node["call"] for node in graph["nodes"]] + alignment = next( + call + for call in calls + if call.get("call_id") == "gen_sim.align_held" + and call["resources"] == {"primary": "right"} + and call["arguments"]["preserve_yaw"] is True + ) + assert alignment["resources"] == {"primary": "right"} + assert alignment["arguments"] == { + "object": "can", + "target": "current_object_pose", + "preserve_yaw": True, + } + if task_type == "E4": + source_alignment = next( + call + for call in calls + if call.get("call_id") == "gen_sim.align_held" + and call["resources"] == {"primary": "left"} + ) + assert calls.index(source_alignment) < next( + i for i, call in enumerate(calls) if call["kind"] == "hand_over" + ) + assert calls.index(alignment) > next( + i for i, call in enumerate(calls) if call["kind"] == "hand_over" + ) + if terminal == "place": + assert calls.index(alignment) < next( + i + for i, call in enumerate(calls) + if call.get("call_id") == "simulation.place_relative" + ) + assert ( + calls[-2]["call_id"] == "simulation.park" + if task_type == "E4" + else calls[-2]["call_id"] == "gen_sim.clear_released" + ) + else: + assert calls[-1] is alignment + + +def test_handover_continuation_does_not_pick_an_already_held_object() -> None: + first = _step(step_id="one", reference="can") + first.update( + task_type="E4", + orientation_goal="none", + required_arm="none", + transfer_arm="left_arm", + receive_arm="right_arm", + terminal_behavior="hold", + ) + second = deepcopy(first) + second.update( + id="two", + object=_selector("step_result", step_id="one"), + transfer_arm="right_arm", + receive_arm="left_arm", + depends_on=["one"], + ) + interpreted = _result(first) + interpreted.intent["steps"].append(second) + candidate = TaskAgent(interpreter=lambda *_a, **_kw: interpreted).generate( + "return_handover", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "return_handover", + "candidate_id": candidate["candidate_id"], + "reference_bindings": {"step_01.object": ["can"]}, + "role_bindings": {}, + }, + [{"runtime_uid": "can", "init_pos": [0.0, -0.2, 0.8]}], + ) + assert [node["call"]["kind"] for node in graph["nodes"]] == [ + "pick", + "hand_over", + "hand_over", + ] + + +def test_semantic_planner_preserves_e4_terminal_place() -> None: + """A handover-place task transfers and then releases at its requested relation.""" + + def interpreter(_instruction, **_kwargs): + step = _step(step_id="handover", reference="can") + step.update( + task_type="E4", + target=_selector("scene_ref", reference="tray"), + relation="inside", + required_arm="none", + transfer_arm="left_arm", + receive_arm="right_arm", + orientation_goal="none", + terminal_behavior="place", + ) + return _result(step) + + candidate = TaskAgent(interpreter=interpreter).generate( + "handover_place", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "handover_place", + "candidate_id": candidate["candidate_id"], + "reference_bindings": { + "step_01.object": ["can"], + "step_01.target": ["tray"], + }, + "role_bindings": {}, + }, + [ + {"runtime_uid": "can", "init_pos": [0.0, -0.2, 0.75]}, + {"runtime_uid": "tray", "init_pos": [0.0, 0.2, 0.75]}, + ], + ) + + assert [node["call"]["kind"] for node in graph["nodes"]] == [ + "pick", + "hand_over", + "place", + "registered", + "registered", + ] + assert graph["nodes"][2]["call"] == { + "kind": "place", + "object": "can", + "inside": "inside__tray__can", + "resources": {"primary": "right"}, + } + assert [node["call"]["resources"]["primary"] for node in graph["nodes"][3:]] == [ + "left", + "right", + ] + assert graph["task_groups"][0]["success"]["type"] == "semantic_goal" + + +@pytest.mark.parametrize( + ("terminal_behavior", "call_id", "expected_displacement", "cleanup_count"), + [ + ("hold", "simulation.coordinated_hold", [0.0, 0.0, 0.14], 0), + ( + "place", + "simulation.coordinated_transport", + [-0.14 / 2**0.5, 0.14 / 2**0.5, 0.0], + 2, + ), + ], +) +def test_semantic_planner_preserves_e5_direction_and_terminal_behavior( + terminal_behavior: str, + call_id: str, + expected_displacement: list[float], + cleanup_count: int, +) -> None: + def interpreter(_instruction, **_kwargs): + step = _step(step_id="transport", reference="tray") + step.update( + task_type="E5", + required_arm="none", + orientation_goal="none", + direction=("up" if terminal_behavior == "hold" else "front_right"), + terminal_behavior=terminal_behavior, + ) + return _result(step) + + candidate = TaskAgent(interpreter=interpreter).generate( + "coordinated", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + graph = SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "coordinated", + "candidate_id": candidate["candidate_id"], + "reference_bindings": {"step_01.object": ["tray"]}, + "role_bindings": {}, + }, + [{"runtime_uid": "tray", "init_pos": [0.0, 0.0, 0.75]}], + ) + + call = graph["nodes"][0]["call"] + assert call["call_id"] == call_id + assert call["arguments"]["world_displacement"] == pytest.approx( + expected_displacement + ) + assert len(graph["nodes"]) == 1 + cleanup_count + assert graph["task_groups"][0]["success"]["type"] == ( + "held_by_both_grippers" if terminal_behavior == "hold" else "semantic_goal" + ) + + +@pytest.mark.parametrize( + ("task_type", "target_state"), [("E6", "open"), ("E7", "closed")] +) +def test_semantic_planner_rejects_out_of_scope_articulation_slide( + task_type: str, + target_state: str, +) -> None: + def interpreter(_instruction, **_kwargs): + step = _step(step_id="drawer", reference="drawer") + step.update( + task_type=task_type, + required_arm="left_arm", + orientation_goal="none", + target_state=target_state, + ) + return _result(step) + + candidate = TaskAgent(interpreter=interpreter).generate( + "drawer_task", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + with pytest.raises(UnsupportedSemanticCapabilityError, match="only E1-E5"): + SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "drawer_task", + "candidate_id": candidate["candidate_id"], + "reference_bindings": {"step_01.object": ["drawer"]}, + "role_bindings": {}, + }, + [{"runtime_uid": "drawer", "role": "articulation", "init_pos": [0, 0, 0]}], + ) + + +@pytest.mark.parametrize( + ("task_type", "call_id", "argument_key", "argument_value"), + [ + ("E8", "simulation.articulation_link_twist", "target_setting", 2), + ("E9", "simulation.articulation_link_press", "target_state", "activated"), + ], +) +def test_semantic_planner_rejects_out_of_scope_calibrated_articulation_call( + task_type: str, + call_id: str, + argument_key: str, + argument_value: object, +) -> None: + def interpreter(_instruction, **_kwargs): + step = _step(step_id="control", reference="control") + step.update( + task_type=task_type, + required_arm="right_arm", + orientation_goal="none", + ) + step[argument_key] = argument_value + return _result(step) + + candidate = TaskAgent(interpreter=interpreter).generate( + "control_task", _TEST_INSTRUCTION, candidate_count=1 + )["candidates"][0] + with pytest.raises(UnsupportedSemanticCapabilityError, match="only E1-E5"): + SemanticTaskPlanner().plan( + candidate, + { + "schema_version": ROLE_BINDINGS_SCHEMA, + "task_id": "control_task", + "candidate_id": candidate["candidate_id"], + "reference_bindings": {"step_01.object": ["control"]}, + "role_bindings": {}, + }, + [{"runtime_uid": "control", "role": "articulation", "init_pos": [0, 0, 0]}], + ) diff --git a/tests/gen_sim/task_engine/test_bundle_runner.py b/tests/gen_sim/task_engine/test_bundle_runner.py new file mode 100644 index 000000000..f063d1c7d --- /dev/null +++ b/tests/gen_sim/task_engine/test_bundle_runner.py @@ -0,0 +1,212 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for the isolated Task Program bundle subprocess boundary.""" + +from __future__ import annotations + +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from embodichain.gen_sim.task_engine import _bundle_runner +from embodichain.gen_sim.task_engine._bundle_runner import _exception_metadata + + +@pytest.mark.parametrize("argv, expected_seed", [([], 0), (["--seed", "7"], 7)]) +def test_runner_parser_preserves_seed_with_shared_launcher( + argv: list[str], expected_seed: int +) -> None: + """Compose the real launcher without registering its seed argument twice.""" + parser = _bundle_runner._runner_parser() + + assert parser.parse_args(argv).seed == expected_seed + + +def test_exception_metadata_preserves_explicit_causal_chain() -> None: + """The report retains the physical planner error hidden by demo cleanup.""" + try: + try: + raise ValueError("invalid coordinated trajectory") + except ValueError as planner_error: + raise RuntimeError("demo safe-stop completed") from planner_error + except RuntimeError as runtime_error: + metadata = _exception_metadata(runtime_error) + + assert metadata == { + "type": "RuntimeError", + "message": "demo safe-stop completed", + "causes": [ + { + "type": "ValueError", + "message": "invalid coordinated trajectory", + } + ], + } + + +def test_exception_metadata_rejects_non_exception_values() -> None: + """The private serializer fails closed on an invalid diagnostic value.""" + with pytest.raises(TypeError, match="BaseException"): + _exception_metadata("failure") # type: ignore[arg-type] + + +@pytest.mark.parametrize( + "fingerprint", + [ + {}, + {"schema_version": "semantic_integration_fingerprint/v1"}, + { + "schema_version": "semantic_integration_fingerprint/v2", + "adapter_contract": "gen_sim.task_program/2620929c/v2", + }, + { + "schema_version": "semantic_integration_fingerprint/v2", + "adapter_contract": "unknown", + }, + ], +) +def test_old_bundle_is_rejected_before_component_loading( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, fingerprint: dict +) -> None: + def forbidden_load(*_args, **_kwargs): + raise AssertionError("Old contracts must fail before component loading.") + + monkeypatch.setattr(_bundle_runner, "load_config", forbidden_load) + with pytest.raises(ValueError, match="regenerate"): + _bundle_runner._verify_integration_fingerprint( + tmp_path, tmp_path / "deployment.yaml", {}, fingerprint + ) + + +def test_execution_report_preserves_partial_row_success() -> None: + """A normal partial result remains eligible for Task Engine any/at-least.""" + graph = { + "task_id": "partial", + "integration_fingerprint": "0" * 64, + "nodes": [{"id": "step_01"}], + "task_groups": [{"id": "group_01", "node_ids": ["step_01"]}], + } + runtime_result = { + "segments": [ + { + "name": "step_01", + "active": [True, True], + "successes": [True, False], + } + ] + } + + report = _bundle_runner._build_execution_report( + graph, + runtime_result, + row_success=[True, False], + terminal_reasons=["success", "task_incomplete"], + failure=None, + trajectory_root=Path("trajectory"), + ) + + assert report["status"] == "failed" + assert [row["success"] for row in report["environments"]] == [True, False] + assert report["failure"] is None + + +def test_execution_report_masks_rows_after_global_failure() -> None: + """An infrastructure failure invalidates every row in the attempt.""" + graph = { + "task_id": "failed", + "integration_fingerprint": "0" * 64, + "nodes": [], + "task_groups": [], + } + + report = _bundle_runner._build_execution_report( + graph, + None, + row_success=[True, False], + terminal_reasons=["success", "runtime_failed"], + failure={"type": "RuntimeError", "message": "transport failed"}, + trajectory_root=Path("trajectory"), + ) + + assert [row["success"] for row in report["environments"]] == [False, False] + + +@pytest.mark.parametrize("capture_fails", [False, True]) +def test_failed_attempt_captures_a_terminal_frame_before_flush( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, capture_fails: bool +) -> None: + from embodichain.lab.gym.envs.managers import record + + calls = [] + + class Recorder: + def __call__(self, env, env_ids, **params): + calls.append(("capture", env, env_ids, params)) + if capture_fails: + raise RuntimeError("camera fetch failed") + + def save_and_clear(self): + calls.append(("flush",)) + + monkeypatch.setattr(record, "record_camera_data", Recorder) + params = {"name": "audience", "resolution": [640, 360]} + env = SimpleNamespace( + event_manager=SimpleNamespace( + _mode_functor_cfgs={ + "interval": [SimpleNamespace(func=Recorder(), params=params)] + } + ) + ) + _bundle_runner._preserve_failed_execution_recording(env, tmp_path, num_envs=1) + + assert calls == [("capture", env, None, params), ("flush",)] + + +def test_module_entrypoint_flushes_protocol_before_fast_exit( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """The simulator worker skips native interpreter-order destruction.""" + flushes: list[str] = [] + exit_codes: list[int] = [] + + monkeypatch.setattr(_bundle_runner, "main", lambda: 7) + monkeypatch.setattr( + _bundle_runner.sys, + "stdout", + SimpleNamespace(flush=lambda: flushes.append("stdout")), + ) + monkeypatch.setattr( + _bundle_runner.sys, + "stderr", + SimpleNamespace(flush=lambda: flushes.append("stderr")), + ) + + def fake_exit(exit_code: int) -> None: + exit_codes.append(exit_code) + raise SystemExit(exit_code) + + monkeypatch.setattr(_bundle_runner.os, "_exit", fake_exit) + + with pytest.raises(SystemExit, match="7"): + _bundle_runner._module_entrypoint() + + assert flushes == ["stdout", "stderr"] + assert exit_codes == [7] + + +__all__: list[str] = [] diff --git a/tests/gen_sim/task_engine/test_grasp_filter.py b/tests/gen_sim/task_engine/test_grasp_filter.py new file mode 100644 index 000000000..470af686d --- /dev/null +++ b/tests/gen_sim/task_engine/test_grasp_filter.py @@ -0,0 +1,166 @@ +# ---------------------------------------------------------------------------- +# 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 grasp filters preserve baseline candidate and failure contracts.""" + +from __future__ import annotations + + +import pytest +import torch + +from embodichain.gen_sim.task_engine._task_program.grasp_filter import ( + GraspRule, + TaskGraspPoseGenerator, + accepted_candidates, + geometry_key, +) +from embodichain.toolkits.graspkit import ( + ParallelJawGraspPoseGenerator, + ParallelJawGripperModelCfg, +) + +__all__: list[str] = [] + +VERTICES = torch.tensor([[-0.02, 0.0, 0.0], [0.02, 0.0, 0.20], [0.0, 0.01, 0.10]]) +TRIANGLES = torch.tensor([[0, 1, 2]]) + + +class CandidateGenerator(ParallelJawGraspPoseGenerator): + def __init__(self, rows: tuple[tuple[float, ...], ...]) -> None: + super().__init__( + ParallelJawGripperModelCfg( + model_id="test", + finger_length=0.10, + finger_width=0.02, + finger_thickness=0.01, + ) + ) + self.rows = rows + + def get_valid_grasp_poses(self, **kwargs): + poses = kwargs["obj_poses"] + results = [] + for row, heights in enumerate(self.rows): + local = torch.eye(4).repeat(len(heights), 1, 1) + local[:, 2, 3] = torch.tensor(heights) + results.append( + (poses[row] @ local, torch.arange(len(heights), dtype=torch.float32)) + ) + return results + + def get_best_grasp_poses(self, **kwargs): + poses = kwargs["obj_poses"] + return ( + torch.ones(len(poses), dtype=torch.bool), + poses.clone(), + torch.full((len(poses),), 0.05), + ) + + def get_dual_arm_valid_grasp_poses(self, **kwargs): + return [None] * len(kwargs["obj_poses"]) + + +def rule(*, upper_half=True, margin=0.02) -> GraspRule: + return GraspRule( + "can", + "release", + geometry_key(VERTICES, TRIANGLES), + (0.0, 0.0, 1.0), + 0.10, + upper_half, + tuple(torch.eye(4).reshape(-1).tolist()), + 0.0, + margin, + ) + + +def sample(provider, poses, **kwargs): + return provider.get_valid_grasp_poses( + mesh_vertices=VERTICES, + mesh_triangles=TRIANGLES, + obj_poses=poses, + approach_direction=torch.tensor([0.0, 0.0, -1.0]), + **kwargs, + ) + + +def test_filter_uses_object_local_region_independently_for_each_row() -> None: + poses = torch.eye(4).repeat(2, 1, 1) + poses[1, :3, :3] = torch.tensor( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]] + ) + before = poses.clone() + provider = TaskGraspPoseGenerator( + CandidateGenerator(((0.08, 0.15), (0.08,))), (rule(),) + ) + rows = sample(provider, poses) + assert rows[0][0].shape == (1, 4, 4) + assert rows[0][0][0, 2, 3].item() == pytest.approx(0.15) + assert torch.isfinite(rows[0][1]).all() + assert rows[1][0].shape == (1, 4, 4) + assert torch.isinf(rows[1][1]).all() + torch.testing.assert_close(poses, before) + + +def test_release_filter_rejects_a_low_grasp_before_stock_ik() -> None: + provider = TaskGraspPoseGenerator( + CandidateGenerator(((0.06, 0.10),)), (rule(upper_half=False),) + ) + rows = sample(provider, torch.eye(4).unsqueeze(0)) + assert rows[0][0][:, 2, 3].tolist() == pytest.approx([0.10]) + + +def test_symmetric_roll_variants_have_identical_release_clearance() -> None: + poses = torch.eye(4).repeat(2, 1, 1) + poses[:, 2, 3] = torch.tensor([0.06, 0.15]) + mirrored = poses.clone() + mirrored[:, :3, :2] *= -1 + model = CandidateGenerator(((),)).gripper_model + torch.testing.assert_close( + accepted_candidates(poses, torch.eye(4), rule(), model), + accepted_candidates(mirrored, torch.eye(4), rule(), model), + ) + + +def test_no_candidate_is_encoded_as_ineligible_not_as_a_fake_success() -> None: + provider = TaskGraspPoseGenerator(CandidateGenerator(((), (0.05,))), (rule(),)) + rows = sample(provider, torch.eye(4).repeat(2, 1, 1)) + assert all( + p.shape == (1, 4, 4) and torch.isfinite(p).all() and torch.isinf(c).all() + for p, c in rows + ) + + +def test_end_specific_sampling_keeps_its_baseline_contract() -> None: + provider = TaskGraspPoseGenerator(CandidateGenerator(((0.06, 0.10),)), (rule(),)) + rows = sample( + provider, + torch.eye(4).unsqueeze(0), + obj_longest_axis=torch.tensor([0.0, 0.0, 1.0]), + is_positive_part=False, + ) + assert rows[0][0].shape[0] == 2 + assert torch.isfinite(rows[0][1]).all() + + +def test_rule_identity_must_match_the_bound_object_and_geometry() -> None: + provider = TaskGraspPoseGenerator(CandidateGenerator(((0.15,),)), (rule(),)) + provider.require_rule("can", "release", VERTICES, TRIANGLES) + with pytest.raises(ValueError, match="matching"): + provider.require_rule("other", "release", VERTICES, TRIANGLES) + with pytest.raises(ValueError, match="matching"): + provider.require_rule("can", "release", VERTICES * 2, TRIANGLES) diff --git a/tests/gen_sim/task_engine/test_interpretation.py b/tests/gen_sim/task_engine/test_interpretation.py new file mode 100644 index 000000000..fbb53bb7d --- /dev/null +++ b/tests/gen_sim/task_engine/test_interpretation.py @@ -0,0 +1,117 @@ +# ---------------------------------------------------------------------------- +# 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 pathlib import Path +from unittest.mock import Mock + +import pytest + +from embodichain.gen_sim.task_engine import interpretation as interpretation_module + + +@pytest.mark.parametrize( + "settings", + [ + {"model": "mimo-v2-flash", "base_url": "https://api.xiaomimimo.com/v1"}, + {"model": "compatible-model", "base_url": "https://provider.example/v1"}, + ], + ids=["mimo", "other-openai-compatible"], +) +def test_structured_output_uses_json_mode(settings: dict[str, str]) -> None: + client = Mock() + schema = {"type": "object", "properties": {"task": {"type": "string"}}} + + result = interpretation_module._structured_output_runnable( + client, schema, settings=settings + ) + + client.with_structured_output.assert_called_once_with(schema, method="json_mode") + assert result is client.with_structured_output.return_value + + +def _write_dotenv(path: Path) -> None: + path.write_text( + "\n".join( + ( + "OPENAI_API_KEY=dotenv-key", + "OPENAI_BASE_URL=https://dotenv.example/v1", + "OPENAI_MODEL=dotenv-model", + ) + ), + encoding="utf-8", + ) + + +def _clear_process_provider(monkeypatch: pytest.MonkeyPatch) -> None: + for name in ( + "OPENAI_API_KEY", + "OPENAI_BASE_URL", + "OPENAI_API_BASE", + "LLM_URL", + "TASK_ENGINE_LLM_MODEL", + "ACTION_ENGINE_LLM_MODEL", + "OPENAI_MODEL", + "LLM_MODEL", + ): + monkeypatch.delenv(name, raising=False) + + +def test_partial_process_transport_does_not_mix_with_dotenv( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + env_path = tmp_path / ".env" + _write_dotenv(env_path) + monkeypatch.setattr(interpretation_module, "_GEN_SIM_ENV_PATH", env_path) + monkeypatch.setattr( + interpretation_module, + "_GEN_CONFIG_PATH", + tmp_path / "missing.json", + ) + _clear_process_provider(monkeypatch) + monkeypatch.setenv("OPENAI_API_KEY", "unrelated-process-key") + + settings = interpretation_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_complete_process_transport_overrides_dotenv( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + env_path = tmp_path / ".env" + _write_dotenv(env_path) + monkeypatch.setattr(interpretation_module, "_GEN_SIM_ENV_PATH", env_path) + monkeypatch.setattr( + interpretation_module, + "_GEN_CONFIG_PATH", + tmp_path / "missing.json", + ) + _clear_process_provider(monkeypatch) + monkeypatch.setenv("OPENAI_API_KEY", "process-key") + monkeypatch.setenv("OPENAI_BASE_URL", "https://process.example/v1/") + monkeypatch.setenv("TASK_ENGINE_LLM_MODEL", "process-model") + + settings = interpretation_module._load_llm_settings(model=None) + + assert settings["api_key"] == "process-key" + assert settings["base_url"] == "https://process.example/v1" + assert settings["model"] == "process-model" diff --git a/tests/gen_sim/task_engine/test_parallel_workflow.py b/tests/gen_sim/task_engine/test_parallel_workflow.py new file mode 100644 index 000000000..9caa30f05 --- /dev/null +++ b/tests/gen_sim/task_engine/test_parallel_workflow.py @@ -0,0 +1,987 @@ +# ---------------------------------------------------------------------------- +# 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 +import json +from pathlib import Path +import sys +from types import SimpleNamespace +from threading import Barrier + +import pytest + +from embodichain.gen_sim.scene_engine.errors import SceneServiceError +from embodichain.gen_sim.task_engine.config import ( + TaskEngineExecutionCfg, + TaskEnginePlanningCfg, + TaskEngineWorkflowCfg, +) +from embodichain.gen_sim.task_engine.orchestration.scene_adapter import ( + CandidateSelection, + SceneAdapterProtocolError, +) +from embodichain.gen_sim.task_engine.orchestration.scene_source import SceneSourceRef +from embodichain.gen_sim.task_engine.scene_backend import SceneAnalysis, SceneRevision +from embodichain.gen_sim.task_engine.workflow import ( + SubprocessActionExecutor, + TaskEngineWorkflow, + _environment_successes, + _run_streaming_process, +) +from embodichain.gen_sim.task_engine.workflow_contracts import TASK_RUN_REQUEST_SCHEMA + + +def _candidate_set() -> dict: + candidate = { + "candidate_id": "candidate_01", + "draft": { + "task_id": "place_can", + "instruction": "Place the can on the table.", + "steps": [ + { + "id": "place", + "task_type": "E1", + "object": { + "kind": "scene_ref", + "step_id": "", + "reference": "the can", + "quantifier": "one", + "count": 0, + }, + "target": { + "kind": "scene_ref", + "step_id": "", + "reference": "the table", + "quantifier": "one", + "count": 0, + }, + "depends_on": [], + } + ], + }, + } + return { + "task_id": "place_can", + "instruction": "Place the can on the table.", + "candidates": [candidate], + } + + +def _selection(candidate_set: Mapping[str, object]) -> CandidateSelection: + candidate = candidate_set["candidates"][0] + return CandidateSelection( + scene_manifest={}, + role_bindings={}, + binding_report={ + "status": "bound", + "selection_reason": "test", + "candidates": [{"candidate_id": "candidate_01", "status": "resolved"}], + }, + selected_candidate=candidate, + candidate_bindings={"candidate_01": {}}, + ) + + +def _request(tmp_path: Path, *, existing: bool = False, edit: bool = False) -> dict: + return { + "schema_version": TASK_RUN_REQUEST_SCHEMA, + "task_id": "place_can", + "task_instruction": "Place the can on the table.", + "image_path": None if existing else str(tmp_path / "input.png"), + "gym_project": str(tmp_path / "project") if existing else None, + "scene_edit_prompt": "Move the can left." if edit else None, + "output_dir": str(tmp_path / "run"), + } + + +class _TaskAgent: + def __init__(self, candidates: dict, barrier: Barrier | None = None) -> None: + self.candidates = candidates + self.barrier = barrier + + def generate(self, *_args, **_kwargs) -> dict: + if self.barrier is not None: + self.barrier.wait(timeout=2) + return self.candidates + + +class _SceneBackend: + def __init__( + self, + selection: CandidateSelection, + *, + input_kind: str = "image", + input_barrier: Barrier | None = None, + materialize_barrier: Barrier | None = None, + materialize_failures: int = 0, + selection_error: Exception | None = None, + ) -> None: + self.selection = selection + self.input_kind = input_kind + self.input_barrier = input_barrier + self.materialize_barrier = materialize_barrier + self.materialize_failures = materialize_failures + self.selection_error = selection_error + self.seeds: list[int] = [] + + def analyze(self, request, output_root) -> SceneAnalysis: + if self.input_barrier is not None: + self.input_barrier.wait(timeout=2) + return SceneAnalysis( + input_kind=self.input_kind, + source=Path(request["image_path"] or request["gym_project"]), + blueprint=None, + source_fingerprint=None, + ) + + def select(self, *_args, **_kwargs) -> CandidateSelection: + if self.selection_error is not None: + raise self.selection_error + return self.selection + + def materialize( + self, _analysis, _request, output_root, *, seed: int + ) -> SceneRevision: + if self.materialize_barrier is not None: + self.materialize_barrier.wait(timeout=2) + root = Path(output_root) + root.mkdir(parents=True) + self.seeds.append(seed) + if len(self.seeds) <= self.materialize_failures: + raise SceneServiceError("scene service failed") + source = root / "scene_config.json" + source.write_text("{}\n", encoding="utf-8") + return SceneRevision( + source=source, + output_root=root, + revision_id="0" * 64, + seed=seed, + edit_plan=None, + source_fingerprint=None, + ) + + def inspect(self, revision, output_path): + value = { + "schema_version": "embodichain.final-scene-inspection/v1", + "scene_revision_id": revision.revision_id, + "source_config_path": revision.source.as_posix(), + "contact_tolerance_m": 0.03, + "objects": [], + } + path = Path(output_path) + path.write_text(json.dumps(value), encoding="utf-8") + return value + + +class _Coordinator: + def __init__( + self, + statuses: list[str], + *, + infeasible_remediation: str = "scene_remediable", + ) -> None: + self.statuses = list(statuses) + self.infeasible_remediation = infeasible_remediation + self.calls = 0 + self.kwargs: list[dict] = [] + self.sources: list[object] = [] + + def prepare(self, _task_id, _instruction, _source, output_dir, **_kwargs): + status = self.statuses[min(self.calls, len(self.statuses) - 1)] + self.calls += 1 + self.kwargs.append(dict(_kwargs)) + self.sources.append(_source) + root = Path(output_dir) + root.mkdir(parents=True) + for name in ( + "conservative_scene_graph.json", + "semantic_task_graph.json", + "task_program_deployment.yaml", + ): + (root / name).write_text("{}\n", encoding="utf-8") + return SimpleNamespace( + status=status, + output_dir=root, + planning_attempts=(), + feasibility_report=( + {"remediation_class": self.infeasible_remediation} + if status == "infeasible" + else None + ), + selected_candidate_id="candidate_01" if status == "bound" else None, + ) + + +class _FailingCoordinator: + def prepare(self, *_args, **_kwargs): + raise RuntimeError("grounding service unavailable") + + +class _RebindingCoordinator(_Coordinator): + def __init__(self, final_candidate: Mapping[str, object]) -> None: + super().__init__(["bound"]) + self.final_candidate = final_candidate + + def prepare(self, *args, **kwargs): + result = super().prepare(*args, **kwargs) + result.selected_candidate_id = str(self.final_candidate["candidate_id"]) + result.unbound_action_plan = { + "schema_version": "semantic_task_plan_draft/v1", + "task_id": str(self.final_candidate["draft"]["task_id"]), + "candidate_id": str(self.final_candidate["candidate_id"]), + "steps": [ + { + "id": str(step["id"]), + "task_type": str(step["task_type"]), + "depends_on": list(step["depends_on"]), + } + for step in self.final_candidate["draft"]["steps"] + ], + } + return result + + +class _InvalidSceneBackend(_SceneBackend): + def materialize(self, *_args, **kwargs): + self.seeds.append(int(kwargs["seed"])) + raise ValueError("invalid deterministic scene input") + + +class _Executor: + def __init__( + self, + successes: list[list[bool]], + *, + expected_dataset_saving: bool = False, + expected_open_window: bool = False, + ) -> None: + self.successes = successes + self.expected_dataset_saving = expected_dataset_saving + self.expected_open_window = expected_open_window + self.calls = 0 + + def __call__( + self, + _bundle, + _output_root, + *, + seed: int, + num_envs: int, + dataset_saving: bool = False, + failure_policy: str = "stop", + open_window: bool = False, + ): + values = self.successes[min(self.calls, len(self.successes) - 1)] + self.calls += 1 + assert len(values) == num_envs + assert dataset_saving is self.expected_dataset_saving + assert open_window is self.expected_open_window + assert failure_policy == "stop" + return { + "status": "succeeded" if all(values) else "failed", + "seed": seed, + "environments": [ + {"env_id": str(index), "success": success} + for index, success in enumerate(values) + ], + } + + +def test_parallel_workflow_propagates_open_window(tmp_path: Path) -> None: + candidates = _candidate_set() + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=_SceneBackend(_selection(candidates)), + coordinator=_Coordinator(["bound"]), + action_executor=_Executor( + [[True]], + expected_open_window=True, + ), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(), + planning_cfg=TaskEnginePlanningCfg(), + execution_cfg=TaskEngineExecutionCfg(num_envs=1), + open_window=True, + ) + + assert result.succeeded + + +@pytest.mark.parametrize("existing", [False, True]) +@pytest.mark.parametrize("edit", [False, True]) +def test_parallel_workflow_supports_all_four_scene_inputs( + tmp_path: Path, + *, + existing: bool, + edit: bool, +) -> None: + candidates = _candidate_set() + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=_SceneBackend( + _selection(candidates), + input_kind="gym_project" if existing else "image", + ), + coordinator=_Coordinator(["bound"]), + action_executor=_Executor( + [[True, False, False, False]], + expected_dataset_saving=True, + ), + ) + + result = workflow.run( + _request(tmp_path, existing=existing, edit=edit), + workflow_cfg=TaskEngineWorkflowCfg(), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + dataset_saving=True, + ) + + assert result.succeeded + + +def test_candidate_selection_programming_error_is_internal_failure( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + scene = _SceneBackend( + _selection(candidates), + selection_error=AttributeError("stale cross-engine field"), + ) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator(["bound"]), + action_executor=_Executor([[True]]), + ) + + result = workflow.run(_request(tmp_path)) + + assert result.status == "failed" + assert result.failure_class == "internal_error" + state = json.loads( + (result.output_dir / "workflow_state.json").read_text(encoding="utf-8") + ) + assert state["events"][-1]["reason"] == "stale cross-engine field" + + +def test_candidate_selection_protocol_error_is_not_input_conflict( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + scene = _SceneBackend( + _selection(candidates), + selection_error=SceneAdapterProtocolError("invalid grounding response"), + ) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator(["bound"]), + action_executor=_Executor([[True]]), + ) + + result = workflow.run(_request(tmp_path)) + + assert result.status == "failed" + assert result.failure_class == "candidate_selection" + + +def test_parallel_workflow_preserves_requested_robot_profile(tmp_path: Path) -> None: + candidates = _candidate_set() + coordinator = _Coordinator(["bound"]) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_adapter=SimpleNamespace(robot_profile="ur10"), + scene_backend=_SceneBackend(_selection(candidates)), + coordinator=coordinator, + action_executor=_Executor([[True]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(), + execution_cfg=TaskEngineExecutionCfg(num_envs=1), + ) + + assert result.succeeded + assert isinstance(coordinator.sources[0], SceneSourceRef) + assert coordinator.sources[0].robot_profile == "ur10" + + +def test_parallel_workflow_accepts_one_success_and_publishes_all_graphs( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + input_barrier = Barrier(2) + scene = _SceneBackend( + _selection(candidates), + input_barrier=input_barrier, + ) + coordinator = _Coordinator(["bound"]) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates, input_barrier), + scene_backend=scene, + coordinator=coordinator, + action_executor=_Executor([[False, True, False, False]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(), + planning_cfg=TaskEnginePlanningCfg( + candidate_count=3, + planning_mode="offline", + max_episodes=1, + max_episode_steps=6000, + ), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + base_seed=11, + run_id="20260820_072436", + ) + + assert result.succeeded + assert scene.seeds == [11] + assert result.final_bundle is not None + assert (result.final_bundle / "conservative_scene_graph.json").is_file() + assert (result.final_bundle / "semantic_task_graph.json").is_file() + assert (result.final_bundle / "task_program_deployment.yaml").is_file() + manifest = json.loads(result.manifest_path.read_text(encoding="utf-8")) + assert manifest["run_id"] == "20260820_072436" + assert manifest["configuration"]["planning"] == { + "candidate_count": 3, + "planning_mode": "offline", + "max_episodes": 1, + "max_episode_steps": 6000, + } + assert manifest["configuration"]["execution"]["dataset_saving"] is False + assert coordinator.kwargs[0]["max_episode_steps"] == 6000 + assert coordinator.kwargs[0]["final_inspection"]["scene_revision_id"] == "0" * 64 + assert ( + coordinator.kwargs[0]["unbound_action_plan"]["candidate_id"] == "candidate_01" + ) + assert manifest["attempts"][0]["action_attempts"][0]["status"] == "succeeded" + assert manifest["attempts"][0]["final_unbound_action_plan"]["candidate_id"] == ( + "candidate_01" + ) + assert manifest["attempts"][0]["unbound_transition"]["changed"] is False + state = json.loads(result.state_path.read_text(encoding="utf-8")) + succeeded = [ + event["stage"] for event in state["events"] if event["to"] == "succeeded" + ] + assert ( + succeeded.index("scene_finalization") + < succeeded.index("final_inspection") + < succeeded.index("final_binding") + ) + + +def test_prepare_only_publishes_bundle_without_action_execution( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + + def fail_execution(*_args, **_kwargs): + pytest.fail("prepare-only workflow must not execute Action Engine") + + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=_SceneBackend(_selection(candidates)), + coordinator=_Coordinator(["bound"]), + action_executor=fail_execution, + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(), + execution_cfg=TaskEngineExecutionCfg(), + execute=False, + ) + + assert result.status == "prepared" + assert result.final_bundle is not None + assert result.final_bundle.is_dir() + + +def test_final_candidate_rebinding_updates_attempt_unbound_audit( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + final_candidate = deepcopy(candidates["candidates"][0]) + final_candidate["candidate_id"] = "candidate_02" + candidates["candidates"].append(final_candidate) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=_SceneBackend(_selection(candidates)), + coordinator=_RebindingCoordinator(final_candidate), + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + ) + + manifest = json.loads(result.manifest_path.read_text(encoding="utf-8")) + attempt = manifest["attempts"][0] + assert attempt["unbound_action_plan"]["candidate_id"] == "candidate_01" + assert attempt["final_unbound_action_plan"]["candidate_id"] == "candidate_02" + assert attempt["unbound_transition"]["changed"] is True + + +@pytest.mark.parametrize( + ("dataset_saving", "expects_filter"), + [(False, True), (True, False)], +) +@pytest.mark.parametrize( + ("open_window", "expects_headless"), + [(False, True), (True, False)], +) +def test_subprocess_executor_controls_launch_options_and_copies_trajectory( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + dataset_saving: bool, + expects_filter: bool, + open_window: bool, + expects_headless: bool, +) -> None: + bundle = tmp_path / "bundle" + bundle.mkdir() + trajectory = tmp_path / "trajectory-source" + trajectory.mkdir() + (trajectory / "episode.json").write_text("{}\n", encoding="utf-8") + grasp_image = trajectory / "env_0000" / "grasp_poses" / "task_01.png" + grasp_image.parent.mkdir(parents=True) + grasp_image.write_bytes(b"grasp-pose-png") + captured = {} + + def fake_run(command, log_path): + captured["command"] = command + captured["log_path"] = Path(log_path) + Path(log_path).write_text("child output\n", encoding="utf-8") + report = { + "schema_version": "task_program_execution_report/v1", + "status": "succeeded", + "task_id": "place_can", + "semantic_call_count": 1, + "integration_fingerprint": "0" * 64, + "record_dir": trajectory.as_posix(), + "environments": [ + { + "env_id": index, + "success": True, + "terminal_reason": "success", + "semantic_success": {}, + } + for index in range(4) + ], + "runtime_result": {}, + "failure": None, + } + (Path(log_path).parent / "execution_report.json").write_text( + json.dumps(report), encoding="utf-8" + ) + return SimpleNamespace(returncode=0, stdout="ok", stderr="") + + monkeypatch.setattr( + "embodichain.gen_sim.task_engine.workflow._run_streaming_process", + fake_run, + ) + attempt = tmp_path / "attempt" + + report = SubprocessActionExecutor()( + bundle, + attempt, + seed=7, + num_envs=4, + dataset_saving=dataset_saving, + failure_policy="continue", + open_window=open_window, + ) + + assert report["status"] == "succeeded" + assert captured["command"][1:5] == [ + "-u", + "-m", + "embodichain.gen_sim.task_engine._bundle_runner", + "--bundle", + ] + assert " prepare" not in " ".join(captured["command"]) + assert " workflow" not in " ".join(captured["command"]) + assert ("--filter_dataset_saving" in captured["command"]) is expects_filter + assert ("--headless" in captured["command"]) is expects_headless + if expects_headless: + renderer_index = captured["command"].index("--renderer") + assert captured["command"][renderer_index : renderer_index + 2] == [ + "--renderer", + "fast-rt", + ] + else: + assert "--renderer" not in captured["command"] + assert "--show-grasp-poses" not in captured["command"] + assert captured["command"][-2:] == ["--failure-policy", "continue"] + assert captured["log_path"] == attempt / "action.log" + assert (attempt / "action.log").read_text(encoding="utf-8") == "child output\n" + assert (attempt / "trajectory" / "episode.json").is_file() + assert ( + attempt / "trajectory" / "env_0000" / "grasp_poses" / "task_01.png" + ).read_bytes() == b"grasp-pose-png" + process = json.loads((attempt / "process.json").read_text(encoding="utf-8")) + assert process["combined_log"] == "action.log" + assert process["stdout"] == "ok" + assert process["stderr"] == "" + + +def test_streaming_process_tees_combined_binary_output( + tmp_path: Path, + capfd: pytest.CaptureFixture[str], +) -> None: + log_path = tmp_path / "action.log" + script = ( + "import os; " + "os.write(1, b'stdout\\x00'); " + "os.write(2, b'stderr\\rprogress\\n'); " + "raise SystemExit(7)" + ) + + completed = _run_streaming_process( + [sys.executable, "-c", script], + log_path, + ) + + expected = b"stdout\x00stderr\rprogress\n" + assert completed.returncode == 7 + assert completed.stdout.encode("utf-8") == expected + assert completed.stderr == "" + assert log_path.read_bytes() == expected + terminal = capfd.readouterr().out + assert "stdout\x00" in terminal + assert "stderr\rprogress" in terminal + + +def test_scene_remediation_changes_seed_before_action_execution(tmp_path: Path) -> None: + candidates = _candidate_set() + scene = _SceneBackend(_selection(candidates)) + coordinator = _Coordinator(["infeasible", "bound"]) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=coordinator, + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(max_scene_attempts=2), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + base_seed=20, + ) + + assert result.succeeded + assert scene.seeds == [20, 21] + assert coordinator.calls == 2 + manifest = json.loads(result.manifest_path.read_text(encoding="utf-8")) + assert [item["status"] for item in manifest["attempts"]] == [ + "preparation_failed", + "succeeded", + ] + + +def test_input_conflict_feasibility_does_not_regenerate_scene(tmp_path: Path) -> None: + candidates = _candidate_set() + scene = _SceneBackend(_selection(candidates)) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator( + ["infeasible", "bound"], + infeasible_remediation="input_conflict", + ), + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(max_scene_attempts=2), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + ) + + assert result.failure_class == "input_conflict" + assert scene.seeds == [0] + + +def test_scene_service_retry_keeps_completed_unbound_plan(tmp_path: Path) -> None: + candidates = _candidate_set() + scene = _SceneBackend(_selection(candidates), materialize_failures=1) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator(["bound"]), + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(max_scene_attempts=2), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + base_seed=30, + ) + + assert result.succeeded + assert scene.seeds == [30, 31] + manifest = json.loads(result.manifest_path.read_text(encoding="utf-8")) + assert manifest["attempts"][0]["unbound_action_plan"] is not None + + +def test_nonremediable_scene_error_does_not_change_scene_attempt_seed( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + scene = _InvalidSceneBackend(_selection(candidates)) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator(["bound"]), + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(max_scene_attempts=3), + execution_cfg=TaskEngineExecutionCfg(), + base_seed=9, + ) + + assert not result.succeeded + assert scene.seeds == [9] + + +def test_execution_acceptance_requires_every_success_spec_term() -> None: + report = { + "environments": [ + { + "success": True, + "semantic_success": {"step_01": True, "step_02": False}, + }, + { + "success": True, + "semantic_success": {"step_01": True, "step_02": True}, + }, + ] + } + + assert _environment_successes( + report, + required_semantic_steps=("step_01", "step_02"), + ) == [False, True] + + +def test_unsupported_semantic_draft_retains_completed_parallel_scene( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + candidates["candidates"][0]["draft"]["steps"][0]["task_type"] = "E10" + scene = _SceneBackend(_selection(candidates)) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator(["bound"]), + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + ) + + assert not result.succeeded + assert result.failure_class == "action_capability" + assert scene.seeds == [0] + manifest = json.loads(result.manifest_path.read_text(encoding="utf-8")) + assert manifest["attempts"][0]["scene_revision"] is not None + state = json.loads(result.state_path.read_text(encoding="utf-8")) + assert state["stages"]["scene_finalization"] == "succeeded" + assert state["stages"]["unbound_action"] == "failed" + + +def test_preparation_exception_is_published_as_audited_failure(tmp_path: Path) -> None: + candidates = _candidate_set() + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=_SceneBackend(_selection(candidates)), + coordinator=_FailingCoordinator(), + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + ) + + assert not result.succeeded + assert result.failure_class == "preparation_error" + manifest = json.loads(result.manifest_path.read_text(encoding="utf-8")) + assert manifest["attempts"][0]["status"] == "preparation_error" + assert manifest["attempts"][0]["error"]["type"] == "RuntimeError" + + +def test_explicit_edit_may_materialize_initially_missing_reference( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + unresolved = CandidateSelection( + scene_manifest={}, + role_bindings={}, + binding_report={ + "status": "unsatisfied", + "selection_reason": "the can is not visible before the explicit edit", + "candidates": [{"candidate_id": "candidate_01", "status": "unsatisfied"}], + }, + selected_candidate=None, + candidate_bindings={"candidate_01": {}}, + ) + scene = _SceneBackend(unresolved, input_kind="gym_project") + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator(["bound"]), + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path, existing=True, edit=True), + workflow_cfg=TaskEngineWorkflowCfg(), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + ) + + assert result.succeeded + provisional = json.loads( + (result.output_dir / "provisional_candidate.json").read_text(encoding="utf-8") + ) + assert provisional == { + "binding_status": "unsatisfied", + "candidate_id": "candidate_01", + "reason": "explicit_scene_edit_may_materialize_missing_reference", + } + + +def test_action_failure_retries_action_only_and_retains_attempts( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + scene = _SceneBackend(_selection(candidates)) + executor = _Executor([[False, False, False, False]]) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator(["bound"]), + action_executor=executor, + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(max_action_attempts=3), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + ) + + assert not result.succeeded + assert result.failure_class == "action_execution" + assert scene.seeds == [0] + assert executor.calls == 3 + manifest = json.loads(result.manifest_path.read_text(encoding="utf-8")) + assert len(manifest["attempts"][0]["action_attempts"]) == 3 + + +def test_action_retry_stops_after_first_success(tmp_path: Path) -> None: + candidates = _candidate_set() + executor = _Executor( + [ + [False, False, False, False], + [True, True, True, True], + [True, True, True, True], + ] + ) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=_SceneBackend(_selection(candidates)), + coordinator=_Coordinator(["bound"]), + action_executor=executor, + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(max_action_attempts=3), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + ) + + assert result.succeeded + assert executor.calls == 2 + manifest = json.loads(result.manifest_path.read_text(encoding="utf-8")) + assert [item["status"] for item in manifest["attempts"][0]["action_attempts"]] == [ + "failed", + "succeeded", + ] + + +def test_existing_edit_binding_conflict_does_not_invent_scene_repair( + tmp_path: Path, +) -> None: + candidates = _candidate_set() + scene = _SceneBackend(_selection(candidates), input_kind="gym_project") + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator(["unsatisfied"]), + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path, existing=True, edit=True), + workflow_cfg=TaskEngineWorkflowCfg(max_scene_attempts=2), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + ) + + assert result.status == "input_conflict" + assert result.failure_class == "input_conflict" + assert scene.seeds == [0] + + +def test_image_binding_conflict_does_not_regenerate_scene(tmp_path: Path) -> None: + candidates = _candidate_set() + scene = _SceneBackend(_selection(candidates)) + workflow = TaskEngineWorkflow( + task_agent=_TaskAgent(candidates), + scene_backend=scene, + coordinator=_Coordinator(["unsatisfied"]), + action_executor=_Executor([[True, False, False, False]]), + ) + + result = workflow.run( + _request(tmp_path), + workflow_cfg=TaskEngineWorkflowCfg(max_scene_attempts=2), + execution_cfg=TaskEngineExecutionCfg(num_envs=4), + ) + + assert result.status == "input_conflict" + assert result.failure_class == "input_conflict" + assert scene.seeds == [0] diff --git a/tests/gen_sim/task_engine/test_run_directory.py b/tests/gen_sim/task_engine/test_run_directory.py new file mode 100644 index 000000000..e0305d59e --- /dev/null +++ b/tests/gen_sim/task_engine/test_run_directory.py @@ -0,0 +1,58 @@ +# ---------------------------------------------------------------------------- +# 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 datetime import datetime, timedelta, timezone +from pathlib import Path + +import pytest + +from embodichain.gen_sim.task_engine.run_directory import reserve_run_directory + +_NOW = datetime(2026, 8, 20, 7, 24, 36, tzinfo=timezone(timedelta(hours=8))) + + +def test_run_directory_uses_local_second_timestamp(tmp_path: Path) -> None: + root = tmp_path / "task2_2" + + with reserve_run_directory(root, now=_NOW) as allocation: + assert allocation.run_id == "20260820_072436" + assert allocation.path == root / "20260820_072436" + assert not allocation.path.exists() + allocation.path.mkdir() + + assert allocation.path.is_dir() + assert not (root / ".20260820_072436.reserve").exists() + + +def test_run_directory_adds_suffix_for_same_second_runs(tmp_path: Path) -> None: + root = tmp_path / "task2_2" + (root / "20260820_072436").mkdir(parents=True) + + with reserve_run_directory(root, now=_NOW) as first: + with reserve_run_directory(root, now=_NOW) as second: + assert first.run_id == "20260820_072436_01" + assert second.run_id == "20260820_072436_02" + + +def test_run_directory_rejects_naive_timestamp(tmp_path: Path) -> None: + with pytest.raises(ValueError, match="timezone"): + with reserve_run_directory( + tmp_path, + now=datetime(2026, 8, 20, 7, 24, 36), + ): + pass diff --git a/tests/gen_sim/task_engine/test_scene_backend.py b/tests/gen_sim/task_engine/test_scene_backend.py new file mode 100644 index 000000000..f93dff965 --- /dev/null +++ b/tests/gen_sim/task_engine/test_scene_backend.py @@ -0,0 +1,263 @@ +# ---------------------------------------------------------------------------- +# 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 json +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from embodichain.gen_sim.scene_engine.core.scene import Scene +from embodichain.gen_sim.scene_engine.core.scene_graph import ( + SceneGraph, + SceneGraphNode, +) +from embodichain.gen_sim.scene_engine.core.scene_object import SceneObject +from embodichain.gen_sim.scene_engine.pipeline.api import ( + SCENE_BLUEPRINT_SCHEMA, + SceneBlueprintPackage, + SceneMaterialization, +) +import embodichain.gen_sim.task_engine.scene_backend as scene_backend_module +from embodichain.gen_sim.task_engine.scene_backend import ( + SceneAnalysis, + SceneEngineBackend, + scene_blueprint_objects, +) +from embodichain.gen_sim.task_engine.workflow_contracts import TASK_RUN_REQUEST_SCHEMA + + +def _request(tmp_path: Path, project: Path, *, edit: str | None) -> dict: + return { + "schema_version": TASK_RUN_REQUEST_SCHEMA, + "task_id": "task", + "task_instruction": "Move the cup.", + "image_path": None, + "gym_project": project.as_posix(), + "scene_edit_prompt": edit, + "output_dir": (tmp_path / "run").as_posix(), + } + + +def _scene_export(tmp_path: Path) -> Path: + export = tmp_path / "project" / "scene_export" + assets = export / "mesh_assets" + assets.mkdir(parents=True) + (assets / "table.glb").write_bytes(b"glTF-table") + (assets / "cup.glb").write_bytes(b"glTF-cup") + (export / "scene_config.json").write_text( + json.dumps( + { + "format": "embodichain.scene-export/v1", + "scene_id": "scene", + "background": [ + { + "uid": "table", + "shape": { + "shape_type": "Mesh", + "fpath": "mesh_assets/table.glb", + }, + } + ], + "rigid_object": [ + { + "uid": "cup", + "shape": { + "shape_type": "Mesh", + "fpath": "mesh_assets/cup.glb", + }, + } + ], + } + ), + encoding="utf-8", + ) + return export.parent + + +def test_blueprint_objects_keep_pose_description_orientation_unknown( + tmp_path: Path, +) -> None: + scene = Scene( + objects=[ + SceneObject("table", "table", "table", "table", "A table."), + SceneObject("cup", "asset", "cup", "red cup", "A red cup."), + ] + ) + graph = SceneGraph( + nodes=[ + SceneGraphNode("table", None), + SceneGraphNode( + "cup", + "table", + "on", + pose_description="Lie flat on the support surface.", + ), + ] + ) + package = SceneBlueprintPackage( + schema_version=SCENE_BLUEPRINT_SCHEMA, + blueprint_id="blueprint", + image_path=tmp_path / "input.png", + output_root=tmp_path, + manifest_path=tmp_path / "scene_blueprint.json", + scene=scene, + scene_graph=graph, + ) + + objects = scene_blueprint_objects(package) + + cup = next(item for item in objects if item["uid"] == "cup") + assert cup["description"] == "A red cup." + assert cup["initial_state"] == {} + assert cup["affordances"] == [] + assert cup["init_pos"] == [0.0, 0.0, 0.0] + + +def test_backend_select_passes_v2_blueprint_contract(tmp_path: Path) -> None: + scene = Scene(objects=[SceneObject("table", "table", "table", "table", "A table.")]) + graph = SceneGraph(nodes=[SceneGraphNode("table", None)]) + package = SceneBlueprintPackage( + schema_version=SCENE_BLUEPRINT_SCHEMA, + blueprint_id="blueprint", + image_path=tmp_path / "input.png", + output_root=tmp_path, + manifest_path=tmp_path / "scene_blueprint.json", + scene=scene, + scene_graph=graph, + ) + analysis = SceneAnalysis( + input_kind="image", + source=package.image_path, + blueprint=package, + source_fingerprint=None, + ) + captured = {} + marker = object() + + class CapturingAdapter: + def select_objects(self, candidate_set, scene_objects, **kwargs): + captured["candidate_set"] = candidate_set + captured["scene_objects"] = scene_objects + captured.update(kwargs) + return marker + + result = SceneEngineBackend().select( + analysis, + {"candidate": "value"}, + CapturingAdapter(), + force_most_likely=True, + ) + + assert result is marker + assert captured["source_format"] == "embodichain.scene-blueprint/v2" + assert captured["scene_objects"][0]["uid"] == "table" + + +def test_existing_scene_edit_creates_revision_and_never_writes_source( + tmp_path: Path, + monkeypatch, +) -> None: + project = _scene_export(tmp_path) + source_config = project / "scene_export" / "scene_config.json" + source_value = json.loads(source_config.read_text(encoding="utf-8")) + articulation_path = project / "scene_export" / "cabinet.urdf" + articulation_path.write_text( + '\n', + encoding="utf-8", + ) + source_value["articulation"] = [ + { + "uid": "cabinet", + "name": "cabinet", + "description": "A fixed cabinet.", + "category": "cabinet", + "fpath": "cabinet.urdf", + } + ] + source_config.write_text(json.dumps(source_value), encoding="utf-8") + original = source_config.read_bytes() + prompts: list[str] = [] + + def fake_analyze_edit(*, output_root, edit_prompt): + prompts.append(edit_prompt) + return SimpleNamespace( + output_root=Path(output_root), + scene_edit_plan=SimpleNamespace( + to_dict=lambda: {"operations": [{"op": "move", "object_id": "cup"}]} + ), + ) + + def fake_materialize_edit(blueprint): + return SceneMaterialization( + scene=Scene(), + scene_graph=SceneGraph(nodes=[SceneGraphNode("table", None)]), + output_root=blueprint.output_root, + scene_config_path=blueprint.output_root + / "scene_export" + / "scene_config.json", + ) + + monkeypatch.setattr(scene_backend_module, "analyze_edit", fake_analyze_edit) + monkeypatch.setattr(scene_backend_module, "materialize_edit", fake_materialize_edit) + backend = SceneEngineBackend() + request = _request(tmp_path, project, edit="Move the cup left.") + analysis = backend.analyze(request, tmp_path / "analysis") + + revision = backend.materialize( + analysis, + request, + tmp_path / "revision", + seed=7, + ) + + assert prompts == ["Move the cup left."] + assert revision.source != source_config + assert revision.source.is_file() + assert len(revision.revision_id) == 64 + assert revision.edit_plan == {"operations": [{"op": "move", "object_id": "cup"}]} + assert source_config.read_bytes() == original + revision_config = json.loads(revision.source.read_text(encoding="utf-8")) + assert revision_config["articulation"][0]["uid"] == "cabinet" + audit = json.loads( + (tmp_path / "revision" / "scene_revision_attempt.json").read_text( + encoding="utf-8" + ) + ) + assert audit["seed"] == 7 + assert audit["revision_id"] == revision.revision_id + assert audit["edit_plan"] == revision.edit_plan + + +def test_final_inspection_rejects_scene_changed_after_revision(tmp_path: Path) -> None: + project = _scene_export(tmp_path) + backend = SceneEngineBackend() + request = _request(tmp_path, project, edit=None) + revision = backend.materialize( + backend.analyze(request, tmp_path / "analysis"), + request, + tmp_path / "unused", + seed=0, + ) + config_path = project / "scene_export" / "scene_config.json" + config = json.loads(config_path.read_text(encoding="utf-8")) + config["rigid_object"][0]["init_pos"] = [0.25, 0.0, 0.0] + config_path.write_text(json.dumps(config), encoding="utf-8") + + with pytest.raises(RuntimeError, match="changed before geometry inspection"): + backend.inspect(revision, tmp_path / "inspection.json") diff --git a/tests/gen_sim/task_engine/test_semantic_graph.py b/tests/gen_sim/task_engine/test_semantic_graph.py new file mode 100644 index 000000000..4fa32735c --- /dev/null +++ b/tests/gen_sim/task_engine/test_semantic_graph.py @@ -0,0 +1,1178 @@ +# ---------------------------------------------------------------------------- +# 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 pathlib import Path + +import numpy as np +from types import SimpleNamespace + +import trimesh +from embodichain.gen_sim.task_engine._task_program.configured import ( + decode_task_lowerer, +) + +import pytest + +import embodichain.gen_sim.task_engine.task_program_bundle as task_program_bundle +from embodichain.gen_sim.task_engine._bundle_runner import ( + _semantic_success_by_env, + _verify_program_projection, +) +from embodichain.gen_sim.task_engine.semantic_graph import ( + semantic_task_graph_hash, + validate_semantic_task_graph, +) +from embodichain.gen_sim.task_engine.task_program_bundle import ( + _bind_embodiment_to_scene, + _program_node, + generate_task_program_bundle, + _integration_payload, + _program_payload, + _refine_upright_targets, + _scene_payload, + _support_target_pose, +) +from embodichain.utils.utility import load_config, save_config +from embodichain.gen_sim.task_engine.orchestration.source_scene import PreparedScene + + +def _graph() -> dict: + return { + "schema_version": "semantic_task_graph/v1", + "task_id": "place_cube", + "instruction": "Place the cube", + "planner_route": "offline", + "integration_fingerprint": "0" * 64, + "targets": {}, + "nodes": [ + { + "id": "pick_cube", + "call": {"kind": "pick", "object": "cube"}, + "depends_on": [], + "task_instance_id": "pick_group", + "task_type": "E1", + "role": "primary", + }, + { + "id": "place_cube", + "call": { + "kind": "place", + "object": "cube", + "inside": "tray_inside", + }, + "depends_on": ["pick_cube"], + "task_instance_id": "place_group", + "task_type": "E1", + "role": "primary", + }, + ], + "task_groups": [ + { + "id": "pick_group", + "task_type": "E1", + "node_ids": ["pick_cube"], + "depends_on": [], + "success": {"kind": "call_completed"}, + }, + { + "id": "place_group", + "task_type": "E1", + "node_ids": ["place_cube"], + "depends_on": ["pick_group"], + "success": {"kind": "object_inside", "object": "cube"}, + }, + ], + "success": {"kind": "all_task_groups"}, + } + + +def _program(graph: dict) -> dict: + return { + "program_id": "place_cube", + "targets": deepcopy(graph["targets"]), + "program": { + "kind": "sequence", + "items": [ + { + "kind": "segment", + "name": node["id"], + "steps": {"kind": "invoke", "call": deepcopy(node["call"])}, + } + for node in graph["nodes"] + ], + }, + } + + +def test_semantic_graph_uses_canonical_calls_and_has_stable_hash() -> None: + graph = _graph() + validated = validate_semantic_task_graph(graph) + expected_hash = semantic_task_graph_hash(graph) + + graph["nodes"][0]["call"]["object"] = "mutated" + + assert validated["nodes"][0]["call"] == {"kind": "pick", "object": "cube"} + assert semantic_task_graph_hash(validated) == expected_hash + + +def test_semantic_graph_rejects_nested_grounded_execution_data() -> None: + graph = _graph() + graph["nodes"][0]["call"]["metadata"] = {"fallback": {"trajectory": [[0.0, 1.0]]}} + + with pytest.raises(ValueError, match="trajectory.*forbidden"): + validate_semantic_task_graph(graph) + + +def test_program_must_be_exact_projection_of_semantic_graph(tmp_path: Path) -> None: + graph = validate_semantic_task_graph(_graph()) + program_path = tmp_path / "program.yaml" + program = _program(graph) + save_config(program_path, program) + _verify_program_projection(program_path, graph) + + program["program"]["items"][1]["steps"]["call"]["object"] = "apple" + save_config(program_path, program) + with pytest.raises(ValueError, match="exact SemanticTaskGraph call projection"): + _verify_program_projection(program_path, graph) + + +def test_completed_task_groups_survive_later_runtime_failure() -> None: + graph = validate_semantic_task_graph(_graph()) + runtime_result = { + "segments": [ + { + "name": "pick_cube", + "active": [True], + "successes": [True], + }, + { + "name": "place_cube", + "active": [True], + "successes": [False], + }, + ] + } + + assert _semantic_success_by_env(graph, runtime_result, num_envs=1) == [ + {"pick_group": True, "place_group": False} + ] + + +def test_inside_place_waits_for_released_object() -> None: + node = { + "id": "place_cube", + "call": { + "kind": "place", + "object": "cube", + "inside": "inside__tray__cube", + }, + } + + program_node = _program_node(node, relative_routes={}) + + assert program_node["post"] == [ + { + "kind": "wait_stable", + "entity": "cube", + "preset": "contained_rigid_object", + }, + ] + + +def test_coordinated_transport_waits_for_observed_object_motion() -> None: + node = { + "id": "move_tray", + "call": { + "kind": "registered", + "call_id": "simulation.coordinated_transport", + "arguments": { + "object": "tray", + "target": "tray_forward", + }, + }, + } + + program_node = _program_node(node, relative_routes={}) + + assert program_node["post"] == [ + { + "kind": "wait_stable", + "entity": "tray", + "preset": "transported_rigid_object", + }, + ] + + +def test_coordinated_placement_checks_destination_after_release_and_cleanup() -> None: + graph = _graph() + graph["nodes"] = [ + { + "id": "transport", + "task_type": "E5", + "call": { + "kind": "registered", + "call_id": "simulation.coordinated_transport", + "arguments": { + "object": "tray", + "target": "forward", + "world_displacement": [-0.14, 0.0, 0.0], + }, + "resources": {"left": "left", "right": "right"}, + }, + }, + { + "id": "park", + "task_type": "E5", + "call": { + "kind": "registered", + "call_id": "simulation.park", + "arguments": {}, + }, + }, + ] + graph["task_groups"] = [{"node_ids": ["transport", "park"]}] + scene = SimpleNamespace( + planner_objects=({"runtime_uid": "tray", "init_pos": [0.0, 0.0, 0.75]},), + table_top_z=0.72, + ) + embodiment = {"skill_profile": {"resources": []}} + constraints = task_program_bundle._task_stability_payload(graph, scene, embodiment) + cfg = constraints["presets"]["gen_sim.transport.stable"] + assert cfg["kind"] == "placement" + assert cfg["target_position"] == [-0.14, 0.0, 0.75] + assert cfg["motion_parts"] == [] + program = _program_payload( + graph, "carry", stability_presets=set(constraints["presets"]) + ) + transport, park = program["program"]["items"] + assert park["post"] == [transport["post"][-1]] + assert park["post"][0]["preset"] == "gen_sim.transport.stable" + + +def test_cleanup_rechecks_public_position_validators_without_a_local_preset() -> None: + graph = _graph() + graph["nodes"] = [ + { + "id": "restore", + "task_type": "E3", + "call": { + "kind": "place", + "object": "bottle", + "at": {"kind": "target_ref", "target": "original"}, + }, + }, + { + "id": "park", + "task_type": "E3", + "call": { + "kind": "registered", + "call_id": "simulation.park", + "arguments": {}, + }, + }, + ] + graph["task_groups"] = [{"node_ids": ["restore", "park"]}] + restore, park = _program_payload(graph, "pour")["program"]["items"] + assert park["post"] == restore["post"] + assert park["validators"] == restore["validators"] + assert park["validators"][0]["kind"] == "object_near_target" + + +def test_relative_place_waits_and_validates_fresh_reference_pose() -> None: + node = { + "id": "place_cube_left_of_tray", + "call": { + "kind": "registered", + "call_id": "simulation.place_relative", + "arguments": { + "object": "cube", + "reference": "tray", + "relation": "left_of", + }, + }, + } + route = { + "object_id": "cube", + "reference_entity_id": "tray", + "relation": "left_of", + "world_displacement": [0.0, -0.12, 0.04], + } + + program_node = _program_node( + node, + relative_routes={("cube", "tray", "left_of"): route}, + ) + + assert program_node["post"][0]["kind"] == "wait_stable" + assert program_node["validators"] == [ + { + "kind": "object_near_relative_target", + "object": "cube", + "reference": "tray", + "displacement": [0.0, -0.12, 0.04], + "position_tolerance": 0.04, + } + ] + + +def test_phase_one_bundle_rejects_unsupported_robot_profile(tmp_path: Path) -> None: + with pytest.raises(ValueError, match="supports only dual_franka"): + generate_task_program_bundle( + _graph(), + object(), + tmp_path, + robot_profile="franka", + ) + + +@pytest.mark.parametrize("task_type", ["E6", "E7", "E8", "E9"]) +def test_bundle_rejects_out_of_scope_tasks_before_writing_assets( + tmp_path: Path, task_type: str +) -> None: + graph = _graph() + for node in graph["nodes"]: + node["task_type"] = task_type + for group in graph["task_groups"]: + group["task_type"] = task_type + output = tmp_path / "bundle" + with pytest.raises(ValueError, match="only E1-E5"): + generate_task_program_bundle(graph, None, output, robot_profile="dual_franka") + assert not output.exists() + + +def test_dual_franka_mount_is_bound_to_the_scene_table() -> None: + embodiment = {"simulation": {"init_pos": [-0.7, 0.0, 0.322894]}} + + _bind_embodiment_to_scene(embodiment, table_top_z=1.054499) + + assert embodiment["simulation"]["init_pos"] == pytest.approx([-0.7, 0.0, 0.704499]) + + +def test_horizontal_relation_distance_keeps_gripper_clearance( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Scene extents retain free space for a later parallel-jaw Pick.""" + meshes = { + "can": np.asarray( + [ + [-0.03, -0.03, -0.06], + [0.03, 0.03, 0.06], + ] + ), + "notebook": np.asarray( + [ + [-0.09, -0.07, -0.01], + [0.09, 0.07, 0.01], + ] + ), + } + monkeypatch.setattr( + task_program_bundle, + "_mesh_vertices", + lambda source: meshes[source["runtime_uid"]], + ) + + distance = task_program_bundle._horizontal_relation_distance( + {"runtime_uid": "can"}, + {"runtime_uid": "notebook", "init_rot": [0.0, 0.0, 0.0]}, + world_axis=1, + object_axis_aligned=True, + reference_axis_aligned=False, + minimum=0.10, + ) + + assert distance == pytest.approx(0.14) + + ordinary_distance = task_program_bundle._horizontal_relation_distance( + {"runtime_uid": "can", "init_rot": [0.0, 0.0, 0.0]}, + {"runtime_uid": "notebook", "init_rot": [0.0, 0.0, 0.0]}, + world_axis=1, + object_axis_aligned=False, + reference_axis_aligned=False, + minimum=0.10, + ) + + assert ordinary_distance == pytest.approx(0.12) + + +def test_e2_place_requires_task_owned_stability_and_shared_position_validation() -> ( + None +): + node = { + "id": "upright_can__call_04", + "task_instance_id": "upright_can", + "task_type": "E2", + "call": { + "kind": "registered", + "call_id": "simulation.place_relative", + "arguments": {"object": "can", "reference": "table", "relation": "on"}, + }, + } + route = { + "object_id": "can", + "reference_entity_id": "table", + "world_displacement": [0.0, 0.0, 0.1], + } + routes = {("can", "table", "on"): route} + with pytest.raises(ValueError, match="task-owned upright stability"): + _program_node(node, relative_routes=routes) + program_node = _program_node(node, relative_routes=routes, task_stability=True) + + assert program_node["post"] == [ + {"kind": "wait_stable", "entity": "can", "preset": "rigid_object"}, + { + "kind": "wait_stable", + "entity": "can", + "preset": "gen_sim.upright_can__call_04.stable", + }, + ] + assert program_node["validators"] == [ + { + "kind": "object_near_relative_target", + "object": "can", + "reference": "table", + "displacement": [0.0, 0.0, 0.1], + "position_tolerance": 0.04, + } + ] + + +def test_relative_place_waits_then_validates_live_relation() -> None: + node = { + "id": "place_behind", + "task_type": "E1", + "call": { + "kind": "registered", + "call_id": "simulation.place_relative", + "arguments": { + "object": "can", + "reference": "bottle", + "relation": "behind", + }, + "resources": {"primary": "right"}, + }, + } + + route = { + "object_id": "can", + "reference_entity_id": "bottle", + "relation": "behind", + "world_displacement": [0.18, 0.0, 0.05], + } + program_node = _program_node( + node, relative_routes={("can", "bottle", "behind"): route} + ) + + assert program_node["post"] == [ + {"kind": "wait_stable", "entity": "can", "preset": "rigid_object"} + ] + assert program_node["validators"] == [ + { + "kind": "object_near_relative_target", + "object": "can", + "reference": "bottle", + "displacement": [0.18, 0.0, 0.05], + "position_tolerance": 0.04, + } + ] + + +def test_upright_target_uses_the_normalized_mesh_origin_offset(tmp_path: Path) -> None: + mesh = trimesh.creation.box(extents=[0.06, 0.24, 0.06]) + mesh.apply_translation([0.0, 0.12, 0.0]) + mesh_path = tmp_path / "bottle.glb" + mesh.export(mesh_path) + graph = { + "schema_version": "semantic_task_graph/v1", + "task_id": "upright", + "instruction": "upright bottle", + "planner_route": "offline", + "integration_fingerprint": "0" * 64, + "targets": { + "step_upright_target": { + "kind": "cyclic_pose", + "values": [ + { + "position": [0.1, 0.2, 0.85], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + } + ], + }, + "step_upright_staging_target": { + "kind": "cyclic_pose", + "values": [ + { + "position": [0.1, 0.2, 0.85], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + } + ], + }, + }, + "nodes": [ + { + "id": "pick", + "call": { + "kind": "registered", + "call_id": "simulation.pick", + "arguments": {"object": "bottle", "target": "step_upright_target"}, + }, + "depends_on": [], + "task_instance_id": "step", + "task_type": "E2", + "role": "primary", + }, + { + "id": "stage", + "call": { + "kind": "registered", + "call_id": "simulation.move_held_object", + "arguments": { + "object": "bottle", + "target": "step_upright_staging_target", + }, + }, + "depends_on": ["pick"], + "task_instance_id": "step", + "task_type": "E2", + "role": "primary", + }, + { + "id": "move", + "call": { + "kind": "registered", + "call_id": "simulation.move_held_object", + "arguments": { + "object": "bottle", + "target": "step_upright_target", + }, + }, + "depends_on": ["stage"], + "task_instance_id": "step", + "task_type": "E2", + "role": "primary", + }, + ], + "task_groups": [ + { + "id": "step", + "task_type": "E2", + "node_ids": ["pick", "stage", "move"], + "depends_on": [], + "success": { + "kind": "semantic_task_term", + "step_id": "step", + "type": "object_upright", + }, + } + ], + "success": {"kind": "all_task_groups"}, + } + scene = SimpleNamespace( + table_top_z=0.72, + articulations=(), + planner_objects=( + { + "runtime_uid": "bottle", + "role": "rigid_object", + "shape": {"shape_type": "Mesh", "fpath": str(mesh_path)}, + "init_rot": [90.0, 0.0, 0.0], + }, + { + "runtime_uid": "table", + "role": "background", + "init_pos": [0.0, 0.0, 0.0], + "attributes": { + "final_world_aabb": { + "min": [-0.5, -0.5, 0.0], + "max": [0.5, 0.5, 0.72], + } + }, + }, + ), + ) + + refined = _refine_upright_targets(graph, scene) + + assert refined["targets"]["step_upright_target"]["values"][0][ + "position" + ] == pytest.approx([0.1, 0.2, 0.73]) + assert refined["targets"]["step_upright_staging_target"]["values"][0][ + "position" + ] == pytest.approx([0.1, 0.2, 0.93]) + + integration = _integration_payload( + refined, + scene, + program_id="upright", + scene_contract="upright_scene_v1", + ) + release_safe_options = integration["profile"]["action_options"]["simulation.pick"] + assert release_safe_options["kind"] == "pick_up" + assert release_safe_options["grasp_settle_steps"] == 16 + assert release_safe_options["pick_object_part"] == "center" + assert "object_axis_approach_weight" not in release_safe_options + lowerer = next( + item + for item in integration["runtime_services"]["registered_semantic_lowerers"] + if item["kind"] == "pick" + ) + route = lowerer["routes"][0] + assert route["grasp_region"] == "upper_half" + assert "approach_axis_weight" not in route + assert release_safe_options["approach_direction"] == pytest.approx( + [0.0, 2**-0.5, -(2**-0.5)] + ) + assert "required_object_target_poses" not in route + assert route["release_clearance_object_pose"]["position"] == pytest.approx( + [0.1, 0.2, 0.73] + ) + assert route["release_clearance_plane_z"] == pytest.approx(0.72) + assert route["release_clearance_safety_margin"] == pytest.approx(0.02) + bottle_binding = next( + item + for item in integration["scene_binding"]["rigid_objects"] + if item["entity_id"] == "bottle" + ) + assert bottle_binding["affordances"][0]["internal_axis"] == [0.0, -0.0, 1.0] + assert decode_task_lowerer(lowerer, path="lowerer").call_id == ("simulation.pick") + + +def test_generated_handover_uses_only_baseline_release_and_retreat_options() -> None: + graph = { + "schema_version": "semantic_task_graph/v1", + "task_id": "handover", + "instruction": "hand over can", + "planner_route": "offline", + "integration_fingerprint": "0" * 64, + "targets": {}, + "nodes": [ + { + "id": "handover", + "call": { + "kind": "hand_over", + "object": "can", + "resources": {"source": "left", "destination": "right"}, + }, + "depends_on": [], + "task_instance_id": "step", + "task_type": "E3", + "role": "primary", + } + ], + "task_groups": [], + "success": {"kind": "all_task_groups"}, + } + scene = SimpleNamespace( + table_top_z=0.72, + articulations=(), + planner_objects=( + { + "runtime_uid": "can", + "role": "rigid_object", + "shape": {"shape_type": "Cube", "size": [0.06, 0.06, 0.12]}, + }, + {"runtime_uid": "table", "role": "background"}, + ), + ) + + integration = _integration_payload( + graph, + scene, + program_id="handover", + scene_contract="handover_scene_v1", + ) + options = integration["profile"]["action_options"]["hand_over"] + + assert options["hand_interp_steps"] == 16 + assert options["hold_steps"] == 8 + assert "source_release_settle_steps" not in options + assert options["retreat_steps"] == 36 + assert options["retreat_distance"] == pytest.approx(0.10) + assert integration["runtime_services"]["handover_pose_providers"][0][ + "final_position" + ] == pytest.approx([0.0, -0.08, 0.913]) + + +def test_task_pick_directions_are_declared_per_policy_not_injected_by_lowerers() -> ( + None +): + graph = _graph() + graph["targets"] = { + name: { + "kind": "cyclic_pose", + "values": [ + {"position": [0.0, 0.0, 0.9], "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0]} + ], + } + for name in ("one", "two") + } + graph["nodes"] = [ + { + "id": name, + "task_type": "E2", + "call": { + "kind": "registered", + "call_id": f"gen_sim.pick.{name}", + "arguments": {"object": name, "target": name}, + }, + } + for name in ("one", "two") + ] + scene = SimpleNamespace( + table_top_z=0.72, + articulations=(), + planner_objects=( + { + "runtime_uid": "one", + "role": "rigid_object", + "init_pos": [0.0, -0.2, 0.8], + "init_rot": [90.0, 0.0, 0.0], + }, + { + "runtime_uid": "two", + "role": "rigid_object", + "init_pos": [0.0, 0.2, 0.8], + "init_rot": [-90.0, 0.0, 0.0], + }, + {"runtime_uid": "table", "role": "background", "init_pos": [0.0, 0.0, 0.0]}, + ), + ) + integration = _integration_payload( + graph, scene, program_id="directions", scene_contract="directions_scene" + ) + options = integration["profile"]["action_options"] + assert options["gen_sim.pick.one"]["approach_direction"] == pytest.approx( + [0.0, 2**-0.5, -(2**-0.5)] + ) + assert options["gen_sim.pick.two"]["approach_direction"] == pytest.approx( + [0.0, -(2**-0.5), -(2**-0.5)] + ) + factories = integration["runtime_services"]["registered_semantic_lowerers"] + assert {f["call_id"] for f in factories if f["kind"] == "pick"} == { + "gen_sim.pick.one", + "gen_sim.pick.two", + } + + +def test_upright_support_target_uses_the_mesh_top_not_half_extent( + tmp_path: Path, +) -> None: + support_mesh = trimesh.creation.box(extents=[0.06, 0.12, 0.06]) + support_mesh.apply_translation([0.0, 0.06, 0.0]) + support_path = tmp_path / "can.glb" + support_mesh.export(support_path) + child_mesh = trimesh.creation.box(extents=[0.10, 0.10, 0.10]) + child_path = tmp_path / "apple.glb" + child_mesh.export(child_path) + + pose = _support_target_pose( + {"shape": {"shape_type": "Mesh", "fpath": str(support_path)}}, + {"shape": {"shape_type": "Mesh", "fpath": str(child_path)}}, + axis_aligned=True, + ) + + assert pose[3] == pytest.approx(0.0) + assert pose[7] == pytest.approx(0.0) + assert pose[11] == pytest.approx(0.18) + + +def test_generated_usd_articulation_disables_urdf_only_pk_chain() -> None: + scene = SimpleNamespace( + table_top_z=0.72, + background=(), + rigid_objects=(), + articulations=( + { + "uid": "button", + "fpath": "/tmp/button.usdc", + "category": "button_box", + "name": "red button", + "proxy_glb_fpath": "/tmp/button.glb", + }, + ), + ) + + payload = _scene_payload(scene, program_id="press") + + articulation = payload["simulation"]["articulation"][0] + assert articulation["build_pk_chain"] is False + assert "category" not in articulation + assert "name" not in articulation + assert "proxy_glb_fpath" not in articulation + + +def _prepared_axis_scene(tmp_path: Path) -> PreparedScene: + mesh_path = tmp_path / "bottle.glb" + trimesh.creation.box(extents=[0.06, 0.24, 0.06]).export(mesh_path) + bottle = { + "uid": "bottle", + "shape": {"shape_type": "Mesh", "fpath": str(mesh_path)}, + "init_pos": [0.1, -0.2, 0.85], + "init_rot": [90.0, 0.0, 0.0], + } + table = { + "uid": "table", + "shape": {"shape_type": "Cube", "size": [1.0, 1.0, 0.72]}, + "init_pos": [0.0, 0.0, 0.36], + } + scene = PreparedScene( + source_config_path=tmp_path / "scene.yaml", + scene_dir=tmp_path, + planner_objects=( + {**bottle, "runtime_uid": "bottle", "role": "rigid_object"}, + { + **table, + "runtime_uid": "table", + "role": "background", + # PreparedScene already owns the measured tabletop; no duplicate AABB is needed. + }, + ), + background=(table,), + rigid_objects=(bottle,), + articulations=(), + uid_map={"bottle": "bottle", "table": "table"}, + table_top_z=0.72, + z_rotation_degrees=0.0, + body_scale_policy="preserve", + body_scale=(1.0, 1.0, 1.0), + asset_hashes={}, + ) + return scene + + +@pytest.mark.parametrize( + ("task_type", "terminal"), [("E1", "place"), ("E4", "hold"), ("E4", "place")] +) +def test_explicit_orientation_bundle_uses_shared_preflight_and_terminal_post( + tmp_path: Path, task_type: str, terminal: str +) -> None: + scene = _prepared_axis_scene(tmp_path) + graph = _graph() + resource = "left" if task_type == "E1" else "right" + calls = [{"kind": "pick", "object": "bottle", "resources": {"primary": "left"}}] + if task_type == "E4": + calls.append( + { + "kind": "hand_over", + "object": "bottle", + "resources": {"source": "left", "destination": "right"}, + } + ) + calls.append( + { + "kind": "registered", + "call_id": "gen_sim.align_held", + "arguments": { + "object": "bottle", + "target": "current_object_pose", + "preserve_yaw": True, + }, + "resources": {"primary": resource}, + } + ) + if terminal == "place": + graph["targets"] = { + "orientation_upright_staging_target": { + "kind": "cyclic_pose", + "values": [ + { + "position": [0.1, -0.2, 1.05], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + } + ], + } + } + calls.extend( + [ + { + "kind": "registered", + "call_id": "simulation.place_relative", + "arguments": { + "object": "bottle", + "reference": "table", + "relation": "on", + }, + "resources": {"primary": resource}, + }, + { + "kind": "registered", + "call_id": "gen_sim.clear_released", + "arguments": { + "object": "bottle", + "target": "orientation_upright_staging_target", + }, + "resources": {"primary": resource}, + }, + { + "kind": "registered", + "call_id": "simulation.park", + "arguments": {}, + "resources": {"primary": resource}, + }, + ] + ) + graph["nodes"] = [ + { + "id": f"call_{i}", + "task_type": task_type, + "task_instance_id": "orientation", + "role": "primary", + "depends_on": [] if i == 0 else [f"call_{i-1}"], + "call": call, + } + for i, call in enumerate(calls) + ] + graph["task_groups"] = [ + { + "id": "orientation", + "task_type": task_type, + "node_ids": [n["id"] for n in graph["nodes"]], + "depends_on": [], + "success": {"kind": "call_completed"}, + } + ] + generated, paths = generate_task_program_bundle( + graph, scene, tmp_path / "bundle", robot_profile="dual_franka" + ) + _verify_program_projection(paths.program, generated) + program = load_config(paths.program) + post = program["program"]["items"][-1]["post"][-1] + constraints = load_config(paths.program.parent / "constraints.json")["presets"] + assert constraints[post["preset"]]["kind"] == ( + "hold" if terminal == "hold" else "upright" + ) + assert constraints[post["preset"]]["local_axis"] == [0.0, -0.0, 1.0] + if terminal == "hold": + assert constraints[post["preset"]]["motion_parts"] == ["right_arm"] + assert "target_position" not in constraints[post["preset"]] + + +def test_stack_alignment_does_not_replace_support_acceptance_with_upright_only( + tmp_path: Path, +) -> None: + source_scene = _prepared_axis_scene(tmp_path) + objects = source_scene.planner_objects + scene = SimpleNamespace( + planner_objects=(*objects, {**objects[0], "runtime_uid": "support"}), + table_top_z=0.72, + ) + graph = _graph() + calls = [ + ("E2", "support_group", "simulation.axis_align", {"object": "support"}), + ( + "E1", + "stack_group", + "gen_sim.align_held", + {"object": "bottle", "target": "staging", "preserve_yaw": True}, + ), + ( + "E1", + "stack_group", + "gen_sim.stack_place", + {"object": "bottle", "reference": "support", "relation": "on"}, + ), + ] + graph["nodes"] = [ + { + "id": f"node_{i}", + "task_type": kind, + "task_instance_id": group, + "call": {"kind": "registered", "call_id": call_id, "arguments": args}, + } + for i, (kind, group, call_id, args) in enumerate(calls) + ] + graph["task_groups"] = [ + {"node_ids": ["node_0"]}, + {"node_ids": ["node_1", "node_2"]}, + ] + constraints = task_program_bundle._task_stability_payload( + graph, scene, {"skill_profile": {"resources": []}} + ) + assert constraints["presets"]["gen_sim.node_2.stable"]["kind"] == "stack" + + +@pytest.mark.parametrize( + "call_id", ["simulation.coordinated_hold", "simulation.coordinated_transport"] +) +def test_coordinated_bundle_composes_against_unmodified_public_options( + tmp_path: Path, call_id: str +) -> None: + scene = _prepared_axis_scene(tmp_path) + graph = _graph() + graph["nodes"] = [ + { + "id": "carry", + "task_instance_id": "carry", + "task_type": "E5", + "role": "primary", + "depends_on": [], + "call": { + "kind": "registered", + "call_id": call_id, + "arguments": { + "object": "bottle", + "target": "forward", + "world_displacement": [-0.1, 0.0, 0.05], + }, + "resources": {"left": "left", "right": "right"}, + }, + } + ] + graph["task_groups"] = [ + { + "id": "carry", + "task_type": "E5", + "node_ids": ["carry"], + "depends_on": [], + "success": {"kind": "call_completed"}, + } + ] + generated, paths = generate_task_program_bundle( + graph, scene, tmp_path / "bundle", robot_profile="dual_franka" + ) + _verify_program_projection(paths.program, generated) + integration = load_config(paths.integration) + assert ( + "source_release_settle_steps" + not in integration["profile"]["action_options"]["hand_over"] + ) + assert ( + load_config(paths.integration_fingerprint)["schema_version"] + == "semantic_integration_fingerprint/v2" + ) + + +def test_generated_e2_bundle_shares_release_route_with_axis_acceptance( + tmp_path: Path, +) -> None: + """The merged E2 route passes real configured composition and preflight.""" + scene = _prepared_axis_scene(tmp_path) + graph = _graph() + graph["task_id"] = "upright" + graph["targets"] = { + name: { + "kind": "cyclic_pose", + "values": [ + { + "position": [0.1, -0.2, 0.85], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + } + ], + } + for name in ("upright_upright_target", "upright_upright_staging_target") + } + calls = [ + ( + "simulation.pick", + {"object": "bottle", "target": "upright_upright_target"}, + ), + ( + "gen_sim.align_held", + { + "object": "bottle", + "target": "upright_upright_staging_target", + "preserve_yaw": False, + }, + ), + ( + "gen_sim.align_held", + { + "object": "bottle", + "target": "upright_upright_staging_target", + "preserve_yaw": True, + }, + ), + ( + "simulation.place_relative", + {"object": "bottle", "reference": "table", "relation": "on"}, + ), + ( + "gen_sim.clear_released", + {"object": "bottle", "target": "upright_upright_staging_target"}, + ), + ("simulation.park", {}), + ] + graph["nodes"] = [ + { + "id": f"call_{index}", + "task_instance_id": "upright", + "task_type": "E2", + "role": "primary", + "depends_on": [] if index == 0 else [f"call_{index - 1}"], + "call": { + "kind": "registered", + "call_id": call_id, + "arguments": arguments, + "resources": {"primary": "left"}, + }, + } + for index, (call_id, arguments) in enumerate(calls) + ] + graph["task_groups"] = [ + { + "id": "upright", + "task_type": "E2", + "depends_on": [], + "node_ids": [node["id"] for node in graph["nodes"]], + "success": {"kind": "call_completed"}, + } + ] + + generated, paths = generate_task_program_bundle( + graph, + scene, + tmp_path / "bundle", + robot_profile="dual_franka", + ) + + _verify_program_projection(paths.program, generated) + integration = load_config(paths.integration) + segments = load_config(paths.program)["program"]["items"] + assert generated["nodes"][0]["call"]["call_id"] == "gen_sim.pick.upright" + assert "gen_sim.pick.upright" in integration["profile"]["action_options"] + release = segments[-3] + terminal = segments[-1] + route = next( + item + for item in integration["runtime_services"]["registered_semantic_lowerers"] + if item["kind"] == "place_relative" + )["routes"][0] + assert release["validators"][0]["kind"] == "object_near_relative_target" + # Planning includes 1 cm release clearance; acceptance uses the support surface. + expected_displacement = list(route["world_displacement"]) + expected_displacement[2] -= 0.01 + assert release["validators"][0]["displacement"] == pytest.approx( + expected_displacement + ) + constraints = load_config(paths.program.parent / "constraints.json")["presets"] + preset = release["post"][-1]["preset"] + assert constraints[preset]["kind"] == "upright" + # The imported rotation is baked into the normalized mesh before binding. + assert constraints[preset]["local_axis"] == [0.0, -0.0, 1.0] + assert constraints[preset]["displacement"] == pytest.approx(expected_displacement) + assert len(release["validators"]) == 1 + assert terminal["steps"]["call"]["call_id"] == "simulation.park" + assert terminal["post"] == [release["post"][-1]] + assert terminal["validators"] == release["validators"] + assert set(release["steps"]["call"]["arguments"]) == { + "object", + "reference", + "relation", + } + assert generated["integration_fingerprint"] != "0" * 64 diff --git a/tests/gen_sim/task_engine/test_task_program_clearance.py b/tests/gen_sim/task_engine/test_task_program_clearance.py new file mode 100644 index 000000000..b34fd2db8 --- /dev/null +++ b/tests/gen_sim/task_engine/test_task_program_clearance.py @@ -0,0 +1,140 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Released-hand clearance binds poses without taking over execution.""" + +from __future__ import annotations + +from types import SimpleNamespace +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.gen_sim.task_engine._task_program.release_clearance import ( + _ClearReleasedFactory, + _ClearReleasedLowerer, + _clearance_poses, +) +from embodichain.lab.sim.atomic_actions import ( + EndEffectorPoseGoal, + MoveEndEffectorOptions, +) +from embodichain.lab.task_program.semantics import RegisteredSemanticCall + +__all__: list[str] = [] + + +def test_clearance_raises_before_withdrawing_and_preserves_each_row() -> None: + current = torch.eye(4).repeat(2, 1, 1) + current[:, 2, 3] = torch.tensor([0.8, 1.4]) + root = torch.eye(4).repeat(2, 1, 1) + root[:, 0, 3] = torch.tensor([1.0, -1.0]) + before = current.clone() + poses = _clearance_poses(current, root, height=1.2, retreat=0.1) + + assert poses.shape == (2, 2, 4, 4) + torch.testing.assert_close( + poses[:, :, 2, 3], torch.tensor([[1.2, 1.2], [1.4, 1.4]]) + ) + torch.testing.assert_close(poses[:, 0, :2, 3], current[:, :2, 3]) + torch.testing.assert_close(poses[:, 1, 0, 3], torch.tensor([0.1, -0.1])) + torch.testing.assert_close( + poses[:, :, :3, :3], current[:, None, :3, :3].expand(-1, 2, -1, -1) + ) + torch.testing.assert_close(current, before) + + +def _binding(): + endpoint = SimpleNamespace( + task_state_key="left", + runtime_target=SimpleNamespace(control_part="arm", joint_ids=(0, 1)), + ) + return SimpleNamespace( + binding=SimpleNamespace( + resources={"primary": SimpleNamespace(endpoints={"motion": endpoint})} + ) + ) + + +def test_clearance_rejects_a_live_attachment_before_kinematics() -> None: + robot = Mock() + lowerer = _ClearReleasedLowerer( + (("can", "safe", 1.2),), robot, retreat_distance=0.1 + ) + context = SimpleNamespace( + task=SimpleNamespace( + get_held_object=lambda key: SimpleNamespace( + active_mask=torch.tensor([True]) + ) + ) + ) + call = RegisteredSemanticCall( + call_id="gen_sim.clear_released", + arguments={"object": "can", "target": "safe"}, + ) + with pytest.raises(ValueError, match="cannot move a held object"): + lowerer.lower( + call, + context=context, + bound=_binding(), + option_template=MoveEndEffectorOptions(), + ) + assert robot.mock_calls == [] + + +def test_clearance_binds_one_shared_cartesian_goal_without_planning() -> None: + current = torch.eye(4).unsqueeze(0) + root = current.clone() + root[:, 0, 3] = 1.0 + robot = Mock() + robot.compute_fk.return_value = current + robot.get_link_pose.return_value = root + robot.cfg.solver_cfg = {"arm": SimpleNamespace(root_link_name="arm_root")} + lowerer = _ClearReleasedLowerer( + (("can", "safe", 1.2),), robot, retreat_distance=0.1 + ) + context = SimpleNamespace( + task=SimpleNamespace(get_held_object=lambda key: None), + robot=SimpleNamespace(qpos=torch.zeros(1, 2)), + env_ids=torch.tensor([0]), + ) + result = lowerer.lower( + RegisteredSemanticCall( + call_id="gen_sim.clear_released", + arguments={"object": "can", "target": "safe"}, + ), + context=context, + bound=_binding(), + option_template=MoveEndEffectorOptions(), + ) + assert type(result.goal) is EndEffectorPoseGoal + assert result.goal.xpos.shape == (1, 2, 4, 4) + assert result.registered_effect is None + robot.compute_ik.assert_not_called() + robot.plan.assert_not_called() + robot.step.assert_not_called() + + +def test_clearance_factory_requires_the_same_robot() -> None: + factory = _ClearReleasedFactory((("can", "safe", 1.2),)) + with pytest.raises(ValueError, match="factory's robot"): + factory.create( + simulation=None, + robot=object(), + scene_registry=Mock(), + engine=SimpleNamespace(robot=object()), + ) diff --git a/tests/gen_sim/task_engine/test_task_program_services.py b/tests/gen_sim/task_engine/test_task_program_services.py new file mode 100644 index 000000000..49e372d64 --- /dev/null +++ b/tests/gen_sim/task_engine/test_task_program_services.py @@ -0,0 +1,381 @@ +# ---------------------------------------------------------------------------- +# 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-owned service contracts migrated from shared integration tests.""" + +from __future__ import annotations + +from copy import deepcopy +from dataclasses import FrozenInstanceError +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.gen_sim.task_engine._task_program.align_held import _AlignHeldLowerer + +from embodichain.gen_sim.task_engine._task_program.configured import ( + decode_task_lowerer, +) +from embodichain.lab.task_program.integrations.configured import _decode_robot_profile +from embodichain.gen_sim.task_engine._task_program.services import ( + _AbsolutePoseTarget, + _CoordinatedHoldLowerer, + _CoordinatedTransportRoute, + _MoveHeldObjectLowerer, + _MoveHeldObjectRoute, + _PickLowerer, + _PickRoute, +) +from embodichain.lab.gym.utils._component_composition import _resolve_gym_components +from embodichain.lab.task_program.integrations._configured_composition import ( + _compose_integration_payload, + _resolve_task_program_components, +) +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + CoordinatedPickmentOptions, + EntityState, + GraspGoal, + HandOverOptions, + HeldObjectPoseGoal, + MoveHeldObjectOptions, + ObjectSemantics, + PickUpOptions, + PlanningContext, + RobotObservation, + SceneSnapshot, + TaskState, +) +from embodichain.lab.task_program.semantics import ( + HeldObjectRelation, + RegisteredSemanticCall, + SceneObjectRef, + SemanticEffectKind, +) +from embodichain.utils.utility import load_config + +__all__: list[str] = [] + + +def test_current_pose_upright_binding_preserves_each_environments_position() -> None: + poses = torch.eye(4).repeat(2, 1, 1) + poses[:, :3, 3] = torch.tensor([[0.1, -0.2, 1.0], [0.3, 0.2, 1.2]]) + before = poses.clone() + robot = Mock() + lowerer = _AlignHeldLowerer( + (("can", "current_object_pose", True, (1.0, 0.0, 0.0), None),), robot + ) + context = SimpleNamespace( + batch_size=2, + task=SimpleNamespace( + get_held_object=lambda key: SimpleNamespace( + semantics=SimpleNamespace(entity_id="can") + ) + ), + scene=SimpleNamespace( + entities={"can": SimpleNamespace(pose=poses, confidence=1.0)} + ), + ) + bound = SimpleNamespace( + binding=SimpleNamespace( + resources={ + "primary": SimpleNamespace( + endpoints={"motion": SimpleNamespace(task_state_key="left")} + ) + } + ) + ) + result = lowerer.lower( + RegisteredSemanticCall( + call_id="gen_sim.align_held", + arguments={ + "object": "can", + "target": "current_object_pose", + "preserve_yaw": True, + }, + ), + context=context, + bound=bound, + option_template=MoveHeldObjectOptions(), + ) + target = result.goal.object_target_pose + torch.testing.assert_close(target[:, :3, 3], before[:, :3, 3]) + torch.testing.assert_close( + target[:, :3, 0], + torch.tensor([[0.0, 0.0, 1.0]]).repeat(2, 1), + atol=1e-6, + rtol=0, + ) + torch.testing.assert_close(poses, before) + assert robot.mock_calls == [] + + +def test_coordinated_hold_lowerer_retains_both_verified_attachments() -> None: + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="tray", + entity_id="tray", + ) + lowerer = _CoordinatedHoldLowerer( + ( + _CoordinatedTransportRoute( + object_id="tray", + target_id="tray_up", + world_displacement=(0.0, 0.0, 0.14), + ), + ), + (semantics,), + ) + + lowering = lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.coordinated_hold", + arguments={ + "object": "tray", + "target": "tray_up", + "world_displacement": [0.0, 0.0, 0.14], + }, + ), + context=PlanningContext( + robot=RobotObservation( + timestamp=1.0, + qpos=torch.zeros((1, 1)), + qvel=torch.zeros((1, 1)), + ), + task=TaskState(batch_size=1, device="cpu"), + scene=SceneSnapshot( + timestamp=1.0, + version=1, + entities={"tray": EntityState(torch.eye(4).unsqueeze(0))}, + ), + env_ids=torch.tensor([0]), + ), + bound=None, # type: ignore[arg-type] + option_template=CoordinatedPickmentOptions(release=False), + ) + + assert lowering.registered_effect is not None + assert lowering.registered_effect.effect_kind is SemanticEffectKind.ATTACH + assert [effect.relation for effect in lowering.registered_effect.held_objects] == [ + HeldObjectRelation.ATTACHED, + HeldObjectRelation.ATTACHED, + ] + factory = decode_task_lowerer( + { + "kind": "coordinated_hold", + "routes": [ + { + "object_id": "tray", + "target_id": "tray_up", + "world_displacement": [0.0, 0.0, 0.14], + } + ], + }, + path="integration.runtime_services.registered_semantic_lowerers[0]", + ) + assert factory.call_id == "simulation.coordinated_hold" + + +def test_configured_handover_uses_baseline_timing_and_rejects_removed_wait_field() -> ( + None +): + path = ( + Path(__file__).parents[3] + / "embodichain_tasks/configs/tasks/manipulation/hand_over" + / "task.dual_ur5_dh_pgi_140_80.yaml" + ) + physical = _resolve_gym_components(load_config(path), base_dir=path.parent) + _, task, policy = _resolve_task_program_components( + physical.config["task_program"], base_dir=path.parent + ) + payload = _compose_integration_payload( + task=task, + policy=policy, + skill_profile=physical.embodiment_skill_profile, + scene=task["scene_binding"], + )["robot_profile"] + payload["presets"][0]["action_options"]["hand_over"].update( + retreat_distance=0.12, retreat_steps=28 + ) + before = deepcopy(payload) + options = ( + _decode_robot_profile(payload).presets[0].action_option_templates["hand_over"] + ) + + assert payload == before + assert type(options) is HandOverOptions + assert options.retreat_distance == pytest.approx(0.12) + assert options.retreat_steps == 28 + assert not hasattr(options, "source_release_settle_steps") + payload["presets"][0]["action_options"]["hand_over"][ + "source_release_settle_steps" + ] = 16 + with pytest.raises(ValueError, match="source_release_settle_steps"): + _decode_robot_profile(payload) + + +def test_configured_transport_binds_one_baseline_pose_without_planning() -> None: + pose = _AbsolutePoseTarget((0.1, 0.2, 0.8), (1.0, 0.0, 0.0, 0.0)) + lowerer = _MoveHeldObjectLowerer( + (_MoveHeldObjectRoute("part", "inspection", pose),) + ) + forbidden_robot = Mock() + goal = lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.move_held_object", + arguments={"object": "part", "target": "inspection"}, + ), + context=forbidden_robot, + bound=forbidden_robot, + option_template=MoveHeldObjectOptions(), + ).goal + forbidden_robot.assert_not_called() + assert forbidden_robot.mock_calls == [] + assert type(goal) is HeldObjectPoseGoal + torch.testing.assert_close(goal.object_target_pose, pose.to_matrix()) + assert not hasattr(goal, "alternative_object_target_poses") + lookahead = lowerer.pick_lookahead_targets( + RegisteredSemanticCall( + call_id="simulation.move_held_object", arguments={"target": "inspection"} + ), + picked_object=SceneObjectRef("different_part"), + bound=None, + previous_target=None, + ) + assert lookahead is None + + +def test_transport_decoder_rejects_old_alternative_pose_declarations() -> None: + pose = { + "kind": "pose", + "position": [0.1, 0.2, 0.8], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + } + with pytest.raises(ValueError, match="alternatives"): + decode_task_lowerer( + { + "kind": "move_held_object", + "routes": [ + { + "object_id": "part", + "target_id": "inspection", + "pose": pose, + "alternatives": [pose], + } + ], + }, + path="lowerer", + ) + + +def test_configured_pick_keeps_baseline_goal_and_preset_ownership() -> None: + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), geometry={}, entity_id="part" + ) + route = _PickRoute("part", "inspection") + options = PickUpOptions( + pick_object_part="bottom", approach_direction=torch.tensor([1.0, 0.0, 0.0]) + ) + lowering = _PickLowerer((route,), (semantics,)).lower( + RegisteredSemanticCall( + call_id="simulation.pick", + arguments={"object": "part", "target": "inspection"}, + ), + context=None, + bound=None, + option_template=options, + ) + assert type(lowering.goal) is GraspGoal + assert lowering.goal.semantics is semantics + assert lowering.skill_options is None + assert options.pick_object_part == "bottom" + assert options.downstream_object_target_poses == () + assert lowering.registered_effect.effect_kind is SemanticEffectKind.ATTACH + + +def test_pick_decoder_rejects_removed_runtime_option_declarations() -> None: + with pytest.raises(ValueError, match="required_object_target_poses"): + decode_task_lowerer( + { + "kind": "pick", + "routes": [ + { + "object_id": "part", + "target_id": "inspection", + "required_object_target_poses": [], + } + ], + }, + path="pick", + ) + + +def test_task_pick_alias_factory_has_stable_immutable_identity() -> None: + payload = { + "kind": "pick", + "call_id": "gen_sim.pick.step_01", + "routes": [{"object_id": "part", "target_id": "release"}], + } + first = decode_task_lowerer(payload, path="pick") + second = decode_task_lowerer(payload, path="pick") + assert first.call_id == "gen_sim.pick.step_01" + assert first.lowerer_type.call_id == first.call_id + assert type(first).__qualname__ == type(second).__qualname__ + assert first.target_descriptor.skill_id == "pick_up" + robot = object() + registry = Mock() + registry.resolve.return_value = SceneObjectRef("part") + registry.object_semantics.return_value = ObjectSemantics( + affordance=AntipodalAffordance(), geometry={}, entity_id="part" + ) + lowerer = first.create( + simulation=None, + robot=robot, + scene_registry=registry, + engine=SimpleNamespace(robot=robot, grasp_pose_generators={}), + ) + assert type(lowerer).call_id == first.call_id + result = lowerer.lower( + RegisteredSemanticCall( + call_id=first.call_id, arguments={"object": "part", "target": "release"} + ), + context=None, + bound=None, + option_template=PickUpOptions(), + ) + assert type(result.goal) is GraspGoal + assert result.skill_options is None + with pytest.raises(FrozenInstanceError): + first.routes = () + + +@pytest.mark.parametrize( + "kind", + [ + "articulation_link_slide", + "articulation_link_press", + "articulation_link_twist", + "release_safe_pick", + "move_held_object_upright", + ], +) +def test_task_decoder_rejects_out_of_scope_services(kind: str) -> None: + with pytest.raises(ValueError, match="unsupported Task Engine service"): + decode_task_lowerer({"kind": kind}, path="lowerer") diff --git a/tests/gen_sim/task_engine/test_task_program_stability.py b/tests/gen_sim/task_engine/test_task_program_stability.py new file mode 100644 index 000000000..3141b663a --- /dev/null +++ b/tests/gen_sim/task_engine/test_task_program_stability.py @@ -0,0 +1,289 @@ +# ---------------------------------------------------------------------------- +# 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-owned upright acceptance migrated from the removed public validator.""" + +from __future__ import annotations + +import math +from types import SimpleNamespace +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.gen_sim.task_engine._task_program.stability import ( + StabilityConstraint, + TaskStabilityPort, +) +from embodichain.lab.task_program.compiler.program import CompiledPostPolicy +from embodichain.lab.task_program.language.schema import WaitStablePostCfg +from embodichain.lab.task_program.semantics import SceneObjectRef + +__all__: list[str] = [] + + +def _local_port( + cfg: StabilityConstraint, + pose: torch.Tensor, + *, + reference_pose: torch.Tensor | None = None, +): + preset = "gen_sim.target.stable" + policy = CompiledPostPolicy( + cfg=WaitStablePostCfg(entity=cfg.entity, preset=preset), + entity=SceneObjectRef(cfg.entity), + source_path=("program", "post", 0), + ) + segment = SimpleNamespace(post_policies=(policy,)) + robot = SimpleNamespace( + get_qpos=lambda **kwargs: torch.zeros(pose.shape[0], 2), + get_joint_ids=lambda **kwargs: [0, 1], + compute_fk=lambda **kwargs: torch.eye(4).repeat(pose.shape[0], 1, 1), + ) + entities = { + cfg.entity: SimpleNamespace(get_local_pose=lambda **kwargs: pose.clone()) + } + if reference_pose is not None: + assert cfg.reference is not None + entities[cfg.reference] = SimpleNamespace( + get_local_pose=lambda **kwargs: reference_pose.clone() + ) + port = TaskStabilityPort( + Mock(), + SimpleNamespace(get_rigid_object=entities.get), + robot, + SimpleNamespace( + rigid_objects=tuple( + SimpleNamespace(entity_id=entity, simulation_uid=entity) + for entity in entities + ) + ), + {preset: cfg}, + step_dt=0.04, + ) + return port, policy, segment + + +@pytest.mark.parametrize("motion", ["translation", "rotation"]) +def test_stack_requires_both_objects_stable_in_each_environment(motion: str) -> None: + upper = torch.eye(4).repeat(2, 1, 1) + upper[:, 2, 3] = 0.1 + support = torch.eye(4).repeat(2, 1, 1) + cfg = StabilityConstraint( + entity="upper", + reference="support", + kind="stack", + local_axis=(0.0, 0.0, 1.0), + reference_axis=(0.0, 0.0, 1.0), + object_bottom=-0.05, + reference_top=0.05, + reference_half_extents=(0.05, 0.05), + duration=3.0, + timeout=3.2, + ) + port, policy, segment = _local_port(cfg, upper, reference_pose=support) + for step, _ in enumerate( + port.actions(policy, segment=segment, active_mask=torch.tensor([True, True])) + ): + if motion == "translation": + # Stay under the upper object, but exceed the declared drift limit. + support[1, 0, 3] = 1.5 * cfg.translation_drift if step % 2 == 0 else 0.0 + else: + angle = 1.5 * cfg.rotation_drift if step % 2 == 0 else 0.0 + support[1, :3, :3] = torch.tensor( + [ + [math.cos(angle), -math.sin(angle), 0.0], + [math.sin(angle), math.cos(angle), 0.0], + [0.0, 0.0, 1.0], + ] + ) + assert port.post_policy_result(policy, segment=segment).tolist() == [True, False] + + +def test_stack_restarts_its_complete_window_after_support_motion() -> None: + upper = torch.eye(4).unsqueeze(0) + upper[:, 2, 3] = 0.1 + support = torch.eye(4).unsqueeze(0) + duration = 3.0 + port, policy, segment = _local_port( + StabilityConstraint( + entity="upper", + reference="support", + kind="stack", + local_axis=(0.0, 0.0, 1.0), + reference_axis=(0.0, 0.0, 1.0), + object_bottom=-0.05, + reference_top=0.05, + reference_half_extents=(0.05, 0.05), + duration=duration, + timeout=4.0, + ), + upper, + reference_pose=support, + ) + moved_at_step = 5 + steps = 0 + for _ in port.actions(policy, segment=segment, active_mask=torch.tensor([True])): + steps += 1 + if steps == moved_at_step: + support[0, 0, 3] = 0.03 + assert port.post_policy_result(policy, segment=segment).tolist() == [True] + assert steps == moved_at_step + math.ceil(duration / 0.04) + + +def test_placement_requires_a_target_not_only_a_stationary_object() -> None: + with pytest.raises(ValueError, match="explicit target"): + StabilityConstraint(entity="tray", kind="placement") + + +def test_placement_rejects_a_stable_but_wrong_destination_per_environment() -> None: + pose = torch.eye(4).repeat(2, 1, 1) + pose[1, 0, 3] = 0.10 + port, policy, segment = _local_port( + StabilityConstraint( + entity="tray", + kind="placement", + target_position=(0.0, 0.0, 0.0), + duration=0.08, + timeout=0.12, + ), + pose, + ) + commands = list( + port.actions(policy, segment=segment, active_mask=torch.tensor([True, True])) + ) + assert len(commands) == 3 + assert port.post_policy_result(policy, segment=segment).tolist() == [True, False] + metadata = port.post_policy_metadata(policy, segment=segment) + assert metadata["measurements"]["position_error"] == pytest.approx([0.0, 0.10]) + + +def test_upright_hold_checks_orientation_without_inventing_a_position_goal() -> None: + pose = torch.eye(4).repeat(2, 1, 1) + pose[1, :3, :3] = torch.tensor([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]) + port, policy, segment = _local_port( + StabilityConstraint( + entity="can", + kind="hold", + local_axis=(0.0, 0.0, 1.0), + motion_parts=("arm",), + duration=0.08, + timeout=0.12, + ), + pose, + ) + list(port.actions(policy, segment=segment, active_mask=torch.tensor([True, True]))) + assert port.post_policy_result(policy, segment=segment).tolist() == [True, False] + assert port.post_policy_metadata(policy, segment=segment)["failed_mask"] == [ + False, + True, + ] + + +@pytest.mark.parametrize( + "invalid_field", + [ + {"local_axis": [0.0, 0.0, 0.0]}, + {"entity": None}, + {"local_axis": [True, 0.0, 1.0]}, + {"local_axis": [0.0, 0.0, float("nan")]}, + {"minimum_alignment": 1.1}, + {"absolute_alignment": "false"}, + {"target_axis": [0.0, 0.0, 1.0]}, + ], +) +def test_upright_constraint_rejects_invalid_values(invalid_field: dict) -> None: + with pytest.raises(ValueError): + StabilityConstraint.decode( + { + "kind": "upright", + "entity": "cube", + "local_axis": [1.0, 0.0, 0.0], + **invalid_field, + } + ) + + +def test_upright_constraint_normalizes_declared_local_axis() -> None: + cfg = StabilityConstraint.decode( + {"kind": "upright", "entity": "cube", "local_axis": [2.0, 0.0, 0.0]} + ) + assert cfg.local_axis == (1.0, 0.0, 0.0) + + +def test_stability_rejects_conflicting_absolute_and_relative_targets() -> None: + with pytest.raises(ValueError, match="absolute and relative"): + StabilityConstraint( + entity="tray", + kind="placement", + target_position=(0.0, 0.0, 1.0), + reference="table", + displacement=(0.0, 0.0, 0.5), + ) + + +def test_upright_policy_validates_each_environments_measured_rotation() -> None: + step_dt = 0.04 + pose = torch.eye(4).repeat(2, 1, 1) + pose[0, :3, :3] = torch.tensor([[0.0, 0.0, -1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]]) + delegate = Mock() + robot = SimpleNamespace(get_qpos=lambda **kwargs: torch.zeros(2, 2)) + entity = SimpleNamespace(get_local_pose=lambda **kwargs: pose.clone()) + simulation = SimpleNamespace(get_rigid_object=lambda uid: entity) + binding = SimpleNamespace( + rigid_objects=(SimpleNamespace(entity_id="cube", simulation_uid="native_cube"),) + ) + preset = "gen_sim.cube.upright" + policy = CompiledPostPolicy( + cfg=WaitStablePostCfg(entity="cube", preset=preset), + entity=SceneObjectRef("cube"), + source_path=("program", "post", 0), + ) + segment = SimpleNamespace(post_policies=(policy,)) + port = TaskStabilityPort( + delegate, + simulation, + robot, + binding, + { + preset: StabilityConstraint( + kind="upright", + entity="cube", + local_axis=(1.0, 0.0, 0.0), + minimum_alignment=0.9, + duration=2 * step_dt, + timeout=3 * step_dt, + ) + }, + step_dt=step_dt, + ) + + commands = list( + port.actions( + policy, segment=segment, active_mask=torch.ones(2, dtype=torch.bool) + ) + ) + result = port.post_policy_result(policy, segment=segment) + metadata = port.post_policy_metadata(policy, segment=segment) + + assert len(commands) == 3 + assert all(command.shape == (2, 2) for command in commands) + assert result.tolist() == [True, False] + assert metadata["kind"] == "upright" + assert metadata["measurements"]["alignment"] == pytest.approx([1.0, 0.0]) + assert metadata["accepted_mask"] == [True, False] + assert delegate.mock_calls == [] diff --git a/tests/gen_sim/task_engine/test_workflow.py b/tests/gen_sim/task_engine/test_workflow.py new file mode 100644 index 000000000..fe216c8bb --- /dev/null +++ b/tests/gen_sim/task_engine/test_workflow.py @@ -0,0 +1,343 @@ +# ---------------------------------------------------------------------------- +# 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 pathlib import Path + +import pytest + +from embodichain.gen_sim.task_engine.config import ( + TaskEngineExecutionCfg, + TaskEnginePlanningCfg, + TaskEngineWorkflowCfg, + load_task_engine_config, +) +from embodichain.gen_sim.task_engine.state_machine import ( + StageStatus, + WorkflowStage, + complete_stage, + fail_stage, + initial_state, + replay_events, + start_stage, + skip_stage, +) +from embodichain.gen_sim.task_engine.workflow_contracts import ( + TASK_RUN_REQUEST_SCHEMA, + scene_input_kind, + validate_scene_history_root, + validate_task_run_request, +) + + +def _request(tmp_path: Path, *, image: bool, edit: bool) -> dict[str, object]: + return { + "schema_version": TASK_RUN_REQUEST_SCHEMA, + "task_id": "pick-cup", + "task_instruction": "Pick up the red cup.", + "image_path": str(tmp_path / "input.png") if image else None, + "gym_project": None if image else str(tmp_path / "gym_project"), + "scene_edit_prompt": "Add a tray." if edit else None, + "output_dir": str(tmp_path / "output"), + } + + +@pytest.mark.parametrize("image", [False, True]) +@pytest.mark.parametrize("edit", [False, True]) +def test_run_request_accepts_all_four_input_combinations( + tmp_path: Path, + image: bool, + edit: bool, +) -> None: + request = validate_task_run_request(_request(tmp_path, image=image, edit=edit)) + assert scene_input_kind(request) == ("image" if image else "gym_project") + assert request["scene_edit_prompt"] == ("Add a tray." if edit else None) + + +def test_run_request_rejects_two_scene_inputs(tmp_path: Path) -> None: + request = _request(tmp_path, image=True, edit=False) + request["gym_project"] = str(tmp_path / "gym_project") + with pytest.raises(ValueError, match="exactly one"): + validate_task_run_request(request) + + +def test_run_request_rejects_scene_generation_prompt(tmp_path: Path) -> None: + request = _request(tmp_path, image=True, edit=False) + request["scene_generation_prompt"] = "Make a kitchen." + with pytest.raises(ValueError, match="fields differ"): + validate_task_run_request(request) + + +def test_run_request_rejects_output_inside_gym_project(tmp_path: Path) -> None: + request = _request(tmp_path, image=False, edit=False) + request["output_dir"] = str(tmp_path / "gym_project" / "task_run") + + with pytest.raises(ValueError, match="must not overlap"): + validate_task_run_request(request) + + +def test_run_request_rejects_output_containing_explicit_gym_config( + tmp_path: Path, +) -> None: + project = tmp_path / "gym_project" + project.mkdir() + config_path = project / "gym_config.json" + config_path.write_text("{}", encoding="utf-8") + request = _request(tmp_path, image=False, edit=False) + request["gym_project"] = str(config_path) + request["output_dir"] = str(project) + + with pytest.raises(ValueError, match="must not overlap"): + validate_task_run_request(request) + + +def test_scene_history_root_allows_a_source_from_a_prior_run( + tmp_path: Path, +) -> None: + history = tmp_path / "task_history" + source = history / "20260820_105939" / "attempts" / "scene_export" + source.mkdir(parents=True) + + validate_scene_history_root(source, history) + + request = _request(tmp_path, image=False, edit=False) + request["gym_project"] = str(source) + request["output_dir"] = str(history / "20260820_130000") + assert validate_task_run_request(request)["gym_project"] == source.as_posix() + + +@pytest.mark.parametrize("relative_output", [".", "new_runs", "new_runs/task"]) +def test_scene_history_root_rejects_writes_into_source_project( + tmp_path: Path, + relative_output: str, +) -> None: + source = tmp_path / "scene_export" + source.mkdir() + output_root = source / relative_output + + with pytest.raises(ValueError, match="read-only source"): + validate_scene_history_root(source, output_root) + + +def test_scene_history_root_resolves_symlinks_before_comparison( + tmp_path: Path, +) -> None: + source = tmp_path / "scene_export" + source.mkdir() + source_link = tmp_path / "scene_link" + source_link.symlink_to(source, target_is_directory=True) + + with pytest.raises(ValueError, match="read-only source"): + validate_scene_history_root(source_link, source / "new_runs") + + +def test_scene_history_root_protects_explicit_config_parent(tmp_path: Path) -> None: + source = tmp_path / "scene_export" + source.mkdir() + config = source / "scene_config.json" + config.write_text("{}\n", encoding="utf-8") + + with pytest.raises(ValueError, match="read-only source"): + validate_scene_history_root(config, source) + + +def test_task_and_scene_stages_can_run_concurrently(tmp_path: Path) -> None: + state = initial_state(_request(tmp_path, image=True, edit=False)) + state = start_stage(state, WorkflowStage.TASK_CANDIDATES) + state = start_stage(state, WorkflowStage.SCENE_PREPARATION) + assert state.stages[WorkflowStage.TASK_CANDIDATES] == StageStatus.RUNNING + assert state.stages[WorkflowStage.SCENE_PREPARATION] == StageStatus.RUNNING + assert state.stages[WorkflowStage.SCENE_EDIT] == StageStatus.SKIPPED + + +def test_candidate_selection_waits_for_both_branches(tmp_path: Path) -> None: + state = initial_state(_request(tmp_path, image=False, edit=False)) + state = start_stage(state, WorkflowStage.TASK_CANDIDATES) + state = complete_stage(state, WorkflowStage.TASK_CANDIDATES) + with pytest.raises(ValueError, match="incomplete dependencies"): + start_stage(state, WorkflowStage.CANDIDATE_SELECTION) + + +def test_unbound_action_can_run_while_user_scene_edit_is_running( + tmp_path: Path, +) -> None: + state = initial_state(_request(tmp_path, image=True, edit=True)) + for stage in (WorkflowStage.TASK_CANDIDATES, WorkflowStage.SCENE_PREPARATION): + state = start_stage(state, stage) + state = complete_stage(state, stage) + state = start_stage(state, WorkflowStage.CANDIDATE_SELECTION) + state = complete_stage(state, WorkflowStage.CANDIDATE_SELECTION) + state = start_stage(state, WorkflowStage.SCENE_EDIT) + state = start_stage(state, WorkflowStage.UNBOUND_ACTION) + + assert state.stages[WorkflowStage.SCENE_EDIT] == StageStatus.RUNNING + assert state.stages[WorkflowStage.UNBOUND_ACTION] == StageStatus.RUNNING + with pytest.raises(ValueError, match="incomplete dependencies"): + start_stage(state, WorkflowStage.SCENE_FINALIZATION) + + +def test_only_scene_edit_can_be_skipped(tmp_path: Path) -> None: + state = initial_state(_request(tmp_path, image=True, edit=True)) + + with pytest.raises(ValueError, match="Only the optional scene_edit stage"): + skip_stage(state, WorkflowStage.FINAL_BINDING) + + +def test_state_events_replay_to_the_same_snapshot(tmp_path: Path) -> None: + request = _request(tmp_path, image=True, edit=False) + state = initial_state(request) + state = start_stage(state, WorkflowStage.TASK_CANDIDATES) + state = start_stage(state, WorkflowStage.SCENE_PREPARATION) + state = complete_stage(state, WorkflowStage.TASK_CANDIDATES) + state = complete_stage(state, WorkflowStage.SCENE_PREPARATION) + state = start_stage(state, WorkflowStage.CANDIDATE_SELECTION) + state = complete_stage(state, WorkflowStage.CANDIDATE_SELECTION) + + replayed = replay_events(request, state.events) + + assert replayed.to_dict() == state.to_dict() + + +def test_state_replay_rejects_tampered_transition(tmp_path: Path) -> None: + request = _request(tmp_path, image=True, edit=False) + state = initial_state(request) + events = [dict(event) for event in state.events] + events[-1]["stage"] = WorkflowStage.FINAL_BINDING.value + + with pytest.raises(ValueError, match="event does not match"): + replay_events(request, events) + + +def test_state_replay_preserves_failure_reason(tmp_path: Path) -> None: + request = _request(tmp_path, image=True, edit=False) + state = initial_state(request) + state = start_stage(state, WorkflowStage.TASK_CANDIDATES) + state = fail_stage(state, WorkflowStage.TASK_CANDIDATES, reason="model timeout") + + replayed = replay_events(request, state.events) + + assert replayed.terminal + assert replayed.to_dict() == state.to_dict() + + +def test_later_retry_can_fail_a_previously_successful_stage(tmp_path: Path) -> None: + request = _request(tmp_path, image=True, edit=False) + state = initial_state(request) + state = start_stage(state, WorkflowStage.SCENE_PREPARATION) + state = complete_stage(state, WorkflowStage.SCENE_PREPARATION) + state = fail_stage( + state, + WorkflowStage.SCENE_PREPARATION, + reason="later scene attempt failed", + ) + + assert state.terminal + assert replay_events(request, state.events).to_dict() == state.to_dict() + + +def test_state_snapshot_mappings_are_immutable(tmp_path: Path) -> None: + state = initial_state(_request(tmp_path, image=True, edit=False)) + + with pytest.raises(TypeError): + state.stages[WorkflowStage.TASK_CANDIDATES] = StageStatus.SUCCEEDED + with pytest.raises(TypeError): + state.request["task_id"] = "changed" + with pytest.raises(TypeError): + state.events[0]["to"] = StageStatus.FAILED.value + + +def test_workflow_configuration_rejects_non_positive_limits() -> None: + with pytest.raises(ValueError, match="max_scene_attempts"): + TaskEngineWorkflowCfg(max_scene_attempts=0) + + +def test_packaged_workflow_configuration_uses_recovery_defaults() -> None: + workflow, planning, execution = load_task_engine_config() + + assert workflow.max_scene_attempts == 2 + assert workflow.max_action_attempts == 1 + assert planning.candidate_count == 3 + assert planning.planning_mode == "offline" + assert planning.max_episodes == 1 + assert planning.max_episode_steps == 8000 + assert execution.num_envs == 1 + assert execution.required_successes == 1 + + +def test_workflow_configuration_can_be_tuned_from_yaml(tmp_path: Path) -> None: + config = tmp_path / "task_engine.yaml" + config.write_text( + """\ +schema_version: embodichain.task-engine-defaults/v1 +workflow: + max_parallel_workers: 3 + max_scene_attempts: 4 + max_action_attempts: 5 +planning: + candidate_count: 7 + planning_mode: offline + max_episodes: 2 + max_episode_steps: 5000 +execution: + num_envs: 6 + success_policy: at_least + min_successful_envs: 2 +""", + encoding="utf-8", + ) + + workflow, planning, execution = load_task_engine_config(config) + + assert workflow.max_parallel_workers == 3 + assert workflow.max_scene_attempts == 4 + assert workflow.max_action_attempts == 5 + assert planning.candidate_count == 7 + assert planning.max_episodes == 2 + assert planning.max_episode_steps == 5000 + assert execution.num_envs == 6 + assert execution.required_successes == 2 + + +def test_execution_configuration_validates_success_policy() -> None: + assert TaskEngineExecutionCfg().num_envs == 1 + assert ( + TaskEngineExecutionCfg( + num_envs=4, + success_policy="at_least", + min_successful_envs=2, + ).required_successes + == 2 + ) + with pytest.raises(ValueError, match="success_policy=all"): + TaskEngineExecutionCfg( + num_envs=4, + success_policy="all", + min_successful_envs=1, + ) + + +def test_planning_configuration_rejects_invalid_values() -> None: + with pytest.raises(ValueError, match="candidate_count"): + TaskEnginePlanningCfg(candidate_count=0) + with pytest.raises(ValueError, match="planning_mode"): + TaskEnginePlanningCfg(planning_mode="unsupported") + with pytest.raises(TypeError): + TaskEnginePlanningCfg(gripper_model="unsupported") + with pytest.raises(TypeError): + TaskEnginePlanningCfg(ik_solver="unsupported") + with pytest.raises(TypeError): + TaskEnginePlanningCfg(planner={"mode": "toppra", "dynamic_collision": True}) diff --git a/tests/gen_sim/test_video_archive.py b/tests/gen_sim/test_video_archive.py new file mode 100644 index 000000000..feaa770d7 --- /dev/null +++ b/tests/gen_sim/test_video_archive.py @@ -0,0 +1,145 @@ +# ---------------------------------------------------------------------------- +# 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 pathlib import Path +from types import SimpleNamespace + +import pytest + +from embodichain.gen_sim.video_archive import ( + _archive_task_recording, + _archive_task_video, +) + +SOURCE_STEM = "episode_0_record_cam_audience_view" + + +class record_camera_data: + def __init__(self, save_path: Path) -> None: + self._name = "record_cam_audience_view" + self._save_path = save_path + + +def _env(recorder: record_camera_data | None = None) -> SimpleNamespace: + event_manager = SimpleNamespace( + _mode_functor_cfgs={ + "interval": ( + [SimpleNamespace(func=recorder)] if recorder is not None else [] + ) + } + ) + env = SimpleNamespace(event_manager=event_manager) + env.unwrapped = env + return env + + +def _write_source(directory: Path, extension: str, content: bytes = b"video") -> Path: + source = directory / f"{SOURCE_STEM}{extension}" + source.write_bytes(content) + return source + + +def test_archive_task_video_copies_source_and_preserves_extension( + tmp_path: Path, +) -> None: + source = _write_source(tmp_path, ".webm") + + destination = _archive_task_video( + tmp_path, + source_stem=SOURCE_STEM, + task_id="task2_1", + ) + + assert destination == tmp_path / "task2_1.webm" + assert destination.read_bytes() == b"video" + assert source.read_bytes() == b"video" + + +@pytest.mark.parametrize("task_id", ["../task2_1", "task2/1", r"task2\1", ".."]) +def test_archive_task_video_rejects_path_characters( + tmp_path: Path, + task_id: str, +) -> None: + source = _write_source(tmp_path, ".mp4") + + with pytest.raises(ValueError, match="Invalid task ID"): + _archive_task_video(tmp_path, source_stem=SOURCE_STEM, task_id=task_id) + + assert source.is_file() + + +def test_archive_task_video_reports_missing_source_with_task_and_path( + tmp_path: Path, +) -> None: + with pytest.raises(FileNotFoundError) as error: + _archive_task_video( + tmp_path, + source_stem=SOURCE_STEM, + task_id="task2_1", + ) + + message = str(error.value) + assert "task2_1" in message + assert str(tmp_path / f"{SOURCE_STEM}.") in message + + +def test_archive_task_video_overwrites_existing_target( + tmp_path: Path, +) -> None: + source = _write_source(tmp_path, ".mp4", b"new") + destination = tmp_path / "task2_1.mp4" + destination.write_bytes(b"existing") + + result = _archive_task_video( + tmp_path, + source_stem=SOURCE_STEM, + task_id="task2_1", + ) + + assert result == destination + assert destination.read_bytes() == b"new" + assert source.read_bytes() == b"new" + + +def test_consecutive_tasks_keep_independent_videos(tmp_path: Path) -> None: + for task_id, content in (("task2_1", b"first"), ("task2_2", b"second")): + _write_source(tmp_path, ".mp4", content) + _archive_task_video( + tmp_path, + source_stem=SOURCE_STEM, + task_id=task_id, + ) + + assert (tmp_path / "task2_1.mp4").read_bytes() == b"first" + assert (tmp_path / "task2_2.mp4").read_bytes() == b"second" + assert (tmp_path / f"{SOURCE_STEM}.mp4").read_bytes() == b"second" + + +def test_task_recording_uses_runtime_recorder_path(tmp_path: Path) -> None: + recorder = record_camera_data(tmp_path) + source = _write_source(tmp_path, ".mkv") + + destination = _archive_task_recording(_env(recorder), "task2_1") + + assert destination == tmp_path / "task2_1.mkv" + assert destination.read_bytes() == b"video" + assert source.read_bytes() == b"video" + + +def test_task_recording_is_noop_when_recording_is_disabled() -> None: + assert _archive_task_recording(_env(), "task2_1") is None diff --git a/tests/gym/envs/task_program/test_bridge.py b/tests/gym/envs/task_program/test_bridge.py index 38f93ca14..2e64aef5d 100644 --- a/tests/gym/envs/task_program/test_bridge.py +++ b/tests/gym/envs/task_program/test_bridge.py @@ -1020,6 +1020,29 @@ def test_joint_encoder_emits_full_qpos_and_holds_inactive_rows() -> None: assert torch.equal(action[1], qpos[1]) +def test_joint_encoder_holds_unaddressed_joints_at_controller_targets() -> None: + """A scoped command must not chase measured drift on another resource.""" + current = torch.zeros(BATCH_SIZE, ROBOT_DOF) + controller_targets = torch.full_like(current, 0.5) + encoder = RuntimeCommandFrameEncoder( + _QposProvider(current), + hold_qpos_provider=lambda env_ids: controller_targets.clone(), + ) + + action = encoder.encode( + _joint_frame( + duration=STEP_DT, + active_mask=torch.tensor([True, False]), + ) + ) + + assert torch.equal(action[0, torch.tensor([1, 3])], torch.tensor([10.0, 30.0])) + assert torch.equal( + action[0, torch.tensor([0, 2, 4])], controller_targets[0, [0, 2, 4]] + ) + assert torch.equal(action[1], controller_targets[1]) + + def test_frame_encoder_supports_registered_future_transport() -> None: encoder = RuntimeCommandFrameEncoder( _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)) diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index 025f855ce..50e71c9b1 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -21,6 +21,7 @@ from collections.abc import Iterator from copy import deepcopy import importlib.util +import math from pathlib import Path from types import SimpleNamespace @@ -31,12 +32,20 @@ from embodichain.lab.task_program import load_task_program from embodichain.lab.gym.envs import EmbodiedEnv from embodichain.lab.task_program.integrations._configured_services import ( + _AxisAlignLowerer, + _CoordinatedTransportLowerer, + _CoordinatedTransportRoute, _MoveHeldObjectLowerer, + _ParkLowerer, _PourLowerer, + _RelativePlaceLowerer, + _RelativePlaceRoute, ) from embodichain.lab.task_program.integrations.configured import ( + _decode_action_options, _decode_configured_task_program_integration, _decode_grasp_generator, + _decode_registered_lowerer, ) from embodichain.lab.task_program.integrations._configured_composition import ( _compose_integration_payload, @@ -51,17 +60,43 @@ _register_configured_task_program_integration, ) from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + AxisAlignAffordance, + AxisAlignGoal, + AxisAlignOptions, + CoordinatedPickGoal, + CoordinatedPickmentOptions, + EntityState, + HandOverOptions, + HeldObjectState, HeldObjectPoseGoal, + JointPositionGoal, MoveHeldObjectOptions, + MoveJoints, + MoveJointsOptions, + ObjectSemantics, + PARK_COMMAND, PickUpOptions, + PlanningContext, PlaceOptions, + PlaceGoal, PourGoal, PourOptions, + RobotObservation, + SceneEntityPose, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.atomic_actions.goals import ( + collect_scene_dependencies, + resolve_pose_goal, ) from embodichain.lab.sim.sensors import CameraCfg from embodichain.lab.task_program.semantics import ( + HeldObjectRelation, RegisteredSemanticCall, SceneObjectRef, + SemanticEffectKind, ) from embodichain.lab.gym.utils.gym_utils import config_to_cfg from embodichain.lab.gym.utils.registration import REGISTERED_ENVS @@ -543,6 +578,507 @@ def test_move_held_object_lowerer_rejects_non_se3_relative_pose() -> None: _decode_configured_task_program_integration(payload) +def test_coordinated_transport_lowerer_builds_one_releasing_atomic_goal() -> None: + """A registered transport owns no arm names, goals, or trajectory data.""" + relative_pose = ( + 1.0, + 0.0, + 0.0, + 0.2, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + ) + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="tray", + entity_id="tray", + ) + lowerer = _CoordinatedTransportLowerer( + (("tray", "tray_forward", "tray", relative_pose),), + (semantics,), + ) + + lowering = lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.coordinated_transport", + arguments={"object": "tray", "target": "tray_forward"}, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=CoordinatedPickmentOptions(release=True), + ) + + assert type(lowering.goal) is CoordinatedPickGoal + assert lowering.goal.object_initial_pose is None + assert lowering.goal.object_target_pose.entity_id == "tray" + assert lowering.goal.object_target_pose.relative_pose == pytest.approx( + torch.tensor(relative_pose, dtype=torch.float32).reshape(4, 4) + ) + assert lowering.registered_effect is not None + assert lowering.registered_effect.effect_kind is SemanticEffectKind.RELEASE + assert tuple( + (effect.slot_id, effect.relation, effect.object_id) + for effect in lowering.registered_effect.held_objects + ) == ( + ("left", HeldObjectRelation.DETACHED, "tray"), + ("right", HeldObjectRelation.DETACHED, "tray"), + ) + with pytest.raises(ValueError, match="enable coordinated release"): + lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.coordinated_transport", + arguments={"object": "tray", "target": "tray_forward"}, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=CoordinatedPickmentOptions(), + ) + + +def test_axis_align_registered_call_builds_typed_goal_and_attach_effect() -> None: + """The semantic call exposes AxisAlign without duplicating its planner.""" + semantics = ObjectSemantics( + affordance=AxisAlignAffordance(internal_axis=torch.tensor([0.0, 0.0, 1.0])), + geometry={}, + label="can", + entity_id="can", + ) + lowerer = _AxisAlignLowerer((semantics,)) + + lowering = lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.axis_align", + arguments={"object": "can"}, + resources={"primary": "right"}, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=AxisAlignOptions(), + ) + + assert type(lowering.goal) is AxisAlignGoal + assert lowering.goal.semantics is semantics + assert lowering.skill_options is None + assert lowering.registered_effect is not None + assert lowering.registered_effect.effect_kind is SemanticEffectKind.ATTACH + assert tuple( + (effect.slot_id, effect.relation, effect.object_id) + for effect in lowering.registered_effect.held_objects + ) == ( + ("primary", HeldObjectRelation.ATTACHED, "can"), + ) + with pytest.raises(ValueError, match="contain only 'object'"): + lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.axis_align", + arguments={"object": "can", "target_axis": [0.0, 0.0, 1.0]}, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=AxisAlignOptions(), + ) + + +def test_relative_place_uses_fresh_reference_pose_and_verified_grasp() -> None: + """Relative placement is grounded from the latest scene and TaskState.""" + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="can", + entity_id="can", + ) + object_to_eef = torch.eye(4).unsqueeze(0) + object_to_eef[:, 2, 3] = 0.2 + held = HeldObjectState( + semantics=semantics, + object_to_eef=object_to_eef, + grasp_xpos=torch.eye(4).unsqueeze(0), + ) + object_pose = torch.eye(4).unsqueeze(0) + object_pose[:, 1, 3] = -0.3 + reference_pose = torch.eye(4).unsqueeze(0) + reference_pose[:, :3, 3] = torch.tensor(((0.4, 0.1, 1.05),)) + qpos = torch.zeros((1, 1)) + context = PlanningContext( + robot=RobotObservation(timestamp=1.0, qpos=qpos, qvel=torch.zeros_like(qpos)), + task=TaskState( + batch_size=1, + device="cpu", + held_objects={"right_arm": held}, + ), + scene=SceneSnapshot( + timestamp=1.0, + version=1, + entities={ + "can": EntityState(object_pose), + "notebook": EntityState(reference_pose), + }, + ), + env_ids=torch.tensor((0,), dtype=torch.long), + ) + options = PlaceOptions(preserve_current_object_orientation=True) + bound = SimpleNamespace( + binding=SimpleNamespace( + resources={ + "primary": SimpleNamespace( + endpoints={"motion": SimpleNamespace(task_state_key="right_arm")} + ) + } + ), + preset=SimpleNamespace(action_option_template=lambda _semantic_id: options), + ) + lowerer = _RelativePlaceLowerer( + ( + _RelativePlaceRoute( + object_id="can", + reference_entity_id="notebook", + relation="behind", + world_displacement=(0.18, 0.0, 0.02), + ), + ) + ) + + call = RegisteredSemanticCall( + call_id="simulation.place_relative", + arguments={ + "object": "can", + "reference": "notebook", + "relation": "behind", + }, + resources={"primary": "right"}, + ) + lowering = lowerer.lower( + call, + context=context, + bound=bound, # type: ignore[arg-type] + option_template=options, + ) + + assert type(lowering.goal) is PlaceGoal + assert type(lowering.goal.xpos) is SceneEntityPose + assert lowering.goal.xpos.entity_id == "notebook" + assert collect_scene_dependencies(lowering.goal) == ("notebook",) + resolved = resolve_pose_goal(lowering.goal.xpos, context, name="xpos") + torch.testing.assert_close( + resolved[:, :3, 3], + torch.tensor(((0.58, 0.1, 1.27),)), + ) + moved_reference_pose = reference_pose.clone() + moved_reference_pose[:, :3, :3] = torch.tensor( + [ + [0.0, -1.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + ] + ) + moved_reference_pose[:, 1, 3] = 0.4 + moved_context = PlanningContext( + robot=context.robot, + task=context.task, + scene=SceneSnapshot( + timestamp=2.0, + version=2, + entities={ + "can": EntityState(object_pose), + "notebook": EntityState(moved_reference_pose), + }, + ), + env_ids=context.env_ids, + ) + moved_resolved = resolve_pose_goal( + lowering.goal.xpos, + moved_context, + name="xpos", + ) + torch.testing.assert_close( + moved_resolved[:, :3, 3], + torch.tensor(((0.58, 0.4, 1.27),)), + ) + torch.testing.assert_close(moved_resolved[:, :3, :3], torch.eye(3).unsqueeze(0)) + assert lowering.registered_effect is not None + assert lowering.registered_effect.effect_kind is SemanticEffectKind.RELEASE + assert lowering.registered_effect.held_objects[0].relation is ( + HeldObjectRelation.DETACHED + ) + lookahead = lowerer.pick_lookahead_targets( + call, + picked_object=SceneObjectRef("can"), + bound=bound, # type: ignore[arg-type] + previous_target=None, + ) + assert lookahead is not None and len(lookahead) == 1 + assert type(lookahead[0].pose) is SceneEntityPose + assert lookahead[0].pose.entity_id == "notebook" + torch.testing.assert_close( + lookahead[0].pose.world_displacement, + torch.tensor((0.18, 0.0, 0.02)), + ) + assert lookahead[0].preserve_current_object_orientation + + +def test_relative_place_rejects_unmigrated_local_service_kind() -> None: + """Do not retain a second factory contract for the same semantic call.""" + with pytest.raises(ValueError, match="Unsupported.*relative_place"): + _decode_registered_lowerer({"kind": "relative_place"}, path="lowerer") + + +def test_relative_place_rejects_call_owned_world_offset() -> None: + """Only the trusted integration may declare relative placement geometry.""" + lowerer = _RelativePlaceLowerer( + ( + _RelativePlaceRoute( + object_id="cube", + reference_entity_id="table", + relation="on", + world_displacement=(0.0, 0.0, 0.1), + ), + ) + ) + with pytest.raises(ValueError, match="arguments must contain only"): + lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.place_relative", + arguments={ + "object": "cube", + "reference": "table", + "relation": "on", + "world_offset": [0.0, 0.0, 0.1], + }, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=PlaceOptions(), + ) + + +def test_axis_align_and_relative_place_configs_use_closed_decoders() -> None: + """Both generated extensions contribute typed options and descriptors.""" + axis_factory = _decode_registered_lowerer( + {"kind": "axis_align", "object_ids": ["can"]}, + path="integration.runtime_services.registered_semantic_lowerers[0]", + ) + place_factory = _decode_registered_lowerer( + { + "kind": "place_relative", + "routes": [ + { + "object_id": "can", + "reference_entity_id": "notebook", + "relation": "on", + "world_displacement": [0.0, 0.0, 0.04], + } + ], + }, + path="integration.runtime_services.registered_semantic_lowerers[1]", + ) + axis_options = _decode_action_options( + {"kind": "axis_align", "target_axis": [0.0, 0.0, 1.0]}, + path="policy.action_options.simulation.axis_align", + ) + + assert axis_factory.call_id == "simulation.axis_align" + assert axis_factory.object_ids == ("can",) + assert place_factory.call_id == "simulation.place_relative" + assert place_factory.routes[0].relation == "on" + assert type(axis_options) is AxisAlignOptions + torch.testing.assert_close(axis_options.target_axis, torch.tensor((0.0, 0.0, 1.0))) + + +def test_coordinated_transport_config_decodes_closed_routes_and_options() -> None: + """Configured transport data round-trips through the strict allowlist.""" + identity = [ + 1.0, + 0.0, + 0.0, + 0.2, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + ] + factory = _decode_registered_lowerer( + { + "kind": "coordinated_transport", + "routes": [ + { + "object_id": "tray", + "target_id": "tray_forward", + "reference_entity_id": "tray", + "relative_pose": identity, + } + ], + }, + path="integration.runtime_services.registered_semantic_lowerers[0]", + ) + options = _decode_action_options( + { + "kind": "coordinated_pickment", + "release": True, + "release_steps": 6, + "retreat_steps": 8, + "grasp_seed": 17393, + }, + path="policy.action_options.simulation.coordinated_transport", + ) + + assert factory.call_id == "simulation.coordinated_transport" + assert type(options) is CoordinatedPickmentOptions + assert options.release is True + assert options.release_steps == 6 + assert options.retreat_steps == 8 + assert options.grasp_seed == 17393 + + +def test_park_lowerer_and_config_keep_joint_values_in_the_profile() -> None: + """Semantic Park reuses MoveJoints while profile data continues to own qpos.""" + lowerer = _ParkLowerer() + lowering = lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.park", + arguments={}, + resources={"primary": "left"}, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=MoveJointsOptions(), + ) + factory = _decode_registered_lowerer( + {"kind": "park"}, + path="integration.runtime_services.registered_semantic_lowerers[0]", + ) + options = _decode_action_options( + {"kind": "move_joints"}, + path="policy.action_options.simulation.park", + ) + + assert type(lowering.goal) is JointPositionGoal + assert lowering.goal.target == PARK_COMMAND + assert factory.call_id == "simulation.park" + assert factory.target_descriptor == MoveJoints.descriptor() + assert type(options) is MoveJointsOptions + with pytest.raises(ValueError, match="arguments must be empty"): + lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.park", + arguments={"qpos": [0.0]}, + resources={"primary": "left"}, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=MoveJointsOptions(), + ) + + +def test_configured_handover_decodes_source_retreat_clearance() -> None: + """The semantic profile may tune physical source-hand clearance.""" + options = _decode_action_options( + { + "kind": "hand_over", + "retreat_distance": 0.12, + "retreat_steps": 28, + }, + path="policy.action_options.hand_over", + ) + + assert type(options) is HandOverOptions + assert options.retreat_distance == pytest.approx(0.12) + assert options.retreat_steps == 28 + + +def test_coordinated_transport_world_displacement_uses_fresh_object_pose() -> None: + """A relative task motion keeps live orientation and robot-frame direction.""" + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="tray", + entity_id="tray", + ) + lowerer = _CoordinatedTransportLowerer( + ( + _CoordinatedTransportRoute( + object_id="tray", + target_id="tray_forward", + world_displacement=(-0.16, 0.0, 0.0), + ), + ), + (semantics,), + ) + pose = torch.eye(4).unsqueeze(0) + pose[:, :3, :3] = torch.tensor( + (((0.0, -1.0, 0.0), (1.0, 0.0, 0.0), (0.0, 0.0, 1.0)),) + ) + pose[:, :3, 3] = torch.tensor(((0.03, -0.02, 0.68),)) + qpos = torch.zeros((1, 1)) + context = PlanningContext( + robot=RobotObservation(timestamp=1.0, qpos=qpos, qvel=torch.zeros_like(qpos)), + task=TaskState(batch_size=1, device="cpu"), + scene=SceneSnapshot( + timestamp=1.0, + version=1, + entities={"tray": EntityState(pose)}, + ), + env_ids=torch.tensor((0,), dtype=torch.long), + ) + + lowering = lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.coordinated_transport", + arguments={"object": "tray", "target": "tray_forward"}, + ), + context=context, + bound=None, # type: ignore[arg-type] + option_template=CoordinatedPickmentOptions(release=True), + ) + + assert type(lowering.goal) is CoordinatedPickGoal + assert isinstance(lowering.goal.object_target_pose, torch.Tensor) + torch.testing.assert_close( + lowering.goal.object_target_pose[:, :3, :3], + pose[:, :3, :3], + ) + torch.testing.assert_close( + lowering.goal.object_target_pose[:, :3, 3], + torch.tensor(((-0.13, -0.02, 0.68),)), + ) + + +def test_coordinated_transport_config_decodes_world_displacement() -> None: + factory = _decode_registered_lowerer( + { + "kind": "coordinated_transport", + "routes": [ + { + "object_id": "tray", + "target_id": "tray_forward", + "world_displacement": [-0.16, 0.0, 0.0], + } + ], + }, + path="integration.runtime_services.registered_semantic_lowerers[0]", + ) + + assert factory.routes[0].world_displacement == (-0.16, 0.0, 0.0) + + def test_pick_option_rejects_malformed_fixed_object_to_eef() -> None: """Configured fixed grasps must contain exactly one SE(3) transform.""" payload = deepcopy(_tableware_integration_payload("pour_water")) @@ -614,7 +1150,9 @@ def test_grasp_generator_resolves_named_model_and_library_defaults() -> None: assert factory.min_opening_width == pytest.approx(0.005) assert factory.palm_depth == pytest.approx(0.096) assert factory.sample_count is None + assert factory.approach_deviation_angle is None assert factory.approach_direction_samples is None + assert factory.max_candidates is None assert factory.opening_margin is None assert factory.point_sample_density is None assert factory.filter_ground_collision is None @@ -633,7 +1171,11 @@ def test_grasp_generator_factory_defers_to_toolkit_policy_defaults() -> None: )() assert generator.algorithm_cfg.sample_count == 20_000 + assert generator.algorithm_cfg.approach_deviation_angle == pytest.approx( + math.pi / 6 + ) assert generator.algorithm_cfg.approach_direction_samples == 4 + assert generator.algorithm_cfg.max_candidates == 50 assert generator.collision_cfg.opening_margin == pytest.approx(0.01) assert generator.collision_cfg.point_sample_density == pytest.approx(0.01) assert generator.collision_cfg.filter_ground_collision is True @@ -641,6 +1183,24 @@ def test_grasp_generator_factory_defers_to_toolkit_policy_defaults() -> None: assert generator.annotation_cfg.force_refresh is False +def test_grasp_generator_decodes_candidate_search_policy() -> None: + """Robot profiles may widen and constrain the canonical grasp search.""" + generator = _decode_grasp_generator( + { + "kind": "antipodal_parallel_jaw", + "model": "dh_pgi_140_80", + "approach_deviation_angle": math.pi / 9, + "max_candidates": 500, + }, + path="generator", + )() + + assert generator.algorithm_cfg.approach_deviation_angle == pytest.approx( + math.pi / 9 + ) + assert generator.algorithm_cfg.max_candidates == 500 + + def test_grasp_generator_inline_model_uses_geometry_defaults() -> None: """Custom inline models only override geometry that differs from defaults.""" factory = _decode_grasp_generator( @@ -726,6 +1286,7 @@ def test_scene_entity_nesting_derives_all_affordance_parents() -> None: "entity_id": "cube_inside", "kind": "container", "native_name": "inside", + "release_clearance": 0.12, }, ) ) @@ -737,6 +1298,7 @@ def test_scene_entity_nesting_derives_all_affordance_parents() -> None: assert binding.antipodal_grasps[0].object_id == "cube" assert binding.support_surfaces[0].parent_id == "cube" assert binding.containers[0].parent_id == "cube" + assert binding.containers[0].release_clearance == pytest.approx(0.12) def test_placement_affordances_can_belong_to_articulations_and_links() -> None: @@ -976,3 +1538,51 @@ def test_examples_have_no_importable_task_environment_modules() -> None: __all__: list[str] = [] + + +def test_axis_align_lowerer_and_config_reuse_the_existing_atomic_skill() -> None: + """Configured E2 composition targets AxisAlign with a verified attachment.""" + semantics = ObjectSemantics( + affordance=AxisAlignAffordance(internal_axis=torch.tensor([1.0, 0.0, 0.0])), + geometry={}, + label="can", + entity_id="can", + ) + lowerer = _AxisAlignLowerer((semantics,)) + lowering = lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.axis_align", + arguments={"object": "can"}, + resources={"primary": "left"}, + ), + context=None, # type: ignore[arg-type] + bound=None, # type: ignore[arg-type] + option_template=AxisAlignOptions(), + ) + factory = _decode_registered_lowerer( + {"kind": "axis_align", "object_ids": ["can"]}, + path="integration.runtime_services.registered_semantic_lowerers[0]", + ) + options = _decode_action_options( + { + "kind": "axis_align", + "target_axis": [0.0, 0.0, 1.0], + "pre_grasp_distance": 0.12, + }, + path="policy.action_options.simulation.axis_align", + ) + + assert type(lowering.goal) is AxisAlignGoal + assert lowering.goal.semantics is semantics + assert lowering.registered_effect is not None + assert lowering.registered_effect.effect_kind is SemanticEffectKind.ATTACH + assert factory.call_id == "simulation.axis_align" + assert type(options) is AxisAlignOptions + assert options.pre_grasp_distance == pytest.approx(0.12) + torch.testing.assert_close(options.target_axis, torch.tensor([0.0, 0.0, 1.0])) + + +@pytest.mark.parametrize("kind", ["release_safe_pick", "move_held_object_upright"]) +def test_configured_services_reject_task_specific_e2_kinds(kind: str) -> None: + with pytest.raises(ValueError, match="Unsupported"): + _decode_registered_lowerer({"kind": kind, "routes": []}, path="lowerer") diff --git a/tests/gym/envs/task_program/test_simulation.py b/tests/gym/envs/task_program/test_simulation.py index ca9af66d1..3dd226360 100644 --- a/tests/gym/envs/task_program/test_simulation.py +++ b/tests/gym/envs/task_program/test_simulation.py @@ -248,6 +248,7 @@ def _scene_binding() -> SimulationSceneBinding: 1.0, ), minimum_confidence=0.8, + release_clearance=0.12, is_default=True, ), ), @@ -341,6 +342,7 @@ def test_scene_binding_builds_existing_registry_contracts() -> None: container = registry.lookup(container_ref).affordance assert type(container) is ContainerAffordance assert container.minimum_confidence == pytest.approx(0.8) + assert container.release_clearance == pytest.approx(0.12) assert torch.allclose( snapshot.entities[container_ref.entity_id].pose[:, 0, 3], torch.tensor((0.4, 0.5)), diff --git a/tests/gym/envs/task_program/test_simulation_environment.py b/tests/gym/envs/task_program/test_simulation_environment.py index d9924fd4b..ae1fa77ff 100644 --- a/tests/gym/envs/task_program/test_simulation_environment.py +++ b/tests/gym/envs/task_program/test_simulation_environment.py @@ -301,6 +301,7 @@ class _Robot: def __init__(self) -> None: self.qpos = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) + self.target_qpos = self.qpos.clone() def get_qpos( self, @@ -308,8 +309,8 @@ def get_qpos( target: bool = False, ) -> torch.Tensor: """Return full or control-part positions.""" - del target - return self.qpos if name is None else self.qpos[:, :1] + qpos = self.target_qpos if target else self.qpos + return qpos if name is None else qpos[:, :1] def get_qvel( self, @@ -1417,6 +1418,11 @@ def test_simulation_factory_builds_shared_observation_and_evidence_ports() -> No assert context.robot.timestamp == pytest.approx(0.0) assert context.control_dt == pytest.approx(_STEP_DT) assert torch.equal(observation.current_qpos(context.env_ids), robot.qpos) + robot.target_qpos.fill_(0.25) + assert torch.equal( + observation.hold_qpos(context.env_ids), + robot.target_qpos, + ) assert len(providers) == 2 assert all( getattr(provider, "_scene_provider") is observation.scene_provider diff --git a/tests/gym/envs/task_program/test_simulation_policies.py b/tests/gym/envs/task_program/test_simulation_policies.py index 67998b66e..9e5820052 100644 --- a/tests/gym/envs/task_program/test_simulation_policies.py +++ b/tests/gym/envs/task_program/test_simulation_policies.py @@ -123,13 +123,19 @@ class _Simulation: def __init__( self, entity: _RigidObject | None = None, + reference: _RigidObject | None = None, articulation: _Articulation | None = None, ) -> None: self.entity = entity + self.reference = reference self.articulation = articulation def get_rigid_object(self, uid: str) -> _RigidObject | None: - return self.entity if uid == "native_cube" else None + if uid == "native_cube": + return self.entity + if uid == "native_reference": + return self.reference + return None def get_articulation(self, uid: str) -> _Articulation | None: return self.articulation if uid == "native_drawer" else None @@ -242,9 +248,56 @@ def _compiled_articulation_segment(): return next(compiled.iter_segments()) +def _compiled_relative_segment(): + """Compile one validator with two late-observed object identities.""" + payload = { + "program_id": "relative_policy_test", + "integration": { + "robot_profile": "test_robot", + "scene_registry": "test_scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": { + "kind": "segment", + "name": "place_relative", + "steps": { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + "validators": [ + { + "kind": "object_near_relative_target", + "object": "cube", + "reference": "reference", + "displacement": [0.0, -0.1, 0.04], + "position_tolerance": 0.05, + } + ], + }, + } + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StaticStateProvider(), + ), + SceneEntityRegistration( + ref=SceneObjectRef("reference"), + state_provider=_StaticStateProvider(), + ), + ) + ) + compiled = TaskProgramCompiler.from_scene_registry(registry).compile( + decode_task_program(payload) + ) + return next(compiled.iter_segments()) + + def _port( positions: torch.Tensor, *, + preset_id: str = "fast", preset: DynamicSettleMonitorCfg | None = None, target_qpos: torch.Tensor | None = None, ) -> tuple[SimulationSegmentPolicyPort, _RigidObject, _Robot]: @@ -266,8 +319,9 @@ def _port( ), ), ), + step_dt=0.04, settle_presets={ - "fast": preset + preset_id: preset or DynamicSettleMonitorCfg( min_steps=0, max_steps=3, @@ -303,9 +357,15 @@ def test_default_settle_presets_cover_rigid_objects_and_articulations() -> None: ), ), ), + step_dt=0.04, ) - assert port.settle_preset_ids == ("rigid_object", "articulation") + assert port.settle_preset_ids == ( + "rigid_object", + "contained_rigid_object", + "transported_rigid_object", + "articulation", + ) def test_pure_preflight_validates_hooks_without_reading_live_state() -> None: @@ -393,6 +453,81 @@ def test_wait_stable_yields_fresh_target_qpos_holds_through_gym() -> None: ).tolist() == [True, True] +@pytest.mark.parametrize( + "preset_id", + ["contained_rigid_object", "transported_rigid_object"], +) +def test_contact_sensitive_wait_stable_uses_observed_pose_delta( + preset_id: str, +) -> None: + """Contact-sensitive objects use pose motion over stale solver velocity.""" + segment = _compiled_segment(settle_preset=preset_id) + port, entity, _ = _port( + torch.zeros(2, 3), + preset_id=preset_id, + preset=DynamicSettleMonitorCfg( + min_steps=0, + max_steps=4, + check_interval_steps=1, + required_stable_checks=2, + ), + ) + entity.body_data.lin_vel.fill_(4.0) + entity.body_data.ang_vel.fill_(8.0) + + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + assert sum(1 for _ in actions) == 2 + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + assert metadata["status"] == "settled" + assert metadata["measurement_source"] == "pose_delta" + assert metadata["state"]["max_linear_speed"] == [0.0, 0.0] + assert metadata["state"]["max_angular_speed"] == [0.0, 0.0] + + +def test_contained_wait_stable_rejects_observed_pose_motion() -> None: + """Pose-delta settling still times out a geometrically moving object.""" + preset_id = "contained_rigid_object" + segment = _compiled_segment(settle_preset=preset_id) + port, entity, _ = _port( + torch.zeros(2, 3), + preset_id=preset_id, + preset=DynamicSettleMonitorCfg( + min_steps=0, + max_steps=3, + check_interval_steps=1, + required_stable_checks=2, + ), + ) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + for _ in range(3): + next(actions) + entity._pose[:, 0, 3] += 0.01 + with pytest.raises(StopIteration): + next(actions) + + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + assert metadata["status"] == "timed_out" + assert metadata["measurement_source"] == "pose_delta" + assert metadata["state"]["settled_mask"] == [False, False] + assert metadata["state"]["timeout_mask"] == [True, True] + + def test_wait_stable_holds_active_targets_and_inactive_current_qpos() -> None: """Initial inactive rows use measured holds while active rows keep preload.""" segment = _compiled_segment() @@ -586,6 +721,45 @@ def test_object_near_target_validates_rows_independently() -> None: assert metadata["accepted_mask"] == [True, False] +def test_object_near_relative_target_observes_both_rows_independently() -> None: + """The validator derives each target from the current reference pose.""" + segment = _compiled_relative_segment() + obj = _RigidObject(torch.tensor([[0.01, -0.1, 0.04], [0.20, -0.1, 0.04]])) + reference = _RigidObject(torch.tensor([[0.0, 0.0, 0.0], [0.10, 0.0, 0.0]])) + port = SimulationSegmentPolicyPort( + _Simulation(entity=obj, reference=reference), + _Robot(torch.zeros(2, 2)), + SimulationSceneBinding( + registry_id="test_scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="native_cube", + ), + SimulationRigidObjectBinding( + entity_id="reference", + simulation_uid="native_reference", + ), + ), + ), + step_dt=0.04, + ) + + validator = segment.validators[0] + result = port.validate(validator, segment=segment) + metadata = port.validator_metadata(validator, segment=segment) + + assert result.tolist() == [True, False] + assert metadata["kind"] == "object_near_relative_target" + assert metadata["object_id"] == "cube" + assert metadata["reference_id"] == "reference" + assert metadata["displacement"] == pytest.approx([0.0, -0.1, 0.04]) + assert metadata["position_error"] == pytest.approx([0.01, 0.10]) + assert metadata["accepted_mask"] == [True, False] + assert obj.pose_reads == 1 + assert reference.pose_reads == 1 + + def test_articulation_joint_position_validates_measured_rows() -> None: """The joint validator applies its inclusive bound to each simulator row.""" segment = _compiled_articulation_segment() @@ -605,6 +779,7 @@ def test_articulation_joint_position_validates_measured_rows() -> None: ), ), ), + step_dt=0.04, ) validator = segment.validators[0] @@ -640,6 +815,7 @@ def test_policy_port_rejects_unbound_native_entities_and_foreign_members() -> No _Simulation(_RigidObject(torch.zeros(2, 3))), robot, binding, + step_dt=0.04, ) segment = _compiled_segment() diff --git a/tests/lab/task_program/semantics/test_profiles.py b/tests/lab/task_program/semantics/test_profiles.py index 7ba7d2c2f..9b1e2fef9 100644 --- a/tests/lab/task_program/semantics/test_profiles.py +++ b/tests/lab/task_program/semantics/test_profiles.py @@ -417,8 +417,9 @@ def test_engine_skills_only_exposes_visible_explicit_installed_contracts() -> No } assert set(engine.skills) == expected + assert "park" not in engine.actions assert "move_joints" in engine.actions - assert "move_joints" not in engine.skills + assert "move_joints" in engine.skills def test_new_skill_subclass_must_redeclare_binding_contract() -> None: diff --git a/tests/lab/task_program/test_decoder.py b/tests/lab/task_program/test_decoder.py index 04a5f75fb..1e1d4e116 100644 --- a/tests/lab/task_program/test_decoder.py +++ b/tests/lab/task_program/test_decoder.py @@ -29,6 +29,7 @@ TaskProgramIntegrationCfg, TaskProgramValidationError, HandOverCfg, + ObjectNearRelativeTargetValidatorCfg, PickCfg, PlaceCfg, RegisteredSemanticCallCfg, @@ -47,6 +48,20 @@ ) +def test_relative_validator_rejects_unmigrated_local_kind() -> None: + data = _program_data() + data["program"]["body"]["validators"] = [ + { + "kind": "object_relative_position", + "object": "cube", + "reference": "table", + "world_offset": [0.0, 0.0, 0.1], + } + ] + with pytest.raises(TaskProgramDecodeError, match="object_relative_position"): + decode_task_program(data) + + def _program_data() -> dict[str, object]: """Return the repeated-cube example as plain JSON values.""" return { @@ -166,6 +181,30 @@ def test_decoder_supports_articulation_joint_position_validator() -> None: assert validator.maximum_position is None +def test_decoder_supports_relative_object_validator() -> None: + """Relative validators own only scene identities and a finite offset.""" + data = _program_data() + segment = data["program"]["body"] + segment["validators"] = [ + { + "kind": "object_near_relative_target", + "object": "cube", + "reference": "tray_top", + "displacement": [0.0, -0.1, 0.04], + "position_tolerance": 0.05, + } + ] + + config = decode_task_program(data) + + validator = config.program.body.validators[0] + assert type(validator) is ObjectNearRelativeTargetValidatorCfg + assert validator.object == "cube" + assert validator.reference == "tray_top" + assert validator.displacement == pytest.approx((0.0, -0.1, 0.04)) + assert validator.position_tolerance == pytest.approx(0.05) + + def test_decoder_supports_every_builtin_semantic_call() -> None: data = _program_data() data["program"] = { diff --git a/tests/lab/task_program/test_program_compiler.py b/tests/lab/task_program/test_program_compiler.py index fed4343da..1edcf03f0 100644 --- a/tests/lab/task_program/test_program_compiler.py +++ b/tests/lab/task_program/test_program_compiler.py @@ -33,6 +33,7 @@ TaskProgramIntegrationCfg, HandOverCfg, InvokeCfg, + ObjectNearRelativeTargetValidatorCfg, ObjectNearTargetValidatorCfg, PickCfg, PlaceCfg, @@ -355,6 +356,38 @@ def test_compiler_resolves_articulation_joint_validator_provider_free() -> None: assert provider.calls == 0 +def test_compiler_resolves_relative_validator_provider_free() -> None: + """Compilation resolves both identities without observing either pose.""" + registry, provider = _scene_registry() + config = _program( + SegmentCfg( + name="place_relative", + steps=InvokeCfg(call=PickCfg(object="cube")), + validators=( + ObjectNearRelativeTargetValidatorCfg( + object="cube", + reference="tray", + displacement=(0.0, -0.1, 0.04), + position_tolerance=0.05, + ), + ), + ) + ) + + segment = next( + TaskProgramCompiler.from_scene_registry(registry) + .compile(config) + .iter_segments() + ) + validator = segment.validators[0] + + assert validator.object == SceneObjectRef("cube") + assert validator.reference == SceneObjectRef("tray") + assert validator.cfg.displacement == pytest.approx((0.0, -0.1, 0.04)) + assert validator.cfg.position_tolerance == pytest.approx(0.05) + assert provider.calls == 0 + + def test_repeat_expansion_is_bounded_and_never_observes_scene_providers() -> None: registry, provider = _scene_registry() config = _program( diff --git a/tests/lab/task_program/test_semantic_compiler.py b/tests/lab/task_program/test_semantic_compiler.py index e16a60be2..7746de8e1 100644 --- a/tests/lab/task_program/test_semantic_compiler.py +++ b/tests/lab/task_program/test_semantic_compiler.py @@ -72,7 +72,9 @@ HandOverPoseProvider, HandOverPoseTargets, HeldObjectGuardBaseline, + RegisteredHeldObjectEffect, RegisteredSemanticLowerer, + RegisteredSemanticEffect, RelationTargetGrounder, SemanticLowering, SemanticObjectTarget, @@ -248,6 +250,76 @@ def lower( ) +class _EffectfulInspectLowerer(_InspectLowerer): + """Registered test call declaring one compiler-grounded attach effect.""" + + effect_contract_kind: ClassVar[SemanticEffectKind] = SemanticEffectKind.ATTACH + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: object, + option_template: ActionOptions, + ) -> SemanticLowering: + lowering = super().lower( + call, + context=context, + bound=bound, + option_template=option_template, + ) + return SemanticLowering( + goal=lowering.goal, + registered_effect=RegisteredSemanticEffect( + effect_kind=SemanticEffectKind.ATTACH, + held_objects=( + RegisteredHeldObjectEffect( + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + object_id="cube", + slot_id="primary", + ), + ), + ), + ) + + +class _ReleasingInspectLowerer(_InspectLowerer): + """Registered test call requiring one verified release baseline.""" + + effect_contract_kind: ClassVar[SemanticEffectKind] = SemanticEffectKind.RELEASE + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: object, + option_template: ActionOptions, + ) -> SemanticLowering: + lowering = super().lower( + call, + context=context, + bound=bound, + option_template=option_template, + ) + return SemanticLowering( + goal=lowering.goal, + registered_effect=RegisteredSemanticEffect( + effect_kind=SemanticEffectKind.RELEASE, + held_objects=( + RegisteredHeldObjectEffect( + expectation_id="source", + relation=HeldObjectRelation.DETACHED, + object_id="cube", + slot_id="primary", + ), + ), + ), + ) + + class _RetainingInspectLowerer(_InspectLowerer): """Test extension that safely exposes retained-object look-ahead.""" @@ -267,6 +339,12 @@ def pick_lookahead_targets( return (SemanticObjectTarget(pose=self.target),) +class _StatePreservingInspectLowerer(_InspectLowerer): + """Test extension declaring an exact effectless symbolic-state contract.""" + + preserves_symbolic_state: ClassVar[bool] = True + + class _DerivedGraspGoal(GraspGoal): """Executable subclass that an extension must not smuggle into the core.""" @@ -674,6 +752,32 @@ def test_builtin_relation_grounders_preserve_late_pose_and_confidence( assert target.minimum_confidence == pytest.approx(0.65) +def test_container_relation_grounder_applies_release_clearance() -> None: + """Container placement releases above, without changing its final frame.""" + registry, _ = _scene_registry() + relation = SemanticRelationTarget( + capability=PLACE_IN_AFFORDANCE_CAPABILITY, + affordance=SceneAffordanceRef("declared_target"), + payload_type=ContainerAffordance, + payload_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + ) + + target = ContainerRelationTargetGrounder().ground( + relation, + affordance=ContainerAffordance( + minimum_confidence=0.65, + release_clearance=0.12, + ), + context=_context(registry), + ) + + assert target.entity_id == "declared_target" + assert target.relative_pose is not None + assert torch.equal(target.relative_pose[:3, :3], torch.eye(3)) + assert target.relative_pose[:3, 3].tolist() == pytest.approx([0.0, 0.0, 0.12]) + assert target.minimum_confidence == pytest.approx(0.65) + + def test_curated_analysis_selects_exact_preset_monitor_without_creating_it() -> None: registry, providers = _scene_registry() factory = _CountingRelationMonitorFactory() @@ -973,6 +1077,10 @@ def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: assert isinstance(gate_relation, HeldObjectStateExpectation) assert gate_relation.expectation_id == "source" assert gate_relation.relation is HeldObjectRelation.DETACHED + assert len(gate.effect_spec.clauses) == 1 + gate_constraint = gate.effect_spec.clauses[0] + assert isinstance(gate_constraint, BinaryEffectClause) + assert gate_constraint.expected is False assert grounded.invocation.phase_effect_gates == (gate.requirement,) @@ -1044,12 +1152,10 @@ def test_handover_effect_spec_binds_source_and_destination_relations() -> None: assert tuple(gate.gate_id for gate in grounded.effect_gates) == ( "source_acquired", "destination_acquired", - "source_released", ) - source_acquired, destination_acquired, source_released = grounded.effect_gates + source_acquired, destination_acquired = grounded.effect_gates assert source_acquired.segment_name == "pickup_transport" assert destination_acquired.segment_name == "handover_release" - assert source_released.segment_name == "place" assert all(gate.retry_action for gate in grounded.effect_gates) assert source_acquired.effect_monitor is not source_guard.effect_monitor assert destination_acquired.effect_monitor is not destination_guard.effect_monitor @@ -1057,18 +1163,66 @@ def test_handover_effect_spec_binds_source_and_destination_relations() -> None: destination_acquired_relation = destination_acquired.effect_spec.state_expectations[ 0 ] - source_released_relation = source_released.effect_spec.state_expectations[0] assert isinstance(source_acquired_relation, HeldObjectStateExpectation) assert isinstance(destination_acquired_relation, HeldObjectStateExpectation) - assert isinstance(source_released_relation, HeldObjectStateExpectation) assert source_acquired_relation.relation is HeldObjectRelation.ATTACHED assert destination_acquired_relation.relation is HeldObjectRelation.ATTACHED - assert source_released_relation.relation is HeldObjectRelation.DETACHED assert grounded.invocation.phase_effect_gates == tuple( gate.requirement for gate in grounded.effect_gates ) +def test_isolated_handover_adopts_verified_source_hold_during_grounding() -> None: + registry, _ = _scene_registry() + provider = _DualCenterHandOverProvider() + compiler, _ = _compiler( + registry, + profile=_dual_profile(), + handover_pose_providers=(provider,), + ) + workflow = compiler.analyze((HandOver(object=SceneObjectRef("cube")),)) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = compiler.ground( + pick_workflow, + 0, + _context(registry, robot_dof=4), + ).invocation.goal.semantics + context = _held_context( + registry, + semantics, + torch.eye(4).repeat(2, 1, 1), + task_state_key="left", + robot_dof=4, + ) + + grounded = compiler.ground(workflow, 0, context) + + assert workflow.calls[0].handover_from_held is False + assert grounded.analyzed.handover_from_held is True + assert grounded.analyzed.effect_kind is SemanticEffectKind.TRANSFER + assert grounded.analyzed.requires_verified_held_object is True + assert type(grounded.invocation.skill_options) is HandOverOptions + assert grounded.invocation.skill_options.release_at_target is False + assert tuple(gate.gate_id for gate in grounded.effect_gates) == ( + "destination_acquired", + ) + assert tuple(gate.segment_name for gate in grounded.effect_gates) == ( + "handover_release", + ) + source_guard, destination_guard = grounded.effect_guards + assert source_guard.active_segments == ( + "transfer", + "receive_approach", + "receive_close", + "receive_hold", + ) + assert destination_guard.active_segments == ( + "handover_release", + "source_retreat", + ) + assert destination_guard.retry_action is False + + def test_registered_call_without_monitor_has_no_effect_contract() -> None: registry, _ = _scene_registry() factory = _CountingRelationMonitorFactory() @@ -1108,6 +1262,31 @@ def test_registered_call_without_monitor_has_no_effect_contract() -> None: assert factory.calls == 0 +def test_registered_state_preserving_call_keeps_held_dependency() -> None: + """An explicitly effectless extension does not erase verified held flow.""" + registry, _ = _scene_registry() + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_StatePreservingInspectLowerer(),), + ) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + RegisteredSemanticCall(call_id="vendor.inspect"), + Place( + object=SceneObjectRef("cube"), + at=SemanticPose((0.3, 0.0, 0.2), (1.0, 0.0, 0.0, 0.0)), + ), + ) + ) + + assert not workflow.calls[1].opaque_symbolic_effect + assert workflow.calls[1].symbolic_writes == frozenset() + assert workflow.effect_dependencies[0].producer_index == 0 + assert workflow.calls[0].downstream_object_targets == () + + def test_registered_monitor_without_effect_grounder_fails_during_analysis() -> None: registry, _ = _scene_registry() profile = _profile( @@ -1136,6 +1315,96 @@ def test_registered_monitor_without_effect_grounder_fails_during_analysis() -> N assert error.value.diagnostic.path == ("workflow", 0, "effect_monitor") +def test_registered_effect_contract_is_grounded_by_compiler() -> None: + """An extension declares intent while the compiler binds physical sources.""" + registry, _ = _scene_registry() + templates = _action_option_templates(registered=True) + templates["vendor.inspect"] = PickUpOptions(pre_grasp_distance=0.07) + profile = _profile( + preset=_preset( + "safe", + registered=True, + action_option_templates=templates, + effect_monitors={ + "vendor.inspect": EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + }, + ) + ) + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_EffectfulInspectLowerer(),), + profile=profile, + ) + + workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + grounded = compiler.ground(workflow, 0, _context(registry)) + + assert grounded.effect_spec is not None + assert grounded.effect_spec.effect_kind is SemanticEffectKind.ATTACH + expectation = grounded.effect_spec.state_expectation("destination") + assert type(expectation) is HeldObjectStateExpectation + assert expectation.relation is HeldObjectRelation.ATTACHED + assert expectation.slot_id == "primary" + assert expectation.object_id == "cube" + assert grounded.effect_monitor is not None + + +def test_registered_release_requires_object_held_in_every_eligible_row() -> None: + """Registered release calls cannot consume inactive held-state rows.""" + registry, _ = _scene_registry() + templates = _action_option_templates(registered=True) + templates["vendor.inspect"] = PickUpOptions(pre_grasp_distance=0.07) + profile = _profile( + preset=_preset( + "safe", + registered=True, + action_option_templates=templates, + effect_monitors={ + "vendor.inspect": EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + }, + ) + ) + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_ReleasingInspectLowerer(),), + profile=profile, + ) + pick_compiler, _ = _compiler(registry) + pick_workflow = pick_compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = pick_compiler.ground( + pick_workflow, + 0, + _context(registry), + ).invocation.goal.semantics + context = _held_context( + registry, + semantics, + torch.eye(4).repeat(2, 1, 1), + env_mask=torch.tensor([True, False]), + ) + workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, context) + + assert error.value.diagnostic.code == "verified_held_object_required" + grounded = compiler.ground( + workflow, + 0, + context, + eligible_mask=torch.tensor([True, False]), + ) + assert grounded.eligible_mask.tolist() == [True, False] + + def test_ground_wraps_effect_monitor_factory_contract_failure_with_path() -> None: registry, _ = _scene_registry() compiler, _ = _compiler( @@ -1194,6 +1463,42 @@ def test_analysis_is_provider_free_and_propagates_object_target() -> None: engine.resolve(grounded.invocation) +def test_pick_lookahead_reserves_the_opposite_handover_object_part() -> None: + """A selected HandOver suffix, rather than Task Engine, chooses the grasp.""" + registry, _ = _scene_registry() + provider = _DualCenterHandOverProvider() + compiler, _ = _compiler( + registry, + profile=_dual_profile(), + handover_pose_providers=(provider,), + ) + + isolated = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + HandOver(object=SceneObjectRef("cube")), + ) + ) + + assert isolated.calls[0].handover_source_pick_object_part is None + isolated_options = compiler.ground( + isolated, + 0, + _context(registry, robot_dof=4), + ).invocation.skill_options + assert type(isolated_options) is PickUpOptions + assert isolated_options.pick_object_part == "center" + assert workflow.calls[0].handover_source_pick_object_part == "top" + handover_options = compiler.ground( + workflow, + 0, + _context(registry, robot_dof=4), + ).invocation.skill_options + assert type(handover_options) is PickUpOptions + assert handover_options.pick_object_part == "top" + + def test_pick_lookahead_uses_downstream_place_orientation_policy() -> None: """Pickup feasibility must screen the object pose that Place will use.""" registry, providers = _scene_registry() @@ -1275,7 +1580,7 @@ def test_grounded_safe_invocation_requires_registered_dynamic_collision() -> Non assert resolved_tracking.metrics[0].tolerance == 0.125 -def test_pick_relation_lookahead_stays_late_bound_scene_dependency() -> None: +def test_pick_relation_lookahead_is_not_an_active_scene_dependency() -> None: registry, _ = _scene_registry() compiler, engine = _compiler(registry) workflow = compiler.analyze( @@ -1298,7 +1603,9 @@ def test_pick_relation_lookahead_stays_late_bound_scene_dependency() -> None: assert downstream.entity_id == "table_top" request = engine.resolve(grounded.invocation) action = engine.actions["pick_up"] - assert "table_top" in action._scene_dependencies(request) + # The suffix still guides grasp selection, but Place observes and grounds + # its destination again after Pick reaches a verified semantic boundary. + assert action._scene_dependencies(request) == ("cube",) def test_pick_replan_resolves_downstream_target_from_latest_snapshot() -> None: diff --git a/tests/lab/task_program/test_semantic_executor.py b/tests/lab/task_program/test_semantic_executor.py index b65c6c644..9d894d076 100644 --- a/tests/lab/task_program/test_semantic_executor.py +++ b/tests/lab/task_program/test_semantic_executor.py @@ -313,6 +313,29 @@ def _plan( ) +class _EffectlessAction(_EffectAction): + """Zero-frame action whose successful completion has no task-state effect.""" + + skill_id: ClassVar[str] = "runtime_test_effectless" + open_loop: ClassVar[bool] = True + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract() + + def _plan( + self, + request: ResolvedActionRequest[_EffectGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + self.plan_count += 1 + return self.build_command_plan( + request, + context, + success=goal.plan_success, + commands=TimedCommandSequence((), context.env_ids), + replannable=False, + ) + + @dataclass(frozen=True, slots=True) class _WorkflowEffectGoal: """Test-only held-object effect for workflow recovery.""" @@ -473,6 +496,8 @@ def __init__( runner_cfg: ExecutionRunnerCfg, *, install_effect_monitor: bool, + effect_assurance: EffectAssurance | None = None, + skill_id: str = _EffectAction.skill_id, ) -> None: self._test_integration = _Integration(engine, SceneRegistry()) self._decisions = tuple( @@ -494,6 +519,8 @@ def __init__( self._plan_success = plan_success self._runner_cfg = runner_cfg self._install_effect_monitor = install_effect_monitor + self._effect_assurance = effect_assurance + self._skill_id = skill_id self.analyze_count = 0 self.ground_count = 0 self.ground_timestamps: list[float] = [] @@ -534,13 +561,13 @@ def ground( self.ground_task_masks.append(None if state is None else state.env_mask.clone()) call = workflow.calls[call_index] invocation = ActionInvocation( - skill_id=_EffectAction.skill_id, + skill_id=self._skill_id, goal=_EffectGoal( self._plan_success[call_index].clone(), float(call_index + 1), ), binding=self.integration.engine.bind_control_parts( - _EffectAction.skill_id, + self._skill_id, {}, ), motion_policy=MotionPolicy( @@ -592,9 +619,13 @@ def ground( analyzed = SimpleNamespace( call=call, effect_assurance=( - EffectAssurance.VERIFIED - if self._install_effect_monitor - else EffectAssurance.PROJECTED + self._effect_assurance + if self._effect_assurance is not None + else ( + EffectAssurance.VERIFIED + if self._install_effect_monitor + else EffectAssurance.PROJECTED + ) ), effect_monitor_ref=None, bound=SimpleNamespace( @@ -870,6 +901,8 @@ def _system( preset_runner_cfg: ExecutionRunnerCfg | None = None, runtime_runner_cfg: ExecutionRunnerCfg | None = None, install_effect_monitor: bool = True, + effect_assurance: EffectAssurance | None = None, + effectless_action: bool = False, ) -> _System: robot = Mock() robot.device = torch.device("cpu") @@ -882,7 +915,7 @@ def _system( generator.device = torch.device("cpu") generator.planner.cfg.planner_type = "runtime_test" engine = AtomicActionEngine(generator, load_builtins=False) - action = _EffectAction() + action = _EffectlessAction() if effectless_action else _EffectAction() engine.register(action) selected_plan_success = plan_success or tuple(_mask(True, True) for _ in decisions) selected_runner_cfg = ( @@ -894,6 +927,8 @@ def _system( selected_plan_success, selected_runner_cfg, install_effect_monitor=install_effect_monitor, + effect_assurance=effect_assurance, + skill_id=action.skill_id, ) observation = _ObservationProvider() sink = _CommandSink() @@ -1028,6 +1063,22 @@ def test_runtime_projects_planned_effect_when_grounded_call_has_no_monitor() -> assert torch.allclose(joint.position, torch.ones(BATCH_SIZE, 1)) +def test_verified_runtime_accepts_an_effectless_motion_without_a_monitor() -> None: + """Tracking completion is sufficient when no effect claim exists to verify.""" + system = _system( + (EffectMonitorDecision(_mask(True, True), _mask(False, False)),), + install_effect_monitor=False, + effect_assurance=EffectAssurance.VERIFIED, + effectless_action=True, + ) + + result = system.runtime.run(_call("park")) + + assert result.status is SemanticExecutionStatus.COMPLETED + assert result.effects == () + assert result.task_state.get_articulation_joint_state("fixture", "joint") is None + + def test_runtime_uses_selected_preset_runner_cfg_without_override( monkeypatch: pytest.MonkeyPatch, ) -> None: diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index e96c35353..61fdf9696 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -72,6 +72,7 @@ OpenDoorAffordance, OpenDoorGoal, OpenDoorOptions, + PARK_COMMAND, PickUp, PickUpOptions, Place, @@ -603,6 +604,18 @@ def compute_ik( offset = 0.1 if name == "left_arm" else 0.2 return torch.ones(seed.shape[0], dtype=torch.bool), seed + offset + def compute_batch_ik( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + **_: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + offset = 0.1 if name == "left_arm" else 0.2 + return ( + torch.ones(pose.shape[:2], dtype=torch.bool), + joint_seed + offset, + ) + def compute_fk( qpos: torch.Tensor | None = None, name: str | None = None, @@ -614,6 +627,7 @@ def compute_fk( robot.get_qpos.side_effect = get_qpos robot.get_joint_ids.side_effect = get_joint_ids robot.compute_ik.side_effect = compute_ik + robot.compute_batch_ik.side_effect = compute_batch_ik robot.compute_fk.side_effect = compute_fk robot.get_link_pose.side_effect = get_link_pose @@ -969,6 +983,39 @@ def test_move_joints_uses_binding_and_preserves_uncontrolled_joints() -> None: assert [target.target_id for target in plan.commands.targets] == ["arm"] +def test_move_joints_uses_park_profile_command_and_preserves_other_joints() -> None: + """The named park target stays in the embodiment's command profile.""" + generator = _motion_generator() + target = torch.full((ARM_DOF,), 0.4) + action = _bind_action( + generator, + MoveJoints(), + control_profiles={ + "arm": ControlPartCommandProfile.joint_positions(**{PARK_COMMAND: target}), + }, + ) + qpos = torch.zeros(NUM_ENVS, ROBOT_DOF) + qpos[:, ARM_DOF:] = 0.7 + context = PlanningContext( + robot=RobotObservation(0.0, qpos, torch.zeros_like(qpos)), + task=TaskState.empty(NUM_ENVS, "cpu"), + scene=SceneSnapshot.empty(), + env_ids=torch.arange(NUM_ENVS), + control_dt=CONTROL_DT, + ) + + plan = _plan_action( + action, + _invocation(action, JointPositionGoal(PARK_COMMAND), sample_count=8), + context, + ) + + assert torch.allclose(_joint_command_positions(plan, "arm")[:, -1], target) + assert [target.target_id for target in plan.commands.targets] == ["arm"] + assert plan.expected_effects.is_empty + assert MoveJoints.descriptor().agent_visible is True + + def test_pick_and_place_declare_effects_without_mutating_context() -> None: generator = _motion_generator() pick = _bind_action(generator, PickUp()) @@ -1029,15 +1076,29 @@ def test_place_holds_fully_open_before_retracting_when_configured() -> None: settle_steps = 3 invocation = ActionInvocation( skill_id=action.skill_id, - goal=PlaceGoal(torch.eye(4)), + goal=PlaceGoal(SceneEntityPose("destination")), binding=_binding(action), motion_policy=MotionPolicy(sample_count=sample_count), skill_options=PlaceOptions(release_settle_steps=settle_steps), ) - plan = _plan_action(action, invocation, _context(task)) + destination_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + plan = _plan_action( + action, + invocation, + _context( + task, + scene=SceneSnapshot( + timestamp=0.0, + version=0, + entities={"destination": EntityState(destination_pose)}, + ), + ), + ) release = plan.segment("release") + assert plan.scene_dependencies == ("destination",) + assert plan.scene_dependency_monitor_until == {"destination": release.start} assert plan.commands.frame_count == sample_count + settle_steps assert release.stop - release.start == 5 + settle_steps hand_positions = _joint_command_positions(plan, "hand") @@ -1256,8 +1317,8 @@ def compute_ik( semantics = ObjectSemantics( affordance=AxisAlignAffordance(internal_axis=torch.tensor([1.0, 0.0, 0.0])), geometry={}, - label="pourable-object", entity_id="pourable-object", + label="pourable-object", ) task = TaskState( batch_size=NUM_ENVS, @@ -1311,8 +1372,8 @@ def test_pour_reads_held_state_from_the_bound_logical_resource() -> None: semantics = ObjectSemantics( affordance=AxisAlignAffordance(internal_axis=torch.tensor([1.0, 0.0, 0.0])), geometry={}, - label="pourable-object", entity_id="pourable-object", + label="pourable-object", ) logical_resource = "right_manipulator" task = TaskState( @@ -1420,8 +1481,8 @@ def test_pour_requires_exclusively_held_axis_align_affordance() -> None: semantics = ObjectSemantics( affordance=AxisAlignAffordance(), geometry={}, - label="shared-pourable-object", entity_id="shared-pourable-object", + label="shared-pourable-object", ) task = TaskState( batch_size=NUM_ENVS, @@ -1667,13 +1728,27 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] ) object_pose[:, 0, 3] = torch.tensor([0.03, 0.07]) - context = _context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) + context = _context( + scene=SceneSnapshot( + timestamp=0.0, + version=0, + entities={ + "target": EntityState(object_pose), + "downstream": EntityState(torch.eye(4).repeat(NUM_ENVS, 1, 1)), + }, + ) + ) request = action.resolve_request( - _invocation( - action, - GraspGoal(semantics=semantics, grasp_xpos=grasp), - sample_count=20, + ActionInvocation( + skill_id=action.skill_id, + goal=GraspGoal(semantics=semantics, grasp_xpos=grasp), + binding=_binding(action), + motion_policy=MotionPolicy(sample_count=20), + skill_options=PickUpOptions( + downstream_object_target_poses=(SceneEntityPose("downstream"),), + grasp_commit_fraction=0.6, + ), ) ) plan = action.plan(request, context) @@ -1684,6 +1759,8 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: assert held is not None assert torch.allclose(held.grasp_xpos, grasp) assert torch.allclose(held.object_to_eef, torch.bmm(pose_inv(object_pose), grasp)) + # Downstream targets influence grasp selection only. They are re-grounded + # at the next Semantic Call boundary and do not invalidate active pickup. assert plan.scene_dependencies == ("target",) assert [segment.name for segment in plan.segments] == [ "approach", @@ -1692,7 +1769,7 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: ] assert plan.segment("close").stop == plan.segment("lift").start assert plan.scene_dependency_monitor_until == { - "target": plan.segment("close").start + "target": math.ceil(plan.segment("approach").stop * 0.6) } @@ -1819,6 +1896,7 @@ def compute_ik( torch.testing.assert_close(held.grasp_xpos, solved_poses[-1]) assert context.task is original_task assert plan.scene_dependencies == ("target",) + assert plan.scene_dependency_monitor_until == {"target": plan.segments[0].stop} final_object_rotation = solved_poses[-1][:, :3, :3] final_world_axis = torch.matmul( final_object_rotation, @@ -1940,8 +2018,8 @@ def test_axis_align_validates_goal_and_binding_contract() -> None: semantics = ObjectSemantics( affordance=AxisAlignAffordance(), geometry={}, - label="axis-object", entity_id="axis-object", + label="axis-object", ) with pytest.raises(TypeError, match="expects goal AxisAlignGoal"): @@ -2375,6 +2453,12 @@ def test_pick_uses_selected_control_part_for_state_and_commands() -> None: "alternate_arm", "alternate_hand", } + assert plan.tracking is not None + assert all( + {setpoint.endpoint_key for setpoint in frame.setpoints} + == {("primary", "motion")} + for frame in plan.tracking.frames + ) def test_press_closes_hand_without_changing_projected_attachment() -> None: @@ -4105,8 +4189,224 @@ def plan_from_start( assert context.task is original_task assert plan.scene_dependencies == ("handover_object",) assert plan.scene_dependency_monitor_until == { - "handover_object": plan.segment("pickup_close").stop + "handover_object": plan.segment("pickup_close").start + } + + +def test_handover_can_end_with_receiving_resource_holding_object( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Transfer-only mode stops after source release and publishes held state.""" + action = _bind_action(_dual_motion_generator(), HandOver()) + semantics, _ = _handover_semantics() + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + object_pose[:, :3, 3] = torch.tensor([-0.8, 0.1, 0.5]) + + action._resolve_grasp = Mock( + side_effect=lambda _affordance, sampled_pose, *_args, **_kwargs: ( + sampled_pose.clone(), + torch.ones(NUM_ENVS, dtype=torch.bool), + ) + ) + + def plan_from_start( + motion_generator: MotionGenerator, + control_part: str, + start_qpos: torch.Tensor, + target_poses: torch.Tensor, + n_waypoints: int, + motion_policy: MotionPolicy, + interpolation_dt: float | None, + ) -> tuple[torch.Tensor, torch.Tensor]: + del ( + motion_generator, + control_part, + target_poses, + motion_policy, + interpolation_dt, + ) + trajectory = start_qpos.unsqueeze(1).repeat(1, n_waypoints, 1) + return torch.ones(NUM_ENVS, dtype=torch.bool), trajectory + + monkeypatch.setattr( + "embodichain.lab.sim.atomic_actions.primitives.hand_over." + "plan_named_arm_trajectory", + plan_from_start, + ) + final_pose = torch.eye(4) + final_pose[2, 3] = 2.0 + invocation = ActionInvocation( + skill_id="hand_over", + goal=HandOverGoal(semantics, target_pose=final_pose), + binding=_dual_binding(action, "source", "destination"), + motion_policy=MotionPolicy(sample_count=18), + skill_options=HandOverOptions( + hand_interp_steps=2, + release_at_target=False, + arm_selection="bound", + ), + ) + context = _handover_context(object_pose) + + plan = _plan_action(action, invocation, context) + + assert plan.plan_success.tolist() == [True, True] + assert [segment.name for segment in plan.segments] == [ + "pickup_approach", + "pickup_close", + "pickup_transport", + "receive_approach", + "receive_close", + "handover_release", + ] + projected = plan.expected_effects.apply( + context.task, + torch.ones(NUM_ENVS, dtype=torch.bool), + ) + assert plan.expected_effects.held_object_updates["left_arm"] is None + assert projected.get_held_object("left_arm") is None + received = projected.get_held_object("right_arm") + assert isinstance(received, HeldObjectState) + assert received.env_mask is not None and received.env_mask.tolist() == [True, True] + assert all( + candidate.env_mask is not None and candidate.env_mask.tolist() == [True, True] + for candidate in plan.effect_candidates.held_object_updates.values() + if isinstance(candidate, HeldObjectState) + ) + assert plan.scene_dependency_monitor_until == { + "handover_object": plan.segment("pickup_close").start } + trajectory = _joint_trajectory(plan) + assert torch.all(trajectory.positions[:, -1, DUAL_ARM_DOF : DUAL_ARM_DOF + 2] == 0) + assert torch.all(trajectory.positions[:, -1, DUAL_ARM_DOF + 2 :] == 1) + + +def test_handover_existing_hold_uses_root_midpoint_and_absolute_height( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """The receiver enters diagonally from its embodiment side.""" + action = _bind_action(_dual_motion_generator(), HandOver()) + semantics, _ = _handover_semantics() + held = _held(semantics, env_mask=torch.ones(NUM_ENVS, dtype=torch.bool)) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"left_arm": held}, + ) + action._resolve_grasp = Mock( + side_effect=lambda _affordance, sampled_pose, *_args, **_kwargs: ( + sampled_pose.clone(), + torch.ones(NUM_ENVS, dtype=torch.bool), + ) + ) + planned_targets: list[torch.Tensor] = [] + + def plan_from_start( + motion_generator: MotionGenerator, + control_part: str, + start_qpos: torch.Tensor, + target_poses: torch.Tensor, + n_waypoints: int, + motion_policy: MotionPolicy, + interpolation_dt: float | None, + ) -> tuple[torch.Tensor, torch.Tensor]: + del motion_generator, control_part, motion_policy, interpolation_dt + planned_targets.append(target_poses.clone()) + trajectory = start_qpos.unsqueeze(1).repeat(1, n_waypoints, 1) + return torch.ones(NUM_ENVS, dtype=torch.bool), trajectory + + monkeypatch.setattr( + "embodichain.lab.sim.atomic_actions.primitives.hand_over." + "plan_named_arm_trajectory", + plan_from_start, + ) + exchange = torch.eye(4) + exchange[0, 3] = 0.6 + exchange[1, 3] = 0.4 + exchange[2, 3] = 0.05 + + plan = _plan_action( + action, + ActionInvocation( + skill_id="hand_over", + goal=HandOverGoal(semantics, target_pose=exchange), + binding=_dual_binding(action, "source", "destination"), + motion_policy=MotionPolicy(sample_count=60), + skill_options=HandOverOptions( + release_at_target=False, + receive_pick_object_part="center", + ), + ), + _handover_context(torch.eye(4).repeat(NUM_ENVS, 1, 1), task), + ) + + assert plan.plan_success.tolist() == [True, True] + grasp_call = action._resolve_grasp.call_args + component = math.sqrt(0.5) + expected = torch.tensor([-component, 0.0, -component]).expand(NUM_ENVS, -1) + torch.testing.assert_close(grasp_call.args[2], expected) + assert grasp_call.kwargs["obj_longest_axis"] is None + torch.testing.assert_close( + grasp_call.kwargs["center_axis"], + torch.tensor([[0.0, 0.0, 1.0]]).expand(NUM_ENVS, -1), + ) + # Root geometry owns the shared-workspace coordinate. The provider's x/y + # values are final-delivery hints and cannot bias a transfer-only route. + torch.testing.assert_close( + planned_targets[0][:, 0, :2, 3], + torch.zeros(NUM_ENVS, 2), + ) + # The configured provider still owns the absolute safe exchange height; + # existing-hold mode must not add lift_height once more. + torch.testing.assert_close( + planned_targets[0][:, 0, 2, 3], + torch.full((NUM_ENVS,), 0.05), + ) + assert [segment.name for segment in plan.segments] == [ + "transfer", + "receive_approach", + "receive_close", + "receive_hold", + "handover_release", + "source_retreat", + ] + + +def test_handover_center_grasp_rejects_lower_cost_outer_candidates() -> None: + """Center mode selects the object's middle third, not either end.""" + action = _bind_action(_dual_motion_generator(), HandOver()) + vertices = torch.tensor( + [ + [-0.1, -0.1, -1.0], + [0.1, -0.1, -1.0], + [0.1, 0.1, 1.0], + [-0.1, 0.1, 1.0], + ], + dtype=torch.float32, + ) + affordance = AntipodalAffordance( + mesh_vertices=vertices, + mesh_triangles=torch.tensor([[0, 1, 2], [0, 2, 3]]), + ) + candidates = torch.eye(4).repeat(3, 1, 1) + candidates[:, 2, 3] = torch.tensor([0.8, 0.0, -0.8]) + costs = torch.tensor([0.0, 1.0, 0.5]) + _GRASP_GENERATORS[id(action)].get_valid_grasp_poses = Mock( + return_value=[(candidates, costs) for _ in range(NUM_ENVS)] + ) + + poses, success = action._resolve_grasp( + affordance, + torch.eye(4).repeat(NUM_ENVS, 1, 1), + torch.tensor([[1.0, 0.0, -1.0]]).expand(NUM_ENVS, -1), + "right_hand", + obj_longest_axis=None, + is_positive_part=torch.ones(NUM_ENVS, dtype=torch.bool), + center_axis=torch.tensor([[0.0, 0.0, 1.0]]).expand(NUM_ENVS, -1), + ) + + assert success.tolist() == [True, True] + torch.testing.assert_close(poses[:, 2, 3], torch.zeros(NUM_ENVS)) def test_handover_horizontal_mode_uses_downward_opposite_end_grasps() -> None: @@ -4365,6 +4665,69 @@ def test_handover_requires_antipodal_affordance_and_valid_options() -> None: HandOverOptions(lift_height=float("nan")) with pytest.raises(ValueError, match="hand_interp_steps"): HandOverOptions(hand_interp_steps=0) + with pytest.raises(ValueError, match="retreat_distance"): + HandOverOptions(retreat_distance=float("nan")) + + +def test_handover_source_retreat_retraces_grasp_before_lifting() -> None: + source = torch.eye(4).repeat(NUM_ENVS, 1, 1) + source[:, 1, 3] = -0.06 + source[:, 2, 3] = 0.90 + source[:, :3, :3] = torch.diag(torch.tensor([1.0, -1.0, -1.0])) + destination = torch.eye(4).repeat(NUM_ENVS, 1, 1) + destination[:, 2, 3] = 0.88 + + waypoints = HandOver._source_retreat_waypoints( + source, + destination, + source_fallback=source, + destination_fallback=destination, + retreat_distance=0.12, + lift_height=0.08, + ) + + assert waypoints.shape == (NUM_ENVS, 5, 4, 4) + expected_y = torch.full((5,), -0.06) + expected_z = torch.tensor([0.94, 0.98, 1.02, 1.06, 1.10]) + torch.testing.assert_close( + waypoints[:, :, 1, 3], + expected_y.expand(NUM_ENVS, -1), + ) + torch.testing.assert_close( + waypoints[:, :, 2, 3], + expected_z.expand(NUM_ENVS, -1), + ) + torch.testing.assert_close( + waypoints[:, :, :3, :3], + source[:, None, :3, :3].expand(-1, 5, -1, -1), + ) + + +def test_handover_source_retreat_does_not_sweep_toward_receiver() -> None: + source_exchange = torch.eye(4).repeat(NUM_ENVS, 1, 1) + source_exchange[:, 1, 3] = -0.06 + source_exchange[:, :3, :3] = torch.diag(torch.tensor([1.0, -1.0, -1.0])) + destination_grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) + source_start = torch.eye(4).repeat(NUM_ENVS, 1, 1) + source_start[:, 1, 3] = -0.5 + destination_start = torch.eye(4).repeat(NUM_ENVS, 1, 1) + destination_start[:, 1, 3] = 0.5 + + waypoints = HandOver._source_retreat_waypoints( + source_exchange, + destination_grasp, + source_fallback=source_start, + destination_fallback=destination_start, + retreat_distance=0.12, + lift_height=0.08, + ) + + torch.testing.assert_close( + waypoints[:, -1, :2, 3], + torch.tensor([[0.0, -0.06]]).expand(NUM_ENVS, -1), + ) + torch.testing.assert_close(waypoints[:, -1, 2, 3], torch.full((NUM_ENVS,), 0.20)) + torch.testing.assert_close(waypoints[:, -1, :3, :3], source_exchange[:, :3, :3]) @pytest.mark.parametrize( @@ -4487,6 +4850,9 @@ def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: ].get_dual_arm_valid_grasp_poses.call_args.kwargs["obj_poses"] assert torch.equal(sampled_pose, object_pose) assert plan.scene_dependencies == ("target",) + assert plan.scene_dependency_monitor_until == { + "target": math.ceil(plan.segment("approach").stop / 2) + } left_held = projected.get_held_object("left_arm") right_held = projected.get_held_object("right_arm") assert left_held is not None and right_held is not None @@ -4495,6 +4861,309 @@ def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: assert torch.allclose(right_held.object_to_eef, pose_inv(object_pose)) +def test_coordinated_pick_selects_reachable_candidate_over_lower_cost() -> None: + generator = _dual_motion_generator() + original_batch_ik = generator.robot.compute_batch_ik.side_effect + + def reject_distant_candidate( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + **kwargs: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + success, qpos = original_batch_ik( + pose=pose, + name=name, + joint_seed=joint_seed, + **kwargs, + ) + return success & (pose[..., 0, 3].abs() < 1.0), qpos + + generator.robot.compute_batch_ik.side_effect = reject_distant_candidate + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + object_motion_keyframes=3, + ), + ), + ) + distant = torch.eye(4, dtype=torch.float32) + distant[0, 3] = 5.0 + reachable = torch.eye(4, dtype=torch.float32) + candidates = torch.stack((distant, reachable)) + arm_result = { + "is_success": True, + "grasp_poses": candidates, + "open_lengths": torch.zeros(2), + "total_cost": torch.tensor([0.0, 1.0]), + } + _GRASP_GENERATORS[id(action)].get_dual_arm_valid_grasp_poses = Mock( + return_value=[ + {"left": arm_result, "right": arm_result} for _ in range(NUM_ENVS) + ] + ) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="tray", + entity_id="tray", + ), + object_target_pose=torch.eye(4), + object_initial_pose=torch.eye(4), + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + context = _dual_context() + + plan = _plan_action(action, invocation, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) + + assert plan.plan_success.tolist() == [True, True] + # Both arms screen approach/grasp/lift plus the configured object-motion + # continuation, rather than jumping directly from lift to the final pose. + assert generator.robot.compute_batch_ik.call_count == 10 + left_held = projected.get_held_object("left_arm") + right_held = projected.get_held_object("right_arm") + assert left_held is not None and right_held is not None + torch.testing.assert_close( + left_held.grasp_xpos, + reachable.expand(NUM_ENVS, -1, -1), + ) + torch.testing.assert_close( + right_held.grasp_xpos, + reachable.expand(NUM_ENVS, -1, -1), + ) + + +def test_coordinated_pick_canonicalizes_parallel_jaw_half_turn() -> None: + """A sampled wrist-roll equivalent must not make a top grasp unreachable.""" + generator = _dual_motion_generator() + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + object_motion_keyframes=3, + ), + ), + ) + half_turn = torch.eye(4, dtype=torch.float32) + half_turn[0, 0] = -1.0 + half_turn[1, 1] = -1.0 + arm_result = { + "is_success": True, + "grasp_poses": half_turn.unsqueeze(0), + "open_lengths": torch.zeros(1), + "total_cost": torch.zeros(1), + } + _GRASP_GENERATORS[id(action)].get_dual_arm_valid_grasp_poses = Mock( + return_value=[ + {"left": arm_result, "right": arm_result} for _ in range(NUM_ENVS) + ] + ) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="tray", + entity_id="tray", + ), + object_target_pose=torch.eye(4), + object_initial_pose=torch.eye(4), + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + + plan = _plan_action(action, invocation, _dual_context()) + projected = plan.expected_effects.apply( + TaskState.empty(NUM_ENVS, "cpu"), + plan.plan_success, + ) + + assert plan.success_all + left_held = projected.get_held_object("left_arm") + right_held = projected.get_held_object("right_arm") + assert left_held is not None and right_held is not None + identity = torch.eye(4).expand(NUM_ENVS, -1, -1) + torch.testing.assert_close(left_held.grasp_xpos, identity) + torch.testing.assert_close(right_held.grasp_xpos, identity) + + +def test_coordinated_pick_searches_geometry_adaptive_partitions() -> None: + """The Atomic Action, not its caller, recovers a blocked tray partition.""" + generator = _dual_motion_generator() + action = _bind_action(generator, CoordinatedPickment()) + vertices = torch.tensor( + [ + [-0.2, -0.1, -0.02], + [-0.2, -0.1, 0.02], + [-0.2, 0.1, -0.02], + [-0.2, 0.1, 0.02], + [0.2, -0.1, -0.02], + [0.2, -0.1, 0.02], + [0.2, 0.1, -0.02], + [0.2, 0.1, 0.02], + ], + dtype=torch.float32, + ) + triangles = torch.tensor( + [[0, 1, 2], [1, 2, 3], [4, 5, 6], [5, 6, 7]], + dtype=torch.long, + ) + sampled_ratios: list[float] = [] + + def sample_with_blocked_preferred_partition( + *, + obj_poses: torch.Tensor, + middle_empty_ratio: float, + **_kwargs: object, + ) -> list[dict[str, dict[str, object]] | None]: + sampled_ratios.append(middle_empty_ratio) + if len(sampled_ratios) == 1: + return [None for _ in range(obj_poses.shape[0])] + arm = { + "is_success": True, + "grasp_poses": torch.eye(4, dtype=torch.float32).unsqueeze(0), + "open_lengths": torch.tensor([0.0], dtype=torch.float32), + "total_cost": torch.tensor([0.0], dtype=torch.float32), + } + return [{"left": arm, "right": arm} for _ in range(obj_poses.shape[0])] + + _GRASP_GENERATORS[id(action)].get_dual_arm_valid_grasp_poses = Mock( + side_effect=sample_with_blocked_preferred_partition + ) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance( + mesh_vertices=vertices, + mesh_triangles=triangles, + ), + geometry={}, + label="tray", + entity_id="tray", + ), + object_target_pose=torch.eye(4), + object_initial_pose=torch.eye(4), + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + + plan = _plan_action(action, invocation, _dual_context()) + + assert plan.plan_success.tolist() == [True, True] + assert len(sampled_ratios) == 2 + assert sampled_ratios[0] != pytest.approx(0.4) + assert sampled_ratios[1] == pytest.approx(0.4) + + +def test_coordinated_pick_keeps_axis_aligned_partition_for_rotated_object() -> None: + """A rotated tray retains a geometry-only end-grasp partition fallback.""" + vertices = torch.tensor( + [ + [-0.2, -0.1, -0.02], + [-0.2, -0.1, 0.02], + [-0.2, 0.1, -0.02], + [-0.2, 0.1, 0.02], + [0.2, -0.1, -0.02], + [0.2, -0.1, 0.02], + [0.2, 0.1, -0.02], + [0.2, 0.1, 0.02], + ], + dtype=torch.float32, + ) + angle = torch.tensor(torch.pi / 6.0) + pose = torch.eye(4, dtype=torch.float32).unsqueeze(0) + pose[0, 0, 0] = torch.cos(angle) + pose[0, 0, 1] = -torch.sin(angle) + pose[0, 1, 0] = torch.sin(angle) + pose[0, 1, 1] = torch.cos(angle) + + ratios = CoordinatedPickment._candidate_middle_empty_ratios( + AntipodalAffordance( + mesh_vertices=vertices, + mesh_triangles=torch.tensor( + [[0, 1, 2], [1, 2, 3], [4, 5, 6], [5, 6, 7]], + dtype=torch.long, + ), + ), + pose, + torch.tensor([1.0, 0.0, 0.0]), + base_ratio=0.4, + ) + + assert len(ratios) == 5 + assert ratios[0] < ratios[1] + assert ratios[1] == pytest.approx(0.6) + assert ratios[2] == pytest.approx(0.4) + + +def test_coordinated_pick_can_release_both_hands_and_retreat() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + release=True, + release_steps=4, + retreat_steps=5, + object_motion_keyframes=3, + ), + ), + ) + _stub_dual_arm_grasp_poses(action) + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + label="shared-tray", + entity_id="tray", + ) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=semantics, + object_target_pose=torch.eye(4), + object_initial_pose=torch.eye(4), + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=50), + ) + context = _dual_context() + + plan = _plan_action(action, invocation, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) + + assert plan.plan_success.tolist() == [True, True] + assert plan.commands.frame_count == 50 + assert tuple(segment.name for segment in plan.segments) == ( + "approach", + "close", + "lift", + "move", + "hold", + "release", + "retreat", + ) + assert torch.all(_joint_trajectory(plan).positions[:, -1, DUAL_ARM_DOF:] == 0.0) + assert projected.get_held_object("left_arm") is None + assert projected.get_held_object("right_arm") is None + + def test_assemble_place_uses_explicit_base_snapshot() -> None: generator = _motion_generator() action = _bind_action(generator, Place()) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 6c5ad2352..4541739db 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -716,6 +716,87 @@ def test_scene_entity_pose_is_resolved_late_from_snapshot() -> None: assert collect_scene_dependencies(EndEffectorPoseGoal(reference)) == ("cup",) +def test_scene_entity_pose_applies_world_displacement_after_local_transform() -> None: + entity_pose = torch.eye(4).repeat(2, 1, 1) + entity_pose[:, :3, :3] = torch.tensor( + [ + [0.0, -1.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + ] + ) + local_offset = torch.eye(4) + local_offset[0, 3] = 0.2 + world_displacement = torch.tensor([-0.3, 0.0, 0.1]) + reference = SceneEntityPose( + "cup", + relative_pose=local_offset, + world_displacement=world_displacement, + ) + world_displacement.fill_(9.0) + context = _context( + SceneSnapshot( + timestamp=1.0, + version=3, + entities={"cup": EntityState(entity_pose, confidence=0.9)}, + ) + ) + + resolved = resolve_pose_goal(reference, context, name="xpos") + + torch.testing.assert_close( + resolved[:, :3, 3], + torch.tensor([[-0.3, 0.2, 0.1], [-0.3, 0.2, 0.1]]), + ) + snapshot = reference.snapshot() + assert snapshot.world_displacement is not reference.world_displacement + torch.testing.assert_close( + snapshot.world_displacement, + torch.tensor([-0.3, 0.0, 0.1]), + ) + + +def test_scene_entity_pose_applies_world_orientation_before_local_transform() -> None: + entity_pose = torch.eye(4).repeat(2, 1, 1) + entity_pose[:, :3, :3] = torch.tensor( + [ + [0.0, -1.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + ] + ) + entity_pose[:, :3, 3] = torch.tensor([[0.5, 0.2, 0.0], [0.7, 0.4, 0.0]]) + object_to_eef = torch.eye(4) + object_to_eef[2, 3] = 0.2 + world_orientation = torch.eye(3) + reference = SceneEntityPose( + "tray", + relative_pose=object_to_eef, + world_orientation=world_orientation, + world_displacement=torch.tensor([0.1, 0.0, 0.0]), + ) + world_orientation.fill_(9.0) + context = _context( + SceneSnapshot( + timestamp=1.0, + version=3, + entities={"tray": EntityState(entity_pose, confidence=0.9)}, + ) + ) + + resolved = resolve_pose_goal(reference, context, name="xpos") + + torch.testing.assert_close(resolved[:, :3, :3], torch.eye(3).repeat(2, 1, 1)) + torch.testing.assert_close( + resolved[:, :3, 3], + torch.tensor([[0.6, 0.2, 0.2], [0.8, 0.4, 0.2]]), + ) + assert collect_scene_dependencies(EndEffectorPoseGoal(reference)) == ("tray",) + snapshot = reference.snapshot() + assert snapshot.world_orientation is not reference.world_orientation + torch.testing.assert_close(snapshot.world_orientation, torch.eye(3)) + + def test_scene_entity_pose_enforces_confidence() -> None: context = _context( SceneSnapshot( diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 238a9fbb2..d4910fe1a 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -186,6 +186,24 @@ def _plan( ) +class FailedPhaseGateAction(PhaseGateAction): + """Fully failed planning attempt that never reaches a gated segment.""" + + skill_id: ClassVar[str] = "failed_phase_gate" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + return self.failed_plan( + request, + context, + message="No feasible grasp was found.", + ) + + class EffectAction(DynamicAction): """Dynamic test action that declares an attachment effect.""" @@ -846,13 +864,14 @@ def _destination_invocation( def _phase_gate_invocation( engine: AtomicActionEngine, *, + skill_id: str = PhaseGateAction.skill_id, segment_name: str = "commit", max_action_retries: int = 2, ) -> ActionInvocation[EndEffectorPoseGoal]: """Build a test invocation whose core owns one named segment gate.""" base = _invocation( engine, - skill_id=PhaseGateAction.skill_id, + skill_id=skill_id, max_action_retries=max_action_retries, ) return replace( @@ -950,6 +969,28 @@ def test_phase_effect_gate_requires_a_noninitial_named_segment( ) +def test_fully_failed_plan_preserves_diagnostics_before_phase_gate_validation() -> None: + engine, _ = _engine() + action = FailedPhaseGateAction() + engine.register(action) + + session = engine.start( + ( + _phase_gate_invocation( + engine, + skill_id=FailedPhaseGateAction.skill_id, + ), + ), + _context(0.0, 0.0, 0.2, 0), + ) + + assert not session.active_plan.plan_success.any() + assert session.active_plan.diagnostics.failure == PlanningFailure( + "planning_failed", + retryable=True, + ) + + def test_unresolved_phase_effect_gate_replays_preceding_command_for_full_cohort() -> ( None ): diff --git a/tests/sim/motion/solvers/test_pytorch_solver.py b/tests/sim/motion/solvers/test_pytorch_solver.py index be720e7ce..a2ca60ad8 100644 --- a/tests/sim/motion/solvers/test_pytorch_solver.py +++ b/tests/sim/motion/solvers/test_pytorch_solver.py @@ -24,6 +24,7 @@ from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.cfg import RobotCfg, RenderCfg +from embodichain.lab.sim.motion.solvers import PytorchSolver from embodichain.data import get_data_path from embodichain.utils.utility import reset_all_seeds @@ -67,6 +68,46 @@ def grid_sample_qpos_from_limits( return stacked +def test_get_ik_uses_true_inverse_for_rotated_tcp() -> None: + """Preserve the requested TCP pose when lowering it to the end link.""" + solver = object.__new__(PytorchSolver) + solver.device = torch.device("cpu") + solver.dof = 2 + solver._num_samples = 1 + solver.lower_qpos_limits = torch.full((2,), -1.0) + solver.upper_qpos_limits = torch.full((2,), 1.0) + solver.ik_nearest_weight = torch.ones(2) + solver.tcp_xpos = np.array( + [ + [0.0, -1.0, 0.0, 0.1], + [1.0, 0.0, 0.0, 0.2], + [0.0, 0.0, 1.0, 0.3], + [0.0, 0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + received: list[torch.Tensor] = [] + + def solve( + target_pose: torch.Tensor, + joint_seed: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + del joint_seed + received.append(target_pose) + return torch.ones(1, dtype=torch.bool), torch.zeros((1, solver.dof)) + + solver._compute_inverse_kinematics = solve + target = torch.eye(4).unsqueeze(0) + + success, _ = solver.get_ik(target, qpos_seed=torch.zeros(solver.dof)) + + assert success.tolist() == [True] + assert torch.allclose( + received[0], + target @ torch.linalg.inv(torch.as_tensor(solver.tcp_xpos)), + ) + + # Base test class for CPU and CUDA class BaseSolverTest: sim = None # Define as a class attribute diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index 9b379fa28..96e3acbf1 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -16,11 +16,14 @@ from __future__ import annotations +from types import SimpleNamespace + import dexsim import pytest from dexsim.types import DenoiserType, Renderer, ToneMappingType +from embodichain.lab.sim import cfg as cfg_module from embodichain.lab.sim.cfg import ( ArticulationCfg, DLSSCfg, @@ -138,6 +141,67 @@ def test_render_cfg_applies_renderer_and_sample_count() -> None: assert world_config.raytrace_config.render_iterations_per_frame == expected_spp +@pytest.mark.parametrize("has_dlss_api", [False, True]) +@pytest.mark.parametrize("enabled", [False, True]) +@pytest.mark.parametrize("renderer", ["hybrid", "fast-rt", "rt"]) +def test_render_cfg_supports_legacy_and_dlss_engines( + monkeypatch: pytest.MonkeyPatch, + has_dlss_api: bool, + enabled: bool, + renderer: str, +) -> None: + """Missing native DLSS support must not discard ordinary render settings.""" + if has_dlss_api: + monkeypatch.setattr(dexsim, "DLSSConfig", SimpleNamespace, raising=False) + else: + monkeypatch.delattr(dexsim, "DLSSConfig", raising=False) + warnings: list[str] = [] + monkeypatch.setattr(cfg_module.logger, "log_warning", warnings.append) + world = SimpleNamespace( + win_config=SimpleNamespace(width=640, height=360), + raytrace_config=SimpleNamespace(), + postprocess_config=SimpleNamespace(), + ) + config = RenderCfg( + renderer=renderer, + spp=4, + tone_mapping_enabled=True, + tone_mapping_exposure=1.25, + dlss=DLSSCfg(dlss_enabled=enabled, offscreen_dlss_enabled=enabled), + ) + + config.apply_to_dexsim_config(world) + + assert world.renderer == config.to_dexsim_flags() + assert world.raytrace_config.render_iterations_per_frame == config.spp + assert world.raytrace_config.denoiser_type == DenoiserType.OPTIX + assert world.raytrace_config.open_denoise is True + assert world.postprocess_config.tone_mapping_enabled is True + assert ( + world.postprocess_config.tone_mapping_exposure == config.tone_mapping_exposure + ) + if has_dlss_api: + assert world.dlss_config.dlss_enabled is enabled + assert world.dlss_config.offscreen_dlss_enabled is enabled + assert warnings == [] + else: + assert not hasattr(world, "dlss_config") + assert len(warnings) == 1 + assert "DLSSConfig" in warnings[0] + + +def test_render_cfg_revalidates_dlss_on_legacy_engine( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """The legacy fallback must not silently accept malformed mutable settings.""" + monkeypatch.delattr(dexsim, "DLSSConfig", raising=False) + config = RenderCfg(renderer="fast-rt") + config.dlss.dlss_enabled = "false" + + with pytest.raises(ValueError, match="DLSSCfg.dlss_enabled"): + config.apply_to_dexsim_config(dexsim.WorldConfig()) + + @pytest.mark.parametrize( ("field_name", "invalid_value"), [ diff --git a/tests/toolkits/test_batch_convex_collision.py b/tests/toolkits/test_batch_convex_collision.py index 5e6255f4a..3a70b6778 100644 --- a/tests/toolkits/test_batch_convex_collision.py +++ b/tests/toolkits/test_batch_convex_collision.py @@ -30,6 +30,8 @@ pytestmark = pytest.mark.gpu +_EXPECTED_VHACD_MAX_SURFACE_DISTANCE = 0.5945 + def batch_convex_collision_query(device=torch.device("cuda")): mug_path = get_data_path("ScannedBottle/moliwulong_processed.ply") @@ -73,7 +75,10 @@ def batch_convex_collision_query(device=torch.device("cuda")): is_pose_collide = is_point_collide.any(dim=1) pose_surface_distance = point_surface_distance.min(dim=1).values assert is_pose_collide.sum().item() == 1 - assert abs(pose_surface_distance.max().item() - 0.8492) < 1e-2 + assert ( + abs(pose_surface_distance.max().item() - _EXPECTED_VHACD_MAX_SURFACE_DISTANCE) + < 1e-2 + ) def test_batch_convex_collision_cpu(): diff --git a/tests/toolkits/test_convex_collision_checker.py b/tests/toolkits/test_convex_collision_checker.py new file mode 100644 index 000000000..f0b0f3e3a --- /dev/null +++ b/tests/toolkits/test_convex_collision_checker.py @@ -0,0 +1,128 @@ +# ---------------------------------------------------------------------------- +# 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 hashlib +from pathlib import Path + +import numpy as np +import pytest +import torch + +from embodichain.toolkits.graspkit.pg_grasp import collision_checker as module +from embodichain.toolkits.graspkit.pg_grasp.collision_checker import ( + ConvexCollisionChecker, +) + + +def _tetrahedron() -> tuple[np.ndarray, np.ndarray]: + vertices = np.array( + [ + [0.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + faces = np.array( + [ + [0, 2, 1], + [0, 1, 3], + [0, 3, 2], + [1, 2, 3], + ], + dtype=np.int32, + ) + return vertices, faces + + +def test_plane_equations_use_vhacd(monkeypatch: pytest.MonkeyPatch) -> None: + vertices, faces = _tetrahedron() + calls: list[int] = [] + + def fake_vhacd(mesh, *, max_convex_hull_num: int): + calls.append(max_convex_hull_num) + return True, (mesh,) + + monkeypatch.setattr(module, "convex_decomposition_vhacd", fake_vhacd) + + plane_equations = ConvexCollisionChecker._compute_plane_equations( + vertices, + faces, + max_decomposition_hulls=16, + ) + + assert calls == [16] + assert len(plane_equations) == 1 + + +def test_vhacd_failure_is_reported(monkeypatch: pytest.MonkeyPatch) -> None: + vertices, faces = _tetrahedron() + + monkeypatch.setattr( + module, + "convex_decomposition_vhacd", + lambda *_args, **_kwargs: (False, ()), + ) + + with pytest.raises(RuntimeError, match="V-HACD convex decomposition failed"): + ConvexCollisionChecker._compute_plane_equations( + vertices, + faces, + max_decomposition_hulls=16, + ) + + +def test_vhacd_cache_does_not_reuse_legacy_backend( + monkeypatch: pytest.MonkeyPatch, + tmp_path: Path, +) -> None: + vertices, faces = _tetrahedron() + mesh_hash = hashlib.md5(vertices.tobytes() + faces.tobytes()).hexdigest() + legacy_path = tmp_path / f"{mesh_hash}_16.pkl" + legacy_path.write_bytes(b"legacy CoACD cache") + calls: list[int] = [] + + def fake_plane_equations( + _vertices: np.ndarray, + _faces: np.ndarray, + max_decomposition_hulls: int, + ) -> list[tuple[np.ndarray, np.ndarray]]: + calls.append(max_decomposition_hulls) + return [ + ( + np.array([[1.0, 0.0, 0.0]], dtype=np.float32), + np.array([0.0], dtype=np.float32), + ) + ] + + monkeypatch.setattr(module, "CONVEX_DECOMPOSITION_CACHE_DIR", tmp_path) + monkeypatch.setattr( + ConvexCollisionChecker, + "_compute_plane_equations", + staticmethod(fake_plane_equations), + ) + + checker = ConvexCollisionChecker( + torch.from_numpy(vertices), + torch.from_numpy(faces), + max_decomposition_hulls=16, + ) + + assert calls == [16] + assert checker.cache_path == str(tmp_path / f"{mesh_hash}_16_vhacd_v1.pkl")