diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index aa2726ee4..87a7ec9ee 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -4,9 +4,11 @@ branches. A real CUDA/cuRobo dynamic-obstacle recovery gate is landed and runs conditionally when cuRobo is installed, CUDA is available, and GPU/slow tests are explicitly enabled. Open Drawer has completed its - supported-simulation physical run; repeated cube pick/place has completed one - Pick/Place/settle/validator cycle, while the full three-cycle run remains in - threshold calibration. Dual-UR5/PGI HandOver has completed three consecutive + supported-simulation physical run; repeated cube pick/place now completes all + three independently observed Pick/Place/settle/validator cycles. A physical + gripper-command fault gate also proves held-object loss, symbolic invalidation, + real re-acquisition, and retry without simulator-side repair. Dual-UR5/PGI + HandOver has completed three consecutive supported-simulation Pick/transfer/settle/validator runs using contact dynamics only. Named trajectory-segment effect gates now block Pick lift, Place retract, and HandOver source release until fresh physical evidence @@ -1232,9 +1234,11 @@ it only when GPU and slow tests are explicitly selected. Implementation status: the semantic facade, provider-free linking, canonical compiler, bounded program preflight, and cross-segment sequential look-ahead are -implemented. Relation placement remains an exact typed integration capability; -a reusable production support-surface/container affordance and grounder are -follow-up work rather than inferred behavior. +implemented. Relation placement remains an exact typed integration capability. +Production support-surface and container bindings now declare a desired object +target frame relative to an object, articulation, or link parent; registration +installs their exact versioned grounders automatically. No geometric target is +guessed from a name, mesh, or bounding box. Deliverables: @@ -1260,16 +1264,19 @@ effects, row-local composite hysteresis kernel, canonical `SkillRuntime`, and production simulation evidence ports are wired end to end. Segment-scoped held-object guard requests, live evidence collection, row-local symbolic invalidation, bounded Pick retry, and typed external-recovery hand-off are also -implemented. Physical simulation acceptance is partial: Open Drawer and one -cube Pick/Place/settle/validator cycle have completed. The embodiment-owned +implemented. Physical simulation acceptance covers Open Drawer and all three +cube Pick/Place/settle/validator cycles. The embodiment-owned dual-UR5/PGI HandOver slice now completes Pick, transfer, terminal physical-effect verification, settling, and target validation through real contact dynamics. Per-expectation terminal outcomes, core-owned failure invalidation, row-local retry/recovery decisions, fail-closed deadline reconciliation, and blocking named-segment effect gates are implemented. Workflow-level re-acquisition is implemented through the preset-owned bounded -policy and canonical runtime. Real-simulation fault-injection coverage and the -full repeated-cube run remain validation work. +policy and canonical runtime. A supported-simulation fault gate now replaces a +bounded window of outgoing gripper commands with a real open command during +Place, observes pose-relation contradiction and symbolic invalidation, performs +a real Pick, retries Place, and completes the remaining program. The wrapper +never writes object pose, velocity, attachment, constraint, or task state. Deliverables: @@ -1314,9 +1321,9 @@ strict decoder/loader, lazy compiler, environment/CLI integration, shared simulation factory, and three-segment cube program are implemented. The task combines declarative program configuration with typed scene/profile integration declarations and installs the shared adapter without overriding task motion -generation. A supported-simulation run has completed the first physical -Pick/Place/settle/validator cycle; completing all three cycles remains an -acceptance item while thresholds are calibrated. +generation. The UR5 embodiment preset uses 100 motion samples while retaining +the 0.08-rad tracking gate and bounded replanning. A supported-simulation run +now completes all three physical Pick/Place/settle/validator cycles. Deliverables: @@ -1361,13 +1368,16 @@ trajectories in task code. ### Phase 7: parallel execution and PourWater -Implementation status: the schema/runtime contracts and fail-closed safety -boundary are implemented. Schema +Implementation status: the schema/runtime contracts, fail-closed safety +boundary, and production simulation validator are implemented. Schema version 2 provides explicit parallel branches and barriers; static resource conflict analysis, shared-clock lane coordination, deterministic hold padding, transport/safety validation, row-local failure and cancellation, timeouts, and -deterministic state merge are covered by tests. A production simulation safety -validator and parallel physical integration remain pending. The PourWater task +deterministic state merge are covered by tests. The cuRobo validator assembles +the exact aggregate joint segment, densifies it under a configured maximum joint +step, and checks every sample against joint bounds, self collision, and the +registry-backed live world without replanning or replacing the command. A +parallel physical integration remains pending. The PourWater task migration is outside the current scope because it would require modifying Action Bank code. @@ -1516,22 +1526,22 @@ The design is complete when all of the following hold: - [x] Automatic grasping tracks target revisions and receives downstream object goals without caller duplication. - [x] `Place` is object-centric and consumes verified held-object state. -- [ ] Built-in grasp, release, handover, and supported articulation effect - monitors work in simulation. The dual-UR5/PGI HandOver vertical slice is - physically validated; remaining skill/embodiment coverage keeps this - aggregate item open. -- [ ] Grasp and handover simulation gates retain objects through configured +- [x] Built-in grasp, release, handover, and supported articulation effect + monitors work in simulation across the repeated cube, dual-UR5/PGI + HandOver, and Open Drawer vertical slices. +- [x] Grasp and handover simulation gates retain objects through configured drive/contact dynamics only; no monitor or runtime path creates a synthetic attachment, freezes the object, or overrides its pose. -- [ ] Physical held-object loss is observed as effect failure, invalidates the +- [x] Physical held-object loss is observed as effect failure, invalidates the affected symbolic relation, and exercises bounded recovery rather than being hidden by a simulator-side attachment. The segment-aware observation, row-local core-owned invalidation, per-expectation terminal reconciliation, fail-closed deadline handling, bounded Pick/retained-Place retry, typed recovery boundary, and blocking acquisition/release gates are implemented. Preset-owned per-row workflow re-acquisition now performs - real `Pick` and semantic-call retries; real-simulation fault injection - remains open. + real `Pick` and semantic-call retries. The supported-simulation gate opens + the real gripper during Place, observes the loss, re-acquires the fallen + cube, and completes the retried call. - [x] Repeated sub-threshold motion eventually publishes the correct scene revision. - [x] Custom actions have a documented and tested intentional hard-break @@ -1543,7 +1553,7 @@ The design is complete when all of the following hold: through `env.step()`. - [x] No program post-policy, effect, or tracing integration depends on hard-coded waypoint indices. -- [ ] Repeated cube pick/place completes at least three lazy, independently +- [x] Repeated cube pick/place completes at least three lazy, independently observed program/demo segments with settle/effect/validation metadata. - [x] Version 1 uses one shared program/call barrier while per-environment task state, effects, recovery, eligibility, success, and failure remain diff --git a/docs/design/expert_program_rollout_report.md b/docs/design/expert_program_rollout_report.md index 6c8228748..175e74e53 100644 --- a/docs/design/expert_program_rollout_report.md +++ b/docs/design/expert_program_rollout_report.md @@ -13,25 +13,26 @@ This is a deterministic, static Phase 8 snapshot of checked-in framework and int | OperateArticulation | framework-tested | per-embodiment integration | Typed articulation goals and execution contracts are covered. | | Articulation effect | framework-tested | per-embodiment integration | Joint-state terminal effect validation is covered. | | V1 sequential | framework-tested | per-task integration | Ordered call execution and failure propagation are covered. | -| HandOver | framework-tested | integration-required | No landed task integration is claimed by this report. | -| Place relation (on/inside) | framework-tested | integration-required | Embodiment frames and relation validators must be supplied. | +| HandOver | framework-tested | per-embodiment integration | Coordinated effects and bounded recovery are covered. | +| Place relation (on/inside) | framework-tested | per-scene integration | Standard support/container target-frame bindings install exact grounders. | | Registered call | framework-tested | integration-required | Production registration must declare and validate its concrete contract. | -| V2 parallel | framework-tested | integration-required | Fail-closed by default; production use requires an authoritative validator. | +| V2 parallel | framework-tested | integration-required | Joint/cuRobo validation is available; physical parallel acceptance remains. | Parallel execution remains fail-closed by default. Resource declarations alone do not authorize production concurrency; the selected embodiment must provide an authoritative validator. ## Checked-in Integration Matrix -Only the two checked-in vertical slices below are classified as integration/production code. Physical acceptance is tracked separately. +Only the checked-in vertical slices below are classified as integration/production code. Physical acceptance is tracked separately. | Embodiment | Task | Skill contract | Terminal effect | Program schema | Code status | Physical acceptance | | --- | --- | --- | --- | --- | --- | --- | -| UR5 | Cube Pick + Place | Pick + Place(at) | attach/release | V1 sequential | checked in | pending: one cycle passed; full three-cycle gate remains | +| UR5 | Cube Pick + Place | Pick + Place(at) | attach/release | V1 sequential | checked in | fixed-seed three-cycle and physical-loss recovery slow gates | | CobotMagic | Open Drawer | OperateArticulation | articulation effect | V1 sequential | checked in | fixed-seed supported-simulation slow gate; not release-required | +| Dual UR5 + PGI | HandOver | Pick + HandOver | attach/transfer | V1 sequential | checked in | three consecutive supported-simulation contact-dynamics runs | -HandOver, Place relations (`on`/`inside`), Registered calls, and V2 parallel are framework-tested but integration-required. They are intentionally not listed as checked-in integrations. +Place-relation bindings are reusable scene integration rather than a task vertical slice. Registered calls and V2 parallel remain integration-required; physical parallel acceptance is still open. -Both checked-in environment classes have zero task-local motion or demo-generation overrides; `test_task_classes_do_not_override_motion_or_demo_generation` keeps that structural metric at zero. +The checked-in environment classes have zero task-local motion or demo-generation overrides; `test_task_classes_do_not_override_motion_or_demo_generation` keeps that structural metric at zero. ## Migration Size Snapshot @@ -43,15 +44,15 @@ Counting rule: `lines` is the number of raw LF (`0x0A`) bytes; `bytes` is the ra | Task | Baseline lines | Current lines | Line delta | Baseline bytes | Current bytes | Byte delta | Current source files | | --- | --- | --- | --- | --- | --- | --- | --- | -| Cube | 598 | 366 | -232 (-38.8%) | 23912 | 12448 | -11464 (-47.9%) | `embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py`
`embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml` | -| Drawer | 245 | 246 | +1 (+0.4%) | 8833 | 8391 | -442 (-5.0%) | `embodichain_tasks/embodichain_tasks/tableware/open_drawer.py`
`embodichain_tasks/configs/expert_program/tableware/open_drawer.json` | -| Total | 843 | 612 | -231 (-27.4%) | 32745 | 20839 | -11906 (-36.4%) | the four files above | +| Cube | 598 | 395 | -203 (-33.9%) | 23912 | 13272 | -10640 (-44.5%) | `embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py`
`embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml` | +| Drawer | 245 | 265 | +20 (+8.2%) | 8833 | 8916 | +83 (+0.9%) | `embodichain_tasks/embodichain_tasks/tableware/open_drawer.py`
`embodichain_tasks/configs/expert_program/tableware/open_drawer.json` | +| Total | 843 | 660 | -183 (-21.7%) | 32745 | 22188 | -10557 (-32.2%) | the four files above | ## Demo Success Measurement `scripts/benchmark/expert_program/demo_success.py` executes each fixed seed exactly once, always discards the episode buffer, and counts executor exceptions as failed rows. It writes raw JSON plus a three-table Markdown report. Its CLI supports offline raw-JSON re-aggregation and an explicit `--run-simulation` mode that constructs one standard Gym environment from Gym and Expert Program configurations. -No success-rate result or release gate is checked in yet. Open Drawer has a single real-simulation smoke pass, while repeated Cube still needs the tracking-threshold decision and three-cycle physical acceptance before a fixed-seed rate is meaningful. +No multi-seed success-rate or release gate is checked in yet. Open Drawer has a real-simulation smoke pass; repeated Cube has a fixed-seed three-cycle pass plus physical-loss/re-acquisition gate; and HandOver has three consecutive contact-dynamics runs. ## Drift Check diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst index f94f74cd8..662abd1ac 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst @@ -16,6 +16,8 @@ embodichain.lab.gym.envs.expert_program ExpertProgramEnvironmentMixin ExpertProgramEnvironmentAdapter SimulationSceneBinding + SupportSurfaceAffordanceBinding + ContainerAffordanceBinding SimulationResourceEndpointBinding SimulationRobotResourceBinding RobotResourceBinding @@ -25,6 +27,7 @@ embodichain.lab.gym.envs.expert_program SimulationExpertProgramFactory SimulationSegmentPolicyPort ControlCommandStateEvidenceTracker + CuroboParallelSafetyValidatorFactory .. currentmodule:: embodichain.lab.gym.envs.expert_program @@ -80,6 +83,12 @@ Simulation integration .. autoclass:: SimulationSceneBinding :members: +.. autoclass:: SupportSurfaceAffordanceBinding + :members: + +.. autoclass:: ContainerAffordanceBinding + :members: + .. autoclass:: SimulationResourceEndpointBinding .. autoclass:: SimulationRobotResourceBinding @@ -104,3 +113,6 @@ Simulation integration .. autoclass:: ControlCommandStateEvidenceTracker :members: + +.. autoclass:: CuroboParallelSafetyValidatorFactory + :members: diff --git a/docs/source/overview/sim/atomic_actions/expert_programs.md b/docs/source/overview/sim/atomic_actions/expert_programs.md index 3ba563d83..95275883a 100644 --- a/docs/source/overview/sim/atomic_actions/expert_programs.md +++ b/docs/source/overview/sim/atomic_actions/expert_programs.md @@ -176,6 +176,16 @@ requires the profile-selected `HandOverPoseProvider`. A direct `Place(at=...)` does not require a relation grounder. Missing, ambiguous, or stale providers fail during provider-aware program preflight, before the first physical action. +The standard simulation registration supplies the common relation path without +task-owned grounder code. Declare `SupportSurfaceAffordanceBinding` or +`ContainerAffordanceBinding` in `SimulationSceneBinding`; its +`object_target_pose` is the desired object pose relative to the explicitly named +object, articulation, or link parent. Mark one capability-scoped target per +parent as `is_default=True` when calls should reference only that parent. The +registration installs the matching exact grounder automatically and resolves +the target from a fresh scene snapshot. It never infers a placement frame from +an entity name, mesh, or bounding box. + The same preflight rejects a reachable `safe` preset for a dynamic scene before the first observation when the active motion generator cannot provide the required dynamic collision world. @@ -229,6 +239,16 @@ Version 2 also uses strict symbolic key-level conflict detection at the barrier: two branches may not commit the same task-state key, even when their physical changes occurred in disjoint environment rows. +Joint-position integrations using the active cuRobo planner can declare +`CuroboParallelSafetyValidatorFactory(validation_control_part="dual_arm")`. +The selected aggregate control part must contain every joint commanded by any +lane. Before dispatch, the validator combines the exact merged target with the +current measured joint state, interpolates the segment under `max_joint_step`, +and asks cuRobo to check every supplied sample against joint bounds, +self-collision, and the registry-backed static/dynamic world. It fails closed +when a joint is uncovered or the configured sample cap would under-sample the +segment. This gate does not replan or replace the trajectory. + ## Python semantic calls Standalone applications can use the same compiler and runtime through @@ -242,8 +262,9 @@ result = skills.run(Pick(object=cube), Place(object=cube, on=tray)) ``` In this example, `runtime_provider` owns the typed relation grounder for the -tray's placement affordance. Applications without such a provider can use a -direct `SemanticPose` through `Place(at=...)`. +tray's placement affordance. A standard simulation registration obtains it from +the support/container binding above. Applications without such a provider can +use a direct `SemanticPose` through `Place(at=...)`. `from_env` requires an explicit `SkillRuntimeProvider`; it never scans arbitrary environment attributes. Gym demonstration environments intentionally use the @@ -261,18 +282,20 @@ robot resource and endpoint declarations, see {doc}`robot_skill_profiles`. | --- | --- | --- | | `Pick` | Compiler, runtime, effect verification | Antipodal grasp binding plus motion/grasp resources | | `Place(at=...)` | Object-centric lowering with verified held state | Direct semantic pose target | -| `Place(on=...)` / `Place(inside=...)` | Exact typed relation dispatch | Integration must install the matching `RelationTargetGrounder` | +| `Place(on=...)` / `Place(inside=...)` | Exact typed relation dispatch | Support/container bindings install standard target-frame grounders; custom payloads install an exact `RelationTargetGrounder` | | `HandOver` | Coordinated call, state flow, and effect contract | Embodiment must install its named `HandOverPoseProvider` and evidence sources | | `OperateArticulation` | Named/absolute/displacement target and joint effect | Link, joint, operation-affordance, and interaction endpoint bindings | | Registered calls | Typed call catalog and explicit lowerer | Physical extensions must add an explicit effect contract | | Mobile/whole-body extensions | Generic resources, claims, endpoint targets, command frames, and routing | Requires a reusable semantic skill/lowerer plus matching adapter, payload, transport, and effect integration; no curated navigation or whole-body skill is installed today | -| Parallel blocks | Shared-clock coordinator and strict barrier merge | Requires an authoritative `ParallelCommandSafetyValidator`; none is inferred by default | +| Parallel blocks | Shared-clock coordinator and strict barrier merge | Joint-position/cuRobo tasks may declare the aggregate collision validator; other transports require an authoritative validator | The table separates implemented reusable contracts from embodiment-specific providers. It is not a claim that every row has completed task-level physical simulation acceptance. The Open Drawer vertical slice has completed its supported-simulation physical -run and reached the configured drawer joint target. Repeated cube pick/place has -completed one physical Pick/Place/settle/validator cycle; its full three-cycle -run remains in threshold calibration. +run and reached the configured drawer joint target. Repeated cube pick/place +completes three independently observed physical Pick/Place/settle/validator +cycles. Its physical-fault gate opens the commanded gripper during Place, +observes held-object loss, performs a real re-acquisition Pick, retries Place, +and completes without simulator-side repair. diff --git a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py index 1a5b7d4cf..4ea67de33 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -53,6 +53,7 @@ CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY, GRASP_CAPABILITY, + MotionPolicy, PickUpOptions, PlaceOptions, RecoveryPolicy, @@ -303,6 +304,7 @@ def create_cube_robot_profile_binding() -> SimulationRobotSkillProfileBinding: "pick": PickUpOptions(), "place": PlaceOptions(), }, + motion_policy=MotionPolicy(sample_count=100), recovery_policy=RecoveryPolicy(), workflow_recovery_policy=WorkflowRecoveryPolicy( max_recovery_attempts=2, diff --git a/scripts/tools/expert_program_rollout_report.py b/scripts/tools/expert_program_rollout_report.py index eeff71a75..ee0bf2b42 100644 --- a/scripts/tools/expert_program_rollout_report.py +++ b/scripts/tools/expert_program_rollout_report.py @@ -150,14 +150,14 @@ class _TaskSizeSpec: ( "HandOver", "framework-tested", - "integration-required", - "No landed task integration is claimed by this report.", + "per-embodiment integration", + "Coordinated effects and bounded recovery are covered.", ), ( "Place relation (on/inside)", "framework-tested", - "integration-required", - "Embodiment frames and relation validators must be supplied.", + "per-scene integration", + "Standard support/container target-frame bindings install exact grounders.", ), ( "Registered call", @@ -169,7 +169,7 @@ class _TaskSizeSpec: "V2 parallel", "framework-tested", "integration-required", - "Fail-closed by default; production use requires an authoritative validator.", + "Joint/cuRobo validation is available; physical parallel acceptance remains.", ), ) @@ -182,7 +182,7 @@ class _TaskSizeSpec: "attach/release", "V1 sequential", "checked in", - "pending: one cycle passed; full three-cycle gate remains", + "fixed-seed three-cycle and physical-loss recovery slow gates", ), ( "CobotMagic", @@ -193,6 +193,15 @@ class _TaskSizeSpec: "checked in", "fixed-seed supported-simulation slow gate; not release-required", ), + ( + "Dual UR5 + PGI", + "HandOver", + "Pick + HandOver", + "attach/transfer", + "V1 sequential", + "checked in", + "three consecutive supported-simulation contact-dynamics runs", + ), ) @@ -329,7 +338,7 @@ def render_report(metrics: Sequence[TaskSizeMetric]) -> str: "## Checked-in Integration Matrix", "", ( - "Only the two checked-in vertical slices below are classified as " + "Only the checked-in vertical slices below are classified as " "integration/production code. Physical acceptance is tracked " "separately." ), @@ -354,13 +363,13 @@ def render_report(metrics: Sequence[TaskSizeMetric]) -> str: [ "", ( - "HandOver, Place relations (`on`/`inside`), Registered calls, and V2 " - "parallel are framework-tested but integration-required. They are " - "intentionally not listed as checked-in integrations." + "Place-relation bindings are reusable scene integration rather than " + "a task vertical slice. Registered calls and V2 parallel remain " + "integration-required; physical parallel acceptance is still open." ), "", ( - "Both checked-in environment classes have zero task-local motion or " + "The checked-in environment classes have zero task-local motion or " "demo-generation overrides; " "`test_task_classes_do_not_override_motion_or_demo_generation` " "keeps that structural metric at zero." @@ -422,10 +431,10 @@ def render_report(metrics: Sequence[TaskSizeMetric]) -> str: ), "", ( - "No success-rate result or release gate is checked in yet. Open " - "Drawer has a single real-simulation smoke pass, while repeated Cube " - "still needs the tracking-threshold decision and three-cycle physical " - "acceptance before a fixed-seed rate is meaningful." + "No multi-seed success-rate or release gate is checked in yet. Open " + "Drawer has a real-simulation smoke pass; repeated Cube has a " + "fixed-seed three-cycle pass plus physical-loss/re-acquisition gate; " + "and HandOver has three consecutive contact-dynamics runs." ), "", "## Drift Check", diff --git a/tests/benchmark/expert_program/test_cube_physical_recovery_sim.py b/tests/benchmark/expert_program/test_cube_physical_recovery_sim.py new file mode 100644 index 000000000..f1b64384a --- /dev/null +++ b/tests/benchmark/expert_program/test_cube_physical_recovery_sim.py @@ -0,0 +1,241 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Live physical held-object loss and workflow-recovery regression coverage.""" + +from __future__ import annotations + +import argparse +from pathlib import Path +import subprocess +import sys +from typing import Any + +import pytest +import torch + +from embodichain.lab.gym.envs.demo import ( + DemoEpisodeResult, + ProcessedEnvAction, + execute_demo_episode, +) +from embodichain_tasks.configs import get_config_path +from scripts.benchmark.expert_program.demo_success import ( + DemoSuccessCase, + _build_parser, + load_raw_trials, + run_gym_demo_success_benchmark, +) + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +_CUBE_GYM_CONFIG = get_config_path("gym/multi_segments/cube_pick_place.json") +_CUBE_EXPERT_PROGRAM = get_config_path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" +) +_FAULT_SEGMENT_INDEX = 1 +_FAULT_CALL_INDEX = 1 +_FAULT_OPEN_STEPS = 20 +_SUBPROCESS_TIMEOUT_SECONDS = 240 + + +class _GripperOpenFaultEnvironment: + """Replace a bounded command window with a real gripper-open command. + + The wrapper changes only the controller-ready action before the ordinary + ``env.step()`` call. It never writes an object pose, velocity, attachment, + constraint, or symbolic task state. + """ + + def __init__(self, environment: Any) -> None: + self._environment = environment + target = getattr(environment, "unwrapped", environment) + self._hand_joint_ids = tuple(target.robot.get_joint_ids(name="hand")) + if not self._hand_joint_ids: + raise ValueError("The cube recovery gate requires hand joint IDs.") + self.injected_open_steps = 0 + + @property + def unwrapped(self) -> Any: + """Return the original task environment for demo lifecycle hooks.""" + return getattr(self._environment, "unwrapped", self._environment) + + def __getattr__(self, name: str) -> Any: + return getattr(self._environment, name) + + def step(self, action: object) -> object: + """Open the physical gripper during the selected Place approach.""" + if isinstance(action, ProcessedEnvAction): + metadata = action.metadata + should_inject = ( + metadata.get("program_segment_index") == _FAULT_SEGMENT_INDEX + and metadata.get("runtime_call_index") == _FAULT_CALL_INDEX + and metadata.get("bridge_action_kind") == "runtime_command" + and self.injected_open_steps < _FAULT_OPEN_STEPS + ) + if should_inject: + if not isinstance(action.value, torch.Tensor): + raise TypeError("The cube task must emit a tensor action.") + value = action.value.clone() + value[:, self._hand_joint_ids] = 0.0 + action = ProcessedEnvAction( + value=value, + metadata={ + **metadata, + "test_fault": "open_gripper_before_place", + }, + ) + self.injected_open_steps += 1 + return self._environment.step(action) + + +def _execute_fault_episode( + environment: Any, + *, + episode_index: int, +) -> DemoEpisodeResult: + """Execute one episode through the controller-command fault wrapper.""" + fault_environment = _GripperOpenFaultEnvironment(environment) + result = execute_demo_episode( + fault_environment, + episode_index=episode_index, + ) + print(f"injected_open_steps={fault_environment.injected_open_steps}") + return result + + +def _run_fault_subprocess(raw_path: Path, report_path: Path) -> int: + """Create and close the native simulator inside the child process.""" + launcher_args = _build_parser().parse_args( + [ + "--run-simulation", + "--gym_config", + str(_CUBE_GYM_CONFIG), + "--expert-program", + str(_CUBE_EXPERT_PROGRAM), + "--case-id", + "cube_physical_loss_recovery", + "--seeds", + "0", + "--raw-json", + str(raw_path), + "--report", + str(report_path), + "--headless", + "--device", + "cuda", + "--num_envs", + "1", + "--filter_dataset_saving", + ] + ) + run_gym_demo_success_benchmark( + DemoSuccessCase("cube_physical_loss_recovery", (0,)), + launcher_args=launcher_args, + expert_program_path=_CUBE_EXPERT_PROGRAM, + raw_json_path=raw_path, + report_path=report_path, + episode_executor=_execute_fault_episode, + ) + return 0 + + +@pytest.mark.requires_sim +@pytest.mark.slow +@pytest.mark.gpu +def test_physical_cube_loss_triggers_real_reacquisition(tmp_path: Path) -> None: + """Observe physical loss, invalidate state, reacquire, and finish the task.""" + raw_path = tmp_path / "fault_raw.json" + report_path = tmp_path / "fault_report.md" + completed = subprocess.run( + [ + sys.executable, + str(Path(__file__).resolve()), + "--run-fault", + "--raw-json", + str(raw_path), + "--report", + str(report_path), + ], + cwd=_REPOSITORY_ROOT, + capture_output=True, + text=True, + timeout=_SUBPROCESS_TIMEOUT_SECONDS, + check=False, + ) + + assert completed.returncode == 0, completed.stdout + completed.stderr + assert f"injected_open_steps={_FAULT_OPEN_STEPS}" in completed.stdout + trial = load_raw_trials(raw_path)[0] + assert trial.rows[0].success + assert trial.rows[0].terminal_reason == "success" + + segments = trial.episode_result["segments"] + assert isinstance(segments, list) + assert len(segments) == 3 + recovery_segment = segments[_FAULT_SEGMENT_INDEX] + runtime = recovery_segment["metadata"]["runtime"] + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + assert runtime["task_state"]["held_objects"] == [] + + failed_place = runtime["calls"][_FAULT_CALL_INDEX] + assert failed_place["semantic_id"] == "place" + assert failed_place["status"] == "failed" + assert failed_place["masks"]["failed"] == [True] + assert {event["kind"] for event in failed_place["events"]} >= { + "held_object_lost", + "recovery_required", + } + physical_failures = [ + effect + for effect in failed_place["effects"] + if effect["boundary"]["kind"] == "in_flight_guard" + and effect["decision"]["failure_mask"] == [True] + ] + assert len(physical_failures) == 1 + physical_failure = physical_failures[0] + assert physical_failure["decision"]["expectations"][0]["contradicted_mask"] == [ + True + ] + assert physical_failure["evidence"]["source.constraint"]["values"] == [True] + assert physical_failure["evidence"]["source.pose"]["valid_mask"] == [True] + + recoveries = runtime["workflow_recoveries"] + assert [recovery["role"] for recovery in recoveries] == [ + "reacquire", + "retry_reacquired", + ] + assert [recovery["attempt_index"] for recovery in recoveries] == [1, 1] + assert [recovery["call"]["semantic_id"] for recovery in recoveries] == [ + "pick", + "place", + ] + assert all(recovery["call"]["status"] == "completed" for recovery in recoveries) + assert recovery_segment["metadata"]["validation"]["accepted_mask"] == [True] + + +def _main(argv: list[str] | None = None) -> int: + """Run only the isolated native-simulation helper mode.""" + parser = argparse.ArgumentParser() + parser.add_argument("--run-fault", action="store_true", required=True) + parser.add_argument("--raw-json", type=Path, required=True) + parser.add_argument("--report", type=Path, required=True) + args = parser.parse_args(argv) + return _run_fault_subprocess(args.raw_json, args.report) + + +if __name__ == "__main__": + raise SystemExit(_main()) diff --git a/tests/benchmark/expert_program/test_demo_success_cube_sim.py b/tests/benchmark/expert_program/test_demo_success_cube_sim.py new file mode 100644 index 000000000..d77d757e7 --- /dev/null +++ b/tests/benchmark/expert_program/test_demo_success_cube_sim.py @@ -0,0 +1,150 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Live repeated-cube regression coverage for the Expert Program benchmark.""" + +from __future__ import annotations + +from pathlib import Path +import subprocess +import sys + +import pytest + +from embodichain_tasks.configs import get_config_path +from scripts.benchmark.expert_program.demo_success import ( + aggregate_demo_success_trials, + load_raw_trials, +) + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +_CUBE_GYM_CONFIG = get_config_path("gym/multi_segments/cube_pick_place.json") +_CUBE_EXPERT_PROGRAM = get_config_path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" +) +_CASE_ID = "repeated_cube_three_cycle_live" +_SEED = 0 +_NUM_ENVS = 1 +_SUBPROCESS_TIMEOUT_SECONDS = 180 +_RUN_PUBLIC_MAIN = ( + "from scripts.benchmark.expert_program.demo_success import main; " + "raise SystemExit(main())" +) + + +def _successful_effect_decisions(call: dict[str, object]) -> list[dict[str, object]]: + """Return successful physical-effect observations from one call trace.""" + effects = call["effects"] + assert isinstance(effects, list) + return [ + effect + for effect in effects + if isinstance(effect, dict) + and isinstance(effect.get("decision"), dict) + and effect["decision"].get("success_mask") == [True] + ] + + +@pytest.mark.requires_sim +@pytest.mark.slow +@pytest.mark.gpu +def test_live_repeated_cube_completes_three_physical_cycles( + tmp_path: Path, +) -> None: + """Run all three no-retry segments through the public live entry point.""" + raw_path = tmp_path / "cube_raw.json" + report_path = tmp_path / "cube_report.md" + completed = subprocess.run( + [ + sys.executable, + "-c", + _RUN_PUBLIC_MAIN, + "--run-simulation", + "--gym_config", + str(_CUBE_GYM_CONFIG), + "--expert-program", + str(_CUBE_EXPERT_PROGRAM), + "--case-id", + _CASE_ID, + "--seeds", + str(_SEED), + "--raw-json", + str(raw_path), + "--report", + str(report_path), + "--headless", + "--device", + "cuda", + "--num_envs", + str(_NUM_ENVS), + "--filter_dataset_saving", + ], + cwd=_REPOSITORY_ROOT, + capture_output=True, + text=True, + timeout=_SUBPROCESS_TIMEOUT_SECONDS, + check=False, + ) + + assert completed.returncode == 0, completed.stdout + completed.stderr + decoded_trials = load_raw_trials(raw_path) + assert len(decoded_trials) == 1 + trial = decoded_trials[0] + assert trial.case_id == _CASE_ID + assert trial.seed == _SEED + assert trial.rows[0].success + assert trial.rows[0].terminal_reason == "success" + + episode = trial.episode_result + assert episode["completed"] is True + assert episode["success"] == [True] + assert episode["terminal_reason"] == "success" + segments = episode["segments"] + assert isinstance(segments, list) + assert len(segments) == 3 + + for segment_index, (segment, target_index) in enumerate( + zip(segments, (0, 1, 0), strict=True) + ): + assert isinstance(segment, dict) + assert segment["segment_id"] == segment_index + assert segment["name"] == "move_cube" + metadata = segment["metadata"] + assert isinstance(metadata, dict) + runtime = metadata["runtime"] + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + calls = runtime["calls"] + assert [call["semantic_id"] for call in calls] == ["pick", "place"] + assert all(call["status"] == "completed" for call in calls) + assert all(_successful_effect_decisions(call) for call in calls) + + post_policies = metadata["post_policies"] + assert len(post_policies) == 1 + assert post_policies[0]["kind"] == "wait_stable" + assert post_policies[0]["result"]["status"] == "settled" + validation = metadata["validation"] + assert validation["accepted_mask"] == [True] + validators = validation["validators"] + assert len(validators) == 1 + assert validators[0]["result"]["target_value_index"] == target_index + assert validators[0]["result"]["accepted_mask"] == [True] + + aggregates = aggregate_demo_success_trials(decoded_trials) + metrics = aggregates.success_and_metrics[0] + assert metrics["attempted"] == 1 + assert metrics["successes"] == 1 + assert metrics["success_rate"] == 1.0 diff --git a/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py b/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py index fe816f85a..5649a005a 100644 --- a/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py +++ b/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py @@ -124,11 +124,12 @@ def test_direct_default_cfg_loads_the_same_typed_program() -> None: assert settle.params["entity_cfgs"][0].uid == "cube" -def test_robot_profile_calibrates_physical_tracking_tolerance() -> None: - """The UR5 preset tolerates its measured drive lag without disabling feedback.""" +def test_robot_profile_calibrates_physical_motion_and_tracking() -> None: + """The UR5 preset slows motion without weakening feedback or recovery.""" binding = create_cube_robot_profile_binding() assert binding.presets[0].preset_id == "safe" + assert binding.presets[0].motion_policy.sample_count == 100 tracking = binding.presets[0].tracking_policy assert tracking.in_flight is not None assert tracking.in_flight.metrics[0].tolerance == 0.08 diff --git a/tests/scripts/tools/test_expert_program_rollout_report.py b/tests/scripts/tools/test_expert_program_rollout_report.py index 260f15a81..0c295cce0 100644 --- a/tests/scripts/tools/test_expert_program_rollout_report.py +++ b/tests/scripts/tools/test_expert_program_rollout_report.py @@ -28,8 +28,8 @@ EXPECTED_CURRENT_COUNTS = { # Each tuple is (raw LF bytes, raw file bytes) for the explicit task pair. - "Cube": (366, 12_448), - "Drawer": (246, 8_391), + "Cube": (395, 13_272), + "Drawer": (265, 8_916), } EXPECTED_SOURCE_PATHS = {