diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 308d42ea3..01323f9a5 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -449,6 +449,10 @@ topics: - resource graph - resource DAG - semantic skill catalog + - semantic skill runtime + - expert program + - declarative expert program + - atomic demo bridge - capability binding - AtomicAction - ActionInvocation @@ -468,6 +472,26 @@ topics: - ExecutionSession - EffectVerificationRequest - EffectVerificationResult + - attempt_generation + - SemanticEffectSpec + - EffectMonitorRef + - EffectMonitorRegistry + - EffectMonitorDecision + - PoseRelationEvidenceBatch + - relation hysteresis + - SkillRuntime + - SkillResult + - AtomicSkills + - SemanticCallSpec + - SemanticSkillCompiler + - ExpertProgramCfg + - ExpertProgramCompiler + - AtomicDemoBridge + - BufferedGymCommandSink + - ControlCommandStateEvidenceTracker + - DynamicSettleMonitor + - ParallelSkillRuntime + - program segment metadata - eligible_mask - deactivate_rows - effect verification deadline @@ -537,6 +561,8 @@ topics: - ResolvedRobotResource - ResolvedSkillBinding - SkillPolicyPreset + - effect_monitors + - semantic effect monitor - binding_contract - engine.skills - skill_profile @@ -613,8 +639,18 @@ topics: - embodichain/lab/sim/atomic_actions/primitives/ - embodichain/lab/sim/atomic_actions/__init__.py - embodichain/lab/sim/skills/scene.py + - embodichain/lab/sim/skills/calls.py + - embodichain/lab/sim/skills/compiler.py + - embodichain/lab/sim/skills/effects.py + - embodichain/lab/sim/skills/evidence.py + - embodichain/lab/sim/skills/integration.py + - embodichain/lab/sim/skills/runtime.py + - embodichain/lab/sim/skills/parallel.py + - embodichain/lab/sim/skills/parallel_runtime.py - embodichain/lab/sim/skills/profiles.py - embodichain/lab/sim/skills/__init__.py + - embodichain/lab/gym/envs/expert_program/ + - embodichain/lab/gym/envs/settling.py related_topics: - motion-planning - robot-system diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index dfcd1dbb1..ec1489b17 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -168,7 +168,8 @@ Binding and policy authority is split deliberately: - the `RobotSkillProfile` owns the resource DAG, capability declarations, complete per-skill default `ResourceBinding` values, semantic command profiles keyed by generic profile IDs, and named `SkillPolicyPreset` - snapshots; endpoint declarations or adapters select those profile IDs; + snapshots that also select exact semantic-effect monitors; endpoint + declarations or adapters select those profile IDs; - the bound robot owns actual control-part membership and joint IDs, and its configured solver is checked for known solver-backed capabilities; - endpoint adapters own controller-specific validation, physical claims, and @@ -207,10 +208,12 @@ match the engine's configured planner. IDs, and adapter-defined `claim_tokens`. Claims conflict when any category overlaps, so a `whole_body` composite conflicts with a contained arm even when their endpoint or control-part names differ. This is deterministic conflict -metadata only: there is no resource lease manager, parallel scheduler, -or concurrency guarantee yet. Dynamic execution can dispatch multiple -endpoint commands in one synchronized frame, but that does not imply resource -scheduling or safe parallelism. A custom mobile/base or whole-body endpoint is +metadata only: a `ResourceClaim` by itself is not a resource lease manager, +parallel scheduler, or concurrency guarantee. The separate explicit +`ParallelSkillRuntime` described below coordinates analyzed branch lanes and +still requires an authoritative safety validator. Dynamic execution can +dispatch multiple endpoint commands in one synchronized frame, but that alone +does not imply resource scheduling or safe parallelism. A custom mobile/base or whole-body endpoint is executable only when its adapter supplies a target, the action emits a matching runtime payload, and the target's transport is registered with the `EndpointCommandRouter`. Successful binding or a non-conflicting claim alone is @@ -448,6 +451,42 @@ effect_result = EffectVerificationResult( result = runner.step(effect_result=effect_result) ``` +The semantic layer keeps physical observation separate from symbolic effect +commit. `SkillPolicyPreset.effect_monitors` maps exact semantic call IDs to +versioned, bounded-declarative `EffectMonitorRef` values. Omitting the mapping +selects the built-in `builtin.composite_effect@1` monitor for `pick`, +`place`, and `hand_over`; an explicit empty mapping disables the default and +makes analysis of those curated calls fail with `missing_effect_monitor`. +`SemanticIntegrationManifest` rejects monitor keys absent from its call +catalog. `SemanticSkillCompiler.analyze()` resolves the exact factory and +validates monitor parameters without observing scene providers or constructing +stateful monitors. + +Grounding creates an immutable `SemanticEffectSpec` and an independent monitor +for the call. The spec separates typed symbolic state expectations from typed +physical clauses. Pick declares an attached destination, Place a detached +source with an owned pre-effect pose baseline, and HandOver both. Endpoint +adapters publish immutable `EffectEvidenceSourceRef` values and a logical +`task_state_key`; evidence routes use `EffectEvidenceAddress`, never the +command-only `RuntimeEndpointTarget`. This keeps motion, mobile, whole-body, +articulation, and custom controller transports extensible without treating a +control part as symbolic state identity. + +Providers emit raw `PoseRelationEvidenceBatch`, `BinaryEffectEvidenceBatch`, +`ScalarEffectEvidenceBatch`, or `JointStateEvidenceBatch` values with stable +environment IDs, per-row validity/acquisition diagnostics, timestamps, and +observation revisions. Providers do not apply policy thresholds. The composite +monitor evaluates clauses as a conjunction per state expectation, applies +pose/force/joint hysteresis, treats invalid rows as unresolved, and reports +explicit contradictory evidence as failure. It never uses `TaskState` as +physical proof. The `SkillRuntime` adapter validates the decision, attaches +only the current verification ID, and returns an exact +`EffectVerificationResult` in the same due observation cycle. Request shrink +within one `attempt_generation` preserves remaining-row hysteresis; installing +a retry/replan/revision increments the generation and resets it. Evidence at +the exact deadline is allowed; evidence after it is rejected and normal runner +timeout/recovery remains authoritative. + Cause events (`ACTION_PLANNING_FAILED`, `EFFECT_VERIFICATION_FAILED`, and `EFFECT_VERIFICATION_TIMEOUT`) are distinct from the `ACTION_RETRY` recovery event. `SESSION_COMPLETED` and `SESSION_FAILED` are distinct terminal events. @@ -566,6 +605,130 @@ live observation fails. Environment IDs must remain stable and ordered for the entire session; robot and scene timestamps and scene versions must be monotonic. Collision-world revisions must also remain monotonic per environment. +## Semantic runtime and Expert Programs + +`embodichain.lab.sim.skills` is the semantic frontend over the core contracts. +`Pick`, `Place`, `HandOver`, `OperateArticulation`, and registered extension +calls are immutable, robot-independent intent values. `SemanticSkillCompiler` +performs provider-free workflow analysis first, then grounds exactly one call +from a fresh `PlanningContext`. It resolves the authoritative `SceneRegistry`, +profile resource binding and preset, downstream target look-ahead, typed goal, +effect specification, and effect monitor before producing one +`ActionInvocation`. + +`SkillRuntime` owns the shared call barrier and persistent verified `TaskState`. +Every call creates exactly one one-invocation `ExecutionSession` and re-observes +before the next call. Eligibility, success, failure, cancellation, recovery, +and effect state are row-local; active rows share the call boundary. The +runtime exposes non-blocking `start()`/`step()` and synchronous `run()` over the +same path. `AtomicSkills` is a convenience facade. `AtomicSkills.from_env()` +accepts only an explicit `SkillRuntimeProvider` and never scans arbitrary +environment attributes; Gym demo environments use the lazy bridge below so +commands cannot bypass `env.step()`. + +`embodichain.lab.gym.envs.expert_program` owns strict declarative programs. +Schema version 1 supports bounded `Sequence`, `Repeat`, `Segment`, and `Invoke`; +version 2 adds deterministic `Parallel` branches and explicit `Barrier` nodes. +The decoder rejects unknown fields/discriminators, duplicate serialized keys, +unsupported versions, executable values, dotted environment traversal, +unbounded expansion, and invalid registry/catalog references before runtime. +JSON and YAML files are loaded with `load_expert_program()`. A Gym config can +select one with `expert_program_path`, resolved relative to that config file. + +`ExpertProgramCompiler` expands program/demo segments lazily while preserving +typed target selections, post-policies, validators, and parallel blocks. +`AtomicDemoBridge` assembles each segment around the canonical runtime and a +buffered command sink. A `ProcessedEnvAction` marks controller-ready output so +the action manager does not transform it twice, but every command and +post-policy hold still passes through ordinary `env.step()`. `BaseEnv.step_dt` +is authoritative; frame durations must be integral multiples of that cadence. +Parallel lanes are aligned on that strict grid and shorter lanes repeat their +last safe target as hold padding; fractional frames are rejected rather than +implicitly resampled. Early generator termination performs the bridge's +explicit cancel-then-hold handshake before the iterator is closed. + +Bridge creation materializes the bounded segment stream and performs +provider-aware semantic preflight before the first command is emitted. +Sequential stretches analyze their remaining downstream calls together, so a +Pick retains target look-ahead across logical segment boundaries; an explicit +parallel block is a conservative look-ahead barrier. Runtime grounding remains +just-in-time against the latest observation. Relation Place calls require an +exact typed/versioned `RelationTargetGrounder`, and HandOver requires the +profile-selected `HandOverPoseProvider`; neither provider is inferred from +names. + +The production simulation path is +`create_simulation_expert_program_adapter(environment, scene_binding=..., +robot_profile_binding=...)`. `SimulationSceneBinding` declares canonical/native +scene data, while `SimulationRobotSkillProfileBinding` declares reusable robot +resources, capabilities, commands, defaults, and presets. The factory creates +the registry, profile, motion generator, engine, shared-tick observation/evidence +port, command encoder, runtime, and segment policy port. Task classes combine an +external declarative program with typed scene/profile integration declarations +and install the returned adapter; they do not assemble skill trajectories. + +`SimulationRobotSkillProfileBinding` accepts generic `RobotResourceBinding` +declarations containing arbitrary typed `ResourceEndpoint` values; +`ControlPartResourceBinding` is the joint-backed convenience. Mobile-base, +whole-body, and non-joint integrations install a matching +`ResourceEndpointAdapter` and `RuntimeTransportActionEncoder` through the same +standard simulation factory. Task-level Expert Programs remain unchanged. This +is an extension seam rather than built-in locomotion: current curated semantic +skills do not consume the example base/whole-body capabilities. A reusable +production capability also installs its semantic descriptor/lowerer, atomic +skill, payload, safe-state transport behavior, and effect integration as +applicable. + +The standard Gym encoder currently composes custom transports over a full-qpos +hold and the standard simulation factory owns a `MotionGenerator`. A robot may +omit named control parts, but a truly jointless or natively structured mobile +controller still needs a reusable base-action composition/provider +integration. That integration must not add base- or whole-body-shaped fields to +the generic resource, binding, runner, or router contracts. + +Task vertical slices may keep typed profile bindings locally during API +stabilization, but repeated use should promote them into an embodiment-owned +profile catalog rather than duplicate robot data across tasks. + +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; the full three-cycle +run remains in threshold calibration. + +When no explicit contact or constraint callback is installed, simulation grasp +and release evidence combines the live object-to-endpoint pose relation with +`ControlCommandStateEvidenceTracker`. The tracker changes row-local state only +after an exact profile-owned `open` or `grasp` command is successfully encoded +and buffered. Intermediate commands and inactive rows retain prior state; +cancel, discard, or observer failure invalidates affected evidence. Stable +`env_ids`, not simulator array assumptions, correlate full and subset batches. +This command state is evidence of accepted controller intent, not physical +contact by itself. + +`DynamicSettleMonitor` is shared by reset events and the Expert Program +`wait_stable` post-policy. It owns threshold, cadence, consecutive-check, +settled, and timeout state but never steps simulation. The demo policy yields +full-qpos holds through the normal environment step path. Segment validators +remain a separate dataset/task boundary. + +Runtime and demo results expose deterministic JSON-safe metadata. Call traces +include invocation identity, masks, command counts, execution/recovery events, +plan-attempt trajectory segments, scene/collision revisions and dependencies, +plus effect decisions and monitor evidence. Segment metadata adds post-policy +settling and validator results. Trajectory segments are trace ranges inside an +atomic plan and never own separate recovery, effect, or timeout state. + +Parallel execution is an explicit schema/runtime layer rather than a second +atomic scheduler. Static analysis rejects overlapping `ResourceClaim` values. +Independent lane runtimes share one clock and barrier, command frames are +merged only after destination/claim/safety validation, failure handling is +row-local, and verified `StateDelta` values merge deterministically at the +barrier. Parallel execution also requires an authoritative +`ParallelCommandSafetyValidator`; resource disjointness alone is never promoted +to physical-safety evidence, and a missing validator fails closed. Schema +version 2 intentionally uses strict task-state key-level merge conflicts; +mask-aware same-key branch merges are not part of this version. + ## Parameter ownership Goal dataclasses carry only semantic task intent. They do not carry robot part @@ -651,6 +814,7 @@ on their resolved endpoint. | `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | | `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | | `hand_over` | `GraspGoal` | `source.motion`, `source.grasp`, `destination.motion`, `destination.grasp` | +| `operate_articulation` | `OperateArticulationGoal` | `primary.motion`, `primary.interaction` | `GraspGoal.grasp_xpos` accepts an explicit pose tensor, a late-bound `SceneEntityPose`, or `None` for affordance sampling. A `SceneEntityPose` diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index bc70bd30d..7a2ac157c 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,7 +1,12 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: implementation in progress; Phase 0 and PR1 complete, and PR2A, - PR2B, and PR2C implemented on stacked feature branches +- Status: core contracts are implemented through Phase 7 on stacked feature + 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. - Baseline: `main@bcccb787e8f9165e9c8acf6f39f165ba6ac752a4` - Last updated: 2026-08-11 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), @@ -45,11 +50,13 @@ same layer and run through one runtime built on `ExecutionRunner`. The target authoring cost is: -- a new task that uses existing semantic capabilities: scene configuration, - Expert Program configuration, and optionally a declarative validator; +- a new task that uses existing semantic capabilities: Expert Program + configuration plus typed scene/profile integration declarations, and + optionally a declarative validator, with no task-specific motion code; - a new robot: one reusable `RobotSkillProfile`, not task-specific motion code; -- a genuinely new physical interaction: one reusable semantic skill/compiler/ - monitor implementation, after which tasks use it from configuration. +- a genuinely new physical interaction: one reusable capability bundle + containing its semantic skill/compiler/monitor and controller integration as + applicable, after which tasks select it through program and integration data. This design preserves the core direction of #471. Issue #474 changes the middle of the architecture: ordinary configuration must describe semantic @@ -91,7 +98,7 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is updated against committed `main@e445133c` after PR #475. The +This plan is updated against committed `main@bcccb787` after PRs #475 and #476. The implementation series is stacked from that baseline: PR1 is complete on `refactor/atomic-actions-phase0`, PR2A is implemented by `feat/atomic-action-pr2a-scene-registry`, and PR2B is implemented by @@ -881,7 +888,7 @@ PR2A SceneRegistry PR2B RobotSkillProfile | | | v | PR2C Runtime Endpoints - | (in progress) + | (implemented) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -1036,10 +1043,28 @@ Deliverables: same-slot endpoint disjointness for future conflict analysis, without claiming safe parallel execution. -The profile API can represent mobile-base and whole-body resources today. A -new endpoint kind still needs one shared adapter and a compatible shared atomic -skill before the current core can execute it; adding tasks that reuse that -capability then remains configuration-only. +The profile and endpoint-runtime APIs can represent mobile-base and whole-body +resources today, and the generic paths are covered by whole-body joint and +custom planar-velocity tests. They are extension seams, not built-in navigation +or whole-body behavior: no current curated semantic skill consumes the example +`motion.base.*` or `motion.whole_body` capabilities. A production shared +capability still needs its semantic descriptor/lowerer, atomic skill, payload, +endpoint adapter, transport, and effect integration as applicable. Once that +reusable bundle exists, another task supplies an Expert Program plus typed +scene/profile integration declarations without task-specific motion code. + +The standard Gym bridge currently composes every custom transport action over +a full-qpos hold and the standard simulation factory owns a +`MotionGenerator`. This supports robots without named control parts, but a +truly jointless or natively structured mobile controller still needs a reusable +base-action composition/provider integration. That extension must not add +base- or whole-body-shaped fields to the generic resource, binding, runner, or +router contracts. + +The current task vertical slices still construct their typed profile bindings +from task modules. Promoting stable bindings into an embodiment-owned profile +catalog is rollout packaging needed for cross-task reuse; it does not require a +new resource or runtime contract. PR2B may proceed in parallel with PR2A after the PR1 bridge. Neither follow-up requires official task migration; the repeated-cube vertical slice opts in only @@ -1101,8 +1126,24 @@ diagnostic, robot capabilities resolve bindings/presets without task-owned motion code, and generic resolved endpoints can reach their registered runtime transports without adding arm/tool-specific core paths. +Implementation status: when `safe` is reachable and the registry declares +dynamic collision entities, binding rejects an unsupported active planner +before observation or planning. Linking produces an effective +`DynamicCollisionMode.REQUIRED` preset snapshot without mutating the profile's +source preset. A real-simulation gate now covers semantic lowering, CUDA/cuRobo +planning, a post-plan dynamic-obstacle world change, collision-revision-aware +replanning, and successful completion. This is a conditional GPU gate: the +module skips when cuRobo is unavailable or CUDA is unavailable, and pytest runs +it only when GPU and slow tests are explicitly selected. + ### Phase 2: semantic facade and compiler +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. + Deliverables: - `SemanticCallSpec`, object-centric `Pick`, `Place`, and `HandOver`; @@ -1119,6 +1160,16 @@ effect verifier. ### Phase 3: canonical runtime and effects +Implementation status: core contracts are implemented in the current stack. +The backend-neutral typed state expectations, evidence addresses and sources, +pose/binary/scalar/joint evidence clauses, versioned monitor registry, +profile-owned monitor selection, grounded Pick/Place/HandOver/articulation +effects, row-local composite hysteresis kernel, canonical `SkillRuntime`, and +production simulation evidence ports are wired end to end. Physical simulation +acceptance is partial: Open Drawer and one cube Pick/Place/settle/validator +cycle have completed, while the full repeated-cube run and embodiment-owned +HandOver pose integration remain validation work. + Deliverables: - `SkillRuntime` wrapping `ExecutionRunner` for sync and step-wise use; @@ -1135,6 +1186,12 @@ compiler/runtime code and produce equivalent results. ### Phase 4: demo integration primitives +Implementation status: implemented. The bridge uses buffered runtime commands and +an environment-step clock, dynamic settling is shared with reset behavior, and +JSON-safe lifecycle metadata covers every installed plan attempt, named +trajectory segment, effect decision/evidence, recovery event, scene/collision +revision, post-policy outcome, and validator result. + Deliverables: - expose the existing named plan trajectory segments through optional demo @@ -1151,13 +1208,23 @@ effect, or trace integration contains a hard-coded trajectory index. ### Phase 5: Expert Program version 1 and repeated-cube vertical slice +Implementation status: configuration and task migration are implemented. The +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. + Deliverables: - strict `@configclass` schema and versioned decoder; - `Sequence`, bounded `Repeat`, `Segment`, and `Invoke`; - registered targets, post-policies, and validators; - `EmbodiedEnvCfg` and CLI integration with legacy fallback; -- configuration-only migration of repeated cube pick/place. +- motion-code-free migration of repeated cube pick/place using a declarative + program and typed scene/profile integration declarations. Exit criteria: @@ -1172,6 +1239,13 @@ Exit criteria: ### Phase 6: sequential skill coverage and articulated interaction +Implementation status: the articulation path and task migration are +implemented. Articulation/link/operation-affordance registration, +`OperateArticulation`, typed joint-state effects/evidence, and the declarative +Open Drawer program with typed integration declarations use the same +compiler/runtime path as pick/place. Its supported-simulation physical run now +completes and reaches the configured drawer joint target. + Deliverables: - articulation/link/affordance registry integration; @@ -1186,11 +1260,22 @@ trajectories in task code. ### Phase 7: parallel execution and PourWater +Implementation status: the schema/runtime contracts and fail-closed safety +boundary 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 +migration is outside the current scope because it would require modifying +Action Bank code. + Deliverables: - `Parallel` and explicit `Barrier` nodes in a new schema version; - robot-resource conflict analysis; -- deterministic trajectory alignment/resampling policy; +- deterministic strict-step-grid alignment with hold padding; fractional frame + durations are rejected rather than implicitly resampled; - synchronization and timeout behavior; - deterministic per-environment `StateDelta` merge rules; - PourWater migration from its Action Bank subclass. @@ -1200,6 +1285,18 @@ tests pass before the legacy task is switched. ### Phase 8: rollout, documentation, and deprecation +The deterministic framework/integration capability matrix and migration-size +snapshot are maintained in +[`expert_program_rollout_report.md`](expert_program_rollout_report.md). Demo +success collection uses the no-retry benchmark harness; real success-rate +claims and gates remain deferred until the repeated Cube threshold contract and +three-cycle physical acceptance are settled. + +Implementation status: partial. The canonical semantic/Expert Program documentation, +project-development context, task vertical slices, and public integration +guidance are included in this stack. Metrics, migrations that touch Action +Bank, and any deprecation proposal remain explicitly separate follow-up work. + Deliverables: - semantic quickstart and advanced-core integration guide; @@ -1255,18 +1352,27 @@ independent of adoption of the new path. - grasp/release/handover effect monitors; - settling success and timeout metadata; - Open Drawer articulation effect; -- GPU-backed dynamic cuRobo coverage where supported; +- conditionally executed real CUDA/cuRobo dynamic-obstacle recovery coverage + where cuRobo and CUDA are available and GPU/slow tests are enabled; - parallel PourWater only after Phase 7 contracts land. ## 14. Acceptance criteria The design is complete when all of the following hold: -- [ ] A versioned Expert Program is fully validated before execution and cannot +- [x] A reachable `safe` preset in a dynamic-collision scene resolves to + `DynamicCollisionMode.REQUIRED` and rejects an unsupported active planner + before observation, planning, or command emission without mutating the + profile configuration. +- [x] On supported CUDA/cuRobo installations, a conditional real-simulation + gate moves a dynamic obstacle after the initial plan, observes the + collision-world change and replan, and reaches the target successfully; + environments without cuRobo or CUDA skip this GPU/slow gate. +- [x] A versioned Expert Program is fully validated before execution and cannot evaluate arbitrary code or traverse environment attributes by string. -- [ ] Python, configuration, and future MLLM calls share one semantic compiler, +- [x] Python, configuration, and MLLM calls share one semantic compiler, typed atomic-action core, and runtime. -- [ ] A common new task using existing semantic skills needs no task-specific +- [x] A common new task using existing semantic skills needs no task-specific motion-generation code. - [x] Robot capability binding is expressed through generic participant resources and endpoints, so mobile-base and whole-body skills do not @@ -1274,14 +1380,14 @@ The design is complete when all of the following hold: - [x] Runtime binding, command framing, routing, and safe stop are endpoint generic; joint trajectories remain an optional planning/feedback artifact rather than the only runtime carrier. -- [ ] Each scene entity is registered once under an authoritative registry ID +- [x] Each scene entity is registered once under an authoritative registry ID across semantics, observation, affordance, and collision handling; simulation `uid` values are legacy aliases only. -- [ ] The default pick/place path does not expose raw qpos, grasp/EEF matrix +- [x] The default pick/place path does not expose raw qpos, grasp/EEF matrix math, planner construction, session plumbing, or custom verification. -- [ ] Automatic grasping tracks target revisions and receives downstream object +- [x] Automatic grasping tracks target revisions and receives downstream object goals without caller duplication. -- [ ] `Place` is object-centric and consumes verified held-object state. +- [x] `Place` is object-centric and consumes verified held-object state. - [ ] Built-in grasp, release, handover, and supported articulation effect monitors work in simulation. - [x] Repeated sub-threshold motion eventually publishes the correct scene @@ -1289,20 +1395,20 @@ The design is complete when all of the following hold: - [x] Custom actions have a documented and tested intentional hard-break migration from overriding `plan()` to implementing `_plan()`; no compatibility adapter is required. -- [ ] Version 1 creates exactly one one-invocation `ExecutionSession` for each +- [x] Version 1 creates exactly one one-invocation `ExecutionSession` for each semantic call and re-observes before lowering the next call. -- [ ] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass +- [x] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass through `env.step()`. -- [ ] No program post-policy, effect, or tracing integration depends on +- [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 observed program/demo segments with settle/effect/validation metadata. -- [ ] Version 1 uses one shared program/call barrier while per-environment task +- [x] Version 1 uses one shared program/call barrier while per-environment task state, effects, recovery, eligibility, success, and failure remain independent. -- [ ] Advanced users retain typed goals, invocations, policies, providers, +- [x] Advanced users retain typed goals, invocations, policies, providers, sessions, and planners as escape hatches. -- [ ] Parallel resource conflicts, synchronization, timing, cancellation, and +- [x] Parallel resource conflicts, synchronization, timing, cancellation, and state merging are tested before PourWater migration. - [ ] Action Bank remains usable until feature parity and a deprecation window are documented. diff --git a/docs/design/expert_program_rollout_report.md b/docs/design/expert_program_rollout_report.md new file mode 100644 index 000000000..6c8228748 --- /dev/null +++ b/docs/design/expert_program_rollout_report.md @@ -0,0 +1,68 @@ +# Declarative Expert Program Rollout Report + +This is a deterministic, static Phase 8 snapshot of checked-in framework and integration code. It does not run simulation, report physical acceptance, or certify production readiness for an embodiment. + +## Framework Contract Matrix + +`framework-tested` describes the reusable framework contract only. A task appears in the matrix below only when its integration/production code is checked in; that code status does not imply physical acceptance. + +| Capability | Framework status | Integration gate | Scope | +| --- | --- | --- | --- | +| Pick + Place(at) | framework-tested | per-embodiment integration | Typed goals, compilation, execution, and terminal effects are covered. | +| Attach/release effect | framework-tested | per-embodiment integration | Effects use accepted commands plus live object-to-endpoint pose evidence. | +| 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. | +| 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. | + +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. + +| 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 | +| CobotMagic | Open Drawer | OperateArticulation | articulation effect | V1 sequential | checked in | fixed-seed supported-simulation slow gate; not release-required | + +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. + +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. + +## Migration Size Snapshot + +The baseline is a fixed, manually recorded pre-migration snapshot: Cube is 598 lines / 23912 bytes and Drawer is 245 lines / 8833 bytes. The tool does not inspect Git history. Current values are recomputed only from the four explicit files in the table. + +Baseline identity: Cube uses Git blob `1965563b060d1fc889f03ad13d47655c2edcd99b` and Drawer uses Git blob `3b4cbdc09537098b4f109d46efb8785b88f31ce1` at each task's Python path listed in the current-source column. Blob IDs remain stable across stack rebases. + +Counting rule: `lines` is the number of raw LF (`0x0A`) bytes; `bytes` is the raw on-disk byte length. Counts are summed per task without normalizing encoding or line endings. + +| 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 | + +## 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. + +## Drift Check + +Regenerate the checked-in report after an intentional source or capability snapshot change: + +```bash +python scripts/tools/expert_program_rollout_report.py +``` + +CI and local validation can reject stale output without rewriting it: + +```bash +python scripts/tools/expert_program_rollout_report.py --check +``` diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 9783df20f..c14b62527 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -147,6 +147,7 @@ The animations below are the focused simulator demos under | `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | both grasp endpoints: `open`, `grasp` | semantic object/entity | create coordinated attachment; clear individual attachments | | `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | `placing.grasp`: `open`, `grasp`; `support.grasp`: `grasp` | one individually held object per motion target | optionally detach placing object; preserve support attachment | | `hand_over` | `GraspGoal` | `source.motion`, `source.grasp`, `destination.motion`, `destination.grasp` | both grasp endpoints: `open`, `grasp` | object held by the source motion target | transfer attachment to the destination motion target | +| `operate_articulation` | `OperateArticulationGoal` | `primary.motion`, `primary.interaction` | `primary.interaction`: `open`, `grasp` | registered articulation and handle operation affordance | update and physically verify the target articulation joint position | ### Participant slot meanings @@ -362,6 +363,13 @@ same tensor for grasp sampling, upright adjustment, and `object_to_eef`, and automatically records the ID as a scene dependency. An explicit ID never falls back to a live simulation entity when the snapshot entry is missing. +The object dependency is monitored only while the `approach` segment is active. +Its exclusive cutoff is `close.start`: object motion observed before that frame +invalidates the plan, while motion from gripper closure and lift does not. After +the cutoff, every object-pose change is ignored by scene recovery, including an +external disturbance, so the accepted `grasp` command and live +object-to-endpoint effect evidence become the authoritative completion check. + `PickUp` requires typed `open` and `grasp` commands on `primary.grasp`. Important `PickUpOptions` fields: @@ -615,6 +623,47 @@ dual-arm `strategy="motion_gen"` path. **Example:** `scripts/tutorials/atomic_action/hand_over.py` +(builtin-operate-articulation)= + +## `OperateArticulation` + +Runs one reusable **approach -> engage -> operate -> release -> retract** +interaction for a drawer, slider, or another handle-driven articulation. + +| Contract | Value | +|---|---| +| Skill ID | `operate_articulation` | +| Goal | `OperateArticulationGoal(articulation_id, joint_id, geometry, source_position, target_position, target_displacement)` | +| Binding contract | disjoint `primary.motion` and `primary.interaction` endpoints | +| Required commands | `primary.interaction`: `open`, `grasp` | +| Effect | `ArticulationJointState[(articulation_id, joint_id)] = target_position` | +| Verification | explicit joint-state evidence is required before committing the effect | + +The first-class semantic call takes an articulation reference, an optional +handle affordance reference, and either a named target or an explicit +`target_position` plus `target_displacement` pair. The pair is intentionally +not inferred from simulator state: the core scene snapshot contains entity +poses, not articulation qpos. + +`ArticulationOperationAffordance` owns the joint ID, approach/contact/ +operation/retract offsets, operation axis, position scale, and optional named +position/displacement pairs. At every JIT grounding boundary the compiler +reads the latest registered handle pose and derives all four end-effector +poses. The displacement is measured from that observed handle pose. A named +target also supplies both its absolute joint postcondition and its explicit +handle-relative displacement. + +The grounded semantic effect uses an `ArticulationJointStateExpectation` and a +`JointStateEffectClause` addressed by canonical articulation and joint IDs. +Planning success alone never commits the symbolic joint state. + +The handle scene dependency has an exclusive cutoff at `operate.start`. Motion +before engagement can still invalidate and replan the trajectory; motion after +that boundary is expected to be caused by the operation and is not classified +as target drift. The joint-state effect monitor remains authoritative for +completion, and the cutoff also ignores unrelated external handle motion after +the operation starts. + ## Running the demos Every focused script is interactive by default. Add `--auto_play` to skip diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index e033a7937..6ad3ae46c 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -7,6 +7,7 @@ builtin_actions robot_skill_profiles +expert_programs ``` ```{currentmodule} embodichain.lab.sim.atomic_actions @@ -88,7 +89,8 @@ The boundary is deliberate: | Scene observation | Registry-derived `SceneProvider` | Captures canonical ordered entities plus monotonic global or per-environment collision-world revisions | | Scheduling and controller lifecycle | `ExecutionRunner` | Observes only when due, dispatches timed commands, records acknowledgements, and performs safe stop | | Robot/simulator I/O | `ObservationProvider`, `EndpointCommandRouter`, `EndpointCommandTransport`, and `ExecutionClock` adapters | Isolates observation, per-controller command transport, and time/physics advancement from planning and session state | -| Physical-effect verification | Application observer | Verifies grasp, release, handover, and other symbolic effects | +| Physical-effect evidence | Backend provider or application adapter | Acquires typed pose/contact/controller evidence without applying policy thresholds | +| Effect decision and correlation | `EffectMonitor` plus the semantic runtime adapter, or an application verifier on the direct-core path | Interprets evidence, attaches the current request ID, and reports grasp, release, handover, or other symbolic effects | `ExecutionRunner.step()` is non-blocking. Its convenience `run_until_blocked()` loop waits or advances simulation through an injected @@ -803,6 +805,26 @@ its `effect_result`: schedule another call using `wait_duration`, re-read the current request, and submit a result for that current ID. Partial resolution and row deactivation can also replace the request before the delayed result arrives. +The semantic layer provides a reusable verifier kernel for the curated +`Pick`, `Place`, and `HandOver` calls. A +{class}`~embodichain.lab.sim.skills.SemanticEffectSpec` binds the canonical +object and expected attach/detach relations to concrete runtime endpoints. Its +fresh per-call {class}`~embodichain.lab.sim.skills.EffectMonitor` consumes +backend-neutral {class}`~embodichain.lab.sim.skills.PoseRelationEvidenceBatch` +values and returns an uncorrelated +{class}`~embodichain.lab.sim.skills.EffectMonitorDecision`. The semantic runtime +must validate that decision, attach the *current* request ID, and pass the +result to the runner in the same due observation cycle. + +This split is deliberate: the evidence provider owns physical observation, +the monitor owns thresholds and hysteresis, and `ExecutionSession` remains the +only owner of deadlines, retries, partial-row commits, and verified +`TaskState`. A request-mask shrink keeps monitor history for remaining rows via +`attempt_generation`; a replacement plan or retry increments that generation +and resets the history. Evidence exactly at the deadline is valid, while a due +observation after the deadline is handled by session timeout without invoking +the verifier. + ## Action Agent integration An MLLM should not construct `ActionInvocation` by copying arbitrary JSON into diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md index 66488542d..cbddb478b 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -80,7 +80,10 @@ from embodichain.lab.sim.atomic_actions import ( MotionPolicy, ) from embodichain.lab.sim.skills import ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, ControlPartEndpoint, + EffectMonitorRef, ResourceBinding, RobotResource, RobotSkillProfile, @@ -138,12 +141,32 @@ profile = RobotSkillProfile( "default": SkillPolicyPreset( preset_id="default", motion_policy=MotionPolicy(strategy="ik_interp"), + effect_monitors={ + semantic_id: EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + { + "attached_translation_threshold": 0.02, + "detached_translation_threshold": 0.05, + "consecutive_samples": 2, + }, + ) + for semantic_id in ("pick", "place", "hand_over") + }, ), }, default_preset="default", ) ``` +During binding, each resolved endpoint also receives a logical +`task_state_key` and immutable, channel-keyed `effect_sources`. By default the +logical key is the selected resource ID, so the `motion` and `grasp` endpoints +of `left_participant` share one symbolic held-object state even though they use +different control parts. An effect source contains an `EffectEvidenceAddress`; +it is intentionally separate from the endpoint's command-only +`RuntimeEndpointTarget`. + Every `ControlPartEndpoint.control_part` must be a key in `robot.control_parts`. A composite endpoint may reuse a member's control part, but all joints controlled directly by the composite must already be covered by @@ -311,6 +334,56 @@ endpoint subtype and adapter when controller semantics differ. An adapter may set `requires_command_profile=True` when a missing generic command-profile ID must make profile binding fail immediately. +The standard Expert Program simulation declaration accepts these endpoints +directly for robots that expose the normal full-state/qpos action base; a task +does not need a custom runtime factory solely to register the endpoint and Gym +transport: + +```python +profile = SimulationRobotSkillProfileBinding( + profile_id="mobile_v1", + resources=( + RobotResourceBinding( + resource_id="mobile_base", + endpoints={ + "motion": MobileVelocityEndpoint( + controller_id="base_controller", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ), + ), +) + +adapter = create_simulation_expert_program_adapter( + env, + scene_binding=scene_binding, + robot_profile_binding=profile, + endpoint_adapters={MobileVelocityEndpoint: MobileVelocityEndpointAdapter()}, + runtime_transports=(MobileVelocityGymEncoder(),), +) +``` + +`RobotResourceBinding` snapshots arbitrary typed `ResourceEndpoint` values. +`ControlPartResourceBinding` remains the stricter joint-backed convenience and +continues to validate native control parts, joint IDs, and command-preset +widths. + +Endpoint registration is not a navigation or whole-body planner. Existing +built-in semantic skills do not consume the example base/whole-body +capabilities. A reusable capability must also install its semantic descriptor +and lowerer, atomic planner, command payload, safe-state transport behavior, and +effect integration as applicable. The current standard Gym encoder composes +custom transports over a full-qpos hold and the standard simulation factory +owns a `MotionGenerator`; a truly jointless or natively structured controller +therefore needs a reusable base-action composition/provider integration. This +does not require base- or whole-body-specific fields in the generic profile or +runtime core. + +Task vertical slices may declare a typed profile binding locally while the API +stabilizes. Repeated use should move that binding into an embodiment-owned +profile catalog so new tasks select it instead of redefining robot data. + A resolved action binding is keyed only by the skill-local `(slot_id, endpoint_id)` pair. A reusable non-joint capability supplies a matching {class}`~embodichain.lab.sim.atomic_actions.RuntimeCommandPayload`, a @@ -325,11 +398,13 @@ code. ```{important} `ResourceClaim` combines leaf IDs, concrete joint IDs, and adapter claim tokens. -It and explicit disjoint constraints detect physical overlap for binding and -future scheduling work. They do not enable parallel action execution. The -runtime does not merge concurrent endpoint-command streams. Joint-backed plans -may retain a full-robot trajectory for feedback and offline compilation, but -runtime dispatch is scoped to the endpoints in each command frame. +It and explicit disjoint constraints detect physical overlap for binding. A +claim alone does not enable or prove safe parallel action execution. The +separate explicit `ParallelSkillRuntime` can coordinate disjoint branch lanes, +but it merges command frames only through an authoritative +`ParallelCommandSafetyValidator`. Joint-backed plans may retain a full-robot +trajectory for feedback and offline compilation, while runtime dispatch remains +scoped to the endpoints in each command frame. ``` See {doc}`index` for the direct atomic-action core and diff --git a/docs/source/overview/sim/index.rst b/docs/source/overview/sim/index.rst index 20d25c7a5..b63b9395a 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -139,6 +139,9 @@ Choosing Where to Start - Use :doc:`atomic_actions/robot_skill_profiles` when semantic skills should resolve robot resources and policy presets from reusable embodiment configuration. +- Use :doc:`atomic_actions/expert_programs` when a task should declare semantic + calls, settling, validation, or parallel barriers from JSON/YAML without + implementing task-local motion generation. - Use :doc:`atomic actions ` when building scripted manipulation from reusable motion primitives. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index b451fe9ce..adceebbed 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -470,6 +470,15 @@ scene snapshot; for example, ``PickUp`` automatically tracks that ID. The legacy ``ObjectSemantics.entity`` live-pose fallback is deprecated and does not create a scene dependency. +An action may give selected dependencies an exclusive waypoint cutoff through +``ActionPlan.scene_dependency_monitor_until``. A dependency is monitored while +the current waypoint index is smaller than its cutoff; ``0`` disables monitoring +from the start, and an omitted dependency remains monitored for the whole +action. Reaching the cutoff ignores every later pose change, not only motion +caused by the skill. Built-in ``PickUp`` uses ``close.start`` for the grasped +object, and ``OperateArticulation`` uses ``operate.start`` for the handle; their +physical effect monitors are authoritative after those boundaries. + Task-state effects ------------------ diff --git a/embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml b/embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml new file mode 100644 index 000000000..107236109 --- /dev/null +++ b/embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml @@ -0,0 +1,46 @@ +schema_version: 1 +program_id: repeated_cube_pick_place + +integration: + robot_profile: ur5_parallel_gripper_v1 + scene_registry: multi_segments_cube_v1 + runtime_preset: safe + +targets: + drop_pose: + kind: cyclic_pose + values: + - position: [-0.40, 0.48, 0.10] + quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + - position: [-0.42, -0.08, 0.10] + quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + +program: + kind: repeat + count: 3 + body: + kind: segment + name: move_cube + steps: + kind: sequence + items: + - kind: invoke + call: + kind: pick + object: cube + - kind: invoke + call: + kind: place + object: cube + at: + kind: target_ref + target: drop_pose + post: + - kind: wait_stable + entity: cube + preset: rigid_object + validators: + - kind: object_near_target + object: cube + target: drop_pose + position_tolerance: 0.12 diff --git a/embodichain_tasks/configs/expert_program/tableware/open_drawer.json b/embodichain_tasks/configs/expert_program/tableware/open_drawer.json new file mode 100644 index 000000000..9bd54f210 --- /dev/null +++ b/embodichain_tasks/configs/expert_program/tableware/open_drawer.json @@ -0,0 +1,23 @@ +{ + "schema_version": 1, + "program_id": "open_drawer", + "integration": { + "robot_profile": "cobot_magic_right_manipulator_v1", + "scene_registry": "open_drawer_v1", + "runtime_preset": "safe" + }, + "targets": {}, + "program": { + "kind": "segment", + "name": "open_drawer", + "steps": { + "kind": "invoke", + "call": { + "kind": "operate_articulation", + "articulation": "drawer", + "handle": "drawer_handle", + "target": "open" + } + } + } +} diff --git a/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json index 6543d8fa1..32cf15513 100644 --- a/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json +++ b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json @@ -1,5 +1,6 @@ { "id": "MultiSegmentsCubePickPlace-v1", + "expert_program_path": "../../expert_program/multi_segments/repeated_cube_pick_place.yaml", "max_episodes": 1, "max_episode_steps": 1200, "num_envs": 1, @@ -9,6 +10,24 @@ }, "env": { "sim_steps_per_control": 4, + "events": { + "settle_cube_on_reset": { + "func": "wait_for_dynamic_objects_to_settle", + "mode": "reset", + "params": { + "entity_cfgs": [ + { + "uid": "cube" + } + ], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise" + } + } + }, "dataset": { "lerobot": { "func": "LeRobotRecorder", @@ -32,20 +51,8 @@ } }, "extensions": { - "num_cycles": 3, - "place_positions": [ - [-0.40, 0.48, 0.10], - [-0.42, -0.08, 0.10] - ], "grasp_samples": 10000, - "force_reannotate": false, - "grasp_hold_steps": 45, - "settle_min_steps": 15, - "settle_max_steps": 80, - "settle_stable_steps": 5, - "linear_velocity_threshold": 0.03, - "angular_velocity_threshold": 0.20, - "place_position_tolerance": 0.12 + "force_reannotate": false } }, "robot": { diff --git a/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json b/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json index 60ab001f8..100fc9c21 100644 --- a/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json +++ b/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json @@ -1,5 +1,6 @@ { "id": "OpenDrawer-v1", + "expert_program_path": "../../expert_program/tableware/open_drawer.json", "max_episodes": 3, "max_episode_steps": 300, "env": { 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 1965563b0..6965c6f95 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -14,25 +14,46 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Repeated cube pick-and-place task using lazy demonstration segments. +"""Declarative repeated cube pick-and-place environment. -Each segment plans one complete ``PickUp -> Place -> settle`` cycle. The outer -segment generator resumes only after the previous segment has executed and its -free-falling cube has settled. Consequently, the next pickup always plans from -the cube pose currently measured in simulation instead of a pose predicted -before the episode started. +The task declares its simulation identities and robot resources, while the +packaged Expert Program defines the three semantic pick/place cycles. Shared +Expert Program components own motion generation, execution, settling, and +validation; extending the cycle count or destinations requires config only. """ from __future__ import annotations -from collections.abc import Iterable, Sequence -from functools import partial -from typing import TYPE_CHECKING, Any +from pathlib import Path +from typing import Any -import torch - -from embodichain.lab.gym.envs import DemoSegment, EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs.managers import EventCfg, SceneEntityCfg +from embodichain.lab.gym.envs.managers.events import ( + wait_for_dynamic_objects_to_settle, +) +from embodichain.lab.gym.envs.expert_program import ( + AntipodalGraspAffordanceBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramCfg, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + SimulationRigidObjectBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, + load_expert_program, +) from embodichain.lab.gym.utils.registration import register_env +from embodichain.lab.sim.atomic_actions import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + RecoveryPolicy, +) from embodichain.lab.sim.cfg import ( LightCfg, RigidBodyAttributesCfg, @@ -40,21 +61,28 @@ ) from embodichain.lab.sim.robots import URRobotCfg from embodichain.lab.sim.shapes import CubeCfg -from embodichain.utils import logger - -if TYPE_CHECKING: - from embodichain.lab.sim.atomic_actions import AtomicActionEngine, ObjectSemantics - from embodichain.lab.sim.objects import RigidObject +from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics +from embodichain.lab.sim.skills.profiles import SkillPolicyPreset +from embodichain.toolkits.graspkit.pg_grasp import ( + AntipodalSamplerCfg, + GraspGeneratorCfg, + GripperCollisionCfg, +) +from embodichain_tasks.configs import get_config_path -__all__ = ["MultiSegmentsCubePickPlaceEnv"] +__all__ = [ + "MultiSegmentsCubePickPlaceEnv", + "create_cube_robot_profile_binding", + "create_cube_scene_binding", +] CUBE_UID = "cube" CUBE_SIZE = 0.05 -DEFAULT_NUM_CYCLES = 3 -DEFAULT_GRASP_HOLD_STEPS = 45 -DEFAULT_PLACE_POSITIONS = ( - (-0.40, 0.48, 0.10), - (-0.42, -0.08, 0.10), +CUBE_SCENE_REGISTRY_ID = "multi_segments_cube_v1" +CUBE_ROBOT_PROFILE_ID = "ur5_parallel_gripper_v1" +CUBE_GRASP_AFFORDANCE_ID = "cube_antipodal_grasp" +CUBE_EXPERT_PROGRAM_PATH = Path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" ) GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" @@ -64,11 +92,12 @@ GRIPPER_FINGER_LENGTH = 0.12 GRIPPER_ROOT_Z_WIDTH = 0.096 GRIPPER_Y_THICKNESS = 0.040 -DEFAULT_GRIPPER_CLOSE_QPOS = 0.024 +GRIPPER_OPEN_QPOS = 0.0 +GRIPPER_GRASP_QPOS = 0.024 def _create_default_robot_cfg() -> URRobotCfg: - """Create the UR5 and parallel-gripper setup used by atomic-action demos.""" + """Create the UR5 scene embodiment used by the declarative task.""" return URRobotCfg.from_dict( { "robot_type": "ur5", @@ -81,19 +110,11 @@ def _create_default_robot_cfg() -> URRobotCfg: }, ], }, - "control_parts": { - "hand": [GRIPPER_HAND_JOINT_PATTERN], - }, + "control_parts": {"hand": [GRIPPER_HAND_JOINT_PATTERN]}, "drive_pros": { - "stiffness": { - GRIPPER_HAND_JOINT_PATTERN: 1e3, - }, - "damping": { - GRIPPER_HAND_JOINT_PATTERN: 1e2, - }, - "max_effort": { - GRIPPER_HAND_JOINT_PATTERN: 1e4, - }, + "stiffness": {GRIPPER_HAND_JOINT_PATTERN: 1e3}, + "damping": {GRIPPER_HAND_JOINT_PATTERN: 1e2}, + "max_effort": {GRIPPER_HAND_JOINT_PATTERN: 1e4}, }, "solver_cfg": { "arm": { @@ -110,8 +131,13 @@ def _create_default_robot_cfg() -> URRobotCfg: ) +def _load_default_expert_program() -> ExpertProgramCfg: + """Decode the packaged semantic program for direct instantiation.""" + return load_expert_program(get_config_path(CUBE_EXPERT_PROGRAM_PATH)) + + def _create_default_env_cfg() -> EmbodiedEnvCfg: - """Create a directly-instantiable default task configuration.""" + """Create a directly-instantiable task configuration.""" cfg = EmbodiedEnvCfg() cfg.max_episode_steps = 1200 cfg.robot = _create_default_robot_cfg() @@ -142,457 +168,153 @@ def _create_default_env_cfg() -> EmbodiedEnvCfg: ) ] cfg.extensions = { - "num_cycles": DEFAULT_NUM_CYCLES, - "place_positions": [list(position) for position in DEFAULT_PLACE_POSITIONS], "grasp_samples": 10000, "force_reannotate": False, - "grasp_hold_steps": DEFAULT_GRASP_HOLD_STEPS, - "settle_min_steps": 15, - "settle_max_steps": 80, - "settle_stable_steps": 5, - "linear_velocity_threshold": 0.03, - "angular_velocity_threshold": 0.20, - "place_position_tolerance": 0.12, } - return cfg - - -@register_env("MultiSegmentsCubePickPlace-v1", max_episode_steps=1200) -class MultiSegmentsCubePickPlaceEnv(EmbodiedEnv): - """Repeatedly pick up and freely place one cube. - - The demonstration planner is intentionally lazy. It yields one complete - pick/place cycle at a time, waits for that cycle to execute and settle, and - only then reads the cube pose and plans the following cycle. - """ - - PICK_SAMPLE_INTERVAL = 120 - PLACE_SAMPLE_INTERVAL = 120 - HAND_INTERP_STEPS = 12 - - def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs: Any) -> None: - if cfg is None: - cfg = _create_default_env_cfg() - - extensions = getattr(cfg, "extensions", {}) or {} - self.num_cycles = int(extensions.get("num_cycles", DEFAULT_NUM_CYCLES)) - self.place_positions = self._validate_place_positions( - extensions.get("place_positions", DEFAULT_PLACE_POSITIONS) - ) - self.grasp_samples = int(extensions.get("grasp_samples", 10000)) - self.force_reannotate = bool(extensions.get("force_reannotate", False)) - self.grasp_hold_steps = int( - extensions.get("grasp_hold_steps", DEFAULT_GRASP_HOLD_STEPS) - ) - self.settle_min_steps = int(extensions.get("settle_min_steps", 15)) - self.settle_max_steps = int(extensions.get("settle_max_steps", 80)) - self.settle_stable_steps = int(extensions.get("settle_stable_steps", 5)) - self.linear_velocity_threshold = float( - extensions.get("linear_velocity_threshold", 0.03) - ) - self.angular_velocity_threshold = float( - extensions.get("angular_velocity_threshold", 0.20) - ) - self.place_position_tolerance = float( - extensions.get("place_position_tolerance", 0.12) - ) - self._validate_settings() - - super().__init__(cfg, **kwargs) - - # ``EmbodiedEnv`` exposes extension values as instance attributes. - # Re-normalize them because that binding intentionally preserves the - # JSON-native list/scalar types supplied by the launcher. - self.num_cycles = int(self.num_cycles) - self.place_positions = self._validate_place_positions(self.place_positions) - self.grasp_samples = int(self.grasp_samples) - self.force_reannotate = bool(self.force_reannotate) - self.grasp_hold_steps = int(self.grasp_hold_steps) - self.settle_min_steps = int(self.settle_min_steps) - self.settle_max_steps = int(self.settle_max_steps) - self.settle_stable_steps = int(self.settle_stable_steps) - self.linear_velocity_threshold = float(self.linear_velocity_threshold) - self.angular_velocity_threshold = float(self.angular_velocity_threshold) - self.place_position_tolerance = float(self.place_position_tolerance) - self._validate_settings() - - cube = self.sim.get_rigid_object(CUBE_UID) - if cube is None: - raise RuntimeError(f"Task requires a rigid object with uid {CUBE_UID!r}.") - self._cube: RigidObject = cube - self._completed_cycles = 0 - self._planned_cycle_count = 0 - self._last_target_position: torch.Tensor | None = None - self._initialize_atomic_actions() - - @staticmethod - def _validate_place_positions( - positions: Sequence[Sequence[float]], - ) -> tuple[tuple[float, float, float], ...]: - """Validate and normalize release positions from task configuration.""" - normalized = tuple( - tuple(float(value) for value in position) for position in positions - ) - if not normalized or any(len(position) != 3 for position in normalized): - raise ValueError("place_positions must contain at least one XYZ position.") - return normalized - - def _validate_settings(self) -> None: - """Validate task settings before allocating a simulation.""" - if self.num_cycles < 1: - raise ValueError("num_cycles must be at least 1.") - if self.grasp_samples < 1: - raise ValueError("grasp_samples must be at least 1.") - if self.grasp_hold_steps < 0: - raise ValueError("grasp_hold_steps must be non-negative.") - if not 0 <= self.settle_min_steps <= self.settle_max_steps: - raise ValueError( - "settle_min_steps must be non-negative and no larger than " - "settle_max_steps." - ) - if self.settle_stable_steps < 1: - raise ValueError("settle_stable_steps must be at least 1.") - if self.linear_velocity_threshold < 0 or self.angular_velocity_threshold < 0: - raise ValueError("Velocity thresholds must be non-negative.") - if self.place_position_tolerance <= 0: - raise ValueError("place_position_tolerance must be positive.") - - def _initialize_atomic_actions(self) -> None: - """Create the motion generator, action engine and cube semantics.""" - from embodichain.lab.sim.atomic_actions import ( - AtomicActionEngine, - ControlPartCommandProfile, - ) - from embodichain.lab.sim.planners import ( - MotionGenCfg, - MotionGenerator, - ToppraPlannerCfg, - ) - - hand_limits = self.robot.get_qpos_limits(name="hand")[0].to( - device=self.device, dtype=torch.float32 - ) - hand_open_qpos = hand_limits[:, 0] - hand_close_qpos = torch.clamp( - torch.full_like(hand_limits[:, 1], DEFAULT_GRIPPER_CLOSE_QPOS), - min=hand_limits[:, 0], - max=hand_limits[:, 1], - ) - motion_generator = MotionGenerator( - cfg=MotionGenCfg(planner_cfg=ToppraPlannerCfg(robot_uid=self.robot.uid)) - ) - self._action_engine: AtomicActionEngine = AtomicActionEngine( - motion_generator, - control_profiles={ - "hand": ControlPartCommandProfile.joint_positions( - open=hand_open_qpos, - grasp=hand_close_qpos, - ) + cfg.events = { + "settle_cube_on_reset": EventCfg( + func=wait_for_dynamic_objects_to_settle, + mode="reset", + params={ + "entity_cfgs": [SceneEntityCfg(uid=CUBE_UID)], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise", }, ) - self._cube_semantics: ObjectSemantics = self._create_cube_semantics() + } + cfg.expert_program = _load_default_expert_program() + return cfg - def _create_cube_semantics(self) -> ObjectSemantics: - """Create reusable antipodal semantics for the task cube.""" - from embodichain.lab.sim.atomic_actions import ( - AntipodalAffordance, - ObjectSemantics, - ) - from embodichain.toolkits.graspkit.pg_grasp.antipodal_generator import ( - AntipodalSamplerCfg, - GraspGeneratorCfg, - ) - from embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker import ( - GripperCollisionCfg, - ) - vertices = self._cube.get_vertices(env_ids=[0], scale=True)[0] - triangles = self._cube.get_triangles(env_ids=[0])[0] - return ObjectSemantics( - label=CUBE_UID, - geometry={}, - affordance=AntipodalAffordance( - mesh_vertices=vertices, - mesh_triangles=triangles, - gripper_collision_cfg=GripperCollisionCfg( - max_open_length=GRIPPER_MAX_OPEN_WIDTH, - finger_length=GRIPPER_FINGER_LENGTH, - y_thickness=GRIPPER_Y_THICKNESS, - root_z_width=GRIPPER_ROOT_Z_WIDTH, - open_check_margin=0.002, - point_sample_dense=0.012, - ), +def create_cube_scene_binding( + *, + grasp_samples: int = 10000, + force_reannotate: bool = False, +) -> SimulationSceneBinding: + """Declare the cube and its exact antipodal-grasp affordance.""" + if isinstance(grasp_samples, bool) or not isinstance(grasp_samples, int): + raise TypeError("grasp_samples must be an integer.") + if grasp_samples < 1: + raise ValueError("grasp_samples must be positive.") + if not isinstance(force_reannotate, bool): + raise TypeError("force_reannotate must be a bool.") + return SimulationSceneBinding( + registry_id=CUBE_SCENE_REGISTRY_ID, + rigid_objects=( + SimulationRigidObjectBinding( + entity_id=CUBE_UID, + simulation_uid=CUBE_UID, + dynamics=SceneDynamics.DYNAMIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="cube", + default_grasp_affordance=CUBE_GRASP_AFFORDANCE_ID, + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id=CUBE_GRASP_AFFORDANCE_ID, + object_id=CUBE_UID, + native_name="cube_mesh_antipodal", + revision="cube-antipodal-v1", generator_cfg=GraspGeneratorCfg( viser_port=11801, antipodal_sampler_cfg=AntipodalSamplerCfg( - n_sample=self.grasp_samples, + n_sample=grasp_samples, max_length=GRIPPER_MAX_OPEN_WIDTH, min_length=0.005, ), is_partial_annotate=False, is_filter_ground_collision=False, ), - force_reannotate=self.force_reannotate, - ), - entity=self._cube, - ) - - def create_demo_segments( - self, *, num_cycles: int | None = None, **kwargs: Any - ) -> Iterable[DemoSegment]: - """Lazily plan repeated cube pick-and-place segments. - - Args: - num_cycles: Optional per-rollout override for the configured cycle count. - **kwargs: Reserved for future expert-planning options. - - Yields: - One :class:`DemoSegment` for every pickup/place cycle. - """ - del kwargs - cycle_count = self.num_cycles if num_cycles is None else int(num_cycles) - if cycle_count < 1: - raise ValueError("num_cycles must be at least 1.") - - self._completed_cycles = 0 - self._planned_cycle_count = cycle_count - self._last_target_position = None - for cycle_index in range(cycle_count): - target_position = torch.tensor( - self.place_positions[cycle_index % len(self.place_positions)], - dtype=torch.float32, - device=self.device, - ) - plan_success, actions, source_pose = self._plan_pick_place_cycle( - target_position - ) - self._last_target_position = target_position - source_position = source_pose[:, :3, 3].detach().cpu().tolist() - logger.log_info( - f"Planned cube pick/place segment {cycle_index + 1}/{cycle_count} " - f"from {source_position} to {target_position.detach().cpu().tolist()}." - ) - yield DemoSegment( - actions=actions, - name=f"cube_pick_place_{cycle_index + 1}", - target_uid=CUBE_UID, - instruction=( - "Pick up the cube from its current settled pose and freely " - f"place it at target {cycle_index + 1}." - ), - metadata={ - "cycle_index": cycle_index, - "cycle_count": cycle_count, - "planning_success": plan_success.detach().cpu().tolist(), - "planned_source_poses": source_pose.detach().cpu().tolist(), - "target_position": target_position.detach().cpu().tolist(), - "free_fall_settle": True, - }, - validator=partial( - self._validate_cycle, - plan_success.detach().clone(), - target_position.detach().clone(), + gripper_collision_cfg=GripperCollisionCfg( + max_open_length=GRIPPER_MAX_OPEN_WIDTH, + finger_length=GRIPPER_FINGER_LENGTH, + y_thickness=GRIPPER_Y_THICKNESS, + root_z_width=GRIPPER_ROOT_Z_WIDTH, + open_check_margin=0.002, + point_sample_dense=0.012, ), - ) - # Execution and validation happen while the generator is suspended at - # ``yield``. Advancing to the next iteration therefore means that the - # cube has already reached its new, measured scene pose. - self._completed_cycles = cycle_index + 1 + force_reannotate=force_reannotate, + ), + ), + ) - def _plan_pick_place_cycle( - self, target_position: torch.Tensor - ) -> tuple[torch.Tensor, Iterable[torch.Tensor], torch.Tensor]: - """Plan one pickup/place cycle from the cube's current measured pose.""" - from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, - GraspGoal, - MotionPolicy, - PickUpOptions, - PlaceGoal, - PlaceOptions, - ) - source_pose = self._cube.get_local_pose(to_matrix=True).to( - device=self.device, dtype=torch.float32 - ) - endpoints = { - "primary": { - "motion": "arm", - "grasp": "hand", - } +def create_cube_robot_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare the UR5 arm and parallel-gripper semantic resource.""" + motion_capabilities = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, } - pick_binding = self._action_engine.bind_control_parts( - "pick_up", - endpoints, - ) - place_binding = self._action_engine.bind_control_parts( - "place", - endpoints, - ) - pick_compiled = self._action_engine.compile( - ( - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal(self._cube_semantics), - binding=pick_binding, - motion_policy=MotionPolicy(sample_count=self.PICK_SAMPLE_INTERVAL), - skill_options=PickUpOptions( - pre_grasp_distance=0.15, - lift_height=0.16, - hand_interp_steps=self.HAND_INTERP_STEPS, + ) + return SimulationRobotSkillProfileBinding( + profile_id=CUBE_ROBOT_PROFILE_ID, + resources=( + ControlPartResourceBinding( + resource_id="manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=motion_capabilities, ), - ), - ) - ) - pick_success = pick_compiled.plan_success - pick_trajectory = pick_compiled.trajectory.positions - picked_context = pick_compiled.projected_context - held = picked_context.get_held_object("arm") - if held is None or not bool(pick_success.all().item()): - trajectory = self._ensure_nonempty_trajectory(pick_trajectory) - return ( - torch.zeros_like(pick_success, dtype=torch.bool), - self._iter_cycle_actions(trajectory, clear_dynamics_step=None), - source_pose, - ) - - pick_trajectory, clear_dynamics_step = self._insert_grasp_hold(pick_trajectory) - desired_cube_pose = source_pose.clone() - desired_cube_pose[:, :3, 3] = target_position.unsqueeze(0).expand( - self.num_envs, -1 - ) - place_eef_pose = torch.bmm(desired_cube_pose, held.object_to_eef) - place_compiled = self._action_engine.compile( - ( - ActionInvocation( - skill_id="place", - goal=PlaceGoal(place_eef_pose), - binding=place_binding, - motion_policy=MotionPolicy(sample_count=self.PLACE_SAMPLE_INTERVAL), - skill_options=PlaceOptions( - lift_height=0.14, - hand_interp_steps=self.HAND_INTERP_STEPS, + ControlPartEndpointBinding( + endpoint_id="grasp", + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="parallel_gripper", ), ), ), - picked_context, - ) - place_success = place_compiled.plan_success - place_trajectory = place_compiled.trajectory.positions - trajectory = self._ensure_nonempty_trajectory( - torch.cat((pick_trajectory, place_trajectory), dim=1) - ) - return ( - pick_success & place_success, - self._iter_cycle_actions(trajectory, clear_dynamics_step), - source_pose, - ) - - def _insert_grasp_hold( - self, pick_trajectory: torch.Tensor - ) -> tuple[torch.Tensor, int]: - """Hold the closed command at the grasp pose before beginning the lift.""" - close_end_step = min( - int(round(self.PICK_SAMPLE_INTERVAL - self.HAND_INTERP_STEPS) * 0.6) - + self.HAND_INTERP_STEPS, - pick_trajectory.shape[1], - ) - if self.grasp_hold_steps == 0: - return pick_trajectory, close_end_step - - grasp_hold = pick_trajectory[:, close_end_step - 1 : close_end_step, :].repeat( - 1, self.grasp_hold_steps, 1 - ) - augmented = torch.cat( - ( - pick_trajectory[:, :close_end_step, :], - grasp_hold, - pick_trajectory[:, close_end_step:, :], + ), + command_presets=( + ControlPartCommandPreset( + preset_id="parallel_gripper", + control_part="hand", + commands={ + "open": (GRIPPER_OPEN_QPOS,), + "grasp": (GRIPPER_GRASP_QPOS,), + }, ), - dim=1, - ) - return augmented, close_end_step + self.grasp_hold_steps - - def _ensure_nonempty_trajectory(self, trajectory: torch.Tensor) -> torch.Tensor: - """Return at least one hold command so planning failure is recordable.""" - if trajectory.shape[1] > 0: - return trajectory - return self.robot.get_qpos().clone().unsqueeze(1) - - def _iter_cycle_actions( - self, - trajectory: torch.Tensor, - clear_dynamics_step: int | None, - ) -> Iterable[torch.Tensor]: - """Replay a planned trajectory, then hold until the cube is stable.""" - for step_index, action in enumerate(trajectory.unbind(dim=1), start=1): - yield action - if clear_dynamics_step is not None and step_index == clear_dynamics_step: - # Match the pickup tutorial: clear residual object velocity just - # after gripper closure and before the lift phase. - self._cube.clear_dynamics() - - hold_action = trajectory[:, -1].clone() - stable_steps = 0 - for settle_step in range(self.settle_max_steps): - yield hold_action - if settle_step + 1 < self.settle_min_steps: - continue - if bool(self._cube_is_stable().all().item()): - stable_steps += 1 - if stable_steps >= self.settle_stable_steps: - break - else: - stable_steps = 0 + ), + defaults={ + "pick_up": {"primary": "manipulator"}, + "place": {"primary": "manipulator"}, + }, + presets=( + SkillPolicyPreset( + "safe", + recovery_policy=RecoveryPolicy(tracking_error_threshold=0.08), + ), + ), + default_preset="safe", + ) - def _cube_is_stable(self) -> torch.Tensor: - """Return whether cube linear and angular speeds are below thresholds.""" - linear_speed = torch.linalg.vector_norm(self._cube.body_data.lin_vel, dim=-1) - angular_speed = torch.linalg.vector_norm(self._cube.body_data.ang_vel, dim=-1) - return (linear_speed <= self.linear_velocity_threshold) & ( - angular_speed <= self.angular_velocity_threshold - ) - def _cube_settled_near(self, target_position: torch.Tensor) -> torch.Tensor: - """Validate that the cube settled near a release target after free fall.""" - cube_position = self._cube.get_local_pose(to_matrix=True)[:, :3, 3] - target_position = target_position.to( - device=cube_position.device, dtype=cube_position.dtype - ) - xy_error = torch.linalg.vector_norm( - cube_position[:, :2] - target_position[None, :2], dim=-1 - ) - valid_height = (cube_position[:, 2] >= -0.01) & ( - cube_position[:, 2] <= target_position[2] + CUBE_SIZE - ) - return ( - (xy_error <= self.place_position_tolerance) - & valid_height - & self._cube_is_stable() - ) +@register_env("MultiSegmentsCubePickPlace-v1", max_episode_steps=1200) +class MultiSegmentsCubePickPlaceEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + """Repeatedly pick and place a cube from a semantic config program.""" - def _validate_cycle( - self, plan_success: torch.Tensor, target_position: torch.Tensor - ) -> torch.Tensor: - """Combine motion-planning and post-free-fall validation.""" - return plan_success.to(device=self.device, dtype=torch.bool) & ( - self._cube_settled_near(target_position) + def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs: Any) -> None: + """Initialize the configured scene without task-level motion code.""" + if cfg is None: + cfg = _create_default_env_cfg() + super().__init__(cfg, **kwargs) + self._expert_program_adapter = create_simulation_expert_program_adapter( + self, + scene_binding=create_cube_scene_binding( + grasp_samples=getattr(self, "grasp_samples", 10000), + force_reannotate=getattr(self, "force_reannotate", False), + ), + robot_profile_binding=create_cube_robot_profile_binding(), ) - def is_task_success(self, **kwargs: Any) -> torch.Tensor: - """Return success after all lazy segments have executed and validated. - - Args: - **kwargs: Reserved for task-evaluation options. - - Returns: - One success flag per parallel environment. - """ - del kwargs - if ( - self._planned_cycle_count < 1 - or self._completed_cycles < self._planned_cycle_count - or self._last_target_position is None - ): - return torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) - return self._cube_settled_near(self._last_target_position) + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the shared adapter assembled for this environment.""" + return self._expert_program_adapter diff --git a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py index 3b4cbdc09..ff1166c67 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py +++ b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py @@ -14,232 +14,210 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Expert demonstration environment for opening a drawer.""" +"""Declarative expert environment for opening a sliding drawer. + +The task owns only scene and embodiment declarations. The packaged Expert +Program selects the semantic ``operate_articulation`` skill and its named +``open`` target; shared runtime components generate and execute all motion. +""" from __future__ import annotations from typing import Any -import torch - from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs.expert_program import ( + ArticulationOperationAffordanceBinding, + ArticulationOperationTargetBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + SimulationArticulationBinding, + SimulationArticulationLinkBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, +) from embodichain.lab.gym.utils.registration import register_env -from embodichain.lab.sim.planners import ( - MotionGenCfg, - MotionGenerator, - MotionGenOptions, - MoveType, - PlanResult, - PlanState, - ToppraPlannerCfg, - ToppraPlanOptions, - TrajectorySampleMethod, +from embodichain.lab.sim.atomic_actions import ( + CARTESIAN_POSE_CAPABILITY, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, +) +from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics +from embodichain.lab.sim.skills.profiles import SkillPolicyPreset + +__all__ = [ + "OpenDrawerEnv", + "create_open_drawer_robot_profile_binding", + "create_open_drawer_scene_binding", +] + +DRAWER_SCENE_REGISTRY_ID = "open_drawer_v1" +DRAWER_ROBOT_PROFILE_ID = "cobot_magic_right_manipulator_v1" +DRAWER_UID = "drawer" +DRAWER_HANDLE_LINK_ID = "drawer_handle_link" +DRAWER_HANDLE_AFFORDANCE_ID = "drawer_handle" +DRAWER_NATIVE_HANDLE_LINK = "handle_xpos" +DRAWER_NATIVE_SLIDE_JOINT = "slide_rails" +DRAWER_OPEN_POSITION = 0.11 +DRAWER_OPEN_DISPLACEMENT = 0.11 + +# Rotation from the drawer handle frame to the historical right-arm TCP frame. +_HANDLE_POSE_OFFSET = ( + -0.023958006, + -0.999453075, + -0.022793945, + 0.0, + 0.999712744, + -0.023966955, + 0.000119456, + 0.0, + -0.000665692, + -0.022784535, + 0.999740177, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, ) -from embodichain.lab.sim.utility.action_utils import interpolate_with_nums - -__all__ = ["OpenDrawerEnv"] - - -def _require_plan_positions(result: PlanResult, *, phase: str) -> torch.Tensor: - """Return a successful single-environment trajectory. - - Args: - result: Motion-planning result to validate. - phase: Human-readable planning phase for error reporting. - - Returns: - Joint positions for the task's single environment. - - Raises: - RuntimeError: If planning failed or returned no joint positions. - """ - if not result.is_all_success(): - raise RuntimeError(f"Motion planning failed during {phase}.") - if result.positions is None: - raise RuntimeError( - f"Motion planning returned no joint positions during {phase}." - ) - return result.positions[0] - - -@register_env("OpenDrawer-v1", max_episode_steps=300) -class OpenDrawerEnv(EmbodiedEnv): - """Open a sliding drawer with the right arm of a CobotMagic robot.""" - - def __init__(self, cfg: EmbodiedEnvCfg, **kwargs: Any) -> None: - """Initialize the environment and its TOPPRA motion generator. - Args: - cfg: Declarative environment configuration. - **kwargs: Additional arguments forwarded to :class:`EmbodiedEnv`. - """ - super().__init__(cfg, **kwargs) - self.motion_gen = MotionGenerator( - cfg=MotionGenCfg( - planner_cfg=ToppraPlannerCfg( - robot_uid=self.robot.uid, - ) - ) - ) - self.eef_open = self.robot.get_qpos_limits(name="right_eef")[:, :, 1] - self.eef_close = self.robot.get_qpos_limits(name="right_eef")[:, :, 0] - - def _generate_eef_motion( - self, num_steps: int = 10, *, opening: bool = True - ) -> torch.Tensor: - """Interpolate the right gripper between its closed and open limits. - - Args: - num_steps: Number of trajectory samples. - opening: Whether to open rather than close the gripper. - - Returns: - Gripper joint trajectory with shape ``(num_steps, eef_dof)``. - """ - if num_steps < 2: - raise ValueError("num_steps must be at least 2.") - - current_qpos = self.eef_close if opening else self.eef_open - target_qpos = self.eef_open if opening else self.eef_close - return interpolate_with_nums( - torch.stack([current_qpos, target_qpos], dim=1), - interp_nums=[num_steps - 1], - device=self.device, - ).squeeze(0) - - def create_demo_action_list(self, *args: Any, **kwargs: Any) -> torch.Tensor: - """Generate an expert trajectory that grasps and pulls the drawer handle. - - The demonstration is defined for the single-environment CobotMagic task - configuration and consists of four phases: move to the start pose, - approach the handle, close the gripper, and pull the drawer open. - - Returns: - Joint-position actions with shape ``(num_steps, action_dof)``. - - Raises: - ValueError: If the environment contains more than one arena. - RuntimeError: If any motion-planning phase fails. - """ - if self.num_envs != 1: - raise ValueError( - "OpenDrawerEnv expert demonstrations currently require num_envs=1." - ) - - qpos_start = torch.tensor( - [[0.0, 2.06, -0.75, 0.0, -1.20, 1.6]], - dtype=torch.float32, - device=self.device, - ) - - options_to_start = MotionGenOptions( - control_part="right_arm", - is_interpolate=True, - start_qpos=self.robot.get_qpos("right_arm")[0], - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=50, +def _translation_pose(x: float, y: float, z: float) -> tuple[float, ...]: + """Return a flattened identity-rotation pose with one translation.""" + return ( + 1.0, + 0.0, + 0.0, + x, + 0.0, + 1.0, + 0.0, + y, + 0.0, + 0.0, + 1.0, + z, + 0.0, + 0.0, + 0.0, + 1.0, + ) + + +def create_open_drawer_scene_binding() -> SimulationSceneBinding: + """Declare the exact native drawer identities used by the semantic task.""" + approach = _translation_pose(-0.00442594, -0.00050044, -0.10508996) + contact = _translation_pose(-0.00442594, -0.00050041, 0.00491005) + retract = _translation_pose(-0.00442594, -0.00050044, -0.00508996) + return SimulationSceneBinding( + registry_id=DRAWER_SCENE_REGISTRY_ID, + articulations=( + SimulationArticulationBinding( + entity_id=DRAWER_UID, + simulation_uid=DRAWER_UID, + dynamics=SceneDynamics.DYNAMIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="sliding_drawer", + default_operation_affordance=DRAWER_HANDLE_AFFORDANCE_ID, ), - ) - plan_to_start_result = self.motion_gen.generate( - target_states=[ - PlanState.single(move_type=MoveType.JOINT_MOVE, qpos=qpos_start[0]) - ], - options=options_to_start, - ) - plan_to_start = _require_plan_positions( - plan_to_start_result, phase="move to start" - ) - - xpos_begin = self.robot.compute_fk( - name="right_arm", qpos=qpos_start, to_matrix=True - )[0] - xpos_mid = xpos_begin.clone() - xpos_mid[0, 3] += 0.11 - - options_to_handle = MotionGenOptions( - control_part="right_arm", - is_interpolate=True, - is_linear=True, - start_qpos=qpos_start[0], - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=50, + ), + links=( + SimulationArticulationLinkBinding( + entity_id=DRAWER_HANDLE_LINK_ID, + articulation_id=DRAWER_UID, + native_link_name=DRAWER_NATIVE_HANDLE_LINK, + dynamics=SceneDynamics.DYNAMIC, + semantic_type="drawer_handle_link", ), - ) - plan_to_handle_result = self.motion_gen.generate( - target_states=[ - PlanState.single(move_type=MoveType.EEF_MOVE, xpos=xpos) - for xpos in (xpos_begin, xpos_mid) - ], - options=options_to_handle, - ) - plan_to_handle = _require_plan_positions( - plan_to_handle_result, phase="handle approach" - ) - - options_leave_handle = MotionGenOptions( - control_part="right_arm", - is_interpolate=True, - is_linear=True, - start_qpos=plan_to_handle[-1], - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=50, + ), + articulation_operations=( + ArticulationOperationAffordanceBinding( + entity_id=DRAWER_HANDLE_AFFORDANCE_ID, + articulation_id=DRAWER_UID, + link_id=DRAWER_HANDLE_LINK_ID, + joint_id=DRAWER_NATIVE_SLIDE_JOINT, + revision="open-drawer-v1", + semantic_targets={ + "open": ArticulationOperationTargetBinding( + target_position=DRAWER_OPEN_POSITION, + displacement=DRAWER_OPEN_DISPLACEMENT, + ), + }, + handle_pose_offset=_HANDLE_POSE_OFFSET, + approach_offset=approach, + contact_offset=contact, + operation_offset=contact, + retract_offset=retract, + operation_axis=(0.0, 0.0, -1.0), + position_scale=1.0, ), - ) - plan_leave_handle_result = self.motion_gen.generate( - target_states=[ - PlanState.single(move_type=MoveType.EEF_MOVE, xpos=xpos) - for xpos in (xpos_mid, xpos_begin) - ], - options=options_leave_handle, - ) - plan_leave_handle = _require_plan_positions( - plan_leave_handle_result, phase="drawer pull" - ) - - num_grasp_steps = 20 - eef_grasp_motion = self._generate_eef_motion( - num_steps=num_grasp_steps, opening=False - ) - - len_to_start = plan_to_start.shape[0] - len_to_handle = plan_to_handle.shape[0] - len_leave_handle = plan_leave_handle.shape[0] - total_len = len_to_start + len_to_handle + num_grasp_steps + len_leave_handle - trajectory = torch.zeros( - (total_len, self.robot.dof), - dtype=torch.float32, - device=self.device, - ) - - right_arm_ids = self.robot.get_joint_ids("right_arm") - right_eef_ids = self.robot.get_joint_ids("right_eef") - idx = 0 + ), + ) + + +def create_open_drawer_robot_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare the CobotMagic right-arm and right-gripper skill resource.""" + return SimulationRobotSkillProfileBinding( + profile_id=DRAWER_ROBOT_PROFILE_ID, + resources=( + ControlPartResourceBinding( + resource_id="right_manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="right_arm", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + ControlPartEndpointBinding( + endpoint_id="interaction", + control_part="right_eef", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="right_parallel_gripper", + ), + ), + ), + ), + command_presets=( + ControlPartCommandPreset( + preset_id="right_parallel_gripper", + control_part="right_eef", + commands={ + "open": (0.05, 0.05), + "grasp": (0.0, 0.0), + }, + ), + ), + defaults={ + "operate_articulation": {"primary": "right_manipulator"}, + }, + presets=(SkillPolicyPreset("safe"),), + default_preset="safe", + ) - trajectory[idx : idx + len_to_start, right_arm_ids] = plan_to_start - trajectory[idx : idx + len_to_start, right_eef_ids] = self._generate_eef_motion( - num_steps=len_to_start, opening=True - ) - idx += len_to_start - trajectory[idx : idx + len_to_handle, right_arm_ids] = plan_to_handle - trajectory[idx : idx + len_to_handle, right_eef_ids] = self.eef_open.expand( - len_to_handle, -1 - ) - idx += len_to_handle - - trajectory[idx : idx + num_grasp_steps, right_arm_ids] = ( - plan_to_handle[-1].unsqueeze(0).expand(num_grasp_steps, -1) - ) - trajectory[idx : idx + num_grasp_steps, right_eef_ids] = eef_grasp_motion - idx += num_grasp_steps +@register_env("OpenDrawer-v1", max_episode_steps=300) +class OpenDrawerEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + """Open a drawer through a configured semantic Expert Program.""" - trajectory[idx : idx + len_leave_handle, right_arm_ids] = plan_leave_handle - trajectory[idx : idx + len_leave_handle, right_eef_ids] = self.eef_close.expand( - len_leave_handle, -1 + def __init__(self, cfg: EmbodiedEnvCfg, **kwargs: Any) -> None: + """Initialize the configured scene without task-level motion code.""" + super().__init__(cfg, **kwargs) + self._expert_program_adapter = create_simulation_expert_program_adapter( + self, + scene_binding=create_open_drawer_scene_binding(), + robot_profile_binding=create_open_drawer_robot_profile_binding(), ) - return trajectory[:, self.active_joint_ids] + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the shared adapter assembled for this environment.""" + return self._expert_program_adapter diff --git a/scripts/benchmark/expert_program/__init__.py b/scripts/benchmark/expert_program/__init__.py new file mode 100644 index 000000000..57445d243 --- /dev/null +++ b/scripts/benchmark/expert_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. +# ---------------------------------------------------------------------------- + +"""Expert Program benchmark helpers.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/scripts/benchmark/expert_program/demo_success.py b/scripts/benchmark/expert_program/demo_success.py new file mode 100644 index 000000000..154468f3c --- /dev/null +++ b/scripts/benchmark/expert_program/demo_success.py @@ -0,0 +1,1378 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Measure Expert Program demo success without retries. + +The command line can either aggregate an existing raw artifact or construct one +real Gym environment from explicit Gym and Expert Program configurations. Live +runs execute every fixed seed once, discard every episode buffer, then reuse the +same raw JSON and three-table report pipeline as injected programmatic runs. + +Run offline: +``python -m scripts.benchmark.expert_program.demo_success --raw-json RAW`` + +Run live: +``python -m scripts.benchmark.expert_program.demo_success --run-simulation +--gym_config GYM --expert-program PROGRAM --case-id CASE --seeds 0 1 +--raw-json RAW`` +""" + +from __future__ import annotations + +import argparse +from collections import Counter, defaultdict +from collections.abc import Callable, Mapping, Sequence +from copy import deepcopy +from dataclasses import asdict, dataclass +from datetime import datetime, timezone +import json +import math +import os +from pathlib import Path +from statistics import mean +import sys +import time +from typing import Any + +import psutil +import torch +import gymnasium + +from embodichain.lab.gym.envs.demo import ( + DEMO_SCHEMA_VERSION, + DemoEpisodeResult, + execute_demo_episode, +) +from embodichain.lab.gym.envs.expert_program import load_expert_program +from embodichain.lab.gym.utils.gym_utils import ( + add_env_launcher_args_to_parser, + build_env_cfg_from_args, +) +from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, +) + +__all__ = [ + "DEMO_SUCCESS_SCHEMA_VERSION", + "DemoSuccessAggregates", + "DemoSuccessArtifacts", + "DemoSuccessCase", + "DemoSuccessRow", + "DemoSuccessTrial", + "MemorySnapshot", + "aggregate_demo_success_trials", + "capture_memory", + "collect_demo_success_trials", + "load_raw_trials", + "main", + "run_all_benchmarks", + "run_demo_success_benchmark", + "run_gym_demo_success_benchmark", + "write_markdown_report", + "write_raw_trials", +] + +DEMO_SUCCESS_SCHEMA_VERSION = 1 +_BENCHMARK_ID = "expert_program_demo_success" + +_TIME_COLUMNS = ( + "case", + "episodes", + "attempted_rows", + "cost_time_ms", + "mean_episode_ms", + "cpu_delta_mb", + "gpu_delta_mb", + "peak_gpu_mb", +) +_METRIC_COLUMNS = ( + "case", + "attempted", + "successes", + "success_rate", + "terminal_reasons", + "segment_failures", + "segment_failure_breakdown", + "call_failures", + "call_failure_breakdown", + "length_mean", + "length_min", + "length_max", +) +_LEADERBOARD_COLUMNS = ( + "rank", + "case", + "attempted", + "successes", + "overall_success_rate", + "length_mean", + "mean_episode_ms", +) + +EpisodeExecutor = Callable[..., DemoEpisodeResult] +EnvironmentProvider = Callable[["DemoSuccessCase"], Any] +MemorySampler = Callable[..., "MemorySnapshot"] +GymEnvironmentFactory = Callable[[argparse.Namespace, str | Path], Any] +EnvironmentCloser = Callable[[Any], None] + + +def _validate_nonempty_string(value: object, *, field_name: str) -> str: + """Return one exact non-empty string without outer whitespace.""" + if type(value) is not str: + raise TypeError(f"{field_name} must be a string.") + if not value or value != value.strip(): + raise ValueError(f"{field_name} must be non-empty without outer whitespace.") + return value + + +def _snapshot_string_tuple( + values: object, + *, + field_name: str, +) -> tuple[str, ...]: + """Validate and snapshot one list-or-tuple of stable string labels.""" + if type(values) not in (list, tuple): + raise TypeError(f"{field_name} must be a list or tuple.") + snapshot = tuple(values) + for index, value in enumerate(snapshot): + _validate_nonempty_string( + value, + field_name=f"{field_name}[{index}]", + ) + return snapshot + + +@dataclass(frozen=True, slots=True) +class DemoSuccessCase: + """One named demo benchmark case and its fixed evaluation seeds. + + Args: + case_id: Stable identity shown in raw artifacts and reports. + seeds: Unique seeds, each executed exactly once in the given order. + """ + + case_id: str + seeds: tuple[int, ...] + + def __post_init__(self) -> None: + _validate_nonempty_string(self.case_id, field_name="case_id") + if type(self.seeds) not in (list, tuple): + raise TypeError("seeds must be a list or tuple.") + owned_seeds = tuple(self.seeds) + if not owned_seeds: + raise ValueError("seeds must contain at least one fixed evaluation seed.") + if any(type(seed) is not int for seed in owned_seeds): + raise TypeError("Every evaluation seed must be an integer.") + if len(set(owned_seeds)) != len(owned_seeds): + raise ValueError("Evaluation seeds must be unique within a case.") + object.__setattr__(self, "seeds", owned_seeds) + + +@dataclass(frozen=True, slots=True) +class MemorySnapshot: + """Current process and PyTorch GPU memory in megabytes. + + Args: + cpu_rss_mb: Current process resident memory. + gpu_allocated_mb: Current PyTorch-allocated GPU memory. + gpu_peak_allocated_mb: Peak PyTorch GPU allocation since the last reset. + """ + + cpu_rss_mb: float + gpu_allocated_mb: float + gpu_peak_allocated_mb: float + + +@dataclass(frozen=True, slots=True) +class DemoSuccessRow: + """Normalized result for one vector-environment row. + + Args: + env_index: Zero-based row index in the vector environment. + success: Whether this row completed the episode successfully. + terminal_reason: Stable terminal-reason label. + length: Recorded row length in environment steps. + segment_failure_reasons: Segment-name-qualified failure keys. + call_failure_keys: Segment/call/status-qualified runtime failure keys. + """ + + env_index: int + success: bool + terminal_reason: str + length: int + segment_failure_reasons: tuple[str, ...] = () + call_failure_keys: tuple[str, ...] = () + + def __post_init__(self) -> None: + if type(self.env_index) is not int: + raise TypeError("env_index must be an integer.") + if self.env_index < 0: + raise ValueError("env_index must be non-negative.") + if type(self.success) is not bool: + raise TypeError("success must be a boolean.") + _validate_nonempty_string( + self.terminal_reason, + field_name="terminal_reason", + ) + if type(self.length) is not int: + raise TypeError("length must be an integer.") + if self.length < 0: + raise ValueError("length must be non-negative.") + object.__setattr__( + self, + "segment_failure_reasons", + _snapshot_string_tuple( + self.segment_failure_reasons, + field_name="segment_failure_reasons", + ), + ) + object.__setattr__( + self, + "call_failure_keys", + _snapshot_string_tuple( + self.call_failure_keys, + field_name="call_failure_keys", + ), + ) + + +@dataclass(frozen=True, slots=True) +class DemoSuccessTrial: + """Raw result for one no-retry seed execution. + + Args: + case_id: Stable benchmark case identity. + seed: Fixed seed executed exactly once. + cost_time_ms: Executor wall-clock duration in milliseconds. + cpu_delta_mb: Process RSS delta across execution. + gpu_delta_mb: PyTorch GPU allocation delta across execution. + peak_gpu_mb: Peak PyTorch GPU allocation during execution. + rows: Normalized per-environment outcomes. + episode_result: Owned JSON-compatible executor metadata. + """ + + case_id: str + seed: int + cost_time_ms: float + cpu_delta_mb: float + gpu_delta_mb: float + peak_gpu_mb: float + rows: tuple[DemoSuccessRow, ...] + episode_result: dict[str, object] + + def __post_init__(self) -> None: + _validate_nonempty_string(self.case_id, field_name="case_id") + if type(self.seed) is not int: + raise TypeError("seed must be an integer.") + numeric_fields = { + "cost_time_ms": self.cost_time_ms, + "cpu_delta_mb": self.cpu_delta_mb, + "gpu_delta_mb": self.gpu_delta_mb, + "peak_gpu_mb": self.peak_gpu_mb, + } + normalized_numeric: dict[str, float] = {} + for field_name, value in numeric_fields.items(): + if type(value) not in (int, float): + raise TypeError(f"{field_name} must be a real number.") + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + normalized_numeric[field_name] = normalized + if ( + normalized_numeric["cost_time_ms"] < 0.0 + or normalized_numeric["peak_gpu_mb"] < 0.0 + ): + raise ValueError("Elapsed time and peak GPU memory cannot be negative.") + if type(self.rows) not in (list, tuple): + raise TypeError("rows must be a list or tuple.") + owned_rows = tuple(self.rows) + if not owned_rows: + raise ValueError("A demo success trial must contain at least one row.") + if not all(type(row) is DemoSuccessRow for row in owned_rows): + raise TypeError("rows must contain exactly DemoSuccessRow values.") + env_indices = tuple(row.env_index for row in owned_rows) + if env_indices != tuple(range(len(owned_rows))): + raise ValueError( + "rows must have unique contiguous env_index values starting at zero." + ) + if type(self.episode_result) is not dict: + raise TypeError("episode_result must be a dictionary.") + owned_result = deepcopy(self.episode_result) + json.dumps(owned_result, allow_nan=False) + for field_name, value in normalized_numeric.items(): + object.__setattr__(self, field_name, value) + object.__setattr__(self, "rows", owned_rows) + object.__setattr__(self, "episode_result", owned_result) + + def to_dict(self) -> dict[str, object]: + """Return a JSON-compatible raw trial mapping. + + Returns: + An independently owned raw trial mapping. + """ + return { + "case_id": self.case_id, + "seed": self.seed, + "cost_time_ms": self.cost_time_ms, + "cpu_delta_mb": self.cpu_delta_mb, + "gpu_delta_mb": self.gpu_delta_mb, + "peak_gpu_mb": self.peak_gpu_mb, + "rows": [asdict(row) for row in self.rows], + "episode_result": deepcopy(self.episode_result), + } + + +@dataclass(frozen=True, slots=True) +class DemoSuccessAggregates: + """The three stable row sets rendered into the Markdown report. + + Args: + time_and_memory: Per-case timing and memory summaries. + success_and_metrics: Per-case success and diagnostic summaries. + leaderboard: All cases ranked by success rate. + """ + + time_and_memory: tuple[dict[str, object], ...] + success_and_metrics: tuple[dict[str, object], ...] + leaderboard: tuple[dict[str, object], ...] + + +@dataclass(frozen=True, slots=True) +class DemoSuccessArtifacts: + """Paths and in-memory results produced by one benchmark run. + + Args: + raw_json_path: Written lossless raw artifact. + report_path: Written three-table Markdown report. + trials: In-memory no-retry trials. + aggregates: In-memory report rows. + """ + + raw_json_path: Path + report_path: Path + trials: tuple[DemoSuccessTrial, ...] + aggregates: DemoSuccessAggregates + + +def capture_memory(*, reset_gpu_peak: bool = False) -> MemorySnapshot: + """Capture CPU RSS and PyTorch GPU allocation. + + Args: + reset_gpu_peak: Reset the PyTorch peak-memory counter before sampling. + + Returns: + Current CPU, GPU, and peak GPU memory in megabytes. + """ + cuda_available = torch.cuda.is_available() + if cuda_available and reset_gpu_peak: + torch.cuda.reset_peak_memory_stats() + cpu_rss_mb = psutil.Process(os.getpid()).memory_info().rss / 1024**2 + gpu_allocated_mb = ( + torch.cuda.memory_allocated() / 1024**2 if cuda_available else 0.0 + ) + gpu_peak_allocated_mb = ( + torch.cuda.max_memory_allocated() / 1024**2 if cuda_available else 0.0 + ) + return MemorySnapshot( + cpu_rss_mb=cpu_rss_mb, + gpu_allocated_mb=gpu_allocated_mb, + gpu_peak_allocated_mb=gpu_peak_allocated_mb, + ) + + +def _vector_or_default( + values: tuple[Any, ...], + *, + row_count: int, + default: Any, + field_name: str, +) -> tuple[Any, ...]: + """Return a validated per-row tuple or broadcast its scalar fallback.""" + if not values: + return tuple(default for _ in range(row_count)) + if len(values) != row_count: + raise ValueError( + f"DemoEpisodeResult.{field_name} has {len(values)} rows; " + f"expected {row_count}." + ) + return values + + +def _normalize_episode_rows(result: DemoEpisodeResult) -> tuple[DemoSuccessRow, ...]: + """Project a batched episode result into independent benchmark rows.""" + row_count = len(result.success) + if row_count == 0: + raise ValueError("DemoEpisodeResult.success must contain at least one row.") + lengths = _vector_or_default( + result.lengths, + row_count=row_count, + default=result.length, + field_name="lengths", + ) + terminal_reasons = _vector_or_default( + result.terminal_reasons, + row_count=row_count, + default=result.terminal_reason, + field_name="terminal_reasons", + ) + failures: list[list[str]] = [[] for _ in range(row_count)] + call_failures: list[list[str]] = [[] for _ in range(row_count)] + for segment in result.segments: + active = _vector_or_default( + segment.active, + row_count=row_count, + default=True, + field_name="segments.active", + ) + successes = _vector_or_default( + segment.successes, + row_count=row_count, + default=segment.success, + field_name="segments.successes", + ) + reasons = _vector_or_default( + segment.failure_reasons, + row_count=row_count, + default=segment.failure_reason, + field_name="segments.failure_reasons", + ) + for env_index in range(row_count): + if not active[env_index]: + continue + reason = reasons[env_index] + if reason is not None: + failures[env_index].append(f"{segment.name}:{reason}") + elif not successes[env_index]: + failures[env_index].append(f"{segment.name}:segment_failed") + runtime = segment.metadata.get("runtime") + if isinstance(runtime, Mapping): + _append_runtime_call_failures( + runtime, + segment_name=segment.name, + row_failures=call_failures, + ) + + return tuple( + DemoSuccessRow( + env_index=env_index, + success=bool(result.success[env_index]), + terminal_reason=str(terminal_reasons[env_index]), + length=int(lengths[env_index]), + segment_failure_reasons=tuple(failures[env_index]), + call_failure_keys=tuple(call_failures[env_index]), + ) + for env_index in range(row_count) + ) + + +def _append_runtime_call_failures( + runtime: Mapping[str, object], + *, + segment_name: str, + row_failures: list[list[str]], + branch_id: str | None = None, +) -> None: + """Attribute canonical runtime call failures to their environment rows.""" + env_ids = runtime.get("env_ids") + calls = runtime.get("calls") + if isinstance(env_ids, list) and isinstance(calls, list): + for call in calls: + if not isinstance(call, Mapping): + continue + semantic_id = call.get("semantic_id") + status = call.get("status") + masks = call.get("masks") + failed = masks.get("failed") if isinstance(masks, Mapping) else None + if ( + not isinstance(semantic_id, str) + or not isinstance(status, str) + or not isinstance(failed, list) + or len(failed) != len(env_ids) + ): + continue + identity = ( + f"{segment_name}:{semantic_id}:{status}" + if branch_id is None + else f"{segment_name}:{branch_id}:{semantic_id}:{status}" + ) + for env_id, is_failed in zip(env_ids, failed): + if ( + type(env_id) is int + and type(is_failed) is bool + and is_failed + and 0 <= env_id < len(row_failures) + ): + row_failures[env_id].append(identity) + + branches = runtime.get("branches") + if isinstance(branches, Mapping): + branch_ids = sorted(key for key in branches if isinstance(key, str)) + for child_branch_id in branch_ids: + branch_runtime = branches[child_branch_id] + if isinstance(branch_runtime, Mapping): + _append_runtime_call_failures( + branch_runtime, + segment_name=segment_name, + row_failures=row_failures, + branch_id=child_branch_id, + ) + + +def _executor_error_trial_rows(env: Any, reason: str) -> tuple[DemoSuccessRow, ...]: + """Return zero-length failed rows for one executor exception.""" + configured_rows = getattr(env, "num_envs", 1) + row_count = ( + configured_rows if type(configured_rows) is int and configured_rows > 0 else 1 + ) + return tuple( + DemoSuccessRow( + env_index=env_index, + success=False, + terminal_reason=reason, + length=0, + ) + for env_index in range(row_count) + ) + + +def _executor_error_metadata( + *, + episode_index: int, + reason: str, + error: Exception, + row_count: int, +) -> dict[str, object]: + """Return raw episode-shaped metadata that preserves one executor error.""" + return { + "schema_version": DEMO_SCHEMA_VERSION, + "episode_index": episode_index, + "length": 0, + "completed": False, + "success": [False] * row_count, + "terminated": [False] * row_count, + "truncated": [False] * row_count, + "terminal_reason": reason, + "segments": [], + "lengths": [0] * row_count, + "completed_by_env": [False] * row_count, + "terminal_reasons": [reason] * row_count, + "executor_error": { + "type": type(error).__name__, + "message": str(error), + }, + } + + +def collect_demo_success_trials( + cases: Sequence[DemoSuccessCase], + env_provider: EnvironmentProvider, + *, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, +) -> tuple[DemoSuccessTrial, ...]: + """Execute every fixed seed once and discard every resulting episode buffer. + + The caller owns environment construction and teardown. The harness performs + one non-committing seeded reset, one executor call, and one mandatory + non-committing discard reset for each seed. Executor exceptions become + failed trials only after that discard succeeds. + + Args: + cases: Named cases with fixed, unique seed sequences. + env_provider: Required environment injection. It is called once per case. + episode_executor: Demo executor, injectable for pure unit tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + + Returns: + Raw per-seed trials in case and seed order. + + Raises: + ValueError: If cases are empty, case IDs are duplicated, or an episode + result is malformed. + TypeError: If ``cases`` contains non-``DemoSuccessCase`` values. + """ + try: + case_values = tuple(cases) + except TypeError as error: + raise TypeError( + "cases must be an iterable of DemoSuccessCase values." + ) from error + if not case_values: + raise ValueError("cases must contain at least one benchmark case.") + if not all(type(case) is DemoSuccessCase for case in case_values): + raise TypeError("cases must contain exactly DemoSuccessCase values.") + case_ids = [case.case_id for case in case_values] + if len(set(case_ids)) != len(case_ids): + raise ValueError("Demo success benchmark case IDs must be unique.") + + trials: list[DemoSuccessTrial] = [] + episode_index = 0 + for case in case_values: + env = env_provider(case) + for seed in case.seeds: + env.reset(seed=seed, options={"save_data": False}) + executor_error: Exception | None = None + body_error: BaseException | None = None + try: + before = memory_sampler(reset_gpu_peak=True) + start = clock() + result: DemoEpisodeResult | None = None + try: + result = episode_executor(env, episode_index=episode_index) + except Exception as error: + executor_error = error + elapsed_ms = (clock() - start) * 1000.0 + after = memory_sampler(reset_gpu_peak=False) + except BaseException as error: + body_error = error + if executor_error is not None: + body_error.add_note( + "Episode executor also failed before benchmark measurement " + f"completed: {type(executor_error).__name__}: " + f"{executor_error}" + ) + raise + finally: + try: + env.reset(options={"save_data": False}) + except BaseException as discard_error: + discard_note = ( + "Episode discard also failed: " + f"{type(discard_error).__name__}: {discard_error}" + ) + if body_error is not None: + body_error.add_note(discard_note) + elif executor_error is not None: + executor_error.add_note(discard_note) + raise executor_error + else: + raise + + if executor_error is None: + if result is None: + raise RuntimeError("The demo episode executor returned no result.") + rows = _normalize_episode_rows(result) + episode_result = result.to_metadata() + else: + reason = f"executor_error:{type(executor_error).__name__}" + rows = _executor_error_trial_rows(env, reason) + episode_result = _executor_error_metadata( + episode_index=episode_index, + reason=reason, + error=executor_error, + row_count=len(rows), + ) + trials.append( + DemoSuccessTrial( + case_id=case.case_id, + seed=seed, + cost_time_ms=elapsed_ms, + cpu_delta_mb=after.cpu_rss_mb - before.cpu_rss_mb, + gpu_delta_mb=after.gpu_allocated_mb - before.gpu_allocated_mb, + peak_gpu_mb=after.gpu_peak_allocated_mb, + rows=rows, + episode_result=episode_result, + ) + ) + episode_index += 1 + return tuple(trials) + + +def _counter_json(counter: Counter[str]) -> str: + """Render a deterministic compact JSON counter for one Markdown cell.""" + ordered = dict(sorted(counter.items(), key=lambda item: (-item[1], item[0]))) + return json.dumps(ordered, ensure_ascii=False, separators=(",", ":")) + + +def _validate_unique_trials( + trials: Sequence[DemoSuccessTrial], +) -> tuple[DemoSuccessTrial, ...]: + """Snapshot non-empty exact trials and reject duplicate identities.""" + try: + trial_values = tuple(trials) + except TypeError as error: + raise TypeError( + "trials must be an iterable of DemoSuccessTrial values." + ) from error + if not trial_values: + raise ValueError("trials must contain at least one demo success trial.") + if not all(type(trial) is DemoSuccessTrial for trial in trial_values): + raise TypeError("trials must contain exactly DemoSuccessTrial values.") + seen: set[tuple[str, int]] = set() + for trial in trial_values: + identity = (trial.case_id, trial.seed) + if identity in seen: + raise ValueError( + "Duplicate demo success trial for " + f"case_id={trial.case_id!r}, seed={trial.seed}." + ) + seen.add(identity) + return trial_values + + +def aggregate_demo_success_trials( + trials: Sequence[DemoSuccessTrial], +) -> DemoSuccessAggregates: + """Aggregate raw trials by case and rank every represented case. + + Args: + trials: Unique case-and-seed trials. + + Returns: + Stable rows for the three report tables. + + Raises: + ValueError: If trials are empty or a case-and-seed identity occurs more + than once. + TypeError: If ``trials`` contains non-``DemoSuccessTrial`` values. + """ + trial_values = _validate_unique_trials(trials) + grouped: dict[str, list[DemoSuccessTrial]] = defaultdict(list) + for trial in trial_values: + grouped[trial.case_id].append(trial) + + time_rows: list[dict[str, object]] = [] + metric_rows: list[dict[str, object]] = [] + for case_id in sorted(grouped): + case_trials = grouped[case_id] + rows = [row for trial in case_trials for row in trial.rows] + attempted = len(rows) + successes = sum(row.success for row in rows) + lengths = [row.length for row in rows] + terminal_reasons = Counter(row.terminal_reason for row in rows) + segment_reasons = Counter( + reason for row in rows for reason in row.segment_failure_reasons + ) + call_failure_keys = Counter( + key for row in rows for key in row.call_failure_keys + ) + time_rows.append( + { + "case": case_id, + "episodes": len(case_trials), + "attempted_rows": attempted, + "cost_time_ms": sum(trial.cost_time_ms for trial in case_trials), + "mean_episode_ms": mean(trial.cost_time_ms for trial in case_trials), + "cpu_delta_mb": mean(trial.cpu_delta_mb for trial in case_trials), + "gpu_delta_mb": mean(trial.gpu_delta_mb for trial in case_trials), + "peak_gpu_mb": max(trial.peak_gpu_mb for trial in case_trials), + } + ) + metric_rows.append( + { + "case": case_id, + "attempted": attempted, + "successes": successes, + "success_rate": successes / attempted, + "terminal_reasons": _counter_json(terminal_reasons), + "segment_failures": sum(segment_reasons.values()), + "segment_failure_breakdown": _counter_json(segment_reasons), + "call_failures": sum(call_failure_keys.values()), + "call_failure_breakdown": _counter_json(call_failure_keys), + "length_mean": mean(lengths), + "length_min": min(lengths), + "length_max": max(lengths), + } + ) + + time_by_case = {str(row["case"]): row for row in time_rows} + ranked_metrics = sorted( + metric_rows, + key=lambda row: (-float(row["success_rate"]), str(row["case"])), + ) + leaderboard = tuple( + { + "rank": rank, + "case": row["case"], + "attempted": row["attempted"], + "successes": row["successes"], + "overall_success_rate": row["success_rate"], + "length_mean": row["length_mean"], + "mean_episode_ms": time_by_case[str(row["case"])]["mean_episode_ms"], + } + for rank, row in enumerate(ranked_metrics, start=1) + ) + return DemoSuccessAggregates( + time_and_memory=tuple(time_rows), + success_and_metrics=tuple(metric_rows), + leaderboard=leaderboard, + ) + + +def write_raw_trials(path: str | Path, trials: Sequence[DemoSuccessTrial]) -> Path: + """Write lossless per-seed and per-row results to one raw JSON artifact. + + Args: + path: Destination JSON path. + trials: Unique case-and-seed trials. + + Returns: + Written artifact path. + + Raises: + ValueError: If trials are empty or a case-and-seed identity occurs more + than once. + TypeError: If ``trials`` contains non-``DemoSuccessTrial`` values. + """ + trial_values = _validate_unique_trials(trials) + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + payload = { + "schema_version": DEMO_SUCCESS_SCHEMA_VERSION, + "benchmark": _BENCHMARK_ID, + "trials": [trial.to_dict() for trial in trial_values], + } + output.write_text( + json.dumps(payload, indent=2, ensure_ascii=False, allow_nan=False) + "\n", + encoding="utf-8", + ) + return output + + +def _require_mapping(value: object, field_name: str) -> Mapping[str, object]: + """Validate one raw JSON mapping boundary.""" + if not isinstance(value, Mapping): + raise ValueError(f"{field_name} must be a JSON object.") + return value + + +def _load_row(value: object, field_name: str) -> DemoSuccessRow: + """Decode one normalized row from a raw JSON trial.""" + data = _require_mapping(value, field_name) + failures = data.get("segment_failure_reasons") + if not isinstance(failures, list) or not all( + isinstance(reason, str) for reason in failures + ): + raise ValueError(f"{field_name}.segment_failure_reasons must be a string list.") + call_failures = data.get("call_failure_keys") + if not isinstance(call_failures, list) or not all( + isinstance(key, str) for key in call_failures + ): + raise ValueError(f"{field_name}.call_failure_keys must be a string list.") + env_index = data.get("env_index") + success = data.get("success") + terminal_reason = data.get("terminal_reason") + length = data.get("length") + if type(env_index) is not int or env_index < 0: + raise ValueError(f"{field_name}.env_index must be a non-negative integer.") + if type(success) is not bool: + raise ValueError(f"{field_name}.success must be a boolean.") + if not isinstance(terminal_reason, str): + raise ValueError(f"{field_name}.terminal_reason must be a string.") + if type(length) is not int or length < 0: + raise ValueError(f"{field_name}.length must be a non-negative integer.") + return DemoSuccessRow( + env_index=env_index, + success=success, + terminal_reason=terminal_reason, + length=length, + segment_failure_reasons=tuple(failures), + call_failure_keys=tuple(call_failures), + ) + + +def _required_number(data: Mapping[str, object], key: str, field_name: str) -> float: + """Read one finite raw numeric field without accepting booleans.""" + value = data.get(key) + if type(value) not in {int, float} or not math.isfinite(float(value)): + raise ValueError(f"{field_name}.{key} must be a finite number.") + return float(value) + + +def _load_trial(value: object, index: int) -> DemoSuccessTrial: + """Decode one validated trial from a raw JSON artifact.""" + field_name = f"trials[{index}]" + data = _require_mapping(value, field_name) + case_id = data.get("case_id") + seed = data.get("seed") + rows = data.get("rows") + episode_result = data.get("episode_result") + if not isinstance(case_id, str) or not case_id: + raise ValueError(f"{field_name}.case_id must be a non-empty string.") + if type(seed) is not int: + raise ValueError(f"{field_name}.seed must be an integer.") + if not isinstance(rows, list): + raise ValueError(f"{field_name}.rows must be a list.") + episode_mapping = _require_mapping(episode_result, f"{field_name}.episode_result") + return DemoSuccessTrial( + case_id=case_id, + seed=seed, + cost_time_ms=_required_number(data, "cost_time_ms", field_name), + cpu_delta_mb=_required_number(data, "cpu_delta_mb", field_name), + gpu_delta_mb=_required_number(data, "gpu_delta_mb", field_name), + peak_gpu_mb=_required_number(data, "peak_gpu_mb", field_name), + rows=tuple( + _load_row(row, f"{field_name}.rows[{i}]") for i, row in enumerate(rows) + ), + episode_result=dict(episode_mapping), + ) + + +def load_raw_trials(path: str | Path) -> tuple[DemoSuccessTrial, ...]: + """Load a raw artifact for deterministic offline re-aggregation. + + Args: + path: Existing raw JSON artifact. + + Returns: + Validated trials in artifact order. + + Raises: + ValueError: If the artifact schema is invalid or contains no valid trial. + """ + payload = json.loads(Path(path).read_text(encoding="utf-8")) + data = _require_mapping(payload, "raw benchmark") + if data.get("schema_version") != DEMO_SUCCESS_SCHEMA_VERSION: + raise ValueError( + "Unsupported demo success raw schema version: " + f"{data.get('schema_version')!r}." + ) + if data.get("benchmark") != _BENCHMARK_ID: + raise ValueError("Raw JSON is not an Expert Program demo success artifact.") + raw_trials = data.get("trials") + if not isinstance(raw_trials, list): + raise ValueError("raw benchmark.trials must be a list.") + trials = tuple(_load_trial(trial, index) for index, trial in enumerate(raw_trials)) + return _validate_unique_trials(trials) + + +def _format_value(column: str, value: object) -> str: + """Format one Markdown value deterministically.""" + if isinstance(value, float): + if column.endswith("rate"): + return f"{value:.2%}" + return f"{value:.6f}" + return str(value).replace("|", "\\|").replace("\n", " ") + + +def _format_table( + rows: Sequence[Mapping[str, object]], columns: tuple[str, ...] +) -> list[str]: + """Render one Markdown table with a stable schema.""" + lines = [ + "| " + " | ".join(columns) + " |", + "| " + " | ".join("---" for _ in columns) + " |", + ] + lines.extend( + "| " + + " | ".join(_format_value(column, row.get(column)) for column in columns) + + " |" + for row in rows + ) + return lines + + +def write_markdown_report(path: str | Path, aggregates: DemoSuccessAggregates) -> Path: + """Write exactly one report containing exactly the required three tables. + + Args: + path: Destination Markdown path. + aggregates: Rows for timing, success metrics, and leaderboard tables. + + Returns: + Written report path. + """ + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + lines = [ + "# Expert Program Demo Success Benchmark", + "", + f"Generated at: {datetime.now(timezone.utc).isoformat(timespec='seconds')}", + "", + "Each fixed seed is executed once, no failed episode is retried, and all " + "episode buffers are discarded without being committed.", + "", + "## Time & Memory", + "", + ] + lines.extend(_format_table(aggregates.time_and_memory, _TIME_COLUMNS)) + lines.extend(["", "## Success & Other Metrics", ""]) + lines.extend(_format_table(aggregates.success_and_metrics, _METRIC_COLUMNS)) + lines.extend(["", "## Leaderboard", ""]) + lines.extend(_format_table(aggregates.leaderboard, _LEADERBOARD_COLUMNS)) + output.write_text("\n".join(lines) + "\n", encoding="utf-8") + return output + + +def run_demo_success_benchmark( + cases: Sequence[DemoSuccessCase], + env_provider: EnvironmentProvider, + *, + raw_json_path: str | Path, + report_path: str | Path, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, +) -> DemoSuccessArtifacts: + """Collect no-retry trials and write one raw JSON plus one Markdown report. + + Args: + cases: Named cases with fixed, unique seed sequences. + env_provider: Environment injection called once per case. + raw_json_path: Destination for lossless trials. + report_path: Destination for the three-table report. + episode_executor: Demo executor, injectable for tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + + Returns: + Written paths, raw trials, and aggregate rows. + + Raises: + ValueError: If output paths collide or trial identities are invalid. + """ + if Path(raw_json_path).resolve() == Path(report_path).resolve(): + raise ValueError("raw_json_path and report_path must be different files.") + trials = collect_demo_success_trials( + cases, + env_provider, + episode_executor=episode_executor, + clock=clock, + memory_sampler=memory_sampler, + ) + aggregates = aggregate_demo_success_trials(trials) + raw_path = write_raw_trials(raw_json_path, trials) + markdown_path = write_markdown_report(report_path, aggregates) + return DemoSuccessArtifacts( + raw_json_path=raw_path, + report_path=markdown_path, + trials=trials, + aggregates=aggregates, + ) + + +def _create_gym_demo_success_environment( + launcher_args: argparse.Namespace, + expert_program_path: str | Path, +) -> Any: + """Create one configured Gym environment through the standard launcher APIs.""" + gym_config_path = getattr(launcher_args, "gym_config", "") + if not gym_config_path: + raise ValueError("launcher_args.gym_config must select a Gym config file.") + if getattr(launcher_args, "action_config", None) is not None: + raise ValueError( + "--action_config is not supported by the Expert Program benchmark." + ) + + discover_task_packages() + execute_init_hooks() + env_cfg, gym_config, action_config = build_env_cfg_from_args(launcher_args) + if action_config: + raise RuntimeError( + "The Expert Program benchmark environment builder produced an " + "unexpected action configuration." + ) + env_cfg.expert_program = load_expert_program(expert_program_path) + return gymnasium.make(id=gym_config["id"], cfg=env_cfg) + + +def _flush_simulation_cleanup_queue() -> None: + """Flush deferred simulation cleanup after live benchmark work.""" + from embodichain.lab.sim.sim_manager import SimulationManager + + SimulationManager.flush_cleanup_queue() + + +def _close_gym_demo_success_environment(env: Any) -> None: + """Close one benchmark environment without terminating the host process.""" + target = getattr(env, "unwrapped", env) + close = getattr(target, "close", None) + if not callable(close): + raise TypeError("Benchmark environment must expose close().") + + close_error: BaseException | None = None + try: + close(exit_process=False) + except BaseException as error: + close_error = error + + try: + _flush_simulation_cleanup_queue() + except BaseException as error: + if close_error is None: + raise + close_error.add_note( + "Simulation cleanup also failed: " f"{type(error).__name__}: {error}" + ) + if close_error is not None: + raise close_error + + +def run_gym_demo_success_benchmark( + case: DemoSuccessCase, + *, + launcher_args: argparse.Namespace, + expert_program_path: str | Path, + raw_json_path: str | Path, + report_path: str | Path, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, + environment_factory: GymEnvironmentFactory | None = None, + environment_closer: EnvironmentCloser | None = None, +) -> DemoSuccessArtifacts: + """Run one configured real-environment benchmark case and close it safely. + + One environment is constructed for the case and reused across its fixed + seeds. The shared harness performs exactly one execution per seed between + non-committing seeded and discard resets. Closing the environment is an + additional abort barrier and never commits an episode. + + Args: + case: Named case and unique fixed evaluation seeds. + launcher_args: Standard environment-launcher arguments containing the + Gym configuration path and simulation overrides. + expert_program_path: Explicit Expert Program JSON/YAML configuration. + raw_json_path: Destination for lossless per-seed results. + report_path: Destination for the three-table Markdown report. + episode_executor: Demo executor, injectable for pure tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + environment_factory: Optional environment construction override. + environment_closer: Optional deterministic close override. + + Returns: + Written artifacts and the in-memory no-retry results. + + Raises: + ValueError: If launcher inputs, output paths, or trials are invalid. + RuntimeError: If environment construction, execution, or cleanup fails. + """ + factory = environment_factory or _create_gym_demo_success_environment + closer = environment_closer or _close_gym_demo_success_environment + try: + env = factory(launcher_args, expert_program_path) + except BaseException as factory_error: + try: + _flush_simulation_cleanup_queue() + except BaseException as cleanup_error: + factory_error.add_note( + "Benchmark environment construction cleanup also failed: " + f"{type(cleanup_error).__name__}: {cleanup_error}" + ) + raise + body_error: BaseException | None = None + try: + return run_all_benchmarks( + (case,), + lambda requested_case: env, + raw_json_path=raw_json_path, + report_path=report_path, + episode_executor=episode_executor, + clock=clock, + memory_sampler=memory_sampler, + ) + except BaseException as error: + body_error = error + raise + finally: + try: + closer(env) + except BaseException as cleanup_error: + if body_error is None: + raise + body_error.add_note( + "Benchmark environment cleanup also failed: " + f"{type(cleanup_error).__name__}: {cleanup_error}" + ) + + +def run_all_benchmarks( + cases: Sequence[DemoSuccessCase], + env_provider: EnvironmentProvider, + *, + raw_json_path: str | Path, + report_path: str | Path, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, +) -> DemoSuccessArtifacts: + """Run the injected demo benchmark and print its two artifact paths. + + Args: + cases: Named cases with fixed, unique seed sequences. + env_provider: Environment injection called once per case. + raw_json_path: Destination for lossless trials. + report_path: Destination for the three-table report. + episode_executor: Demo executor, injectable for tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + + Returns: + Written paths, raw trials, and aggregate rows. + """ + print("=" * 60) + print("Expert Program Demo Success Benchmark") + print("=" * 60) + artifacts = run_demo_success_benchmark( + cases, + env_provider, + raw_json_path=raw_json_path, + report_path=report_path, + episode_executor=episode_executor, + clock=clock, + memory_sampler=memory_sampler, + ) + print(f"Raw JSON saved: {artifacts.raw_json_path}") + print(f"Markdown report saved: {artifacts.report_path}") + print("=" * 60) + print("Benchmarks complete.") + print("=" * 60) + return artifacts + + +def _build_parser() -> argparse.ArgumentParser: + """Build the offline-aggregation and live-simulation command parser.""" + parser = argparse.ArgumentParser( + description=( + "Run one fixed-seed Expert Program benchmark or aggregate an " + "existing raw JSON artifact." + ) + ) + add_env_launcher_args_to_parser(parser, require_gym_config=False) + parser.set_defaults( + num_envs=None, + renderer=None, + viser_image_fps=None, + ) + parser.add_argument( + "--run-simulation", + action="store_true", + help="Create a Gym environment and collect raw fixed-seed trials.", + ) + parser.add_argument( + "--expert-program", + type=Path, + default=None, + help="Expert Program JSON/YAML file used by --run-simulation.", + ) + parser.add_argument( + "--case-id", + type=str, + default=None, + help="Stable benchmark case identity used by --run-simulation.", + ) + parser.add_argument( + "--seeds", + type=int, + nargs="+", + default=None, + help="Unique fixed seeds, each executed exactly once in the given order.", + ) + parser.add_argument( + "--raw-json", + type=Path, + required=True, + help=( + "Raw JSON destination for --run-simulation, or an existing raw " + "artifact in offline aggregation mode." + ), + ) + parser.add_argument( + "--report", + type=Path, + default=None, + help="Output Markdown path (default: RAW with a .md suffix).", + ) + return parser + + +def _provided_option_strings(argv: Sequence[str]) -> frozenset[str]: + """Return normalized long option names explicitly present in ``argv``.""" + return frozenset(token.split("=", 1)[0] for token in argv if token.startswith("--")) + + +def _validate_cli_mode( + parser: argparse.ArgumentParser, + args: argparse.Namespace, + *, + provided_options: frozenset[str], +) -> None: + """Reject incomplete or mixed live/offline command-line inputs.""" + live_values = { + "--gym_config": args.gym_config, + "--expert-program": args.expert_program, + "--case-id": args.case_id, + "--seeds": args.seeds, + } + if args.run_simulation: + missing = [name for name, value in live_values.items() if not value] + if missing: + parser.error("--run-simulation requires " + ", ".join(missing) + ".") + if args.preview: + parser.error("--preview is not supported by --run-simulation.") + if args.action_config is not None: + parser.error("--action_config is not supported by --run-simulation.") + return + + offline_options = frozenset({"--raw-json", "--report"}) + mixed_options = sorted(provided_options - offline_options) + if mixed_options: + parser.error( + "Offline aggregation accepts only --raw-json and --report; " + "live environment options require --run-simulation: " + + ", ".join(mixed_options) + + "." + ) + + +def main(argv: Sequence[str] | None = None) -> int: + """Run live fixed-seed trials or aggregate existing raw benchmark data. + + Args: + argv: Optional command-line arguments for embedding and tests. + + Returns: + Zero after the report is written. + """ + raw_argv = tuple(sys.argv[1:] if argv is None else argv) + parser = _build_parser() + args = parser.parse_args(raw_argv) + _validate_cli_mode( + parser, + args, + provided_options=_provided_option_strings(raw_argv), + ) + report_path = args.report or args.raw_json.with_suffix(".md") + if args.raw_json.resolve() == report_path.resolve(): + raise ValueError( + "The Markdown report must not overwrite the raw JSON artifact." + ) + if args.run_simulation: + case = DemoSuccessCase( + case_id=args.case_id, + seeds=tuple(args.seeds), + ) + run_gym_demo_success_benchmark( + case, + launcher_args=args, + expert_program_path=args.expert_program, + raw_json_path=args.raw_json, + report_path=report_path, + ) + return 0 + + trials = load_raw_trials(args.raw_json) + write_markdown_report(report_path, aggregate_demo_success_trials(trials)) + print(f"Markdown report saved: {report_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/tools/expert_program_rollout_report.py b/scripts/tools/expert_program_rollout_report.py new file mode 100644 index 000000000..eeff71a75 --- /dev/null +++ b/scripts/tools/expert_program_rollout_report.py @@ -0,0 +1,503 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Render the deterministic declarative Expert Program rollout report.""" + +from __future__ import annotations + +import argparse +from collections.abc import Sequence +from dataclasses import dataclass +from pathlib import Path + +__all__ = [ + "DEFAULT_REPORT_PATH", + "REPOSITORY_ROOT", + "SourceSnapshot", + "TaskSizeMetric", + "build_task_size_metrics", + "main", + "render_report", +] + + +REPOSITORY_ROOT = Path(__file__).resolve().parents[2] +DEFAULT_REPORT_PATH = REPOSITORY_ROOT / "docs/design/expert_program_rollout_report.md" + + +@dataclass(frozen=True) +class SourceSnapshot: + """One source file included in a task migration size snapshot. + + Args: + path: Repository-relative source path. + lines: Raw LF-byte count. + bytes: Raw on-disk byte count. + """ + + path: str + lines: int + bytes: int + + +@dataclass(frozen=True) +class TaskSizeMetric: + """Baseline and current source size for one migrated task. + + Args: + task: Stable task label. + baseline_lines: Recorded pre-migration LF-byte count. + baseline_bytes: Recorded pre-migration byte count. + sources: Explicit current source snapshots. + """ + + task: str + baseline_lines: int + baseline_bytes: int + sources: tuple[SourceSnapshot, ...] + + @property + def current_lines(self) -> int: + """Return the current LF-delimited line count across all source files.""" + return sum(source.lines for source in self.sources) + + @property + def current_bytes(self) -> int: + """Return the current raw byte count across all source files.""" + return sum(source.bytes for source in self.sources) + + +@dataclass(frozen=True) +class _TaskSizeSpec: + """Stable baseline snapshot and explicit current source paths.""" + + task: str + baseline_lines: int + baseline_bytes: int + baseline_blob: str + source_paths: tuple[str, ...] + + +_TASK_SIZE_SPECS = ( + _TaskSizeSpec( + task="Cube", + baseline_lines=598, + baseline_bytes=23_912, + baseline_blob="1965563b060d1fc889f03ad13d47655c2edcd99b", + source_paths=( + "embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py", + "embodichain_tasks/configs/expert_program/multi_segments/" + "repeated_cube_pick_place.yaml", + ), + ), + _TaskSizeSpec( + task="Drawer", + baseline_lines=245, + baseline_bytes=8_833, + baseline_blob="3b4cbdc09537098b4f109d46efb8785b88f31ce1", + source_paths=( + "embodichain_tasks/embodichain_tasks/tableware/open_drawer.py", + "embodichain_tasks/configs/expert_program/tableware/open_drawer.json", + ), + ), +) + + +_FRAMEWORK_CAPABILITIES = ( + ( + "Pick + Place(at)", + "framework-tested", + "per-embodiment integration", + "Typed goals, compilation, execution, and terminal effects are covered.", + ), + ( + "Attach/release effect", + "framework-tested", + "per-embodiment integration", + "Effects use accepted commands plus live object-to-endpoint pose evidence.", + ), + ( + "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.", + ), + ( + "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.", + ), +) + + +_LANDED_INTEGRATIONS = ( + ( + "UR5", + "Cube Pick + Place", + "Pick + Place(at)", + "attach/release", + "V1 sequential", + "checked in", + "pending: one cycle passed; full three-cycle gate remains", + ), + ( + "CobotMagic", + "Open Drawer", + "OperateArticulation", + "articulation effect", + "V1 sequential", + "checked in", + "fixed-seed supported-simulation slow gate; not release-required", + ), +) + + +def _count_source(repository_root: Path, relative_path: str) -> SourceSnapshot: + """Count raw LF bytes and total bytes for one explicit repository file.""" + data = (repository_root / relative_path).read_bytes() + return SourceSnapshot( + path=relative_path, + lines=data.count(b"\n"), + bytes=len(data), + ) + + +def build_task_size_metrics( + repository_root: str | Path = REPOSITORY_ROOT, +) -> tuple[TaskSizeMetric, ...]: + """Build deterministic migration metrics from the four declared source files. + + Args: + repository_root: EmbodiChain checkout root containing the declared files. + + Returns: + Metrics in the stable order defined by the report specification. + """ + root = Path(repository_root) + return tuple( + TaskSizeMetric( + task=spec.task, + baseline_lines=spec.baseline_lines, + baseline_bytes=spec.baseline_bytes, + sources=tuple( + _count_source(root, relative_path) + for relative_path in spec.source_paths + ), + ) + for spec in _TASK_SIZE_SPECS + ) + + +def _render_table(headers: tuple[str, ...], rows: Sequence[Sequence[str]]) -> list[str]: + """Render a Markdown table with stable column and row ordering.""" + return [ + "| " + " | ".join(headers) + " |", + "| " + " | ".join("---" for _ in headers) + " |", + *("| " + " | ".join(row) + " |" for row in rows), + ] + + +def _format_delta(current: int, baseline: int) -> str: + """Format an absolute and baseline-relative size delta.""" + delta = current - baseline + percentage = delta / baseline * 100.0 + return f"{delta:+d} ({percentage:+.1f}%)" + + +def render_report(metrics: Sequence[TaskSizeMetric]) -> str: + """Render the static rollout snapshot as deterministic Markdown. + + Args: + metrics: Task size metrics, normally from :func:`build_task_size_metrics`. + + Returns: + Complete Markdown document ending with exactly one newline. + """ + if not metrics: + raise ValueError("metrics must contain at least one task snapshot.") + + metric_rows = [] + for metric in metrics: + source_paths = "
".join(f"`{source.path}`" for source in metric.sources) + metric_rows.append( + ( + metric.task, + str(metric.baseline_lines), + str(metric.current_lines), + _format_delta(metric.current_lines, metric.baseline_lines), + str(metric.baseline_bytes), + str(metric.current_bytes), + _format_delta(metric.current_bytes, metric.baseline_bytes), + source_paths, + ) + ) + + total_baseline_lines = sum(metric.baseline_lines for metric in metrics) + total_current_lines = sum(metric.current_lines for metric in metrics) + total_baseline_bytes = sum(metric.baseline_bytes for metric in metrics) + total_current_bytes = sum(metric.current_bytes for metric in metrics) + metric_rows.append( + ( + "Total", + str(total_baseline_lines), + str(total_current_lines), + _format_delta(total_current_lines, total_baseline_lines), + str(total_baseline_bytes), + str(total_current_bytes), + _format_delta(total_current_bytes, total_baseline_bytes), + "the four files above", + ) + ) + + lines = [ + "# Declarative Expert Program Rollout Report", + "", + ( + "This is a deterministic, static Phase 8 snapshot of checked-in " + "framework and integration code. It does not run simulation, report " + "physical acceptance, or certify production readiness for an embodiment." + ), + "", + "## Framework Contract Matrix", + "", + ( + "`framework-tested` describes the reusable framework contract only. A " + "task appears in the matrix below only when its integration/production " + "code is checked in; that code status does not imply physical acceptance." + ), + "", + ] + lines.extend( + _render_table( + ("Capability", "Framework status", "Integration gate", "Scope"), + _FRAMEWORK_CAPABILITIES, + ) + ) + lines.extend( + [ + "", + ( + "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." + ), + "", + ] + ) + lines.extend( + _render_table( + ( + "Embodiment", + "Task", + "Skill contract", + "Terminal effect", + "Program schema", + "Code status", + "Physical acceptance", + ), + _LANDED_INTEGRATIONS, + ) + ) + lines.extend( + [ + "", + ( + "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." + ), + "", + ( + "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." + ), + "", + "## Migration Size Snapshot", + "", + ( + "The baseline is a fixed, manually recorded pre-migration snapshot: " + "Cube is 598 lines / 23912 bytes and Drawer is 245 lines / 8833 bytes. " + "The tool does not inspect Git history. Current values are recomputed " + "only from the four explicit files in the table." + ), + "", + ( + "Baseline identity: Cube uses Git blob " + f"`{_TASK_SIZE_SPECS[0].baseline_blob}` and Drawer uses Git blob " + f"`{_TASK_SIZE_SPECS[1].baseline_blob}` at each task's Python path " + "listed in the current-source column. Blob IDs remain stable across " + "stack rebases." + ), + "", + ( + "Counting rule: `lines` is the number of raw LF (`0x0A`) bytes; " + "`bytes` is the raw on-disk byte length. Counts are summed per task " + "without normalizing encoding or line endings." + ), + "", + ] + ) + lines.extend( + _render_table( + ( + "Task", + "Baseline lines", + "Current lines", + "Line delta", + "Baseline bytes", + "Current bytes", + "Byte delta", + "Current source files", + ), + metric_rows, + ) + ) + lines.extend( + [ + "", + "## 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." + ), + "", + "## Drift Check", + "", + ( + "Regenerate the checked-in report after an intentional source or " + "capability snapshot change:" + ), + "", + "```bash", + "python scripts/tools/expert_program_rollout_report.py", + "```", + "", + "CI and local validation can reject stale output without rewriting it:", + "", + "```bash", + "python scripts/tools/expert_program_rollout_report.py --check", + "```", + ] + ) + return "\n".join(lines) + "\n" + + +def _build_parser() -> argparse.ArgumentParser: + """Create the command-line parser.""" + parser = argparse.ArgumentParser( + description="Generate or check the declarative Expert Program rollout report." + ) + parser.add_argument( + "--check", + action="store_true", + help="Fail when the output file differs from the deterministic render.", + ) + parser.add_argument( + "--output", + type=Path, + default=DEFAULT_REPORT_PATH, + help="Markdown output path (defaults to the checked-in design report).", + ) + return parser + + +def main(argv: Sequence[str] | None = None) -> int: + """Generate the rollout report or check its checked-in representation. + + Args: + argv: Optional command-line argument sequence for tests and embedding. + + Returns: + Zero on success, or one when ``--check`` detects missing or stale output. + """ + args = _build_parser().parse_args(argv) + rendered = render_report(build_task_size_metrics()) + output = args.output + + if args.check: + try: + existing = output.read_text(encoding="utf-8") + except FileNotFoundError: + print(f"rollout report is missing: {output}") + return 1 + if existing != rendered: + print(f"rollout report is stale: {output}") + return 1 + print(f"rollout report is up to date: {output}") + return 0 + + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(rendered, encoding="utf-8") + print(f"wrote rollout report: {output}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/benchmark/expert_program/__init__.py b/tests/benchmark/expert_program/__init__.py new file mode 100644 index 000000000..b1ad75924 --- /dev/null +++ b/tests/benchmark/expert_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. +# ---------------------------------------------------------------------------- + +"""Tests for Expert Program benchmarks.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/tests/benchmark/expert_program/test_demo_success.py b/tests/benchmark/expert_program/test_demo_success.py new file mode 100644 index 000000000..535941a15 --- /dev/null +++ b/tests/benchmark/expert_program/test_demo_success.py @@ -0,0 +1,971 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Pure-Python tests for the no-retry demo-success benchmark.""" + +from __future__ import annotations + +import argparse +from collections import deque +import json +from pathlib import Path + +import pytest + +from embodichain.lab.gym.envs.demo import DemoEpisodeResult, DemoSegmentResult +from scripts.benchmark.expert_program import demo_success as demo_success_module +from scripts.benchmark.expert_program.demo_success import ( + DemoSuccessCase, + DemoSuccessRow, + DemoSuccessTrial, + MemorySnapshot, + aggregate_demo_success_trials, + collect_demo_success_trials, + load_raw_trials, + main, + run_all_benchmarks, + run_gym_demo_success_benchmark, + write_markdown_report, + write_raw_trials, +) + + +class _FakeEnv: + """Record benchmark reset calls without creating a simulation.""" + + def __init__(self, num_envs: int = 1) -> None: + self.num_envs = num_envs + self.reset_calls: list[dict[str, object]] = [] + self.seed: int | None = None + + def reset(self, **kwargs: object) -> None: + self.reset_calls.append(dict(kwargs)) + if "seed" in kwargs: + self.seed = int(kwargs["seed"]) + + +class _PostEpisodeDiscardFailureEnv(_FakeEnv): + """Fail the discard reset after allowing the non-committing seed reset.""" + + def __init__(self) -> None: + super().__init__() + self.non_committing_resets = 0 + + def reset(self, **kwargs: object) -> None: + super().reset(**kwargs) + if kwargs.get("options") == {"save_data": False}: + self.non_committing_resets += 1 + if self.non_committing_resets == 2: + raise RuntimeError("synthetic discard failure") + + +class _EpisodeExecutor: + """Return queued demo results and record one call per seed.""" + + def __init__(self, results: list[DemoEpisodeResult]) -> None: + self.results = deque(results) + self.calls: list[tuple[int | None, int]] = [] + + def __call__(self, env: _FakeEnv, *, episode_index: int) -> DemoEpisodeResult: + self.calls.append((env.seed, episode_index)) + return self.results.popleft() + + +def _result( + successes: tuple[bool, ...], + *, + lengths: tuple[int, ...] | None = None, + reasons: tuple[str, ...] | None = None, + segments: tuple[DemoSegmentResult, ...] = (), +) -> DemoEpisodeResult: + """Build a compact batched result with consistent vector metadata.""" + row_count = len(successes) + row_lengths = lengths or tuple(1 for _ in successes) + row_reasons = reasons or tuple( + "success" if success else "task_incomplete" for success in successes + ) + return DemoEpisodeResult( + episode_index=0, + length=max(row_lengths), + completed=all(successes), + success=successes, + terminated=tuple(successes), + truncated=tuple(False for _ in successes), + terminal_reason="success" if all(successes) else "task_incomplete", + segments=segments, + lengths=row_lengths, + completed_by_env=successes, + terminal_reasons=row_reasons, + ) + + +def _clock(values: list[float]): + """Return a deterministic clock backed by the supplied readings.""" + readings = iter(values) + return lambda: next(readings) + + +def _memory_sampler(values: list[MemorySnapshot]): + """Return a deterministic memory sampler backed by supplied snapshots.""" + snapshots = iter(values) + + def sample(*, reset_gpu_peak: bool = False) -> MemorySnapshot: # noqa: ARG001 + return next(snapshots) + + return sample + + +def test_public_case_and_row_types_validate_and_snapshot_inputs() -> None: + seeds = [3, 5] + segment_failures = ["place:timeout"] + call_failures = ["place:place:failed"] + + case = DemoSuccessCase("cube", seeds) # type: ignore[arg-type] + row = DemoSuccessRow( + env_index=0, + success=False, + terminal_reason="timeout", + length=4, + segment_failure_reasons=segment_failures, # type: ignore[arg-type] + call_failure_keys=call_failures, # type: ignore[arg-type] + ) + seeds.append(7) + segment_failures.append("mutated") + call_failures.append("mutated") + + assert case.seeds == (3, 5) + assert row.segment_failure_reasons == ("place:timeout",) + assert row.call_failure_keys == ("place:place:failed",) + with pytest.raises(TypeError, match="case_id must be a string"): + DemoSuccessCase(7, (1,)) # type: ignore[arg-type] + with pytest.raises(TypeError, match="evaluation seed"): + DemoSuccessCase("cube", (True,)) + with pytest.raises(ValueError, match="env_index must be non-negative"): + DemoSuccessRow(-1, False, "timeout", 0) + with pytest.raises(TypeError, match="success must be a boolean"): + DemoSuccessRow(0, 1, "timeout", 0) # type: ignore[arg-type] + + +def test_public_trial_validates_rows_and_owns_nested_inputs() -> None: + row = DemoSuccessRow(0, True, "success", 2) + rows = [row] + episode_result: dict[str, object] = {"success": [True]} + + trial = DemoSuccessTrial( + case_id="cube", + seed=3, + cost_time_ms=1, + cpu_delta_mb=0, + gpu_delta_mb=0, + peak_gpu_mb=0, + rows=rows, # type: ignore[arg-type] + episode_result=episode_result, + ) + rows.clear() + episode_result["success"] = [False] + + assert trial.rows == (row,) + assert trial.cost_time_ms == 1.0 + assert trial.episode_result == {"success": [True]} + with pytest.raises(ValueError, match="unique contiguous env_index"): + DemoSuccessTrial( + "cube", + 3, + 1.0, + 0.0, + 0.0, + 0.0, + (DemoSuccessRow(1, True, "success", 1),), + {}, + ) + with pytest.raises(TypeError, match="exactly DemoSuccessRow"): + DemoSuccessTrial( + "cube", + 3, + 1.0, + 0.0, + 0.0, + 0.0, + (object(),), # type: ignore[arg-type] + {}, + ) + + +def test_each_seed_executes_once_without_retry_and_discards_data() -> None: + env = _FakeEnv() + executor = _EpisodeExecutor( + [_result((False,)), _result((True,)), _result((False,))] + ) + case = DemoSuccessCase(case_id="drawer", seeds=(11, 22, 33)) + memory_values = [MemorySnapshot(100.0, 10.0, 10.0)] * 6 + + trials = collect_demo_success_trials( + [case], + lambda requested: env, + episode_executor=executor, + clock=_clock([0.0, 0.1, 1.0, 1.2, 2.0, 2.3]), + memory_sampler=_memory_sampler(memory_values), + ) + + assert [call[0] for call in executor.calls] == [11, 22, 33] + assert len(trials) == len(case.seeds) + assert env.reset_calls == [ + {"seed": 11, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + {"seed": 22, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + {"seed": 33, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + ] + + +def test_executor_error_is_counted_and_next_seed_still_executes() -> None: + env = _FakeEnv(num_envs=2) + calls: list[int | None] = [] + + def execute( + env: _FakeEnv, *, episode_index: int + ) -> DemoEpisodeResult: # noqa: ARG001 + calls.append(env.seed) + if env.seed == 7: + raise RuntimeError("synthetic execution failure") + return _result((True, True)) + + trials = collect_demo_success_trials( + [DemoSuccessCase("drawer", (7, 8))], + lambda requested: env, + episode_executor=execute, + clock=_clock([0.0, 0.1, 1.0, 1.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 4), + ) + + assert calls == [7, 8] + assert [row.terminal_reason for row in trials[0].rows] == [ + "executor_error:RuntimeError", + "executor_error:RuntimeError", + ] + assert [row.length for row in trials[0].rows] == [0, 0] + assert trials[0].episode_result["executor_error"] == { + "type": "RuntimeError", + "message": "synthetic execution failure", + } + assert all(row.success for row in trials[1].rows) + metric = aggregate_demo_success_trials(trials).success_and_metrics[0] + assert metric["attempted"] == 4 + assert metric["successes"] == 2 + assert metric["success_rate"] == pytest.approx(0.5) + assert env.reset_calls[-1] == {"options": {"save_data": False}} + + +def test_executor_error_remains_primary_when_discard_also_fails() -> None: + env = _PostEpisodeDiscardFailureEnv() + + def execute(env: _FakeEnv, *, episode_index: int) -> DemoEpisodeResult: + del env, episode_index + raise ValueError("synthetic executor failure") + + with pytest.raises(ValueError, match="synthetic executor failure") as error: + collect_demo_success_trials( + [DemoSuccessCase("drawer", (7,))], + lambda requested: env, + episode_executor=execute, + clock=_clock([0.0, 0.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 2), + ) + + assert error.value.__notes__ == [ + "Episode discard also failed: RuntimeError: synthetic discard failure" + ] + assert env.reset_calls == [ + {"seed": 7, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + ] + + +def test_measurement_error_remains_primary_when_discard_also_fails() -> None: + env = _PostEpisodeDiscardFailureEnv() + clock_calls = 0 + + def failing_clock() -> float: + nonlocal clock_calls + clock_calls += 1 + if clock_calls == 2: + raise LookupError("synthetic clock failure") + return 0.0 + + with pytest.raises(LookupError, match="synthetic clock failure") as error: + collect_demo_success_trials( + [DemoSuccessCase("drawer", (7,))], + lambda requested: env, + episode_executor=_EpisodeExecutor([_result((True,))]), + clock=failing_clock, + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)]), + ) + + assert error.value.__notes__ == [ + "Episode discard also failed: RuntimeError: synthetic discard failure" + ] + + +def test_batched_rows_aggregate_success_reasons_failures_and_lengths() -> None: + env = _FakeEnv() + segment = DemoSegmentResult( + segment_id=0, + name="place", + start_step=0, + end_step=5, + success=False, + failure_reason="segment_validation_failed", + active=(True, True), + start_steps=(0, 0), + end_steps=(3, 5), + successes=(True, False), + failure_reasons=(None, "segment_validation_failed"), + ) + executor = _EpisodeExecutor( + [ + _result( + (True, False), + lengths=(3, 5), + reasons=("success", "segment_validation_failed"), + segments=(segment,), + ) + ] + ) + trials = collect_demo_success_trials( + [DemoSuccessCase("batched", (5,))], + lambda requested: env, + episode_executor=executor, + clock=_clock([1.0, 1.25]), + memory_sampler=_memory_sampler( + [ + MemorySnapshot(100.0, 20.0, 20.0), + MemorySnapshot(104.0, 22.0, 25.0), + ] + ), + ) + + metric = aggregate_demo_success_trials(trials).success_and_metrics[0] + + assert metric["attempted"] == 2 + assert metric["successes"] == 1 + assert metric["success_rate"] == pytest.approx(0.5) + assert json.loads(str(metric["terminal_reasons"])) == { + "segment_validation_failed": 1, + "success": 1, + } + assert metric["segment_failures"] == 1 + assert json.loads(str(metric["segment_failure_breakdown"])) == { + "place:segment_validation_failed": 1 + } + assert metric["length_mean"] == pytest.approx(4.0) + + +def test_runtime_call_failures_are_attributed_by_env_and_segment() -> None: + env = _FakeEnv(num_envs=3) + sequential = DemoSegmentResult( + segment_id=0, + name="prepare", + start_step=0, + end_step=1, + success=False, + metadata={ + "runtime": { + "kind": "skill_result", + "env_ids": [0, 1, 2], + "calls": [ + { + "semantic_id": "open", + "status": "failed", + "masks": {"failed": [True, False, False]}, + } + ], + } + }, + active=(True, True, True), + start_steps=(0, 0, 0), + end_steps=(1, 1, 1), + successes=(False, True, True), + failure_reasons=("timeout", None, None), + ) + parallel = DemoSegmentResult( + segment_id=1, + name="transfer", + start_step=1, + end_step=2, + success=False, + metadata={ + "runtime": { + "kind": "parallel_skill_result", + "branches": { + "left": { + "kind": "skill_result", + "env_ids": [0, 2], + "calls": [ + { + "semantic_id": "pick", + "status": "completed", + "masks": {"failed": [False, True]}, + } + ], + }, + "right": { + "kind": "skill_result", + "env_ids": [1], + "calls": [ + { + "semantic_id": "place", + "status": "failed", + "masks": {"failed": [True]}, + } + ], + }, + }, + } + }, + active=(True, True, True), + start_steps=(1, 1, 1), + end_steps=(2, 2, 2), + successes=(True, False, False), + failure_reasons=(None, "collision", "batch_aborted"), + ) + trials = collect_demo_success_trials( + [DemoSuccessCase("runtime", (3,))], + lambda requested: env, + episode_executor=_EpisodeExecutor( + [_result((False, False, False), segments=(sequential, parallel))] + ), + clock=_clock([0.0, 0.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 2), + ) + + metric = aggregate_demo_success_trials(trials).success_and_metrics[0] + + assert metric["call_failures"] == 3 + assert json.loads(str(metric["call_failure_breakdown"])) == { + "prepare:open:failed": 1, + "transfer:left:pick:completed": 1, + "transfer:right:place:failed": 1, + } + assert json.loads(str(metric["segment_failure_breakdown"])) == { + "prepare:timeout": 1, + "transfer:batch_aborted": 1, + "transfer:collision": 1, + } + + +def _single_trial(case_id: str, successes: tuple[bool, ...]): + """Collect one deterministic trial for ranking/report tests.""" + env = _FakeEnv() + return collect_demo_success_trials( + [DemoSuccessCase(case_id, (1,))], + lambda requested: env, + episode_executor=_EpisodeExecutor([_result(successes)]), + clock=_clock([0.0, 0.01]), + memory_sampler=_memory_sampler( + [ + MemorySnapshot(100.0, 0.0, 0.0), + MemorySnapshot(100.0, 0.0, 0.0), + ] + ), + )[0] + + +def test_leaderboard_contains_every_case_with_deterministic_tie_break() -> None: + trials = ( + _single_trial("zeta", (True, False)), + _single_trial("alpha", (True, False)), + _single_trial("winner", (True, True)), + ) + + leaderboard = aggregate_demo_success_trials(trials).leaderboard + + assert [row["case"] for row in leaderboard] == ["winner", "alpha", "zeta"] + assert [row["rank"] for row in leaderboard] == [1, 2, 3] + + +def test_report_contains_exactly_three_tables(tmp_path: Path) -> None: + trials = (_single_trial("case-a", (True,)),) + report = write_markdown_report( + tmp_path / "report.md", aggregate_demo_success_trials(trials) + ) + + text = report.read_text(encoding="utf-8") + + assert text.count("\n## ") == 3 + assert text.count("\n| ---") == 3 + assert "## Time & Memory" in text + assert "## Success & Other Metrics" in text + assert "## Leaderboard" in text + + +def test_raw_json_round_trip_preserves_trials(tmp_path: Path) -> None: + trials = (_single_trial("case-a", (True, False)),) + raw_path = write_raw_trials(tmp_path / "raw.json", trials) + + loaded = load_raw_trials(raw_path) + + assert [trial.to_dict() for trial in loaded] == [ + trial.to_dict() for trial in trials + ] + + +def test_duplicate_case_seed_is_rejected_by_aggregate_write_and_load( + tmp_path: Path, +) -> None: + trial = _single_trial("case-a", (True,)) + duplicates = (trial, trial) + + with pytest.raises(ValueError, match="Duplicate demo success trial"): + aggregate_demo_success_trials(duplicates) + with pytest.raises(ValueError, match="Duplicate demo success trial"): + write_raw_trials(tmp_path / "duplicates.json", duplicates) + + raw_path = write_raw_trials(tmp_path / "raw.json", (trial,)) + payload = json.loads(raw_path.read_text(encoding="utf-8")) + payload["trials"].append(payload["trials"][0]) + raw_path.write_text(json.dumps(payload), encoding="utf-8") + with pytest.raises(ValueError, match="Duplicate demo success trial"): + load_raw_trials(raw_path) + + +def test_zero_case_and_zero_trial_benchmarks_are_rejected(tmp_path: Path) -> None: + with pytest.raises(ValueError, match="at least one benchmark case"): + collect_demo_success_trials((), lambda requested: _FakeEnv()) + with pytest.raises(ValueError, match="at least one demo success trial"): + aggregate_demo_success_trials(()) + with pytest.raises(ValueError, match="at least one demo success trial"): + write_raw_trials(tmp_path / "empty.json", ()) + + empty_raw = tmp_path / "empty-input.json" + empty_raw.write_text( + json.dumps( + { + "schema_version": 1, + "benchmark": "expert_program_demo_success", + "trials": [], + } + ), + encoding="utf-8", + ) + with pytest.raises(ValueError, match="at least one demo success trial"): + load_raw_trials(empty_raw) + + +def test_cli_offline_mode_aggregates_existing_raw_json(tmp_path: Path) -> None: + raw_path = write_raw_trials( + tmp_path / "raw.json", (_single_trial("case-a", (True,)),) + ) + report_path = tmp_path / "offline-report.md" + + exit_code = main(["--raw-json", str(raw_path), "--report", str(report_path)]) + + assert exit_code == 0 + assert report_path.is_file() + assert len(list(tmp_path.glob("*.md"))) == 1 + + +@pytest.mark.parametrize( + "live_args", + ( + ("--preview",), + ("--action_config", "actions.json"), + ("--headless",), + ("--device", "cpu"), + ("--num_envs", "1"), + ("--renderer", "auto"), + ), +) +def test_cli_offline_mode_rejects_explicit_live_options( + tmp_path: Path, + live_args: tuple[str, ...], +) -> None: + with pytest.raises(SystemExit) as error: + main([*live_args, "--raw-json", str(tmp_path / "raw.json")]) + + assert error.value.code == 2 + + +def test_gym_runner_reuses_one_environment_and_shared_no_retry_harness( + tmp_path: Path, +) -> None: + env = _FakeEnv() + launcher_args = argparse.Namespace(gym_config="gym.json", action_config=None) + factory_calls: list[tuple[object, Path]] = [] + closed: list[object] = [] + + def environment_factory(args: object, program_path: str | Path) -> _FakeEnv: + factory_calls.append((args, Path(program_path))) + return env + + artifacts = run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3, 5)), + launcher_args=launcher_args, + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + episode_executor=_EpisodeExecutor([_result((False,)), _result((True,))]), + clock=_clock([0.0, 0.1, 1.0, 1.2]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 4), + environment_factory=environment_factory, + environment_closer=closed.append, + ) + + assert factory_calls == [(launcher_args, tmp_path / "program.yaml")] + assert closed == [env] + assert env.reset_calls == [ + {"seed": 3, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + {"seed": 5, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + ] + assert [trial.seed for trial in artifacts.trials] == [3, 5] + assert artifacts.raw_json_path.is_file() + assert artifacts.report_path.is_file() + + +def test_gym_runner_closes_environment_when_seed_reset_fails(tmp_path: Path) -> None: + class _ResetFailureEnv(_FakeEnv): + def reset(self, **kwargs: object) -> None: + super().reset(**kwargs) + if "seed" in kwargs: + raise RuntimeError("synthetic reset failure") + + env = _ResetFailureEnv() + closed: list[object] = [] + + with pytest.raises(RuntimeError, match="synthetic reset failure"): + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=lambda args, path: env, + environment_closer=closed.append, + ) + + assert closed == [env] + + +def test_gym_runner_flushes_cleanup_without_closing_when_factory_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + factory_error = LookupError("synthetic factory failure") + cleanup_calls: list[str] = [] + close_calls: list[object] = [] + + def fail_factory( + launcher_args: argparse.Namespace, + expert_program_path: str | Path, + ) -> _FakeEnv: + raise factory_error + + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: cleanup_calls.append("flush_cleanup_queue"), + ) + + with pytest.raises(LookupError, match="synthetic factory failure") as error: + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=fail_factory, + environment_closer=close_calls.append, + ) + + assert error.value is factory_error + assert cleanup_calls == ["flush_cleanup_queue"] + assert close_calls == [] + + +def test_gym_runner_preserves_factory_error_when_cleanup_flush_also_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + factory_error = LookupError("synthetic factory failure") + cleanup_calls: list[str] = [] + close_calls: list[object] = [] + + def fail_factory( + launcher_args: argparse.Namespace, + expert_program_path: str | Path, + ) -> _FakeEnv: + raise factory_error + + def fail_cleanup() -> None: + cleanup_calls.append("flush_cleanup_queue") + raise RuntimeError("synthetic cleanup failure") + + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + fail_cleanup, + ) + + with pytest.raises(LookupError, match="synthetic factory failure") as error: + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=fail_factory, + environment_closer=close_calls.append, + ) + + assert error.value is factory_error + assert error.value.__notes__ == [ + "Benchmark environment construction cleanup also failed: " + "RuntimeError: synthetic cleanup failure" + ] + assert cleanup_calls == ["flush_cleanup_queue"] + assert close_calls == [] + + +def test_default_gym_environment_closer_uses_unwrapped_target_and_flushes_cleanup( + monkeypatch: pytest.MonkeyPatch, +) -> None: + calls: list[object] = [] + + class _UnwrappedEnv: + def close(self, *, exit_process: bool) -> None: + calls.append(("close", exit_process)) + + env = argparse.Namespace(unwrapped=_UnwrappedEnv()) + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: calls.append("flush_cleanup_queue"), + ) + + demo_success_module._close_gym_demo_success_environment(env) + + assert calls == [("close", False), "flush_cleanup_queue"] + + +def test_gym_runner_preserves_body_error_when_default_close_also_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + close_calls: list[bool] = [] + cleanup_calls: list[str] = [] + + class _CloseFailureTarget: + def close(self, *, exit_process: bool) -> None: + close_calls.append(exit_process) + raise RuntimeError("synthetic close failure") + + class _BodyFailureEnv(_FakeEnv): + def __init__(self) -> None: + super().__init__() + self.unwrapped = _CloseFailureTarget() + + def reset(self, **kwargs: object) -> None: + super().reset(**kwargs) + if "seed" in kwargs: + raise LookupError("synthetic benchmark body failure") + + env = _BodyFailureEnv() + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: cleanup_calls.append("flush_cleanup_queue"), + ) + + with pytest.raises(LookupError, match="synthetic benchmark body failure") as error: + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=lambda args, path: env, + ) + + assert error.value.__notes__ == [ + "Benchmark environment cleanup also failed: " + "RuntimeError: synthetic close failure" + ] + assert close_calls == [False] + assert cleanup_calls == ["flush_cleanup_queue"] + + +def test_gym_environment_builder_uses_standard_public_config_pipeline( + monkeypatch: pytest.MonkeyPatch, +) -> None: + calls: list[object] = [] + launcher_args = argparse.Namespace( + gym_config="gym.json", + action_config=None, + ) + env_cfg = argparse.Namespace(expert_program=None) + program = object() + env = object() + + monkeypatch.setattr( + demo_success_module, + "discover_task_packages", + lambda: calls.append("discover"), + ) + monkeypatch.setattr( + demo_success_module, + "execute_init_hooks", + lambda: calls.append("hooks"), + ) + + def build(args: argparse.Namespace): + calls.append(("build", args)) + return env_cfg, {"id": "ExpertTask-v1"}, {} + + monkeypatch.setattr(demo_success_module, "build_env_cfg_from_args", build) + monkeypatch.setattr( + demo_success_module, + "load_expert_program", + lambda path: calls.append(("load", path)) or program, + ) + monkeypatch.setattr( + demo_success_module.gymnasium, + "make", + lambda **kwargs: calls.append(("make", kwargs)) or env, + ) + + created = demo_success_module._create_gym_demo_success_environment( + launcher_args, + "program.yaml", + ) + + assert created is env + assert env_cfg.expert_program is program + assert calls == [ + "discover", + "hooks", + ("build", launcher_args), + ("load", "program.yaml"), + ("make", {"id": "ExpertTask-v1", "cfg": env_cfg}), + ] + + +@pytest.mark.parametrize( + "unsupported", + ( + ("--preview",), + ("--action_config", "actions.json"), + ), +) +def test_cli_live_mode_rejects_unsupported_launcher_options( + tmp_path: Path, + unsupported: tuple[str, ...], +) -> None: + with pytest.raises(SystemExit) as error: + main( + [ + "--run-simulation", + "--gym_config", + "gym.json", + "--expert-program", + "program.yaml", + "--case-id", + "cube", + "--seeds", + "7", + "--raw-json", + str(tmp_path / "raw.json"), + *unsupported, + ] + ) + + assert error.value.code == 2 + + +def test_cli_live_mode_dispatches_fixed_seed_case( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + captured: dict[str, object] = {} + + def run(case: DemoSuccessCase, **kwargs: object) -> object: + captured["case"] = case + captured.update(kwargs) + return object() + + monkeypatch.setattr( + demo_success_module, + "run_gym_demo_success_benchmark", + run, + ) + raw_path = tmp_path / "raw.json" + + exit_code = main( + [ + "--run-simulation", + "--gym_config", + "gym.json", + "--expert-program", + "program.yaml", + "--case-id", + "cube", + "--seeds", + "7", + "11", + "--raw-json", + str(raw_path), + ] + ) + + assert exit_code == 0 + assert captured["case"] == DemoSuccessCase("cube", (7, 11)) + assert captured["expert_program_path"] == Path("program.yaml") + assert captured["raw_json_path"] == raw_path + assert captured["report_path"] == raw_path.with_suffix(".md") + launcher_args = captured["launcher_args"] + assert isinstance(launcher_args, argparse.Namespace) + assert launcher_args.gym_config == "gym.json" + assert launcher_args.num_envs is None + assert launcher_args.renderer is None + + +def test_run_all_benchmarks_prints_report_path( + tmp_path: Path, capsys: pytest.CaptureFixture[str] +) -> None: + env = _FakeEnv() + report_path = tmp_path / "report.md" + + artifacts = run_all_benchmarks( + [DemoSuccessCase("case-a", (1,))], + lambda requested: env, + raw_json_path=tmp_path / "raw.json", + report_path=report_path, + episode_executor=_EpisodeExecutor([_result((True,))]), + clock=_clock([0.0, 0.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 2), + ) + + assert artifacts.report_path == report_path + assert f"Markdown report saved: {report_path}" in capsys.readouterr().out diff --git a/tests/benchmark/expert_program/test_demo_success_open_drawer_sim.py b/tests/benchmark/expert_program/test_demo_success_open_drawer_sim.py new file mode 100644 index 000000000..c3af1a94c --- /dev/null +++ b/tests/benchmark/expert_program/test_demo_success_open_drawer_sim.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. +# ---------------------------------------------------------------------------- + +"""Live OpenDrawer regression coverage for the Expert Program benchmark.""" + +from __future__ import annotations + +import json +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] +_OPEN_DRAWER_GYM_CONFIG = get_config_path("gym/open_drawer/cobot_magic_3cam.json") +_OPEN_DRAWER_EXPERT_PROGRAM = get_config_path( + "expert_program/tableware/open_drawer.json" +) +_CASE_ID = "open_drawer_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 _write_headless_cpu_gym_config(tmp_path: Path) -> Path: + """Write a camera-free copy of the packaged live-physics configuration.""" + payload = json.loads(_OPEN_DRAWER_GYM_CONFIG.read_text(encoding="utf-8")) + if type(payload) is not dict: + raise TypeError("The packaged OpenDrawer Gym config must be a JSON object.") + env_config = payload.get("env") + if type(env_config) is not dict: + raise TypeError("The packaged OpenDrawer env config must be a JSON object.") + + # Cameras and their recording event are orthogonal to drawer physics and make + # this CPU regression unnecessarily renderer-sensitive. + payload["sensor"] = [] + env_config["events"] = {} + env_config["observations"] = {} + env_config["dataset"] = {} + payload["expert_program_path"] = str(_OPEN_DRAWER_EXPERT_PROGRAM) + + output = tmp_path / "open_drawer_headless_cpu.json" + output.write_text( + json.dumps(payload, indent=2, ensure_ascii=False) + "\n", + encoding="utf-8", + ) + return output + + +@pytest.mark.requires_sim +@pytest.mark.slow +def test_live_open_drawer_benchmark_writes_successful_decodable_artifacts( + tmp_path: Path, +) -> None: + """Run one no-retry seed through the public live benchmark entry point.""" + gym_config_path = _write_headless_cpu_gym_config(tmp_path) + raw_path = tmp_path / "open_drawer_raw.json" + report_path = tmp_path / "open_drawer_report.md" + completed = subprocess.run( + [ + sys.executable, + "-c", + _RUN_PUBLIC_MAIN, + "--run-simulation", + "--gym_config", + str(gym_config_path), + "--expert-program", + str(_OPEN_DRAWER_EXPERT_PROGRAM), + "--case-id", + _CASE_ID, + "--seeds", + str(_SEED), + "--raw-json", + str(raw_path), + "--report", + str(report_path), + "--headless", + "--device", + "cpu", + "--num_envs", + str(_NUM_ENVS), + "--filter_dataset_saving", + ], + cwd=_REPOSITORY_ROOT, + capture_output=True, + text=True, + timeout=_SUBPROCESS_TIMEOUT_SECONDS, + check=False, + ) + + # main() returns zero only after the live runner's default closer completes; + # the process boundary also isolates native simulator teardown from pytest. + assert completed.returncode == 0, completed.stdout + completed.stderr + assert f"Raw JSON saved: {raw_path}" in completed.stdout + assert f"Markdown report saved: {report_path}" in completed.stdout + + 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 len(trial.rows) == _NUM_ENVS + row = trial.rows[0] + assert row.success + assert row.terminal_reason == "success" + assert row.length > 0 + + segments = trial.episode_result["segments"] + assert isinstance(segments, list) + assert len(segments) == 1 + segment = segments[0] + assert isinstance(segment, dict) + assert segment["name"] == "open_drawer" + runtime = segment["metadata"]["runtime"] + assert runtime["kind"] == "skill_result" + assert runtime["status"] == "completed" + calls = runtime["calls"] + assert isinstance(calls, list) + assert len(calls) == 1 + call = calls[0] + assert call["semantic_id"] == "operate_articulation" + assert call["status"] == "completed" + effects = call["effects"] + assert isinstance(effects, list) + assert effects + for effect in effects: + evidence = effect["evidence"]["joint.position"] + assert evidence["valid_mask"] == [True] + assert evidence["acquisition_errors"] == [None] + + aggregates = aggregate_demo_success_trials(decoded_trials) + assert len(aggregates.success_and_metrics) == 1 + metrics = aggregates.success_and_metrics[0] + assert metrics["attempted"] == 1 + assert metrics["successes"] == 1 + assert metrics["success_rate"] == 1.0 + + report = report_path.read_text(encoding="utf-8") + assert report.count("\n## ") == 3 + assert "## Success & Other Metrics" in report + assert "## Leaderboard" in report + assert _CASE_ID in report diff --git a/tests/gym/envs/expert_program/test_task_vertical_slices.py b/tests/gym/envs/expert_program/test_task_vertical_slices.py new file mode 100644 index 000000000..af67f89e3 --- /dev/null +++ b/tests/gym/envs/expert_program/test_task_vertical_slices.py @@ -0,0 +1,625 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Configuration and non-physical bridge vertical slices for Expert Programs.""" + +from __future__ import annotations + +from copy import deepcopy +import json +from pathlib import Path + +import pytest +import torch +import yaml + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramCompiler, + decode_expert_program, +) +from embodichain.lab.gym.envs.expert_program.bridge import ( + AtomicDemoBridge, + BufferedGymCommandSink, + EnvironmentStepClock, + RuntimeCommandFrameEncoder, +) +from embodichain.lab.sim.atomic_actions import Affordance, EntityState, TaskState +from embodichain.lab.sim.skills.calls import OperateArticulation, Pick, Place +from embodichain.lab.sim.skills.runtime import SkillResult, SkillStatus +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) +from embodichain_tasks.configs import get_config_path +from embodichain_tasks.multi_segments import cube_pick_place as cube_task +from embodichain_tasks.tableware import open_drawer as drawer_task + +_REPEATED_CUBE_PROGRAM = Path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" +) +_OPEN_DRAWER_PROGRAM = Path("expert_program/tableware/open_drawer.json") +_LIFECYCLE_BATCH_SIZE = 2 +_LIFECYCLE_ROBOT_DOF = 3 +_LIFECYCLE_STEP_DT = 0.02 + + +class _NeverObserveProvider: + """Reject dynamic observations during configuration decoding/compilation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + raise AssertionError("Task configuration compilation must not observe state.") + + +class _FixedQposProvider: + """Return a finite full-qpos hold for the bridge's unused command sink.""" + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + return torch.zeros( + (env_ids.numel(), _LIFECYCLE_ROBOT_DOF), + dtype=torch.float32, + device=env_ids.device, + ) + + +class _FreshObservationPort: + """Issue one distinct observation generation for every segment runtime.""" + + def __init__(self) -> None: + self.generations: list[int] = [] + + def capture(self) -> int: + generation = len(self.generations) + 1 + self.generations.append(generation) + return generation + + +class _CompletedSegmentRuntime: + """Complete each semantic prefix from one freshly captured observation.""" + + def __init__( + self, + observation: _FreshObservationPort, + lifecycle_events: list[tuple[str, int]], + ) -> None: + self._observation = observation + self._lifecycle_events = lifecycle_events + self._status = SkillStatus.IDLE + self._result = self._make_result( + status=SkillStatus.IDLE, + workflow_id=None, + eligible_mask=torch.ones(_LIFECYCLE_BATCH_SIZE, dtype=torch.bool), + generation=0, + ) + self.analysis_window_lengths: list[int] = [] + self.executed_semantic_ids: list[str] = [] + self.eligible_masks: list[torch.Tensor | None] = [] + + @staticmethod + def _make_result( + *, + status: SkillStatus, + workflow_id: str | None, + eligible_mask: torch.Tensor, + generation: int, + ) -> SkillResult: + terminal = status is SkillStatus.COMPLETED + return SkillResult( + status=status, + workflow_id=workflow_id, + current_call_index=None, + env_ids=torch.arange(_LIFECYCLE_BATCH_SIZE, dtype=torch.long), + success_mask=( + eligible_mask.clone() if terminal else torch.zeros_like(eligible_mask) + ), + failure_mask=torch.zeros_like(eligible_mask), + cancelled_mask=torch.zeros_like(eligible_mask), + eligible_mask=eligible_mask, + task_state=TaskState.empty(_LIFECYCLE_BATCH_SIZE, "cpu"), + message=f"observation_generation={generation}", + ) + + @property + def result(self) -> SkillResult: + return self._result + + @property + def status(self) -> SkillStatus: + return self._status + + def start( + self, + *calls: object, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + call_values = tuple(calls[0]) if len(calls) == 1 else tuple(calls) + if execution_prefix_length is None: + raise AssertionError("A packaged sequential segment requires a prefix.") + selected = ( + torch.ones(_LIFECYCLE_BATCH_SIZE, dtype=torch.bool) + if eligible_mask is None + else eligible_mask.clone() + ) + execution_calls = call_values[:execution_prefix_length] + generation = self._observation.capture() + self._lifecycle_events.append(("observe", generation)) + self.analysis_window_lengths.append(len(call_values)) + self.executed_semantic_ids.extend( + str(getattr(call, "semantic_id")) for call in execution_calls + ) + self.eligible_masks.append( + None if eligible_mask is None else eligible_mask.clone() + ) + self._status = SkillStatus.COMPLETED + self._result = self._make_result( + status=SkillStatus.COMPLETED, + workflow_id=workflow_id, + eligible_mask=selected, + generation=generation, + ) + return self._result + + def step(self) -> SkillResult: + raise AssertionError("A terminal fake runtime must not be stepped.") + + def cancel(self, reason: str) -> SkillResult: + raise AssertionError(f"A completed fake runtime cannot be cancelled: {reason}") + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + del task_state + return self._result + + +class _LifecyclePostPolicyPort: + """Run every packaged settle policy and expose deterministic metadata.""" + + def __init__( + self, + observation: _FreshObservationPort, + lifecycle_events: list[tuple[str, int]], + ) -> None: + self._observation = observation + self._lifecycle_events = lifecycle_events + self.active_masks: list[torch.Tensor] = [] + self._metadata: dict[int, dict[str, object]] = {} + + def validate_policy(self, policy: object, *, segment: object) -> None: + del policy, segment + + def actions( + self, + policy: object, + *, + segment: object, + active_mask: torch.Tensor, + ): + segment_index = int(getattr(segment, "segment_index")) + generation = self._observation.generations[-1] + self._lifecycle_events.append(("settle", segment_index)) + self.active_masks.append(active_mask.clone()) + self._metadata[id(policy)] = { + "status": "settled", + "segment_index": segment_index, + "observation_generation": generation, + } + yield torch.zeros( + (_LIFECYCLE_BATCH_SIZE, _LIFECYCLE_ROBOT_DOF), + dtype=torch.float32, + ) + + def post_policy_result( + self, + policy: object, + *, + segment: object, + ) -> torch.Tensor: + del policy, segment + return self.active_masks[-1].clone() + + def post_policy_metadata( + self, + policy: object, + *, + segment: object, + ) -> dict[str, object]: + del segment + return dict(self._metadata[id(policy)]) + + +class _LifecycleValidatorPort: + """Validate every segment and filter one row after the first cycle.""" + + def __init__( + self, + observation: _FreshObservationPort, + lifecycle_events: list[tuple[str, int]], + ) -> None: + self._observation = observation + self._lifecycle_events = lifecycle_events + self._metadata: dict[int, dict[str, object]] = {} + + def validate_validator(self, validator: object, *, segment: object) -> None: + del validator, segment + + def validate(self, validator: object, *, segment: object) -> torch.Tensor: + segment_index = int(getattr(segment, "segment_index")) + generation = self._observation.generations[-1] + self._lifecycle_events.append(("validate", segment_index)) + result = ( + torch.tensor([True, False]) + if segment_index == 0 + else torch.ones(_LIFECYCLE_BATCH_SIZE, dtype=torch.bool) + ) + self._metadata[id(validator)] = { + "segment_index": segment_index, + "observation_generation": generation, + "accepted_mask": result.tolist(), + } + return result + + def validator_metadata( + self, + validator: object, + *, + segment: object, + ) -> dict[str, object]: + del segment + return dict(self._metadata[id(validator)]) + + +def _read_payload(relative_path: Path) -> dict[str, object]: + """Load one packaged JSON/YAML example as inert data.""" + path = get_config_path(relative_path) + if path.suffix == ".json": + payload = json.loads(path.read_text(encoding="utf-8")) + else: + payload = yaml.safe_load(path.read_text(encoding="utf-8")) + assert type(payload) is dict + return payload + + +def _cube_compiler() -> ExpertProgramCompiler: + """Build the smallest typed identity registry needed by the cube program.""" + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_NeverObserveProvider(), + ), + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def _drawer_compiler() -> ExpertProgramCompiler: + """Build typed drawer and handle identities without any motion code.""" + provider = _NeverObserveProvider() + drawer = SceneArticulationRef("drawer") + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=drawer, state_provider=provider), + SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + parent=drawer, + native_name="handle_xpos", + affordance=Affordance(), + relative_pose=torch.eye(4), + ), + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def test_repeated_cube_program_is_three_lazy_semantic_segments() -> None: + """The packaged cube task expands to three independently scoped cycles.""" + config = decode_expert_program(_read_payload(_REPEATED_CUBE_PROGRAM)) + + assert config.integration.scene_registry == cube_task.CUBE_SCENE_REGISTRY_ID + assert config.integration.robot_profile == cube_task.CUBE_ROBOT_PROFILE_ID + + segments = tuple(_cube_compiler().compile(config)) + + assert [segment.name for segment in segments] == ["move_cube"] * 3 + assert [segment.segment_index for segment in segments] == [0, 1, 2] + assert [len(segment.calls) for segment in segments] == [2, 2, 2] + assert all(type(segment.calls[0].call) is Pick for segment in segments) + assert all(type(segment.calls[1].call) is Place for segment in segments) + assert [ + segment.calls[1].target_selections[0].value_index for segment in segments + ] == [0, 1, 0] + assert [ + segment.validators[0].target_selection.value_index for segment in segments + ] == [ + 0, + 1, + 0, + ] + assert all( + segment.post_policies[0].cfg.kind == "wait_stable" for segment in segments + ) + assert all( + segment.validators[0].cfg.position_tolerance == 0.12 for segment in segments + ) + + +def test_packaged_repeated_cube_runs_three_lazy_bridge_lifecycles() -> None: + """The real packaged program owns three ordered observable lifecycles.""" + config = decode_expert_program(_read_payload(_REPEATED_CUBE_PROGRAM)) + compiled = _cube_compiler().compile(config).materialize() + lifecycle_events: list[tuple[str, int]] = [] + observation = _FreshObservationPort() + clock = EnvironmentStepClock(_LIFECYCLE_STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_FixedQposProvider()), + clock, + ) + runtime = _CompletedSegmentRuntime(observation, lifecycle_events) + post_port = _LifecyclePostPolicyPort(observation, lifecycle_events) + validator_port = _LifecycleValidatorPort(observation, lifecycle_events) + bridge = AtomicDemoBridge( + compiled, + runtime, + sink, + clock, + post_policy_port=post_port, + validator_port=validator_port, + ) + + iterator = iter(bridge.iter_segments()) + segment_names: list[str | None] = [] + segment_metadata: list[dict[str, object]] = [] + action_metadata: list[dict[str, object]] = [] + accepted_masks: list[list[bool]] = [] + for segment_index in range(3): + observation_count = len(observation.generations) + demo_segment = next(iterator) + segment_names.append(demo_segment.name) + + # Merely requesting the next lazy segment must not capture live state. + assert len(observation.generations) == observation_count + actions = tuple(demo_segment.actions) + + assert observation.generations == list(range(1, segment_index + 2)) + assert len(actions) == 1 + assert demo_segment.metadata["validation"] is None + action_metadata.append(dict(actions[0].metadata)) + accepted_masks.append(demo_segment.validator().tolist()) + segment_metadata.append(dict(demo_segment.metadata)) + + with pytest.raises(StopIteration): + next(iterator) + + assert segment_names == ["move_cube"] * 3 + assert runtime.analysis_window_lengths == [6, 4, 2] + assert runtime.executed_semantic_ids == ["pick", "place"] * 3 + assert observation.generations == [1, 2, 3] + assert lifecycle_events == [ + ("observe", 1), + ("settle", 0), + ("validate", 0), + ("observe", 2), + ("settle", 1), + ("validate", 1), + ("observe", 3), + ("settle", 2), + ("validate", 2), + ] + assert runtime.eligible_masks[0] is None + assert [mask.tolist() for mask in runtime.eligible_masks[1:]] == [ + [True, False], + [True, False], + ] + assert [mask.tolist() for mask in post_port.active_masks] == [ + [True, True], + [True, False], + [True, False], + ] + assert accepted_masks == [[True, False]] * 3 + + for segment_index, metadata in enumerate(segment_metadata): + eligible_before = [True, True] if segment_index == 0 else [True, False] + validator_result = [True, False] if segment_index == 0 else [True, True] + assert metadata["expert_program_id"] == compiled.program_id + assert metadata["program_segment_index"] == segment_index + assert metadata["semantic_call_indices"] == [ + 2 * segment_index, + 2 * segment_index + 1, + ] + assert metadata["post_policy_count"] == 1 + assert metadata["validator_count"] == 1 + runtime_metadata = metadata["runtime"] + assert isinstance(runtime_metadata, dict) + assert runtime_metadata["message"] == ( + f"observation_generation={segment_index + 1}" + ) + post_policies = metadata["post_policies"] + assert isinstance(post_policies, list) + assert post_policies[0]["kind"] == "wait_stable" + assert post_policies[0]["result_mask"] == eligible_before + assert post_policies[0]["result"] == { + "status": "settled", + "segment_index": segment_index, + "observation_generation": segment_index + 1, + } + validation = metadata["validation"] + assert isinstance(validation, dict) + assert validation["eligible_mask_before_validation"] == eligible_before + assert validation["accepted_mask"] == [True, False] + validators = validation["validators"] + assert validators[0]["kind"] == "object_near_target" + assert validators[0]["result_mask"] == validator_result + assert validators[0]["result"] == { + "segment_index": segment_index, + "observation_generation": segment_index + 1, + "accepted_mask": validator_result, + } + json.dumps(metadata, allow_nan=False, sort_keys=True) + + assert action_metadata[segment_index]["bridge_action_kind"] == ( + "program_post_policy" + ) + assert action_metadata[segment_index]["program_segment_index"] == ( + segment_index + ) + + +def test_cube_variant_extends_by_data_without_motion_generation_code() -> None: + """A fourth destination and cycle require only serialized-data changes.""" + payload = deepcopy(_read_payload(_REPEATED_CUBE_PROGRAM)) + target = payload["targets"]["drop_pose"] + target["values"].extend( + ( + { + "position": [-0.25, -0.20, 0.10], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + { + "position": [-0.25, 0.20, 0.10], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + ) + ) + payload["program"]["count"] = 4 + + segments = tuple(_cube_compiler().compile(decode_expert_program(payload))) + + assert len(segments) == 4 + last_place = segments[-1].calls[-1].call + assert type(last_place) is Place + assert last_place.at is not None + assert last_place.at.position.tolist() == pytest.approx([-0.25, 0.20, 0.10]) + + +def test_open_drawer_program_compiles_to_reusable_articulation_skill() -> None: + """The drawer task supplies a goal and identities, never a trajectory.""" + payload = _read_payload(_OPEN_DRAWER_PROGRAM) + config = decode_expert_program(payload) + + assert config.integration.scene_registry == drawer_task.DRAWER_SCENE_REGISTRY_ID + assert config.integration.robot_profile == drawer_task.DRAWER_ROBOT_PROFILE_ID + + segments = tuple(_drawer_compiler().compile(config)) + + assert len(segments) == 1 + assert segments[0].name == "open_drawer" + assert len(segments[0].calls) == 1 + call = segments[0].calls[0].call + assert type(call) is OperateArticulation + assert call.articulation == SceneArticulationRef("drawer") + assert call.handle == SceneAffordanceRef("drawer_handle") + assert call.target == "open" + assert call.target_position is None + assert call.target_displacement is None + assert dict(call.resources) == {} + + +def test_task_classes_do_not_override_motion_or_demo_generation() -> None: + """Both environments delegate planning and execution to the shared runtime.""" + forbidden_overrides = { + "create_demo_action_list", + "create_demo_segments", + "_generate_eef_motion", + "_initialize_atomic_actions", + "_plan_pick_place_cycle", + } + + for env_type in ( + cube_task.MultiSegmentsCubePickPlaceEnv, + drawer_task.OpenDrawerEnv, + ): + assert forbidden_overrides.isdisjoint(env_type.__dict__) + + +def test_cube_task_declares_scene_and_robot_bindings_without_trajectory_code() -> None: + """Cube integration is an auditable identity/resource declaration.""" + scene = cube_task.create_cube_scene_binding(grasp_samples=32) + profile = cube_task.create_cube_robot_profile_binding() + + assert scene.registry_id == cube_task.CUBE_SCENE_REGISTRY_ID + assert scene.rigid_objects[0].simulation_uid == "cube" + assert scene.rigid_objects[0].collision_role is SceneCollisionRole.NONE + assert scene.rigid_objects[0].default_grasp_affordance == ( + cube_task.CUBE_GRASP_AFFORDANCE_ID + ) + assert scene.antipodal_grasps[0].object_id == "cube" + assert profile.profile_id == cube_task.CUBE_ROBOT_PROFILE_ID + assert dict(profile.defaults) == { + "pick_up": {"primary": "manipulator"}, + "place": {"primary": "manipulator"}, + } + assert profile.command_presets[0].commands["grasp"] == (0.024,) + + +def test_drawer_task_declares_native_link_joint_and_named_target() -> None: + """Drawer operation grounds through explicit native simulation identities.""" + scene = drawer_task.create_open_drawer_scene_binding() + profile = drawer_task.create_open_drawer_robot_profile_binding() + + operation = scene.articulation_operations[0] + assert scene.registry_id == drawer_task.DRAWER_SCENE_REGISTRY_ID + assert scene.articulations[0].collision_role is SceneCollisionRole.NONE + assert scene.links[0].native_link_name == "handle_xpos" + assert operation.joint_id == "slide_rails" + assert operation.operation_axis == (0.0, 0.0, -1.0) + assert operation.semantic_targets["open"].target_position == 0.11 + assert profile.profile_id == drawer_task.DRAWER_ROBOT_PROFILE_ID + assert dict(profile.defaults) == { + "operate_articulation": {"primary": "right_manipulator"} + } + assert profile.command_presets[0].commands == { + "open": (0.05, 0.05), + "grasp": (0.0, 0.0), + } + + +def test_vertical_slice_payloads_expose_no_motion_layer_fields() -> None: + """Official examples remain semantic data without controller/planner knobs.""" + forbidden_fields = { + "action", + "control_part", + "eef", + "joint_ids", + "motion_generator", + "planner", + "qpos", + "sample_count", + "tcp", + "trajectory", + } + + def keys(value: object) -> set[str]: + if type(value) is dict: + return set(value).union(*(keys(item) for item in value.values())) + if type(value) is list: + return set().union(*(keys(item) for item in value)) + return set() + + for path in (_REPEATED_CUBE_PROGRAM, _OPEN_DRAWER_PROGRAM): + assert forbidden_fields.isdisjoint(keys(_read_payload(path))) + + +__all__: list[str] = [] 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 1c04d6ca7..a54203df9 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 @@ -14,18 +14,19 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Tests for the lazy multi-segment cube pick-and-place task.""" +"""Tests for the declarative multi-segment cube task.""" from __future__ import annotations +import importlib import json from pathlib import Path -from types import MethodType, SimpleNamespace +from types import SimpleNamespace -import pytest import torch from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.gym.envs.expert_program import ExpertProgramEnvironmentMixin from embodichain.lab.gym.utils.gym_utils import config_to_cfg from embodichain.lab.gym.utils.registration import ( REGISTERED_ENVS, @@ -37,125 +38,205 @@ discover_task_packages() from embodichain_tasks.multi_segments.cube_pick_place import ( # noqa: E402 + CUBE_ROBOT_PROFILE_ID, + CUBE_SCENE_REGISTRY_ID, MultiSegmentsCubePickPlaceEnv, + _create_default_env_cfg, + create_cube_robot_profile_binding, ) -class TestMultiSegmentsCubePickPlaceEnv: - """Registration, config, and lazy-planning tests.""" - - def test_registered_and_exported(self) -> None: - """The new task category exports a registered environment.""" - from embodichain_tasks.multi_segments import __all__ - - assert "MultiSegmentsCubePickPlaceEnv" in __all__ - spec = REGISTERED_ENVS["MultiSegmentsCubePickPlace-v1"] - assert spec.cls is MultiSegmentsCubePickPlaceEnv - assert spec.max_episode_steps == 1200 - assert issubclass(MultiSegmentsCubePickPlaceEnv, EmbodiedEnv) - - def test_gym_config_targets_the_registered_task(self) -> None: - """The runnable gym config selects the task and three cycles.""" - config_path = ( - Path(__file__).parents[4] - / "embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json" - ) - config = json.loads(config_path.read_text()) - - assert config["id"] == "MultiSegmentsCubePickPlace-v1" - assert config["env"]["extensions"]["num_cycles"] == 3 - assert config["env"]["extensions"]["grasp_hold_steps"] == 45 - assert len(config["env"]["extensions"]["place_positions"]) == 2 - assert config["rigid_object"][0]["uid"] == "cube" - assert config["robot"]["class_type"] == "URRobot" - assert config["robot"]["robot_type"] == "ur5" - recorder = config["env"]["dataset"]["lerobot"] - assert recorder["func"] == "LeRobotRecorder" - assert recorder["params"]["robot_meta"] == { - "robot_type": "UR5", - "control_freq": 25, - } - assert recorder["params"]["save_path"] == "outputs/lerobot/multi_segments" - - cfg = config_to_cfg(config) - - assert isinstance(cfg.robot, URRobotCfg) - assert cfg.robot.robot_type == "ur5" - assert cfg.robot.control_parts["arm"] == [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - ] - assert cfg.robot.solver_cfg["arm"].ur_type == "ur5" - assert cfg.robot.solver_cfg["arm"].d1 == 0.089159 - - def test_segments_are_planned_lazily_from_updated_scene(self) -> None: - """Requesting the next segment observes the post-execution cube pose.""" - env = object.__new__(MultiSegmentsCubePickPlaceEnv) - env.num_cycles = 3 - env.place_positions = ((1.0, 0.0, 0.1), (2.0, 0.0, 0.1)) - env._completed_cycles = 0 - env._last_target_position = None - env.sim = SimpleNamespace(device=torch.device("cpu")) - env._scene_position_for_test = 0.0 - env._planned_positions_for_test = [] - - def fake_plan( - self: MultiSegmentsCubePickPlaceEnv, target_position: torch.Tensor - ): - source_pose = torch.eye(4).unsqueeze(0) - source_pose[:, 0, 3] = self._scene_position_for_test - self._planned_positions_for_test.append(self._scene_position_for_test) - action = torch.tensor([[self._scene_position_for_test]]) - return torch.ones(1, dtype=torch.bool), (action,), source_pose - - env._plan_pick_place_cycle = MethodType(fake_plan, env) - segments = iter(env.create_demo_segments()) - - first = next(segments) - assert env._planned_positions_for_test == [0.0] - assert first.metadata["planned_source_poses"][0][0][3] == 0.0 - - # In the real executor the first segment actions run while the outer - # generator is suspended. Emulate the resulting free-fall displacement. - list(first.actions) - env._scene_position_for_test = 0.17 - second = next(segments) - assert env._planned_positions_for_test == [0.0, 0.17] - assert second.metadata["planned_source_poses"][0][0][3] == pytest.approx(0.17) - - env._scene_position_for_test = -0.04 - third = next(segments) - assert env._planned_positions_for_test == [0.0, 0.17, -0.04] - assert third.metadata["target_position"] == pytest.approx([1.0, 0.0, 0.1]) - - list(third.actions) - try: - next(segments) - except StopIteration: - pass - else: - raise AssertionError("Expected exactly three demo segments.") - assert env._completed_cycles == 3 - - def test_invalid_positions_are_rejected(self) -> None: - """Every configured placement target must be an XYZ position.""" - with pytest.raises(ValueError, match="XYZ"): - MultiSegmentsCubePickPlaceEnv._validate_place_positions([(1.0, 2.0)]) - - def test_grasp_hold_is_inserted_before_lift(self) -> None: - """The closed grasp waypoint is held before the pickup lift starts.""" - env = object.__new__(MultiSegmentsCubePickPlaceEnv) - env.grasp_hold_steps = 2 - trajectory = torch.arange(120, dtype=torch.float32).reshape(1, 120, 1) - - augmented, clear_step = env._insert_grasp_hold(trajectory) - - assert augmented.shape == (1, 122, 1) - assert clear_step == 78 - assert augmented[0, 75, 0] == 75 - assert augmented[0, 76:78, 0].tolist() == [75, 75] - assert augmented[0, 78, 0] == 76 +def _gym_config_path() -> Path: + """Return the installed-source cube Gym config path.""" + return ( + Path(__file__).parents[4] + / "embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json" + ) + + +def _gym_payload() -> dict[str, object]: + """Load the runnable Gym configuration as inert JSON data.""" + path = _gym_config_path() + payload = json.loads(path.read_text(encoding="utf-8")) + assert type(payload) is dict + return payload + + +def test_registered_task_uses_shared_expert_program_mixin() -> None: + """The task is registered and delegates semantic execution to the mixin.""" + from embodichain_tasks.multi_segments import __all__ + + assert "MultiSegmentsCubePickPlaceEnv" in __all__ + spec = REGISTERED_ENVS["MultiSegmentsCubePickPlace-v1"] + assert spec.cls is MultiSegmentsCubePickPlaceEnv + assert spec.max_episode_steps == 1200 + assert issubclass(MultiSegmentsCubePickPlaceEnv, ExpertProgramEnvironmentMixin) + assert issubclass(MultiSegmentsCubePickPlaceEnv, EmbodiedEnv) + + +def test_gym_config_selects_packaged_expert_program() -> None: + """Normal Gym startup selects the semantic program by a relative path.""" + payload = _gym_payload() + + assert payload["id"] == "MultiSegmentsCubePickPlace-v1" + assert payload["expert_program_path"] == ( + "../../expert_program/multi_segments/repeated_cube_pick_place.yaml" + ) + extensions = payload["env"]["extensions"] + assert extensions == { + "grasp_samples": 10000, + "force_reannotate": False, + } + settle = payload["env"]["events"]["settle_cube_on_reset"] + assert settle["func"] == "wait_for_dynamic_objects_to_settle" + assert settle["mode"] == "reset" + assert settle["params"]["entity_cfgs"] == [{"uid": "cube"}] + + +def test_gym_config_keeps_scene_and_robot_configuration() -> None: + """The migration changes the expert layer, not the physical environment.""" + payload = _gym_payload() + cfg = config_to_cfg(payload, source_path=_gym_config_path()) + + assert isinstance(cfg.robot, URRobotCfg) + assert cfg.robot.robot_type == "ur5" + assert cfg.robot.control_parts["arm"] == [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + ] + assert cfg.robot.control_parts["hand"] == ["gripper_finger1_joint_1"] + assert cfg.rigid_object[0].uid == "cube" + + +def test_direct_default_cfg_loads_the_same_typed_program() -> None: + """Direct Python construction and Gym startup share one packaged program.""" + cfg = _create_default_env_cfg() + + assert cfg.expert_program is not None + assert cfg.expert_program.integration.scene_registry == CUBE_SCENE_REGISTRY_ID + assert cfg.expert_program.integration.robot_profile == CUBE_ROBOT_PROFILE_ID + assert cfg.expert_program.program_id == "repeated_cube_pick_place" + settle = cfg.events["settle_cube_on_reset"] + assert settle.func is not 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.""" + binding = create_cube_robot_profile_binding() + + assert binding.presets[0].preset_id == "safe" + assert binding.presets[0].recovery_policy.tracking_error_threshold == 0.08 + + +def test_task_initialization_delegates_to_shared_simulation_factory( + monkeypatch, +) -> None: + """Task setup contributes bindings but no task-local motion generator.""" + adapter = object() + captured: dict[str, object] = {} + + def fake_base_init(self, cfg, **kwargs) -> None: + del cfg, kwargs + self.grasp_samples = 48 + self.force_reannotate = True + + def fake_create_adapter(environment, **kwargs): + captured["environment"] = environment + captured.update(kwargs) + return adapter + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + task_module = importlib.import_module(MultiSegmentsCubePickPlaceEnv.__module__) + monkeypatch.setattr( + task_module, + "create_simulation_expert_program_adapter", + fake_create_adapter, + ) + + env = MultiSegmentsCubePickPlaceEnv(cfg=object()) + + assert env.expert_program_adapter is adapter + assert captured["environment"] is env + assert ( + captured["scene_binding"] + .antipodal_grasps[0] + .generator_cfg.antipodal_sampler_cfg.n_sample + == 48 + ) + assert captured["scene_binding"].antipodal_grasps[0].force_reannotate is True + assert captured["robot_profile_binding"].profile_id == CUBE_ROBOT_PROFILE_ID + + +def test_task_config_compiles_through_real_simulation_factory( + monkeypatch, +) -> None: + """Packaged config reaches the real adapter with explicitly bound mocks.""" + + class FakeRobot: + uid = "UR5" + + @staticmethod + def get_qpos() -> torch.Tensor: + return torch.zeros((1, 8), dtype=torch.float32) + + class FakeCube: + is_non_dynamic = False + + @staticmethod + def get_vertices(*, env_ids, scale) -> torch.Tensor: + assert env_ids == [0] + assert scale is True + return torch.tensor( + [[[-0.5, -0.5, 0.0], [0.5, -0.5, 0.0], [0.0, 0.5, 0.0]]], + dtype=torch.float32, + ) + + @staticmethod + def get_triangles(*, env_ids) -> torch.Tensor: + assert env_ids == [0] + return torch.tensor([[[0, 1, 2]]], dtype=torch.int64) + + @staticmethod + def get_local_pose(*, to_matrix) -> torch.Tensor: + assert to_matrix is True + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + robot = FakeRobot() + cube = FakeCube() + + class FakeSimulation: + @staticmethod + def get_robot(uid: str): + return robot if uid == "UR5" else None + + @staticmethod + def get_rigid_object(uid: str): + return cube if uid == "cube" else None + + def fake_base_init(self, cfg, **kwargs) -> None: + del kwargs + self.cfg = cfg + self.sim_cfg = SimpleNamespace(physics_dt=0.01) + self.sim = FakeSimulation() + self.robot = robot + for name, value in cfg.extensions.items(): + setattr(self, name, value) + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + cfg = _create_default_env_cfg() + + env = MultiSegmentsCubePickPlaceEnv(cfg=cfg) + segments = tuple(env.compile_expert_program(cfg.expert_program)) + + assert len(segments) == 3 + assert [segment.name for segment in segments] == ["move_cube"] * 3 + assert env.expert_program_adapter.scene_registry_id == CUBE_SCENE_REGISTRY_ID + assert env.expert_program_adapter.robot_profile_id == CUBE_ROBOT_PROFILE_ID + + +__all__: list[str] = [] diff --git a/tests/gym/envs/tasks/test_open_drawer.py b/tests/gym/envs/tasks/test_open_drawer.py new file mode 100644 index 000000000..81893c5a0 --- /dev/null +++ b/tests/gym/envs/tasks/test_open_drawer.py @@ -0,0 +1,306 @@ +# ---------------------------------------------------------------------------- +# 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 declarative drawer-opening task.""" + +from __future__ import annotations + +import importlib +import json +from pathlib import Path +from types import SimpleNamespace + +import pytest +import torch + +from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.gym.envs.demo import execute_demo_episode +from embodichain.lab.gym.envs.expert_program import ExpertProgramEnvironmentMixin +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, +) + +# Trigger official task auto-registration (idempotent). +discover_task_packages() + +from embodichain_tasks.tableware.open_drawer import ( # noqa: E402 + DRAWER_NATIVE_SLIDE_JOINT, + DRAWER_OPEN_POSITION, + DRAWER_ROBOT_PROFILE_ID, + DRAWER_UID, + OpenDrawerEnv, + create_open_drawer_scene_binding, +) + + +def _gym_config_path() -> Path: + """Return the installed-source drawer Gym config path.""" + return ( + Path(__file__).parents[4] + / "embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json" + ) + + +def _gym_payload() -> dict[str, object]: + """Load the drawer Gym config as inert JSON data.""" + payload = json.loads(_gym_config_path().read_text(encoding="utf-8")) + assert type(payload) is dict + return payload + + +def test_registered_drawer_task_uses_shared_expert_program_mixin() -> None: + """The environment delegates all demo generation to the shared runtime.""" + spec = REGISTERED_ENVS["OpenDrawer-v1"] + + assert spec.cls is OpenDrawerEnv + assert issubclass(OpenDrawerEnv, ExpertProgramEnvironmentMixin) + assert issubclass(OpenDrawerEnv, EmbodiedEnv) + assert "create_demo_action_list" not in OpenDrawerEnv.__dict__ + + +def test_drawer_gym_config_selects_packaged_semantic_program() -> None: + """The runnable task config points at the named-target Expert Program.""" + payload = _gym_payload() + + assert payload["id"] == "OpenDrawer-v1" + assert payload["expert_program_path"] == ( + "../../expert_program/tableware/open_drawer.json" + ) + assert payload["env"]["extensions"] == {} + + +def test_drawer_gym_config_preserves_physical_scene() -> None: + """Parsing still creates the CobotMagic robot and native drawer entity.""" + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + + assert cfg.robot.uid == "CobotMagic" + assert cfg.robot.control_parts["right_arm"] == [ + "right_joint1", + "right_joint2", + "right_joint3", + "right_joint4", + "right_joint5", + "right_joint6", + ] + assert cfg.robot.control_parts["right_eef"] == [ + "right_joint7", + "right_joint8", + ] + assert cfg.articulation[0].uid == "drawer" + assert cfg.expert_program is not None + assert cfg.expert_program.program_id == "open_drawer" + + +def test_drawer_affordance_uses_reachable_post_release_retract() -> None: + """The opened drawer retract remains clear of the handle and IK-reachable.""" + operation = create_open_drawer_scene_binding().articulation_operations[0] + contact_z = operation.contact_offset[11] + retract_z = operation.retract_offset[11] + + assert retract_z < contact_z + assert contact_z - retract_z == pytest.approx(0.01) + + +def test_task_initialization_delegates_to_shared_simulation_factory( + monkeypatch, +) -> None: + """Drawer setup contributes declarations but no planner implementation.""" + adapter = object() + captured: dict[str, object] = {} + + def fake_base_init(self, cfg, **kwargs) -> None: + del self, cfg, kwargs + + def fake_create_adapter(environment, **kwargs): + captured["environment"] = environment + captured.update(kwargs) + return adapter + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + task_module = importlib.import_module(OpenDrawerEnv.__module__) + monkeypatch.setattr( + task_module, + "create_simulation_expert_program_adapter", + fake_create_adapter, + ) + + env = OpenDrawerEnv(cfg=object()) + + assert env.expert_program_adapter is adapter + assert captured["environment"] is env + assert captured["scene_binding"].links[0].native_link_name == "handle_xpos" + assert captured["robot_profile_binding"].profile_id == DRAWER_ROBOT_PROFILE_ID + + +def test_task_config_compiles_through_real_simulation_factory( + monkeypatch, +) -> None: + """Packaged drawer config reaches the real adapter with explicit mocks.""" + + class FakeRobot: + uid = "CobotMagic" + + @staticmethod + def get_qpos() -> torch.Tensor: + return torch.zeros((1, 16), dtype=torch.float32) + + class FakeDrawer: + link_names = ("outer_box", "inner_box", "handle_xpos") + joint_names = ("slide_rails",) + + @staticmethod + def get_local_pose(*, to_matrix) -> torch.Tensor: + assert to_matrix is True + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + @staticmethod + def get_link_pose(name: str, *, env_ids, to_matrix) -> torch.Tensor: + assert name == "handle_xpos" + assert env_ids == [0] + assert to_matrix is True + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + robot = FakeRobot() + drawer = FakeDrawer() + + class FakeSimulation: + @staticmethod + def get_robot(uid: str): + return robot if uid == "CobotMagic" else None + + @staticmethod + def get_articulation(uid: str): + return drawer if uid == "drawer" else None + + def fake_base_init(self, cfg, **kwargs) -> None: + del kwargs + self.cfg = cfg + self.sim_cfg = SimpleNamespace(physics_dt=0.01) + self.sim = FakeSimulation() + self.robot = robot + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + + env = OpenDrawerEnv(cfg=cfg) + segments = tuple(env.compile_expert_program(cfg.expert_program)) + + assert len(segments) == 1 + assert segments[0].name == "open_drawer" + assert env.expert_program_adapter.scene_registry_id == "open_drawer_v1" + assert env.expert_program_adapter.robot_profile_id == DRAWER_ROBOT_PROFILE_ID + + +@pytest.mark.requires_sim +@pytest.mark.slow +def test_real_sim_expert_episode_opens_drawer_with_joint_effect_trace() -> None: + """The packaged program completes against live drawer physics and evidence.""" + import gc + + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + cfg.num_envs = 1 + cfg.sim_cfg = SimulationManagerCfg( + headless=True, + sim_device="cpu", + num_envs=1, + ) + cfg.sensor = [] + cfg.events = None + cfg.observations = None + cfg.dataset = None + cfg.init_rollout_buffer = False + cfg.record_trajectory = False + cfg.filter_dataset_saving = True + + env: OpenDrawerEnv | None = None + try: + env = OpenDrawerEnv(cfg=cfg) + env.reset(seed=0) + + result = execute_demo_episode(env) + + assert result.completed + assert result.all_success + assert result.terminal_reason == "success" + assert len(result.segments) == 1 + segment = result.segments[0] + assert segment.name == "open_drawer" + assert segment.success + + metadata = segment.metadata + runtime = metadata["runtime"] + assert runtime["kind"] == "skill_result" + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + assert len(runtime["calls"]) == 1 + call = runtime["calls"][0] + assert call["semantic_id"] == "operate_articulation" + assert call["status"] == "completed" + assert call["masks"] == { + "entered": [True], + "completed": [True], + "failed": [False], + } + assert call["plan_attempts"] + assert call["plan_attempts"][-1]["plan_success_mask"] == [True] + + effects = call["effects"] + assert effects + for effect in effects: + assert effect["effect_spec"]["semantic_id"] == "operate_articulation" + evidence = effect["evidence"]["joint.position"] + assert evidence["valid_mask"] == [True] + assert evidence["acquisition_errors"] == [None] + assert evidence["env_ids"] == [0] + final_effect = effects[-1] + assert final_effect["decision"] == { + "success_mask": [True], + "failure_mask": [False], + } + + assert metadata["post_policies"] == [] + assert metadata["validation"] == { + "env_ids": [0], + "runtime_success_mask": [True], + "eligible_mask_before_validation": [True], + "post_policy_success_mask": None, + "validators": [], + "accepted_mask": [True], + } + + drawer = env.sim.get_articulation(DRAWER_UID) + assert drawer is not None + joint_index = drawer.joint_names.index(DRAWER_NATIVE_SLIDE_JOINT) + final_position = float(drawer.get_qpos()[0, joint_index].item()) + joint_tolerance = float( + final_effect["monitor"]["resolved_params"]["joint_success_tolerance"] + ) + assert abs(final_position - DRAWER_OPEN_POSITION) <= joint_tolerance + finally: + if env is not None: + env.close() + SimulationManager.flush_cleanup_queue() + gc.collect() + + +__all__: list[str] = [] diff --git a/tests/scripts/tools/test_expert_program_rollout_report.py b/tests/scripts/tools/test_expert_program_rollout_report.py new file mode 100644 index 000000000..260f15a81 --- /dev/null +++ b/tests/scripts/tools/test_expert_program_rollout_report.py @@ -0,0 +1,94 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest + +from scripts.tools.expert_program_rollout_report import ( + DEFAULT_REPORT_PATH, + REPOSITORY_ROOT, + build_task_size_metrics, + main, + render_report, +) + +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), +} + +EXPECTED_SOURCE_PATHS = { + "Cube": ( + "embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py", + "embodichain_tasks/configs/expert_program/multi_segments/" + "repeated_cube_pick_place.yaml", + ), + "Drawer": ( + "embodichain_tasks/embodichain_tasks/tableware/open_drawer.py", + "embodichain_tasks/configs/expert_program/tableware/open_drawer.json", + ), +} + + +def test_current_counts_use_only_the_four_declared_sources() -> None: + metrics = build_task_size_metrics(REPOSITORY_ROOT) + + actual = { + metric.task: ( + metric.current_lines, + metric.current_bytes, + tuple(source.path for source in metric.sources), + ) + for metric in metrics + } + expected = { + task: (*EXPECTED_CURRENT_COUNTS[task], EXPECTED_SOURCE_PATHS[task]) + for task in EXPECTED_CURRENT_COUNTS + } + assert actual == expected + + +def test_render_is_deterministic() -> None: + metrics = build_task_size_metrics(REPOSITORY_ROOT) + + first = render_report(metrics) + second = render_report(metrics) + + assert first == second + + +def test_render_rejects_empty_metric_snapshot() -> None: + with pytest.raises(ValueError, match="at least one task snapshot"): + render_report(()) + + +def test_checked_in_report_matches_deterministic_render() -> None: + expected = render_report(build_task_size_metrics(REPOSITORY_ROOT)) + + assert DEFAULT_REPORT_PATH.read_text(encoding="utf-8") == expected + + +def test_check_mode_accepts_current_report() -> None: + assert main(["--check"]) == 0 + + +def test_check_mode_rejects_stale_report(tmp_path) -> None: + stale_report = tmp_path / "expert_program_rollout_report.md" + stale_report.write_text("stale\n", encoding="utf-8") + + assert main(["--check", "--output", str(stale_report)]) == 1 diff --git a/tests/test_expert_program_package_data.py b/tests/test_expert_program_package_data.py new file mode 100644 index 000000000..4d695881f --- /dev/null +++ b/tests/test_expert_program_package_data.py @@ -0,0 +1,196 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Focused setuptools coverage for packaged Expert Program resources.""" + +from __future__ import annotations + +import ast +import json +import os +from pathlib import Path +import shutil +import subprocess +import sys +from typing import NamedTuple + +import pytest +from setuptools import Distribution +from setuptools.command.build_py import build_py + +from setup import get_package_dir + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[1] +_SETUP_PATH = _REPOSITORY_ROOT / "setup.py" +_CONFIG_PACKAGE = "embodichain_tasks.configs" +_CONFIG_SOURCE = _REPOSITORY_ROOT / "embodichain_tasks" / "configs" +_PROGRAMS = { + Path("expert_program/multi_segments/repeated_cube_pick_place.yaml"): ( + "repeated_cube_pick_place" + ), + Path("expert_program/tableware/open_drawer.json"): "open_drawer", +} + + +class _StagedConfigPackage(NamedTuple): + """Isolated setuptools output and the setup options that produced it.""" + + build_lib: Path + relative_outputs: frozenset[Path] + package_data: dict[str, list[str]] + include_package_data: bool + + +def _literal_setup_keyword(keyword_name: str) -> object: + """Read one literal keyword from the repository's setup() call.""" + tree = ast.parse(_SETUP_PATH.read_text(encoding="utf-8"), filename=str(_SETUP_PATH)) + setup_calls = tuple( + node + for node in ast.walk(tree) + if isinstance(node, ast.Call) + and isinstance(node.func, ast.Name) + and node.func.id == "setup" + ) + if len(setup_calls) != 1: + raise AssertionError("setup.py must contain exactly one setup() call.") + keywords = { + keyword.arg: keyword.value + for keyword in setup_calls[0].keywords + if keyword.arg is not None + } + if keyword_name not in keywords: + raise AssertionError(f"setup.py does not declare {keyword_name!r}.") + return ast.literal_eval(keywords[keyword_name]) + + +@pytest.fixture +def staged_config_package(tmp_path: Path) -> _StagedConfigPackage: + """Stage only the two official programs through the real build_py command.""" + package_data = _literal_setup_keyword("package_data") + include_package_data = _literal_setup_keyword("include_package_data") + assert type(package_data) is dict + assert type(include_package_data) is bool + + isolated_source = tmp_path / "source" / "embodichain_tasks" / "configs" + isolated_source.mkdir(parents=True) + shutil.copyfile(_CONFIG_SOURCE / "__init__.py", isolated_source / "__init__.py") + for relative_path in _PROGRAMS: + destination = isolated_source / relative_path + destination.parent.mkdir(parents=True, exist_ok=True) + shutil.copyfile(_CONFIG_SOURCE / relative_path, destination) + + build_lib = tmp_path / "build_lib" + distribution = Distribution( + { + "packages": [_CONFIG_PACKAGE], + "package_dir": {_CONFIG_PACKAGE: str(isolated_source)}, + "package_data": package_data, + "include_package_data": include_package_data, + } + ) + distribution.script_name = str(_SETUP_PATH) + command = build_py(distribution) + command.build_lib = str(build_lib) + command.ensure_finalized() + + def reject_manifest_command(command_name: str) -> None: + raise AssertionError( + f"Focused package-data staging must not run {command_name!r}." + ) + + command.run_command = reject_manifest_command + relative_outputs = frozenset( + Path(output).resolve().relative_to(build_lib.resolve()) + for output in command.get_outputs(include_bytecode=False) + ) + command.run() + return _StagedConfigPackage( + build_lib=build_lib, + relative_outputs=relative_outputs, + package_data=package_data, + include_package_data=include_package_data, + ) + + +def test_setup_stages_both_official_expert_program_formats( + staged_config_package: _StagedConfigPackage, +) -> None: + """The actual setup patterns put nested JSON and YAML in wheel staging.""" + assert staged_config_package.include_package_data is False + assert get_package_dir()[_CONFIG_PACKAGE] == "embodichain_tasks/configs" + assert staged_config_package.package_data[_CONFIG_PACKAGE] == [ + "**/*.json", + "**/*.yaml", + "**/*.yml", + ] + expected_outputs = { + Path("embodichain_tasks") / "configs" / relative_path + for relative_path in _PROGRAMS + } + assert expected_outputs <= staged_config_package.relative_outputs + + +def test_staged_programs_decode_through_installed_config_paths( + staged_config_package: _StagedConfigPackage, + tmp_path: Path, +) -> None: + """A clean process resolves and decodes both files from wheel staging.""" + runtime_dir = tmp_path / "runtime" + runtime_dir.mkdir() + expected_ids = { + relative_path.as_posix(): program_id + for relative_path, program_id in _PROGRAMS.items() + } + script = """ +import json +from pathlib import Path +import sys + +import embodichain_tasks.configs as config_package +from embodichain.lab.gym.envs.expert_program import load_expert_program +from embodichain_tasks.configs import get_config_path + +build_lib = Path(sys.argv[1]).resolve() +expected = json.loads(sys.argv[2]) +module_path = Path(config_package.__file__).resolve() +assert module_path.is_relative_to(build_lib), (module_path, build_lib) +decoded = {} +for relative_path, expected_program_id in expected.items(): + resource_path = get_config_path(relative_path).resolve() + assert resource_path.is_relative_to(build_lib), (resource_path, build_lib) + program = load_expert_program(resource_path) + assert program.program_id == expected_program_id + decoded[relative_path] = program.program_id +print(json.dumps(decoded, sort_keys=True)) +""" + environment = os.environ.copy() + environment["PYTHONPATH"] = str(staged_config_package.build_lib) + completed = subprocess.run( + [ + sys.executable, + "-c", + script, + str(staged_config_package.build_lib), + json.dumps(expected_ids, sort_keys=True), + ], + cwd=runtime_dir, + env=environment, + check=True, + capture_output=True, + text=True, + ) + + assert json.loads(completed.stdout) == expected_ids