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..6b33529b2 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,7 +1,9 @@ # 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. 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 +47,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 +95,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 +885,7 @@ PR2A SceneRegistry PR2B RobotSkillProfile | | | v | PR2C Runtime Endpoints - | (in progress) + | (implemented) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -1036,10 +1040,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 +1123,21 @@ 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. This preflight coverage does not replace the remaining +end-to-end dynamic-obstacle recovery simulation. + ### 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 +1154,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 +1180,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 +1202,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 +1233,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 +1254,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 +1279,11 @@ tests pass before the legacy task is switched. ### Phase 8: rollout, documentation, and deprecation +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; @@ -1262,11 +1346,15 @@ independent of adoption of the new path. 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] 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 +1362,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 +1377,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/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/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/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