diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index ab128e81b4..6f10b4b2eb 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -207,7 +207,7 @@ topics: - id: expansion-engine title: Expansion Engine aliases: [Expansion Engine, offline expansion, 扩增引擎, 批量专家采集] - keywords: [ExpansionEngine, ExpansionRunCfg, ExpansionJobCfg, ExpansionSession, ExpansionSessionCfg, ExpansionCoverage, EpisodeCoverage, GymExpansionHost, ArtifactEpisodeSink, MediaSpool, render_observation_variants] + keywords: [ExpansionEngine, ExpansionRunCfg, ExpansionJobCfg, ExpansionSession, ExpansionSessionCfg, ExpansionCoverage, EpisodeCoverage, GymExpansionHost, ArtifactEpisodeSink, MediaSpool, render_observation_variants, WorkloadProgramExecutor, WorkloadExecutionCfg, workload_count, GymExpansionHostExtensions, MeasuredExpansionValidator, MeasuredTrajectory, preview_expansion_comparison, 扩增虚影] paths: [topics/expansion-engine/expansion-engine.md] source_of_truth: - embodichain/lab/expansion/ @@ -304,6 +304,7 @@ topics: SimulationSceneExpansionHost, SimulationSceneInitialState, episode_prepare] paths: [topics/scene-expansion/scene-expansion.md] source_of_truth: + - embodichain/lab/_scene_config/ - embodichain/lab/sim/scene/feasibility/ - embodichain/lab/sim/scene/expansion/ - embodichain/lab/task_program/integrations/feasibility.py diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index f8927e9e9b..c538354cfd 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -24,6 +24,12 @@ Hosts acquire versioned measurements; verification is the boundary that commits or invalidates observed effects. Preserve required stop/hold boundaries on the executed control grid instead of relying only on native planner timing. +Row-local planning within a larger World must carry the same explicit +`context.env_ids` through FK/IK, root poses and diagnostics. HandOver applies this +to both source/destination arms, including its continuation and failed-waypoint +diagnostic routes. A one-row planning context does not implicitly select arena +zero or all arenas of the robot. + ## Package map | Concern | Source of truth | @@ -99,6 +105,10 @@ are calibrated scene inputs. Preserve their selected owners and fingerprints; never infer capabilities or general physical success from a sample name. The host's sampled footprint/support checks remain finite measured acceptance, separate from geometric proposal qualification. +`CoordinatedPlacement` retains each binding's configured grasp command during +approach/hold and on the support hand through release/retreat. Contact-blocked +measured jaw positions cannot replace these closing targets. Measured robot +observations and held-object transforms still supply geometry and validation. ## Recommended change sites diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index c5bd37a172..84c13178ee 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -10,13 +10,21 @@ algorithms stay in `sim.motion.expansion`; scene and workspace algorithms stay i | Closed configuration and preflight | `embodichain/lab/expansion/configured.py` | | Canonical in-memory session, lifecycle limits and coverage port | `embodichain/lab/expansion/session.py`, `session_cfg.py`, `coverage.py` | | Shared scheduling, admission and commit ports | `embodichain/lab/expansion/engine.py` | +| Rolling workload × B execution in one caller-owned World | `embodichain/lab/expansion/workloads.py` | +| Native workload World, source/evidence assembly and bounded export | `embodichain/lab/expansion/gym_workloads.py` | +| Declarative scene/motion jobs and pure configuration validation | `embodichain/lab/expansion/task_program_jobs.py`, `_task_program_config.py` | +| Owned result-preview launch, wait and stop | `embodichain/lab/expansion/preview.py` | | Job assembly, stage recovery and inspect | `embodichain/lab/expansion/workflow.py`, `stages.py` | | Native World ownership and compatibility facade | `embodichain/lab/expansion/host.py` | +| Task-owned render/source/transform/acceptance ports | `embodichain/lab/expansion/host_extensions.py` | +| Measured geometry, native substeps and sustained-contact acceptance | `embodichain/lab/expansion/acceptance.py` | +| Physics-free measured reference/ghost comparison | `embodichain/lab/expansion/comparison.py` | | Native recipes, cohort preparation, step observation and episode assembly | `embodichain/lab/expansion/_gym_recipe.py`, `_gym_prepare.py`, `_gym_execution.py`, `_gym_frames.py`, `_gym_episode.py` | | Durable recipes, attempts and invocation budgets | `embodichain/lab/expansion/journal.py` | | Task Program execution with native or prototype motion | `embodichain/lab/expansion/program.py`, `real2sim.py` | | Source-pinned sampling and joint scene candidates | `embodichain/lab/expansion/recipes.py`, `scene.py` | | Artifact staging, checks and dataset export | `embodichain/lab/expansion/persistence.py`, `export.py` | +| Portable one-episode dataset export | `embodichain/lab/expansion/episode_export.py` | | Native appearance and state rerender | `embodichain/lab/expansion/visual.py` | | Bounded media blocks | `embodichain/data_pipeline/recording/media.py` | @@ -38,7 +46,10 @@ configured jobs. `sampled_rigid_v1` collision profiles independently check all physical box/plane scene bodies and task predicates from measured states. Their robot-base transforms are rebound after each cohort's scene preparation. State-only jobs retain independent and robot-mounted ContactSensors while -omitting cameras; contact acquisition does not require media capture. +omitting cameras; contact acquisition does not require media capture. Sustained +contact windows round off-grid durations upward, allowing only ULP-scale grid +roundoff. Their elapsed time sums measured physics intervals within the bounded +window; it does not subtract accumulated World clocks. Immutable recipes declare source/backend/phase/scope and shared-resource compatibility; the host checks them before mutating the World. @@ -71,6 +82,46 @@ sampling, whole-cohort preparation, active-row frame buffering and measured epis assembly have separate owners. Preserve the host constructor, port methods and existing readout properties used by benchmark/showcase instrumentation. +`GymExpansionHostExtensions` supplies non-owning callbacks for collection renderer, +scene source, immutable recipe transformation, measured validator and row plan +factory. `ExpansionRowContext` carries pinned identity without exposing mutable +host buffers. Hosts retain preparation, control clocks, T+1 capture, attachment +archival and confirmed receipts. Do not replace configured nominal sources with +an extension transform. `MeasuredExpansionValidator` presents the same lifecycle +for independent geometry and native contacts, including task-declared sustained +finger contact and drift limits. Every original physical substep remains required; +missing impulses, unknown actors and dropped samples reject acceptance. + +`preview_expansion_comparison` consumes a caller-owned environment factory and +qualified measured episodes. Shared-time pause/switch controls retain the selected +time; physics stays disabled and render updates do not mutate saved evidence. +`MarkerFramePublisher` owns resident robot/object groups and publishes a coherent +cross-group frame through the public marker layer. Task assets and any collection +process orchestration remain outside the replay core. + +`embodichain expansion preview --input` reads confirmed job outputs through `_preview_data.py`, +then replays full measured states using `_preview_render.py` and a loopback-only +`_preview_server.py`. Inputs retain their original evidence; no collection or +physics stepping occurs. It accepts one URDF robot and recorded rigid visual +objects (including file Mesh/USD via native visual loading), with fixed layouts; +unsupported geometry is rejected. Constant static backgrounds remain single +instances. Space filters group +input directories; missing motion annotations remain explicit. HTTP publishes a +single immutable image/state snapshot; WebRTC is follow-up work. FK and episode +budgets bound preload, controls are queued for the render owner, and native +resources/ports close on exit. See the example's `ghost-preview.md` for usage. + +`preview --job` uses `_preview_job.py` to freeze lower/upper/center and stratified +interior parameter points, then invokes ordinary native collection in owned child +processes before opening the viewer. Physical ranges, atlas request domains and +explicit joint/via-point draws retain original validation. Each point enumerates +its configured motion/affordance recipes; exact-zero residuals collapse to the +unique nominal variant. Source semantics and aggregate attempts/time budgets are +checked, failed endpoints remain in the coverage manifest, and no replacement +sampling or success credit is allowed. Original job outputs remain untouched. Episodes archive `source_env_id` for +row-correct diagnostics even without a motion profile; ambiguous legacy grasp +annotations remain unavailable rather than selecting row zero. + `TaskProgramDemoBridge.program_id` and `env_ids` expose binding identity without runtime traversal. `EmbodiedEnv.task_program_bridge_scope()` restores the borrowed bridge even after failure. `Real2SimTabletopSource` and explicit @@ -82,7 +133,7 @@ journal/duplicate-submission behavior. Cohorts have fixed robot/joint topology, backend, clock, observation schema and World appearance. Native scene-atlas input retains the complete layout/installation candidate. Assets with different topology require separate deployment Worlds. -All rows reset and settle together. Only the owner steps physics, and row generators +The configured cohort path resets and settles its rows together. Only the owner steps physics, and row generators acknowledge commands only after successful steps. Early termination must drain the Task Program safe-stop handshake; completed rows stop contributing frames. Native vector rejection cancels the row across semantic lanes and masks retained @@ -91,6 +142,27 @@ post-policy observation clock. Expected Real2Sim acceptance rejection retains measured checks and rejects only that row; source corruption and runtime errors still stop collection. +`WorkloadProgramExecutor` is an optional rolling integration for custom native +sources. Its supplied environment must have exactly `workload_count * batch_size` +rows and be the sole live World in the process. Each workload owns quotas, local +candidate ordinals and admission budgets; its B-row lane is reused after that +group drains. Preparing rows settle through ordinary control steps while live +rows continue to record and validate every boundary. The source owns scene +proposals, bridge binding and all physical checks, using `ExpansionEngine.admit_row`, +`mark_row_started` and `complete_row` instead of editing private engine queues. +Only confirmed native receipts earn quota or permit reuse; duplicate identities +cannot earn credit twice. Clock spans must equal each trajectory's command count. +Compatible fixed-grid planning and pure solver requests can be cooperatively +merged on the owner thread. Scoped planning budgets retain per-row charges; +unsupported requests keep native execution. Export callbacks receive frozen +group evidence and never access the World. See [rolling execution](workloads.md). +Optional idle-row reuse keeps the same W×B capacity while competing retry +candidates use otherwise idle slots. Native write reservations still enforce +the original quota; peers are cancelled only after a confirmed receipt, using +measured partial evidence and the bridge's ordinary stop handshake. The low-level +source must supply an explicit cancellation port. Native Task Program jobs opt in +with the [idle-row reuse setting](workloads.md#source-ports-and-native-accounting). + Native numerical evidence has T submitted actions and T+1 measured states. It includes all represented dynamic scene entities, source/action semantics, physical parameters and call provenance. Position-velocity commands must use their ExpertActionSpec; @@ -102,6 +174,11 @@ has bounded transient copies; no per-candidate dense full-episode image array is Frame readers stream selected frames from stored blocks; full artifact audits still validate bounded blocks. LeRobot export streams confirmed parents and visual children through the existing recorder and checks the exact final journal identity set. ODS remains unchanged. +`export_expansion_episodes` additionally publishes one portable LeRobot dataset +per confirmed parent. Cached evidence reads still hash every file byte and +reconstruct checked, independently owned numerical tensors; source/receipt/media +inspection remains required. Immutable JSON nodes can be shared internally, while +public mutable projections own separate containers. Pure visual replay requires complete scene state and an explicit observation-independent expert contract. Physics is disabled across the entire World; render-state publication @@ -130,3 +207,18 @@ measured state plus explicitly labeled FK, and pure-visual children retain their physical parent's clock. Qualification requires admitted operator evidence, not just a requested profile or completed nominal episodes. Report completeness and capability acceptance are separate, including diagnostic unsupported combinations. + +## Offline distribution reports + +`python -m embodichain.lab.expansion._distribution` compares explicit before/after +confirmed job directories using the existing receipt inspector and episode loader. +It emits structured JSON/JSONL and a standalone HTML report with world-XY layout +bins, start/end joint counts, measured object paths, joint-motion summaries, and +existing geometry descriptor buckets. Version 2 adds full SO(3) orientation +features and recorded-URDF TCP FK with explicit gripper calibration. A lightweight +index drives filters; 100-record detail pages are shared by the offline viewer +and Agent JSON consumers. Matching task/robot/scene domains are required. +The report distinguishes cumulative additions from independent cohorts; observed +bins are not a feasible-volume certificate. The reader uses existing artifact +validation dependencies but creates no World. See +[distribution usage](../../../embodichain/lab/expansion/distribution.md). diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md new file mode 100644 index 0000000000..c4396fe6f4 --- /dev/null +++ b/agent_context/topics/expansion-engine/workloads.md @@ -0,0 +1,279 @@ +# Rolling workload execution + +The public owner is `embodichain.lab.expansion.workloads`. This optional port is +independent of task names and downstream operator configuration. Existing +`ExpansionEngine.from_job` and cohort callers retain their interfaces and defaults. + +## Capacity and ownership + +`WorkloadExecutionCfg` declares W active groups and B physical candidate rows +per group; the supplied environment must contain exactly W times B rows. The +executor creates no simulation World, process or planning thread. It runs once +on the thread that owns its World. `world_info` records actual process, World, +backend, state and solver devices; selecting a visible CUDA device or renderer +does not select the physics device. + +`ExpansionWorkloadCfg` declares a unique group, qualified target, attempt cap and +admission wall-time budget. Candidate ordinals and seeds belong to that group, +not its lane or physical row. Admission includes in-flight candidates in the +remaining quota, avoiding extra speculative trajectories credited beyond target. +Another queued group takes the lane when the previous group has drained. A +difficult group does not silently borrow another group's B-row allocation. + +## Source ports and native accounting + +`run_task_program_expansion` binds a deployment and a closed scene/motion +configuration to `run_gym_expansion_workloads`. It owns bounded pose/follower +proposals, grasp-policy snapshots, original-grid plan transforms, common measured +acceptance and aggregate results. Configuration/viewer loaders do not import the +simulator. Integrators retain task values and provenance inputs rather than +copying these binding components. `derive_grasp_expansion_deployment` exposes the +same immutable-source preparation for configuration preflight. + +`launch_expansion_preview`, `preview_expansion_results` and +`stop_expansion_preview` manage a bounded Linux native service with recorded PID +ownership. The preview reader accepts native aggregate results and derives path +endpoints from recorded motion; downstream workflow-root unwrapping stays with +the integrator. These APIs never recollect or advance physical time. + +`run_gym_expansion_workloads` is the public native composition for Task Program +scene/plan workloads. `GymExpansionWorkload` supplies a pinned deployment, +independent collection budget, artifacts and row callbacks. The facade opens one +World, captures the original seeded nominal scene in row zero, and composes the +existing session, frame collector, episode assembler and confirmed artifact sink. +Different integration/grasp policies may share that World only when their +physical configuration, program, cadence and expert action schema agree. The +effective scene-preparation declaration and its parameters must also match: +explicit configuration wins over the environment's registered default, following +`EmbodiedEnv` construction. The common nominal scene is prepared once before +constructing the World. +Command mode, ActionManager declarations and controlled-joint selections cannot +vary between workloads in one World. Selections belong to the resolved environment +configurations, not the shared registered environment implementation class. Physical/visual/prototype +operators remain on the cohort API; unsupported payloads fail explicitly. +Explicit contact sampling conventions may differ per integration through +`SimulationTaskProgramAdapterFactory.create_adapter(grasp_contact_frames=...)`; +the declared frame must match its grasp service, while TCP, hardware geometry +and drive/command bindings remain strict and the live robot is unchanged. +Only the initial nominal reset may run scene-settling events. The facade then +freezes reset events; rolling preparation is checked against the physical step +counter so another row cannot advance outside the World-owner loop. +Frozen groups export through one bounded writer thread without World access. +Fresh output is required; this facade does not resume interrupted collection. + +`WorkloadExecutionCfg.reuse_idle_rows` optionally lends unused physical rows to +unfinished groups; capacity remains exactly W times B. Queued groups keep their +normal lane priority. Healthy candidates fill only their remaining quota. Each +observed unsuccessful attempt permits one additional competing candidate, bounded +by the existing attempt, admission-time, settling, step and pending-byte limits. +The low-level flag and native Task Program job both default false. Enable +reuse explicitly with `engine.reuse_idle_rows: true`. On +CPU-bound planning, competing retries may increase total work despite higher +row occupancy; validate qualified output per wall time before enabling reuse. + +This mode requires a source `cancel(result, *, reason)` port. Only a confirmed +receipt fills quota. Remaining live candidates use the ordinary safe-stop +handshake; unstarted warming rows are cancelled without fabricated commands. +Already completed peers retain their canonical program acceptance in cancelled +diagnostics; they do not become physical failures or successful persisted rows. +The native source retains measured T+1 partial buffers and plan traces, then +releases the native assignment. Cancelled rows earn no receipt or quota credit. + +`WorkloadSource.prepare` runs inside the selected-row `episode_prepare` reset +callback. It must not step physics, alter an active peer or mutate World-wide +appearance/resources. Incompatible topology or shared resources require a +different deployment, rather than pretending they are independent rows. + +After at least the declared physical settling substeps, `activate` captures the +initial measured boundary, performs the source's required preparation checks, +registers a native candidate and uses `ExpansionEngine.admit_row`. It returns a +fresh bridge bound exactly to that row and the environment's control cadence. +Rejected preparation returns None and never earns quota. The native source retires +warming rows when the admission time or independent rollout budget is exhausted. +It records the stop reason before proposal, including exhaustion detected during +initial-state validation. The group can finish incomplete while already-admitted +peers continue and export normally. + +Settling rows remain controller-active so Gym consumes their explicit reset +hold targets. They are not live trajectories: only cursor rows receive source +observations, command acknowledgements, trajectory lengths or receipt credit. +Using Gym's inactive measured-qpos substitution during settling would remove +restoring drive error on every tick and change the admitted initial posture. + +`started` calls `mark_row_started` after the first actual command. `observe` +captures every active T+1 boundary and performs the original physical checks; +it returns only active rows needing native safe rejection. The executor retains +the bridge acknowledgement/hold handshake. Preparing and completed peers cannot +fabricate actions or observations for another trajectory. + +`complete` receives the canonical bridge result and exact first/last World +control indices. It assembles the original source evidence and calls +`ExpansionEngine.complete_row`. Only that confirmed receipt earns a qualified +count. None is a rejection, never a placeholder success. Unavailable or ambiguous +writes retain the ordinary native stage/reconciliation contract and stop the +rolling owner from issuing unsupported quota credit. +`ExpansionSession.take_ready(speculative=True)` may reserve competing running +rows while quota is unfilled. Required physical checks are unchanged, and +pending writes plus confirmed commits can never exceed the original target. +`ExpansionEngine.cancel_row` cannot release pending persistence; its existing +reconciliation path still owns uncertain or failed writes. Safe-stop controller +values retain the interrupted invocation/phase metadata for ordinary physical +validators, while their cancellation kind and reason remain explicit. +When a runtime is already terminal with a final unconsumed motion, the joint +controller owner supplies its actual already-applied position targets and zero +velocity through the abort callback's optional `controller_hold`. The bridge +prefers a runtime-generated hold, never relabels pending motion, and advances +the clock only after the fallback hold receives a real `env.step`. Without an +explicit owner-proven hold, the existing fail-closed abort behavior remains. + +Frozen groups can call `export_expansion_episodes` from bounded writer threads. +The callback must not retain mutable live state or operate on the World. +Partial quotas remain explicit failures even when some episodes were exported. + +## Planning and validation + +The optional cooperative planner merges compatible fixed-grid motion requests +and pure FK/IK/Jacobian calls. Budget-bearing whole-motion requests keep their +original execution so charge semantics are preserved. Solvers declaring the +strong sequential-path capability also merge compatible native path requests, +retaining each row's preceding-valid seed and independently owned results. +Context variables and +candidate random streams remain row-local; results are independently owned. +Default CPU command gathering shares native numeric reads through a bounded +World-owner scope in `sim/objects/backends/_numeric_observations.py`. It ends +before completion callbacks and physical stepping, and starts after reset and +activation. Checked native writes, physics steps and binding topology changes +invalidate samples. Derived dynamic rigid poses share the same boundary and invalidation, with +separate CPU autocast representations. Consumers receive independent output +buffers; semantic +state, episode clocks and validation remain row-owned. CUDA and other backends +retain native reads. Raw SDK mutation or concurrent World writes inside the +read boundary are outside this internal side-effect-free planning contract. +Semantic programs and some trajectory processing still run per row. Per-control +planning readiness is a barrier; this API does not claim complete vectorization +or universal throughput improvement. + +`NewtonCollisionChecker.check_batch` uses the same physical geometry, pair +exemptions, halfspace supports and precision/saturation rules as scalar checks. +It returns discrete row reports, without claiming continuous clearance. Its +query model is never stepped and is not another simulation World. +`MeasuredTaskMonitor.batch_scope` shares measured numeric inputs while preserving +each native event state machine and receipts. `ContactPermissionChecker` checks +all valid native contact slots with the original applied-impulse and loss gates; +hosts still own sampling at every required physics substep. + +`MeasuredExpansionValidator` uses these same batch interfaces. Compatible rows +share one immutable geometry declaration/checker; selected rows are compacted +into bounded Newton queries and mapped back in their original order. Different +geometry retains separate checker groups. Query models never step physics. +Predicate inputs share the current complete measured snapshot, while events, +consecutive counts, failures and contact holds remain row-owned. Shape snapshots +are needed only by declared support-bound predicates; native-contact rows do not +gain a homogeneous-topology requirement. + +The built-in joint transport writes all of a frame's destinations into one owned +output. Single commands, subclasses, replaced encoder methods and custom +transports retain normal dispatch and registration order. Numerical/frame checks, +inactive holds and position-velocity evidence remain unchanged. Episode assembly +shares validated immutable segment metadata internally; mutable readback still +owns its containers. Durable JSON uses the same finite sorted representation and +fsync/atomic publication, with one encoded text allocation. + +Default GPU dynamics use Direct GPU reporting, whose static contact identity +limitations remain explicit admission constraints. Physics devices and task +qualification must be reported separately. Existing CPU deployments do not +switch devices. + +## Optional task-pose proposal feedback + +Task-facing expansion configs select `scene_sampling.strategy` (`bounded_pose` +or `workspace_joint`) independently of `scheduling.policy` (`fifo` or +`task_pose_coverage`). Absent settings preserve the legacy bounded sampler. +`engine.nominal_seed` optionally fixes the authored scene reset independently of +the expansion seed. Without it, both retain the historical shared seed. Runtime +candidate streams and proposal streams still use the expansion seed/ordinal; +the reference manifest records both seeds. This prevents a sampler comparison +from silently changing the nominal scene's reset randomization and moving its +declared local domain. Rolling resets use the same explicit nominal reset seed. +The raw pose budget enforces the original admission deadline. An admitted +trajectory's planning view retains shared IK/planning call limits and explicit +cancellation without treating that deadline as a new mid-trajectory hard stop. +The original max_steps, physical checks and safe-stop handshake still apply. +`_pose_sampling.py` owns bounded candidate snapshots, source feedback and budgets; +`task_pose_coverage.TaskPoseCoverage` implements the session coverage protocol. +Measured rigid initial/terminal poses contribute only after native sink receipts. +An optional `pose_coverage.measurements` mapping selects entity states at initial, +terminal or an explicitly named successful segment end. Its grids use the same +names. This supports held-object and multi-stage tasks without creating fake +place-reference entities. `fixed: true` contributes one category inside its +declared grid; tracking noise does not create placement novelty. Proposal ranking +predicts only initial cells; later boundaries require measured T+1 evidence. +Separate SceneCase identities aggregate within a declared configuration/grid +fingerprint; installation changes remain separate domains. FIFO can omit coverage. + +Joint sampling uses an explicit cache or joint-uniform FK prior and repeats +full-pose IK/FK checks after planar projection. Fixed pick grasp policy and prior +must agree; unsupported dynamic grasp replacement rejects. Normal scene preparation, +planning, measured acceptance and persistence still determine success. Feedback +checkpoints do not restore a live World or replace job receipt reconciliation. +The configured acceptance task is +`extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place`; +its four-way runner is `scripts/benchmark/expansion/task_pose_coverage.py`. + +## Conditional layout proposals and task screening + +`scene_sampling.strategy: scene_first` adds a prepared support-tree layout source +before ordinary native execution. `_scene_first_sampling.py` reuses shared scene +preparation/conditional generation and `RobotFeasibilityBackend`/`plan_task_mode`; +it creates no physics World and owns only the isolated collision-query resources. +`layout` and `task_mode` are explicit, validated task declarations. Root layout +variables currently use XY only; selected nested members retain declared XY/yaw. +Homogeneous replicas support rolling W times B collection, including eight rows. +Preparation snapshots the common prototype geometry, installation and nominal +full-joint planning start before any row executes. The feasibility adapter uses +an explicit solver row and never treats an active peer's live posture as its +planning start. It creates no additional physical World. A `bounded_pose` job may opt into the +same layout/task-mode screens for a controlled comparison without changing +ordinary simple-sampling defaults. Screening budgets are bounded and separate +from fixed physical quality thresholds. Prepared candidates stay immutable by +ordinal; failed screening cannot turn into an accepted simple fallback. +Static policy and task-mode decoding use shared simulator-free declarations. +Authored anchors must satisfy the prepared layout domain. An out-of-domain +bounded draw rejects only that proposal. Followers outside the support tree +inherit the final parent's displacement before task screening. + +`robot_first` anchors one movable support-tree root per proposal from the +workspace of a declared `robot_anchors` phase/part target. It uses the target's +initial rigid relation to the root and projects current-arena TCP XY onto the +root's allowed XY domain, preserving authored height and rotation. Per-part FK +populations are prepared once and selected across occupied cells. The layout +generator reserves the anchor and completes the remaining groups. A prior +without an in-domain sample is a budgeted rejection. + +`bidirectional` alternates scene-first and robot-first by raw ordinal, including +rejections. Attempt budgets, FIFO or task-pose coverage selection, execution and +receipt rules stay in their shared owners. The source records the direction of +each draw and selection, and records the requested anchor separately from the +generated and measured poses. + +The current geometry, joint limits, full-pose FK residuals, full-state/mimic +branches and synchronized path collision checks remain in their existing owners. +Actual execution replans and measured acceptance/persistence remain mandatory. +`scene-first-checks.json` retains phase results, workspace prior and anchor +evidence, and the unchanged World clock; +the query is closed before the owner environment, including setup failures. +The reproducible cross-task runner is +`scripts/benchmark/expansion/scene_first.py`; per-task fixtures are under the +existing grab/place, stack and dual-arm RGB sorting task configurations. The +sorting program operates the two arms sequentially and retains its original +segment validators; receipt-based coverage counts both manipulated blocks and +does not turn fixed-goal tracking noise into placement diversity. + +## Focused validation + +`tests/lab/expansion/test_workloads_sim.py` runs a real four-row task through +native session admission and artifact receipts. It covers rejected-row safe +holds, refill, queued groups, no missing control boundaries, scalar/batched task +predicates and compatible whole-motion result parity. Geometry and contact +coverage lives in the existing native feasibility/contact suites. Per-episode +export uses the real LeRobot SDK and checks portable T+1 evidence. diff --git a/agent_context/topics/gen-sim/gen-sim.md b/agent_context/topics/gen-sim/gen-sim.md index 9a524e18b5..ccb97da5a2 100644 --- a/agent_context/topics/gen-sim/gen-sim.md +++ b/agent_context/topics/gen-sim/gen-sim.md @@ -65,6 +65,12 @@ normative TaskSpec certification remain distinct. See the [command guide](../../../embodichain/gen_sim/README.md) and provider-free [scripted reproduction](../../../examples/gensim/scripted_pick_place/README.md). +Automatic placements default to 0.02 m release clearance above their measured +resting target and a 0.4 s open-command hold on the native control grid. Explicit +clearance/Place timing declarations keep their ownership. Opening, terminal +separation, resting-position and stability evidence are unchanged; clearance +and actuation time are planning behavior, not weaker acceptance thresholds. + `_run_image.py` owns the optional head-frame preprocessing handoff. A verified `input_camera` declaration pins the raw head frame by hash. Task Engine and scene generation consume a single effective image. Task Plan reuse copies the diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index df16f69a42..900db5cad7 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -53,12 +53,28 @@ positions `(B, N, dof)`. Use `PlanState.single()` for one environment. duration is their sum. Failed results may omit positions. Planning timing never changes simulator `physics_dt`. +`MotionGenOptions.waypoint_modes` explicitly selects joint/Cartesian paths to +individual keyframes under fixed-grid sequential IK. Adjacent equal modes share +ordinary native requests; composition retains the total control grid and row +failure masks. No robot UID or waypoint count enables the behavior. Atomic +`MotionPolicy.phase_paths` names the PickUp/Place phases that opt into this path; +ordinary calls and other primitives retain their own planning semantics. +All inserted Place translation keyframes inherit their original phase's mode. +PickUp's explicit `cartesian_lift` and Place's `cartesian_approach` are mutually +exclusive with `phase_paths`; conflicting declarations reject before trajectory +generation. + `motion_gen` delegates timing to its backend. `ik_interp` needs explicit start/count/dt, rejects backend-specific options and uses minimum-jerk segments when limits apply. Adaptive timing and batched IK remain defaults. Explicit `fixed_grid` preserves count/intervals and rejects overspeed rows against runtime and authored joint caps; `sequential` retains previous-valid-posture seeds and callback/RNG order. Neither mode is a generic option for other strategies. +Solvers may opt into `supports_sequential_batch_ik` only when their continuous +path hook enumerates the ordinary candidate pool against each preceding valid +seed. SSIK Warp SRS `seed_local` supplies this stronger contract, allowing +bounded path batches instead of Python point dispatch. Other solvers and active +planning budgets retain the original point-query sequence and charging rules. Rejected rows hold their start; `rejected_candidate` retains original samples and rejected-row identity for audit/recovery. A zero-speed hold does not prove @@ -86,6 +102,12 @@ unchanged physics substeps; Gym experts use their environment's command period. prepared actions without retiming again. See [retiming and playback](planner-details.md#retiming-and-playback). +Gripper surface queries with CPU inputs still classify convex distances on CUDA. +Native multi-chunk calls own one query-local geometry snapshot and bounded scratch, +retain CPU point/ground transforms and all samples, and return owned reduced CPU +results. This is distinct from the simulation physics device. Visualization, +custom point-query overrides and already-device-resident callers keep their paths. + GraspKit owns sampled/native geometric qualification from explicit hand inputs; Scene captures geometry and Robot adapters provide FK/calibration. GraspKit performs no hardware lookup. Sampling uses the contact frame and maps to runtime diff --git a/agent_context/topics/motion-planning/trajectory-expansion.md b/agent_context/topics/motion-planning/trajectory-expansion.md index 6a626491df..d6faa8a58f 100644 --- a/agent_context/topics/motion-planning/trajectory-expansion.md +++ b/agent_context/topics/motion-planning/trajectory-expansion.md @@ -65,6 +65,10 @@ position search reuses its pool; approach search draws one pool per direction. Each successful row freezes independently. Per-row resets or filtering before ranking/NMS change populations. Freeze grasp, direction and position before comparing planners and review population fingerprints after selection changes. +`utils.nms.pose_nms_scope` supplies a nested, task-local pairwise tile budget; +explicit call arguments override it. It preserves threshold arithmetic and +selection order without changing an imported backend function. Copied planning +contexts inherit the budget while independent threads keep their own default. Explicit goal-conditioned endpoint searches also change the population and retain rejected goals; they establish neither paths nor physical success. @@ -89,3 +93,13 @@ separate from path, dynamic and task validation. Use `tests/sim/motion/expansion/` for algorithm/contracts coverage and host tests for stepping, acceptance and persistence. Examples are not production test APIs. + +## Explicit boundary draws + +Spatial profiles optionally carry `joint_samples` (named controlled-joint signed +fractions) and `knot_samples` (via-point magnitudes), each finite in [-1,1]. +The combined Task Program adapter forwards them to the ordinary operators; +absence retains local-RNG sampling. Explicit draws never bypass phase permissions, +endpoint protection, deduplication, joint limits or measured acceptance. One named +joint draw is reused across permitted phases. The Expansion Engine preview owns +parameter-domain sampling and coverage accounting, not the motion operators. diff --git a/agent_context/topics/scene-expansion/scene-expansion.md b/agent_context/topics/scene-expansion/scene-expansion.md index bc4ecbf6c2..ae6adb910c 100644 --- a/agent_context/topics/scene-expansion/scene-expansion.md +++ b/agent_context/topics/scene-expansion/scene-expansion.md @@ -11,6 +11,12 @@ imported from `embodichain.lab.sim.scene.expansion`. movable permissions. `prepare_scene_randomization` prepares geometry; `generate_scene_variants` returns addressed CPU/CUDA candidates; `apply_scene_variants` preflights selected rigid poses without stepping. +`SceneLayoutCondition` restricts prepared generation to explicit arena-pose +anchors, with root XY/member XY-yaw permissions retained. Moving ancestors +must also be anchored. Anchored roots reserve space before unanchored groups +are sampled. Invalid domains reject before generation; unsupported +geometry returns budget exhaustion rather than silently moving the anchors. +The source/policy remain reusable; empty conditions preserve the old stream. `SimulationSceneExpansionHost` prepares/restores ordered rows with independent validation. Offline scheduling and confirmed persistence belong to the [Expansion Engine](../expansion-engine/expansion-engine.md). @@ -28,10 +34,30 @@ Joint task-conditioned layout/installation search is owned by attempts. Its configured search adapter still evaluates one B=1 deployment per attempt through the shared selected-row preparation/restoration contract. +The task-facing rolling collector also exposes selectable bounded versus +workspace-conditioned pose proposals with optional receipt-confirmed task-pose +coverage feedback; see [workload feedback](../expansion-engine/workloads.md#optional-task-pose-proposal-feedback). +The collector's optional scene-first, robot-first and bidirectional strategies +compose support-tree layout generation with the existing full-state task +feasibility backend. Robot-first projects a declared target's workspace TCPs to +root XY conditions before generating the rest of the layout. See the +[conditional layout source contract](../expansion-engine/workloads.md#conditional-layout-proposals-and-task-screening) +for selectors and direction scheduling. Explicit +`layout` and `task_mode` declarations bind support parents, permissions and +synchronized task stages. Homogeneous replicas can share the prepared layout +and frozen nominal planning start within one rolling W times B World; the +feasibility adapter explicitly selects its solver row. A bounded-sampling +baseline can opt into the same screens. Declared +workspace caches supply seeds; no cache hit certifies a pose or task. Pure +layout queries do not advance the owning World; topology changes invalidate +the prepared geometry. This is distinct from offline atlas region validation +and does not certify continuous reachable volume. + ## Entry points | Request | Owner | |---|---| +| Simulator-free layout policy and feasibility declarations | `embodichain/lab/_scene_config/` | | Shared proposal contracts and fixed source | `embodichain/lab/sim/scene/expansion/` | | Candidate limits and movable entity permissions | `cfg.py` in that package | | Arena poses and proposal identity | `contracts.py` in that package | diff --git a/agent_context/topics/sensor-system/sensor-system.md b/agent_context/topics/sensor-system/sensor-system.md index 6caa5d3a25..92d688738a 100644 --- a/agent_context/topics/sensor-system/sensor-system.md +++ b/agent_context/topics/sensor-system/sensor-system.md @@ -228,6 +228,15 @@ changing filtering, force assumptions, topology handling or actor metadata. Geometry-only contacts and multiple backend-emitted rows per actor pair are valid; sensor adaptation does not synthesize a common contact manifold. +`ContactPermissionChecker` consumes complete native samples with explicit +actor/frame permissions, returning owned per-row device summaries. It does not +choose the host's sampling frequency or relax unavailable-impulse/loss gates. +Eager checks wait for the latest sensor sample on their CUDA stream without +refreshing contacts or advancing histories. +Global static actors retain their explicit row memberships. Rebind after query +topology changes. Default CUDA dynamics use Direct GPU reporting; its static +actor identity limitations remain explicit admission constraints. + The contact scatter kernel remains sensor-owned because its columns, IDs and quota are integration contracts. Generic image tiling belongs to `embodichain/compute/image/_warp/tiling.py`; compatibility exports under diff --git a/agent_context/topics/sim-visualization/sim-visualization.md b/agent_context/topics/sim-visualization/sim-visualization.md index ed739ac24a..86f592ac06 100644 --- a/agent_context/topics/sim-visualization/sim-visualization.md +++ b/agent_context/topics/sim-visualization/sim-visualization.md @@ -103,6 +103,15 @@ with visual origins. URDF FK accepts all `joint_names`, including caller-resolve mimic joints. Multiple poses can be displayed in one environment. Tasks own motion loading, sampling and alignment. +`MarkerFramePublisher` stages caller-owned groups for one cross-group native +publication. Initial preload retains hidden resources; subsequent frames upload +visible instances and change real visibility without moving hidden geometry. +`MarkerGroup.snapshot(visible_only=True)` preserves original instance IDs while +omitting hidden meshes. Close the publisher before its World. The measured +reference/ghost timeline and keyboard playback live in +`lab/expansion/comparison.py`; task/environment and collection ownership stay +with the caller. + `RobotMarkersAppearanceCfg` selects source appearance or a base-color override, with adjustable opacity. Source textures and material parameters are preserved; opaque visuals use scene lighting. Transparent shading may differ by renderer. diff --git a/docs/architecture/curated.json b/docs/architecture/curated.json index 23cce03d11..faefda5424 100644 --- a/docs/architecture/curated.json +++ b/docs/architecture/curated.json @@ -1275,9 +1275,9 @@ { "path": "embodichain/lab/sim/scene/expansion/__init__.py", "symbol": "", - "start_line": 25, - "end_line": 27, - "excerpt": "from .cfg import SceneExpansionCfg\nfrom .contracts import ScenePoseChange, SceneVariant\nfrom .source import FixedSceneCandidateSource" + "start_line": 28, + "end_line": 31, + "excerpt": "from .cfg import SceneExpansionCfg\nfrom .contracts import ScenePoseChange, SceneVariant\nfrom .conditions import SceneLayoutCondition\nfrom .source import FixedSceneCandidateSource" }, { "path": "embodichain/lab/sim/scene/expansion/source.py", diff --git a/docs/architecture/generated/architecture.json b/docs/architecture/generated/architecture.json index 72669cdbd4..b60d4bf307 100644 --- a/docs/architecture/generated/architecture.json +++ b/docs/architecture/generated/architecture.json @@ -3,7 +3,7 @@ "kind": "static-architecture", "coverage": "curated-sample", "repository": "DexForce/EmbodiChain", - "revision": "5d4561aca503f7897af8cfcd3246f9d7e3f701d4", + "revision": "37014e73d252f8c7c9d89f6ca081306f55f882bd", "source_ref": "HEAD", "package_version": "0.3.2", "topic_index": "agent_context/MAP.yaml", @@ -145,8 +145,8 @@ { "path": "embodichain/lab/sim/objects/articulation.py", "symbol": "Articulation", - "start_line": 724, - "end_line": 724, + "start_line": 739, + "end_line": 739, "excerpt": "class Articulation(BatchEntity):" } ], @@ -496,8 +496,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "BufferedGymCommandSink", - "start_line": 864, - "end_line": 864, + "start_line": 892, + "end_line": 892, "excerpt": "class BufferedGymCommandSink:" } ], @@ -530,8 +530,8 @@ { "path": "embodichain/lab/task_program/compiler/program.py", "symbol": "CompiledTaskProgram", - "start_line": 1343, - "end_line": 1343, + "start_line": 1345, + "end_line": 1345, "excerpt": "class CompiledTaskProgram:" } ], @@ -681,8 +681,8 @@ { "path": "embodichain/lab/gym/utils/gym_utils.py", "symbol": "build_env_cfg_from_args", - "start_line": 1954, - "end_line": 1954, + "start_line": 1978, + "end_line": 1978, "excerpt": "def build_env_cfg_from_args(" } ], @@ -1181,8 +1181,8 @@ { "path": "embodichain/lab/scripts/run_task.py", "symbol": "cli", - "start_line": 2015, - "end_line": 2015, + "start_line": 2059, + "end_line": 2059, "excerpt": "def cli(argv: Sequence[str] | None = None) -> None:" } ], @@ -1418,8 +1418,8 @@ { "path": "embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py", "symbol": "GripperCollisionChecker", - "start_line": 39, - "end_line": 39, + "start_line": 96, + "end_line": 96, "excerpt": "class GripperCollisionChecker:" } ], @@ -1556,8 +1556,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge", - "start_line": 1149, - "end_line": 1149, + "start_line": 1177, + "end_line": 1177, "excerpt": "class TaskProgramDemoBridge:" } ], @@ -1591,8 +1591,8 @@ { "path": "embodichain/lab/task_program/integrations/catalog.py", "symbol": "TaskProgramIntegrationCatalog", - "start_line": 611, - "end_line": 611, + "start_line": 618, + "end_line": 618, "excerpt": "class TaskProgramIntegrationCatalog:" } ], @@ -1765,8 +1765,8 @@ { "path": "embodichain/lab/sim/motion/motion_generator.py", "symbol": "MotionGenerator", - "start_line": 310, - "end_line": 310, + "start_line": 334, + "end_line": 334, "excerpt": "class MotionGenerator:" } ], @@ -2125,8 +2125,8 @@ { "path": "embodichain/lab/task_program/compiler/program.py", "symbol": "TaskProgramCompiler", - "start_line": 1613, - "end_line": 1613, + "start_line": 1615, + "end_line": 1615, "excerpt": "class TaskProgramCompiler:" } ], @@ -2310,8 +2310,8 @@ { "path": "embodichain/lab/sim/objects/rigid_object.py", "symbol": "RigidObject", - "start_line": 430, - "end_line": 430, + "start_line": 434, + "end_line": 434, "excerpt": "class RigidObject(BatchEntity):" } ], @@ -2607,8 +2607,8 @@ "path": "embodichain/lab/sim/scene/expansion/__init__.py", "symbol": "", "start_line": 28, - "end_line": 30, - "excerpt": "from .cfg import SceneExpansionCfg\nfrom .contracts import ScenePoseChange, SceneVariant\nfrom .source import FixedSceneCandidateSource" + "end_line": 31, + "excerpt": "from .cfg import SceneExpansionCfg\nfrom .contracts import ScenePoseChange, SceneVariant\nfrom .conditions import SceneLayoutCondition\nfrom .source import FixedSceneCandidateSource" }, { "path": "embodichain/lab/sim/scene/expansion/source.py", @@ -2947,8 +2947,8 @@ { "path": "embodichain/lab/task_program/compiler/lowering.py", "symbol": "SemanticCallCompiler", - "start_line": 1033, - "end_line": 1033, + "start_line": 1046, + "end_line": 1046, "excerpt": "class SemanticCallCompiler:" } ], @@ -3116,8 +3116,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager", - "start_line": 562, - "end_line": 562, + "start_line": 580, + "end_line": 580, "excerpt": "class SimulationManager:" } ], @@ -3349,8 +3349,8 @@ { "path": "embodichain/gen_sim/task_engine/semantic_plan_validation.py", "symbol": "_validate_interpreted_plan", - "start_line": 824, - "end_line": 831, + "start_line": 838, + "end_line": 845, "excerpt": " \"status\": \"covered\",\n \"semantic_status\": \"valid\",\n \"visual_status\": (\"complete\" if observed == total else \"pending\"),\n \"pending_bindings\": len(bindings),\n \"visual_observed_entities\": observed,\n \"visual_total_entities\": total,\n \"execution_ready\": False,\n \"runtime_evidence_status\": \"not_run\"," } ], @@ -3947,8 +3947,8 @@ { "path": "embodichain/lab/sim/objects/articulation.py", "symbol": "Articulation", - "start_line": 724, - "end_line": 724, + "start_line": 739, + "end_line": 739, "excerpt": "class Articulation(BatchEntity):" } ] @@ -3983,8 +3983,8 @@ { "path": "embodichain/lab/gym/envs/base_env.py", "symbol": "BaseEnv._setup_scene", - "start_line": 535, - "end_line": 539, + "start_line": 545, + "end_line": 549, "excerpt": " self.sim = SimulationManager(\n self.sim_cfg,\n defer_startup_summary=True,\n requires_native_renderer=requires_native_renderer,\n )" } ] @@ -4001,8 +4001,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge.__init__", - "start_line": 1287, - "end_line": 1287, + "start_line": 1315, + "end_line": 1315, "excerpt": " self._clock = clock" } ] @@ -4019,8 +4019,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge.__init__", - "start_line": 1285, - "end_line": 1285, + "start_line": 1313, + "end_line": 1313, "excerpt": " self._runtime = runtime" }, { @@ -4044,8 +4044,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge.__init__", - "start_line": 1284, - "end_line": 1284, + "start_line": 1312, + "end_line": 1312, "excerpt": " self._program = program" }, { @@ -4069,8 +4069,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge.__init__", - "start_line": 1286, - "end_line": 1286, + "start_line": 1314, + "end_line": 1314, "excerpt": " self._sink = command_sink" } ] @@ -4087,8 +4087,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge._segment_actions", - "start_line": 1995, - "end_line": 1995, + "start_line": 2023, + "end_line": 2023, "excerpt": " self._sink.pop()," } ] @@ -4331,15 +4331,15 @@ { "path": "embodichain/lab/task_program/compiler/program.py", "symbol": "TaskProgramCompiler._compile", - "start_line": 2379, - "end_line": 2384, + "start_line": 2381, + "end_line": 2386, "excerpt": " return _materialize_program(\n program_id=config.program_id,\n instruction=config.instruction,\n integration=integration,\n targets=targets,\n root=root," }, { "path": "embodichain/lab/task_program/compiler/program.py", "symbol": "_materialize_program", - "start_line": 1605, - "end_line": 1610, + "start_line": 1607, + "end_line": 1612, "excerpt": " return CompiledTaskProgram._create(\n program_id=program_id,\n instruction=instruction,\n integration=integration,\n segments=tuple(segments),\n )" } ] @@ -4356,8 +4356,8 @@ { "path": "embodichain/lab/task_program/compiler/program.py", "symbol": "TaskProgramCompiler._compile", - "start_line": 2366, - "end_line": 2368, + "start_line": 2368, + "end_line": 2370, "excerpt": " targets = self._compile_targets(config.targets)\n root = self._compile_node(\n config.program," } ] @@ -4399,15 +4399,15 @@ { "path": "embodichain/lab/gym/utils/gym_utils.py", "symbol": "build_env_cfg_from_args", - "start_line": 2012, - "end_line": 2017, + "start_line": 2036, + "end_line": 2041, "excerpt": " cfg: EmbodiedEnvCfg = config_to_cfg(\n gym_config,\n manager_modules=get_manager_modules(),\n source_path=gym_config_source_path,\n task_program_path_override=getattr(args, \"task_program\", None),\n )" }, { "path": "embodichain/lab/scripts/run_task.py", "symbol": "cli", - "start_line": 2185, - "end_line": 2185, + "start_line": 2229, + "end_line": 2229, "excerpt": " env = gymnasium.make(id=gym_config[\"id\"], cfg=env_cfg, **action_config)" } ] @@ -4604,8 +4604,8 @@ { "path": "pyproject.toml", "symbol": "", - "start_line": 100, - "end_line": 101, + "start_line": 101, + "end_line": 102, "excerpt": "[project.entry-points.\"embodichain.tasks\"]\nembodichain_tasks = \"embodichain_tasks\"" }, { @@ -4697,8 +4697,8 @@ { "path": "embodichain/lab/gym/envs/embodied_env.py", "symbol": "EmbodiedEnv.create_task_program_bridge", - "start_line": 4311, - "end_line": 4311, + "start_line": 4316, + "end_line": 4316, "excerpt": " bridge = self._checked_task_program_adapter().create_bridge(" } ] @@ -4715,8 +4715,8 @@ { "path": "embodichain/lab/gym/envs/embodied_env.py", "symbol": "EmbodiedEnv.compile_task_program", - "start_line": 4246, - "end_line": 4246, + "start_line": 4251, + "end_line": 4251, "excerpt": " return self._checked_task_program_adapter().compile(program)" } ] @@ -4733,8 +4733,8 @@ { "path": "embodichain/lab/gym/envs/embodied_env.py", "symbol": "EmbodiedEnv._declare_robot", - "start_line": 3970, - "end_line": 3970, + "start_line": 3975, + "end_line": 3975, "excerpt": " return self.sim.add_robot(robot_cfg)" } ] @@ -4981,8 +4981,8 @@ { "path": "embodichain/lab/scripts/run_task.py", "symbol": "cli", - "start_line": 2053, - "end_line": 2053, + "start_line": 2097, + "end_line": 2097, "excerpt": " env_cfg, gym_config, action_config = build_env_cfg_from_args(args)" } ] @@ -4999,8 +4999,8 @@ { "path": "embodichain/lab/scripts/run_task.py", "symbol": "cli", - "start_line": 2048, - "end_line": 2048, + "start_line": 2092, + "end_line": 2092, "excerpt": " discover_task_packages()" } ] @@ -5251,8 +5251,8 @@ { "path": "embodichain/lab/sim/objects/rigid_object.py", "symbol": "RigidObject", - "start_line": 430, - "end_line": 430, + "start_line": 434, + "end_line": 434, "excerpt": "class RigidObject(BatchEntity):" } ] @@ -5819,15 +5819,15 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_robot", - "start_line": 4395, - "end_line": 4396, + "start_line": 4638, + "end_line": 4639, "excerpt": " robot = self._declare_spawn_articulation(cfg, Robot)\n self._robots[uid] = robot" }, { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager._declare_spawn_articulation", - "start_line": 4435, - "end_line": 4438, + "start_line": 4678, + "end_line": 4681, "excerpt": " facade = facade_type(\n cfg=cfg,\n device=self.device,\n )" } ] @@ -5844,8 +5844,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.start_visualization", - "start_line": 1194, - "end_line": 1199, + "start_line": 1245, + "end_line": 1250, "excerpt": " runtime = VisualizationRuntime(\n SceneExporter(self, visualization_cfg),\n visualization_cfg,\n )\n self._visualization_runtime = runtime\n runtime.start()" } ] @@ -5862,8 +5862,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_robot", - "start_line": 4396, - "end_line": 4401, + "start_line": 4639, + "end_line": 4644, "excerpt": " self._robots[uid] = robot\n if was_materialized:\n self.prepare()\n for sensor_cfg in mounted_sensors:\n self.add_sensor(sensor_cfg)\n return robot" } ] @@ -5880,8 +5880,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.enable_gizmo", - "start_line": 4591, - "end_line": 4591, + "start_line": 4834, + "end_line": 4834, "excerpt": " gizmo = Gizmo(target, gizmo_cfg, control_part)" } ] @@ -5898,8 +5898,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.create_rigid_constraint", - "start_line": 4041, - "end_line": 4041, + "start_line": 4300, + "end_line": 4300, "excerpt": " constraint = RigidConstraint(" } ] @@ -5916,8 +5916,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_rigid_object_group", - "start_line": 4182, - "end_line": 4182, + "start_line": 4441, + "end_line": 4441, "excerpt": " group = RigidObjectGroup(" } ] @@ -5934,8 +5934,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager._declare_rigid_objects", - "start_line": 3462, - "end_line": 3462, + "start_line": 3721, + "end_line": 3721, "excerpt": " rigid_obj = RigidObject(cfg=cfg, device=self.device)" } ] @@ -5952,8 +5952,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_articulation", - "start_line": 4283, - "end_line": 4283, + "start_line": 4542, + "end_line": 4542, "excerpt": " self._articulations[uid] = articulation" } ] @@ -5970,8 +5970,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_deformable_object", - "start_line": 3841, - "end_line": 3841, + "start_line": 4100, + "end_line": 4100, "excerpt": " self._deformable_objects[uid] = deformable" } ] @@ -5988,8 +5988,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_light", - "start_line": 2973, - "end_line": 2973, + "start_line": 3227, + "end_line": 3227, "excerpt": " self._lights[uid] = batch_lights" } ] @@ -6006,8 +6006,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "BufferedGymCommandSink.__init__", - "start_line": 880, - "end_line": 881, + "start_line": 908, + "end_line": 909, "excerpt": " self._encoder = encoder\n self._clock = clock" } ] @@ -6030,24 +6030,6 @@ } ] }, - { - "id": "static-imports-01a9592b5f6f", - "source": "simulation-manager", - "target": "configuration", - "relation": "imports", - "description": "The selected source module declares an absolute import associated with this target.", - "provenance": "static-extracted", - "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", - "evidence": [ - { - "path": "embodichain/lab/sim/sim_manager.py", - "symbol": "", - "start_line": 166, - "end_line": 166, - "excerpt": "from embodichain.utils import configclass, logger" - } - ] - }, { "id": "static-imports-023d746bdd8d", "source": "scene-generation", @@ -6084,6 +6066,42 @@ } ] }, + { + "id": "static-imports-02ef4754d36f", + "source": "simulation-manager", + "target": "sensors", + "relation": "imports", + "description": "The selected source module declares an absolute import associated with this target.", + "provenance": "static-extracted", + "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", + "evidence": [ + { + "path": "embodichain/lab/sim/sim_manager.py", + "symbol": "", + "start_line": 106, + "end_line": 106, + "excerpt": "from embodichain.lab.sim.sensors import _builtin_sensor_factories" + } + ] + }, + { + "id": "static-imports-0439aa147cbd", + "source": "integration-catalog", + "target": "task-program-config", + "relation": "imports", + "description": "The selected source module declares an absolute import associated with this target.", + "provenance": "static-extracted", + "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", + "evidence": [ + { + "path": "embodichain/lab/task_program/integrations/catalog.py", + "symbol": "", + "start_line": 96, + "end_line": 104, + "excerpt": "from embodichain.lab.task_program.language.schema import (\n ConditionValidatorCfg,\n TaskProgramCfg,\n TaskProgramIntegrationCfg,\n PostPolicyCfg,\n RegisteredSemanticCallCfg,\n SemanticCallCfg,\n ValidatorCfg,\n)" + } + ] + }, { "id": "static-imports-095579fb8b55", "source": "differentiable-trainer", @@ -6336,6 +6354,24 @@ } ] }, + { + "id": "static-imports-1cb8ca231ed7", + "source": "integration-catalog", + "target": "semantic-compiler", + "relation": "imports", + "description": "The selected source module declares an absolute import associated with this target.", + "provenance": "static-extracted", + "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", + "evidence": [ + { + "path": "embodichain/lab/task_program/integrations/catalog.py", + "symbol": "", + "start_line": 79, + "end_line": 85, + "excerpt": "from embodichain.lab.task_program.compiler.lowering import (\n ContainerRelationTargetGrounder,\n HandOverPoseProvider,\n RegisteredSemanticLowerer,\n RelationTargetGrounder,\n SupportSurfaceRelationTargetGrounder,\n)" + } + ] + }, { "id": "static-imports-1d52e30c0b32", "source": "articulations", @@ -6355,9 +6391,9 @@ ] }, { - "id": "static-imports-1fcaaf2a281e", + "id": "static-imports-1efe7f42a374", "source": "env-launcher", - "target": "task-discovery", + "target": "config-loader", "relation": "imports", "description": "The selected source module declares an absolute import associated with this target.", "provenance": "static-extracted", @@ -6366,9 +6402,9 @@ { "path": "embodichain/lab/scripts/run_task.py", "symbol": "", - "start_line": 54, - "end_line": 57, - "excerpt": "from embodichain.lab.gym.utils.registration import (\n discover_task_packages,\n execute_init_hooks,\n)" + "start_line": 50, + "end_line": 56, + "excerpt": "from embodichain.lab.gym.utils.gym_utils import (\n add_env_launcher_args_to_parser,\n build_env_cfg_from_args,\n configure_fluid_spacing,\n load_trajectory,\n _load_expansion_declaration,\n)" } ] }, @@ -6498,24 +6534,6 @@ } ] }, - { - "id": "static-imports-30e2933cfbeb", - "source": "simulation-manager", - "target": "sensors", - "relation": "imports", - "description": "The selected source module declares an absolute import associated with this target.", - "provenance": "static-extracted", - "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", - "evidence": [ - { - "path": "embodichain/lab/sim/sim_manager.py", - "symbol": "", - "start_line": 95, - "end_line": 104, - "excerpt": "from embodichain.lab.sim.sensors import (\n SensorCfg,\n BaseSensor,\n Camera,\n CameraCfg,\n StereoCamera,\n StereoCameraCfg,\n ContactSensor,\n SensorContext,\n)" - } - ] - }, { "id": "static-imports-31678f8689cc", "source": "urdf-assembly", @@ -6984,24 +7002,6 @@ } ] }, - { - "id": "static-imports-67a3a6ca9069", - "source": "env-launcher", - "target": "config-loader", - "relation": "imports", - "description": "The selected source module declares an absolute import associated with this target.", - "provenance": "static-extracted", - "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", - "evidence": [ - { - "path": "embodichain/lab/scripts/run_task.py", - "symbol": "", - "start_line": 47, - "end_line": 53, - "excerpt": "from embodichain.lab.gym.utils.gym_utils import (\n add_env_launcher_args_to_parser,\n build_env_cfg_from_args,\n configure_fluid_spacing,\n load_trajectory,\n _load_expansion_declaration,\n)" - } - ] - }, { "id": "static-imports-680c8e9017d5", "source": "articulations", @@ -7165,38 +7165,38 @@ ] }, { - "id": "static-imports-72897e797d52", - "source": "robot-world-model", - "target": "configuration", + "id": "static-imports-726d1e8f3927", + "source": "simulation-manager", + "target": "sensors", "relation": "imports", "description": "The selected source module declares an absolute import associated with this target.", "provenance": "static-extracted", "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", "evidence": [ { - "path": "extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py", + "path": "embodichain/lab/sim/sim_manager.py", "symbol": "", - "start_line": 45, - "end_line": 45, - "excerpt": "from embodichain.utils import configclass" + "start_line": 96, + "end_line": 105, + "excerpt": "from embodichain.lab.sim.sensors import (\n SensorCfg,\n BaseSensor,\n Camera,\n CameraCfg,\n StereoCamera,\n StereoCameraCfg,\n ContactSensor,\n SensorContext,\n)" } ] }, { - "id": "static-imports-735fdeb87797", - "source": "simulation-manager", - "target": "gizmo", + "id": "static-imports-72897e797d52", + "source": "robot-world-model", + "target": "configuration", "relation": "imports", "description": "The selected source module declares an absolute import associated with this target.", "provenance": "static-extracted", "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", "evidence": [ { - "path": "embodichain/lab/sim/sim_manager.py", + "path": "extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py", "symbol": "", - "start_line": 94, - "end_line": 94, - "excerpt": "from embodichain.lab.sim.objects.gizmo import Gizmo, GizmoCfg" + "start_line": 45, + "end_line": 45, + "excerpt": "from embodichain.utils import configclass" } ] }, @@ -7237,20 +7237,20 @@ ] }, { - "id": "static-imports-7a0fb061e5eb", - "source": "integration-catalog", - "target": "parallel-execution", + "id": "static-imports-78872b465d8c", + "source": "simulation-manager", + "target": "configuration", "relation": "imports", "description": "The selected source module declares an absolute import associated with this target.", "provenance": "static-extracted", "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", "evidence": [ { - "path": "embodichain/lab/task_program/integrations/catalog.py", + "path": "embodichain/lab/sim/sim_manager.py", "symbol": "", - "start_line": 91, - "end_line": 93, - "excerpt": "from embodichain.lab.task_program.runtime.parallel_executor import (\n ParallelCommandSafetyValidator,\n)" + "start_line": 171, + "end_line": 171, + "excerpt": "from embodichain.utils import configclass, logger" } ] }, @@ -7344,24 +7344,6 @@ } ] }, - { - "id": "static-imports-86942e4dcbbb", - "source": "integration-catalog", - "target": "semantic-compiler", - "relation": "imports", - "description": "The selected source module declares an absolute import associated with this target.", - "provenance": "static-extracted", - "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", - "evidence": [ - { - "path": "embodichain/lab/task_program/integrations/catalog.py", - "symbol": "", - "start_line": 78, - "end_line": 84, - "excerpt": "from embodichain.lab.task_program.compiler.lowering import (\n ContainerRelationTargetGrounder,\n HandOverPoseProvider,\n RegisteredSemanticLowerer,\n RelationTargetGrounder,\n SupportSurfaceRelationTargetGrounder,\n)" - } - ] - }, { "id": "static-imports-87dcd41f5830", "source": "simulation-factory", @@ -7488,6 +7470,24 @@ } ] }, + { + "id": "static-imports-9e8c80d64a20", + "source": "integration-catalog", + "target": "parallel-execution", + "relation": "imports", + "description": "The selected source module declares an absolute import associated with this target.", + "provenance": "static-extracted", + "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", + "evidence": [ + { + "path": "embodichain/lab/task_program/integrations/catalog.py", + "symbol": "", + "start_line": 92, + "end_line": 94, + "excerpt": "from embodichain.lab.task_program.runtime.parallel_executor import (\n ParallelCommandSafetyValidator,\n)" + } + ] + }, { "id": "static-imports-a225c2b26ae1", "source": "environment-adapter", @@ -7543,20 +7543,20 @@ ] }, { - "id": "static-imports-a38ea1f6aa90", - "source": "simulation-manager", - "target": "sensors", + "id": "static-imports-a43d0369f3e6", + "source": "env-launcher", + "target": "task-discovery", "relation": "imports", "description": "The selected source module declares an absolute import associated with this target.", "provenance": "static-extracted", "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", "evidence": [ { - "path": "embodichain/lab/sim/sim_manager.py", + "path": "embodichain/lab/scripts/run_task.py", "symbol": "", - "start_line": 105, - "end_line": 105, - "excerpt": "from embodichain.lab.sim.sensors import _builtin_sensor_factories" + "start_line": 57, + "end_line": 60, + "excerpt": "from embodichain.lab.gym.utils.registration import (\n discover_task_packages,\n execute_init_hooks,\n)" } ] }, @@ -7704,6 +7704,24 @@ } ] }, + { + "id": "static-imports-b2a8f9f1b012", + "source": "simulation-manager", + "target": "physics", + "relation": "imports", + "description": "The selected source module declares an absolute import associated with this target.", + "provenance": "static-extracted", + "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", + "evidence": [ + { + "path": "embodichain/lab/sim/sim_manager.py", + "symbol": "", + "start_line": 142, + "end_line": 142, + "excerpt": "from embodichain.lab.sim.physics import make_physics_backend" + } + ] + }, { "id": "static-imports-b87e50be9752", "source": "recording", @@ -7722,6 +7740,24 @@ } ] }, + { + "id": "static-imports-bacd2acfbd2e", + "source": "simulation-manager", + "target": "gizmo", + "relation": "imports", + "description": "The selected source module declares an absolute import associated with this target.", + "provenance": "static-extracted", + "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", + "evidence": [ + { + "path": "embodichain/lab/sim/sim_manager.py", + "symbol": "", + "start_line": 95, + "end_line": 95, + "excerpt": "from embodichain.lab.sim.objects.gizmo import Gizmo, GizmoCfg" + } + ] + }, { "id": "static-imports-bd39f7e063d7", "source": "embodied-env", @@ -7848,24 +7884,6 @@ } ] }, - { - "id": "static-imports-c84985d043b9", - "source": "simulation-manager", - "target": "physics", - "relation": "imports", - "description": "The selected source module declares an absolute import associated with this target.", - "provenance": "static-extracted", - "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", - "evidence": [ - { - "path": "embodichain/lab/sim/sim_manager.py", - "symbol": "", - "start_line": 137, - "end_line": 137, - "excerpt": "from embodichain.lab.sim.physics import make_physics_backend" - } - ] - }, { "id": "static-imports-c8cfa7225b4a", "source": "embodied-env", @@ -7956,24 +7974,6 @@ } ] }, - { - "id": "static-imports-de7d2002c14c", - "source": "integration-catalog", - "target": "task-program-config", - "relation": "imports", - "description": "The selected source module declares an absolute import associated with this target.", - "provenance": "static-extracted", - "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", - "evidence": [ - { - "path": "embodichain/lab/task_program/integrations/catalog.py", - "symbol": "", - "start_line": 95, - "end_line": 103, - "excerpt": "from embodichain.lab.task_program.language.schema import (\n ConditionValidatorCfg,\n TaskProgramCfg,\n TaskProgramIntegrationCfg,\n PostPolicyCfg,\n RegisteredSemanticCallCfg,\n SemanticCallCfg,\n ValidatorCfg,\n)" - } - ] - }, { "id": "static-imports-e100b414a9a5", "source": "embodied-env", @@ -8586,8 +8586,8 @@ "static-imports-b87e50be9752", "static-imports-0f836c314aba", "static-imports-fbdcbe47c167", - "static-imports-67a3a6ca9069", - "static-imports-1fcaaf2a281e", + "static-imports-1efe7f42a374", + "static-imports-a43d0369f3e6", "static-imports-c6dd97cd134c", "static-imports-ad7ae2c5c0b5", "static-imports-c794d135c34f", @@ -8601,11 +8601,11 @@ "static-imports-a31378055869", "static-imports-a2682243828b", "static-imports-819e3ce626d3", - "static-imports-735fdeb87797", - "static-imports-30e2933cfbeb", - "static-imports-a38ea1f6aa90", - "static-imports-c84985d043b9", - "static-imports-01a9592b5f6f", + "static-imports-bacd2acfbd2e", + "static-imports-726d1e8f3927", + "static-imports-02ef4754d36f", + "static-imports-b2a8f9f1b012", + "static-imports-78872b465d8c", "static-imports-3d4c7a13ce4e", "static-imports-14722eceaa11", "static-imports-84f40c9ef00a", @@ -8855,9 +8855,9 @@ "static-imports-c6dd97cd134c", "static-imports-8350ecf8d7c9", "static-imports-0d77cfea421a", - "static-imports-86942e4dcbbb", - "static-imports-7a0fb061e5eb", - "static-imports-de7d2002c14c", + "static-imports-1cb8ca231ed7", + "static-imports-9e8c80d64a20", + "static-imports-0439aa147cbd", "static-imports-3d4c7a13ce4e", "static-imports-62adb31d7b9c", "static-imports-14722eceaa11", @@ -9057,11 +9057,11 @@ "static-imports-a31378055869", "static-imports-a2682243828b", "static-imports-819e3ce626d3", - "static-imports-735fdeb87797", - "static-imports-30e2933cfbeb", - "static-imports-a38ea1f6aa90", - "static-imports-c84985d043b9", - "static-imports-01a9592b5f6f" + "static-imports-bacd2acfbd2e", + "static-imports-726d1e8f3927", + "static-imports-02ef4754d36f", + "static-imports-b2a8f9f1b012", + "static-imports-78872b465d8c" ], "groups": [ { @@ -9363,7 +9363,7 @@ "static-imports-a954b58f9edc", "static-imports-17df33c96f97", "static-imports-9bc131d4e099", - "static-imports-01a9592b5f6f" + "static-imports-78872b465d8c" ], "groups": [ { diff --git a/docs/architecture/generated/summary.md b/docs/architecture/generated/summary.md index bdae6aed0c..9f65e8199e 100644 --- a/docs/architecture/generated/summary.md +++ b/docs/architecture/generated/summary.md @@ -1,139 +1,139 @@ # Architecture overview -Source revision: `5d4561aca503f7897af8cfcd3246f9d7e3f701d4` · package 0.3.2 +Source revision: `37014e73d252f8c7c9d89f6ca081306f55f882bd` · package 0.3.2 Curated static relationships; not a complete dependency graph or runtime trace. | Module | Responsibility | Evidence and documentation | | --- | --- | --- | -| CLI | Dispatches commands and lazily loads task discovery, environment, and training entry points. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/cli/main.py#L17-L17) · [CLI reference](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/guides/cli.md) | -| Environment launcher | Runs task discovery, configuration loading, and Gym environment creation. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/scripts/run_task.py#L2015-L2015) · [Environment launch guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/guides/run_task.md) | -| Task discovery | Imports installed task packages through entry points to populate the environment registry. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/utils/registration.py#L446-L446) · [Task discovery and supported tasks](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/resources/task/index.rst) | -| Official tasks | Registers official task entry points and organises reusable robot tasks. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/extensions/embodichain_tasks/embodichain_tasks/__init__.py#L17-L23) · [Supported tasks](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/resources/task/index.rst) | -| Config loader | Loads deployment files, resolves components, and assembles the typed environment configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/utils/gym_utils.py#L1954-L1954) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/guides/configuration.md) | -| Component composition | Resolves environment and embodiment components into runnable Gym configurations. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/utils/_component_composition.py#L17-L17) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/guides/configuration.md) | -| EmbodiedEnv | Assembles scenes, managers, and demonstrations with optional Task Program integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/embodied_env.py#L518-L518) · [EmbodiedEnv documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/gym/env.md) | -| BaseEnv | Owns the Gym loop, simulation initialisation, and environment step lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/base_env.py#L118-L118) · [BaseEnv documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/gym/env.md) | -| Managers | Organises observations, actions, rewards, events, and data recording within the environment lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/managers/__init__.py#L17-L20) · [Gym managers and environment lifecycle](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/gym/index.rst) | -| Configuration utilities | Defines typed configuration copying, serialization, and required-field validation. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/utils/configclass.py#L15-L15) · [Configuration utilities documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.utils.rst) | -| Domain randomization | Event functors randomize physical, visual, spatial, and geometric properties. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/managers/randomization/__init__.py#L17-L20) · [Domain randomization documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst) | -| DifferentiableEnv | Preserves the Gym lifecycle while bridging task-defined kinematics to autograd. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/differentiable_env.py#L48-L48) · [DifferentiableEnv documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst) | -| EventManager | Dispatches configured event functors with lifecycle scheduling and scoped seeds. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/managers/event_manager.py#L97-L97) · [EventManager documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst) | -| TaskProgramCompiler | Compiles task configuration into an analysable program using a static scene manifest. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/compiler/program.py#L1613-L1613) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [TaskProgramCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | -| SemanticCallCompiler | Analyses and lowers Semantic Calls against the bound scene and skill integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/compiler/lowering.py#L1033-L1033) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [SemanticCallCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | -| SemanticCallExecutor | Lowers and executes semantic calls, creating an execution session for each call. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/runtime/executor.py#L258-L258) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [SemanticCallExecutor documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst) | -| TaskProgramEnvironmentAdapter | Validates integration choices and assembles the semantic runtime and Gym bridge. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/integrations/environment.py#L315-L315) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [TaskProgramEnvironmentAdapter documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.rst) | -| TaskProgramDemoBridge | Converts program segments into lazy Gym demonstrations with a shared runtime, buffer, and clock. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/task_program/bridge.py#L1149-L1149) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [TaskProgramDemoBridge documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | -| Scene expansion attempts | Prepares selected scene candidates and executes a shared Task Program with per-row measured acceptance. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/integrations/scene_expansion.py#L241-L241) · [Task Program scene preparation and measured acceptance](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.scene_expansion.rst) | -| MLLM frontend | Decodes untrusted model JSON and compiles it through the existing Task Program adapter. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/agents/mllm/task_program.py#L17-L17) · [MLLM frontend documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) | -| AtomicSkillEngine | Manages planning resources and atomic skills to create executable action sessions. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/atomic_skills/engine.py#L85-L85) · [Atomic Skills guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/atomic_skills/index.md) · [AtomicSkillEngine documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_skills.rst) | -| Motion benchmarks | Evaluates motion planners and Atomic Skills using installed suites and external adapters. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/extensions/embodichain_benchmarks/motion_generation/__init__.py#L17-L21) · [Benchmark API and external adapters](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/benchmarks.rst) | -| MotionGenerator | Composes planning backends behind a unified robot motion generation interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/motion/motion_generator.py#L310-L310) · [Motion generation guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/motion/motion_generator.md) · [MotionGenerator documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.motion.motion_generator.rst) | -| Planners | Provides motion planning backends that generate constrained robot trajectories. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/motion/planners/__init__.py#L17-L22) · [Motion planner guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/motion/planners/index.rst) | -| Solvers | Provides inverse kinematics backends and stateful robot solving capabilities. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/motion/solvers/__init__.py#L17-L20) · [Kinematic solver guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/motion/solvers/index.rst) | -| Robot workspace | Loads reachability caches and samples robot joint configurations and poses. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/motion/workspace/runtime.py#L53-L53) · [Robot workspace documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/workspace_analyzer/runtime.md) | -| Expansion session | Owns bounded candidate lifecycles, collection budgets and final commit receipts. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/expansion/session.py#L121-L121) · [Trajectory augmentation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.motion.expansion.rst) | -| SimulationManager | Manages the world, robots, sensors, physics, and visualisation lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/sim_manager.py#L562-L562) · [SimulationManager documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/sim_manager.md) | -| Scene expansion contracts | Defines immutable arena-local scene proposals and bounded authored candidate sources. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/scene/expansion/__init__.py#L28-L30) · [Scene expansion contracts and fixed candidates](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst) | -| Robot | Represents a simulated robot with control parts, joint state, and kinematics. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/robot.py#L17-L17) · [Robots and articulations](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/sim_articulation.md) | -| Sensors | Provides camera, stereo, and contact observations for the environment. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/sensors/__init__.py#L17-L20) · [Sensor guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/sensors/index.rst) | -| Physics backends | Constructs the configured physics backend and connects simulation state to physics solving. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/physics/__init__.py#L16-L21) · [Physics backend guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/sim_manager/physics/index.md) | -| Browser visualization | Publishes simulation snapshots and interactive controls through a browser runtime. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/visualization/runtime.py#L17-L17) · [Browser visualization guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/visualization/viser.md) | -| Rigid objects | Represents rigid scene objects and their physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/rigid_object.py#L430-L430) · [Rigid objects documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Rigid object groups | Groups multiple rigid bodies into a batched scene entity. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/rigid_object_group.py#L187-L187) · [Rigid object group guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/sim_rigid_object_group.md) · [Object API reference](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Articulations | Represents jointed scene entities and articulated state. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/articulation.py#L724-L724) · [Articulations documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Deformable objects | Represents Newton-backed deformable particle-set entities. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/deformable/base.py#L49-L49) · [Deformable objects documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Lights | Controls light color, intensity, falloff, and pose through a batched facade. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/light.py#L40-L40) · [Lights documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Rigid constraints | Wraps per-arena fixed constraints between two rigid objects. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/constraint.py#L36-L36) · [Rigid constraints documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Gizmo | Provides native robot IK and browser manipulation controls for simulation targets. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/gizmo.py#L458-L458) · [Gizmo documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Compute | Shared numerical algorithms for kinematics, trajectories, geometry, and images. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/compute/__init__.py#L17-L21) · [Compute API reference](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.compute.rst) | -| Trajectory computation | Shared interpolation, resampling, timing, and trajectory warping algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/compute/trajectory/__init__.py#L17-L20) · [Trajectory computation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.compute.rst) | -| Kinematics computation | Shared numerical kinematics and manipulability algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/compute/kinematics/__init__.py#L17-L24) · [Kinematics computation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.compute.rst) | -| DatasetManager | Coordinates dataset functors for frame collection, saving, and finalization. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/managers/dataset_manager.py#L46-L46) · [DatasetManager documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/gym/dataset_functors.md) | -| LeRobot recording | Persists demonstrations and metadata in LeRobot datasets with optional depth sidecars. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/managers/datasets.py#L167-L167) · [LeRobot recording documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/gym/dataset_functors.md) | -| Async recording | Moves durable dataset writes to one background worker. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/managers/async_datasets.py#L58-L58) · [Async recording documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/gym/dataset_functors.md) | -| Depth sidecar writer | Coordinates depth-video files and metadata alongside recorded demonstrations. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/data_pipeline/depth_video/writer.py#L226-L226) · [Depth video documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | -| Depth sidecar reader | Loads depth metadata and decodes episode/sensor depth video frames. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/data_pipeline/depth_video/reader.py#L17-L23) · [Depth sidecar reader documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | -| Depth quantization | Converts depth values to and from the sidecar video representation. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/data_pipeline/depth_video/depth_utils.py#L24-L35) · [Depth quantization documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | -| OnlineDataEngine | Online data engine managing workers and their lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/data_pipeline/engine/data.py#L17-L17) · [Online data streaming guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/online_data.md) | -| OnlineDataset | Exposes live engine samples as an infinite PyTorch IterableDataset. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/data_pipeline/datasets/online_data.py#L39-L39) · [OnlineDataset documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst) | -| Chunk samplers | Selects fixed-shape time windows using configurable chunk-length distributions. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/data_pipeline/datasets/sampler.py#L17-L17) · [Chunk samplers documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst) | -| Data assets | Resolves asset paths and coordinates registered dataset downloads and caches. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/data/dataset.py#L17-L17) · [Data assets documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/resources/assets.md) · [Asset catalog and task role contracts](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/asset_generalization.rst) | -| Scene generation | Generates scene state from input images for scene asset workflows. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/scene_engine/pipeline/generate.py#L17-L17) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/scene_engine.md) | -| Scene understanding | Interprets an image, segments objects, and initializes a validated support graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/scene_engine/pipeline/generation/scene_understanding.py#L260-L260) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/scene_engine.md) | -| Asset generation & refinement | Generates geometry and articulated assets, then refines scene placement. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/scene_engine/pipeline/generation/scene_generation.py#L96-L96) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/scene_engine.md) | -| Scene asset preparation | Prepares generated Scene Engine geometry for layout and export. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py#L90-L90) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/scene_engine.md) | -| Scene editing | Edits an existing portable scene using a typed edit plan. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/scene_engine/pipeline/edit.py#L17-L17) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/scene_engine.md) | -| Scene edit layout | Reconstructs scene placement against an edited goal graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_layout_generation.py#L32-L32) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/scene_engine.md) | -| Scene import | Reconstructs an editable scene and graph from a portable scene export. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py#L50-L50) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/scene_engine.md) | -| Scene export | Writes portable scene metadata and assets for downstream simulation preparation. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/scene_engine/pipeline/utils/scene_exporter.py#L53-L53) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/scene_engine.md) | -| Asset Engine | Prepares an existing mesh through source conversion, semantic/physics inference, and mesh/USD export. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/asset_engine/pipeline/ingest.py#L172-L178) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/asset_engine.md) | -| Asset parser stages | Dispatches inspector, geometry, physics, USD, and internal asset parsers. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/asset_engine/parser/base.py#L58-L64) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/asset_engine.md) | -| GenSim UI sessions | Tracks one replaceable subprocess per Gradio session. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/gradio_ui/app_processes.py#L109-L109) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/scene_engine.md) | -| Training assembly | Builds environments, policies, algorithms, and the compatible trainer from configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/train.py#L17-L17) · [Training assembly documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/rl/train_script.md) | -| RL algorithms | Registers PPO, GRPO, and APG with their rollout contracts. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/algo/__init__.py#L17-L17) · [RL algorithms documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/rl/algorithm.md) | -| PPO | Optimizes an actor-critic policy from standard TensorDict rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/algo/ppo.py#L53-L53) · [PPO documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | -| GRPO | Optimizes policies with group-relative advantages from standard rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/algo/grpo.py#L48-L48) · [GRPO documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | -| APG | Optimizes policy parameters through differentiable rollout rewards. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/algo/apg.py#L119-L119) · [APG documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | -| Policy models | Defines policy action inference and model interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/policies/policy.py#L35-L35) · [Policy models documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/rl/models.md) | -| ActorCritic | Provides Gaussian actions and value estimates with separate actor and critic inputs. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/policies/actor_critic.py#L36-L36) · [ActorCritic documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.policies.rst) | -| ActorOnly | Provides Gaussian actions without a learned value network. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/policies/actor_only.py#L31-L31) · [ActorOnly documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.policies.rst) | -| Learning environments | Defines lightweight vector-environment protocols and factory registration. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/env.py#L17-L17) · [Learning environments documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | -| Standard trainer | Coordinates standard rollout collection, algorithm updates, evaluation, and checkpoints. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/utils/trainer.py#L17-L17) · [Reinforcement learning overview](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/rl/index.rst) | -| SyncCollector | Collects standard vector-environment rollouts into the training buffer. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/collector/sync_collector.py#L33-L33) · [SyncCollector documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.collector.rst) | -| Rollout buffer | Stores standard rollout transitions and bootstrap state in TensorDict buffers. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/buffer/standard_buffer.py#L29-L29) · [Rollout buffer documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/rl/buffer.md) | -| DifferentiableTrainer | Coordinates graph-connected rollouts and analytic policy-gradient updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/differentiable_trainer.py#L77-L77) · [DifferentiableTrainer documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | -| Differentiable collector | Collects differentiable trajectories while preserving their autograd graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/collector/differentiable.py#L145-L145) · [Differentiable collector documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.collector.rst) | -| Running observation normalization | Normalizes continuous observation fields with explicit running-statistic updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/normalization.py#L29-L29) · [Running observation normalization documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | -| Policy evaluation | Runs trained policy evaluation in headless or viewer workflows. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/rl/policy_evaluation/cli.py#L17-L17) · [Policy evaluation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.rl.policy_evaluation.rst) | -| Devices | Defines start, stop, and controller-state interfaces for robot input devices. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/devices/device.py#L17-L17) · [Laboratory and device API reference](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.rst) | -| Device controller | Maps input-device state to robot joint commands with joint-limit handling. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/devices/device_controller.py#L29-L29) · [Device controller documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.rst) | -| GraspKit | Defines parallel-jaw gripper models and grasp-pose generation interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/graspkit/pose_generator.py#L17-L17) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/grasp_generator.rst) | -| URDF assembly | Assembles URDF components, connections, sensors, and mesh resources. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py#L81-L81) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/urdf_assembly.md) | -| Convex decomposition | Prepares convex collision meshes for URDF assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/acd/cli.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/convex_decomposition.md) | -| Shared scene services | Composes explicit configurations, proposes layouts, and operates on caller-owned physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/scene/__init__.py#L17-L22) · [Scene services architecture and capabilities](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/scene/index.md) · [Scene construction and preparation workflows](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/scene/workflows.md) · [Production scene service integrations](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/scene/integrations.md) · [Scene composition, layout, state and validation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.rst) · [Shared numerical geometry and layout](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.compute.scene_geometry.rst) | -| NeRD model extension | Bundles the causal NeRD network, physical codec, trajectory windows and supervised recipe in the main wheel. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/extensions/embodichain_models/embodichain_models/dynamics/nerd/model.py#L201-L201) · [Official model library](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/model_catalogs.rst) · [NeRD data and training workflow](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/neural_dynamics/nerd.md) | -| Supervised learning (SL) | Shares structured batches, objective-owned losses, optimizer updates and offline recovery across policy and dynamics fitting. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/learning/sl/learner.py#L52-L52) · [SL batches, updates and recovery](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.learning.sl.rst) | -| NeRD showcase | Prepares pinned external resources and routes default NeRD training through shared SL. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/examples/learning/nerd/__main__.py#L81-L81) · [NeRD showcase commands](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/neural_dynamics/nerd.md) | -| Robot world model (RWM) | Owns action-conditioned robot-state prediction, differentiable rollout objectives and bounded transition replay. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py#L77-L77) · [RWM state, objectives, prediction, replay and SL recipe](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/rwm.rst) · [RWM principles and usage](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/neural_dynamics/rwm.md) | -| TaskSpec | Defines simulator-independent task requirements, milestones, semantic identity and evidence evaluation. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/task_spec/spec.py#L335-L335) · [TaskSpec protocol API](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/public_api.rst) | -| Task Engine | Interprets instructions and image evidence into candidates, TaskSpec requirements and canonical Semantic Call graphs. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/gen_sim/task_engine/agent.py#L72-L72) · [Task Engine workflow and artifact guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/generative_sim/task_engine.md) · [Task Engine semantic contracts](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.gen_sim.task_engine.rst) | +| CLI | Dispatches commands and lazily loads task discovery, environment, and training entry points. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/cli/main.py#L17-L17) · [CLI reference](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/guides/cli.md) | +| Environment launcher | Runs task discovery, configuration loading, and Gym environment creation. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/scripts/run_task.py#L2059-L2059) · [Environment launch guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/guides/run_task.md) | +| Task discovery | Imports installed task packages through entry points to populate the environment registry. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/utils/registration.py#L446-L446) · [Task discovery and supported tasks](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/resources/task/index.rst) | +| Official tasks | Registers official task entry points and organises reusable robot tasks. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/extensions/embodichain_tasks/embodichain_tasks/__init__.py#L17-L23) · [Supported tasks](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/resources/task/index.rst) | +| Config loader | Loads deployment files, resolves components, and assembles the typed environment configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/utils/gym_utils.py#L1978-L1978) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/guides/configuration.md) | +| Component composition | Resolves environment and embodiment components into runnable Gym configurations. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/utils/_component_composition.py#L17-L17) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/guides/configuration.md) | +| EmbodiedEnv | Assembles scenes, managers, and demonstrations with optional Task Program integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/embodied_env.py#L518-L518) · [EmbodiedEnv documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/gym/env.md) | +| BaseEnv | Owns the Gym loop, simulation initialisation, and environment step lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/base_env.py#L118-L118) · [BaseEnv documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/gym/env.md) | +| Managers | Organises observations, actions, rewards, events, and data recording within the environment lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/managers/__init__.py#L17-L20) · [Gym managers and environment lifecycle](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/gym/index.rst) | +| Configuration utilities | Defines typed configuration copying, serialization, and required-field validation. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/utils/configclass.py#L15-L15) · [Configuration utilities documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.utils.rst) | +| Domain randomization | Event functors randomize physical, visual, spatial, and geometric properties. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/managers/randomization/__init__.py#L17-L20) · [Domain randomization documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst) | +| DifferentiableEnv | Preserves the Gym lifecycle while bridging task-defined kinematics to autograd. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/differentiable_env.py#L48-L48) · [DifferentiableEnv documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst) | +| EventManager | Dispatches configured event functors with lifecycle scheduling and scoped seeds. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/managers/event_manager.py#L97-L97) · [EventManager documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst) | +| TaskProgramCompiler | Compiles task configuration into an analysable program using a static scene manifest. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/compiler/program.py#L1615-L1615) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [TaskProgramCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | +| SemanticCallCompiler | Analyses and lowers Semantic Calls against the bound scene and skill integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/compiler/lowering.py#L1046-L1046) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [SemanticCallCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | +| SemanticCallExecutor | Lowers and executes semantic calls, creating an execution session for each call. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/runtime/executor.py#L258-L258) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [SemanticCallExecutor documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst) | +| TaskProgramEnvironmentAdapter | Validates integration choices and assembles the semantic runtime and Gym bridge. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/integrations/environment.py#L315-L315) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [TaskProgramEnvironmentAdapter documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.rst) | +| TaskProgramDemoBridge | Converts program segments into lazy Gym demonstrations with a shared runtime, buffer, and clock. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/task_program/bridge.py#L1177-L1177) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [TaskProgramDemoBridge documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | +| Scene expansion attempts | Prepares selected scene candidates and executes a shared Task Program with per-row measured acceptance. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/integrations/scene_expansion.py#L241-L241) · [Task Program scene preparation and measured acceptance](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.scene_expansion.rst) | +| MLLM frontend | Decodes untrusted model JSON and compiles it through the existing Task Program adapter. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/agents/mllm/task_program.py#L17-L17) · [MLLM frontend documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) | +| AtomicSkillEngine | Manages planning resources and atomic skills to create executable action sessions. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/atomic_skills/engine.py#L85-L85) · [Atomic Skills guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/atomic_skills/index.md) · [AtomicSkillEngine documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_skills.rst) | +| Motion benchmarks | Evaluates motion planners and Atomic Skills using installed suites and external adapters. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/extensions/embodichain_benchmarks/motion_generation/__init__.py#L17-L21) · [Benchmark API and external adapters](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/benchmarks.rst) | +| MotionGenerator | Composes planning backends behind a unified robot motion generation interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/motion/motion_generator.py#L334-L334) · [Motion generation guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/motion/motion_generator.md) · [MotionGenerator documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.motion.motion_generator.rst) | +| Planners | Provides motion planning backends that generate constrained robot trajectories. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/motion/planners/__init__.py#L17-L22) · [Motion planner guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/motion/planners/index.rst) | +| Solvers | Provides inverse kinematics backends and stateful robot solving capabilities. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/motion/solvers/__init__.py#L17-L20) · [Kinematic solver guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/motion/solvers/index.rst) | +| Robot workspace | Loads reachability caches and samples robot joint configurations and poses. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/motion/workspace/runtime.py#L53-L53) · [Robot workspace documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/workspace_analyzer/runtime.md) | +| Expansion session | Owns bounded candidate lifecycles, collection budgets and final commit receipts. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/expansion/session.py#L121-L121) · [Trajectory augmentation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.motion.expansion.rst) | +| SimulationManager | Manages the world, robots, sensors, physics, and visualisation lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/sim_manager.py#L580-L580) · [SimulationManager documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/sim_manager.md) | +| Scene expansion contracts | Defines immutable arena-local scene proposals and bounded authored candidate sources. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/scene/expansion/__init__.py#L28-L31) · [Scene expansion contracts and fixed candidates](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst) | +| Robot | Represents a simulated robot with control parts, joint state, and kinematics. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/robot.py#L17-L17) · [Robots and articulations](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/sim_articulation.md) | +| Sensors | Provides camera, stereo, and contact observations for the environment. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/sensors/__init__.py#L17-L20) · [Sensor guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/sensors/index.rst) | +| Physics backends | Constructs the configured physics backend and connects simulation state to physics solving. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/physics/__init__.py#L16-L21) · [Physics backend guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/sim_manager/physics/index.md) | +| Browser visualization | Publishes simulation snapshots and interactive controls through a browser runtime. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/visualization/runtime.py#L17-L17) · [Browser visualization guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/visualization/viser.md) | +| Rigid objects | Represents rigid scene objects and their physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/rigid_object.py#L434-L434) · [Rigid objects documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Rigid object groups | Groups multiple rigid bodies into a batched scene entity. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/rigid_object_group.py#L187-L187) · [Rigid object group guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/sim_rigid_object_group.md) · [Object API reference](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Articulations | Represents jointed scene entities and articulated state. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/articulation.py#L739-L739) · [Articulations documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Deformable objects | Represents Newton-backed deformable particle-set entities. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/deformable/base.py#L49-L49) · [Deformable objects documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Lights | Controls light color, intensity, falloff, and pose through a batched facade. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/light.py#L40-L40) · [Lights documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Rigid constraints | Wraps per-arena fixed constraints between two rigid objects. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/constraint.py#L36-L36) · [Rigid constraints documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Gizmo | Provides native robot IK and browser manipulation controls for simulation targets. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/gizmo.py#L458-L458) · [Gizmo documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Compute | Shared numerical algorithms for kinematics, trajectories, geometry, and images. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/compute/__init__.py#L17-L21) · [Compute API reference](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.compute.rst) | +| Trajectory computation | Shared interpolation, resampling, timing, and trajectory warping algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/compute/trajectory/__init__.py#L17-L20) · [Trajectory computation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.compute.rst) | +| Kinematics computation | Shared numerical kinematics and manipulability algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/compute/kinematics/__init__.py#L17-L24) · [Kinematics computation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.compute.rst) | +| DatasetManager | Coordinates dataset functors for frame collection, saving, and finalization. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/managers/dataset_manager.py#L46-L46) · [DatasetManager documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/gym/dataset_functors.md) | +| LeRobot recording | Persists demonstrations and metadata in LeRobot datasets with optional depth sidecars. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/managers/datasets.py#L167-L167) · [LeRobot recording documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/gym/dataset_functors.md) | +| Async recording | Moves durable dataset writes to one background worker. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/managers/async_datasets.py#L58-L58) · [Async recording documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/gym/dataset_functors.md) | +| Depth sidecar writer | Coordinates depth-video files and metadata alongside recorded demonstrations. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/data_pipeline/depth_video/writer.py#L226-L226) · [Depth video documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | +| Depth sidecar reader | Loads depth metadata and decodes episode/sensor depth video frames. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/data_pipeline/depth_video/reader.py#L17-L23) · [Depth sidecar reader documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | +| Depth quantization | Converts depth values to and from the sidecar video representation. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/data_pipeline/depth_video/depth_utils.py#L24-L35) · [Depth quantization documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | +| OnlineDataEngine | Online data engine managing workers and their lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/data_pipeline/engine/data.py#L17-L17) · [Online data streaming guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/online_data.md) | +| OnlineDataset | Exposes live engine samples as an infinite PyTorch IterableDataset. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/data_pipeline/datasets/online_data.py#L39-L39) · [OnlineDataset documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst) | +| Chunk samplers | Selects fixed-shape time windows using configurable chunk-length distributions. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/data_pipeline/datasets/sampler.py#L17-L17) · [Chunk samplers documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst) | +| Data assets | Resolves asset paths and coordinates registered dataset downloads and caches. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/data/dataset.py#L17-L17) · [Data assets documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/resources/assets.md) · [Asset catalog and task role contracts](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/asset_generalization.rst) | +| Scene generation | Generates scene state from input images for scene asset workflows. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/scene_engine/pipeline/generate.py#L17-L17) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/scene_engine.md) | +| Scene understanding | Interprets an image, segments objects, and initializes a validated support graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/scene_engine/pipeline/generation/scene_understanding.py#L260-L260) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/scene_engine.md) | +| Asset generation & refinement | Generates geometry and articulated assets, then refines scene placement. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/scene_engine/pipeline/generation/scene_generation.py#L96-L96) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/scene_engine.md) | +| Scene asset preparation | Prepares generated Scene Engine geometry for layout and export. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py#L90-L90) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/scene_engine.md) | +| Scene editing | Edits an existing portable scene using a typed edit plan. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/scene_engine/pipeline/edit.py#L17-L17) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/scene_engine.md) | +| Scene edit layout | Reconstructs scene placement against an edited goal graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_layout_generation.py#L32-L32) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/scene_engine.md) | +| Scene import | Reconstructs an editable scene and graph from a portable scene export. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py#L50-L50) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/scene_engine.md) | +| Scene export | Writes portable scene metadata and assets for downstream simulation preparation. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/scene_engine/pipeline/utils/scene_exporter.py#L53-L53) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/scene_engine.md) | +| Asset Engine | Prepares an existing mesh through source conversion, semantic/physics inference, and mesh/USD export. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/asset_engine/pipeline/ingest.py#L172-L178) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/asset_engine.md) | +| Asset parser stages | Dispatches inspector, geometry, physics, USD, and internal asset parsers. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/asset_engine/parser/base.py#L58-L64) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/asset_engine.md) | +| GenSim UI sessions | Tracks one replaceable subprocess per Gradio session. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/gradio_ui/app_processes.py#L109-L109) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/scene_engine.md) | +| Training assembly | Builds environments, policies, algorithms, and the compatible trainer from configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/train.py#L17-L17) · [Training assembly documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/rl/train_script.md) | +| RL algorithms | Registers PPO, GRPO, and APG with their rollout contracts. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/algo/__init__.py#L17-L17) · [RL algorithms documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/rl/algorithm.md) | +| PPO | Optimizes an actor-critic policy from standard TensorDict rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/algo/ppo.py#L53-L53) · [PPO documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | +| GRPO | Optimizes policies with group-relative advantages from standard rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/algo/grpo.py#L48-L48) · [GRPO documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | +| APG | Optimizes policy parameters through differentiable rollout rewards. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/algo/apg.py#L119-L119) · [APG documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | +| Policy models | Defines policy action inference and model interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/policies/policy.py#L35-L35) · [Policy models documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/rl/models.md) | +| ActorCritic | Provides Gaussian actions and value estimates with separate actor and critic inputs. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/policies/actor_critic.py#L36-L36) · [ActorCritic documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.policies.rst) | +| ActorOnly | Provides Gaussian actions without a learned value network. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/policies/actor_only.py#L31-L31) · [ActorOnly documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.policies.rst) | +| Learning environments | Defines lightweight vector-environment protocols and factory registration. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/env.py#L17-L17) · [Learning environments documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | +| Standard trainer | Coordinates standard rollout collection, algorithm updates, evaluation, and checkpoints. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/utils/trainer.py#L17-L17) · [Reinforcement learning overview](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/rl/index.rst) | +| SyncCollector | Collects standard vector-environment rollouts into the training buffer. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/collector/sync_collector.py#L33-L33) · [SyncCollector documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.collector.rst) | +| Rollout buffer | Stores standard rollout transitions and bootstrap state in TensorDict buffers. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/buffer/standard_buffer.py#L29-L29) · [Rollout buffer documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/rl/buffer.md) | +| DifferentiableTrainer | Coordinates graph-connected rollouts and analytic policy-gradient updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/differentiable_trainer.py#L77-L77) · [DifferentiableTrainer documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | +| Differentiable collector | Collects differentiable trajectories while preserving their autograd graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/collector/differentiable.py#L145-L145) · [Differentiable collector documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.collector.rst) | +| Running observation normalization | Normalizes continuous observation fields with explicit running-statistic updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/normalization.py#L29-L29) · [Running observation normalization documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | +| Policy evaluation | Runs trained policy evaluation in headless or viewer workflows. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/rl/policy_evaluation/cli.py#L17-L17) · [Policy evaluation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.rl.policy_evaluation.rst) | +| Devices | Defines start, stop, and controller-state interfaces for robot input devices. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/devices/device.py#L17-L17) · [Laboratory and device API reference](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.rst) | +| Device controller | Maps input-device state to robot joint commands with joint-limit handling. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/devices/device_controller.py#L29-L29) · [Device controller documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.rst) | +| GraspKit | Defines parallel-jaw gripper models and grasp-pose generation interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/graspkit/pose_generator.py#L17-L17) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/grasp_generator.rst) | +| URDF assembly | Assembles URDF components, connections, sensors, and mesh resources. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py#L81-L81) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/urdf_assembly.md) | +| Convex decomposition | Prepares convex collision meshes for URDF assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/acd/cli.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/convex_decomposition.md) | +| Shared scene services | Composes explicit configurations, proposes layouts, and operates on caller-owned physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/scene/__init__.py#L17-L22) · [Scene services architecture and capabilities](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/scene/index.md) · [Scene construction and preparation workflows](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/scene/workflows.md) · [Production scene service integrations](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/scene/integrations.md) · [Scene composition, layout, state and validation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.rst) · [Shared numerical geometry and layout](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.compute.scene_geometry.rst) | +| NeRD model extension | Bundles the causal NeRD network, physical codec, trajectory windows and supervised recipe in the main wheel. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/extensions/embodichain_models/embodichain_models/dynamics/nerd/model.py#L201-L201) · [Official model library](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/model_catalogs.rst) · [NeRD data and training workflow](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/neural_dynamics/nerd.md) | +| Supervised learning (SL) | Shares structured batches, objective-owned losses, optimizer updates and offline recovery across policy and dynamics fitting. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/learning/sl/learner.py#L52-L52) · [SL batches, updates and recovery](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.learning.sl.rst) | +| NeRD showcase | Prepares pinned external resources and routes default NeRD training through shared SL. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/examples/learning/nerd/__main__.py#L81-L81) · [NeRD showcase commands](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/neural_dynamics/nerd.md) | +| Robot world model (RWM) | Owns action-conditioned robot-state prediction, differentiable rollout objectives and bounded transition replay. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py#L77-L77) · [RWM state, objectives, prediction, replay and SL recipe](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/rwm.rst) · [RWM principles and usage](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/neural_dynamics/rwm.md) | +| TaskSpec | Defines simulator-independent task requirements, milestones, semantic identity and evidence evaluation. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/task_spec/spec.py#L335-L335) · [TaskSpec protocol API](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/public_api.rst) | +| Task Engine | Interprets instructions and image evidence into candidates, TaskSpec requirements and canonical Semantic Call graphs. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/gen_sim/task_engine/agent.py#L72-L72) · [Task Engine workflow and artifact guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/generative_sim/task_engine.md) · [Task Engine semantic contracts](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.gen_sim.task_engine.rst) | ### Additional modules in specialist views | Module | Responsibility | Evidence and documentation | | --- | --- | --- | -| Antipodal grasp generator | Generates candidate grasp poses through a reusable parallel-jaw interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py#L284-L284) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/grasp_generator.rst) | -| Newton autograd bridge | Connects Warp tape derivatives to PyTorch action and functional-state gradients. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/diff/bridge.py#L81-L81) · [Newton autograd bridge documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst) | -| BatchEntity | Defines shared configuration and instance operations for batched simulation entities. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/common.py#L55-L55) · [BatchEntity documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| URDF collision processor | Replaces mesh collisions with convex pieces and optionally recomputes inertia. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/acd/urdf_modifider.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/convex_decomposition.md) | -| BufferedGymCommandSink | Validates and buffers control commands for incremental Gym demonstration consumption. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/task_program/bridge.py#L864-L864) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [BufferedGymCommandSink documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | -| CompiledTaskProgram | Immutable compilation result with materialized task segments and analysis views. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/compiler/program.py#L1343-L1343) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [CompiledTaskProgram documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | -| Effect evidence | Collects typed evidence from registered providers for semantic effects. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/semantics/evidence.py#L438-L438) · [Effect evidence documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.semantics.rst) | -| ExecutionRunner | Advances atomic skill sessions through observation, command, and clock interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/atomic_skills/runner.py#L367-L367) · [Atomic Skills execution guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/sim/atomic_skills/index.md) · [ExecutionRunner documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_skills.rst) | -| Interactive grasp annotation | Offers browser selection of a graspable mesh region. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/scripts/annotate_grasp.py#L42-L42) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/grasp_generator.rst) | -| Mesh grasp backend | Coordinates sampling, annotation, collision checks, and caches for one mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py#L71-L71) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/grasp_generator.rst) | -| Gripper collision checks | Checks gripper geometry against the target mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py#L39-L39) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/grasp_generator.rst) | -| Antipodal sampling | Samples opposing surface contacts using mesh ray casting. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py#L62-L62) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/grasp_generator.rst) | -| Parallel-jaw gripper configuration | Owns native hand physics, calibration and declared grasp-generator support. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/cfg/gripper.py#L53-L53) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/grasp_generator.rst) | -| Integration catalog | Describes one Task Program integration and validates compilation against its fingerprint. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/integrations/catalog.py#L611-L611) · [Integration catalog documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.rst) | -| Object backend views | Adapts rigid and articulated object state to backend-neutral Scene batches. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/backends/__init__.py#L19-L19) · [Object backend views documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Parallel execution | Coordinates parallel semantic branches through command and safety interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/runtime/parallel_executor.py#L629-L629) · [Parallel execution documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst) | -| Program loading & validation | Loads bounded JSON or YAML into the canonical Task Program configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/language/loader.py#L17-L17) · [Program loading & validation documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst) | -| SceneRegistry | Binds semantic entity identifiers to scene metadata and runtime providers. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/semantics/scene.py#L1113-L1113) · [SceneRegistry documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/scene_registry.md) | -| SimulationTaskProgramFactory | Binds simulated robots, planners, and services to construct the atomic skill engine. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/integrations/simulation/environment.py#L731-L731) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [Simulation integration API](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.simulation.rst) | -| RobotSkillProfile | Declares robot resources, skill bindings, and effect-assurance capabilities. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/semantics/profiles.py#L1495-L1495) · [RobotSkillProfile documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/robot_profiles.md) | -| EnvironmentStepClock | Maintains execution time on the environment step grid. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/gym/envs/task_program/bridge.py#L351-L351) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [Gym Task Program API](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | -| Surface deformables | Specializes deformable objects for cloth particle sets. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/deformable/surface.py#L28-L28) · [Surface deformables documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| TaskProgramCfg | Declarative task configuration describing intent and execution structure. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/task_program/language/schema.py#L1067-L1067) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/overview/task_program/index.md) · [TaskProgramCfg documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst) | -| Toolkit directory | Standalone grasp sampling, collision-mesh preparation, URDF composition and precision assembly assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/__init__.py#L17-L21) · [Toolkits documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/index.rst) · [Precision Assembly guide and native demos](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/precision_assembly.md) | -| URDF components | Processes component links, joints, naming, and mesh references. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/urdf_assembly/component.py#L87-L87) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/urdf_assembly.md) | -| URDF connections | Applies component connection rules and sensor attachment joints. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/urdf_assembly/connection.py#L30-L30) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/urdf_assembly.md) | -| URDF mesh resources | Copies mesh resources and rewrites references for the assembled robot. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/urdf_assembly/mesh.py#L33-L33) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/urdf_assembly.md) | -| URDF sensor attachments | Processes sensor descriptions and attachment metadata for assembly. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/urdf_assembly/sensor.py#L75-L75) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/urdf_assembly.md) | -| URDF file output | Formats and persists the assembled URDF document. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/toolkits/urdf_assembly/file_writer.py#L28-L28) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/toolkits/urdf_assembly.md) | -| Volume deformables | Specializes deformable objects for volumetric soft-body particle sets. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/objects/deformable/volume.py#L36-L36) · [Volume deformables documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Workspace analyzer | Analyzes robot reachability and metrics for reusable workspace caches. | [Source](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/embodichain/lab/sim/motion/workspace/analyzer.py#L182-L182) · [Workspace analyzer documentation](https://github.com/DexForce/EmbodiChain/blob/5d4561aca503f7897af8cfcd3246f9d7e3f701d4/docs/source/features/workspace_analyzer/workspace_analyzer.md) | +| Antipodal grasp generator | Generates candidate grasp poses through a reusable parallel-jaw interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py#L284-L284) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/grasp_generator.rst) | +| Newton autograd bridge | Connects Warp tape derivatives to PyTorch action and functional-state gradients. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/diff/bridge.py#L81-L81) · [Newton autograd bridge documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst) | +| BatchEntity | Defines shared configuration and instance operations for batched simulation entities. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/common.py#L55-L55) · [BatchEntity documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| URDF collision processor | Replaces mesh collisions with convex pieces and optionally recomputes inertia. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/acd/urdf_modifider.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/convex_decomposition.md) | +| BufferedGymCommandSink | Validates and buffers control commands for incremental Gym demonstration consumption. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/task_program/bridge.py#L892-L892) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [BufferedGymCommandSink documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | +| CompiledTaskProgram | Immutable compilation result with materialized task segments and analysis views. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/compiler/program.py#L1345-L1345) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [CompiledTaskProgram documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | +| Effect evidence | Collects typed evidence from registered providers for semantic effects. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/semantics/evidence.py#L438-L438) · [Effect evidence documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.semantics.rst) | +| ExecutionRunner | Advances atomic skill sessions through observation, command, and clock interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/atomic_skills/runner.py#L367-L367) · [Atomic Skills execution guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/sim/atomic_skills/index.md) · [ExecutionRunner documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_skills.rst) | +| Interactive grasp annotation | Offers browser selection of a graspable mesh region. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/scripts/annotate_grasp.py#L42-L42) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/grasp_generator.rst) | +| Mesh grasp backend | Coordinates sampling, annotation, collision checks, and caches for one mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py#L71-L71) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/grasp_generator.rst) | +| Gripper collision checks | Checks gripper geometry against the target mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py#L96-L96) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/grasp_generator.rst) | +| Antipodal sampling | Samples opposing surface contacts using mesh ray casting. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py#L62-L62) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/grasp_generator.rst) | +| Parallel-jaw gripper configuration | Owns native hand physics, calibration and declared grasp-generator support. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/cfg/gripper.py#L53-L53) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/grasp_generator.rst) | +| Integration catalog | Describes one Task Program integration and validates compilation against its fingerprint. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/integrations/catalog.py#L618-L618) · [Integration catalog documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.rst) | +| Object backend views | Adapts rigid and articulated object state to backend-neutral Scene batches. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/backends/__init__.py#L19-L19) · [Object backend views documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Parallel execution | Coordinates parallel semantic branches through command and safety interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/runtime/parallel_executor.py#L629-L629) · [Parallel execution documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst) | +| Program loading & validation | Loads bounded JSON or YAML into the canonical Task Program configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/language/loader.py#L17-L17) · [Program loading & validation documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst) | +| SceneRegistry | Binds semantic entity identifiers to scene metadata and runtime providers. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/semantics/scene.py#L1113-L1113) · [SceneRegistry documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/scene_registry.md) | +| SimulationTaskProgramFactory | Binds simulated robots, planners, and services to construct the atomic skill engine. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/integrations/simulation/environment.py#L731-L731) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [Simulation integration API](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.simulation.rst) | +| RobotSkillProfile | Declares robot resources, skill bindings, and effect-assurance capabilities. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/semantics/profiles.py#L1495-L1495) · [RobotSkillProfile documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/robot_profiles.md) | +| EnvironmentStepClock | Maintains execution time on the environment step grid. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/gym/envs/task_program/bridge.py#L351-L351) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [Gym Task Program API](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | +| Surface deformables | Specializes deformable objects for cloth particle sets. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/deformable/surface.py#L28-L28) · [Surface deformables documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| TaskProgramCfg | Declarative task configuration describing intent and execution structure. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/task_program/language/schema.py#L1067-L1067) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/overview/task_program/index.md) · [TaskProgramCfg documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst) | +| Toolkit directory | Standalone grasp sampling, collision-mesh preparation, URDF composition and precision assembly assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/__init__.py#L17-L21) · [Toolkits documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/index.rst) · [Precision Assembly guide and native demos](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/precision_assembly.md) | +| URDF components | Processes component links, joints, naming, and mesh references. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/urdf_assembly/component.py#L87-L87) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/urdf_assembly.md) | +| URDF connections | Applies component connection rules and sensor attachment joints. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/urdf_assembly/connection.py#L30-L30) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/urdf_assembly.md) | +| URDF mesh resources | Copies mesh resources and rewrites references for the assembled robot. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/urdf_assembly/mesh.py#L33-L33) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/urdf_assembly.md) | +| URDF sensor attachments | Processes sensor descriptions and attachment metadata for assembly. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/urdf_assembly/sensor.py#L75-L75) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/urdf_assembly.md) | +| URDF file output | Formats and persists the assembled URDF document. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/toolkits/urdf_assembly/file_writer.py#L28-L28) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/toolkits/urdf_assembly.md) | +| Volume deformables | Specializes deformable objects for volumetric soft-body particle sets. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/objects/deformable/volume.py#L36-L36) · [Volume deformables documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Workspace analyzer | Analyzes robot reachability and metrics for reusable workspace caches. | [Source](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/embodichain/lab/sim/motion/workspace/analyzer.py#L182-L182) · [Workspace analyzer documentation](https://github.com/DexForce/EmbodiChain/blob/37014e73d252f8c7c9d89f6ca081306f55f882bd/docs/source/features/workspace_analyzer/workspace_analyzer.md) | ## Coverage and interpretation diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst index 70132c16ba..84f4e27f9d 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst @@ -135,6 +135,7 @@ proposals. Hosts retain stepping, cache refresh and task-acceptance ownership. :nosignatures: SceneLayoutInput + SceneLayoutCondition PreparedSceneRandomization SceneRandomizationBatch SceneVariantApplicationResult @@ -154,6 +155,22 @@ proposals. Hosts retain stepping, cache refresh and task-acceptance ownership. .. autoclass:: SceneLayoutInput :members: +Conditional layout generation +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +``generate_scene_variants(..., conditions=SceneLayoutCondition(poses))`` fixes +arena-space anchors while completing the rest of the layout through the same +support and clearance checks. Group-root anchors currently restrict XY only; +selected members retain their support-frame XY/yaw domain. Anchor every moving +ancestor of an absolute member anchor. Conditions cannot change permissions, +height, authored tilt, topology or physics. Invalid domains reject before sampling; +geometric infeasibility consumes the bounded proposal budget without repair. +The prepared source and original policy stay reusable and independently owned. +Omitting conditions, or providing an empty condition, retains the legacy sequence. + +.. autoclass:: SceneLayoutCondition + :members: + .. autoclass:: PreparedSceneRandomization :members: @@ -192,3 +209,12 @@ proposals. Hosts retain stepping, cache refresh and task-acceptance ownership. .. autofunction:: save_scene_variants .. autofunction:: load_scene_variants + +Condition implementation module +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. currentmodule:: embodichain.lab.sim.scene.expansion.conditions + +.. autoclass:: SceneLayoutCondition + :members: + :no-index: diff --git a/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst index 6eeee2f284..97394708b1 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst @@ -404,6 +404,7 @@ template contract. TaskProgramMappedRetimeRecord TaskProgramSameGridExpansionFactory TaskProgramSameGridExpansionRecord + TaskProgramNativeExpansionRecord .. autoclass:: TaskProgramSourceAdapter :members: @@ -426,6 +427,9 @@ template contract. .. autoclass:: TaskProgramSameGridExpansionRecord :members: +.. autoclass:: TaskProgramNativeExpansionRecord + :members: + .. currentmodule:: embodichain.lab.task_program MLLM frontend diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 527e63d5a7..11d60792e1 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1776,6 +1776,64 @@ and durable episode-boundary recovery. See the offline expansion example. ExpansionCoverage EpisodeCoverage +.. autosummary:: + + ExpansionWorkloadCfg + WorkloadExecutionCfg + WorkloadAssignment + WorkloadRowResult + WorkloadSource + WorkloadProgramExecutor + GymExpansionWorkload + run_gym_expansion_workloads + load_task_program_expansion + load_expansion_viewer + run_task_program_expansion + derive_grasp_expansion_deployment + launch_expansion_preview + stop_expansion_preview + preview_expansion_results + export_expansion_episodes + +.. autosummary:: + + GymExpansionHostExtensions + ExpansionRowContext + ExpansionValidator + MeasuredExpansionValidator + MeasuredTrajectory + record_reference_trajectory + preview_expansion_comparison + report_expansion_distribution + +Offline distribution reporting validates confirmed native receipts and emits a +complete HTML/JSON/JSONL bundle without opening a physics World. Comparisons +require matching task, robot installation and configured scene domains. + +embodichain.lab.expansion.distribution +-------------------------------------- + +.. currentmodule:: embodichain.lab.expansion.distribution + +.. autofunction:: report_expansion_distribution + +embodichain.lab.expansion.acceptance +------------------------------------ + +Measured task and physical acceptance share a host port. Geometry uses independent +Newton queries; native contacts inspect every physical sample and optionally +require sustained finger contact and bounded object drift. Tasks declare all +permissions and thresholds; missing or dropped evidence rejects the trajectory. + +.. currentmodule:: embodichain.lab.expansion.acceptance + +.. autosummary:: + + MeasuredExpansionValidator + +.. autoclass:: MeasuredExpansionValidator + :members: + embodichain.lab.expansion.artifacts ----------------------------------- @@ -1789,6 +1847,29 @@ Typed content-addressed numeric, media and source references. Paths remain conta EpisodeArtifactManifest resolve_artifact_path +embodichain.lab.expansion.comparison +------------------------------------ + +Reference and accepted expansion recordings share a measured timeline. The +native viewer retains robot/object ghost resources, supports pause and selection, +and publishes render states with physics disabled. Collection is an optional +caller-owned callback, and the environment factory retains task/asset ownership. + +.. currentmodule:: embodichain.lab.expansion.comparison + +.. autosummary:: + + MeasuredTrajectory + record_reference_trajectory + preview_expansion_comparison + +.. autoclass:: MeasuredTrajectory + :members: + +.. autofunction:: record_reference_trajectory + +.. autofunction:: preview_expansion_comparison + embodichain.lab.expansion.configured ------------------------------------ @@ -1880,6 +1961,21 @@ attempts at episode boundaries. Native failures invalidate the active host. ExpansionRecipeSource PreparingExpansionHost +embodichain.lab.expansion.episode_export +---------------------------------------- + +.. currentmodule:: embodichain.lab.expansion.episode_export + +Export each confirmed qualified trajectory to an independently readable LeRobot +dataset, retaining its portable T+1 evidence, source hashes and recording journal. +Export uses frozen evidence and opens no simulation World. + +.. autosummary:: + + export_expansion_episodes + +.. autofunction:: export_expansion_episodes + embodichain.lab.expansion.export -------------------------------- @@ -1891,6 +1987,26 @@ Stream confirmed numeric episodes and observation children through the existing export_expansion_lerobot +embodichain.lab.expansion.gym_workloads +-------------------------------------- + +The native workload facade owns one World containing W times B environments. +Task packages supply pinned source declarations and row-local scene/plan ports; +the facade retains native session admission, measured T+1 evidence, physical +acceptance, confirmed receipts and one bounded artifact export thread. +Integration policies may differ while physical topology and cadence must agree. + +.. currentmodule:: embodichain.lab.expansion.gym_workloads + +.. autosummary:: + + GymExpansionWorkload + run_gym_expansion_workloads + +.. autoclass:: GymExpansionWorkload + +.. autofunction:: run_gym_expansion_workloads + embodichain.lab.expansion.host ------------------------------ @@ -1902,6 +2018,30 @@ The native Gym adapter owns World preparation, parameter readback and Task Progr GymExpansionHost +embodichain.lab.expansion.host_extensions +----------------------------------------- + +Runtime extension callbacks select collection rendering, a scene source, measured +acceptance, immutable recipe transforms and per-row Task Program plan factories. +The native host continues to own preparation, clocks, state capture and receipts. + +.. currentmodule:: embodichain.lab.expansion.host_extensions + +.. autosummary:: + + ExpansionValidator + ExpansionRowContext + GymExpansionHostExtensions + +.. autoclass:: ExpansionValidator + :members: + +.. autoclass:: ExpansionRowContext + :members: + +.. autoclass:: GymExpansionHostExtensions + :members: + embodichain.lab.expansion.journal --------------------------------- @@ -1930,6 +2070,28 @@ Stage immutable numeric evidence and archived source attachments before publishi load_expansion_episode +embodichain.lab.expansion.preview +-------------------------------- + +Launch, wait for or stop a bounded native result-preview service. Status and logs +remain separate from saved episodes; PID ownership is checked before signalling. +Inputs may be native jobs, aggregate scene/motion results or preview plans. +Recorded motion supplies an endpoint when several robot solvers are present. + +.. currentmodule:: embodichain.lab.expansion.preview + +.. autosummary:: + + launch_expansion_preview + preview_expansion_results + stop_expansion_preview + +.. autofunction:: launch_expansion_preview + +.. autofunction:: preview_expansion_results + +.. autofunction:: stop_expansion_preview + embodichain.lab.expansion.program --------------------------------- @@ -2021,6 +2183,32 @@ Source-independent session limits. These settings are separate from deployment-o ExpansionCollectionCfg ExpansionPersistenceCfg +embodichain.lab.expansion.task_program_jobs +------------------------------------------ + +Load closed scene/motion declarations without simulator imports, derive local +grasp policy snapshots and collect through the existing single-World workload +engine. Caller-owned source hashes and runtime versions remain in confirmed +episode evidence. Physical thresholds and protected phase boundaries are retained; +the facade owns configuration binding, preparation, export and optional preview. + +.. currentmodule:: embodichain.lab.expansion.task_program_jobs + +.. autosummary:: + + load_task_program_expansion + load_expansion_viewer + run_task_program_expansion + derive_grasp_expansion_deployment + +.. autofunction:: load_task_program_expansion + +.. autofunction:: load_expansion_viewer + +.. autofunction:: run_task_program_expansion + +.. autofunction:: derive_grasp_expansion_deployment + embodichain.lab.expansion.visual -------------------------------- @@ -2034,6 +2222,32 @@ Apply one native visual profile per compatible World cohort. Render complete rec render_observation_variants inspect_observation_variants +embodichain.lab.expansion.workloads +----------------------------------- + +.. currentmodule:: embodichain.lab.expansion.workloads + +Rolling Task Program execution in one caller-owned World with capacity +``workload_count * batch_size``. Sources retain task-owned scene proposals and +required physical qualification; native session and sink receipts remain the +authority for admission and committed quotas. Preparing rows settle through +ordinary World steps while live trajectories retain every control boundary. + +.. autosummary:: + + ExpansionWorkloadCfg + WorkloadExecutionCfg + WorkloadAssignment + WorkloadRowResult + WorkloadSource + WorkloadProgramExecutor + +.. autoclass:: WorkloadProgramExecutor + :members: + +.. autoclass:: WorkloadSource + :members: + embodichain.lab.expansion.workflow ---------------------------------- @@ -3784,6 +3998,32 @@ embodichain.lab.sim.sensors.camera Camera CameraCfg +embodichain.lab.sim.sensors.contact +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.sensors.contact + +.. autosummary:: + + ContactPermissionChecker + +embodichain.lab.sim.sensors.contact.permissions +----------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.sensors.contact.permissions + +Check complete native contact buffers against explicit actor/frame permissions. +The host chooses the original physical sampling rate; unavailable impulse, +unknown actors and dropped evidence cannot pass. Results are owned device-side +row summaries, suitable for every-substep observers without Python contact lists. + +.. autosummary:: + + ContactPermissionChecker + +.. autoclass:: ContactPermissionChecker + :members: + embodichain.lab.sim.sensors.contact.sensor ------------------------------------------ @@ -4423,6 +4663,8 @@ native simulation windows and browser visualization. MarkerPrototypeCfg MarkerGroup + MarkerFramePublisher + embodichain.lab.visualization.markers.cfg ---------------------------------------- @@ -4433,6 +4675,22 @@ embodichain.lab.visualization.markers.cfg MarkerPrototypeCfg MarkerGroupCfg +embodichain.lab.visualization.markers.frame +------------------------------------------- + +The frame publisher stages multiple marker groups and submits one native visual +batch. Hidden instances retain their resources and actual visibility; later +frames transform only visible instances. Publishing never advances physics. + +.. currentmodule:: embodichain.lab.visualization.markers.frame + +.. autosummary:: + + MarkerFramePublisher + +.. autoclass:: MarkerFramePublisher + :members: + embodichain.lab.visualization.markers.group ------------------------------------------ @@ -6322,6 +6580,21 @@ contracts. The package-level imports above are the convenient extension API. get_sensor_registration list_sensor_types +Task pose coverage +------------------ + +The task pose coverage adapter counts measured pick/place XY-yaw cells across +scene cases in one explicitly declared experiment domain. Reservations remain +pending until the ordinary ExpansionSession persistence receipt confirms them; +planned candidate poses never contribute to confirmed coverage. + +.. autosummary:: + + embodichain.lab.expansion.task_pose_coverage.TaskPoseCoverage + +.. autoclass:: embodichain.lab.expansion.task_pose_coverage.TaskPoseCoverage + :members: + Reference state sensors ----------------------- diff --git a/embodichain/cli/expansion.py b/embodichain/cli/expansion.py index 12166044e3..d3774a65d8 100644 --- a/embodichain/cli/expansion.py +++ b/embodichain/cli/expansion.py @@ -45,7 +45,167 @@ def main(argv: Sequence[str] | None = None) -> int: command.add_argument("--stage", choices=("all", "export"), default="all") command = commands.add_parser("inspect") command.add_argument("--output", required=True) + command = commands.add_parser( + "preview", help="Replay confirmed results with native ghost rendering" + ) + source = command.add_mutually_exclusive_group(required=True) + source.add_argument( + "--input", + "--inpu", + dest="inputs", + action="append", + help="Existing job output or preview-plan directory; repeat for groups", + ) + source.add_argument( + "--job", + help="Compute an endpoint/interior preview from a Task Program expansion job", + ) + command.add_argument( + "--output", + help="New collection directory for --job; never an existing-result input", + ) + command.add_argument( + "--interior-samples", + type=int, + default=5, + help="Stratified interior parameter points, in addition to center and both extrema", + ) + command.add_argument( + "--plan-only", + action="store_true", + help="Freeze and validate the preview plan without collection/rendering", + ) + command.add_argument( + "--no-open", + action="store_true", + help="Do not open a browser after --job collection", + ) + command.add_argument( + "--reference", + help="Optional result directory containing exactly one reference episode", + ) + command.add_argument( + "--robot-asset", help="Matching URDF override if recorded asset paths moved" + ) + command.add_argument( + "--eef-link", + help="URDF link used for measured FK paths; inferred from the solver when unique", + ) + command.add_argument("--gpu", type=int, default=0) + command.add_argument( + "--port", type=int, default=8769, help="Loopback port; 0 selects a free port" + ) + command.add_argument("--max-episodes", type=int, default=32) + command.add_argument( + "--duration", + type=float, + default=0, + help="Exit after these seconds; 0 waits for Ctrl+C", + ) + command.add_argument( + "--open", + action="store_true", + help="Open a browser after the first complete render", + ) args = parser.parse_args(argv) + if args.operation == "preview": + import math + from pathlib import Path + + if ( + not 0 <= args.port <= 65535 + or args.gpu < 0 + or not math.isfinite(args.duration) + or args.duration < 0 + ): + parser.error("invalid preview port, GPU or duration") + from embodichain.lab.expansion._preview_data import load_preview + from embodichain.lab.expansion._preview_server import serve_preview + + try: + if args.open and args.no_open: + parser.error("--open and --no-open are mutually exclusive") + coverage = None + if args.job: + if args.reference: + parser.error( + "--reference is only valid with existing --input results" + ) + from embodichain.lab.expansion._preview_job import ( + plan_preview, + collect_preview, + ) + + coverage = plan_preview( + Path(args.job), + Path(args.output) if args.output else None, + interior=args.interior_samples, + max_episodes=args.max_episodes, + ) + print( + json.dumps( + { + "preview_plan": str( + Path(coverage["output"]) / "preview-plan.json" + ), + "parameter_points": len(coverage["records"]), + "planned_attempts": coverage["planned_attempts"], + } + ), + flush=True, + ) + if args.plan_only: + return 0 + import torch + + if args.gpu >= torch.cuda.device_count(): + parser.error( + "preview GPU is outside CUDA_VISIBLE_DEVICES; use a visible device ordinal" + ) + coverage = collect_preview(coverage) + print( + json.dumps( + { + "confirmed": coverage["confirmed"], + "planned_attempts": coverage["planned_attempts"], + "coverage_complete": coverage["coverage_complete"], + "output": coverage["output"], + } + ), + flush=True, + ) + if not coverage["confirmed"]: + print( + "No qualified trajectories; inspect preview-plan.json and sample logs. No boundary was replaced.", + flush=True, + ) + return 2 + inputs = [Path(coverage["output"])] + else: + if args.output or args.plan_only: + parser.error( + "--output and --plan-only require --job; use --input for existing results" + ) + inputs = [Path(p) for p in args.inputs] + data = load_preview( + inputs, + Path(args.reference) if args.reference else None, + robot_asset=Path(args.robot_asset) if args.robot_asset else None, + eef_link=args.eef_link, + max_episodes=args.max_episodes, + ) + serve_preview( + data, + port=args.port, + gpu=args.gpu, + open_browser=(bool(args.job) or args.open) and not args.no_open, + duration=args.duration, + ) + except KeyboardInterrupt: + return 0 + except (ValueError, OSError) as exc: + parser.error(str(exc)) + return 2 if coverage and not coverage["coverage_complete"] else 0 from embodichain.lab.expansion.configured import ( load_expansion_job, ) diff --git a/embodichain/compute/geometry/_hierarchical.py b/embodichain/compute/geometry/_hierarchical.py index 68acbad096..88587d2934 100644 --- a/embodichain/compute/geometry/_hierarchical.py +++ b/embodichain/compute/geometry/_hierarchical.py @@ -212,6 +212,7 @@ def _sample_hierarchy( support_gap: float, movable_roots: tuple[int, ...] | None = None, root_offset_bounds: dict[int, torch.Tensor] | None = None, + anchored_roots: tuple[int, ...] = (), ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: """One proposal per ordinal, with no per-member retry loops.""" local = [matrix.expand(count, 4, 4).clone() for matrix in relative] @@ -299,7 +300,7 @@ def _sample_hierarchy( world.append( local[node] if parent < 0 else _matrix_product(world[parent], local[node]) ) - root_boxes, deltas = [], [] + root_boxes, deltas = [], {} fixed_boxes = {} if randomize_groups and movable_roots is not None: for root in root_keys: @@ -327,7 +328,15 @@ def _sample_hierarchy( ) fixed_boxes[root] = box root_boxes.append(box) - for root, key in root_keys.items(): + # Place exact anchors before variable groups, so their space is already + # reserved when choosing the free region of an unanchored sibling. + roots = ( + sorted(root_keys, key=lambda root: root not in anchored_roots) + if anchored_roots + else root_keys + ) + for root in roots: + key = root_keys[root] points = ( _rotate_points( _subtree_points(root, local, corners, parents), world[root][:, :3, :3] @@ -336,8 +345,8 @@ def _sample_hierarchy( ) box = torch.stack((points[:, :, :2].amin(1), points[:, :, :2].amax(1)), dim=1) if root in fixed_boxes: - deltas.append( - torch.zeros((count, 2), dtype=relative.dtype, device=relative.device) + deltas[root] = torch.zeros( + (count, 2), dtype=relative.dtype, device=relative.device ) continue if randomize_groups: @@ -392,9 +401,9 @@ def _sample_hierarchy( >= group_margin - 1e-10 ) root_boxes.append(box) - deltas.append(delta) + deltas[root] = delta return ( - torch.stack(deltas, dim=1), + torch.stack([deltas[root] for root in root_keys], dim=1), ( torch.stack([sampled[node] for node in sorted(targets)], dim=1) if targets diff --git a/embodichain/compute/geometry/_warp/batch_contact_query.py b/embodichain/compute/geometry/_warp/batch_contact_query.py new file mode 100644 index 0000000000..af31b4fa4f --- /dev/null +++ b/embodichain/compute/geometry/_warp/batch_contact_query.py @@ -0,0 +1,90 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + + +"""Per-environment reduction of one independent batched Newton query.""" + +from __future__ import annotations + +import warp as wp + +__all__: list[str] = [] + + +@wp.kernel(enable_backward=False) +def contact_summary( + count: wp.array(dtype=wp.int32), + first: wp.array(dtype=wp.int32), + second: wp.array(dtype=wp.int32), + point0: wp.array(dtype=wp.vec3), + point1: wp.array(dtype=wp.vec3), + normal: wp.array(dtype=wp.vec3), + margin0: wp.array(dtype=float), + margin1: wp.array(dtype=float), + shape_body: wp.array(dtype=wp.int32), + poses: wp.array(dtype=wp.transform), + active: wp.array(dtype=wp.int32), + width: int, + batches: int, + required: float, + tolerance: wp.array(dtype=float), + capacity: int, + broad_count: wp.array(dtype=wp.int32), + broad_capacity: int, + gjk_count: wp.array(dtype=wp.int32), + gjk_capacity: int, + split_count: wp.array(dtype=wp.int32), + split_capacity: int, + manifold_count: wp.array(dtype=wp.int32), + manifold_capacity: int, + output: wp.array2d(dtype=float), +): + index = wp.tid() + if index == 0: + if ( + count[0] >= capacity + or broad_count[0] > broad_capacity + or gjk_count[0] > gjk_capacity + or split_count[0] > split_capacity + or manifold_count[0] > manifold_capacity + ): + for row in range(batches): + wp.atomic_min(output, row, 2, 0.0) + if index < count[0] and index < capacity: + a, b = first[index], second[index] + if a < 0 or b < 0 or a >= width * batches or b >= width * batches: + for row in range(batches): + wp.atomic_min(output, row, 2, 0.0) + elif a // width != b // width: + for row in range(batches): + wp.atomic_min(output, row, 2, 0.0) + else: + row = a // width + x, y = a % width, b % width + if active[x * width + y] != 0: + p0 = wp.transform_point(poses[shape_body[a]], point0[index]) + p1 = wp.transform_point(poses[shape_body[b]], point1[index]) + distance = ( + wp.dot(normal[index], p1 - p0) - margin0[index] - margin1[index] + ) + if not wp.isfinite(distance): + wp.atomic_min(output, row, 2, 0.0) + else: + wp.atomic_min(output, row, 0, distance) + if distance <= required + tolerance[row]: + wp.atomic_min( + output, row, 1, float(wp.min(x, y) * width + wp.max(x, y)) + ) diff --git a/embodichain/compute/geometry/_warp/contact_permissions.py b/embodichain/compute/geometry/_warp/contact_permissions.py new file mode 100644 index 0000000000..5ebe6bac71 --- /dev/null +++ b/embodichain/compute/geometry/_warp/contact_permissions.py @@ -0,0 +1,98 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + + +"""Inspect every measured native contact without materializing Python pairs.""" + +from __future__ import annotations + +import warp as wp + +__all__: list[str] = [] + + +@wp.func +def actor_index(actors: wp.array(dtype=wp.int32), value: int): + lower = int(0) + upper = actors.shape[0] + while lower < upper: + middle = (lower + upper) // 2 + if actors[middle] < value: + lower = middle + 1 + else: + upper = middle + return lower + + +@wp.kernel(enable_backward=False) +def classify_contacts( + pairs: wp.array3d(dtype=wp.int32), + valid: wp.array2d(dtype=wp.bool), + impulse_valid: wp.array2d(dtype=wp.bool), + actors: wp.array(dtype=wp.int32), + memberships: wp.array2d(dtype=wp.bool), + codes: wp.array(dtype=wp.int64), + moving: wp.array(dtype=wp.bool), + allowed: wp.array2d(dtype=wp.bool), + dropped: wp.array(dtype=wp.bool), + first_error: wp.array(dtype=wp.int32), + any_drop: wp.array(dtype=wp.int32), +): + row, slot = wp.tid() + if slot == 0 and dropped[row]: + wp.atomic_max(any_drop, 0, 1) + if valid[row, slot]: + reason = int(0) + if not impulse_valid[row, slot]: + reason = 1 + else: + left, right = pairs[row, slot, 0], pairs[row, slot, 1] + a, b = actor_index(actors, left), actor_index(actors, right) + if a == actors.shape[0] or b == actors.shape[0]: + reason = 2 + elif actors[a] != left or actors[b] != right: + reason = 2 + elif not memberships[a, row] or not memberships[b, row]: + reason = 2 + else: + x, y = codes[a], codes[b] + if (moving[x] or moving[y]) and not allowed[x, y]: + reason = 3 + if reason != 0: + wp.atomic_min(first_error, row, slot * 8 + reason) + + +@wp.kernel(enable_backward=False) +def contact_failures( + pairs: wp.array3d(dtype=wp.int32), + first_error: wp.array(dtype=wp.int32), + any_drop: wp.array(dtype=wp.int32), + output: wp.array2d(dtype=wp.int32), +): + row = wp.tid() + if any_drop[0] != 0: + output[row, 0] = 4 + output[row, 1] = 0 + output[row, 2] = 0 + elif first_error[row] < pairs.shape[1] * 8: + slot = first_error[row] // 8 + output[row, 0] = first_error[row] % 8 + output[row, 1] = pairs[row, slot, 0] + output[row, 2] = pairs[row, slot, 1] + else: + output[row, 0] = 0 + output[row, 1] = 0 + output[row, 2] = 0 diff --git a/embodichain/data_pipeline/recording/media.py b/embodichain/data_pipeline/recording/media.py index e36105a060..8be9ddbdae 100644 --- a/embodichain/data_pipeline/recording/media.py +++ b/embodichain/data_pipeline/recording/media.py @@ -65,7 +65,10 @@ def _atomic_json(path: Path, value: object) -> None: handle, temporary = tempfile.mkstemp(dir=path.parent, prefix=".pending-") try: with os.fdopen(handle, "w") as stream: - json.dump(value, stream, allow_nan=False, sort_keys=True) + # The C encoder returns one owned string instead of issuing a + # Python write for every metadata leaf. Keep the durable publish + # boundary and finite-value rejection unchanged. + stream.write(json.dumps(value, allow_nan=False, sort_keys=True)) stream.flush() os.fsync(stream.fileno()) os.replace(temporary, path) diff --git a/embodichain/gen_sim/README.md b/embodichain/gen_sim/README.md index 36f875c708..f41b2140b3 100644 --- a/embodichain/gen_sim/README.md +++ b/embodichain/gen_sim/README.md @@ -146,9 +146,17 @@ through the existing closed decoders. `binding.resource_anchors` maps declared resources to planar `[x, y]` anchors: a single-arm call without a resource then uses the arm already holding its object, or the arm anchored nearest to it, and a placement on a support prefers its robot-side region. `binding.release_clearance` -(metres, default 0) releases placements above their resting pose while validators -keep the resting target; the place gate otherwise expects a released object to -separate from the open gripper before retracting. +(metres, default 0.02) releases placements above their resting pose so fingers +can open without wedging against the support. An explicit zero keeps contact +placement. Ordinary generated Place calls also hold the final open command for +0.4 physical seconds before the unchanged release gate; the native planner +rounds this duration upward to the actual control grid. Explicit +`release_settle_duration` values, including zero, are retained; declaring +`release_settle_steps` suppresses that generated duration default. When both +options are explicitly supplied, the longer hold applies. External nominal +sources retain their authored timing. The gate measures opening before retract, +terminal verification measures separation, and the original resting-position +and stability validators remain unchanged. Further optional run fields: `task` reuses a published task-plan directory instead of planning, after verifying its artifacts and that it planned the same instruction diff --git a/embodichain/gen_sim/_run_binding.py b/embodichain/gen_sim/_run_binding.py index 88eece9ce9..4ef54e66b1 100644 --- a/embodichain/gen_sim/_run_binding.py +++ b/embodichain/gen_sim/_run_binding.py @@ -989,10 +989,9 @@ def _bind_deployment( if set(anchors) - declared_resources: raise ValueError("resource_anchors must name declared profile resources") holders: dict[str, str] = {} - # Placements may release above their resting pose. The place gate requires a - # released object to separate from the still-open gripper before retracting, - # which an object already resting on its support cannot do. - release_clearance = float(cfg.binding.get("release_clearance", 0.0)) + # Open above the support to avoid wedging a finger against it. The landing + # validators below retain the original measured resting pose. + release_clearance = cfg.binding["release_clearance"] configured_services = runtime_services # Measured legacy geometry binds only the registered calls whose lowerer the # run configuration does not supply. A configured lowerer owns its targets, @@ -1073,6 +1072,10 @@ def subject_of(node: dict[str, Any]) -> str | None: _register_call_options( options, monitors, call, cid, "pick" if skill == "pick_up" else skill, skill ) + if cid == "place" and "release_settle_steps" not in options[key]: + # Allow actual opening before the unchanged release monitor starts. + # This physical duration is converted on the native control grid. + options[key].setdefault("release_settle_duration", 0.4) if cid == "hand_over": options[key].setdefault("release_at_target", False) options[key].setdefault("arm_selection", "bound") diff --git a/embodichain/gen_sim/_run_config.py b/embodichain/gen_sim/_run_config.py index e0a09bc4ec..1115347f07 100644 --- a/embodichain/gen_sim/_run_config.py +++ b/embodichain/gen_sim/_run_config.py @@ -96,13 +96,14 @@ def __post_init__(self) -> None: "binding supports profile, release_clearance, resource_anchors, " "runtime_services, scene_binding and segments" ) - clearance = self.binding.get("release_clearance", 0.0) + clearance = self.binding.get("release_clearance", 0.02) if ( isinstance(clearance, bool) or not isinstance(clearance, (int, float)) or not 0.0 <= clearance < 0.2 ): raise ValueError("release_clearance must be a height in [0, 0.2) m") + self.binding = {**self.binding, "release_clearance": float(clearance)} anchors = self.binding.get("resource_anchors", {}) if type(anchors) is not dict or any( type(value) is not list diff --git a/embodichain/lab/_scene_config/__init__.py b/embodichain/lab/_scene_config/__init__.py new file mode 100644 index 0000000000..b5ff999488 --- /dev/null +++ b/embodichain/lab/_scene_config/__init__.py @@ -0,0 +1,20 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Scene layout and feasibility declarations with no simulator imports.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/embodichain/lab/_scene_config/feasibility.py b/embodichain/lab/_scene_config/feasibility.py new file mode 100644 index 0000000000..d69e076d71 --- /dev/null +++ b/embodichain/lab/_scene_config/feasibility.py @@ -0,0 +1,294 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Closed, immutable scene and task feasibility request declarations.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, fields +from functools import cached_property +import math +from typing import Literal + +from .scene import ScenePoseChange +from .values import _identifier, _rigid_pose, digest, finite, names, poses + +__all__: list[str] = [] + + +_IDENTITY = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), +) + + +@dataclass(frozen=True) +class PoseSearchVariable: + """One XY/yaw displacement applied to a scene group or robot installation. + + Args: + name: Unique variable name. + kind: ``scene`` or ``robot``; robot members are physical robot UIDs. + reference_pose: Group-to-arena Z-up transform, including mounting height. + lower: Lower XY metre / yaw radian displacement bounds. + upper: Upper displacement bounds; equal endpoints fix a parameter. + members: Member UID to member-to-group transforms. All members move + together, preserving dual-arm mounting and support-group relations. + """ + + name: str + kind: Literal["scene", "robot"] + reference_pose: tuple + lower: tuple[float, float, float] + upper: tuple[float, float, float] + members: Mapping[str, tuple] + + def __post_init__(self) -> None: + _identifier(self.name, "variable name") + if self.kind not in ("scene", "robot"): + raise ValueError("variable kind must be scene or robot") + object.__setattr__(self, "reference_pose", _rigid_pose(self.reference_pose)) + lo, hi = finite(self.lower, "lower"), finite(self.upper, "upper") + if len(lo) != 3 or len(hi) != 3 or any(a > b for a, b in zip(lo, hi)): + raise ValueError("bounds require three ordered XY/yaw coordinates") + if hi[2] - lo[2] > 2 * math.pi + 1e-10: + raise ValueError("yaw interval must span at most one turn") + object.__setattr__(self, "lower", lo) + object.__setattr__(self, "upper", hi) + object.__setattr__(self, "members", poses(self.members)) + if not self.members: + raise ValueError("variables require explicit member UIDs") + + +@dataclass(frozen=True) +class PhaseTarget: + """One simultaneous TCP target in an explicit arena or entity frame. + + Args: + control_part: Explicit solver name, preserving custom names. + pose: TCP-to-frame rigid transform. + frame_uid: Entity frame UID, or ``None`` for the arena frame. + """ + + control_part: str + pose: tuple + frame_uid: str | None = None + + def __post_init__(self) -> None: + _identifier(self.control_part, "control_part") + object.__setattr__(self, "pose", _rigid_pose(self.pose)) + if self.frame_uid is not None: + _identifier(self.frame_uid, "frame_uid") + + +@dataclass(frozen=True) +class HeldObject: + """Rigid attachment during travel; never a physical-success assertion. + + Args: + entity_uid: Held object UID. + control_part: Owner TCP/control part. + tcp_from_object: Object-to-TCP transform used by collision FK. + """ + + entity_uid: str + control_part: str + tcp_from_object: tuple = _IDENTITY + + def __post_init__(self) -> None: + _identifier(self.entity_uid, "held entity") + _identifier(self.control_part, "held owner") + object.__setattr__(self, "tcp_from_object", _rigid_pose(self.tcp_from_object)) + + +@dataclass(frozen=True) +class TaskPhase: + """A synchronized stage and its explicit planning-state transition. + + Args: + phase_id: Stable stage identity. + targets: Simultaneous targets, at most one per control part. + dependencies: Predecessors; input order must be a topological order. + held_objects: Attachments during incoming motion, one owner per object. + allowed_contacts: Physical collision IDs allowed to touch in this stage. + resulting_poses: Projected object states after this stage, used only for + subsequent planning; runtime must still read fresh physical state. + joint_targets: Explicit joint-name commands, e.g. open/closed fingers. + """ + + phase_id: str + targets: tuple[PhaseTarget, ...] = () + dependencies: tuple[str, ...] = () + held_objects: tuple[HeldObject, ...] = () + allowed_contacts: tuple[tuple[str, str], ...] = () + resulting_poses: tuple[ScenePoseChange, ...] = () + joint_targets: tuple[tuple[str, float], ...] = () + + def __post_init__(self) -> None: + _identifier(self.phase_id, "phase_id") + for attr, cls in ( + ("targets", PhaseTarget), + ("held_objects", HeldObject), + ("resulting_poses", ScenePoseChange), + ): + values = tuple(getattr(self, attr)) + if any(not isinstance(v, cls) for v in values): + raise TypeError(f"{attr} contains an invalid contract") + object.__setattr__(self, attr, values) + names([v.control_part for v in self.targets], "phase target parts") + names([v.entity_uid for v in self.held_objects], "held objects") + names([v.entity_uid for v in self.resulting_poses], "resulting poses") + object.__setattr__( + self, "dependencies", names(self.dependencies, "dependencies") + ) + pairs = tuple( + tuple(sorted(names(pair, "contact pair"))) for pair in self.allowed_contacts + ) + if any(len(p) != 2 for p in pairs) or len(set(pairs)) != len(pairs): + raise ValueError("allowed contacts require unique distinct pairs") + object.__setattr__(self, "allowed_contacts", pairs) + joints = tuple( + (_identifier(k, "joint target"), finite((v,), "joint target")[0]) + for k, v in self.joint_targets + ) + names([k for k, _ in joints], "joint targets") + object.__setattr__(self, "joint_targets", joints) + + +@dataclass(frozen=True) +class TaskOperationMode: + """One permitted grasp/assignment/handover branch of the original task.""" + + mode_id: str + phases: tuple[TaskPhase, ...] + + def __post_init__(self) -> None: + _identifier(self.mode_id, "mode_id") + values = tuple(self.phases) + if not values or any(not isinstance(v, TaskPhase) for v in values): + raise ValueError("modes require explicit task phases") + names([v.phase_id for v in values], "phase IDs") + seen = set() + for phase in values: + if not set(phase.dependencies) <= seen: + raise ValueError("phases must be in dependency order without cycles") + seen.add(phase.phase_id) + object.__setattr__(self, "phases", values) + + +@dataclass(frozen=True) +class TaskFeasibilityRequest: + """Versioned fixed semantics, robot, scene, and allowed joint search domain. + + Args: + task_id: Semantic identity including original roles/goals/invariants. + robot_profile_id: Robot, tool and calibrated configuration identity. + policy_id: Planner/runtime policy version. + validator_id: Measured acceptance version. + parent_scene_id: Base scene revision/content identity. + entity_poses: Authored arena poses, including every variable member. + variables: Permitted scene and mounting changes. + modes: Allowed operation modes, preserving resource constraints. + """ + + task_id: str + robot_profile_id: str + policy_id: str + validator_id: str + parent_scene_id: str + entity_poses: Mapping[str, tuple] + variables: tuple[PoseSearchVariable, ...] + modes: tuple[TaskOperationMode, ...] + + def __post_init__(self) -> None: + for key in ( + "task_id", + "robot_profile_id", + "policy_id", + "validator_id", + "parent_scene_id", + ): + _identifier(getattr(self, key), key) + object.__setattr__(self, "entity_poses", poses(self.entity_poses)) + variables, modes = tuple(self.variables), tuple(self.modes) + if any(not isinstance(v, PoseSearchVariable) for v in variables): + raise TypeError("variables require PoseSearchVariable values") + if not modes or any(not isinstance(v, TaskOperationMode) for v in modes): + raise ValueError("request requires explicit operation modes") + names([v.name for v in variables], "variable names") + members = [k for v in variables for k in v.members] + names(members, "variable members") + if not set(members) <= self.entity_poses.keys(): + raise ValueError("variable members must exist in the authored scene") + names([v.mode_id for v in modes], "mode IDs") + for mode in modes: + for phase in mode.phases: + referenced = { + t.frame_uid for t in phase.targets if t.frame_uid is not None + } + referenced |= {h.entity_uid for h in phase.held_objects} + referenced |= {p.entity_uid for p in phase.resulting_poses} + if not referenced <= self.entity_poses.keys(): + raise ValueError("phase refers to an undeclared entity") + object.__setattr__(self, "variables", variables) + object.__setattr__(self, "modes", modes) + + @cached_property + def request_id(self) -> str: + """Return the content identity of all fixed conditions and permissions.""" + return digest(self) + + +def _closed(cls: type, value: Mapping[str, object]) -> dict: + if not isinstance(value, Mapping): + raise ValueError(f"{cls.__name__} requires a mapping") + unknown = set(value) - {f.name for f in fields(cls)} + if unknown: + raise ValueError(f"Unknown {cls.__name__} fields: {sorted(unknown)}") + return dict(value) + + +def request_from_mapping(value: Mapping[str, object]) -> TaskFeasibilityRequest: + """Decode a provider-independent request through closed production contracts. + + Args: + value: JSON/YAML task, scene, mounting and phase declarations. + Returns: + Validated immutable request. No dotted import or model client is loaded. + """ + data = _closed(TaskFeasibilityRequest, value) + data["variables"] = tuple( + PoseSearchVariable(**_closed(PoseSearchVariable, v)) for v in data["variables"] + ) + modes = [] + for mode in data["modes"]: + m = _closed(TaskOperationMode, mode) + phases = [] + for phase in m["phases"]: + p = _closed(TaskPhase, phase) + for key, cls in ( + ("targets", PhaseTarget), + ("held_objects", HeldObject), + ("resulting_poses", ScenePoseChange), + ): + p[key] = tuple(cls(**_closed(cls, v)) for v in p.get(key, ())) + phases.append(TaskPhase(**p)) + m["phases"] = tuple(phases) + modes.append(TaskOperationMode(**m)) + data["modes"] = tuple(modes) + return TaskFeasibilityRequest(**data) diff --git a/embodichain/lab/_scene_config/policy.py b/embodichain/lab/_scene_config/policy.py new file mode 100644 index 0000000000..acbb6342da --- /dev/null +++ b/embodichain/lab/_scene_config/policy.py @@ -0,0 +1,430 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Source-independent policies for bounded scene layout sampling.""" + +from __future__ import annotations +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import MISSING +import math +from typing import Any, TYPE_CHECKING +from embodichain.utils import configclass + +if TYPE_CHECKING: + from shapely.geometry import Polygon, MultiPolygon + +__all__: list[str] = [] + + +@configclass +class MemberRandomizationCfg: + """One non-root member's displacement bounds in its parent support frame. + + ``xy_offset_bounds_m`` is ``[[min_x, min_y], [max_x, max_y]]`` relative + to the original entity origin; ``None`` samples the complete support area. + Yaw is a continuous increment around the support normal, retaining tilt. + """ + + xy_offset_bounds_m: list[list[float]] | None = None + yaw_delta_range_deg: list[float] = [-180.0, 180.0] + + +@configclass +class SupportOverrideCfg: + """Explicit parent-local support frame and GeoJSON Polygon/MultiPolygon. + + The proper rigid ``parent_from_support`` matrix locates the support plane + at support Z=0 in the parent local Z-up geometry after asset scaling. Its polygon must be covered by actual coplanar mesh faces. + """ + + parent_from_support: list[list[float]] = MISSING + polygon: dict = MISSING + + +@configclass +class IntraGroupRandomizationCfg: + """Explicit member selection and conservative support geometry settings. + + Fixed members follow their parent. Clearances and grid resolution are in + metres. Overrides are keyed by parent UID, independently of target order. + """ + + targets: dict[str, MemberRandomizationCfg] = {} + boundary_margin_m: float = 0.002 + sibling_margin_m: float = 0.002 + support_gap_m: float = 0.001 + grid_resolution_m: float = 0.001 + support_overrides: dict[str, SupportOverrideCfg] = {} + + +def _validate_backend(*, backend: str, device: str, batch_size: int) -> None: + """Validate execution controls without initializing CUDA or reading assets.""" + if backend not in ("slsqp", "batched"): + raise ValueError("backend must be slsqp or batched.") + if not isinstance(device, str) or not ( + device == "cpu" + or device == "cuda" + or ( + device.startswith("cuda:") and device[5:].isascii() and device[5:].isdigit() + ) + ): + raise ValueError("device must be cpu, cuda, or cuda:N.") + if backend == "slsqp" and device != "cpu": + raise ValueError( + "The slsqp backend runs on CPU; use backend: batched for CUDA." + ) + if type(batch_size) is not int or batch_size < 1: + raise ValueError("batch_size must be a positive integer.") + + +def _validate_generation_parameters( + *, seed: int, candidate_count: int, max_attempts: int, collision_margin_m: float +) -> None: + """Validate YAML values and Python overrides before accessing scene files.""" + for name, value, minimum in ( + ("seed", seed, 0), + ("candidate_count", candidate_count, 1), + ("max_attempts", max_attempts, 1), + ): + if type(value) is not int or value < minimum: + raise ValueError(f"{name} must be an integer >= {minimum}.") + if candidate_count > max_attempts: + raise ValueError("max_attempts must be at least candidate_count.") + if type(collision_margin_m) not in (int, float) or collision_margin_m < 0: + raise ValueError("collision_margin_m must be finite and nonnegative.") + try: + finite = math.isfinite(collision_margin_m) + except OverflowError: + finite = False + if not finite: + raise ValueError("collision_margin_m must be finite and nonnegative.") + + +def _mapping(value: Any, *, name: str, allowed: set[str]) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{name} must be a mapping.") + unknown = set(value) - allowed + if unknown: + raise ValueError(f"Unknown {name} fields: {', '.join(sorted(unknown))}.") + return value + + +def _decode_intra(value: Any) -> IntraGroupRandomizationCfg | None: + """Strictly decode nested YAML mappings, also used for persisted policies.""" + if value is None: + return None + root = _mapping( + value, + name="intra_group", + allowed={ + "targets", + "boundary_margin_m", + "sibling_margin_m", + "support_gap_m", + "grid_resolution_m", + "support_overrides", + }, + ) + targets = root.get("targets", {}) + if not isinstance(targets, dict): + raise ValueError("targets must be a mapping.") + decoded = {} + for uid, target in targets.items(): + if not isinstance(uid, str) or not uid.strip(): + raise ValueError("Target UIDs must be nonempty strings.") + item = _mapping( + target, + name=f"target {uid}", + allowed={"xy_offset_bounds_m", "yaw_delta_range_deg"}, + ) + decoded[uid] = MemberRandomizationCfg(**item) + overrides = root.get("support_overrides", {}) + if not isinstance(overrides, dict): + raise ValueError("support_overrides must be a mapping.") + override_cfg = {} + for uid, override in overrides.items(): + item = _mapping( + override, + name=f"support override {uid}", + allowed={"parent_from_support", "polygon"}, + ) + if set(item) != {"parent_from_support", "polygon"}: + raise ValueError("Support overrides require a transform and polygon.") + override_cfg[uid] = SupportOverrideCfg(**item) + cfg = IntraGroupRandomizationCfg( + **{k: v for k, v in root.items() if k not in ("targets", "support_overrides")}, + targets=decoded, + support_overrides=override_cfg, + ) + _validate_intra(cfg) + return cfg + + +def _read_support_polygon(value: object) -> Polygon | MultiPolygon: + """Decode finite XY GeoJSON before accessing any scene assets.""" + import numpy as np + import shapely + from shapely.geometry import Polygon, MultiPolygon, shape + + if not isinstance(value, dict) or value.get("type") not in ( + "Polygon", + "MultiPolygon", + ): + raise ValueError( + "Support polygon must be a GeoJSON Polygon or MultiPolygon mapping." + ) + try: + polygon = shape(value) + except ( + AttributeError, + KeyError, + IndexError, + TypeError, + ValueError, + OverflowError, + shapely.errors.GEOSException, + ) as exc: + raise ValueError("Support polygon has invalid GeoJSON coordinates.") from exc + if ( + not isinstance(polygon, (Polygon, MultiPolygon)) + or polygon.is_empty + or polygon.has_z + or not np.isfinite(shapely.get_coordinates(polygon)).all() + or not polygon.is_valid + or polygon.area <= 0 + ): + raise ValueError( + "Support polygon must contain a valid nonempty finite XY area." + ) + return polygon + + +def _validate_intra(cfg: IntraGroupRandomizationCfg) -> None: + import numpy as np + + if not isinstance(cfg, IntraGroupRandomizationCfg): + raise ValueError("intra_group must be an IntraGroupRandomizationCfg.") + cfg.validate() + if not isinstance(cfg.targets, dict) or not isinstance(cfg.support_overrides, dict): + raise ValueError("targets and support_overrides must be mappings.") + for field in ( + "boundary_margin_m", + "sibling_margin_m", + "support_gap_m", + "grid_resolution_m", + ): + value = getattr(cfg, field) + try: + finite = type(value) in (int, float) and math.isfinite(value) + except OverflowError: + finite = False + if not finite or value < 0 or (field == "grid_resolution_m" and value == 0): + raise ValueError( + f"{field} must be finite and {'positive' if field == 'grid_resolution_m' else 'nonnegative'}." + ) + for uid, item in cfg.targets.items(): + if ( + not isinstance(uid, str) + or not uid + or not isinstance(item, MemberRandomizationCfg) + ): + raise ValueError("targets must map UIDs to MemberRandomizationCfg.") + yaw = np.asarray(item.yaw_delta_range_deg) + if ( + yaw.shape != (2,) + or yaw.dtype.kind not in "fi" + or not np.isfinite(yaw).all() + or yaw[0] > yaw[1] + or not np.isfinite(yaw[1] - yaw[0]) + ): + raise ValueError( + "yaw_delta_range_deg requires finite ordered bounds and width." + ) + if item.xy_offset_bounds_m is not None: + xy = np.asarray(item.xy_offset_bounds_m) + if ( + xy.shape != (2, 2) + or xy.dtype.kind not in "fi" + or not np.isfinite(xy).all() + or np.any(xy[0] > xy[1]) + ): + raise ValueError( + "xy_offset_bounds_m requires finite ordered lower/upper XY corners." + ) + for uid, override in cfg.support_overrides.items(): + if ( + not isinstance(uid, str) + or not uid + or not isinstance(override, SupportOverrideCfg) + ): + raise ValueError("support_overrides must map UIDs to SupportOverrideCfg.") + override.validate() + try: + _read_support_polygon(override.polygon) + except ValueError as exc: + raise ValueError(f"Support override {uid!r}: {exc}") from exc + try: + matrix = np.asarray(override.parent_from_support, dtype=float) + except (TypeError, ValueError, OverflowError) as exc: + raise ValueError( + "parent_from_support must contain finite numeric values." + ) from exc + if ( + matrix.shape != (4, 4) + or not np.isfinite(matrix).all() + or not np.array_equal(matrix[3], [0, 0, 0, 1]) + or not np.allclose( + matrix[:3, :3].T @ matrix[:3, :3], np.eye(3), atol=1e-8, rtol=0 + ) + or not np.isclose(np.linalg.det(matrix[:3, :3]), 1, atol=1e-8, rtol=0) + ): + raise ValueError("parent_from_support must be a proper rigid transform.") + + +def _intra_payload(cfg: IntraGroupRandomizationCfg) -> dict: + """Serialize only documented settings, independent of configclass internals.""" + return deepcopy( + { + "targets": { + uid: { + "xy_offset_bounds_m": item.xy_offset_bounds_m, + "yaw_delta_range_deg": list(item.yaw_delta_range_deg), + } + for uid, item in sorted(cfg.targets.items()) + }, + **{ + key: float(getattr(cfg, key)) + for key in ( + "boundary_margin_m", + "sibling_margin_m", + "support_gap_m", + "grid_resolution_m", + ) + }, + "support_overrides": { + uid: { + "parent_from_support": item.parent_from_support, + "polygon": item.polygon, + } + for uid, item in sorted(cfg.support_overrides.items()) + }, + } + ) + + +@configclass +class SceneRandomizationPolicyCfg: + """Geometric policy independent of scene files and runtime lifecycles. + + Args: + backend: Explicit numeric backend; the shared interface uses batched sampling. + device: CPU or CUDA tensor device. + batch_size: Maximum logical proposals evaluated together. + collision_margin_m: Minimum group AABB separation in metres. + randomize_groups: Sample permitted group-root XY translations. + group_targets: Root UIDs to move; None selects all permitted roots. + group_xy_offset_bounds_m: Optional per-root arena XY displacement bounds. + intra_group: Selected non-root XY/yaw changes and support settings. + """ + + backend: str = "batched" + device: str = "cpu" + batch_size: int = 4096 + collision_margin_m: float = 0.02 + randomize_groups: bool = True + group_targets: tuple[str, ...] | None = None + group_xy_offset_bounds_m: dict[str, list[list[float]]] = {} + intra_group: IntraGroupRandomizationCfg | None = None + + +@configclass +class SceneRandomizationJobCfg: + """Bounded caller-owned generation settings and output representation. + + Args: + policy: Source-independent geometric sampling policy. + seed: Addressed generation seed. + candidate_count: Requested valid layouts. + max_attempts: Whole-layout proposal budget. + layout_storage: JSON or packed candidate storage. + """ + + policy: SceneRandomizationPolicyCfg = SceneRandomizationPolicyCfg() + seed: int = 0 + candidate_count: int = 10 + max_attempts: int = 1000 + layout_storage: str = "packed" + + +def _policy_data(cfg: SceneRandomizationPolicyCfg | Mapping) -> dict: + if isinstance(cfg, Mapping): + raw = dict(cfg) + cfg = SceneRandomizationPolicyCfg(**raw) + if not isinstance(cfg, SceneRandomizationPolicyCfg): + raise TypeError("policy must be a SceneRandomizationPolicyCfg or mapping") + cfg = cfg.copy() + cfg.validate() + _validate_backend(backend=cfg.backend, device=cfg.device, batch_size=cfg.batch_size) + if cfg.backend != "batched": + raise ValueError( + "The source-independent layout API supports backend='batched'." + ) + _validate_generation_parameters( + seed=0, + candidate_count=1, + max_attempts=1, + collision_margin_m=cfg.collision_margin_m, + ) + if type(cfg.randomize_groups) is not bool: + raise ValueError("randomize_groups must be a boolean.") + intra = cfg.intra_group + if isinstance(intra, dict) or intra is None: + intra = _decode_intra(intra) + if intra is None: + intra = IntraGroupRandomizationCfg() + _validate_intra(intra) + if not isinstance(cfg.group_xy_offset_bounds_m, dict): + raise ValueError("group_xy_offset_bounds_m must map root UIDs to XY bounds.") + for uid, bounds in cfg.group_xy_offset_bounds_m.items(): + if bounds is None: + raise ValueError( + "Root XY bounds must be explicit lower/upper corners; omit unbounded roots." + ) + _decode_intra( + { + "targets": { + uid: {"xy_offset_bounds_m": bounds, "yaw_delta_range_deg": [0, 0]} + } + } + ) + if cfg.group_xy_offset_bounds_m and not cfg.randomize_groups: + raise ValueError("Root XY bounds require randomize_groups=True.") + if cfg.group_targets is not None and ( + isinstance(cfg.group_targets, str) + or len(set(cfg.group_targets)) != len(cfg.group_targets) + ): + raise ValueError("group_targets must contain unique root UIDs.") + return { + "backend": cfg.backend, + "device": cfg.device, + "batch_size": cfg.batch_size, + "collision_margin_m": float(cfg.collision_margin_m), + "randomize_groups": cfg.randomize_groups, + "group_targets": None if cfg.group_targets is None else list(cfg.group_targets), + "group_xy_offset_bounds_m": deepcopy(cfg.group_xy_offset_bounds_m), + "intra_group": _intra_payload(intra), + } diff --git a/embodichain/lab/_scene_config/scene.py b/embodichain/lab/_scene_config/scene.py new file mode 100644 index 0000000000..e22e38fecc --- /dev/null +++ b/embodichain/lab/_scene_config/scene.py @@ -0,0 +1,157 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Immutable scene proposals, independent of task and execution lifecycles.""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +import json + +__all__: list[str] = [] + + +from .values import _identifier, _nonnegative_integer, _rigid_pose + + +def _variant_id( + parent_scene_id: str, + seed: int, + ordinal: int, + pose_changes: tuple, + metadata: tuple, +) -> str: + """Hash already validated, canonically ordered proposal values.""" + payload = { + "schema": "scene-variant/v1", + "parent_scene_id": parent_scene_id, + "seed": seed, + "ordinal": ordinal, + "pose_changes": pose_changes, + "metadata": metadata, + } + encoded = json.dumps( + payload, sort_keys=True, separators=(",", ":"), allow_nan=False + ).encode("utf-8") + return "scene-variant:" + hashlib.sha256(encoded).hexdigest() + + +@dataclass(frozen=True) +class ScenePoseChange: + """A proposed pose for one existing physical entity. + + Args: + entity_uid: Stable physical entity UID, independent of environment slots. + arena_pose: Homogeneous 4x4 transform from the entity frame into its + local Z-up arena frame. Inputs are copied into immutable tuples. + + .. attention:: + An arena pose excludes replication offsets. The host converts it to + backend coordinates when applying the candidate. This contract does + not establish support, collision freedom, or manipulation feasibility. + """ + + entity_uid: str + arena_pose: tuple[tuple[float, ...], ...] + + def __post_init__(self) -> None: + _identifier(self.entity_uid, "entity_uid") + object.__setattr__(self, "arena_pose", _rigid_pose(self.arena_pose)) + + +@dataclass(frozen=True) +class SceneVariant: + """An immutable scene proposal with reproducible proposal identity. + + Args: + parent_scene_id: Content/revision identity of the original scene. + seed: Nonnegative proposal-generation seed. + ordinal: Nonnegative position in the candidate source. + pose_changes: Unique existing entities and their proposed arena poses. + metadata: Unique string key/value provenance entries. Entries and pose + changes are sorted so equivalent input ordering has one identity. + + .. attention:: + ``variant_id`` identifies a proposal, not a settled initial state or a + successful execution. Hosts record those identities and evidence + separately. Metadata describes provenance, not executable operations. + Since ``ordinal`` participates in the hash, consuming the same layout + at a different source position changes ``variant_id``. File-backed + consumers should retain the original layout identity and proposal + ordinal in metadata for provenance across selection orders; this ID + is not a geometry-only deduplication key. + """ + + parent_scene_id: str + seed: int + ordinal: int + pose_changes: tuple[ScenePoseChange, ...] + metadata: tuple[tuple[str, str], ...] = () + + def __post_init__(self) -> None: + _identifier(self.parent_scene_id, "parent_scene_id") + _nonnegative_integer(self.seed, "seed") + _nonnegative_integer(self.ordinal, "ordinal") + try: + changes = tuple(self.pose_changes) + except TypeError as exc: + raise ValueError( + "pose_changes must contain ScenePoseChange values" + ) from exc + if any(not isinstance(change, ScenePoseChange) for change in changes): + raise ValueError("pose_changes must contain ScenePoseChange values") + if len({change.entity_uid for change in changes}) != len(changes): + raise ValueError("pose_changes must contain unique entity UIDs") + object.__setattr__( + self, + "pose_changes", + tuple(sorted(changes, key=lambda change: change.entity_uid)), + ) + try: + entries = tuple(self.metadata) + if any(isinstance(entry, str) for entry in entries): + raise ValueError("metadata must contain string key/value pairs") + metadata = tuple(tuple(entry) for entry in entries) + except TypeError as exc: + raise ValueError("metadata must contain string key/value pairs") from exc + if any(len(entry) != 2 for entry in metadata): + raise ValueError("metadata must contain string key/value pairs") + for key, value in metadata: + _identifier(key, "metadata key") + if not isinstance(value, str): + raise ValueError("metadata values must be strings") + if len({key for key, _ in metadata}) != len(metadata): + raise ValueError("metadata must contain unique keys") + object.__setattr__(self, "metadata", tuple(sorted(metadata))) + + @property + def variant_id(self) -> str: + """Return a stable identity including geometry, source, seed, and ordinal. + + Returns: + SHA-256 identity independent of Python hash randomization and + temporary physical environment assignments. + """ + return _variant_id( + self.parent_scene_id, + self.seed, + self.ordinal, + tuple( + (change.entity_uid, change.arena_pose) for change in self.pose_changes + ), + self.metadata, + ) diff --git a/embodichain/lab/_scene_config/values.py b/embodichain/lab/_scene_config/values.py new file mode 100644 index 0000000000..53efdf7eb2 --- /dev/null +++ b/embodichain/lab/_scene_config/values.py @@ -0,0 +1,195 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Private canonical value validation for feasibility artifacts.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import fields, is_dataclass +import hashlib +import json +import math +from numbers import Real +from enum import Enum +import numpy as np +import sys +from pathlib import Path +from types import MappingProxyType + +__all__ = [] + + +def _identifier(value: str, name: str) -> str: + if not isinstance(value, str) or not value or value.strip() != value: + raise ValueError(f"{name} must be a nonempty stripped string") + return value + + +def _nonnegative_integer(value: int, name: str) -> int: + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a nonnegative integer") + return value + + +def _rigid_pose(value: object) -> tuple[tuple[float, ...], ...]: + try: + rows = tuple(tuple(row) for row in value) # type: ignore[union-attr] + except TypeError as exc: + raise ValueError("arena_pose must be a 4x4 rigid transform") from exc + if len(rows) != 4 or any(len(row) != 4 for row in rows): + raise ValueError("arena_pose must be a 4x4 rigid transform") + if any( + isinstance(item, bool) or not isinstance(item, Real) or not math.isfinite(item) + for row in rows + for item in row + ): + raise ValueError("arena_pose must contain finite real numbers") + pose = tuple( + tuple(float(item) if item != 0 else 0.0 for item in row) for row in rows + ) + tolerance = 1e-6 + if any( + abs(pose[3][index] - target) > tolerance + for index, target in enumerate((0, 0, 0, 1)) + ): + raise ValueError("arena_pose must have homogeneous last row [0, 0, 0, 1]") + rotation = tuple(row[:3] for row in pose[:3]) + if any( + abs(sum(rotation[k][i] * rotation[k][j] for k in range(3)) - (i == j)) + > tolerance + for i in range(3) + for j in range(3) + ): + raise ValueError("arena_pose rotation must be orthonormal") + determinant = ( + rotation[0][0] + * (rotation[1][1] * rotation[2][2] - rotation[1][2] * rotation[2][1]) + - rotation[0][1] + * (rotation[1][0] * rotation[2][2] - rotation[1][2] * rotation[2][0]) + + rotation[0][2] + * (rotation[1][0] * rotation[2][1] - rotation[1][1] * rotation[2][0]) + ) + if abs(determinant - 1.0) > tolerance: + raise ValueError("arena_pose rotation must be proper (determinant +1)") + return (*pose[:3], (0.0, 0.0, 0.0, 1.0)) + + +def plain(value: object) -> object: + if isinstance(value, np.ndarray): + return plain(value.tolist()) + if isinstance(value, np.generic): + return plain(value.item()) + # Tensor values can only exist when torch is already loaded by the caller. + torch = sys.modules.get("torch") + if torch is not None and isinstance(value, torch.Tensor): + return plain(value.detach().cpu().tolist()) + if isinstance(value, Path) or ( + torch is not None and isinstance(value, torch.device) + ): + return str(value) + if isinstance(value, slice): + return {"slice": [value.start, value.stop, value.step]} + if isinstance(value, Enum): + return plain(value.value) + if isinstance(getattr(value, "name", None), str) and isinstance( + getattr(value, "value", None), Real + ): + return { + "enum_type": type(value).__module__ + "." + type(value).__qualname__, + "name": value.name, + "value": plain(value.value), + } + if isinstance(value, (set, frozenset)): + return sorted( + (plain(v) for v in value), key=lambda v: json.dumps(v, sort_keys=True) + ) + if is_dataclass(value) and not isinstance(value, type): + return {f.name: plain(getattr(value, f.name)) for f in fields(value)} + if isinstance(value, Mapping): + return {str(k): plain(v) for k, v in value.items()} + if isinstance(value, (tuple, list)): + return [plain(v) for v in value] + return value + + +def digest(value: object) -> str: + return hashlib.sha256( + json.dumps( + plain(value), sort_keys=True, separators=(",", ":"), allow_nan=False + ).encode() + ).hexdigest() + + +def owned_json(value: Mapping[str, object]) -> Mapping[str, object]: + copied = json.loads(json.dumps(plain(value), allow_nan=False)) + + def freeze(item: object) -> object: + if isinstance(item, dict): + return MappingProxyType({k: freeze(v) for k, v in item.items()}) + if isinstance(item, list): + return tuple(freeze(v) for v in item) + return item + + return freeze(copied) + + +def finite(values: object, name: str) -> tuple[float, ...]: + result = tuple(values) + if any( + isinstance(v, bool) or not isinstance(v, Real) or not math.isfinite(v) + for v in result + ): + raise ValueError(f"{name} requires finite real values") + return tuple(float(v) for v in result) + + +def count(value: int, name: str, minimum: int = 1) -> None: + if type(value) is not int or value < minimum: + raise ValueError(f"{name} must be an integer >= {minimum}") + + +def names(values: object, name: str) -> tuple[str, ...]: + if isinstance(values, str): + raise ValueError(f"{name} requires a sequence") + result = tuple(values) + for value in result: + _identifier(value, name) + if len(set(result)) != len(result): + raise ValueError(f"{name} must be unique") + return result + + +def poses(values: Mapping[str, object]) -> Mapping[str, tuple]: + return MappingProxyType( + { + _identifier(k, "entity UID"): _rigid_pose(v) + for k, v in sorted(values.items()) + } + ) + + +def atomic_json(path: Path, value: object) -> None: + import os + import tempfile + + path.parent.mkdir(parents=True, exist_ok=True) + fd, temporary = tempfile.mkstemp(prefix=path.name + ".", dir=path.parent) + try: + with os.fdopen(fd, "w", encoding="utf-8") as stream: + json.dump(plain(value), stream, allow_nan=False, sort_keys=True, indent=2) + os.replace(temporary, path) + finally: + Path(temporary).unlink(missing_ok=True) diff --git a/embodichain/lab/expansion/__init__.py b/embodichain/lab/expansion/__init__.py index ff136b8f58..468fa04777 100644 --- a/embodichain/lab/expansion/__init__.py +++ b/embodichain/lab/expansion/__init__.py @@ -60,6 +60,30 @@ "load_expansion_episode", "resolve_expansion_job", "build_expansion_report", + "ExpansionWorkloadCfg", + "WorkloadExecutionCfg", + "WorkloadAssignment", + "WorkloadRowResult", + "WorkloadSource", + "WorkloadProgramExecutor", + "GymExpansionWorkload", + "run_gym_expansion_workloads", + "load_task_program_expansion", + "load_expansion_viewer", + "run_task_program_expansion", + "derive_grasp_expansion_deployment", + "launch_expansion_preview", + "stop_expansion_preview", + "preview_expansion_results", + "export_expansion_episodes", + "GymExpansionHostExtensions", + "ExpansionRowContext", + "ExpansionValidator", + "MeasuredExpansionValidator", + "MeasuredTrajectory", + "record_reference_trajectory", + "preview_expansion_comparison", + "report_expansion_distribution", ] @@ -98,6 +122,30 @@ def __getattr__(name: str) -> object: "load_expansion_episode": "persistence", "resolve_expansion_job": "configured", "build_expansion_report": "reporting", + "ExpansionWorkloadCfg": "workloads", + "WorkloadExecutionCfg": "workloads", + "WorkloadAssignment": "workloads", + "WorkloadRowResult": "workloads", + "WorkloadSource": "workloads", + "WorkloadProgramExecutor": "workloads", + "GymExpansionWorkload": "gym_workloads", + "run_gym_expansion_workloads": "gym_workloads", + "load_task_program_expansion": "_task_program_config", + "load_expansion_viewer": "_task_program_config", + "run_task_program_expansion": "task_program_jobs", + "derive_grasp_expansion_deployment": "task_program_jobs", + "launch_expansion_preview": "preview", + "stop_expansion_preview": "preview", + "preview_expansion_results": "preview", + "export_expansion_episodes": "episode_export", + "GymExpansionHostExtensions": "host_extensions", + "ExpansionRowContext": "host_extensions", + "ExpansionValidator": "host_extensions", + "MeasuredExpansionValidator": "acceptance", + "MeasuredTrajectory": "comparison", + "record_reference_trajectory": "comparison", + "preview_expansion_comparison": "comparison", + "report_expansion_distribution": "distribution", }.get(name, "engine") module = f"{__name__}.{owner}" value = getattr(import_module(module), name) diff --git a/embodichain/lab/expansion/_distribution.html b/embodichain/lab/expansion/_distribution.html new file mode 100644 index 0000000000..af699051a1 --- /dev/null +++ b/embodichain/lab/expansion/_distribution.html @@ -0,0 +1,39 @@ +任务数据分布 + +
EMBODICHAIN / DATA OBSERVATORY

任务数据分布

从空间覆盖到运动多样性,检查每一轮数据补充。
确认落盘 · 实测证据

+ +

空间覆盖地图

世界坐标 XY / m;点击区域筛选实测轨迹。颜色深浅表示确认样本数。

稀疏密集 · 前后共用色标
按网格选择样本

代表轨迹联动

来源与现有预览入口
+

起点 × 终点联合分布

点击计数筛选该组合;零表示无确认数据,不代表不可行。只展示两快照中出现过的区域。

+

完整姿态分布

SO(3) 规范旋转向量三维分箱,步长 30°,非等体积;不合并物体对称姿态。

补数前后对照

数据说明与导出

本报告是 HTML 入口。分页明细位于 pages/;分享时请保留整个目录。

    Agent 结构化结果 · 前快照特征 · 后快照特征
    + diff --git a/embodichain/lab/expansion/_distribution.py b/embodichain/lab/expansion/_distribution.py new file mode 100644 index 0000000000..cb2ec86430 --- /dev/null +++ b/embodichain/lab/expansion/_distribution.py @@ -0,0 +1,374 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Offline confirmed-episode distribution analysis and linked HTML report. + +Run with ``python -m embodichain.lab.expansion._distribution --help``. +""" + +from __future__ import annotations + +import argparse +from collections import Counter +from collections.abc import Mapping +from copy import deepcopy +import hashlib +import json +from pathlib import Path +from typing import Any +import numpy as np + +__all__: list[str] = [] + + +def _digest(value: Any) -> str: + return hashlib.sha256( + json.dumps(value, sort_keys=True, allow_nan=False).encode() + ).hexdigest() + + +def _cell(pose: list[float], cell_size: float) -> str: + return ",".join(str(int(x)) for x in np.floor(np.asarray(pose[:2]) / cell_size)) + + +def _installation_pose(pose: list[float]) -> list[float]: + """Group installation readout jitter only; never alter recorded/FK evidence.""" + value = np.array(pose, dtype=float, copy=True) + if value.shape != (7,) or not np.isfinite(value).all(): + raise ValueError("Installation requires a finite XYZ/xyzw pose") + norm = np.linalg.norm(value[3:]) + if norm < 1e-12: + raise ValueError("Installation quaternion must be nonzero") + value[3:] /= norm + value = np.round(value, 6) + first = next(v for v in value[[6, 3, 4, 5]] if v != 0) + if first < 0: + value[3:] *= -1 + value[value == 0] = 0.0 + return value.tolist() + + +def _motion(points: np.ndarray, times: np.ndarray) -> dict: + if points.ndim != 2 or points.shape[0] != len(times) or len(times) < 2: + raise ValueError("Motion requires aligned T+1 points and timestamps") + if ( + not np.isfinite(points).all() + or not np.isfinite(times).all() + or np.any(np.diff(times) <= 0) + ): + raise ValueError("Motion must be finite with increasing timestamps") + lengths = np.linalg.norm(np.diff(points, axis=0), axis=1) + arc = np.r_[0.0, np.cumsum(lengths)] + keep = np.r_[True, np.diff(arc) > 1e-12] + grid = np.linspace(0, arc[-1], 32) + path = np.stack( + [np.interp(grid, arc[keep], points[keep, i]) for i in range(points.shape[1])], + axis=1, + ) + return { + "path_length": float(arc[-1]), + "duration_s": float(times[-1] - times[0]), + "speed_p50_p95": np.quantile(lengths / np.diff(times), [0.5, 0.95]).tolist(), + "path": path.tolist(), + } + + +def _summarize(rows: list[dict], size: float, minimum: int) -> dict: + unique = {} + for row in rows: + key = row["commit_id"] + if key in unique and unique[key] != row: + raise ValueError("Conflicting commit identity") + unique[key] = deepcopy(row) + rows = list(unique.values()) + pick, place, joint = Counter(), Counter(), Counter() + for row in rows: + if row["object_motion"] is None: + continue + a, b = _cell(row["initial_pose"], size), _cell(row["final_pose"], size) + pick[a] += 1 + place[b] += 1 + joint[a + "|" + b] += 1 + row["pick_cell"], row["place_cell"] = a, b + distributions = {} + for family in ("object_motion", "joint_motion", "tcp_motion"): + for metric in ("path_length", "duration_s"): + values = [ + r[family][metric] for r in rows if r.get(family) and metric in r[family] + ] + distributions[family + "." + metric] = { + "valid_count": len(values), + "missing_count": len(rows) - len(values), + "p0_p25_p50_p75_p100": ( + np.quantile(values, [0, 0.25, 0.5, 0.75, 1]).tolist() + if values + else None + ), + } + orientation_counts = {} + for phase in ("initial", "final"): + orientation_counts[phase] = dict( + Counter( + r["orientation"]["rotation_bin_" + phase] + for r in rows + if r.get("orientation") + ) + ) + return { + "orientation": orientation_counts, + "tcp_valid": sum(r.get("tcp_motion") is not None for r in rows), + "motion_distributions": distributions, + "episodes": rows, + "count": len(rows), + "layout_valid": sum(pick.values()), + "layout_missing": len(rows) - sum(pick.values()), + "pick": dict(pick), + "place": dict(place), + "joint": dict(joint), + "joint_at_minimum": sum(n >= minimum for n in joint.values()), + "unique_geometry_buckets": len( + {r["geometry_bucket"] for r in rows if r["geometry_bucket"]} + ), + "geometry_missing": sum(r["geometry_bucket"] is None for r in rows), + } + + +def _read(paths: list[Path], entity: str) -> tuple[list[dict], list[str]]: + from .persistence import ArtifactEpisodeSink, load_expansion_episode + + from ._distribution_pose import RecordedTCP, orientation + + rows, domains = [], [] + for directory in dict.fromkeys(p.resolve() for p in paths): + if not (directory / "job.json").is_file(): + raise ValueError(f"Expected confirmed job directory: {directory}") + job = json.loads((directory / "job.json").read_text()) + deployment = job.get("resolved_deployment", {}) + tcp = RecordedTCP(deployment) + for receipt in ArtifactEpisodeSink(directory).inspect(): + if not receipt.confirmed: + continue + episode = load_expansion_episode(receipt.storage_id) + if not episode.validation.accepted: + raise ValueError("Confirmed episode lacks acceptance") + meta = episode.metadata + case = meta.get("scene_case") + task_id = case.get("task_id") if isinstance(case, Mapping) else None + source_uuid = meta.get("source_episode_uuid") + if isinstance(task_id, str) and task_id.strip(): + task_identity = ("task_id", task_id) + elif isinstance(source_uuid, str) and source_uuid.strip(): + task_identity = ("legacy_source_episode_uuid", source_uuid) + else: + raise ValueError( + f"Episode {episode.episode_id} lacks a recorded task identity" + ) + # Fixed topology, installation and task identity; sampled object poses are features. + robot = [ + dict( + uid=s["uid"], + joints=list(s["joint_names"]), + pose=_installation_pose(s["pose"]), + ) + for s in meta.get("initial_state", ()) + if s["kind"] == "robot" + ] + domain = _digest( + { + "robot": robot, + "task_identity": task_identity, + "instruction": meta.get("instruction"), + "backend": meta.get("physics_backend"), + "deployment_robot": deployment.get("robot"), + "scene_topology": [ + (v["uid"], v["kind"], v["body_type"]) + for v in meta.get("initial_state", ()) + ], + "scene_configuration": { + k: deployment.get(k) + for k in ("rigid_object", "background", "physics_config") + }, + } + ) + domains.append(domain) + times = episode.timestamps.cpu().numpy() + state = episode.observations.get(f"rigid/{entity}/state") + motion = ( + None if state is None else _motion(state.cpu().numpy()[:, :3], times) + ) + q = episode.observations.get("full_joint_positions") + joint_motion = None if q is None else _motion(q.cpu().numpy(), times) + descriptor = meta.get("coverage_descriptor") + bucket = None + if descriptor: + bucket = _digest( + [ + list(descriptor["phase_ids"]), + np.rint(np.asarray(descriptor["geometry"]) / 0.001) + .astype(int) + .tolist(), + ] + ) + tcp_poses = tcp.poses(episode) + rows.append( + { + "episode_id": episode.episode_id, + "commit_id": episode.commit_id, + "artifact": receipt.storage_id, + "job": str(directory), + "domain_id": domain, + "task_id": task_id, + "source_episode_uuid": source_uuid, + "initial_pose": None if state is None else state[0, :7].tolist(), + "final_pose": None if state is None else state[-1, :7].tolist(), + "orientation": ( + None + if state is None + else orientation(state.cpu().numpy(), times) + ), + "tcp_motion": ( + None if tcp_poses is None else _motion(tcp_poses[:, :3], times) + ), + "tcp_orientation": ( + None if tcp_poses is None else orientation(tcp_poses, times) + ), + "tcp_evidence": ( + {"unavailable": tcp.reason} if tcp.reason else tcp.evidence + ), + "object_motion": motion, + "joint_motion": joint_motion, + "geometry_bucket": bucket, + } + ) + return rows, sorted(set(domains)) + + +def _run( + before: list[Path], + after: list[Path], + output: Path, + entity: str, + size: float, + minimum: int, +) -> None: + if not np.isfinite(size) or size <= 0 or minimum < 1: + raise ValueError("Positive cell size and minimum count required") + if output.exists(): + raise FileExistsError("Use a new output directory") + b, bd = _read(before, entity) + a, ad = _read(after, entity) + if not b or not a or bd != ad or len(bd) != 1: + raise ValueError( + "Comparison requires nonempty snapshots of one matching domain" + ) + report = { + "schema_version": 2, + "feature_version": "se3-world-object-joint-tcp-v2", + "entity": entity, + "coordinate_frame": "world", + "cell_size_m": size, + "minimum": minimum, + "domain_id": bd[0], + "before": _summarize(b, size, minimum), + "after": _summarize(a, size, minimum), + } + report["comparison_kind"] = ( + "cumulative" + if {r["commit_id"] for r in b} <= {r["commit_id"] for r in a} + else "independent_cohorts" + ) + keys = sorted(set(report["before"]["joint"]) | set(report["after"]["joint"])) + report["comparison"] = [ + { + "cell": k, + "before": report["before"]["joint"].get(k, 0), + "after": report["after"]["joint"].get(k, 0), + "deficit": max(0, minimum - report["after"]["joint"].get(k, 0)), + } + for k in keys + ] + report["limitations"] = [ + "XY bins cover observed domain only; no feasible-volume denominator.", + "TCP is recorded-URDF FK from measured qpos/root plus explicit gripper TCP calibration.", + "Orientation bins: canonical SO(3) rotation vectors, 30-degree Cartesian bins, not equal-volume; no symmetry folding.", + "Raw terminal pose bins include tracking variation; receipt-confirmed fixed task categories are a separate coverage metric.", + "Installation grouping rounds measured XYZ and canonical normalized xyzw to 1e-6; declared calibration is exact and raw states/FK remain unrounded.", + "Joint path length mixes joint coordinates; compare only matching domains.", + "Geometry buckets reuse existing 1e-3 descriptor quantization, not clustering.", + ] + report["snapshot_id"] = _digest(report) + output.mkdir(parents=True) + # Detail pages are shared by the web viewer and Agent; summaries retain only an index. + pages = output / "pages" + pages.mkdir() + for name in ("before", "after"): + rows = report[name]["episodes"] + for start in range(0, len(rows), 100): + chunk = rows[start : start + 100] + payload = json.dumps(chunk, ensure_ascii=False, allow_nan=False).replace( + "<", "\\u003c" + ) + key = f"{name}-{start//100}" + (pages / f"{key}.json").write_text(payload) + (pages / f"{key}.js").write_text( + f"window.distributionPage({json.dumps(key)},{payload});" + ) + with (output / f"{name}-episodes.jsonl").open("w") as f: + for row in rows: + f.write(json.dumps(row, ensure_ascii=False, allow_nan=False) + "\n") + report[name]["episodes"] = [ + dict( + episode_id=row["episode_id"], + commit_id=row["commit_id"], + initial_pose=row["initial_pose"], + final_pose=row["final_pose"], + object_motion=row["object_motion"] is not None, + pick_cell=row.get("pick_cell"), + place_cell=row.get("place_cell"), + page=f"{name}-{i//100}", + ) + for i, row in enumerate(rows) + ] + (output / "summary.json").write_text( + json.dumps(report, ensure_ascii=False, indent=2, allow_nan=False) + ) + template = Path(__file__).with_name("_distribution.html").read_text() + (output / "report.html").write_text( + template.replace( + "__REPORT__", + json.dumps(report, ensure_ascii=False, allow_nan=False).replace( + "<", "\\u003c" + ), + ) + ) + + +def _main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--before", type=Path, nargs="+", required=True) + parser.add_argument("--after", type=Path, nargs="+", required=True) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--entity", default="cube") + parser.add_argument("--cell-size", type=float, default=0.1) + parser.add_argument("--minimum", type=int, default=5) + args = parser.parse_args() + _run( + args.before, args.after, args.output, args.entity, args.cell_size, args.minimum + ) + + +if __name__ == "__main__": + _main() diff --git a/embodichain/lab/expansion/_distribution_pose.py b/embodichain/lab/expansion/_distribution_pose.py new file mode 100644 index 0000000000..9558c4ff1b --- /dev/null +++ b/embodichain/lab/expansion/_distribution_pose.py @@ -0,0 +1,141 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Full orientation features and recorded-model TCP reconstruction.""" + +from __future__ import annotations +from pathlib import Path +import hashlib +import numpy as np +from scipy.spatial.transform import Rotation + +__all__: list[str] = [] + + +def orientation(poses: np.ndarray, times: np.ndarray) -> dict: + q = np.asarray(poses, dtype=float)[:, 3:7] + if ( + q.shape != (len(times), 4) + or not np.isfinite(q).all() + or np.any(np.linalg.norm(q, axis=1) < 1e-12) + ): + raise ValueError("Orientation requires finite nonzero xyzw quaternions") + rot = Rotation.from_quat(q) + canonical = rot.as_quat(canonical=True) + vectors = rot.as_rotvec() + angle = (rot[:-1].inv() * rot[1:]).magnitude() + dt = np.diff(times) + if np.any(dt <= 0): + raise ValueError("Increasing orientation times required") + return { + "initial_xyzw": canonical[0].tolist(), + "final_xyzw": canonical[-1].tolist(), + "initial_rotvec_rad": vectors[0].tolist(), + "final_rotvec_rad": vectors[-1].tolist(), + "angular_path_rad": float(angle.sum()), + "angular_speed_p50_p95": np.quantile(angle / dt, [0.5, 0.95]).tolist(), + "net_rotation_rad": float((rot[0].inv() * rot[-1]).magnitude()), + "rotation_bin_initial": ",".join( + map(str, np.floor((vectors[0] + np.pi) / (np.pi / 6)).astype(int)) + ), + "rotation_bin_final": ",".join( + map(str, np.floor((vectors[-1] + np.pi) / (np.pi / 6)).astype(int)) + ), + } + + +class RecordedTCP: + """Cache one recorded URDF chain; reconstruct in bounded 256-frame batches.""" + + def __init__(self, deployment: dict): + import pytorch_kinematics as pk + + robot = deployment.get("robot") or {} + bindings = list((robot.get("gripper_bindings") or {}).values()) + self.reason = None + if len(bindings) != 1: + self.reason = "single_explicit_gripper_binding_required" + return + binding = bindings[0] + self.link = binding.get("root_link") + transform = binding.get("defaults", {}).get("default_root_from_tcp") + asset = robot.get("fpath") + if not self.link or transform is None or not asset or not Path(asset).is_file(): + self.reason = "recorded_tcp_calibration_or_asset_missing" + return + self.offset = np.asarray(transform, dtype=float) + if ( + self.offset.shape != (4, 4) + or not np.isfinite(self.offset).all() + or not np.allclose(self.offset[3], [0, 0, 0, 1]) + or not np.allclose( + self.offset[:3, :3].T @ self.offset[:3, :3], np.eye(3), atol=1e-6 + ) + or not np.isclose(np.linalg.det(self.offset[:3, :3]), 1) + ): + raise ValueError("Invalid TCP transform") + if robot.get("body_scale") not in (None, [1.0, 1.0, 1.0]): + self.reason = "scaled_robot_not_supported" + return + content = Path(asset).read_bytes() + self.chain = pk.build_chain_from_urdf(content) + self.names = self.chain.get_joint_parameter_names() + self.evidence = { + "method": "fk_from_measured_qpos_and_root", + "root_link": self.link, + "root_from_tcp": self.offset.tolist(), + "urdf_sha256": hashlib.sha256(content).hexdigest(), + } + + def poses(self, episode) -> np.ndarray | None: + import torch + + if self.reason: + return None + robots = [ + s for s in episode.metadata.get("initial_state", ()) if s["kind"] == "robot" + ] + if len(robots) != 1: + raise ValueError("TCP requires one recorded robot") + robot = robots[0] + fields = { + s["field"]: s["key"] + for s in episode.metadata["scene_state_fields"] + if s["entity_uid"] == robot["uid"] + } + q = episode.observations[fields["qpos"]].cpu() + roots = episode.observations[fields["pose"]].cpu().numpy() + indices = [list(robot["joint_names"]).index(n) for n in self.names] + out = [] + with torch.no_grad(): + for start in range(0, len(q), 256): + local = ( + self.chain.forward_kinematics( + q[start : start + 256, indices].float() + )[self.link] + .get_matrix() + .numpy() + @ self.offset + ) + rp = roots[start : start + 256] + world = np.broadcast_to(np.eye(4), local.shape).copy() + world[:, :3, :3] = Rotation.from_quat(rp[:, 3:]).as_matrix() + world[:, :3, 3] = rp[:, :3] + tcp = world @ local + out.append( + np.c_[tcp[:, :3, 3], Rotation.from_matrix(tcp[:, :3, :3]).as_quat()] + ) + return np.concatenate(out) diff --git a/embodichain/lab/expansion/_gym_context.py b/embodichain/lab/expansion/_gym_context.py index 1bfd210f86..70ce0d8b48 100644 --- a/embodichain/lab/expansion/_gym_context.py +++ b/embodichain/lab/expansion/_gym_context.py @@ -27,6 +27,7 @@ from .journal import ExpansionJournal, RecipeAttempt from .recipes import BatchCompatibility, OperatorCapability, ExpansionRecipe from .contracts import CandidateIdentity, ValidationResult +from .host_extensions import GymExpansionHostExtensions if TYPE_CHECKING: from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv, EmbodiedEnvCfg @@ -71,6 +72,7 @@ class GymHostContext: auxiliary_baseline: Mapping[str, torch.Tensor] | None = None profile: Real2SimTabletopCollectionCfg | None = None scene_source: JointSceneSource | None = None + feedback: Any | None = None motion_profile: CombinedExpansionProfile | None = None motion_recipes: tuple[CandidateRecipe, ...] = () names: tuple[str, ...] = () @@ -90,3 +92,6 @@ class GymHostContext: services: dict[int, _EpisodeSource] = field(default_factory=dict) bridges: dict[int, TaskProgramDemoBridge] = field(default_factory=dict) recipe: Callable[[int], ExpansionRecipe] | None = None + extensions: GymExpansionHostExtensions = field( + default_factory=GymExpansionHostExtensions + ) diff --git a/embodichain/lab/expansion/_gym_episode.py b/embodichain/lab/expansion/_gym_episode.py index 7d504163a7..274be4bbdd 100644 --- a/embodichain/lab/expansion/_gym_episode.py +++ b/embodichain/lab/expansion/_gym_episode.py @@ -37,6 +37,29 @@ __all__: list[str] = [] +def _save_partial(numeric, path: Path, step_dt: float) -> dict: + """Publish actual T+1 buffers for rejected or administratively cancelled work.""" + arrays = { + "qpos": torch.stack(numeric["qpos"]).numpy(), + "timestamps": np.arange(len(numeric["qpos"])) * step_dt, + } + if numeric["actions"]: + arrays["actions"] = torch.stack(numeric["actions"]).numpy() + arrays.update( + { + "object_" + str(index): torch.stack(values).numpy() + for index, values in enumerate(numeric["objects"].values()) + } + ) + with path.open("wb") as stream: + np.savez_compressed(stream, **arrays) + return { + "path": path.name, + "sha256": hashlib.sha256(path.read_bytes()).hexdigest(), + "object_uids": list(numeric["objects"]), + } + + class GymEpisodeAssembler: """Convert frozen cohort buffers into canonical accepted or rejected evidence.""" @@ -73,27 +96,9 @@ def assemble(self, assignments, collector, services, result, physics_after): record["source_execution_reports"] = dict( services[row].tracer.execution_reports ) - partial = records[row] - path = directory / "rejected.npz" - arrays = { - "qpos": torch.stack(partial["qpos"]).numpy(), - "timestamps": np.arange(len(partial["qpos"])) * env.step_dt, - } - if partial["actions"]: - arrays["actions"] = torch.stack(partial["actions"]).numpy() - arrays.update( - { - "object_" + str(index): torch.stack(values).numpy() - for index, values in enumerate(partial["objects"].values()) - } + record["partial_rollout"] = _save_partial( + records[row], directory / "rejected.npz", env.step_dt ) - with path.open("wb") as stream: - np.savez_compressed(stream, **arrays) - record["partial_rollout"] = { - "path": path.name, - "sha256": hashlib.sha256(path.read_bytes()).hexdigest(), - "object_uids": list(partial["objects"]), - } _atomic_json(directory / "attempt.json", record) episodes[row] = None continue @@ -120,7 +125,13 @@ def assemble(self, assignments, collector, services, result, physics_after): key = f"scene_state_{index}" observations[key] = torch.stack(values) scene_keys.append({"key": key, "entity_uid": uid, "field": field}) + from embodichain.utils._owned_json import freeze_metadata + + # Both compatibility fields describe the same immutable audit tree. + # Validate/own it once; mutable public projections still detach. + segments = freeze_metadata(result.segments[result_row]) metadata = { + "source_env_id": row, "attempt_sequence": ctx.requests[row].sequence, "job_instance_id": ctx.journal.job_instance_id, "recipe_id": ctx.requests[row].recipe.recipe_id, @@ -145,13 +156,13 @@ def assemble(self, assignments, collector, services, result, physics_after): "initial_state": record["initial_state"], "scene_state_fields": scene_keys, "physics_backend": env.sim.physics_backend, - "program_segments": result.segments[result_row], + "program_segments": segments, "expansion": ( result.expansion_records[result_row] if result.expansion_records else () ), - "segments": result.segments[result_row], + "segments": segments, "instruction": program.instruction, "instruction_source": "task_program", } diff --git a/embodichain/lab/expansion/_gym_execution.py b/embodichain/lab/expansion/_gym_execution.py index 8c45786af6..5ced90c201 100644 --- a/embodichain/lab/expansion/_gym_execution.py +++ b/embodichain/lab/expansion/_gym_execution.py @@ -18,10 +18,13 @@ from __future__ import annotations from collections.abc import Callable +from dataclasses import replace import time from embodichain.data_pipeline.recording.media import _atomic_json from embodichain.lab.expansion.program import TaskProgramBatchExecutor from .contracts import CandidateIdentity, ExpertEpisode +from .contracts import SceneCase +from .host_extensions import ExpansionRowContext from embodichain.lab.sim.scene.physics import capture_physics_properties from .engine import ExpansionAssignment @@ -65,6 +68,14 @@ def execute_batch( for assignment in assigned } bridges, services = {}, {} + factories, adapters = {}, [] + extension = ctx.extensions + if extension.plan_transform_factory is not None: + ctx.source_artifacts = { + key: value + for key, value in ctx.source_artifacts.items() + if not key.startswith("plan_") + } ctx.bridges, ctx.services = bridges, services phase_started = time.monotonic() prototype_profile = None @@ -84,6 +95,44 @@ def execute_batch( record, ) in assignments.items(): ordinal = ctx.requests[row].recipe.context.ordinal + if extension.plan_transform_factory is not None: + if ( + profile is not None + or env.task_program_adapter.configured_nominal_sources + or motion_profile is not None + ): + raise ValueError( + "an extension transform cannot replace a configured nominal/motion source" + ) + request = ctx.requests[row] + factory = extension.plan_transform_factory( + env, + ExpansionRowContext( + row, + identity, + ordinal, + request.attempt_id, + directory, + SceneCase(**record["case"]), + request.recipe, + ), + ) + if factory is None: + raise ValueError("plan_transform_factory returned no factory") + factories[row] = factory + adapter = ( + env.task_program_adapter + if cfg.batch_size == 1 + else env.task_program_adapter.for_env_ids((row,)) + ) + if cfg.batch_size != 1: + adapters.append(adapter) + bridges[row] = adapter.create_bridge( + program, + plan_transform_factory=factory, + isolate_policy_sessions=True, + ) + continue if profile is None: from embodichain.lab.task_program.integrations.demo_skill import ( _ConfiguredDemoSkillSource, @@ -168,9 +217,43 @@ def mark_started(row): ), ) timings["execute"] += time.monotonic() - phase_started + if factories: + expansions, accepted, reasons = ( + [], + list(result.accepted), + list(result.reasons), + ) + for index, row in enumerate(result.env_ids): + factory = factories[row] + records = tuple(factory.records) + assignments[row][4]["expansion"] = [ + record.to_metadata() for record in records + ] + portable = [] + for number, record in enumerate(records): + trace = record.to_metadata() + if "plan_artifact" in trace: + key = f"plan_{row}_{number}" + ctx.source_artifacts[key] = trace.pop("plan_artifact") + trace["plan_artifact_key"] = key + portable.append(trace) + expansions.append(tuple(portable)) + if extension.require_plan_transform and accepted[index] and not records: + accepted[index], reasons[index] = ( + False, + "requested_operator_not_applied", + ) + result = replace( + result, + expansion_records=tuple(expansions), + accepted=tuple(accepted), + reasons=tuple(reasons), + ) physics_after = capture_physics_properties(view) episodes = GymEpisodeAssembler(ctx).assemble( assignments, collector, services, result, physics_after ) ctx.services, ctx.bridges = services, bridges + for adapter in adapters: + adapter.close() return tuple(episodes[assignment.env_id] for assignment in assigned) diff --git a/embodichain/lab/expansion/_gym_frames.py b/embodichain/lab/expansion/_gym_frames.py index aee59d96bd..26ee4357bc 100644 --- a/embodichain/lab/expansion/_gym_frames.py +++ b/embodichain/lab/expansion/_gym_frames.py @@ -98,6 +98,19 @@ def __init__(self, context: GymHostContext, assignments: dict) -> None: spools[row] = spool self.records, self.spools = records, spools + def add(self, assignments: dict) -> None: + """Capture newly settled rows without replacing live measured buffers.""" + if set(assignments) & self.records.keys(): + raise ValueError("cannot replace a live measured row") + initial = GymFrameCollector(self.context, assignments) + self.records.update(initial.records) + self.spools.update(initial.spools) + + def discard(self, row: int) -> None: + """Release a row after evidence assembly and persistence confirmation.""" + self.records.pop(row) + self.spools.pop(row, None) + def observe(self, ids, action, observation) -> None: ctx = self.context cfg, env, view, collision, timings = ( @@ -147,30 +160,26 @@ def observe(self, ids, action, observation) -> None: timings["sampled_validation"] = ( timings.get("sampled_validation", 0.0) + time.monotonic() - stamp ) - for row in ids: - record = records[row] - value = ( - {"qpos": action.value} - if isinstance(action.value, torch.Tensor) - else action.value - ) + value = ( + {"qpos": action.value} + if isinstance(action.value, torch.Tensor) + else action.value + ) - def active_command(name): - data = value[name][row] - if data.shape[-1] == env.robot.dof: - data = data[list(env.active_joint_ids)] - elif data.shape[-1] != len(env.active_joint_ids): - raise ValueError("submitted command joint layout differs") - return data.detach().cpu() + def active_command(name): + data = value[name] + if data.shape[-1] == env.robot.dof: + data = data[:, list(env.active_joint_ids)] + elif data.shape[-1] != len(env.active_joint_ids): + raise ValueError("submitted command joint layout differs") + return data - command = active_command("qpos") - if env.expert_action_spec.joint_command_mode == "position_velocity": - command = torch.cat( - ( - command, - active_command("qvel"), - ) - ) + commands = active_command("qpos") + if env.expert_action_spec.joint_command_mode == "position_velocity": + commands = torch.cat((commands, active_command("qvel")), dim=-1) + commands = commands.detach().cpu() + for row in ids: + record = records[row] entity_states = {entity.uid: entity for entity in frame_state.batches[row]} for (uid, field), values in record["scene"].items(): values.append( @@ -179,7 +188,7 @@ def active_command(name): dtype=torch.float32, ) ) - record["actions"].append(command.clone()) + record["actions"].append(commands[row].clone()) record["qpos"].append(measured_qpos[row].clone()) record["qvel"].append(measured_qvel[row].clone()) record["qf"].append(measured_qf[row].clone()) diff --git a/embodichain/lab/expansion/_motion_batches.py b/embodichain/lab/expansion/_motion_batches.py new file mode 100644 index 0000000000..696756d6c2 --- /dev/null +++ b/embodichain/lab/expansion/_motion_batches.py @@ -0,0 +1,163 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + + +"""Fuse compatible fixed-grid native motion requests before scalar transforms.""" + +from __future__ import annotations + +from copy import copy +from dataclasses import fields, is_dataclass, replace +from enum import Enum +from functools import wraps + +__all__: list[str] = [] + + +class MotionBatches: + """Keep selected physical rows and native fixed-grid rejection semantics.""" + + def __init__(self, broker): + from greenlet import getcurrent + from embodichain.lab.sim.motion.motion_generator import MotionGenerator + + self.broker = broker + self.original = original = MotionGenerator.generate + + def direct(generator, targets, options, defer_recovery): + previous = broker.motion_passthrough + broker.motion_passthrough = True + try: + return original( + generator, targets, options, defer_recovery=defer_recovery + ) + finally: + broker.motion_passthrough = previous + + @wraps(original) + def generate(generator, target_states, options=None, *, defer_recovery=False): + from embodichain.lab.sim.motion._budget import _ACTIVE_BUDGET + + task = getcurrent() + if ( + task not in broker.tasks + or options is None + or options.strategy != "ik_interp" + or options.interpolation_mode != "fixed_grid" + or options.segment_sample_count_retries + or options.segment_continuous_ik_recovery + or options.cartesian_path_sampler is not None + or generator.collision_world_info is not None + or generator._env_ids is None + or _ACTIVE_BUDGET.get() is not None + ): + return direct(generator, target_states, options, defer_recovery) + tensors = {} + try: + structure = self._describe( + (target_states, options), tensors, len(generator._env_ids) + ) + except TypeError: + return direct(generator, target_states, options, defer_recovery) + tensors["rows"] = broker.torch.tensor( + generator._env_ids, dtype=broker.torch.long + ) + mode = broker._mode() + key = ( + "motion", + id(generator.robot), + type(generator.planner), + repr(generator.planner.cfg), + structure, + defer_recovery, + mode, + ) + + def invoke(merged): + targets, settings = self._restore((target_states, options), merged) + batched = copy(generator) + batched.planner = copy(generator.planner) + batched._env_ids = tuple(merged["rows"].tolist()) + batched.planner._planning_env_ids = batched._env_ids + return original( + batched, targets, settings, defer_recovery=defer_recovery + ) + + request = (key, invoke, None, None, tensors, len(generator._env_ids), mode) + broker.tasks[task] = broker._rng() + broker._restore_rng(broker.owner_rng) + return broker.owner.switch(request) + + self.generate = generate + + def _describe(self, value, tensors, count, path=""): + torch = self.broker.torch + if isinstance(value, torch.Tensor): + if value.ndim < 1 or value.shape[0] != count: + raise TypeError("Motion tensors must retain the selected batch") + tensors[path] = value + return ("tensor", tuple(value.shape[1:]), value.dtype, value.device) + if is_dataclass(value): + return ( + type(value), + tuple( + ( + f.name, + self._describe( + getattr(value, f.name), tensors, count, path + "/" + f.name + ), + ) + for f in fields(value) + ), + ) + if isinstance(value, (tuple, list)): + return ( + type(value), + tuple( + self._describe(v, tensors, count, path + "/" + str(i)) + for i, v in enumerate(value) + ), + ) + if ( + value is None + or type(value) in (bool, int, float, str) + or isinstance(value, Enum) + ): + return (type(value), value) + raise TypeError("Unsupported mutable motion option") + + def _restore(self, value, tensors, path=""): + if isinstance(value, self.broker.torch.Tensor): + return tensors[path] + if is_dataclass(value): + return replace( + value, + **{ + f.name: self._restore( + getattr(value, f.name), tensors, path + "/" + f.name + ) + for f in fields(value) + }, + ) + if isinstance(value, (tuple, list)): + return type(value)( + self._restore(v, tensors, path + "/" + str(i)) + for i, v in enumerate(value) + ) + return value + + def close(self): + return None diff --git a/embodichain/lab/expansion/_planning_batches.py b/embodichain/lab/expansion/_planning_batches.py new file mode 100644 index 0000000000..87df329d23 --- /dev/null +++ b/embodichain/lab/expansion/_planning_batches.py @@ -0,0 +1,388 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + + +"""Cooperatively batch native kinematics on the sole World-owning thread.""" + +from __future__ import annotations + +from collections import defaultdict +from contextlib import contextmanager +from functools import wraps +from inspect import signature +from dataclasses import fields, is_dataclass, replace +from contextvars import ContextVar, copy_context + +__all__: list[str] = [] + +_ACTIVE: ContextVar[PlanningBatches | None] = ContextVar( + "expansion_planning", default=None +) + + +def _try_motion(generator, targets, options, defer_recovery): + active = _ACTIVE.get() + if active is None or active.motion_passthrough: + return False, None + from greenlet import getcurrent + + if getcurrent() not in active.tasks: + return False, None + return True, active.motions.generate( + generator, targets, options, defer_recovery=defer_recovery + ) + + +class PlanningBatches: + """Suspend scalar planners only at pure solver calls, then fuse their rows. + + Native Robot budget decorators, plan transforms and validation run inside + each original cursor. No World call runs on a worker thread. Results are + cloned into independent rows before any scalar planner resumes. + """ + + def __init__(self, robot, seed): + import numpy as np + import random + import torch + from greenlet import getcurrent + + self.torch, self.np, self.random = torch, np, random + self.owner = getcurrent() + self.tasks = {} + self.saved = [] + self.solo = False + self.direct_depth = 0 + self.motion_passthrough = False + self.devices = ( + [ + ( + robot.device.index + if robot.device.index is not None + else torch.cuda.current_device() + ) + ] + if robot.device.type == "cuda" + else [] + ) + self.stats = { + "solver_calls": 0, + "logical_calls": 0, + "merged_calls": 0, + "max_group": 0, + "motion_calls": 0, + "logical_motion_calls": 0, + "merged_motion_calls": 0, + "max_motion_group": 0, + } + previous = self._rng() + torch.random.default_generator.manual_seed(seed) + for device in self.devices: + with torch.cuda.device(device): + torch.cuda.manual_seed(seed) + np.random.seed(seed % 2**32) + random.seed(seed) + self.solve_rng = self._rng() + self._restore_rng(previous) + for solver in dict.fromkeys(robot._solvers.values()): + names = ("get_fk", "get_ik", "get_jacobian") + if getattr(solver, "supports_sequential_batch_ik", False): + names += ("_get_continuous_ik_path",) + for name in names: + self._bind(solver, name) + from ._motion_batches import MotionBatches + + self.motions = MotionBatches(self) + + def _rng(self): + return ( + self.torch.get_rng_state(), + tuple(self.torch.cuda.get_rng_state(d) for d in self.devices), + self.np.random.get_state(), + self.random.getstate(), + self.torch.is_grad_enabled(), + self.torch.is_inference_mode_enabled(), + self._autocast_state(), + ) + + def _restore_rng(self, state): + self.torch.set_rng_state(state[0]) + for device, value in zip(self.devices, state[1]): + self.torch.cuda.set_rng_state(value, device) + self.np.random.set_state(state[2]) + self.random.setstate(state[3]) + self._restore_autocast(state[6]) + + def _autocast_state(self): + return ( + tuple( + ( + device, + self.torch.is_autocast_enabled(device), + self.torch.get_autocast_dtype(device), + ) + for device in ("cpu", "cuda") + ), + self.torch.is_autocast_cache_enabled(), + ) + + def _restore_autocast(self, state): + if state == self._autocast_state(): + return + # Greenlets share thread-local autocast state and its weight cache. + # A cache populated in another dtype must not cross a row boundary. + self.torch.clear_autocast_cache() + for device, enabled, dtype in state[0]: + self.torch.set_autocast_enabled(device, enabled) + self.torch.set_autocast_dtype(device, dtype) + self.torch.set_autocast_cache_enabled(state[1]) + + def _mode(self): + return ( + self.torch.is_grad_enabled(), + self.torch.is_inference_mode_enabled(), + self._autocast_state(), + ) + + @contextmanager + def _execution_mode(self, mode): + previous = self._autocast_state() + self._restore_autocast(mode[2]) + try: + with ( + self.torch.inference_mode(mode[1]), + self.torch.set_grad_enabled(mode[0]), + ): + yield + finally: + self._restore_autocast(previous) + + def _bind(self, solver, name): + from greenlet import getcurrent + + original = getattr(solver, name, None) + if original is None: + return + spec = signature(original) + missing = object() + before = solver.__dict__.get(name, missing) + + @wraps(original) + def query(*args, **kwargs): + task = getcurrent() + if task not in self.tasks: + counted = self.solo and self.direct_depth == 0 + if counted: + self.stats["solver_calls"] += 1 + self.stats["logical_calls"] += 1 + self.stats["max_group"] = max(self.stats["max_group"], 1) + self.direct_depth += 1 + try: + return original(*args, **kwargs) + finally: + self.direct_depth -= 1 + values = spec.bind(*args, **kwargs) + values.apply_defaults() + tensors, constants, count = {}, {}, None + for key, value in values.arguments.items(): + if isinstance(value, self.torch.Tensor): + if value.ndim < 2 or ( + count is not None and value.shape[0] != count + ): + return original(*args, **kwargs) + count = int(value.shape[0]) + tensors[key] = value + elif value is None or type(value) in (bool, int, float, str): + constants[key] = value + elif type(value) is dict and not value: + constants[key] = value + else: + return original(*args, **kwargs) + if count is None: + return original(*args, **kwargs) + mode = self._mode() + key = ( + id(solver), + name, + mode, + tuple( + (key, tuple(value.shape[1:]), value.dtype, value.device) + for key, value in tensors.items() + ), + tuple((key, repr(value)) for key, value in constants.items()), + ) + request = (key, original, spec, values, tensors, count, mode) + self.tasks[task] = self._rng() + self._restore_rng(self.owner_rng) + return self.owner.switch(request) + + setattr(solver, name, query) + self.saved.append((solver, name, before, missing)) + + def _invoke(self, request, tensors): + _, original, spec, values, _, _, mode = request + if request[0][0] == "motion": + with self._execution_mode(mode): + return original(tensors) + arguments = dict(values.arguments) + arguments.update(tensors) + # BoundArguments retains variadic **kwargs and keyword-only arguments. + bound = spec.bind_partial() + bound.arguments.update(arguments) + with self._execution_mode(mode): + return original(*bound.args, **bound.kwargs) + + def _part(self, result, start, stop, total=None): + if isinstance(result, self.torch.Tensor): + if result.ndim and (total is None or result.shape[0] == total): + return result[start:stop].clone() + return result.clone() + if isinstance(result, tuple): + return tuple(self._part(value, start, stop, total) for value in result) + if isinstance(result, dict): + return { + key: self._part(value, start, stop, total) + for key, value in result.items() + } + if is_dataclass(result): + return replace( + result, + **{ + f.name: self._part(getattr(result, f.name), start, stop, total) + for f in fields(result) + }, + ) + if result is None or type(result) in (bool, int, float, str): + return result + raise TypeError("Batched native solver returned an unsupported result") + + def gather(self, cursors): + """Advance every native cursor once, merging compatible pending solves.""" + from greenlet import greenlet + + if len(cursors) == 1: + row, cursor = next(iter(cursors.items())) + self.solo = True + try: + action = cursor.next() + return {} if action is None else {row: action} + finally: + self.solo = False + self.owner_rng = self._rng() + ready, waiting = {}, {} + + def advance(task, row, value=None, first=False): + state = self.owner_rng if first else self.tasks[task] + self._restore_rng(state) + try: + with ( + self.torch.inference_mode(state[5]), + self.torch.set_grad_enabled(state[4]), + ): + result = task.switch() if first else task.switch(value) + finally: + self._restore_rng(self.owner_rng) + if task.dead: + if result is not None: + ready[row] = result + else: + waiting[task] = (row, result) + + try: + for row, cursor in cursors.items(): + task = greenlet(cursor.next, parent=self.owner) + task.gr_context = copy_context() + self.tasks[task] = self.owner_rng + advance(task, row, first=True) + while waiting: + groups = defaultdict(list) + for task, (row, request) in waiting.items(): + groups[request[0]].append((task, row, request)) + waiting = {} + for group in groups.values(): + first = group[0][2] + tensors = { + key: self.torch.cat( + [entry[2][4][key] for entry in group], dim=0 + ) + for key in first[4] + } + self._restore_rng(self.solve_rng) + try: + result = self._invoke(first, tensors) + finally: + self.solve_rng = self._rng() + self._restore_rng(self.owner_rng) + if first[0][0] == "motion": + self.stats["motion_calls"] += 1 + self.stats["logical_motion_calls"] += len(group) + self.stats["merged_motion_calls"] += len(group) > 1 + self.stats["max_motion_group"] = max( + self.stats["max_motion_group"], len(group) + ) + else: + self.stats["solver_calls"] += 1 + self.stats["logical_calls"] += len(group) + self.stats["merged_calls"] += len(group) > 1 + self.stats["max_group"] = max( + self.stats["max_group"], len(group) + ) + start, parts = 0, [] + total = sum(entry[2][5] for entry in group) + for task, row, request in group: + stop = start + request[5] + parts.append(self._part(result, start, stop, total)) + start = stop + for (task, row, _), part in zip(group, parts): + advance(task, row, part) + return ready + finally: + for task in tuple(self.tasks): + if not task.dead: + self._restore_rng(self.tasks[task]) + try: + state = self.tasks[task] + with ( + self.torch.inference_mode(state[5]), + self.torch.set_grad_enabled(state[4]), + ): + task.throw() + finally: + self._restore_rng(self.owner_rng) + self.tasks.clear() + self._restore_rng(self.owner_rng) + + def close(self): + self.motions.close() + for solver, name, value, missing in reversed(self.saved): + if value is missing: + delattr(solver, name) + else: + setattr(solver, name, value) + self.saved.clear() + + +@contextmanager +def batched_planning(robot, seed): + batches = PlanningBatches(robot, seed) + token = _ACTIVE.set(batches) + try: + yield batches + finally: + try: + batches.close() + finally: + _ACTIVE.reset(token) diff --git a/embodichain/lab/expansion/_pose_cells.py b/embodichain/lab/expansion/_pose_cells.py new file mode 100644 index 0000000000..c62014829c --- /dev/null +++ b/embodichain/lab/expansion/_pose_cells.py @@ -0,0 +1,120 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Pure XY/yaw grid validation and symmetric pose quantization.""" + +from __future__ import annotations +import math +from collections.abc import Mapping +import numpy as np + +__all__: list[str] = [] + + +def _measurements(value: Mapping, grids: Mapping) -> dict: + from copy import deepcopy + + if not isinstance(value, Mapping) or not value or set(value) != set(grids): + raise ValueError("Named coverage measurements require matching grids") + for name, declaration in value.items(): + if ( + not isinstance(name, str) + or not name + or not isinstance(declaration, Mapping) + or set(declaration) - {"entity", "boundary", "segment", "fixed"} + or not {"entity", "boundary"} <= declaration.keys() + or not isinstance(declaration["entity"], str) + or not declaration["entity"] + or declaration["boundary"] not in ("initial", "terminal", "segment_end") + ): + raise ValueError("Invalid named coverage measurement") + if (declaration["boundary"] == "segment_end") != ("segment" in declaration) or ( + "segment" in declaration + and ( + not isinstance(declaration["segment"], str) + or not declaration["segment"] + ) + ): + raise ValueError("Segment coverage requires an explicit segment name") + if "fixed" in declaration and type(declaration["fixed"]) is not bool: + raise ValueError("Coverage fixed flag must be boolean") + return deepcopy(dict(value)) + + +def _grid(value: Mapping) -> dict: + if not isinstance(value, Mapping) or set(value) != { + "lower", + "upper", + "cell_size", + "yaw_period", + }: + raise ValueError("Pose grids require lower, upper, cell_size and yaw_period") + lower, upper, size = ( + np.asarray(value[k], dtype=float) for k in ("lower", "upper", "cell_size") + ) + period = value["yaw_period"] + if any(a.shape != (3,) or not np.isfinite(a).all() for a in (lower, upper, size)): + raise ValueError("Pose grid coordinates must be three finite numbers") + if type(period) not in (int, float) or not math.isfinite(period) or period <= 0: + raise ValueError("yaw_period must be finite and positive") + if period > 2 * math.pi or not math.isclose( + 2 * math.pi / period, round(2 * math.pi / period), rel_tol=0, abs_tol=1e-8 + ): + raise ValueError("yaw_period must divide a complete turn") + if ( + np.any(upper <= lower) + or np.any(size <= 0) + or not math.isclose(upper[2] - lower[2], period) + ): + raise ValueError("Pose grids need positive extents and one complete yaw period") + bins = (upper - lower) / size + if not np.allclose(bins, np.round(bins), rtol=0, atol=1e-8): + raise ValueError("Pose grid extents must be divisible by cell_size") + return { + "lower": lower.tolist(), + "upper": upper.tolist(), + "cell_size": size.tolist(), + "yaw_period": float(period), + } + + +def _pose_cell(pose, grid: Mapping) -> tuple[int, int, int] | None: + value = np.asarray(pose, dtype=float) + if value.shape == (4, 4): + x, y = value[:2, 3] + yaw = math.atan2(value[1, 0], value[0, 0]) + elif value.ndim == 1 and value.shape[0] >= 7: + x, y = value[:2] + q = value[3:7] + norm = np.linalg.norm(q) + if not math.isfinite(norm) or norm < 1e-12: + raise ValueError("Pose quaternion must be finite and nonzero") + qx, qy, qz, qw = q / norm + yaw = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)) + else: + raise ValueError("Expected a pose matrix or xyz/xyzw state") + if not np.isfinite(value).all(): + raise ValueError("Pose must be finite") + lo, hi, size = (np.asarray(grid[k]) for k in ("lower", "upper", "cell_size")) + point = np.array([x, y, (yaw - lo[2]) % grid["yaw_period"] + lo[2]]) + if np.any(point[:2] < lo[:2] - 1e-7) or np.any(point[:2] > hi[:2] + 1e-7): + return None + bins = np.rint((hi - lo) / size).astype(int) + return tuple( + np.minimum( + bins - 1, np.maximum(0, np.floor((point - lo) / size).astype(int)) + ).tolist() + ) diff --git a/embodichain/lab/expansion/_pose_sampling.py b/embodichain/lab/expansion/_pose_sampling.py new file mode 100644 index 0000000000..963f07e61e --- /dev/null +++ b/embodichain/lab/expansion/_pose_sampling.py @@ -0,0 +1,755 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Selectable bounded and workspace-guided scene proposals with owned feedback.""" + +from __future__ import annotations +from collections.abc import Mapping +from collections import Counter +from copy import deepcopy +from pathlib import Path +from typing import Any +import hashlib +import json +import math +import numpy as np +import torch + +__all__: list[str] = [] + + +class _BoundedPoseSource: + """Sample bounded arena-frame changes with task-declared support followers.""" + + def __init__( + self, env: Any, configuration: Mapping[str, Any], *, seed: int, revision: str + ) -> None: + self.configuration, self.seed, self.request_id = ( + {uid: deepcopy(dict(options)) for uid, options in configuration.items()}, + seed, + revision, + ) + self.poses = { + uid: env.sim.get_rigid_object(uid) + .get_local_pose(to_matrix=True)[0] + .detach() + .cpu() + .numpy() + .copy() + for uid in configuration + } + self.roots = { + env.robot.uid: env.robot.get_local_pose(to_matrix=True)[0] + .detach() + .cpu() + .tolist() + } + + def candidate(self, ordinal: int) -> JointSceneCandidate: + import numpy as np + from embodichain.lab.sim.scene.expansion import ScenePoseChange, SceneVariant + from embodichain.lab.sim.scene.feasibility import ( + JointSceneCandidate, + RobotDeploymentCandidate, + ) + + offsets = {} + for uid, options in self.configuration.items(): + if "follow" in options: + continue + seed = int.from_bytes( + hashlib.sha256(f"{self.seed}/{ordinal}/{uid}".encode()).digest()[:8], + "little", + ) + generator = np.random.default_rng(seed) + offsets[uid] = [ + generator.uniform(*bounds) + for bounds in (*options["translation_m"], options["yaw_rad"]) + ] + changes, parameters = [], [] + for uid, options in self.configuration.items(): + dx, dy, dz, yaw = offsets[options.get("follow", uid)] + pose = self.poses[uid].copy() + c, s = np.cos(yaw), np.sin(yaw) + pose[:3, :3] = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) @ pose[:3, :3] + pose[:3, 3] += (dx, dy, dz) + changes.append(ScenePoseChange(uid, tuple(map(tuple, pose)))) + parameters.extend((dx, dy, dz, yaw)) + scene = SceneVariant(self.request_id, self.seed, ordinal, tuple(changes)) + return JointSceneCandidate( + self.request_id, + scene, + RobotDeploymentCandidate(self.roots), + "object_pose", + tuple(parameters), + ) + + +from embodichain.lab.sim.scene.feasibility.budget import ( + FeasibilityBudget, + FeasibilityBudgetExhausted, +) + + +class _PoseBudget(FeasibilityBudget): + """Use whole-job limits, including raw proposals discarded by selection.""" + + def charge(self, kind: str, amount: int = 1) -> None: + self.check() + self._reserve(kind, amount) + + def _reserve(self, kind: str, amount: int = 1) -> None: + if type(amount) is not int or amount < 1: + raise ValueError("Budget charges must be positive integers") + limits = { + "candidates": self.cfg.max_candidates, + "ik": self.cfg.max_ik_calls, + "planning": self.cfg.max_planning_calls, + "rollouts": self.cfg.max_rollouts, + } + if self.counts[kind] + amount > limits[kind]: + self._blocked = (self.validation, kind + "_budget_exhausted") + raise FeasibilityBudgetExhausted(self._blocked[1]) + self.counts[kind] += amount + + +class _AdmittedPoseBudget: + """Retain shared call limits/cancellation after the admission deadline. + + Already admitted trajectories still have the host's original max_steps and + bridge safe-stop rules. The source's raw budget alone admits new work. + """ + + def __init__(self, budget: _PoseBudget) -> None: + self.budget = budget + + def check(self) -> None: + budget = self.budget + if budget._blocked is not None and budget._blocked[0] == budget.validation: + raise FeasibilityBudgetExhausted(budget._blocked[1]) + if budget.cancelled is not None and budget.cancelled.is_set(): + raise FeasibilityBudgetExhausted("cancelled") + + def charge(self, kind: str, amount: int = 1) -> None: + self.check() + self.budget._reserve(kind, amount) + + +class _AdaptivePoseSource(_BoundedPoseSource): + """One bounded proposal stream; candidates never change after allocation.""" + + def __init__(self, env, options, *, seed, revision): + super().__init__(env, options["scene"], seed=seed, revision=revision) + from .task_pose_coverage import TaskPoseCoverage + from ._gym_utils import _hash + from embodichain.lab.sim.scene.feasibility import ( + FeasibilityBudget, + SceneRobotFeasibilityCfg, + ) + + self.env, self.options = env, deepcopy(options) + self.cfg, self.schedule = ( + self.options["scene_sampling"], + self.options["scheduling"], + ) + if self.cfg["control_part"] not in env.robot.control_parts: + raise ValueError("scene_sampling.control_part is absent from the robot") + self.coverage_cfg = self.options.get("pose_coverage") + invariant = { + "robot": env.robot.cfg.to_dict(), + "physics": env.sim.sim_config.physics_cfg.to_dict(), + "poses": self.poses, + "roots": self.roots, + "domain": self.configuration, + "coverage": self.coverage_cfg, + "validation": options["validation"], + "objects": { + uid: obj.cfg.to_dict() + for uid in getattr(env.sim, "get_rigid_object_uid_list", lambda: ())() + if (obj := env.sim.get_rigid_object(uid)) is not None + }, + } + # _hash handles JSON data only; pose arrays are owned host arrays here. + invariant.pop("poses") + self.domain_id = _hash(invariant) + self.coverage = ( + None + if self.coverage_cfg is None + else TaskPoseCoverage( + self.domain_id, + self.coverage_cfg.get("entity") + or next(iter(self.coverage_cfg["measurements"].values()))["entity"], + self.coverage_cfg["grids"], + measurements=self.coverage_cfg.get("measurements"), + ) + ) + self.budget = _PoseBudget( + SceneRobotFeasibilityCfg( + max_candidates=self.cfg["max_candidates"], + max_ik_calls=self.cfg["max_ik_calls"], + max_planning_calls=self.cfg["max_planning_calls"], + max_rollouts=self.cfg["max_rollouts"], + max_wall_time_s=options["engine"]["max_wall_time_s"], + ) + ) + self.budget.validation = ( + True # Use the full host allocation, not search/held-out splitting. + ) + self.planning_budget = _AdmittedPoseBudget(self.budget) + self._candidates = {} + self._checks = {} + self._predicted = {} + self._outcomes = {} + self._stats = {} + self._events = [] + self._raw_ordinal = 0 + self._root_pose = env.robot.get_local_pose(to_matrix=True)[0].detach().clone() + self._joint_seed = ( + env.robot.get_qpos(name=self.cfg["control_part"])[0:1].detach().clone() + if self.cfg["strategy"] == "workspace_joint" + else None + ) + self._local = None + self.output = None + if self.cfg["strategy"] == "workspace_joint": + self._prepare_workspace() + + def bind_program(self, adapter, program): + """Reject mismatched authored grasp branches before any candidate mutates a row.""" + if self.cfg["strategy"] != "workspace_joint": + return + profile = adapter._factory.create_robot_skill_profile() + preset = profile.presets[self.env.cfg.task_program.integration.runtime_preset] + pick = preset.skill_option_templates.get( + "pick_up", preset.skill_option_templates.get("pick") + ) + fixed = None if pick is None else pick.fixed_object_to_eef + if fixed is None: + raise ValueError( + "workspace_joint requires a fixed_object_to_eef pick policy; sampled grasp replacement is unsupported" + ) + from embodichain.lab.task_program.semantics.calls import Pick + + picked = { + entry.call.object.entity_id + for segment in program.iter_segments() + for entry in segment.calls + if type(entry.call) is Pick + } + if not picked: + raise ValueError( + "workspace_joint requires an explicit Pick in its task program" + ) + for entity in picked: + transform = self.cfg["targets"].get(entity) + if transform is None or not np.allclose( + np.array(transform), fixed.detach().cpu().numpy(), rtol=0, atol=1e-6 + ): + raise ValueError( + "Workspace grasp target must match the executed fixed_object_to_eef branch" + ) + + def _prepare_workspace(self): + robot = self.env.robot + part = self.cfg["control_part"] + n = self.cfg["sample_count"] + generator = torch.Generator(device=robot.device).manual_seed(self.seed) + if self.cfg["workspace_source"] == "cache": + cache = robot.get_workspace(part) + settings = robot.cfg.workspace_cfg[part] + ids = cache.sample_indices( + n, + strategy=settings.strategy, + min_score=settings.min_score, + generator=generator, + ) + if bool((ids < 0).any()): + raise ValueError("Workspace cache returned invalid indices") + q = cache.qpos[ids].to(robot.device) + else: + limits = robot.get_qpos_limits(name=part, env_ids=[0])[0] + if not bool(torch.isfinite(limits).all()): + raise ValueError("Workspace FK requires finite joint limits") + q = limits[:, 0] + torch.rand( + (n, len(limits)), device=robot.device, generator=generator + ) * (limits[:, 1] - limits[:, 0]) + fk = robot.compute_batch_fk(q[None], name=part, env_ids=[0], to_matrix=True)[0] + if not bool(torch.isfinite(fk).all()): + raise ValueError("Workspace FK produced nonfinite poses") + self._local = ( + (torch.linalg.inv(self._root_pose)[None] @ fk).detach().cpu().numpy().copy() + ) + + def _guided(self, candidate, rng): + from dataclasses import replace + from embodichain.lab.sim.scene.expansion import ScenePoseChange, SceneVariant + + poses = { + c.entity_uid: np.array(c.arena_pose) for c in candidate.scene.pose_changes + } + origin = "uniform_exploration" + if rng.random() >= self.cfg["uniform_fraction"]: + origin = "workspace_joint" + root = np.array(candidate.deployment.poses[self.env.robot.uid]) + for uid, transform in self.cfg["targets"].items(): + proposed = ( + root[None] @ self._local @ np.linalg.inv(np.array(transform))[None] + ) + base = self.poses[uid] + options = self.configuration[uid] + yaw = np.arctan2(proposed[:, 1, 0], proposed[:, 0, 0]) - math.atan2( + base[1, 0], base[0, 0] + ) + bounds = np.array(options["yaw_rad"]) + yaw += (2 * math.pi) * np.round((bounds.mean() - yaw) / (2 * math.pi)) + xy = proposed[:, :2, 3] - base[:2, 3] + valid = np.flatnonzero( + np.all( + (xy >= np.array(options["translation_m"])[:2, 0]) + & (xy <= np.array(options["translation_m"])[:2, 1]), + axis=1, + ) + & (yaw >= bounds[0]) + & (yaw <= bounds[1]) + ) + if not len(valid): + return candidate, "workspace_no_projected_sample" + index = int(rng.choice(valid)) + angle = float(yaw[index]) + c, s = math.cos(angle), math.sin(angle) + poses[uid][:3, :3] = ( + np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) @ base[:3, :3] + ) + poses[uid][:2, 3] = proposed[index, :2, 3] + for uid, opts in self.configuration.items(): + if "follow" in opts: + parent = opts["follow"] + poses[uid] = self._follower_pose(uid, parent, poses[parent]) + changes = tuple( + ScenePoseChange(uid, tuple(map(tuple, pose))) for uid, pose in poses.items() + ) + return ( + replace( + candidate, + scene=SceneVariant( + self.request_id, self.seed, candidate.scene.ordinal, changes + ), + ), + origin, + ) + + def _check(self, candidate): + from embodichain.lab.sim.motion._budget import _planning_budget_scope + + robot = self.env.robot + part = self.cfg["control_part"] + poses = { + c.entity_uid: np.array(c.arena_pose) for c in candidate.scene.pose_changes + } + for uid, transform in self.cfg["targets"].items(): + target = torch.tensor( + poses[uid] @ np.array(transform), + device=robot.device, + dtype=torch.float32, + )[None] + with _planning_budget_scope(self.budget): + ok, q = robot.compute_ik( + pose=target, joint_seed=self._joint_seed, name=part, env_ids=[0] + ) + limits = robot.get_qpos_limits(name=part, env_ids=[0]) + if ( + not bool(ok.all()) + or not bool(torch.isfinite(q).all()) + or not bool(((q >= limits[..., 0]) & (q <= limits[..., 1])).all()) + ): + return False, "pose_ik_or_joint_limits" + fk = robot.compute_fk(qpos=q, name=part, env_ids=[0], to_matrix=True) + angle = torch.acos( + ( + ( + torch.einsum( + "bii->b", fk[:, :3, :3].transpose(1, 2) @ target[:, :3, :3] + ) + - 1 + ) + / 2 + ).clamp(-1, 1) + ) + if ( + not bool(torch.isfinite(fk).all()) + or float((fk[:, :3, 3] - target[:, :3, 3]).norm()) > 1e-3 + or float(angle.max()) > 1e-2 + ): + return False, "full_pose_fk_residual" + return True, "passed" + + def _follower_pose( + self, uid: str, parent: str, parent_pose: np.ndarray + ) -> np.ndarray: + """Apply the parent's arena displacement and rotation to the nominal follower.""" + nominal_parent = self.poses[parent] + pose = self.poses[uid].astype(parent_pose.dtype, copy=True) + pose[:3, 3] = pose[:3, 3] + parent_pose[:3, 3] - nominal_parent[:3, 3] + pose[:3, :3] = parent_pose[:3, :3] @ nominal_parent[:3, :3].T @ pose[:3, :3] + return pose + + def _offer(self, candidate, rng, raw): + if self.cfg["strategy"] == "workspace_joint": + candidate, origin = self._guided(candidate, rng) + ok, reason = self._check(candidate) + return candidate, origin, ok, reason + return candidate, "bounded_pose", True, "passed" + + def _offer_origin(self, raw): + return self.cfg["strategy"] + + def candidate(self, ordinal): + from dataclasses import replace + from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted + + if ordinal in self._candidates: + return self._candidates[ordinal] + if ( + type(ordinal) is not int + or not 0 <= ordinal < self.options["engine"]["max_attempts"] + ): + raise ValueError("ordinal must be within the configured attempt budget") + rng = np.random.default_rng( + int.from_bytes( + hashlib.sha256( + f"{self.seed}/{ordinal}/pose-selection".encode() + ).digest()[:8], + "little", + ) + ) + remaining = max( + 1, + self.cfg["max_rollouts"] + - self.budget.counts["rollouts"] + - len(self._predicted), + ) + quota = max(1, (self.cfg["max_candidates"] - self._raw_ordinal) // remaining) + count = ( + 1 + if self.schedule["policy"] == "fifo" + and self.cfg["strategy"] == "bounded_pose" + else min(self.schedule["pool_size"], quota) + ) + pool = [] + last = None + origin = "bounded_pose" + reason = "no_candidate" + for _ in range(count): + raw = self._raw_ordinal + # Keep the historical simple FIFO sequence exactly intact. + candidate = super().candidate( + ordinal + if count == 1 and self.cfg["strategy"] == "bounded_pose" + else raw + ) + last = candidate + origin = self._offer_origin(raw) + try: + self.budget.charge("candidates") + self._raw_ordinal += 1 + ok = True + candidate, origin, ok, reason = self._offer(candidate, rng, raw) + except FeasibilityBudgetExhausted as e: + ok = False + reason = str(e) + poses = {c.entity_uid: c.arena_pose for c in candidate.scene.pose_changes} + cells = ( + () + if self.coverage is None + else ( + self.coverage.proposal_cells(poses) + if self.coverage_cfg.get("measurements") is not None + else self.coverage.cells( + poses[self.coverage_cfg["entity"]], + poses[self.coverage_cfg["place_reference"]], + ) + ) + ) + self._events.append( + { + "event": "proposal", + "ordinal": ordinal, + "raw_ordinal": raw, + "origin": origin, + "screened": ok, + "reason": reason, + "pose_changes": { + c.entity_uid: c.arena_pose for c in candidate.scene.pose_changes + }, + "installation": dict(candidate.deployment.poses), + "cells": cells, + } + ) + if ok: + pool.append((candidate, cells, origin, raw)) + if self.schedule["policy"] == "fifo" and pool: + break + if self.budget._blocked is not None: + break + valid = bool(pool) + if pool: + if ( + self.schedule["policy"] == "fifo" + or rng.random() < self.schedule["exploration_fraction"] + ): + chosen = pool[int(rng.integers(len(pool)))] + selection = "exploration" + else: + predicted = Counter( + c for values in self._predicted.values() for c in values + ) + + def score(entry): + cells = entry[1] + success, total = self._stats.get(self._region(cells), (0, 0)) + return ( + (success + 1) + / (total + 2) + * self.coverage.gain(cells, predicted) + ) + + scores = np.array([score(c) for c in pool]) + ids = np.flatnonzero(scores == scores.max()) + chosen = pool[int(rng.choice(ids))] + selection = "coverage" + candidate, cells, origin, raw = chosen + self._predicted[ordinal] = cells + reason = "passed" + else: + candidate = last + cells = () + selection = "rejected" + # Candidate identity includes the complete correlated vector, not just + # the raw draw before workspace conditioning. + parameters = tuple( + v + for change in candidate.scene.pose_changes + for row in change.arena_pose + for v in row + ) + candidate = replace( + candidate, + scene=replace(candidate.scene, ordinal=ordinal), + parameters=parameters, + ) + self._candidates[ordinal] = candidate + self._checks[ordinal] = (valid, reason) + self._events.append( + { + "event": "selection", + "ordinal": ordinal, + "raw_ordinal": raw, + "candidate_id": candidate.candidate_id, + "selection": selection, + "origin": origin, + "cells": cells, + "screened": valid, + } + ) + self.checkpoint() + return candidate + + def validate(self, ordinal): + from .contracts import ValidationCheck, ValidationResult + + ok, reason = self._checks[ordinal] + return ValidationResult( + ( + ValidationCheck( + "scene_sampling.proposal", "passed" if ok else "failed", reason + ), + ) + ) + + @staticmethod + def _region(cells): + return tuple((cell[0], *(axis // 2 for axis in cell[1:])) for cell in cells) + + def finish(self, ordinal, episode, receipt, *, executed): + if ordinal in self._outcomes: + return + cells = self._predicted.pop(ordinal, ()) + success = episode is not None and episode.validation.accepted + if executed: + s, n = self._stats.get(self._region(cells), (0, 0)) + self._stats[self._region(cells)] = (s + int(success), n + 1) + result = { + "elapsed_s": self.budget.clock() - self.budget.started, + "proposals": self.budget.counts["candidates"], + "rollouts": self.budget.counts["rollouts"], + "event": "outcome", + "ordinal": ordinal, + "executed": executed, + "physical_accepted": success, + "committed": receipt is not None and receipt.confirmed, + "coverage": ({} if self.coverage is None else self.coverage.phase_counts()), + } + if episode is not None and "full_joint_positions" in episode.observations: + states = episode.observations["full_joint_positions"] + segments = episode.metadata.get("program_segments", ()) + pick_end = ( + min(len(states) - 1, int(segments[0]["end_step"])) if segments else 0 + ) + ids = self.env.robot.control_parts[self.cfg["control_part"]] + if ids and isinstance(ids[0], str): + ids = [self.env.robot.joint_names.index(name) for name in ids] + row = int(episode.metadata["source_env_id"]) + values = states[[pick_end, len(states) - 1]][:, ids].to( + self.env.robot.device + )[None] + tcp = self.env.robot.compute_batch_fk( + values, name=self.cfg["control_part"], env_ids=[row], to_matrix=True + )[0] + result["measured_tcp_poses"] = tcp.detach().cpu().tolist() + result["measured_tcp_indices"] = [pick_end, len(states) - 1] + self._outcomes[ordinal] = result + self._events.append(result) + self.checkpoint() + + def state_dict(self): + from dataclasses import asdict + + return deepcopy( + { + "version": 1, + "seed": self.seed, + "revision": self.request_id, + "options": self.options, + "coverage": ( + None if self.coverage is None else self.coverage.state_dict() + ), + "raw_ordinal": self._raw_ordinal, + "candidates": { + str(k): { + "request_id": v.request_id, + "scene": asdict(v.scene), + "deployment": {"poses": dict(v.deployment.poses)}, + "mode_id": v.mode_id, + "parameters": v.parameters, + } + for k, v in self._candidates.items() + }, + "checks": self._checks, + "predicted": self._predicted, + "outcomes": self._outcomes, + "events": self._events, + "statistics": [ + [cells, list(counts)] for cells, counts in self._stats.items() + ], + "budget": dict(self.budget.counts), + "elapsed_s": self.budget.clock() - self.budget.started, + "blocked": self.budget._blocked, + } + ) + + def load_state_dict(self, state): + """Restore deterministic proposal history; never promote pending writes.""" + from embodichain.lab.sim.scene.expansion import ScenePoseChange, SceneVariant + from embodichain.lab.sim.scene.feasibility import ( + JointSceneCandidate, + RobotDeploymentCandidate, + ) + + data = deepcopy(state) + if ( + self._candidates + or data.get("version") != 1 + or data.get("seed") != self.seed + or data.get("revision") != self.request_id + or data.get("options") != self.options + ): + raise ValueError("Sampling restore requires an empty, matching source") + candidates = {} + for key, value in data["candidates"].items(): + scene = dict(value["scene"]) + scene["pose_changes"] = tuple( + ScenePoseChange(**change) for change in scene["pose_changes"] + ) + candidates[int(key)] = JointSceneCandidate( + value["request_id"], + SceneVariant(**scene), + RobotDeploymentCandidate(**value["deployment"]), + value["mode_id"], + tuple(value["parameters"]), + ) + if ( + any(not 0 <= k < self.options["engine"]["max_attempts"] for k in candidates) + or not 0 <= data["raw_ordinal"] <= self.cfg["max_candidates"] + ): + raise ValueError("Sampling checkpoint exceeds declared bounds") + budget = data["budget"] + limits = { + "candidates": self.cfg["max_candidates"], + "ik": self.cfg["max_ik_calls"], + "planning": self.cfg["max_planning_calls"], + "rollouts": self.cfg["max_rollouts"], + } + if ( + set(budget) != set(limits) + or any( + type(v) is not int or not 0 <= v <= limits[k] for k, v in budget.items() + ) + or not math.isfinite(data["elapsed_s"]) + or data["elapsed_s"] < 0 + ): + raise ValueError("Malformed sampling budget checkpoint") + if self.coverage is not None: + self.coverage.load_state_dict(data["coverage"]) + elif data["coverage"] is not None: + raise ValueError("Checkpoint unexpectedly contains pose coverage") + self._candidates = candidates + self._checks = {int(k): tuple(v) for k, v in data["checks"].items()} + self._predicted = { + int(k): tuple(tuple(c) for c in v) for k, v in data["predicted"].items() + } + self._outcomes = {int(k): v for k, v in data["outcomes"].items()} + self._events = data["events"] + self._stats = { + tuple(tuple(c) for c in cells): tuple(counts) + for cells, counts in data["statistics"] + } + self._raw_ordinal = data["raw_ordinal"] + self.budget.counts = budget + self.budget.started = self.budget.clock() - data["elapsed_s"] + self.budget.deadline = self.budget.started + self.budget.cfg.max_wall_time_s + self.budget._blocked = ( + None if data["blocked"] is None else tuple(data["blocked"]) + ) + + def checkpoint(self): + if self.output is None: + return + from embodichain.data_pipeline.recording.media import _atomic_json + + _atomic_json( + self.output / "pose-sampling.json", + { + "strategy": self.cfg["strategy"], + "policy": self.schedule["policy"], + "domain_id": self.domain_id, + "budget": dict(self.budget.counts), + "events": self._events, + "coverage": ( + None if self.coverage is None else self.coverage.state_dict() + ), + "checkpoint": self.state_dict(), + }, + ) diff --git a/embodichain/lab/expansion/_pose_sampling_cfg.py b/embodichain/lab/expansion/_pose_sampling_cfg.py new file mode 100644 index 0000000000..e8fc9bfe65 --- /dev/null +++ b/embodichain/lab/expansion/_pose_sampling_cfg.py @@ -0,0 +1,394 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Strict optional scene sampling and pose coverage declarations.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +import math +from typing import TYPE_CHECKING + +import numpy as np + +if TYPE_CHECKING: + from embodichain.lab._scene_config.feasibility import PhaseTarget, TaskOperationMode + +__all__: list[str] = [] + +_ROBOT_LAYOUT_STRATEGIES = ("robot_first", "bidirectional") +_LAYOUT_STRATEGIES = ("scene_first", *_ROBOT_LAYOUT_STRATEGIES) + + +@dataclass(frozen=True) +class _AnchorTarget: + phase_id: str + target: PhaseTarget + root_uid: str + + +def _anchor_targets( + mode: TaskOperationMode, layout: Mapping, policy: Mapping, selections: object +) -> tuple[_AnchorTarget, ...]: + """Resolve each named target to a rigid frame in the initial support tree.""" + if not isinstance(selections, list) or not selections: + raise ValueError( + "robot_anchors requires a nonempty list of phase/part selectors" + ) + targets = {} + moved = set() + support, parents = layout["support_uid"], layout["parent_by_uid"] + variable_members = policy["intra_group"]["targets"] + for phase in mode.phases: + for target in phase.targets: + targets[phase.phase_id, target.control_part] = (target, moved.copy()) + moved.update(held.entity_uid for held in phase.held_objects) + moved.update(change.entity_uid for change in phase.resulting_poses) + result, seen = [], set() + for sel in selections: + if ( + not isinstance(sel, Mapping) + or set(sel) != {"phase_id", "control_part"} + or any(type(value) is not str for value in sel.values()) + ): + raise ValueError("robot_anchors selects explicit phase_id and control_part") + key = (sel["phase_id"], sel["control_part"]) + if key not in targets or key in seen: + raise ValueError("robot_anchors must select unique declared phase targets") + seen.add(key) + target, earlier_moves = targets[key] + root = target.frame_uid + if root not in parents: + raise ValueError("A robot anchor target needs a frame in the support tree") + while True: + if root in variable_members or root in earlier_moves: + raise ValueError( + "A robot anchor needs an initial frame rigidly attached to its layout root" + ) + if parents[root] == support: + break + root = parents[root] + groups = policy["group_targets"] + if not policy["randomize_groups"] or ( + groups is not None and root not in groups + ): + raise ValueError("A robot anchor requires a movable layout root") + if root in layout.get("conditions", {}): + raise ValueError("A robot anchor cannot replace an authored root condition") + result.append(_AnchorTarget(key[0], target, root)) + return tuple(result) + + +def validate_sampling(value: dict) -> None: + """Normalize optional settings without changing legacy configurations.""" + from ._pose_cells import _grid + + allowed = { + "strategy", + "layout", + "task_mode", + "screening", + "robot_anchors", + "workspace_source", + "control_part", + "sample_count", + "uniform_fraction", + "targets", + "max_rollouts", + "max_candidates", + "max_ik_calls", + "max_planning_calls", + } + if not any(k in value for k in ("scene_sampling", "scheduling", "pose_coverage")): + return + sampling = value.setdefault("scene_sampling", {}) + schedule = value.setdefault("scheduling", {}) + if not isinstance(sampling, dict) or set(sampling) - allowed: + raise ValueError("Unknown scene_sampling settings") + if not isinstance(schedule, dict) or set(schedule) - { + "policy", + "pool_size", + "exploration_fraction", + }: + raise ValueError("Unknown pose scheduling settings") + for k, v in { + "strategy": "bounded_pose", + "workspace_source": "joint_uniform_fk", + "control_part": "arm", + "sample_count": 4096, + "uniform_fraction": 0.25, + "targets": {}, + "max_candidates": 4096, + "max_rollouts": value["engine"]["max_attempts"], + "max_ik_calls": 32768, + "max_planning_calls": 4096, + }.items(): + sampling.setdefault(k, v) + for k, v in { + "policy": "fifo", + "pool_size": 64, + "exploration_fraction": 0.25, + }.items(): + schedule.setdefault(k, v) + if sampling["strategy"] not in ( + "bounded_pose", + "workspace_joint", + *_LAYOUT_STRATEGIES, + ): + raise ValueError( + "scene_sampling.strategy must be bounded_pose, workspace_joint, " + "scene_first, robot_first or bidirectional" + ) + if sampling["workspace_source"] not in ("cache", "joint_uniform_fk"): + raise ValueError( + "workspace_source must explicitly select cache or joint_uniform_fk" + ) + if schedule["policy"] not in ("fifo", "task_pose_coverage"): + raise ValueError("Pose scheduling supports fifo or task_pose_coverage") + for k in ( + "sample_count", + "max_candidates", + "max_rollouts", + "max_ik_calls", + "max_planning_calls", + ): + if type(sampling[k]) is not int or not 0 < sampling[k] <= 1000000: + raise ValueError(f"{k} must be a bounded positive integer") + if sampling["sample_count"] > 65536: + raise ValueError("Workspace sample_count must be <= 65536") + if sampling["max_rollouts"] < value["engine"].get("episodes_per_operator", 1): + raise ValueError("max_rollouts must cover the requested episode quota") + if sampling["max_candidates"] < value["engine"]["max_attempts"]: + raise ValueError("max_candidates must cover at least one proposal per attempt") + if type(schedule["pool_size"]) is not int or not 1 <= schedule["pool_size"] <= 64: + raise ValueError("pool_size must be between 1 and 64") + for obj, key in ( + (sampling, "uniform_fraction"), + (schedule, "exploration_fraction"), + ): + if ( + type(obj[key]) not in (int, float) + or not math.isfinite(obj[key]) + or not 0 <= obj[key] <= 1 + ): + raise ValueError(f"{key} must be a probability") + if ( + type(sampling["control_part"]) is not str + or not sampling["control_part"].strip() + ): + raise ValueError("control_part must be explicit") + if not isinstance(sampling["targets"], dict): + raise ValueError( + "workspace targets must map scene entities to object-from-TCP matrices" + ) + if sampling["strategy"] == "workspace_joint" and not sampling["targets"]: + raise ValueError( + "workspace_joint requires explicit object-local target transforms" + ) + for uid, pose in sampling["targets"].items(): + m = np.asarray(pose, dtype=float) + if ( + uid not in value["scene"] + or "follow" in value["scene"][uid] + or m.shape != (4, 4) + or not np.isfinite(m).all() + or not np.allclose(m[3], [0, 0, 0, 1]) + or not np.allclose(m[:3, :3].T @ m[:3, :3], np.eye(3), atol=1e-6) + or not np.isclose(np.linalg.det(m[:3, :3]), 1) + ): + raise ValueError( + "Workspace targets require a declared independent scene entity and rigid transform" + ) + if sampling["strategy"] == "workspace_joint" and "layout" in sampling: + raise ValueError( + "workspace_joint does not consume layout conditions; choose scene_first or a screened bounded_pose baseline" + ) + if sampling["strategy"] in _LAYOUT_STRATEGIES or "layout" in sampling: + layout = sampling.get("layout") + if ( + not isinstance(layout, dict) + or set(layout) - {"support_uid", "parent_by_uid", "policy", "conditions"} + or not {"support_uid", "parent_by_uid", "policy"} <= layout.keys() + ): + raise ValueError( + "Layout sampling requires support_uid, parent_by_uid and policy" + ) + if ( + not isinstance(layout["support_uid"], str) + or not layout["support_uid"].strip() + or not isinstance(layout["parent_by_uid"], dict) + or not layout["parent_by_uid"] + or any( + type(k) is not str or type(v) is not str + for k, v in layout["parent_by_uid"].items() + ) + ): + raise ValueError( + "layout requires an explicit support UID and parent mapping" + ) + if not isinstance(layout["policy"], dict): + raise ValueError("layout.policy must be a mapping") + mode = sampling.get("task_mode") + if ( + not isinstance(mode, dict) + or set(mode) - {"mode_id", "phases"} + or not {"mode_id", "phases"} <= mode.keys() + or not isinstance(mode["phases"], list) + or not mode["phases"] + ): + raise ValueError( + "Layout sampling requires a declared task_mode phase graph" + ) + screening = sampling.setdefault("screening", {}) + if not isinstance(screening, dict) or set(screening) - { + "beam_width", + "ik_seeds", + "max_rrt_nodes", + "planning_timeout_s", + }: + raise ValueError("Unknown scene-first screening budgets") + for key, default in { + "beam_width": 2, + "ik_seeds": 2, + "max_rrt_nodes": 64, + "planning_timeout_s": 0.5, + }.items(): + screening.setdefault(key, default) + for key in ("beam_width", "ik_seeds", "max_rrt_nodes"): + if type(screening[key]) is not int or not 1 <= screening[key] <= 4096: + raise ValueError("Screening counts must be positive and bounded") + if ( + type(screening["planning_timeout_s"]) not in (int, float) + or not math.isfinite(screening["planning_timeout_s"]) + or not 0 < screening["planning_timeout_s"] <= 60 + ): + raise ValueError("Screening timeout must be positive and bounded") + from embodichain.lab._scene_config.feasibility import request_from_mapping + from embodichain.lab._scene_config.policy import _policy_data + + normalized_policy = _policy_data(layout["policy"]) + if any( + "follow" in value["scene"].get(uid, {}) + for uid in normalized_policy["intra_group"]["targets"] + ): + raise ValueError( + "Rigid scene followers cannot also be independent intra-group targets" + ) + parents = layout["parent_by_uid"] + support = layout["support_uid"] + for uid in parents: + seen = set() + current = uid + while current != support: + if current in seen or current not in parents: + raise ValueError("Layout parents must form one rooted support tree") + seen.add(current) + current = parents[current] + identities = {support, *parents, *value["scene"]} + request = request_from_mapping( + dict( + task_id="preflight", + robot_profile_id="preflight", + policy_id="preflight", + validator_id="preflight", + parent_scene_id="preflight", + entity_poses={uid: np.eye(4).tolist() for uid in identities}, + variables=[], + modes=[mode], + ) + ) + for uid, parent in parents.items(): + if ( + parent == support + and uid in value["scene"] + and "follow" not in value["scene"][uid] + ): + bounds = value["scene"][uid] + if bounds["yaw_rad"] != [0, 0] or bounds["translation_m"][2] != [0, 0]: + raise ValueError("Root layout domains currently support XY only") + conditions = layout.get("conditions", {}) + if not isinstance(conditions, dict): + raise ValueError("layout.conditions must map UIDs to arena poses") + for uid, m in conditions.items(): + m = np.asarray(m, dtype=float) + if ( + m.shape != (4, 4) + or not np.isfinite(m).all() + or not np.allclose(m[3], [0, 0, 0, 1]) + or not np.allclose(m[:3, :3].T @ m[:3, :3], np.eye(3), atol=1e-6) + or not np.isclose(np.linalg.det(m[:3, :3]), 1) + ): + raise ValueError("layout conditions require proper rigid matrices") + if ( + "robot_anchors" in sampling + or sampling["strategy"] in _ROBOT_LAYOUT_STRATEGIES + ): + anchors = _anchor_targets( + request.modes[0], + layout, + normalized_policy, + sampling.get("robot_anchors"), + ) + if any( + a.root_uid not in value["scene"] + or "follow" in value["scene"][a.root_uid] + for a in anchors + ): + raise ValueError( + "A robot anchor requires an independently declared scene root" + ) + elif "task_mode" in sampling: + raise ValueError("task_mode screening requires an explicit layout") + elif "robot_anchors" in sampling: + raise ValueError("robot_anchors requires an explicit layout and task_mode") + coverage = value.get("pose_coverage") + if coverage is None: + if schedule["policy"] == "task_pose_coverage": + raise ValueError( + "task_pose_coverage scheduling requires pose_coverage grids" + ) + return + if isinstance(coverage, dict) and set(coverage) == {"measurements", "grids"}: + from ._pose_cells import _measurements + + measurements, grids = coverage["measurements"], coverage["grids"] + if not isinstance(grids, dict): + raise ValueError("Named coverage measurements require matching grids") + coverage["measurements"] = _measurements(measurements, grids) + if any(d["entity"] not in value["scene"] for d in measurements.values()): + raise ValueError("Coverage requires declared scene entities") + coverage["grids"] = {k: _grid(v) for k, v in grids.items()} + return + if not isinstance(coverage, dict) or set(coverage) != { + "entity", + "place_reference", + "grids", + }: + raise ValueError("pose_coverage requires entity, place_reference and grids") + if ( + any(coverage[k] not in value["scene"] for k in ("entity", "place_reference")) + or coverage["entity"] == coverage["place_reference"] + ): + raise ValueError( + "Coverage entity and place_reference must select distinct scene entities" + ) + if not isinstance(coverage["grids"], dict) or set(coverage["grids"]) != { + "pick", + "place", + }: + raise ValueError("Coverage requires pick and place grids") + coverage["grids"] = {k: _grid(v) for k, v in coverage["grids"].items()} diff --git a/embodichain/lab/expansion/_preview.html b/embodichain/lab/expansion/_preview.html new file mode 100644 index 0000000000..f1615652c9 --- /dev/null +++ b/embodichain/lab/expansion/_preview.html @@ -0,0 +1,35 @@ +EmbodiChain · 扩增结果回放 + +

    EmbodiChain · 扩增结果回放

    完整模型虚影 · 已确认的真实扩增结果

    正在连接原生渲染器
    +
    拖动画面旋转 · 滚轮缩放
    EmbodiChain 原生场景渲染画面
    正在加载完整机器人模型…
    ——
    +
    +
    EmbodiChain SimulationManager / RobotMarkers / VisualizationMarkers → DexSim Hybrid 原生渲染。仅回放,物理推进 0 步。机器人与物体使用逐帧记录;轨迹线每 4 帧取点并连接。视角、灯光与透明材质仅用于验收展示。
    + \ No newline at end of file diff --git a/embodichain/lab/expansion/_preview_data.py b/embodichain/lab/expansion/_preview_data.py new file mode 100644 index 0000000000..2fa9ce66d0 --- /dev/null +++ b/embodichain/lab/expansion/_preview_data.py @@ -0,0 +1,575 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Read-only inputs and controls for the native expansion-result browser.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any +import json +import math +import numpy as np +from scipy.spatial.transform import Rotation + +from .comparison import MeasuredTrajectory +from .persistence import ArtifactEpisodeSink, load_expansion_episode + +__all__: list[str] = [] + + +def pose_matrix(pose: np.ndarray) -> np.ndarray: + result = np.eye(4) + result[:3, :3] = Rotation.from_quat(pose[3:]).as_matrix() + result[:3, 3] = pose[:3] + return result + + +@dataclass +class PreviewClip: + measured: MeasuredTrajectory + space: str + affordance: str + variant: str + targets: np.ndarray + phase_names: tuple[str, ...] + anchors: np.ndarray + candidate_ids: tuple[str, ...] + label: str = "" + links: np.ndarray | None = None + tips: np.ndarray | None = None + + +def annotations(metadata: Any, measured: MeasuredTrajectory) -> dict[str, Any]: + records = metadata.get("expansion", ()) + env_ids = {r["env_id"] for r in records if r.get("env_id") is not None} + if metadata.get("source_env_id") is not None: + source_row = metadata["source_env_id"] + if type(source_row) is not int or source_row < 0: + raise ValueError("source_env_id must be a nonnegative integer") + env_ids.add(source_row) + if len(env_ids) > 1: + raise ValueError("Episode expansion annotations refer to multiple rows") + env_id = next(iter(env_ids), None) + branches, candidates, targets, phases = [], [], [], [] + phase_safe = True + for segment in metadata.get("program_segments", ()): + runtime = segment.get("runtime", segment.get("metadata", {}).get("runtime", {})) + for call in runtime.get("calls", ()): + for event in call.get("events", ()): + if event.get("kind") == "trajectory_segment_entered": + mask = event.get("env_mask") + if mask: + if env_id is not None: + index = 0 if len(mask) == 1 else env_id + if index >= len(mask): + phase_safe = False + continue + if not mask[index]: + continue + elif len(set(mask)) > 1: + phase_safe = False + continue + elif not any(mask): + continue + phases.append( + ( + str(call["call"]["call_kind"]) + + "/" + + str(event["segment_name"]), + float(event["timestamp"]), + ) + ) + # Only the successful final planning attempt describes the executed target. + attempts = call.get("plan_attempts", ()) + if not attempts: + continue + sample = ( + attempts[-1] + .get("planner_diagnostics", {}) + .get("metadata", {}) + .get("affordance_sample", {}) + .get("grasp") + ) + if not sample: + continue + rows = list(sample["env_ids"]) + row = ( + rows.index(env_id) + if env_id in rows + else 0 if env_id is None and len(rows) == 1 else None + ) + if row is None: + # Legacy numeric evidence remains usable; never guess another row's grasp. + continue + if not sample["success"][row]: + continue + sampling = sample.get("sampling") or {} + overrides = sampling.get("branch_overrides") + branch = overrides[row] if overrides else sampling.get("branch_override") + if branch is None and sampling.get("count") == 1: + branch = 0 + branches.append("unrecorded" if branch is None else str(branch)) + candidates.append(str(sample["candidate_ids"][row])) + uid = ( + call.get("call", {}) + .get("arguments", {}) + .get("object", {}) + .get("entity_id") + ) + if uid not in measured.object_poses: + continue + reference = np.asarray(sample["reference_poses"][row], dtype=float) + selected = np.asarray(sample["selected_poses"][row], dtype=float) + if ( + reference.shape != (4, 4) + or selected.shape != (4, 4) + or not np.isfinite([reference, selected]).all() + ): + raise ValueError("Invalid recorded grasp transform") + targets.append( + pose_matrix(measured.object_poses[uid][0]) + @ np.linalg.inv(reference) + @ selected + ) + phases = [v for i, v in enumerate(phases) if i == 0 or v != phases[i - 1]] + anchors = np.array([p[1] for p in phases] + [measured.timestamps[-1]]) + if ( + not phase_safe + or not phases + or anchors[0] < 0 + or not np.isfinite(anchors).all() + or not np.all(np.diff(anchors) > 0) + ): + phases = [] + anchors = np.empty(0) + variants = [ + str(r["trajectory_selected"]) + for r in records + if r.get("trajectory_requested") is not None + ] + if not variants: + variants = [ + str(r["operator"]) + for r in records + if r.get("operator") + in {"joint_residual", "via_points", "nullspace_residual"} + ] + variants = list(dict.fromkeys(variants)) + return dict( + affordance="/".join(branches) or "unrecorded", + variant="/".join(variants) or "unrecorded", + targets=np.asarray(targets, dtype=float).reshape(-1, 4, 4), + phase_names=tuple(p[0] for p in phases), + anchors=anchors, + candidate_ids=tuple(candidates), + ) + + +def _object_specs(cfg: dict[str, Any]) -> dict[str, dict[str, Any]]: + objects = {} + for obj in [*(cfg.get("rigid_object") or []), *(cfg.get("background") or [])]: + uid = obj.get("uid") + if not isinstance(uid, str) or not uid or uid in objects: + raise ValueError("Preview requires unique recorded rigid object IDs") + if obj.get("visuals") is not None or obj.get("visual_shape"): + raise ValueError( + "Preview does not yet support explicit rigid visual replacements" + ) + shape, source = obj.get("shape") or {}, obj.get("source") + if source: + if not isinstance(source, dict) or not source.get("file_path") or shape: + raise ValueError("Preview requires an unambiguous source object") + filename = source["file_path"] + elif shape.get("shape_type") == "Mesh": + filename = shape.get("fpath") + elif shape.get("shape_type") in { + "Cube", + "Sphere", + "Cylinder", + "Capsule", + "Cone", + }: + filename = None + else: + raise ValueError("Unsupported recorded rigid visual geometry") + if (source or shape.get("shape_type") == "Mesh") and ( + not filename or not Path(filename).expanduser().is_file() + ): + raise ValueError(f"Recorded object asset is unavailable: {filename}") + scale = obj.get("body_scale") + if scale is not None and ( + np.asarray(scale).shape != (3,) + or not np.isfinite(scale).all() + or np.any(np.asarray(scale) <= 0) + ): + raise ValueError("Invalid recorded object scale") + objects[uid] = { + key: obj[key] + for key in ("shape", "source", "body_scale", "body_type") + if key in obj + } + return objects + + +def _scene(job: dict[str, Any]) -> tuple[dict, dict[str, tuple[float, ...] | None]]: + cfg = job.get("resolved_deployment") + if not isinstance(cfg, dict) or not isinstance(cfg.get("robot"), dict): + raise ValueError( + "Preview requires job.json with a resolved single-robot deployment" + ) + unsupported = ( + "articulation", + "rigid_object_group", + "cable_object", + "fluid_object", + "soft_object", + "cloth_object", + "deformable_object", + ) + if any(cfg.get(k) for k in unsupported) or job.get("config", {}).get("scene_atlas"): + raise ValueError( + "Preview requires a fixed layout, one URDF robot and recorded rigid objects" + ) + robot = cfg["robot"] + if robot.get("visuals") or robot.get("link_visuals"): + raise ValueError("Preview requires the URDF-owned robot visual geometry") + if not np.allclose(robot.get("body_scale", [1, 1, 1]), [1, 1, 1]): + raise ValueError("Scaled robot assets are not supported by this preview") + boxes = {} + for uid, obj in _object_specs(cfg).items(): + shape = obj.get("shape") or {} + if shape.get("shape_type") != "Cube": + boxes[uid] = None + continue + size = np.asarray(shape["size"], float) * np.asarray( + obj.get("body_scale") or [1, 1, 1], float + ) + if size.shape != (3,) or not np.isfinite(size).all() or np.any(size <= 0): + raise ValueError("Invalid recorded Cube dimensions") + boxes[uid] = tuple(size) + return robot, boxes + + +@dataclass +class PreviewData: + clips: list[PreviewClip] + reference: PreviewClip | None + asset: Path + eef_link: str + boxes: dict[str, tuple[float, ...] | None] + ground_visible: bool = True + coverage: dict[str, Any] | None = None + object_specs: dict[str, dict[str, Any]] | None = None + + @property + def all_clips(self) -> list[PreviewClip]: + return self.clips + ([self.reference] if self.reference is not None else []) + + @property + def phase_alignment(self) -> bool: + signature = self.clips[0].phase_names + return bool(signature) and all( + c.phase_names == signature for c in self.all_clips + ) + + +def load_preview( + outputs: list[Path], + reference: Path | None = None, + *, + robot_asset: Path | None = None, + eef_link: str | None = None, + max_episodes: int = 32, +) -> PreviewData: + if not outputs or not 1 <= max_episodes <= 64: + raise ValueError("Provide result directories and max_episodes in [1,64]") + coverage = None + if len(outputs) == 1 and (Path(outputs[0]) / "preview-plan.json").is_file(): + root = Path(outputs[0]).resolve() + coverage = json.loads((root / "preview-plan.json").read_text()) + outputs = [] + for record in coverage["records"]: + path = (root / record["result"]).resolve() + if not path.is_relative_to(root): + raise ValueError("Preview result path escapes the manifest directory") + if record["confirmed"] > 0: + outputs.append(path) + if not outputs: + raise ValueError( + "Preview plan has no qualified trajectories; inspect its rejection/unrun records" + ) + from .preview import _inputs, _recorded_eef + + paths = _inputs([Path(p) for p in outputs]) + eef_link = eef_link or _recorded_eef(paths) + if len(set(paths)) != len(paths): + raise ValueError("Duplicate output directory") + if reference is not None: + paths.append(Path(reference).expanduser().resolve()) + clips = [] + reference_clip = None + asset = None + boxes = None + robot_spec = None + object_specs = None + end = None + ground = True + seen = set() + frames = 0 + reference_index = len(paths) - 1 if reference is not None else None + for group, path in enumerate(paths): + job_path = path / "job.json" + if not job_path.is_file(): + raise ValueError(f"Not an expansion job output: {path} (missing job.json)") + job = json.loads(job_path.read_text()) + robot, shapes = _scene(job) + recorded_specs = _object_specs(job["resolved_deployment"]) + if object_specs is not None and recorded_specs != object_specs: + raise ValueError( + "Result directories disagree about recorded object geometry" + ) + object_specs = recorded_specs + spec = {k: robot.get(k) for k in ("uid", "control_parts", "solver_cfg")} + if robot_spec is not None and (spec != robot_spec or shapes != boxes): + raise ValueError( + "Result directories must share robot/control topology and object geometry" + ) + robot_spec = spec + boxes = shapes + path_value = robot.get("fpath") or (robot.get("urdf_cfg") or {}).get("fpath") + resolved_asset = Path(robot_asset or path_value or "").expanduser().resolve() + if not resolved_asset.is_file() or resolved_asset.suffix.lower() != ".urdf": + raise ValueError( + "Recorded URDF is unavailable; supply --robot-asset with matching visuals" + ) + # URDF assembly comments may differ; compare the actual XML model. + import xml.etree.ElementTree as ET + + if asset is not None and ET.tostring(ET.parse(asset).getroot()) != ET.tostring( + ET.parse(resolved_asset).getroot() + ): + raise ValueError("Output directories reference different robot models") + asset = resolved_asset + ends = { + s.get("end_link_name") + for s in robot.get("solver_cfg", {}).values() + if s.get("end_link_name") + } + inferred = eef_link or (next(iter(ends)) if len(ends) == 1 else None) + if not inferred: + raise ValueError( + "Specify --eef-link when the recorded solver endpoint is ambiguous" + ) + end = inferred + if group == 0: + ground = job["resolved_deployment"].get("ground_plane_visible") is not False + elif ground != ( + job["resolved_deployment"].get("ground_plane_visible") is not False + ): + raise ValueError("Outputs disagree about ground visibility") + receipts = [r for r in ArtifactEpisodeSink(path).inspect() if r.confirmed] + is_reference = group == reference_index + if not receipts or (is_reference and len(receipts) != 1): + raise ValueError( + "Each output needs confirmed episodes; reference must contain exactly one" + ) + if len(clips) + len(receipts) > (max_episodes + int(is_reference)): + raise ValueError( + "Preview episode budget exceeded; select smaller output directories" + ) + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + if not episode.validation.accepted: + raise ValueError("Unqualified episode cannot be previewed") + if episode.episode_id in seen: + raise ValueError("Duplicate episode across inputs/reference") + seen.add(episode.episode_id) + measured = MeasuredTrajectory.load_episode( + receipt.storage_id, robot_uid=robot["uid"] + ) + if len(measured.timestamps) != len(episode.actions) + 1 or set( + measured.object_poses + ) != set(boxes): + raise ValueError( + "Incomplete T+1 scene state or unsupported scene entities" + ) + if ( + any( + x["kind"] not in ("robot", "rigid_object") + for x in episode.metadata["initial_state"] + ) + or sum(x["kind"] == "robot" for x in episode.metadata["initial_state"]) + != 1 + ): + raise ValueError( + "Preview supports one robot and recorded rigid objects only" + ) + frames += len(measured.timestamps) + if frames > 100_000: + raise ValueError("Preview exceeds the 100000 measured-state budget") + clip = PreviewClip( + measured=measured, + space=f"S{group+1} · {path.name}", + **annotations(episode.metadata, measured), + ) + if is_reference: + reference_clip = clip + clip.label = "Reference" + else: + clips.append(clip) + clip.label = f"C-{len(clips):03d}" + return PreviewData( + clips, reference_clip, asset, end, boxes, ground, coverage, object_specs + ) + + +class PreviewState: + def __init__(self, data: PreviewData) -> None: + self.data = data + self.values = dict( + space="all", + aff="all", + variant="all", + selected=0, + progress=0.0, + playing=False, + reference=data.reference is not None, + ghosts=True, + paths=True, + targets=True, + positions=True, + align="phase" if data.phase_alignment else "time", + yaw=-2.15, + elevation=0.55, + distance=1.65, + ) + self.options = { + "space": sorted({c.space for c in data.clips}), + "aff": sorted({c.affordance for c in data.clips}), + "variant": sorted({c.variant for c in data.clips}), + "align": ["time"] + (["phase"] if data.phase_alignment else []), + } + + def validate(self, update: Any) -> dict[str, Any]: + if ( + not isinstance(update, dict) + or not update + or set(update) - self.values.keys() + ): + raise ValueError("Invalid preview controls") + bounds = { + "selected": (0, len(self.data.clips) - 1), + "progress": (0, 1), + "yaw": (-20, 20), + "elevation": (0.1, 1.5), + "distance": (0.2, 20), + } + for key, value in update.items(): + if key in self.options: + if value not in self.options[key] + ([] if key == "align" else ["all"]): + raise ValueError(key) + elif key in bounds: + if ( + type(value) not in (float, int) + or not math.isfinite(value) + or not bounds[key][0] <= value <= bounds[key][1] + or (key == "selected" and type(value) is not int) + ): + raise ValueError(key) + elif type(value) is not bool: + raise ValueError(key) + return dict(update) + + def apply(self, update: dict[str, Any]) -> None: + self.values.update(self.validate(update)) + visible = self.visible() + if visible and self.values["selected"] not in visible: + self.values["selected"] = visible[0] + if not visible: + self.values["playing"] = False + + def visible(self) -> list[int]: + v = self.values + return [ + i + for i, c in enumerate(self.data.clips) + if all( + v[k] == "all" or v[k] == actual + for k, actual in [ + ("space", c.space), + ("aff", c.affordance), + ("variant", c.variant), + ] + ) + ] + + def frame_at(self, clip: PreviewClip) -> int: + p = self.values["progress"] + if self.values["align"] == "phase": + n = len(clip.anchors) - 1 + segment = min(n - 1, int(p * n)) + t = np.interp(p * n - segment, [0, 1], clip.anchors[segment : segment + 2]) + else: + t = p * self.duration + return clip.measured.frame_at(t) + + @property + def duration(self) -> float: + return max(float(c.measured.timestamps[-1]) for c in self.data.all_clips) + + def snapshot(self) -> dict[str, Any]: + visible = self.visible() + c = self.data.clips[self.values["selected"]] + f = self.frame_at(c) + return { + **self.values, + "visible": visible, + "count": len(visible), + "total": len(self.data.clips), + "options": self.options, + "has_reference": self.data.reference is not None, + "coverage": self.data.coverage, + "label": c.label if visible else "No matches", + "source_frame": f if visible else None, + "source_frames": len(c.measured.timestamps) if visible else 0, + "time": float(c.measured.timestamps[f]) if visible else None, + "episode": c.measured.name if visible else "", + "candidate": ", ".join(c.candidate_ids) or "unrecorded", + "phase": ( + next( + ( + name + for name, t in reversed(list(zip(c.phase_names, c.anchors))) + if t <= c.measured.timestamps[f] + ), + "unrecorded", + ) + if visible + else "" + ), + "clips": [ + dict( + index=i, + label=c.label, + space=c.space, + aff=c.affordance, + variant=c.variant, + ) + for i, c in enumerate(self.data.clips) + ], + } diff --git a/embodichain/lab/expansion/_preview_job.py b/embodichain/lab/expansion/_preview_job.py new file mode 100644 index 0000000000..55086e7306 --- /dev/null +++ b/embodichain/lab/expansion/_preview_job.py @@ -0,0 +1,550 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Freeze endpoint/interior experiments and collect them through the native CLI.""" + +from __future__ import annotations + +from copy import deepcopy +from datetime import datetime, timezone +from pathlib import Path +from typing import Any +import hashlib +import json +import os +import signal +import subprocess +import sys +import time +from uuid import uuid4 +import numpy as np +from scipy.spatial.transform import Rotation +import yaml + +from .configured import load_expansion_job, preflight_expansion_job +from .persistence import ArtifactEpisodeSink, load_expansion_episode + +__all__: list[str] = [] + + +def boundary_samples( + dimensions: int, interior: int, seed: int +) -> tuple[np.ndarray, list[str]]: + if ( + type(dimensions) is not int + or dimensions < 0 + or type(interior) is not int + or not 1 <= interior <= 61 + ): + raise ValueError("interior samples must be in [1,61]") + if not dimensions: + return np.empty((1, 0)), ["fixed"] + rng = np.random.default_rng(seed) + points = [np.full(dimensions, 0.5), np.zeros(dimensions), np.ones(dimensions)] + # Independent Latin-hypercube strata in every varying coordinate. + inside = np.stack( + [ + (rng.permutation(interior) + rng.uniform(0.05, 0.95, interior)) / interior + for _ in range(dimensions) + ], + axis=1, + ) + points.extend(inside) + return np.asarray(points), ["center", "lower", "upper"] + [ + f"interior-{i+1}" for i in range(interior) + ] + + +def _atomic(path: Path, value: Any) -> None: + temporary = path.with_suffix(".tmp") + temporary.write_text(json.dumps(value, indent=2, allow_nan=False)) + temporary.replace(path) + + +def plan_preview( + job: Path, output: Path | None, *, interior: int = 5, max_episodes: int = 32 +) -> dict[str, Any]: + from embodichain.lab.sim.motion.expansion.profile import load_expansion_profile + from embodichain.lab.sim.motion.expansion.combined import ( + enumerate_candidate_recipes, + ) + + if type(max_episodes) is not int or not 1 <= max_episodes <= 64: + raise ValueError("max_episodes must be in [1,64]") + cfg = load_expansion_job(job) + report = preflight_expansion_job(cfg) + if cfg.source != "task_program" or report.get("motion_source") != "native": + raise ValueError( + "preview --job currently requires native Task Program planning" + ) + if cfg.visual_profile or cfg.render_profiles or cfg.source_profile: + raise ValueError( + "Distribution preview does not sample visual or prototype profiles" + ) + axes = [] + + def axis(name: str, lower: float, upper: float, kind: str, **details: Any) -> None: + if lower != upper: + axes.append( + dict( + name=name, + lower=float(lower), + upper=float(upper), + kind=kind, + **details, + ) + ) + + for kind in ( + "mass_scale", + "friction", + "joint_stiffness_scale", + "joint_damping_scale", + ): + for uid, bounds in sorted(getattr(cfg, kind).items()): + axis(f"{kind}/{uid}", *bounds, kind, uid=uid) + for uid, ranges in sorted(cfg.center_of_mass.items()): + for i, bounds in enumerate(ranges): + axis( + f"center_of_mass/{uid}/{i}", *bounds, "center_of_mass", uid=uid, index=i + ) + profile = None + motion = None + recipe_count = 1 + if cfg.motion_profile: + profile = load_expansion_profile(cfg.motion_profile) + motion = yaml.safe_load(Path(cfg.motion_profile).read_text()) + recipe_count = len(enumerate_candidate_recipes(profile)) + if profile.scene_randomization.reference_family_count != 1: + raise ValueError( + "Use a scene atlas for spatial bounds, not implicit motion families" + ) + spatial = profile.trajectory.spatial + if spatial.enabled and profile.trajectory.variants_per_family > 1: + if profile.trajectory.variants_per_family < len(spatial.operators) + 1: + raise ValueError( + "Preview needs a nominal variant plus at least one variant per configured operator" + ) + if set(spatial.operators) - {"joint_residual", "via_points"}: + raise ValueError( + "Explicit boundary draws support joint_residual and via_points" + ) + robot = report["resolved_deployment"]["robot"] + groups = robot["control_parts"] + parts = robot.get("solver_cfg", {}).keys() + joints = sorted({name for part in parts for name in groups.get(part, [])}) + if not joints or any(not isinstance(j, str) for j in joints): + raise ValueError( + "Named solver control joints are required to bound motion draws" + ) + for name in joints: + if name not in spatial.joint_samples: + axis(f"joint_residual_fraction/{name}", -1, 1, "joint", uid=name) + if "via_points" in spatial.operators and not spatial.knot_samples: + for i in range(spatial.via_count): + axis(f"via_knot_fraction/{i}", -1, 1, "knot", index=i) + prepared = None + deployment = None + if cfg.scene_atlas: + from embodichain.lab.sim.scene.feasibility import load_feasibility_atlas + from embodichain.lab.sim.scene.feasibility.search import ( + PreparedFeasibilitySearch, + ) + from embodichain.lab.sim.scene.feasibility.cfg import SceneRobotFeasibilityCfg + from embodichain.lab.gym.utils.composition import resolve_gym_components + + atlas = load_feasibility_atlas(cfg.scene_atlas) + if len(atlas.request.modes) != 1: + raise ValueError("Preview requires one task mode per scene atlas") + prepared = PreparedFeasibilitySearch( + atlas.request, SceneRobotFeasibilityCfg(seed=cfg.seed) + ) + for i, var in enumerate(atlas.request.variables): + for j, (lo, hi) in enumerate(zip(var.lower, var.upper)): + axis( + f'scene/{var.name}/{("x","y","yaw")[j]}', + lo, + hi, + "scene", + index=i * 3 + j, + ) + raw = yaml.safe_load(Path(cfg.deployment).read_text()) + deployment = resolve_gym_components( + raw, base_dir=Path(cfg.deployment).parent + ).config + for key in ("program", "integration", "execution_policy"): + value = deployment.get("task_program", {}).get(key) + if isinstance(value, str): + deployment["task_program"][key] = str( + (Path(cfg.deployment).parent / value).resolve() + ) + elif isinstance(value, dict) and isinstance(value.get("component"), str): + value["component"] = str( + (Path(cfg.deployment).parent / value["component"]).resolve() + ) + if "embodiment" in raw: + deployment.pop("robot", None) + deployment.pop("sensor", None) + embodiment = deepcopy(raw["embodiment"]) + embodiment["component"] = str( + (Path(cfg.deployment).parent / embodiment["component"]).resolve() + ) + deployment["embodiment"] = embodiment + points, roles = boundary_samples(len(axes), interior, cfg.seed) + counts = [] + for unit in points: + samples = dict(profile.trajectory.spatial.joint_samples) if profile else {} + for a, u in zip(axes, unit): + if a["kind"] == "joint": + samples[a["uid"]] = a["lower"] + float(u) * (a["upper"] - a["lower"]) + counts.append( + profile.affordance.branches_per_family + if profile and samples and all(v == 0 for v in samples.values()) + else recipe_count + ) + total = sum(counts) + if total > max_episodes or total > 64 or total > cfg.max_attempts: + raise ValueError( + f"Preview needs {len(points)} parameter points and up to {recipe_count} motion/affordance recipes per point = {total} attempts; increase --max-episodes (up to 64) and the job max_attempts budget, reduce --interior-samples or use a smaller recipe. Nothing executed." + ) + root = ( + Path(output).expanduser().resolve() + if output + else Path(cfg.output_dir).parent + / ( + Path(cfg.output_dir).name + + "-preview-" + + datetime.now(timezone.utc).strftime("%Y%m%d-%H%M%S") + + "-" + + uuid4().hex[:6] + ) + ) + if root.exists(): + raise FileExistsError( + f"Preview output must be new: {root}; use --input to replay" + ) + # Validate the renderer's supported scene before any costly collection. + from ._preview_data import _scene + + supported = deepcopy(report) + supported["config"] = dict(report.get("config", {}), scene_atlas=None) + _scene(supported) + root.mkdir(parents=True) + (root / "configs").mkdir() + (root / "logs").mkdir() + plan = { + "schema_version": 1, + "source_job": str(Path(job).resolve()), + "source_sha256": hashlib.sha256(Path(job).read_bytes()).hexdigest(), + "source_inputs": report["input_files"], + "source_atlas": cfg.scene_atlas, + "seed": cfg.seed, + "sampling": "lower + upper + center + stratified interior; not a Cartesian grid", + "axes": axes, + "planned_attempts": total, + "recipes_per_point": recipe_count, + "collection_budget_s": cfg.max_wall_time_s, + "output": str(root), + "records": [], + "complete": False, + "coverage_complete": False, + } + for index, (unit, role) in enumerate(zip(points, roles)): + values = { + a["name"]: a["lower"] + float(u) * (a["upper"] - a["lower"]) + for a, u in zip(axes, unit) + } + case = cfg.to_dict() + case.update( + output_dir=str(root / f"jobs/sample-{index:03d}"), + target_episodes=recipe_count, + max_attempts=recipe_count, + retry_interrupted=False, + ) + m = deepcopy(motion) + scene_unit = np.zeros(prepared.dimensions) if prepared else None + if m: + spatial = m["trajectory"]["spatial"] + spatial["joint_samples"] = dict(profile.trajectory.spatial.joint_samples) + knots = list( + profile.trajectory.spatial.knot_samples + or [0.0] * profile.trajectory.spatial.via_count + ) + for a, u in zip(axes, unit): + value = values[a["name"]] + kind = a["kind"] + if kind == "joint": + spatial["joint_samples"][a["uid"]] = value + elif kind == "knot": + knots[a["index"]] = value + elif kind == "scene": + scene_unit[a["index"]] = u + elif kind == "center_of_mass": + case[kind][a["uid"]] = [list(v) for v in case[kind][a["uid"]]] + case[kind][a["uid"]][a["index"]] = [value, value] + else: + case[kind][a["uid"]] = [value, value] + if m: + if "via_points" in profile.trajectory.spatial.operators: + spatial["knot_samples"] = knots + # Exact zero residuals are the nominal trajectory, not extra successes. + # Keep native deduplication intact by requesting only that unique variant. + if spatial.get("joint_samples") and all( + v == 0 for v in spatial["joint_samples"].values() + ): + m["trajectory"]["variants_per_family"] = 1 + case["target_episodes"] = case["max_attempts"] = ( + profile.affordance.branches_per_family + ) + motion_path = root / "configs" / f"motion-{index:03d}.yaml" + motion_path.write_text(yaml.safe_dump(m, sort_keys=False)) + case["motion_profile"] = str(motion_path) + requested_positions = { + obj["uid"]: obj.get("init_pos", [0, 0, 0]) + for obj in report["resolved_deployment"].get("rigid_object", []) + } + if prepared: + candidate = prepared.candidate( + scene_unit, mode_id=prepared.request.modes[0].mode_id, ordinal=index + ) + derived = deepcopy(deployment) + changes = { + p.entity_uid: np.asarray(p.arena_pose) + for p in candidate.scene.pose_changes + } + for obj in derived.get( + "rigid_object", derived.get("simulation", {}).get("rigid_object", []) + ): + if obj["uid"] in changes: + pose = changes.pop(obj["uid"]) + obj["init_pos"] = pose[:3, 3].tolist() + obj["init_rot"] = ( + Rotation.from_matrix(pose[:3, :3]) + .as_euler("xyz", degrees=True) + .tolist() + ) + requested_positions[obj["uid"]] = obj["init_pos"] + if changes: + raise ValueError( + f"Unsupported scene variable entities: {list(changes)}" + ) + robot_uid = report["resolved_deployment"]["robot"]["uid"] + for uid, pose in candidate.deployment.poses.items(): + if uid != robot_uid: + raise ValueError("Scene variable selects a different robot") + pose = np.asarray(pose) + target = ( + derived["embodiment"].setdefault("overrides", {}) + if "embodiment" in derived + else derived["robot"] + ) + target["init_pos"] = pose[:3, 3].tolist() + target["init_rot"] = ( + Rotation.from_matrix(pose[:3, :3]) + .as_euler("xyz", degrees=True) + .tolist() + ) + deployment_path = root / "configs" / f"task-{index:03d}.yaml" + deployment_path.write_text(yaml.safe_dump(derived, sort_keys=False)) + case["deployment"] = str(deployment_path) + case["scene_atlas"] = None + config_path = root / "configs" / f"job-{index:03d}.yaml" + config_path.write_text(yaml.safe_dump(case, sort_keys=False)) + # Same public preflight and acceptance configuration as ordinary collection. + pinned = preflight_expansion_job(load_expansion_job(config_path)) + plan["records"].append( + dict( + semantic_id=pinned["semantic_id"], + scene_candidate_id=candidate.candidate_id if prepared else None, + index=index, + role=role, + unit=unit.tolist(), + values=values, + requested_positions=requested_positions, + job=str(config_path.relative_to(root)), + result=str(Path(case["output_dir"]).relative_to(root)), + status="unrun", + confirmed=0, + attempts=0, + planned=case["target_episodes"], + ) + ) + plan["planned_attempts"] = sum(r["planned"] for r in plan["records"]) + _atomic(root / "preview-plan.json", plan) + return plan + + +def _exact_recipe(metadata: Any) -> bool: + records = metadata.get("expansion", ()) + if any( + r.get("fallback_count", 0) + or ( + r.get("trajectory_requested") is not None + and r.get("trajectory_requested") != r.get("trajectory_selected") + ) + for r in records + ): + return False + if not any(r.get("affordance_requested") is not None for r in records): + return True + rows = {r["env_id"] for r in records if r.get("env_id") is not None} + env_id = metadata.get("source_env_id", next(iter(rows)) if len(rows) == 1 else None) + found = False + for segment in metadata.get("program_segments", ()): + for call in segment.get("metadata", {}).get("runtime", {}).get("calls", ()): + attempts = call.get("plan_attempts", ()) + if not attempts: + continue + sample = ( + attempts[-1] + .get("planner_diagnostics", {}) + .get("metadata", {}) + .get("affordance_sample", {}) + .get("grasp") + ) + if sample is None: + continue + ids = list(sample["env_ids"]) + index = ( + ids.index(env_id) + if env_id in ids + else 0 if env_id is None and len(ids) == 1 else None + ) + if ( + index is None + or not sample["success"][index] + or sample.get("reused", [True] * len(ids))[index] + ): + return False + found = True + return found + + +def collect_preview(plan: dict[str, Any]) -> dict[str, Any]: + root = Path(plan["output"]) + started = time.monotonic() + for record in plan["records"]: + remaining = plan["collection_budget_s"] - (time.monotonic() - started) + if remaining <= 0: + record["status"] = "budget_exhausted" + continue + job = root / record["job"] + cfg = yaml.safe_load(job.read_text()) + cfg["max_wall_time_s"] = min(cfg["max_wall_time_s"], remaining) + job.write_text(yaml.safe_dump(cfg, sort_keys=False)) + current = preflight_expansion_job(load_expansion_job(job)) + if current["semantic_id"] != record["semantic_id"]: + record["status"] = "source_changed" + _atomic(root / "preview-plan.json", plan) + raise ValueError("Preview inputs changed after planning; create a new plan") + remaining = plan["collection_budget_s"] - (time.monotonic() - started) + if remaining <= 0: + record["status"] = "budget_exhausted" + _atomic(root / "preview-plan.json", plan) + continue + record["status"] = "running" + _atomic(root / "preview-plan.json", plan) + print( + f"Preview sample {record['index']+1}/{len(plan['records'])}: {record['role']}", + flush=True, + ) + env = dict(os.environ) + env.setdefault("OMP_NUM_THREADS", "1") + env.setdefault("SDL_VIDEODRIVER", "offscreen") + with (root / "logs" / f"sample-{record['index']:03d}.log").open("w") as log: + process = subprocess.Popen( + [ + sys.executable, + "-m", + "embodichain", + "expansion", + "run", + "--job", + str(job), + ], + stdout=log, + stderr=subprocess.STDOUT, + env=env, + ) + timed_out = False + try: + process.wait(timeout=remaining) + except (subprocess.TimeoutExpired, KeyboardInterrupt) as error: + process.send_signal(signal.SIGINT) + try: + process.wait(timeout=10) + except subprocess.TimeoutExpired: + process.kill() + process.wait() + record["status"] = ( + "interrupted" + if isinstance(error, KeyboardInterrupt) + else "budget_exhausted" + ) + _atomic(root / "preview-plan.json", plan) + if isinstance(error, KeyboardInterrupt): + raise + timed_out = True + result = root / record["result"] + receipts = ArtifactEpisodeSink(result).inspect() if result.is_dir() else () + attempts = [ + json.loads(p.read_text()) + for p in (result / "attempts").glob("*/attempt.json") + ] + record.update( + confirmed=sum(r.confirmed for r in receipts), + attempts=len(attempts), + returncode=process.returncode, + outcomes=[ + { + "ordinal": a.get("ordinal"), + "status": a["status"], + "reason": a.get("reason") or a.get("error"), + } + for a in attempts + ], + ) + exact = 0 + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + if _exact_recipe(episode.metadata): + exact += 1 + record["exact_recipes"] = exact + record["status"] = ( + "budget_exhausted" + if timed_out + else ( + "confirmed" + if record["confirmed"] == record["planned"] + and exact == record["planned"] + else ( + "partial" + if record["confirmed"] + else "rejected" if process.returncode in (0, 2) else "error" + ) + ) + ) + _atomic(root / "preview-plan.json", plan) + plan["complete"] = all( + r["attempts"] >= r["planned"] + and r["status"] in {"confirmed", "partial", "rejected"} + for r in plan["records"] + ) + plan["coverage_complete"] = all(r["status"] == "confirmed" for r in plan["records"]) + plan["confirmed"] = sum(r["confirmed"] for r in plan["records"]) + plan["elapsed_s"] = time.monotonic() - started + _atomic(root / "preview-plan.json", plan) + return plan diff --git a/embodichain/lab/expansion/_preview_render.py b/embodichain/lab/expansion/_preview_render.py new file mode 100644 index 0000000000..4591772dee --- /dev/null +++ b/embodichain/lab/expansion/_preview_render.py @@ -0,0 +1,470 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Bounded native mesh replay without constructing a task or advancing physics.""" + +from __future__ import annotations + +from contextlib import contextmanager +from collections.abc import Iterator +from typing import Any +import io +import math +import numpy as np +from scipy.spatial.transform import Rotation +from PIL import Image +from ._preview_data import PreviewData, PreviewState, pose_matrix + +__all__: list[str] = [] + + +def prepare_geometry(data: PreviewData) -> tuple[list[str], np.ndarray]: + import torch + import pytorch_kinematics as pk + + chain = pk.build_chain_from_urdf(data.asset.read_bytes()) + names = chain.get_link_names() + joints = chain.get_joint_parameter_names() + if data.eef_link not in names: + raise ValueError(f"Unknown end-effector link: {data.eef_link}") + total = ( + sum(len(c.measured.timestamps) for c in data.all_clips) * len(names) * 16 * 4 + ) + if total > 256 * 1024 * 1024: + raise ValueError( + "Preview FK exceeds the 256 MiB budget; select smaller result directories" + ) + for c in data.all_clips: + m = c.measured + if set(joints) != set(m.joint_names): + raise ValueError( + "Recorded joints do not match the supplied URDF (including mimic joints)" + ) + order = [m.joint_names.index(n) for n in joints] + c.links = np.empty((len(m.timestamps), len(names), 4, 4), np.float32) + for start in range(0, len(m.timestamps), 256): + stop = min(len(m.timestamps), start + 256) + q = torch.tensor(m.qpos[start:stop][:, order], dtype=torch.float32) + with torch.no_grad(): + fk = chain.forward_kinematics(q) + roots = np.stack([pose_matrix(p) for p in m.root_poses[start:stop]]) + c.links[start:stop] = roots[:, None] @ np.stack( + [fk[n].get_matrix().numpy() for n in names], axis=1 + ) + c.tips = c.links[:, names.index(data.eef_link), :3, 3] + if not np.isfinite(c.links).all(): + raise ValueError("FK produced nonfinite poses") + points = np.concatenate([c.links[0, :, :3, 3] for c in data.all_clips]) + center = (points.min(axis=0) + points.max(axis=0)) / 2 + return names, center + + +class NativeRenderer: + def __init__( + self, + data: PreviewData, + sim: Any, + publisher: Any, + names: list[str], + center: np.ndarray, + models: dict[str, Any] | None = None, + geometry: dict[str, Any] | None = None, + ) -> None: + from dexsim import spawn + from embodichain.lab.visualization.markers import ( + RobotMarkers, + RobotMarkersCfg, + RobotMarkersAppearanceCfg, + VisualizationMarkers, + VisualizationMarkersCfg, + MarkerPrototypeCfg, + ) + + self.data, self.sim, self.publisher, self.center = data, sim, publisher, center + scene = sim.spawn_result + self.step = sim.physics_step_index + self.previous = None + self.models = models or {} + self.geometry = geometry or {} + self.backgrounds = { + uid + for uid, spec in (data.object_specs or {}).items() + if spec.get("body_type") in {"static", "kinematic"} + and all( + np.allclose( + c.measured.object_poses[uid], + data.clips[0].measured.object_poses[uid][0], + rtol=0, + atol=1e-7, + ) + for c in data.all_clips + ) + } + self.moving_objects = [uid for uid in data.boxes if uid not in self.backgrounds] + for uid in self.backgrounds: + self.models[uid].set_local_pose( + data.clips[0].measured.object_poses[uid][0:1] + ) + self.models[uid].set_visible(True) + palette = [ + (0.18, 0.58, 1), + (0.15, 0.9, 0.5), + (0.9, 0.55, 0.2), + (0.7, 0.4, 0.95), + ] + variants = sorted({c.variant for c in data.clips}) + self.colors = {v: palette[i % len(palette)] for i, v in enumerate(variants)} + for name, pos, intensity in [ + ("preview_key", [-1, -2, 3], 35), + ("preview_fill", [1, 1, 2], 18), + ]: + pose = np.eye(4, dtype=np.float32) + pose[:3, 3] = np.asarray(pos) + center + scene.add_light( + spawn.LightDesc( + name=name, light_type="POINT", pose=pose, intensity=intensity + ) + ) + self.robots = [] + self.objects = [] + + def markers(name: str, shapes: dict) -> Any: + return VisualizationMarkers( + VisualizationMarkersCfg(prim_path=name, markers=shapes), publisher + ) + + for i, c in enumerate(data.all_clips): + color = ( + self.colors.get(c.variant, (0.65, 0.7, 0.75)) + if i < len(data.clips) + else (0.65, 0.7, 0.75) + ) + robot = RobotMarkers( + RobotMarkersCfg( + name=f"preview_robot_{i}", + asset_path=str(data.asset), + appearance=RobotMarkersAppearanceCfg(color=color, opacity=0.18), + ), + publisher, + ) + self.robots.append((robot, [names.index(n) for n in robot.link_names])) + objects = ( + markers( + f"preview_objects_{i}", + { + uid: self._object_prototype(uid, color=(*color, 0.2)) + for uid in self.moving_objects + }, + ) + if self.moving_objects + else None + ) + self.objects.append(objects) + robot.visualize_link_poses( + c.links[0, [names.index(n) for n in robot.link_names]][None] + ) + robot.set_visibility(False) + if objects: + p = np.array( + [c.measured.object_poses[uid][0] for uid in self.moving_objects] + ) + objects.visualize(p[:, :3], p[:, 3:], marker_indices=np.arange(len(p))) + objects.set_visibility(False) + self.selected = RobotMarkers( + RobotMarkersCfg( + name="selected_model", + asset_path=str(data.asset), + appearance=RobotMarkersAppearanceCfg(mode="asset", opacity=1), + ), + publisher, + ) + self.selected_order = [names.index(n) for n in self.selected.link_names] + self.selected.visualize_link_poses( + data.clips[0].links[0, self.selected_order][None] + ) + self.selected_objects = ( + markers( + "selected_objects", + { + uid: self._object_prototype( + uid, color=(0.9, 0.58, 0.2, 1), scene_lighting=True, unlit=False + ) + for uid in self.moving_objects + }, + ) + if self.moving_objects and not self.models + else None + ) + self.paths = markers( + "measured_paths", + { + str(i): MarkerPrototypeCfg( + shape="cylinder", color=(*self.colors[v], 0.8) + ) + for i, v in enumerate(variants) + }, + ) + self.variant_ids = {v: i for i, v in enumerate(variants)} + self.targets = markers( + "recorded_grasp_targets", + {"target": MarkerPrototypeCfg(shape="frame", scale=(0.065, 0.065, 0.065))}, + ) + self.positions = markers( + "initial_positions", + { + "point": MarkerPrototypeCfg( + shape="sphere", scale=(0.014, 0.014, 0.014), color=(1, 0.7, 0.1, 1) + ) + }, + ) + self.planned = markers( + "planned_parameter_positions", + { + "confirmed": MarkerPrototypeCfg( + shape="sphere", + scale=(0.018, 0.018, 0.018), + color=(0.3, 0.8, 0.4, 0.4), + ), + "unqualified": MarkerPrototypeCfg( + shape="sphere", + scale=(0.022, 0.022, 0.022), + color=(1, 0.25, 0.15, 0.65), + ), + }, + ) + if data.coverage: + points, types = [], [] + for record in data.coverage["records"]: + for point in record.get("requested_positions", {}).values(): + points.append(point) + types.append(0 if record["status"] == "confirmed" else 1) + self.planned.visualize( + np.asarray(points).reshape(-1, 3), + marker_indices=np.asarray(types, dtype=int), + ) + self.camera = scene.add_camera( + spawn.CameraDesc(name="expansion_preview_camera", resolution=(1100, 760)) + ) + self.camera.open_camera() + self.camera.set_visual_overlay_enabled(True) + publisher.publish(preload=True) + + def _object_prototype(self, uid: str, **appearance: Any) -> Any: + from embodichain.lab.visualization.markers import MarkerPrototypeCfg + + if uid in self.geometry: + vertices, faces = self.geometry[uid] + return MarkerPrototypeCfg( + shape="mesh", vertices=vertices, faces=faces, **appearance + ) + return MarkerPrototypeCfg(shape="box", scale=self.data.boxes[uid], **appearance) + + def draw(self, state: PreviewState) -> bytes: + v = state.values + visible = state.visible() + if tuple(visible) != self.previous: + positions = [] + rotations = [] + scales = [] + types = [] + for i in visible: + c = self.data.clips[i] + points = c.tips[ + np.unique(np.r_[np.arange(0, len(c.tips), 4), len(c.tips) - 1]) + ] + for a, b in zip(points[:-1], points[1:]): + delta = b - a + length = np.linalg.norm(delta) + if length < 1e-6: + continue + z = delta / length + axis = np.cross([0, 0, 1], z) + norm = np.linalg.norm(axis) + rotation = ( + Rotation.from_rotvec( + axis / norm * np.arccos(np.clip(z[2], -1, 1)) + ) + if norm > 1e-8 + else Rotation.from_rotvec([np.pi if z[2] < 0 else 0, 0, 0]) + ) + positions.append((a + b) / 2) + rotations.append(rotation.as_quat()) + scales.append([0.002, 0.002, length]) + types.append(self.variant_ids[c.variant]) + self.paths.visualize( + np.array(positions).reshape(-1, 3), + np.array(rotations).reshape(-1, 4), + np.array(scales).reshape(-1, 3), + np.asarray(types, dtype=int), + ) + targets = ( + np.concatenate([self.data.clips[i].targets for i in visible]) + if visible + else np.empty((0, 4, 4)) + ) + self.targets.visualize( + targets[:, :3, 3], + ( + Rotation.from_matrix(targets[:, :3, :3]).as_quat() + if len(targets) + else np.empty((0, 4)) + ), + ) + initial = [ + p[0, :3] + for i in visible + for uid, p in self.data.clips[i].measured.object_poses.items() + if uid not in self.backgrounds + ] + self.positions.visualize(np.asarray(initial).reshape(-1, 3)) + self.previous = tuple(visible) + self.selected.set_visibility(bool(visible)) + if self.selected_objects: + self.selected_objects.set_visibility(bool(visible)) + for uid, obj in self.models.items(): + if uid not in self.backgrounds: + obj.set_visible(bool(visible)) + if visible: + c = self.data.clips[v["selected"]] + f = state.frame_at(c) + self.selected.visualize_link_poses(c.links[f, self.selected_order][None]) + for uid in self.moving_objects: + if uid in self.models: + self.models[uid].set_local_pose( + c.measured.object_poses[uid][f : f + 1] + ) + if self.selected_objects: + p = np.array( + [c.measured.object_poses[uid][f] for uid in self.moving_objects] + ) + self.selected_objects.visualize( + p[:, :3], p[:, 3:], marker_indices=np.arange(len(p)) + ) + ghost_count = 0 + for i, (c, (robot, order), objects) in enumerate( + zip(self.data.all_clips, self.robots, self.objects) + ): + show = ( + v["reference"] + if i == len(self.data.clips) + else v["ghosts"] and i in visible and i != v["selected"] + ) + robot.set_visibility(show) + if objects: + objects.set_visibility(show) + if show: + ghost_count += 1 + f = state.frame_at(c) + robot.visualize_link_poses(c.links[f, order][None]) + if objects: + p = np.array( + [c.measured.object_poses[uid][f] for uid in self.moving_objects] + ) + objects.visualize( + p[:, :3], p[:, 3:], marker_indices=np.arange(len(p)) + ) + self.planned.set_visibility(v["positions"] and v["space"] == "all") + self.paths.set_visibility(v["paths"]) + self.targets.set_visibility(v["targets"]) + self.positions.set_visibility(v["positions"]) + self.publisher.publish() + yaw = v["yaw"] + e = v["elevation"] + d = v["distance"] + eye = self.center + d * np.array( + [math.cos(e) * math.cos(yaw), math.cos(e) * math.sin(yaw), math.sin(e)] + ) + self.camera.look_at(eye.tolist(), self.center.tolist(), [0, 0, 1]) + with self.sim.render_frame(capture_camera_images=False): + self.sim.sync_render_state() + self.camera.render() + if self.sim.physics_step_index != self.step: + raise RuntimeError("Preview unexpectedly advanced physics") + image = Image.fromarray( + np.ascontiguousarray(self.camera.get_rgb_map()[..., :3], dtype=np.uint8) + ) + buffer = io.BytesIO() + image.save(buffer, format="JPEG", quality=88) + self.stats = { + "physics_steps": self.sim.physics_step_index - self.step, + "ghost_count": ghost_count, + "path_segments": self.paths.count if v["paths"] else 0, + "grasp_targets": self.targets.count if v["targets"] else 0, + "scene_objects": len(self.data.boxes), + "fixed_backgrounds": len(self.backgrounds), + } + return buffer.getvalue() + + +@contextmanager +def native_renderer(data: PreviewData, gpu: int = 0) -> Iterator[NativeRenderer]: + import os + + os.environ.setdefault("SDL_VIDEODRIVER", "offscreen") + import torch + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import RenderCfg, DenoisingCfg + from embodichain.lab.visualization.markers import MarkerFramePublisher + + torch.set_num_threads(1) + names, center = prepare_geometry(data) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cpu", + gpu_id=gpu, + num_envs=1, + enable_entity_gizmo=False, + startup_summary="off", + render_cfg=RenderCfg( + renderer="hybrid", + spp=4, + denoising=DenoisingCfg(window="off", offscreen="optix"), + ), + ) + ) + publisher = None + try: + sim.enable_physics(False) + models = {} + if data.object_specs is not None: + from embodichain.lab.sim.cfg import VisualObjectCfg + + for index, (uid, spec) in enumerate(data.object_specs.items()): + visual = { + key: value + for key, value in spec.items() + if key in {"shape", "source", "body_scale"} and value is not None + } + visual.update(uid=f"expansion_preview_asset_{index}", visible=False) + models[uid] = sim.add_visual_object(VisualObjectCfg.from_dict(visual)) + sim.prepare() + sim.set_ground_plane_visibility(data.ground_visible) + geometry = {} + for uid, obj in models.items(): + vertices = obj.get_vertices(env_ids=[0], scale=True)[0].cpu().numpy() + faces = obj.get_triangles(env_ids=[0])[0].cpu().numpy() + if not len(vertices) or not len(faces) or not np.isfinite(vertices).all(): + raise ValueError(f"Recorded object has no finite visual mesh: {uid}") + geometry[uid] = vertices, faces + data.boxes[uid] = tuple(np.ptp(vertices, axis=0)) + publisher = MarkerFramePublisher(sim) + yield NativeRenderer(data, sim, publisher, names, center, models, geometry) + finally: + if publisher is not None: + publisher.close() + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() diff --git a/embodichain/lab/expansion/_preview_server.py b/embodichain/lab/expansion/_preview_server.py new file mode 100644 index 0000000000..fa8392b292 --- /dev/null +++ b/embodichain/lab/expansion/_preview_server.py @@ -0,0 +1,212 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Loopback-only native preview transport. WebRTC integration remains a TODO.""" + +from __future__ import annotations + +import base64 +import json +import queue +import threading +import time +import webbrowser +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +from typing import Any +from urllib.parse import urlsplit +from ._preview_data import PreviewData, PreviewState + +__all__: list[str] = [] + + +class PreviewTransport: + def __init__(self, state: PreviewState, port: int) -> None: + self.state = state + self.commands: queue.Queue[dict[str, Any]] = queue.Queue(maxsize=64) + self.lock = threading.Lock() + self.status: dict[str, Any] = {} + self.jpeg = b"" + self.snapshot = b"{}" + html = Path(__file__).with_name("_preview.html").read_bytes() + owner = self + + class Handler(BaseHTTPRequestHandler): + def setup(self) -> None: + super().setup() + self.connection.settimeout(3) + + def log_message(self, *_: object) -> None: + pass + + def do_GET(self) -> None: + if urlsplit("//" + self.headers.get("Host", "")).hostname not in { + "127.0.0.1", + "localhost", + "::1", + }: + self.send_error(403) + return + route = self.path.split("?")[0] + with owner.lock: + if route == "/": + data, kind = html, "text/html; charset=utf-8" + elif route == "/state": + data, kind = ( + json.dumps(owner.status, allow_nan=False).encode(), + "application/json", + ) + elif route == "/snapshot": + data, kind = owner.snapshot, "application/json" + elif route == "/frame.jpg": + data, kind = owner.jpeg, "image/jpeg" + else: + self.send_error(404) + return + self.send_response(200) + self.send_header("Content-Type", kind) + self.send_header("Cache-Control", "no-store") + self.send_header("X-Content-Type-Options", "nosniff") + self.send_header("Content-Length", str(len(data))) + self.end_headers() + try: + self.wfile.write(data) + except (BrokenPipeError, ConnectionResetError): + pass + + def do_POST(self) -> None: + if self.path != "/control": + self.send_error(404) + return + origin = self.headers.get("Origin") + host = self.headers.get("Host", "") + # Same-origin loopback controls also work through a different forwarded port. + if ( + urlsplit("//" + host).hostname + not in {"127.0.0.1", "localhost", "::1"} + or self.headers.get("Content-Type") != "application/json" + or (origin and origin != "http://" + host) + ): + self.send_error(403) + return + try: + size = int(self.headers.get("Content-Length", "0")) + if not 0 < size <= 4096: + raise ValueError("control size") + update = owner.state.validate(json.loads(self.rfile.read(size))) + owner.commands.put_nowait(update) + except (ValueError, TypeError, queue.Full, TimeoutError): + self.send_error(400) + return + self.send_response(204) + self.end_headers() + + self.server = ThreadingHTTPServer(("127.0.0.1", port), Handler) + self.server.daemon_threads = True + self.url = f"http://127.0.0.1:{self.server.server_port}/" + + def publish(self, status: dict[str, Any], jpeg: bytes) -> None: + snapshot = json.dumps( + { + **status, + "image": "data:image/jpeg;base64," + base64.b64encode(jpeg).decode(), + }, + allow_nan=False, + ).encode() + with self.lock: + self.status = status + self.jpeg = jpeg + self.snapshot = snapshot + + +def serve_preview( + data: PreviewData, + *, + port: int = 8769, + gpu: int = 0, + open_browser: bool = False, + duration: float = 0, +) -> None: + from ._preview_render import native_renderer + + state = PreviewState(data) + transport = PreviewTransport(state, port) + thread = None + try: + with native_renderer(data, gpu) as renderer: + frame = 0 + + def publish() -> None: + nonlocal frame + jpeg = renderer.draw(state) + transport.publish( + { + **state.snapshot(), + **renderer.stats, + "frame": frame, + "renderer": "EmbodiChain / DexSim Hybrid + OptiX", + "transport": "HTTP JPEG", + }, + jpeg, + ) + frame += 1 + + publish() + thread = threading.Thread( + target=transport.server.serve_forever, daemon=True + ) + thread.start() + print( + json.dumps( + { + "state": "ready", + "url": transport.url, + "episodes": len(data.clips), + "physics_steps": 0, + "transport": "HTTP JPEG", + } + ), + flush=True, + ) + if open_browser: + webbrowser.open(transport.url) + start = last = time.monotonic() + while not duration or time.monotonic() - start < duration: + now = time.monotonic() + elapsed = now - last + last = now + dirty = False + while True: + try: + state.apply(transport.commands.get_nowait()) + dirty = True + except queue.Empty: + break + if state.values["playing"]: + state.values["progress"] = min( + 1.0, state.values["progress"] + elapsed / state.duration + ) + if state.values["progress"] >= 1: + state.values["playing"] = False + dirty = True + if dirty: + publish() + time.sleep(max(0.01, 0.1 - (time.monotonic() - now))) + finally: + if thread is not None: + transport.server.shutdown() + thread.join(timeout=5) + transport.server.server_close() diff --git a/embodichain/lab/expansion/_robot_first_sampling.py b/embodichain/lab/expansion/_robot_first_sampling.py new file mode 100644 index 0000000000..34076263ee --- /dev/null +++ b/embodichain/lab/expansion/_robot_first_sampling.py @@ -0,0 +1,146 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Owned workspace priors for conditional layout completion.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from typing import TYPE_CHECKING, Any + +import numpy as np + +from embodichain.lab.sim.scene.expansion import SceneLayoutCondition +from embodichain.lab.sim.scene.feasibility.proposals import _workspace_fk_samples + +from ._pose_sampling_cfg import _AnchorTarget + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.scene.expansion import PreparedSceneRandomization + from embodichain.lab.sim.scene.feasibility import FeasibilityBudget + +__all__: list[str] = [] + + +@dataclass(frozen=True) +class _AnchorPool: + selection: _AnchorTarget + base: np.ndarray + buckets: tuple[np.ndarray, ...] + + def condition( + self, visit: int, fixed: SceneLayoutCondition + ) -> SceneLayoutCondition | None: + """Cycle over occupied cells and shuffle within each cell once per job.""" + if not self.buckets: + return None + cycle, cell = divmod(visit, len(self.buckets)) + points = self.buckets[cell] + pose = self.base.copy() + pose[:2, 3] = points[cycle % len(points)] + return SceneLayoutCondition({**fixed.poses, self.selection.root_uid: pose}) + + def metrics(self) -> dict[str, object]: + return { + "phase_id": self.selection.phase_id, + "control_part": self.selection.target.control_part, + "root_uid": self.selection.root_uid, + "samples_in_domain": sum(map(len, self.buckets)), + "occupied_xy_cells": len(self.buckets), + } + + +def _prepare_anchor_pools( + robot: Robot, + budget: FeasibilityBudget, + prepared: PreparedSceneRandomization, + config: Mapping[str, Any], + selections: Sequence[_AnchorTarget], + scene: Mapping[str, Any], + nominal: Mapping[str, np.ndarray], + *, + seed: int, +) -> tuple[_AnchorPool, ...]: + """Precompute at most sample_count FK rows per arm, retaining only legal XY.""" + policy = prepared.policy + movable_roots = { + root + for root, parent in prepared.source.parent_by_uid.items() + if parent == prepared.source.support_uid and root in prepared.movable_uids + } + nodes_by_part: dict[str, list[tuple[int, _AnchorTarget]]] = {} + for index, selection in enumerate(selections): + root = selection.root_uid + if root not in movable_roots or root not in scene or "follow" in scene[root]: + raise ValueError( + f"Robot anchor root {root!r} must have independent XY bounds" + ) + nodes_by_part.setdefault(selection.target.control_part, []).append( + (index, selection) + ) + + pools = {} + for part, entries in nodes_by_part.items(): + part_index = entries[0][0] + fk = _workspace_fk_samples( + robot, + budget, + control_part=part, + source=config["workspace_source"], + sample_count=config["sample_count"], + seed=seed + part_index, + ) + for index, selection in entries: + root, target = selection.root_uid, selection.target + base = np.array(prepared.source.poses[root], dtype=float) + bounds = np.asarray(scene[root]["translation_m"], dtype=float)[:2].T + bounds = bounds + np.asarray(nominal[root])[:2, 3] + offsets = policy["group_xy_offset_bounds_m"].get(root) + if offsets is not None: + layout_bounds = base[:2, 3] + np.asarray(offsets) + bounds[0] = np.maximum(bounds[0], layout_bounds[0]) + bounds[1] = np.minimum(bounds[1], layout_bounds[1]) + lower, upper = bounds + if np.any(upper < lower): + raise ValueError(f"Robot anchor root {root!r} has disjoint XY bounds") + + # Preserve sampled TCP XY under the root's allowed rotation/height. + # Sampled TCP height/orientation are not retained by this projection. + frame = np.asarray(prepared.source.poses[target.frame_uid]) + root_from_tcp = np.linalg.solve(base, frame @ np.asarray(target.pose)) + xy = fk[:, :2, 3] - (base[:3, :3] @ root_from_tcp[:3, 3])[:2] + xy[:, lower == upper] = lower[lower == upper] + xy = xy[np.all((lower <= xy) & (xy <= upper), axis=1)] + # Balance across occupied cells, so a dense set of joint samples in + # one region cannot dominate every proposal. No extrapolation. + size = budget.cfg.archive_bins + scaled = np.divide( + xy - lower, upper - lower, out=np.zeros_like(xy), where=upper > lower + ) + cells = np.minimum((scaled * size).astype(int), size - 1) + occupied = cells[:, 0] * size + cells[:, 1] + rng = np.random.default_rng(np.random.SeedSequence([seed, index])) + buckets = [] + for cell in rng.permutation(np.unique(occupied)): + points = rng.permutation(xy[occupied == cell]) + points.setflags(write=False) + buckets.append(points) + base.setflags(write=False) + pools[index] = _AnchorPool(selection, base, tuple(buckets)) + budget.check() + return tuple(pools[index] for index in range(len(selections))) diff --git a/embodichain/lab/expansion/_scene_first_sampling.py b/embodichain/lab/expansion/_scene_first_sampling.py new file mode 100644 index 0000000000..ff94f5d5a2 --- /dev/null +++ b/embodichain/lab/expansion/_scene_first_sampling.py @@ -0,0 +1,396 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Conditional layout proposals with shared full task screening.""" + +from __future__ import annotations + +from dataclasses import replace +import math +import time +import numpy as np + +from ._pose_sampling import _AdaptivePoseSource +from ._pose_sampling_cfg import _ROBOT_LAYOUT_STRATEGIES, _anchor_targets +from ._robot_first_sampling import _prepare_anchor_pools +from embodichain.lab.sim.scene.expansion import ( + SceneLayoutCondition, + scene_layout_input_from_sim, + prepare_scene_randomization, + generate_scene_variants, + ScenePoseChange, + SceneVariant, +) +from embodichain.lab.sim.scene.expansion.conditions import _LayoutConditionDomainError + +__all__: list[str] = [] + + +class _LayoutPoseSource(_AdaptivePoseSource): + """Generate or certify complete layouts before checking task branches. + + The numerical layout source owns no World. The existing task backend owns + workspace seed selection, full-state IK and collision-checked path screening. + Runtime execution still replans and independently validates measured outcomes. + """ + + def __init__(self, env, options, *, seed, revision): + before = self._world_clock(env.sim) + self._collision = None + super().__init__(env, options, seed=seed, revision=revision) + for key, value in self.cfg["screening"].items(): + setattr(self.budget.cfg, key, value) + self.budget.cfg.__post_init__() + layout = self.cfg["layout"] + source = scene_layout_input_from_sim( + env.sim, + support_uid=layout["support_uid"], + parent_by_uid=layout["parent_by_uid"], + movable_entity_ids=tuple( + uid for uid in options["scene"] if uid in layout["parent_by_uid"] + ), + scene_id=revision, + ) + self._prepared_layout = prepare_scene_randomization(source, layout["policy"]) + self._fixed_layout = { + uid: ScenePoseChange(uid, source.poses[uid]) + for uid in self.configuration + if uid in source.poses and uid not in self._prepared_layout.movable_uids + } + self._conditions = SceneLayoutCondition(layout.get("conditions", {})) + # Compile/check condition syntax and permissions before any row is reset. + from embodichain.lab.sim.scene.expansion.conditions import _condition_prepared + + _condition_prepared(self._prepared_layout, self._conditions) + nodes = self._prepared_layout._geometry["nodes"] + for n in nodes: + if n["uid"] in options["scene"] and n["parent"] < 0: + bounds = options["scene"][n["uid"]] + if "follow" not in bounds and ( + bounds["yaw_rad"] != [0, 0] or bounds["translation_m"][2] != [0, 0] + ): + raise ValueError( + "Root layout sampling supports XY only; put member yaw in a declared intra-group domain" + ) + from embodichain.lab.sim.scene.feasibility import request_from_mapping + + entities = { + uid: obj.get_local_pose(to_matrix=True)[0].detach().cpu().double().tolist() + for uid in env.sim.get_rigid_object_uid_list() + if (obj := env.sim.get_rigid_object(uid)) is not None + } + entities[env.robot.uid] = self.roots[env.robot.uid] + if getattr(env.sim, "_default_plane", None) is not None: + entities["default_plane"] = np.eye(4).tolist() + request = request_from_mapping( + dict( + task_id=revision, + robot_profile_id=env.robot.uid, + policy_id=revision, + validator_id=revision, + parent_scene_id=revision, + entity_poses=entities, + variables=[], + modes=[self.cfg["task_mode"]], + ) + ) + self._mode = request.modes[0] + for phase in self._mode.phases: + for target in phase.targets: + if ( + target.control_part not in env.robot.control_parts + or env.robot.get_solver(name=target.control_part) is None + ): + raise ValueError( + f"Task phase names unavailable control part {target.control_part!r}" + ) + self._entities = {k: np.array(v) for k, v in entities.items()} + self._initial_qpos = None + self._anchor_pools = () + self._workspace_seconds = self._screening_seconds = 0.0 + if self.cfg["strategy"] in _ROBOT_LAYOUT_STRATEGIES: + started = time.perf_counter() + selections = _anchor_targets( + self._mode, + layout, + self._prepared_layout.policy, + self.cfg["robot_anchors"], + ) + self._anchor_pools = _prepare_anchor_pools( + env.robot, + self.budget, + self._prepared_layout, + self.cfg, + selections, + self.configuration, + self.poses, + seed=seed, + ) + self._workspace_seconds = time.perf_counter() - started + + from embodichain.lab.task_program.integrations.simulation.feasibility_geometry import ( + collision_bodies_from_simulation, + ) + from .acceptance import _GeometryRow + from embodichain.lab.sim.scene.feasibility import NewtonCollisionChecker + + bodies = collision_bodies_from_simulation(env) + known = {b.frame_uid for b in bodies} + ignored, _ = _GeometryRow.contact_permissions(env, options["validation"], known) + try: + self._collision = NewtonCollisionChecker( + bodies, ignored_pairs=ignored, device="cpu", max_shapes=512 + ) + self._check_clock(env.sim, before) + except BaseException: + self.close() + raise + self._scene_identity = ( + (id(env.sim.spawn_result), env.sim.spawn_result.topology_revision) + if hasattr(env.sim, "spawn_result") + else None + ) + self._task_checks = {} + self._anchor_checks = {} + + def bind_program(self, adapter, program): + """Keep the declared phase graph and live runtime independent authorities.""" + if not tuple(program.iter_segments()): + raise ValueError("Layout sampling requires a nonempty task program") + # The owner binds every group before rolling admission. Retain its + # nominal planning start even while another row subsequently executes. + self._initial_qpos = ( + self.env.robot.get_qpos()[0].detach().cpu().double().numpy().copy() + ) + + @staticmethod + def _world_clock(sim): + return getattr(sim, "physics_step_index", None), getattr( + sim, "simulation_time", None + ) + + @classmethod + def _check_clock(cls, sim, before): + if before != cls._world_clock(sim): + raise RuntimeError("Scene proposal screening advanced the owning World") + + def _offer_origin(self, raw): + if self.cfg["strategy"] == "bidirectional": + return "robot_first" if (raw + self.seed) % 2 else "scene_first" + return super()._offer_origin(raw) + + def _offer(self, candidate, rng, raw): + sim = self.env.sim + if self._scene_identity is not None and self._scene_identity != ( + id(sim.spawn_result), + sim.spawn_result.topology_revision, + ): + raise RuntimeError( + "Prepared layout geometry is stale after a topology change" + ) + before = self._world_clock(sim) + started = time.perf_counter() + try: + return self._offer_layout(candidate, raw, self._offer_origin(raw)) + finally: + self._screening_seconds += time.perf_counter() - started + self._check_clock(sim, before) + self._task_checks.setdefault(raw, []).append( + dict( + stage="screening.clock", + status="passed", + reason="Owning World step/time unchanged", + phase_id=None, + ) + ) + + def _offer_layout(self, candidate, raw, origin): + if origin != "bounded_pose": + conditions = self._conditions + if origin == "robot_first": + ordinal = raw // 2 if self.cfg["strategy"] == "bidirectional" else raw + visit, index = divmod(ordinal, len(self._anchor_pools)) + pool = self._anchor_pools[index] + conditions = pool.condition(visit, conditions) + self._anchor_checks[raw] = { + "root_uid": pool.selection.root_uid, + "phase_id": pool.selection.phase_id, + "control_part": pool.selection.target.control_part, + "pose": ( + None + if conditions is None + else conditions.poses[pool.selection.root_uid] + ), + } + if conditions is None: + return candidate, origin, False, "no_workspace_anchor" + batch = generate_scene_variants( + self._prepared_layout, + seed=self.seed, + candidate_count=1, + max_attempts=1, + start_ordinal=raw, + conditions=conditions, + ) + if not batch.variants: + return candidate, origin, False, "layout_budget_exhausted" + generated = batch.variants[0] + # References outside the support tree (e.g. a numerical goal) + # retain their declared independent bounded sampling. + changes = { + c.entity_uid: self._fixed_layout.get(c.entity_uid, c) + for c in candidate.scene.pose_changes + } + changes.update({c.entity_uid: c for c in generated.pose_changes}) + for uid, options in self.configuration.items(): + if ( + uid not in self._prepared_layout.source.poses + and "follow" in options + ): + parent = options["follow"] + pose = self._follower_pose( + uid, parent, np.asarray(changes[parent].arena_pose) + ) + changes[uid] = ScenePoseChange(uid, pose) + candidate = replace( + candidate, + scene=SceneVariant( + candidate.scene.parent_scene_id, + self.seed, + raw, + tuple(changes.values()), + generated.metadata, + ), + ) + else: + # Equal downstream geometric/kinematic gates for the simple baseline. + for change in candidate.scene.pose_changes: + fixed = self._fixed_layout.get(change.entity_uid) + if fixed is not None and not np.allclose( + change.arena_pose, fixed.arena_pose, rtol=0, atol=1e-9 + ): + return candidate, origin, False, "layout_moves_fixed_entity" + anchors = { + c.entity_uid: c.arena_pose + for c in candidate.scene.pose_changes + if c.entity_uid in self._prepared_layout.movable_uids + } + for uid, pose in self._conditions.poses.items(): + actual = anchors.get(uid, self._prepared_layout.source.poses[uid]) + if not np.allclose(actual, pose, rtol=0, atol=1e-9): + return candidate, "bounded_pose", False, "condition_mismatch" + try: + batch = generate_scene_variants( + self._prepared_layout, + seed=self.seed, + candidate_count=1, + max_attempts=1, + start_ordinal=raw, + conditions=SceneLayoutCondition(anchors), + ) + except _LayoutConditionDomainError: + return candidate, origin, False, "layout_outside_sampled_domain" + if not batch.variants: + return candidate, "bounded_pose", False, "layout_budget_exhausted" + poses = { + **self._entities, + **{ + c.entity_uid: np.array(c.arena_pose) + for c in candidate.scene.pose_changes + }, + **{uid: np.array(pose) for uid, pose in candidate.deployment.poses.items()}, + } + # Never expand explicit task-facing ranges while applying layout policy. + for c in candidate.scene.pose_changes: + opts = self.configuration[c.entity_uid] + if "follow" in opts: + continue + delta = np.asarray(c.arena_pose)[:3, 3] - self.poses[c.entity_uid][:3, 3] + bounds = np.asarray(opts["translation_m"]) + rotation = np.asarray(c.arena_pose)[:3, :3] @ np.linalg.inv( + self.poses[c.entity_uid][:3, :3] + ) + yaw = math.atan2(rotation[1, 0], rotation[0, 0]) + low, high = opts["yaw_rad"] + yaw += 2 * math.pi * round(((low + high) / 2 - yaw) / (2 * math.pi)) + if ( + np.any(delta < bounds[:, 0] - 1e-6) + or np.any(delta > bounds[:, 1] + 1e-6) + or yaw < low - 1e-6 + or yaw > high + 1e-6 + ): + return ( + candidate, + origin, + False, + "layout_outside_task_domain", + ) + from embodichain.lab.sim.scene.feasibility import ( + RobotFeasibilityBackend, + plan_task_mode, + ) + + backend = RobotFeasibilityBackend( + self.env.robot, + self._collision, + self.budget.cfg, + poses, + seed=self.seed + raw, + initial_qpos=self._initial_qpos, + ) + plan = plan_task_mode(self._mode, backend, poses, self.budget) + self._task_checks[raw] = [ + { + "stage": c.stage, + "status": c.status, + "reason": c.reason, + "phase_id": c.phase_id, + } + for c in plan.checks + ] + reason = ( + "passed" + if plan.accepted + else next( + (c.reason for c in plan.checks if c.status != "passed"), + "task_path_unverified", + ) + ) + return candidate, origin, plan.accepted, reason + + def checkpoint(self): + super().checkpoint() + if self.output is not None and hasattr(self, "_task_checks"): + from embodichain.data_pipeline.recording.media import _atomic_json + + _atomic_json( + self.output / "scene-first-checks.json", + dict( + phase_checks=self._task_checks, + layout_source=self._prepared_layout.source.fingerprint, + conditions=self._conditions.to_dict(), + workspace_prior=[pool.metrics() for pool in self._anchor_pools], + workspace_anchor_checks=self._anchor_checks, + workspace_prepare_wall_s=self._workspace_seconds, + proposal_screen_wall_s=self._screening_seconds, + ), + ) + + def close(self): + """Release only query resources, without modifying the owning World.""" + if self._collision is not None: + self._collision.close() + self._collision = None diff --git a/embodichain/lab/expansion/_task_program_config.py b/embodichain/lab/expansion/_task_program_config.py new file mode 100644 index 0000000000..40ff525a1a --- /dev/null +++ b/embodichain/lab/expansion/_task_program_config.py @@ -0,0 +1,379 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Closed scene/motion declarations for native Task Program workloads.""" + +from __future__ import annotations +from pathlib import Path +from typing import Any +import math +import json +import yaml + +__all__: list[str] = [] + + +def load_mapping(path: str | Path) -> dict[str, Any]: + """Read declarations without importing the simulator or its logging hooks.""" + path = Path(path) + text = path.read_text(encoding="utf-8") + value = json.loads(text) if path.suffix == ".json" else yaml.safe_load(text) + if not isinstance(value, dict): + raise ValueError(f"Expected a mapping in {path}") + return value + + +OPERATORS = ( + "object_pose", + "affordance", + "joint_residual", + "via_points", + "nullspace_residual", +) + + +def load_expansion_viewer(value: dict | None = None) -> dict: + """Resolve presentation options without importing the native simulator. + + Args: + value: Optional headless and renderer declarations. + Returns: + Owned normalized options; unsupported values reject. + """ + if value is None: + value = {} + if not isinstance(value, dict) or set(value) - {"headless", "renderer"}: + raise ValueError("Expansion viewer accepts only headless and renderer") + headless, renderer = value.get("headless", True), value.get("renderer", "rt") + if type(headless) is not bool: + raise ValueError("Expansion viewer.headless must be a boolean") + if renderer not in ("rt", "fast-rt", "hybrid"): + raise ValueError("Expansion viewer.renderer must be rt, fast-rt or hybrid") + return {"headless": headless, "renderer": renderer} + + +def load_task_program_expansion(path: str | Path) -> dict[str, Any]: + """Load bounded scene/motion declarations while retaining physical criteria. + + Args: + path: YAML/JSON engine, scene, trajectory and validation declarations. + Returns: + Owned normalized declaration, including workload capacity. + Raises: + ValueError: For invalid scopes, protected phases, budgets or criteria. + """ + path = Path(path) + value = load_mapping(path) + if (set(value) - {"scene_sampling", "scheduling", "pose_coverage"}) != { + "schema_version", + "engine", + "scene", + "trajectory", + "validation", + } or value["schema_version"] != 1: + raise ValueError("Invalid demo expansion configuration") + engine, motion = value["engine"], value["trajectory"] + required_engine = { + "episodes_per_operator", + "max_attempts", + "max_steps", + "max_wall_time_s", + "settling_steps", + "capture_media", + } + if required_engine - set(engine) or set(engine) - required_engine - { + "cpu_threads", + "operator_workers", + "workload_count", + "batch_size", + "collision_device", + "grasp_nms_chunk_size", + "collection_renderer", + "collision_backend", + "reuse_idle_rows", + "nominal_seed", + }: + raise ValueError("Unknown or missing expansion engine settings") + engine.setdefault("cpu_threads", 1) + if "operator_workers" in engine: + if "workload_count" in engine: + raise ValueError( + "Use engine.workload_count without legacy operator_workers" + ) + # Compatibility is a capacity migration, never a process fallback. + engine["workload_count"] = engine.pop("operator_workers") + engine.setdefault("workload_count", 1) + engine.setdefault("batch_size", 1) + engine.setdefault("collision_device", "cpu") + engine.setdefault("grasp_nms_chunk_size", 2048) + engine.setdefault("collection_renderer", "auto") + engine.setdefault("collision_backend", "newton") + if "reuse_idle_rows" in engine and type(engine["reuse_idle_rows"]) is not bool: + raise ValueError("engine.reuse_idle_rows must be a boolean") + if "nominal_seed" in engine and ( + type(engine["nominal_seed"]) is not int or engine["nominal_seed"] < 0 + ): + raise ValueError("engine.nominal_seed must be a nonnegative integer") + if engine["collection_renderer"] not in ("auto", "rt", "fast-rt", "hybrid"): + raise ValueError( + "engine.collection_renderer must be auto, rt, fast-rt or hybrid" + ) + if engine["collision_backend"] not in ("newton", "native_contacts"): + raise ValueError("engine.collision_backend must be newton or native_contacts") + for name in ( + "episodes_per_operator", + "max_attempts", + "max_steps", + "settling_steps", + "cpu_threads", + "workload_count", + "batch_size", + "grasp_nms_chunk_size", + ): + if type(engine[name]) is not int or engine[name] < 1: + raise ValueError(f"engine.{name} must be a positive integer") + device = engine["collision_device"] + if not isinstance(device, str) or not ( + device == "cpu" + or device == "cuda" + or (device.startswith("cuda:") and device[5:].isdigit()) + ): + raise ValueError("engine.collision_device must be cpu, cuda or cuda:") + if ( + engine["max_attempts"] < engine["episodes_per_operator"] + or type(engine["capture_media"]) is not bool + ): + raise ValueError("Invalid expansion attempt or media settings") + required = { + "operators", + "joint_offset_scale", + "via_count", + "max_proposals", + "minimum_joint_delta", + "velocity_limit", + "acceleration_limit", + "nullspace_task_rows", + "nullspace_translation_tolerance_m", + "nullspace_rotation_tolerance_rad", + "free_phases", + } + if required - set(motion) or set(motion) - required - { + "affordance", + "call_indices", + }: + raise ValueError("Unknown or missing trajectory settings") + calls = motion.get("call_indices") + if calls is not None and ( + not isinstance(calls, list) + or not calls + or any(type(index) is not int or index < 0 for index in calls) + or len(set(calls)) != len(calls) + ): + raise ValueError( + "Trajectory call_indices must be distinct non-negative integers" + ) + operators = motion["operators"] + if ( + not isinstance(operators, list) + or not operators + or len(set(operators)) != len(operators) + or set(operators) - set(OPERATORS) + ): + raise ValueError( + "trajectory.operators must contain distinct supported operators" + ) + affordance = motion.get("affordance") + if "affordance" in operators and affordance is None: + raise ValueError("trajectory.affordance is required for affordance expansion") + if affordance is not None: + required_affordance = {"branches", "call_indices", "action_options"} + if ( + not isinstance(affordance, dict) + or required_affordance - set(affordance) + or set(affordance) - required_affordance - {"grasp_generators"} + ): + raise ValueError("Invalid trajectory.affordance settings") + if type(affordance["branches"]) is not int or affordance["branches"] < 2: + raise ValueError("trajectory.affordance.branches must be at least two") + calls = affordance["call_indices"] + if ( + not isinstance(calls, list) + or not calls + or any(type(index) is not int or index < 0 for index in calls) + or len(set(calls)) != len(calls) + ): + raise ValueError( + "Affordance call_indices must be distinct non-negative integers" + ) + if not isinstance(affordance["action_options"], dict) or any( + not isinstance(name, str) or not isinstance(options, dict) + for name, options in affordance["action_options"].items() + ): + raise ValueError( + "Affordance action_options must map preset names to settings" + ) + generators = affordance.get("grasp_generators", {}) + if not isinstance(generators, dict): + raise ValueError("Affordance grasp_generators must be a mapping") + for name, settings in generators.items(): + if ( + not isinstance(name, str) + or not isinstance(settings, dict) + or not settings + or set(settings) + - { + "max_candidates", + "approach_deviation_angle", + "kind", + "gripper", + "root_from_contact_frame", + } + ): + raise ValueError("Invalid affordance grasp generator overrides") + if {"kind", "gripper"} & settings.keys() and ( + not {"kind", "gripper"} <= settings.keys() + or settings["kind"] != "antipodal_parallel_jaw" + or not isinstance(settings["gripper"], dict) + ): + raise ValueError( + "A new grasp generator requires its kind and physical gripper description" + ) + count = settings.get("max_candidates", 2) + angle = settings.get("approach_deviation_angle", 0.0) + if type(count) is not int or count < 1: + raise ValueError("Affordance max_candidates must be positive") + if type(angle) not in (int, float) or not math.isfinite(angle) or angle < 0: + raise ValueError( + "Affordance approach_deviation_angle must be finite and non-negative" + ) + for name in ("via_count", "max_proposals"): + if type(motion[name]) is not int or motion[name] < 1: + raise ValueError(f"trajectory.{name} must be a positive integer") + for name in required - { + "operators", + "via_count", + "max_proposals", + "nullspace_task_rows", + "free_phases", + }: + if ( + type(motion[name]) not in (float, int) + or not math.isfinite(motion[name]) + or motion[name] <= 0 + ): + raise ValueError(f"trajectory.{name} must be finite and positive") + if motion["nullspace_task_rows"] not in ([0, 1, 2], [0, 1, 2, 3, 4, 5]): + raise ValueError( + "Nullspace tasks must explicitly constrain position or full pose" + ) + # These are the upstream skills' conservative free phases, narrowed for these demos. + allowed = { + "hand_over": {"pickup_approach"}, + "pick_up": {"approach"}, + "place": {"retract"}, + } + if not isinstance(motion["free_phases"], dict) or not motion["free_phases"]: + raise ValueError("Explicit free_phases are required") + for skill, phases in motion["free_phases"].items(): + if ( + skill not in allowed + or not isinstance(phases, list) + or not phases + or set(phases) - allowed[skill] + ): + raise ValueError(f"Unsupported or protected free phase for {skill}") + scene = value["scene"] + if not isinstance(scene, dict) or not scene: + raise ValueError("Explicit scene pose ranges are required") + for uid, options in scene.items(): + if set(options) == {"follow"}: + if options["follow"] not in scene or "follow" in scene[options["follow"]]: + raise ValueError( + "Scene followers must reference an independent pose range" + ) + continue + if ( + set(options) != {"translation_m", "yaw_rad"} + or len(options["translation_m"]) != 3 + ): + raise ValueError(f"Invalid scene pose range for {uid}") + for limits in (*options["translation_m"], options["yaw_rad"]): + if ( + len(limits) != 2 + or any( + type(x) not in (float, int) or not math.isfinite(x) for x in limits + ) + or limits[0] > limits[1] + ): + raise ValueError(f"Invalid finite ordered pose range for {uid}") + if not math.isfinite(engine["max_wall_time_s"]) or engine["max_wall_time_s"] <= 0: + raise ValueError("engine.max_wall_time_s must be finite and positive") + required_validation = { + "initial_checks", + "final_checks", + "events", + "allowed_contacts", + "mounting_contacts", + } + if required_validation - set(value["validation"]) or set( + value["validation"] + ) - required_validation - {"contact_hold"}: + raise ValueError( + "Explicit measured validation and contact permissions are required" + ) + hold = value["validation"].get("contact_hold") + if hold is not None: + if ( + engine["collision_backend"] != "native_contacts" + or not isinstance(hold, dict) + or set(hold) + != { + "entity", + "links", + "duration_s", + "max_translation_drift_m", + "max_rotation_drift_rad", + } + ): + raise ValueError( + "contact_hold requires native_contacts and explicit hold settings" + ) + if ( + not isinstance(hold["entity"], str) + or not hold["entity"] + or not isinstance(hold["links"], list) + or len(hold["links"]) != 2 + or len(set(hold["links"])) != 2 + or any(not isinstance(link, str) or not link for link in hold["links"]) + ): + raise ValueError( + "contact_hold requires an entity and two distinct robot links" + ) + for field in ( + "duration_s", + "max_translation_drift_m", + "max_rotation_drift_rad", + ): + if ( + type(hold[field]) not in (float, int) + or not math.isfinite(hold[field]) + or hold[field] <= 0 + ): + raise ValueError(f"contact_hold.{field} must be finite and positive") + from ._pose_sampling_cfg import validate_sampling + + validate_sampling(value) + return value diff --git a/embodichain/lab/expansion/_workload_cursor.py b/embodichain/lab/expansion/_workload_cursor.py new file mode 100644 index 0000000000..4e35e96e73 --- /dev/null +++ b/embodichain/lab/expansion/_workload_cursor.py @@ -0,0 +1,171 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Candidate-owned RNG streams around the canonical command handshake.""" + +from __future__ import annotations + +import random +from typing import Any +import numpy as np +import torch +from tensordict import TensorDict + +from embodichain.lab.gym.envs.types import ControllerAction +from embodichain.lab.gym.envs.task_program.bridge import TaskProgramDemoBridge +from .program import _Cursor + +__all__: list[str] = [] + + +class _WorkloadCursor(_Cursor): + def __init__( + self, + bridge: TaskProgramDemoBridge, + robot: Any, + seed: int, + *, + budget: Any = None, + ) -> None: + super().__init__(bridge) + self.robot = robot + self.budget = budget + self.rejection = None + self.last_command = None + self.devices = ( + [ + ( + robot.device.index + if robot.device.index is not None + else torch.cuda.current_device() + ) + ] + if robot.device.type == "cuda" + else [] + ) + self.cpu_rng = torch.Generator().manual_seed(seed).get_state() + self.gpu_rng = ( + torch.Generator(device=robot.device).manual_seed(seed).get_state() + if self.devices + else None + ) + self.numpy_rng = np.random.RandomState(seed % (2**32)).get_state() + self.python_rng = random.Random(seed).getstate() + + def next(self) -> ControllerAction | None: + before_numpy, before_python = np.random.get_state(), random.getstate() + with torch.random.fork_rng(devices=self.devices): + torch.set_rng_state(self.cpu_rng) + if self.devices: + torch.cuda.set_rng_state(self.gpu_rng, self.devices[0]) + np.random.set_state(self.numpy_rng) + random.setstate(self.python_rng) + try: + if ( + self.budget is None + or self.rejection is not None + or self.emergency is not None + ): + action = super().next() + else: + from embodichain.lab.sim.motion._budget import ( + _planning_budget_scope, + ) + from embodichain.lab.sim.scene.feasibility import ( + FeasibilityBudgetExhausted, + ) + + try: + with _planning_budget_scope(self.budget): + action = super().next() + except FeasibilityBudgetExhausted as error: + if self.segment is None: + # No command or segment exists yet: rejection has no + # safe-stop action to acknowledge and earns no step. + self.finished = True + self.accepted = False + self.reason = str(error) + action = None + else: + self.abort(str(error)) + action = super().next() + if action is not None: + if ( + self.rejection is not None or self.emergency is not None + ) and self.last_command is not None: + # The safe-stop value is new; its active invocation and + # phase still belong to the command being interrupted. + # Phase-aware physical validators must see that context. + action = ControllerAction( + action.value, + metadata={ + **self.last_command.metadata, + **{ + key: value + for key, value in action.metadata.items() + if value is not None + or key not in self.last_command.metadata + }, + }, + ) + self.last_command = action + if self.finished and self.rejection is not None: + self.accepted, self.reason = False, self.rejection + return action + finally: + self.cpu_rng = torch.get_rng_state() + if self.devices: + self.gpu_rng = torch.cuda.get_rng_state(self.devices[0]) + self.numpy_rng, self.python_rng = ( + np.random.get_state(), + random.getstate(), + ) + np.random.set_state(before_numpy) + random.setstate(before_python) + + def abort(self, reason: str) -> None: + if self.finished or self.rejection is not None or self.emergency is not None: + return + if not self.pending_consumed and self.last_command is None: + # An unconsumed command must go through the original safe-stop + # iterator; acknowledging it here would manufacture a World step. + super().abort(reason) + return + ids = tuple(self.bridge.env_ids.tolist()) + hold = self.robot.get_qpos(target=True)[list(ids)].clone() + previous = self.last_command.value + if isinstance(previous, torch.Tensor): + value = hold + else: + fields = {"qpos": hold} + if "qvel" in previous: + fields["qvel"] = torch.zeros_like(hold) + value = TensorDict(fields, batch_size=[len(ids)]) + if not self.pending_consumed: + self.reason = reason + self.emergency = iter( + self.segment.abort_actions( + reason, + last_action_consumed=False, + controller_hold=ControllerAction( + value, + metadata={"bridge_action_kind": "runtime_abort_safe_hold"}, + ), + ) + ) + return + self.bridge.request_row_rejection(ids, ControllerAction(value), reason=reason) + self.rejection = reason diff --git a/embodichain/lab/expansion/acceptance.py b/embodichain/lab/expansion/acceptance.py new file mode 100644 index 0000000000..2b7937e28e --- /dev/null +++ b/embodichain/lab/expansion/acceptance.py @@ -0,0 +1,762 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Shared measured geometry/contact acceptance without task-owned World control.""" + +from __future__ import annotations + +from collections import deque +from itertools import islice +from pathlib import Path +from typing import Any, TYPE_CHECKING +from collections.abc import Mapping, Sequence +from types import SimpleNamespace +import xml.etree.ElementTree as ET +import math +import numpy as np +import torch +from embodichain.data_pipeline.recording.media import _atomic_json as atomic_json + +if TYPE_CHECKING: + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.lab.gym.envs.types import ControllerAction + from embodichain.lab.sim.scene import SceneState + +__all__ = ["MeasuredExpansionValidator"] + + +class _GeometryRow: + """Independently check measured task predicates and complete physical geometry.""" + + def _initialize_task_checks(self, env, configuration, output): + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + self.env, self.configuration, self.output = env, configuration, output + self.monitor = MeasuredTaskMonitor( + configuration["final_checks"], configuration["events"], env_id=self.env_id + ) + for condition in configuration["initial_checks"]: + self.monitor._validate(condition) + self.initial, self.terminal, self.results = {}, {}, {} + + @staticmethod + def contact_permissions(env, configuration, known): + """Resolve authored contacts and mechanically adjacent robot assemblies.""" + # Collapse fixed URDF assemblies before deciding mechanical adjacency. + model = ET.parse(env.robot.cfg.fpath).getroot() + joints = model.findall("joint") + links = {link.get("name") for link in model.findall("link")} + groups = {link: link for link in links} + + def group(link): + while groups[link] != link: + link = groups[link] + return link + + for joint in joints: + if joint.get("type") == "fixed": + parent, child = joint.find("parent").get("link"), joint.find( + "child" + ).get("link") + groups[group(child)] = group(parent) + adjacent = { + frozenset( + ( + group(j.find("parent").get("link")), + group(j.find("child").get("link")), + ) + ) + for j in joints + } + physical_links = sorted( + link for link in links if f"{env.robot.uid}/{link}" in known + ) + ignored = [ + (f"{env.robot.uid}/{left}", f"{env.robot.uid}/{right}") + for i, left in enumerate(physical_links) + for right in physical_links[i + 1 :] + if group(left) == group(right) + or frozenset((group(left), group(right))) in adjacent + ] + children = {j.get("name"): j.find("child").get("link") for j in joints} + root_groups = {group(link) for link in links - set(children.values())} + roots = {link for link in physical_links if group(link) in root_groups} + + def expand(name): + if name == "@root": + return [ + f"{env.robot.uid}/{link}" + for link in sorted(roots) + if f"{env.robot.uid}/{link}" in known + ] + if name.startswith("@"): + return [ + f"{env.robot.uid}/{children[joint]}" + for joint in env.robot.control_parts[name[1:]] + ] + return [name] + + def pairs(values): + return tuple( + (a, b) + for left, right in values + for a in expand(left) + for b in expand(right) + ) + + ignored.extend(pairs(configuration["mounting_contacts"])) + allowed = pairs(configuration["allowed_contacts"]) + if any(not set(pair) <= known for pair in (*ignored, *allowed)): + raise ValueError("Contact permissions reference an absent physical frame") + # Match the physical model's authored self-collision policy. Keeping + # robot/scene checks does not require all internal robot shape pairs. + if env.robot.cfg.root_props.self_collision_enabled is False: + robot_frames = sorted( + frame for frame in known if frame.startswith(env.robot.uid + "/") + ) + ignored.extend( + (a, b) + for i, a in enumerate(robot_frames) + for b in robot_frames[i + 1 :] + ) + return ignored, allowed + + def __init__( + self, + env, + configuration: dict, + output: Path, + *, + device="cpu", + env_id=0, + checker_pool=None, + ): + self.env_id = env_id + from embodichain.lab.task_program.integrations.simulation.feasibility_geometry import ( + collision_bodies_from_simulation, + ) + from embodichain.lab.sim.scene.feasibility import NewtonCollisionChecker + + self._initialize_task_checks(env, configuration, output) + bodies = collision_bodies_from_simulation(env, env_id=self.env_id) + known = {body.frame_uid for body in bodies} + ignored, self.allowed_contacts = self.contact_permissions( + env, configuration, known + ) + key = (tuple(bodies), tuple(ignored), device) + checker = None if checker_pool is None else checker_pool.get(key) + if checker is None: + checker = NewtonCollisionChecker( + bodies, ignored_pairs=ignored, device=device, max_shapes=512 + ) + if checker_pool is not None: + checker_pool[key] = checker + self.checkers = [(checker, self.allowed_contacts, known)] + atomic_json( + output / "collision-geometry.json", + { + "shape_count": len(bodies), + "frames": sorted(known), + "query_count": len(self.checkers), + "self_collision_enabled": env.robot.cfg.root_props.self_collision_enabled, + }, + ) + self.moving = tuple( + uid + for uid in env.sim.get_rigid_object_uid_list() + if env.sim.get_rigid_object(uid).body_type == "dynamic" + ) + + def prepare(self, snapshot=None): + self.monitor.reset(self.env) + self.initial = { + self.env_id: all( + self.monitor.predicate(self.env, c) + for c in self.configuration["initial_checks"] + ) + and not all( + self.monitor.predicate(self.env, c) + for c in self.configuration["final_checks"] + ) + } + self.terminal.clear() + self.results.clear() + + def sample(self, ids, action, snapshot=None): + from embodichain.utils.math import matrix_from_quat + + self.monitor.observe(self.env, action) + self.terminal[self.env_id] = self.monitor.validation(self.env) + # Read the measured link buffer once and convert the whole arm in one + # batch. get_link_pose() otherwise repeats these allocations and + # quaternion conversions separately for every physical/visual link. + robot = self.env.robot + poses = robot.body_data.body_link_pose[self.env_id] + matrices = torch.eye(4, dtype=poses.dtype, device=poses.device).repeat( + len(robot.link_names), 1, 1 + ) + matrices[:, :3, :3] = matrix_from_quat(poses[:, 3:]) + matrices[:, :3, 3] = poses[:, :3] + matrices = matrices.detach().cpu().numpy() + frames = { + f"{robot.uid}/{link}": matrix + for link, matrix in zip(robot.link_names, matrices) + } + frames.update( + { + uid: self.env.sim.get_rigid_object(uid) + .get_local_pose(to_matrix=True)[self.env_id] + .detach() + .cpu() + .numpy() + for uid in self.env.sim.get_rigid_object_uid_list() + } + ) + frames["default_plane"] = np.eye(4) + for checker, contacts, aliases in self.checkers: + report, _ = checker.check( + frames, + allowed_contacts=contacts, + moving_objects=tuple(uid for uid in self.moving if uid in aliases), + ) + self.record_geometry(report, checker.last_pair, action) + if report.status != "passed": + break + + def record_geometry(self, report, pair, action): + """Retain measured failure and its original row-owned diagnostic.""" + if self.env_id not in self.results or report.status != "passed": + self.results[self.env_id] = SimpleNamespace( + status=report.status, reason=f"{report.reason}; pair={pair}" + ) + if report.status != "passed": + metadata = action.metadata.get("row_actions", {}).get( + str(self.env_id), action.metadata + ) + atomic_json( + self.output / "collision-failure.json", + { + "reason": self.results[self.env_id].reason, + "action": { + key: str(value) + for key, value in metadata.items() + if isinstance(value, (str, int, float, bool)) + }, + }, + ) + + def close(self): + for checker, _, _ in self.checkers: + checker.close() + self.checkers.clear() + + +class _ContactRow(_GeometryRow): + """Inspect actual physics contacts, including every control substep. + + Unlike the independent Newton query, this backend retains native triangle + meshes. Unknown actors, unavailable impulses and dropped rows fail closed. + The optional final hold audit additionally requires both physical fingers + throughout the final measured control samples and bounds object drift. + """ + + def __init__(self, env, configuration, output, *, sensor, env_id=0): + self.env_id = env_id + self._initialize_task_checks(env, configuration, output) + self.active, self._bad, self.substeps, self._interval_samples = ( + False, + None, + 0, + 0, + ) + self.hold = configuration.get("contact_hold") + count = 1 + if self.hold: + intervals = self.hold["duration_s"] * env.control_frequency + nearest = round(intervals) + # Multiplication can place an exact grid duration just above an + # integer (e.g. 0.14 * 50). Only normalize floating-point roundoff; + # genuinely off-grid holds still round upward. + if abs(intervals - nearest) <= 2 * math.ulp(intervals): + intervals = nearest + count = max(1, math.ceil(intervals)) + 1 + self.samples = deque(maxlen=count) + self._interval_dts: list[float] = [] + self.sensor = sensor + self.frames = {} + for link, actor in zip( + env.robot.link_names, + self.sensor.get_actor_ids(env.robot.uid, env.robot.link_names)[ + self.env_id + ].tolist(), + ): + self.frames[actor] = f"{env.robot.uid}/{link}" + objects = list(env.sim.get_rigid_object_uid_list()) + if env.sim.sim_config.enable_ground_plane: + objects.append("default_plane") + for uid in objects: + self.frames[int(self.sensor.get_actor_ids(uid)[self.env_id, 0])] = uid + ignored, allowed = self.contact_permissions( + env, configuration, set(self.frames.values()) + ) + self.allowed = {frozenset(pair) for pair in (*ignored, *allowed)} + self.moving = {f"{env.robot.uid}/{link}" for link in env.robot.link_names} | { + uid + for uid in env.sim.get_rigid_object_uid_list() + if env.sim.get_rigid_object(uid).body_type == "dynamic" + } + atomic_json( + output / "collision-geometry.json", + { + "backend": "native_contacts", + "geometry": "native physical shapes, including triangle meshes", + "frames": sorted(self.frames.values()), + "substep_sampling": True, + "max_contacts_per_env": 8192, + "self_collision_enabled": env.robot.cfg.root_props.self_collision_enabled, + }, + ) + + def reset(self, env_ids=None): + self.active, self._bad, self.substeps, self._interval_samples = ( + False, + None, + 0, + 0, + ) + self.samples.clear() + self._interval_dts.clear() + + def prepare(self, snapshot=None): + super().prepare(snapshot) + self.reset() + self.active = True + + def begin_control_step(self): + self._bad = None + self._interval_samples = 0 + self._interval_dts.clear() + + def before_sample(self): + return None + + def _hold_report(self): + report = { + "samples": len(self.samples), + "required_samples": self.samples.maxlen, + "physics_substeps": self.substeps, + } + if len(self.samples) != self.samples.maxlen: + return {**report, "passed": False} + # The first pose starts the hold; count only subsequent measured + # physics intervals. Bounded-window fsum avoids subtracting accumulated + # World clocks, whose rounding error grows throughout a collection. + elapsed = math.fsum(sample[2] for sample in islice(self.samples, 1, None)) + report.update( + elapsed_duration_s=elapsed, required_duration_s=self.hold["duration_s"] + ) + poses = np.asarray([sample[0] for sample in self.samples]) + from scipy.spatial.transform import Rotation + + rotations = Rotation.from_quat(poses[:, 3:]) + report.update( + both_fingers_samples=sum(all(sample[1]) for sample in self.samples), + max_translation_drift_m=float( + np.linalg.norm(poses[:, :3] - poses[0, :3], axis=1).max() + ), + max_rotation_drift_rad=float( + (rotations[0].inv() * rotations).magnitude().max() + ), + ) + report["passed"] = ( + ( + elapsed > 0 + and ( + elapsed >= self.hold["duration_s"] + or math.isclose( + elapsed, + self.hold["duration_s"], + rel_tol=0, + abs_tol=2 * math.ulp(self.hold["duration_s"]), + ) + ) + ) + and report["both_fingers_samples"] == self.samples.maxlen + and report["max_translation_drift_m"] + <= self.hold["max_translation_drift_m"] + and report["max_rotation_drift_rad"] <= self.hold["max_rotation_drift_rad"] + ) + return report + + def sample(self, ids, action, snapshot=None): + from embodichain.lab.expansion.contracts import ( + ValidationCheck, + ValidationResult, + ) + + if not self._interval_samples: + self._bad = "native contact substeps were not sampled" + if self.env_id not in self.results or self._bad: + self.results[self.env_id] = SimpleNamespace( + status="failed" if self._bad else "passed", + reason=self._bad or "all measured native contacts are permitted", + ) + if self._bad: + atomic_json(self.output / "collision-failure.json", {"reason": self._bad}) + self.monitor.observe(self.env, action) + terminal = self.monitor.validation(self.env) + if self.hold: + data = self.sensor.get_data() + pairs = data["user_ids"][self.env_id][ + data["is_valid"][self.env_id] & data["impulse_valid"][self.env_id] + ] + book = int(self.sensor.get_actor_ids(self.hold["entity"])[self.env_id, 0]) + fingers = self.sensor.get_actor_ids(self.env.robot.uid, self.hold["links"])[ + self.env_id + ].tolist() + touching = [ + bool( + ((pairs == book).any(dim=-1) & (pairs == finger).any(dim=-1)).any() + ) + for finger in fingers + ] + pose = ( + self.env.sim.get_rigid_object(self.hold["entity"]) + .get_local_pose()[self.env_id] + .tolist() + ) + self.samples.append((pose, touching, math.fsum(self._interval_dts))) + self._interval_dts.clear() + report = ( + self._hold_report() + if terminal.accepted + else {"passed": False, "reason": "awaiting terminal hold"} + ) + terminal = ValidationResult( + ( + *terminal.checks, + ValidationCheck( + "task.contact_hold", + "passed" if report["passed"] else "failed", + str(report), + ), + ) + ) + if terminal.accepted: + atomic_json(self.output / "contact-hold.json", report) + self.terminal[self.env_id] = terminal + + def close(self): + if self.hold and self.samples: + obj = self.env.sim.get_rigid_object(self.hold["entity"]) + atomic_json( + self.output / "terminal-diagnostics.json", + { + "pose": obj.get_local_pose()[self.env_id].tolist(), + "body_state": obj.body_state[self.env_id].tolist(), + "predicates": [ + { + "condition": condition, + "passed": self.monitor.predicate(self.env, condition), + } + for condition in self.configuration["final_checks"] + ], + "hold": self._hold_report(), + "events": self.monitor.receipts, + }, + ) + self.active = False + + +class MeasuredExpansionValidator: + """Use one measured acceptance port for geometry or complete native contacts. + + Args: + environment: Caller-owned native Gym environment. + configuration: Task-owned initial_checks/final_checks/events, declared + allowed_contacts/mounting_contacts, and optional contact_hold with + entity, finger links, duration and translation/rotation drift limits. + output: Artifact directory for diagnostic and terminal evidence. + backend: geometry (independent Newton query) or native_contacts (actual + physical substeps including triangle meshes). Unavailable data rejects. + device: Numerical geometry-query device; never changes physics. + + The World, stepping and acceptance thresholds remain with the host/task. + Selected-row preparation leaves live peers' monitors and evidence intact. + """ + + def __init__( + self, + environment: EmbodiedEnv, + configuration: Mapping[str, Any], + output: str | Path, + *, + backend: str = "geometry", + device: str = "cpu", + ) -> None: + if backend not in ("geometry", "native_contacts"): + raise ValueError("backend must be geometry or native_contacts") + from embodichain.lab.gym.envs._json import json_safe_copy + + configuration = json_safe_copy( + configuration, field_name="measured acceptance configuration" + ) + self.env, self.rows, self.sensor = environment, {}, None + self._observer_registered = False + output = Path(output) + output.mkdir(parents=True, exist_ok=True) + self.initial, self.terminal, self.results = {}, {}, {} + self._closed = False + try: + if backend == "native_contacts": + from embodichain.lab.sim.sensors import ( + ContactSensorCfg, + ArticulationContactFilterCfg, + ContactPermissionChecker, + ) + + self.sensor_uid = "expansion_native_contacts" + if self.sensor_uid in environment.sensors: + raise ValueError( + "the measured contact validator already owns a sensor" + ) + self.sensor = environment.sim.add_sensor( + ContactSensorCfg( + uid=self.sensor_uid, + rigid_uid_list=environment.sim.get_rigid_object_uid_list(), + articulation_cfg_list=[ + ArticulationContactFilterCfg( + articulation_uid=environment.robot.uid + ) + ], + filter_need_both_actor=False, + max_contacts_per_env=8192, + ) + ) + environment.sensors[self.sensor_uid] = self.sensor + checker_pool = {} + for row in range(environment.num_envs): + path = output if environment.num_envs == 1 else output / f"row-{row}" + path.mkdir(parents=True, exist_ok=True) + self.rows[row] = ( + _GeometryRow( + environment, + configuration, + path, + device=device, + env_id=row, + checker_pool=checker_pool, + ) + if self.sensor is None + else _ContactRow( + environment, configuration, path, sensor=self.sensor, env_id=row + ) + ) + conditions = [ + *configuration["initial_checks"], + *configuration["final_checks"], + ] + for event in configuration["events"]: + conditions.extend(event["conditions"]) + shape_entities = { + condition["entity"] + for condition in conditions + if condition["kind"] == "support_bounds" + } + self._shapes = { + uid: ( + tuple( + environment.sim.get_rigid_object(uid).get_collision_shapes( + env_id=row, allow_heterogeneous=True + ) + for row in range(environment.num_envs) + ) + if uid in shape_entities + else () + ) + for uid in environment.sim.get_rigid_object_uid_list() + } + if self.sensor is not None: + permissions = self.rows[0].allowed + self.checker = ContactPermissionChecker( + self.sensor, + [self.rows[row].frames for row in range(environment.num_envs)], + allowed_contacts=tuple( + ( + tuple(pair) + if len(pair) == 2 + else (next(iter(pair)), next(iter(pair))) + ) + for pair in permissions + ), + moving_frames=tuple(self.rows[0].moving), + ) + self.sensor.add_sampling_observer("expansion", self) + self._observer_registered = True + except BaseException: + self.close() + raise + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Reset only the sensor-selected monitor rows.""" + ids = tuple(self.rows) if env_ids is None else tuple(int(i) for i in env_ids) + for row in ids: + self.rows[row].reset() if self.sensor is not None else None + + def prepare( + self, + snapshot: SceneState | None = None, + *, + env_ids: Sequence[int] | None = None, + ) -> None: + """Capture initial qualification after physical preparation/settling.""" + ids = tuple(self.rows) if env_ids is None else tuple(env_ids) + if ( + not ids + or len(set(ids)) != len(ids) + or any(type(row) is not int or row not in self.rows for row in ids) + ): + raise ValueError("prepare requires unique physical rows") + for row in ids: + check = self.rows[row] + check.prepare(snapshot) + self.initial[row] = check.initial[row] + self.terminal.pop(row, None) + self.results.pop(row, None) + + def before_sample(self): + return None + + def after_sample(self, dt: float, token: object) -> None: + """Inspect every physical sample once, reducing all rows together.""" + active = [check for check in self.rows.values() if check.active] + if not active: + return + summaries = self.checker.check().cpu().tolist() + for check in active: + check.substeps += 1 + check._interval_samples += 1 + if check.hold: + check._interval_dts.append(dt) + code, actor_a, actor_b = summaries[check.env_id] + if code: + check._bad = f"native contact evidence rejected: code={code}; actors={actor_a},{actor_b}" + + def begin_control_step(self) -> None: + """Start a physical interval without discarding prior failure results.""" + for check in self.rows.values(): + if self.sensor is not None and check.active: + check.begin_control_step() + + def sample( + self, + ids: Sequence[int], + action: ControllerAction, + snapshot: SceneState | None = None, + ) -> None: + """Check measured task state and retain each row's first physical failure.""" + if not ids: + return + from .collision import _measured_frame + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + env = self.env + measured = ( + _measured_frame(env, snapshot, self._shapes) + if getattr(snapshot, "env_ids", None) == tuple(range(env.num_envs)) + else env + ) + monitors = tuple(self.rows[row].monitor for row in ids) + with MeasuredTaskMonitor.batch_scope(measured, monitors): + for row in ids: + check = self.rows[row] + if self.sensor is None: + check.monitor.observe(measured, action) + check.terminal[row] = check.monitor.validation(measured) + else: + check.sample((row,), action, snapshot) + check._interval_samples = 0 + self.terminal[row] = check.terminal[row] + if self.sensor is None: + self._sample_geometry(ids, action) + for row in ids: + self.results[row] = self.rows[row].results[row] + + def _sample_geometry(self, ids, action): + """Batch compatible physical geometry, retaining heterogeneous-row checks.""" + from embodichain.utils.math import matrix_from_quat + + env = self.env + poses = env.robot.body_data.body_link_pose + matrices = torch.eye(4, dtype=poses.dtype, device=poses.device).repeat( + env.num_envs, len(env.robot.link_names), 1, 1 + ) + matrices[..., :3, :3] = matrix_from_quat(poses[..., 3:]) + matrices[..., :3, 3] = poses[..., :3] + matrices = matrices.detach().cpu().numpy() + frames = { + f"{env.robot.uid}/{link}": matrices[:, index] + for index, link in enumerate(env.robot.link_names) + } + frames.update( + { + uid: env.sim.get_rigid_object(uid) + .get_local_pose(to_matrix=True) + .detach() + .cpu() + .numpy() + for uid in env.sim.get_rigid_object_uid_list() + } + ) + frames["default_plane"] = np.broadcast_to(np.eye(4), (env.num_envs, 4, 4)) + groups = {} + for row in ids: + check = self.rows[row] + for checker, allowed, aliases in check.checkers: + moving = tuple(uid for uid in check.moving if uid in aliases) + groups.setdefault((checker, tuple(allowed), moving), []).append(row) + for (checker, allowed, moving), rows in groups.items(): + # Respect the existing aggregate contact-buffer ceiling. Compact + # selected rows also prevent inactive/heterogeneous rows consuming it. + width = max(1, min(env.num_envs, 1048576 // checker._capacity)) + for start in range(0, len(rows), width): + selected = rows[start : start + width] + reports = checker.check_batch( + {uid: values[selected] for uid, values in frames.items()}, + allowed_contacts=allowed, + moving_objects=moving, + ) + for row, report, pair in zip( + selected, reports, checker.last_batch_pairs + ): + self.rows[row].record_geometry(report, pair, action) + + def close(self) -> None: + """Detach owned observers before releasing resources; safe on failure.""" + if self._closed: + return + self._closed = True + for check in self.rows.values(): + check.close() + if self.sensor is not None: + if self._observer_registered: + self.sensor.remove_sampling_observer("expansion", expected=self) + self.env.sensors.pop(self.sensor_uid, None) + self.env.sim.remove_asset(self.sensor_uid) diff --git a/embodichain/lab/expansion/collision.py b/embodichain/lab/expansion/collision.py index 966161bc50..99d9af01a0 100644 --- a/embodichain/lab/expansion/collision.py +++ b/embodichain/lab/expansion/collision.py @@ -64,6 +64,7 @@ def _measured_frame(env, snapshot, shapes): [state.qpos for state in entities[env.robot.uid]], dtype=torch.float32 ) return SimpleNamespace( + num_envs=env.num_envs, robot=SimpleNamespace(joint_names=env.robot.joint_names, get_qpos=lambda: qpos), sim=SimpleNamespace( get_rigid_object_uid_list=lambda: tuple(bodies), @@ -106,7 +107,7 @@ def __init__(self, env, profile: str, output: Path): ) self.results = {} - def prepare(self, snapshot=None): + def prepare(self, snapshot=None, *, env_ids=None): from embodichain.lab.task_program.integrations.simulation.feasibility_geometry import ( collision_bodies_from_simulation, ) @@ -116,9 +117,17 @@ def prepare(self, snapshot=None): raise ValueError( "cohort collision geometry differs; create a compatible world" ) - self.results.clear() - self.initial.clear() - self.terminal.clear() + ids = tuple(self.monitors) if env_ids is None else tuple(env_ids) + if ( + not ids + or len(set(ids)) != len(ids) + or any(row not in self.monitors for row in ids) + ): + raise ValueError("prepare requires unique physical monitor rows") + for row in ids: + self.results.pop(row, None) + self.initial.pop(row, None) + self.terminal.pop(row, None) self.shapes = { uid: tuple( self.env.sim.get_rigid_object(uid).get_collision_shapes(env_id=row) @@ -131,25 +140,36 @@ def prepare(self, snapshot=None): if snapshot is None else _measured_frame(self.env, snapshot, self.shapes) ) - for row, monitor in self.monitors.items(): - monitor.reset(measured) - self.initial[row] = all( - monitor.predicate(measured, c) for c in self.initial_conditions - ) and not all(monitor.predicate(measured, c) for c in monitor.final_checks) + for row in ids: + self.monitors[row].reset(measured) + with MeasuredTaskMonitor.batch_scope( + measured, + tuple(self.monitors.values()), + extra_conditions=self.initial_conditions, + ): + for row in ids: + monitor = self.monitors[row] + self.initial[row] = all( + monitor.predicate(measured, c) for c in self.initial_conditions + ) and not all( + monitor.predicate(measured, c) for c in monitor.final_checks + ) def sample(self, ids, action, snapshot=None): env = self.env measured = ( env if snapshot is None else _measured_frame(env, snapshot, self.shapes) ) + poses = env.robot.body_data.body_link_pose + matrices = torch.eye(4, device=poses.device, dtype=poses.dtype).repeat( + env.num_envs, len(env.robot.link_names), 1, 1 + ) + matrices[..., :3, :3] = matrix_from_quat(poses[..., 3:]) + matrices[..., :3, 3] = poses[..., :3] + matrices = matrices.detach().cpu().numpy() frames = { - f"{env.robot.uid}/{name}": env.robot.get_link_pose( - link_name=name, to_matrix=True - ) - .detach() - .cpu() - .numpy() - for name in env.robot.link_names + f"{env.robot.uid}/{name}": matrices[:, index] + for index, name in enumerate(env.robot.link_names) } frames.update( { @@ -161,20 +181,26 @@ def sample(self, ids, action, snapshot=None): for uid in env.sim.get_rigid_object_uid_list() } ) + frames["default_plane"] = np.broadcast_to(np.eye(4), (env.num_envs, 4, 4)) + with MeasuredTaskMonitor.batch_scope(measured, tuple(self.monitors.values())): + for row in ids: + self.monitors[row].observe(measured, action) + self.terminal[row] = self.monitors[row].validation(measured) + groups = {} for row in ids: - self.monitors[row].observe(measured, action) - self.terminal[row] = self.monitors[row].validation(measured) metadata = action.metadata["row_actions"][str(row)] phase = self.phase(env, SimpleNamespace(metadata=metadata), None) - row_frames = {key: value[row] for key, value in frames.items()} - row_frames["default_plane"] = np.eye(4) - report, _ = self.checker.check( - row_frames, - allowed_contacts=phase.allowed_contacts, + groups.setdefault(tuple(phase.allowed_contacts), []).append(row) + for allowed, rows in groups.items(): + reports = self.checker.check_batch( + frames, + allowed_contacts=allowed, moving_objects=self.moving, + env_ids=rows, ) - if row not in self.results or report.status != "passed": - self.results[row] = report + for row, report in zip(rows, reports): + if row not in self.results or report.status != "passed": + self.results[row] = report def close(self): self.checker.close() diff --git a/embodichain/lab/expansion/comparison.py b/embodichain/lab/expansion/comparison.py new file mode 100644 index 0000000000..e0c0540c52 --- /dev/null +++ b/embodichain/lab/expansion/comparison.py @@ -0,0 +1,681 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Measured reference/expansion comparison on a shared physics-free timeline.""" + +from __future__ import annotations + +from contextlib import ExitStack, AbstractContextManager +from collections.abc import Callable +from dataclasses import dataclass +from datetime import datetime, timezone +from pathlib import Path +from types import MappingProxyType +import time +import numpy as np +from embodichain.data_pipeline.recording.media import _atomic_json as atomic_json +from embodichain.lab.expansion.persistence import _digest as file_digest +from embodichain.lab.visualization.markers import MarkerFramePublisher + +__all__ = [ + "MeasuredTrajectory", + "record_reference_trajectory", + "preview_expansion_comparison", +] + +GHOST_COLOR = (0.15, 0.8, 0.95, 0.35) + + +def _utc_now() -> str: + return datetime.now(timezone.utc).isoformat() + + +@dataclass(frozen=True) +class MeasuredTrajectory: + """One arena's measured poses; all frames include the initial boundary.""" + + name: str + robot_uid: str + joint_names: tuple[str, ...] + timestamps: np.ndarray + qpos: np.ndarray + root_poses: np.ndarray + object_poses: dict[str, np.ndarray] + + def __post_init__(self) -> None: + for name in ("timestamps", "qpos", "root_poses"): + object.__setattr__(self, name, np.array(getattr(self, name), copy=True)) + object.__setattr__( + self, + "object_poses", + MappingProxyType( + { + key: np.array(value, copy=True) + for key, value in self.object_poses.items() + } + ), + ) + count = len(self.timestamps) + if ( + count < 2 + or self.timestamps.shape != (count,) + or self.timestamps[0] != 0 + or not np.all(np.diff(self.timestamps) > 0) + or not self.joint_names + or len(set(self.joint_names)) != len(self.joint_names) + or self.qpos.shape != (count, len(self.joint_names)) + ): + raise ValueError("Preview requires a nonempty, ordered measured trajectory") + arrays = ( + self.timestamps, + self.qpos, + self.root_poses, + *self.object_poses.values(), + ) + if any(not np.isfinite(array).all() for array in arrays): + raise ValueError("Preview trajectory must contain finite measurements") + for poses in (self.root_poses, *self.object_poses.values()): + if poses.shape != (count, 7) or not np.allclose( + np.linalg.norm(poses[:, 3:], axis=1), 1, atol=1e-5, rtol=0 + ): + raise ValueError("Preview poses must be xyz + normalized xyzw") + for array in ( + self.timestamps, + self.qpos, + self.root_poses, + *self.object_poses.values(), + ): + array.flags.writeable = False + + def frame_at(self, elapsed: float) -> int: + """Select the latest measured boundary, holding the final pose at the end.""" + return int( + np.clip( + np.searchsorted(self.timestamps, elapsed, side="right") - 1, + 0, + len(self.timestamps) - 1, + ) + ) + + @classmethod + def load_reference(cls, path: Path) -> MeasuredTrajectory: + """Read a pickle-free reference captured by record_reference_trajectory.""" + with np.load(path, allow_pickle=False) as data: + return cls( + name="original", + robot_uid=str(data["robot_uid"]), + joint_names=tuple(data["joint_names"].tolist()), + timestamps=data["timestamps"].copy(), + qpos=data["qpos"].copy(), + root_poses=data["root_poses"].copy(), + object_poses={ + str(uid): data[f"object_{index}"].copy() + for index, uid in enumerate(data["object_uids"]) + }, + ) + + @classmethod + def load_episode(cls, storage_id: str, *, robot_uid: str) -> MeasuredTrajectory: + """Read only an accepted native episode, preserving full joint order.""" + from embodichain.lab.expansion import load_expansion_episode + + episode = load_expansion_episode(storage_id) + if not episode.validation.accepted: + raise ValueError("comparison requires a qualified native episode") + metadata, observations = episode.metadata, episode.observations + robot = next( + item for item in metadata["initial_state"] if item["uid"] == robot_uid + ) + root_key = next( + field["key"] + for field in metadata["scene_state_fields"] + if field["entity_uid"] == robot_uid and field["field"] == "pose" + ) + + def array(value): + return value.detach().cpu().numpy().copy() + + return cls( + name=episode.episode_id, + robot_uid=robot_uid, + joint_names=tuple(robot["joint_names"]), + timestamps=array(episode.timestamps), + qpos=array(observations["full_joint_positions"]), + root_poses=array(observations[root_key]), + object_poses={ + key[len("rigid/") : -len("/state")]: array(value)[:, :7] + for key, value in observations.items() + if key.startswith("rigid/") and key.endswith("/state") + }, + ) + + +def record_reference_trajectory(env: object, path: Path) -> None: + """Execute the unmodified expert once and archive its measured scene poses.""" + from embodichain.lab.gym.envs.demo import execute_demo_episode + from embodichain.lab.sim.scene import SceneView, capture_state + + view = SceneView(env.sim, env_ids=(0,)) + frames = [capture_state(view).batches[0]] + + def observe(*_args): + frames.append(capture_state(view).batches[0]) + + episode = execute_demo_episode(env, step_observer=observe) + atomic_json(path.with_suffix(".json"), episode.to_metadata()) + if not episode.completed or not episode.all_success: + raise RuntimeError(f"Nominal preview expert failed: {episode.terminal_reason}") + robot_index = next( + index for index, item in enumerate(frames[0]) if item.uid == env.robot.uid + ) + robot = frames[0][robot_index] + objects = [ + (index, item.uid) + for index, item in enumerate(frames[0]) + if item.kind == "rigid_object" + ] + arrays = { + "robot_uid": np.asarray(robot.uid), + "joint_names": np.asarray(robot.joint_names), + "timestamps": np.arange(len(frames), dtype=np.float64) * env.step_dt, + "qpos": np.asarray([frame[robot_index].qpos for frame in frames]), + "root_poses": np.asarray([frame[robot_index].pose for frame in frames]), + "object_uids": np.asarray([uid for _, uid in objects]), + } + arrays.update( + { + f"object_{ordinal}": np.asarray([frame[index].pose for frame in frames]) + for ordinal, (index, _) in enumerate(objects) + } + ) + np.savez_compressed(path, **arrays) + # Validate the exact archive before another World is created. + MeasuredTrajectory.load_reference(path) + + +def _object_prototype(obj): + """Use the physical object's actual visual shape and scale exactly once.""" + from embodichain.lab.visualization.markers import MarkerPrototypeCfg + from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, SphereCfg + + shape, scale = obj.cfg.shape, np.asarray(obj.cfg.body_scale) + options = {"color": GHOST_COLOR, "scene_lighting": True, "unlit": False} + if isinstance(shape, CubeCfg): + return MarkerPrototypeCfg( + shape="box", scale=tuple(scale * shape.size), **options + ) + if isinstance(shape, SphereCfg): + return MarkerPrototypeCfg( + shape="sphere", scale=tuple(scale * 2 * shape.radius), **options + ) + if isinstance(shape, MeshCfg) or obj.cfg.source is not None: + # Use the simulator's materialized visual geometry: GLB loading may + # convert axes, while USD sources can author their own mesh transforms. + vertices = obj.get_vertices(env_ids=[0], scale=False)[0].cpu().numpy() + faces = obj.get_triangles(env_ids=[0])[0].cpu().numpy() + return MarkerPrototypeCfg( + shape="mesh", vertices=vertices, faces=faces, scale=tuple(scale), **options + ) + raise ValueError(f"Unsupported preview object shape for {obj.uid}") + + +def _precompute_link_poses(clip, robot, asset_path): + """Compute complete-arm FK once; browsing never runs FK or loads episodes.""" + import torch + import pytorch_kinematics as pk + from scipy.spatial.transform import Rotation + + chain = pk.build_chain_from_urdf(Path(asset_path).read_bytes()) + indices = [clip.joint_names.index(name) for name in robot.joint_names] + poses = [] + with torch.no_grad(): + for start in range(0, len(clip.timestamps), 256): + qpos = torch.as_tensor( + clip.qpos[start : start + 256, indices], dtype=torch.float32 + ) + fk = chain.forward_kinematics(qpos) + poses.append( + torch.stack( + [fk[name].get_matrix() for name in robot.link_names], dim=1 + ).numpy() + ) + roots = np.broadcast_to(np.eye(4), (len(clip.timestamps), 4, 4)).copy() + roots[:, :3, :3] = Rotation.from_quat(clip.root_poses[:, 3:]).as_matrix() + roots[:, :3, 3] = clip.root_poses[:, :3] + return roots[:, None] @ np.concatenate(poses) + + +class _ReferencePlayer: + """Publish measured entities through the native render-state owner.""" + + def __init__(self, env, reference): + from embodichain.lab.sim.scene import SceneView, capture_state + + if tuple(env.robot.joint_names) != reference.joint_names: + raise ValueError("Preview robot joint order differs from recording") + self.view = SceneView(env.sim, env_ids=(0,)) + self.initial = capture_state(self.view) + self.reference = reference + + def queue(self, frame): + from dataclasses import replace + from embodichain.lab.sim.scene import restore_render_state + + reference = self.reference + entities = [] + for state in self.initial.batches[0]: + if state.uid == reference.robot_uid: + state = replace( + state, + pose=tuple(map(float, reference.root_poses[frame])), + qpos=tuple(map(float, reference.qpos[frame])), + ) + elif state.uid in reference.object_poses: + state = replace( + state, + pose=tuple(map(float, reference.object_poses[state.uid][frame])), + ) + entities.append(state) + restore_render_state( + self.view, replace(self.initial, batches=(tuple(entities),)) + ) + + def close(self): + from embodichain.lab.sim.scene import restore_render_state + + restore_render_state(self.view, self.initial) + + +def _accepted_candidates(operators): + from embodichain.lab.expansion import ArtifactEpisodeSink + + return [ + { + "operator": name, + "storage_id": receipt.storage_id, + "episode_id": receipt.episode_id, + } + for name, report in operators.items() + if "directory" in report + for receipt in ArtifactEpisodeSink(report["directory"]).inspect() + ] + + +def preview_expansion_comparison( + environment_factory: Callable[[], AbstractContextManager], + reference_path: Path, + operators: dict[str, dict] | None, + *, + renderer: str, + scene_entities: tuple[str, ...], + output: Path, + collect: Callable | None = None, + max_frames: int | None = None, + frame_observer: Callable | None = None, + expected_trajectories: int | None = None, +) -> dict: + """Browse accepted trajectories; N advances, Space pauses and Q closes. + + With a collection callback, warm model resources before opening the window, + then delegate collection and load only its qualified measured trajectories. + Only the selected expansion is visible; N preserves playback time and + pause state. Rendering cannot alter episodes or advance physics. + + Args: + environment_factory: Context manager factory for the caller's native + environment. The viewer displays physical row zero; other rows are + retained with World-wide physics disabled during playback. + reference_path: Pickle-free measured reference produced by the recorder. + operators: Named artifact directories containing accepted native receipts. + renderer: Renderer label in viewer evidence; the factory configures it. + scene_entities: Recorded objects to display with the selected robot ghost. + output: Viewer manifest and interaction evidence directory. + collect: Optional callback receiving simulation, window, output and status; + returns named artifact directories. Collection ownership stays external. + max_frames: Optional display-frame budget for bounded playback. + frame_observer: Optional callback receiving environment, frame number, + selected index and shared elapsed time after publication. + expected_trajectories: Required preload count for early-window collection. + + Returns: + Playback outcome, selection/frame counts and actual physics-step delta. + """ + from collections import deque + from dexsim.engine import InputControl + from dexsim.types import InputKey + from embodichain.lab.visualization.markers import ( + RobotMarkers, + RobotMarkersAppearanceCfg, + RobotMarkersCfg, + VisualizationMarkers, + VisualizationMarkersCfg, + ) + import os + + output = Path(output) + output.mkdir(parents=True, exist_ok=True) + if max_frames is not None and (type(max_frames) is not int or max_frames < 1): + raise ValueError("max_frames must be a positive integer") + if collect is not None and ( + type(expected_trajectories) is not int or expected_trajectories < 1 + ): + raise ValueError("Early preview requires the expected trajectory count") + with environment_factory() as env: + sim = env.sim + physics_enabled = sim.physics_enabled + sim.enable_physics(False) + physics_steps_before = sim.physics_step_index + with ExitStack() as cleanup: + cleanup.callback(sim.enable_physics, physics_enabled) + players = cleanup.enter_context(ExitStack()) + markers = MarkerFramePublisher(sim, xray=True) + cleanup.callback(markers.close) + robot = RobotMarkers( + RobotMarkersCfg( + name="expansion_robot", + asset_path=env.robot.cfg.fpath, + coordinate_space="env", + appearance=RobotMarkersAppearanceCfg( + color=GHOST_COLOR[:3], opacity=GHOST_COLOR[3] + ), + ), + markers, + ) + objects = VisualizationMarkers( + VisualizationMarkersCfg( + prim_path="expansion_objects", + coordinate_space="env", + markers={ + uid: _object_prototype(sim.get_rigid_object(uid)) + for uid in scene_entities + }, + ), + markers, + ) + + def open_window(): + if not sim.open_window(): + raise RuntimeError( + "The requested native expansion preview did not open" + ) + cleanup.callback(sim.close_window) + window = sim.get_world().get_windows() + center = np.mean( + [env.robot.get_local_pose()[0, :3].detach().cpu().numpy()] + + [ + sim.get_rigid_object(uid) + .get_local_pose()[0, :3] + .detach() + .cpu() + .numpy() + for uid in scene_entities + ], + axis=0, + ) + center[2] += 0.2 + window.set_look_at( + center + [1.6, -1.8, 1.1], center, np.array([0.0, 0.0, 1.0]) + ) + return window + + window = None + window_opened_at = None + if collect is not None: + # Allocate all native mesh instances before the render loop. + # Their hidden placeholder poses require no recorded data/FK. + robot.visualize_link_poses( + np.broadcast_to( + np.eye(4), + (1, expected_trajectories, len(robot.link_names), 4, 4), + ), + env_ids=(0,), + ) + count = expected_trajectories * len(scene_entities) + objects.visualize( + translations=np.zeros((count, 3)), + orientations=np.tile([0.0, 0.0, 0.0, 1.0], (count, 1)), + marker_indices=np.tile( + np.arange(len(scene_entities)), expected_trajectories + ), + ) + for name, width in ( + ("expansion_robot", robot.num_visuals), + ("expansion_objects", len(scene_entities)), + ): + markers.groups[name].update( + visible=np.zeros(expected_trajectories * width, dtype=bool), + env_ids=(0,), + ) + markers.publish() + sim.sync_render_state() + window = open_window() + window_opened_at = _utc_now() + status = { + "state": "collecting", + "renderer": renderer, + "pid": os.getpid(), + "window_opened_at": window_opened_at, + "window_open": True, + "expected_trajectories": expected_trajectories, + "preloaded_robots": expected_trajectories, + } + atomic_json(output / "window.json", status) + window.set_window_title("Expansion preparing | reference | Q cancel") + operators = collect(sim, window, output, status) + atomic_json(output / "status.json", {**status, "state": "loading"}) + + reference = MeasuredTrajectory.load_reference(reference_path) + candidates = _accepted_candidates(operators) + atomic_json( + output / "manifest.json", + { + "reference": str(reference_path), + "reference_sha256": file_digest(reference_path), + "candidates": candidates, + }, + ) + if not candidates: + result = { + "state": "empty", + "trajectories": 0, + "reason": "no_accepted_episodes", + } + atomic_json(output / "status.json", result) + return result + clips = [ + MeasuredTrajectory.load_episode( + candidate["storage_id"], robot_uid=reference.robot_uid + ) + for candidate in candidates + ] + if any( + set(clip.joint_names) != set(reference.joint_names) for clip in clips + ): + raise ValueError("Preview expansion has incompatible robot joints") + duration = max(clip.timestamps[-1] for clip in [reference, *clips]) + 0.5 + player = _ReferencePlayer(env, reference) + players.callback(player.close) + selected = 0 + links = [ + _precompute_link_poses(clip, robot, env.robot.cfg.fpath) + for clip in clips + ] + last_frame = None + + def show_frame(elapsed): + nonlocal last_frame + original_frame = reference.frame_at(elapsed) + expanded_frames = [clip.frame_at(elapsed) for clip in clips] + current = (original_frame, *expanded_frames, selected) + if current == last_frame: + sim.get_world().update(0.0) + return original_frame, expanded_frames + player.queue(original_frame) + robot.visualize_link_poses( + np.stack( + [poses[frame] for poses, frame in zip(links, expanded_frames)] + )[None], + env_ids=(0,), + ) + poses = np.stack( + [ + clip.object_poses[uid][frame] + for clip, frame in zip(clips, expanded_frames) + for uid in scene_entities + ] + ) + objects.visualize( + translations=poses[:, :3], + orientations=poses[:, 3:], + marker_indices=np.tile(np.arange(len(scene_entities)), len(clips)), + ) + for name, width in ( + ("expansion_robot", robot.num_visuals), + ("expansion_objects", len(scene_entities)), + ): + markers.groups[name].update( + visible=np.repeat(np.arange(len(clips)) == selected, width), + env_ids=(0,), + ) + markers.publish() + # Publish once, after full-arm, object and reference state writes. + sim.sync_render_state() + # Publication only flushes render uploads in the pinned SDK; + # poll window input and present even when every frame changes. + sim.get_world().update(0.0) + last_frame = current + return original_frame, expanded_frames + + # Early windows reuse the warmed meshes; recorded-only previews + # create their first instances before starting the render loop. + show_frame(0) + if window is None: + window = open_window() + window_opened_at = _utc_now() + switches, frames, paused = 0, 0, False + elapsed, previous = 0.0, time.monotonic() + running = True + pressed, held, polled = deque(), set(), set() + preview_keys = { + key.value + for key in ( + InputKey.SCANCODE_Q, + InputKey.SCANCODE_ESCAPE, + InputKey.SCANCODE_N, + InputKey.SCANCODE_SPACE, + ) + } + + class PreviewInput(InputControl): + def on_key_down(self, key): + if key in preview_keys and key not in held: + held.add(key) + pressed.append(key) + + def on_key_up(self, key): + held.discard(key) + + control = PreviewInput() + window.add_input_control(control) + cleanup.callback(window.remove_input_control, control) + result = { + "state": "playing", + "trajectories": len(candidates), + "renderer": renderer, + "robot_layers": 1, + "object_instances": len(scene_entities), + "preloaded_robots": len(clips), + "preloaded_object_instances": len(clips) * len(scene_entities), + "marker_render_mode": "overlay", + "pid": os.getpid(), + "world_object_id": id(sim.get_world()), + "num_envs": sim.num_envs, + "window_opened_at": window_opened_at, + } + + def publish(): + result.update( + selected_index=selected, + operator=candidates[selected]["operator"], + episode_id=candidates[selected]["episode_id"], + switches=switches, + frames=frames, + state="paused" if paused else "playing", + original_frame=original_frame, + expanded_frame=expanded_frames[selected], + elapsed_s=elapsed, + expanded_frames=expanded_frames, + marker_publications=markers.publications, + solid_qpos=env.robot.get_qpos()[0].detach().cpu().tolist(), + ) + atomic_json(output / "status.json", result) + window.set_window_title( + f"Expansion {selected + 1}/{len(candidates)} | {candidates[selected]['operator']} | N next | Space pause | Q close" + ) + + while ( + running + and sim.get_world().is_window_initialized() + and (max_frames is None or frames < max_frames) + ): + started = time.monotonic() + changed = frames == 0 + # Queue native key edges so a short tap survives a slow render + # frame. A newly opened viewport may not dispatch control + # callbacks before its first click, so retain held-key edges too. + down = {key for key in preview_keys if sim.get_world().key_state(key)} + for key in down - polled: + if key not in held: + held.add(key) + pressed.append(key) + held.difference_update(polled - down) + polled = down + while pressed: + key = pressed.popleft() + if key in ( + InputKey.SCANCODE_Q.value, + InputKey.SCANCODE_ESCAPE.value, + ): + running = False + elif key == InputKey.SCANCODE_N.value: + selected = (selected + 1) % len(candidates) + switches += 1 + changed = True + elif key == InputKey.SCANCODE_SPACE.value: + paused, changed = not paused, True + if not running: + break + if not paused: + elapsed += started - previous + previous = started + elapsed %= duration + original_frame, expanded_frames = show_frame(elapsed) + frames += 1 + if frame_observer is not None: + frame_observer(env, frames, selected, elapsed) + if not sim.get_world().is_window_initialized(): + break + if changed: + publish() + time.sleep(max(0.0, 1 / 30 - (time.monotonic() - started))) + result.update( + state="closed", + frames=frames, + switches=switches, + physics_steps=sim.physics_step_index - physics_steps_before, + ) + atomic_json(output / "status.json", result) + return result diff --git a/embodichain/lab/expansion/contracts.py b/embodichain/lab/expansion/contracts.py index e0612291d2..81d3265b36 100644 --- a/embodichain/lab/expansion/contracts.py +++ b/embodichain/lab/expansion/contracts.py @@ -115,19 +115,9 @@ def _poses(values: Mapping[str, torch.Tensor], name: str) -> Mapping[str, torch. def _json(value: object) -> object: - if value is None or type(value) in (str, bool, int): - return value - if type(value) is float and math.isfinite(value): - return value - if isinstance(value, (list, tuple)): - return tuple(_json(item) for item in value) - if isinstance(value, Mapping): - result = {} - for key, item in value.items(): - _text(key, "metadata key") - result[key] = _json(item) - return MappingProxyType(result) - raise ValueError("metadata must contain only finite JSON values") + from embodichain.utils._owned_json import freeze_metadata + + return freeze_metadata(value) def _phases( diff --git a/embodichain/lab/expansion/distribution.md b/embodichain/lab/expansion/distribution.md new file mode 100644 index 0000000000..c54199475c --- /dev/null +++ b/embodichain/lab/expansion/distribution.md @@ -0,0 +1,79 @@ +# Task distribution report + +Integrators call the lazy public `embodichain.lab.expansion.report_expansion_distribution` +with before/after native jobs, expansion aggregates or workflow directories. It +uses the same receipt reader and report writer as the command below, returns the +HTML path and snapshot identity, and creates no simulation World. Preserve the +complete output directory, including detail pages and JSONL records. + +Run from an EmbodiChain environment with the existing artifact reader dependencies: + +```bash +python -m embodichain.lab.expansion._distribution \ + --before /absolute/job-a \ + --after /absolute/job-a /absolute/job-b \ + --entity cube --cell-size 0.1 --minimum 5 \ + --output /absolute/new-report-directory +``` + +Inputs must be job directories containing `job.json` and confirmed artifact receipts. +Repeated input paths/commit identities do not multiply samples. Conflicting commit +identities fail. Output must be new; inputs are never overwritten. Both snapshots +must have the same task, robot installation, topology and configured scene domain. +The recorded scene-case task ID identifies the task across seeds and source +recordings. Older artifacts can use their recorded source UUID when the task ID +is absent. Artifacts without either identity cannot be compared. + +For installation identity only, measured XYZ and sign-canonical normalized xyzw +components are rounded to 1e-6 to avoid splitting one fixed base by readout noise. +Declared robot calibration remains exact in the domain fingerprint. Raw episode +poses, TCP FK and every physical acceptance threshold remain unchanged. + +Open `report.html`. Select a spatial region or joint-distribution cell to inspect +recorded object paths. The evidence panel identifies the full artifact and the +existing native preview command (which retains its original preload limit). +The report itself needs no server or external network. `summary.json` carries +version, snapshot hash, domain, distributions, observed-bin deficits and limitations; +`before-episodes.jsonl` and `after-episodes.jsonl` carry source references and metrics. + +Version 2 adds full orientation statistics: normalized sign-canonical xyzw, +initial/final SO(3) rotation vectors, angular path length, net rotation and angular +speed quantiles. Three-dimensional rotation-vector bins have 30-degree edges; +they are not equal-volume and no object symmetry is assumed. Original full XYZ +and quaternion poses remain in every episode record. + +TCP is explicitly derived, not directly measured: recorded full joint states and +root poses drive the recorded URDF chain, followed by the single declared gripper +root-to-TCP calibration. The record includes the asset digest, link, transform and +method. Missing/ambiguous calibration returns unavailable; malformed transforms +fail. FK runs in 256-frame batches. Scaled robots are not supported. This is the +same recorded-URDF FK approach used by the existing preview, extended with TCP +calibration; no physics World is created. + +`summary.json` now holds aggregate statistics and a lightweight episode index, +not all trajectory details. `pages/{snapshot}-{page}.json` and matching local JS +files contain 100 records per page; the latter lets the offline HTML open without +an HTTP service. Agent tools use JSON. The viewer loads details on demand, retains +at most two detail pages, and displays 50 episode options per page. Joint heatmaps +are paged in 20-by-20 blocks; orientation counts are paged in rows of 20. Filters +operate on the full index before pagination. Index memory remains O(N); extracting +features also retains O(N) bounded descriptors, not full raw trajectories. Page +files and JSONL contain identical episode records. + +World-XY maps remain the spatial projection; trajectory display switches between +object and TCP XY paths. Raw numeric evidence is never changed. Joint path length +is in joint-coordinate units and must not be interpreted as Cartesian metres. +Whole-episode metrics and existing phase-aware geometry buckets remain separate. + +Counts cover observed cells only; no continuous workspace coverage or unobserved +feasibility claim is made. Different physical parents remain separate samples. +Use physical collection roots, not visual-child datasets. Comparison is cumulative +only if every before commit occurs in after; other comparisons are independent. +No automatic collection, arbitrary LeRobot import or learned clustering is added. + +Validation dataset: the complete `formal-qualified` pick-place matrix has 40 +physical job roots (two scenes, five seeds, four sampling/scheduling variants). +For each scene, the before snapshot is variant A across five seeds; after is +the cumulative union A+B+C+D. Interrupted diagnostics and visual children are +excluded. This compares dataset composition, not a causal learning experiment. +Use all matching job roots in `--before` and `--after`; retain scene separation. diff --git a/embodichain/lab/expansion/distribution.py b/embodichain/lab/expansion/distribution.py new file mode 100644 index 0000000000..128d608fe2 --- /dev/null +++ b/embodichain/lab/expansion/distribution.py @@ -0,0 +1,75 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Offline distribution reports from confirmed native collection artifacts.""" + +from __future__ import annotations + +from collections.abc import Sequence +import json +from pathlib import Path + +__all__ = ["report_expansion_distribution"] + + +def report_expansion_distribution( + before: Sequence[str | Path], + after: Sequence[str | Path], + output: str | Path, + *, + entity: str, + cell_size_m: float = 0.01, + minimum: int = 1, +) -> dict: + """Compare matching-domain receipts without creating a simulation World. + + Args: + before: Native jobs, expansion aggregates or workflow run directories. + after: Independent or cumulative matching-domain collection directories. + output: New destination for the complete HTML/JSON/JSONL report bundle. + entity: Recorded rigid entity whose measured states are compared. + cell_size_m: World-XY bin width, selected before examining results. + minimum: Required episode count for observed-bin deficit diagnostics. + + Returns: + Report directory, HTML entry, snapshot identity and confirmed counts. + + Raises: + ValueError: For invalid inputs, mismatched domains or corrupt evidence. + FileExistsError: If the destination already exists. + """ + from ._distribution import _run + from .preview import _inputs + + def jobs(paths): + return _inputs( + [ + Path(p) / "expand" if (Path(p) / "run.json").is_file() else Path(p) + for p in paths + ] + ) + + output = Path(output).expanduser().resolve() + _run(jobs(before), jobs(after), output, entity, cell_size_m, minimum) + report = json.loads((output / "summary.json").read_text()) + return { + "directory": str(output), + "html": str(output / "report.html"), + "snapshot_id": report["snapshot_id"], + "comparison_kind": report["comparison_kind"], + "before": report["before"]["count"], + "after": report["after"]["count"], + } diff --git a/embodichain/lab/expansion/engine.py b/embodichain/lab/expansion/engine.py index 7e2aa3543d..1c5db9e250 100644 --- a/embodichain/lab/expansion/engine.py +++ b/embodichain/lab/expansion/engine.py @@ -225,6 +225,8 @@ def __init__( self._write_failures: set[str] = set() self._resolved_receipts: dict[tuple[str, int], CommitReceipt] = {} self._failed = False + self._row_assignments: dict[int, ExpansionAssignment] = {} + self._started_rows: set[int] = set() self.timings: dict[str, float] = {} def _measure(self, name, operation, *args, **kwargs): @@ -327,6 +329,8 @@ def run(self) -> Mapping[str, object]: return self._drain() def _drain(self, *, one_batch: bool = False) -> Mapping[str, object]: + if self._row_assignments: + raise RuntimeError("finish active rolling rows before draining a cohort") if self._failed: raise RuntimeError( "host failed; construct a new host before further execution" @@ -476,6 +480,154 @@ def on_started(identity: CandidateIdentity) -> None: break return self._snapshot() + def admit_row( + self, item: PreparedEpisodeWorkItem, *, speculative: bool = False + ) -> ExpansionAssignment | None: + """Admit one independently prepared physical row through the native session. + + Args: + item: Host-owned candidate proposed in this session, bound to its + actual World row. Cohort collection remains unchanged. + speculative: Permit a competing rollout while quota is unfilled; + persistence still reserves at most the original target count. + + Returns: + Assigned episode/commit identity, or None for rejected preparation + or session backpressure. No rollout starts until + :meth:`mark_row_started` is called after a consumed command. + """ + if self._configured_cfg is not None or self._failed: + raise RuntimeError("row admission requires a healthy custom engine") + if type(speculative) is not bool: + raise TypeError("speculative must be a bool") + if not isinstance(item, PreparedEpisodeWorkItem): + raise TypeError("rolling admission requires PreparedEpisodeWorkItem") + if ( + not 0 <= item.env_id < self.cfg.batch_size + or item.env_id in self._row_assignments + ): + raise ValueError("physical row is outside capacity or already assigned") + self.flush() + self.enqueue((item,)) + identity = item.candidate.identity + try: + checked = self._measure("admission", self.host.validate_plan, item) + if not checked.accepted: + self.session.release(identity, reason="planning_validation_failed") + self._queue.remove(item) + return None + self.session.add_prepared_episode(item.candidate, checked) + self._admitted.add(identity.candidate_id) + ready = self.session.take_ready( + identity.scene_case_id, + identity.initial_state_id, + episode_byte_budget=self.cfg.episode_byte_budget, + candidate_id=identity.candidate_id, + speculative=speculative, + ) + if ready is None: + self.session.release(identity, reason="row_admission_backpressure") + self._queue.remove(item) + return None + episode_id, commit_id = self.session.episode_ids(identity) + assignment = ExpansionAssignment(item, item.env_id, episode_id, commit_id) + self._row_assignments[item.env_id] = assignment + return assignment + except BaseException: + self._failed = True + self.session.release(identity, reason="row_admission_failed") + if item in self._queue: + self._queue.remove(item) + raise + + def mark_row_started(self, assignment: ExpansionAssignment) -> None: + """Count a rolling rollout after its first actual World command. + + Args: + assignment: Exact identity returned by :meth:`admit_row`. + """ + row = assignment.env_id + if self._row_assignments.get(row) != assignment or row in self._started_rows: + raise ValueError("unexpected or repeated rolling rollout start") + self.session.mark_rollout_started(assignment.item.spec.identity) + self._started_rows.add(row) + + def complete_row( + self, assignment: ExpansionAssignment, episode: ExpertEpisode | None + ) -> CommitReceipt | None: + """Qualify and persist a row using ordinary native receipt accounting. + + Args: + assignment: Exact active row identity returned by :meth:`admit_row`. + episode: Measured evidence for the consumed commands, or rejection. + + Returns: + Confirmed native receipt or None for rejected evidence. Unconfirmed + writes raise and retain pending evidence for the normal reconciliation + API; a physical slot cannot earn quota before persistence confirms. + """ + row = assignment.env_id + if self._row_assignments.get(row) != assignment: + raise ValueError("completion does not identify an active rolling row") + identity = assignment.item.spec.identity + if episode is not None and ( + not isinstance(episode, ExpertEpisode) + or episode.identity != identity + or row not in self._started_rows + or (episode.episode_id, episode.commit_id) + != (assignment.episode_id, assignment.commit_id) + ): + self._failed = True + self.session.release(identity, reason="invalid_measured_evidence") + raise ValueError("host returned mismatched or unstarted evidence") + receipt = None + if episode is None: + self.session.release(identity, reason="host_rejected") + else: + accepted = self.session.accept_episode(episode) + if accepted: + self._pending[episode.commit_id] = episode + callback = getattr(self.host, "on_acceptance", None) + if callback is not None: + callback(assignment, episode, accepted) + if accepted: + self.flush() + key = episode.commit_id, self._submission_ids.get(episode.commit_id, 0) + receipt = self._resolved_receipts.get(key) + if receipt is None or not receipt.confirmed: + raise RuntimeError( + "unconfirmed persistence cannot release a rolling row" + ) + self._queue.remove(assignment.item) + del self._row_assignments[row] + self._started_rows.discard(row) + return receipt + + def cancel_row(self, assignment: ExpansionAssignment, *, reason: str) -> None: + """Retire an uncommitted row without a receipt or fabricated rejection. + + Args: + assignment: Exact active identity returned by :meth:`admit_row`. + reason: Explicit cancellation reason retained in native diagnostics. + + Raises: + ValueError: For an inactive row or work with pending persistence. + """ + row = assignment.env_id + if self._row_assignments.get(row) != assignment: + raise ValueError("cancellation does not identify an active rolling row") + if ( + assignment.commit_id in self._pending + or assignment.commit_id in self._submitted + ): + raise ValueError( + "pending persistence must be reconciled before cancellation" + ) + self.session.release(assignment.item.spec.identity, reason=reason) + self._queue.remove(assignment.item) + del self._row_assignments[row] + self._started_rows.discard(row) + def run_source(self, source: ExpansionRecipeSource) -> Mapping[str, object]: """Drive bounded source batches through the same admission/execution/writer core. diff --git a/embodichain/lab/expansion/episode_export.py b/embodichain/lab/expansion/episode_export.py new file mode 100644 index 0000000000..8c578a251a --- /dev/null +++ b/embodichain/lab/expansion/episode_export.py @@ -0,0 +1,129 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""One independently readable LeRobot dataset per accepted expansion episode.""" + +from __future__ import annotations + +import hashlib +import json +from pathlib import Path +import shutil +from tempfile import TemporaryDirectory +from uuid import NAMESPACE_URL, uuid5 + +from embodichain.data_pipeline.recording.media import ( + _digest as file_digest, + _atomic_json as atomic_json, +) + +__all__ = ["export_expansion_episodes"] + + +def _verify_dataset(dataset: Path, expected: dict) -> None: + from embodichain.data_pipeline.recording.journal import inspect_recording + from embodichain.lab.expansion import ArtifactEpisodeSink + + saved = json.loads((dataset / "expansion-episode.json").read_text()) + if saved != expected: + raise ValueError(f"LeRobot dataset belongs to different evidence: {dataset}") + info = json.loads((dataset / "meta/info.json").read_text()) + if (info["total_episodes"], info["total_frames"]) != (1, expected["steps"]): + raise ValueError(f"Expected exactly one complete LeRobot episode: {dataset}") + report = inspect_recording(dataset) + identity = str(uuid5(NAMESPACE_URL, expected["episode_id"])) + if not report["ok"] or [row["episode_uuid"] for row in report["commits"]] != [ + identity + ]: + raise ValueError(f"Incomplete LeRobot recording: {dataset}: {report}") + receipts = ArtifactEpisodeSink(dataset / "embodichain_artifacts").inspect() + if [(row.episode_id, row.commit_id) for row in receipts] != [ + (expected["episode_id"], expected["commit_id"]) + ]: + raise ValueError(f"LeRobot evidence bundle differs: {dataset}") + + +def export_expansion_episodes(directory: str | Path) -> dict: + """Save each accepted episode under its source-owned output directory. + + Each returned + ``dataset_path`` is relative to ``directory/lerobot`` and contains exactly + one LeRobot episode. The full T+1 evidence and media remain in the portable + ``embodichain_artifacts/`` bundle alongside the T action rows. + + Args: + directory: Source-owned ArtifactEpisodeSink directory with confirmed + native receipts. It is read and fully verified before exporting. + + Returns: + Manifest with the episode count, export directory and content-addressed + relative dataset paths. Each dataset contains exactly one accepted + episode and its complete portable source/media evidence. Existing exports + are verified rather than silently overwritten. No World is constructed. + """ + from embodichain.lab.expansion import ArtifactEpisodeSink, load_expansion_episode + from embodichain.lab.expansion.export import export_expansion_lerobot + + directory = Path(directory).resolve() + if not directory.is_dir(): + raise FileNotFoundError(f"Expansion directory does not exist: {directory}") + receipts = ArtifactEpisodeSink(directory).inspect() + output = directory / "lerobot" + output.mkdir(parents=True, exist_ok=True) + datasets = [] + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + # Content-addressed names cannot introduce path components from user IDs. + key = hashlib.sha256(receipt.commit_id.encode()).hexdigest() + destination = output / key + identity = { + "schema_version": 1, + "episode_id": episode.episode_id, + "commit_id": episode.commit_id, + "source_sha256": file_digest(Path(receipt.storage_id)), + "steps": len(episode.actions), + } + if destination.exists(): + _verify_dataset(destination, identity) + else: + # The native exporter accepts a complete job. Give it one frozen + # accepted episode, then publish only the self-contained dataset. + # No environment, physics replay or render window is constructed. + with TemporaryDirectory(prefix=".lerobot-", dir=output) as temporary: + isolated = Path(temporary) / "job" + source = Path(receipt.storage_id).parent + if (source / "artifacts.json").is_file(): + # Portable committed evidence is already frozen and fully + # verified above. Preserve its bytes and receipts instead + # of loading/freezing/compressing an identical new episode. + shutil.copytree(source, isolated / "episodes" / source.name) + else: + ArtifactEpisodeSink(isolated).submit(episode, submission_id=0) + result = export_expansion_lerobot(None, isolated) + dataset = Path(result["dataset_path"]) + atomic_json(dataset / "expansion-episode.json", identity) + _verify_dataset(dataset, identity) + dataset.rename(destination) + datasets.append({**identity, "dataset_path": key}) + manifest = { + "schema_version": 1, + "format": "lerobot", + "episodes": len(datasets), + "datasets": datasets, + } + atomic_json(output / "manifest.json", manifest) + return {**manifest, "directory": str(output)} diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py new file mode 100644 index 0000000000..44d2cd8d77 --- /dev/null +++ b/embodichain/lab/expansion/gym_workloads.py @@ -0,0 +1,1091 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Native single-World collection for independent Task Program workloads.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from concurrent.futures import ThreadPoolExecutor +from copy import deepcopy +from dataclasses import asdict, dataclass, replace +from pathlib import Path +import json +import hashlib +import time +from typing import Any + +from embodichain.data_pipeline.recording.media import _atomic_json +from .configured import ExpansionRunCfg +from .host_extensions import ExpansionRowContext, GymExpansionHostExtensions +from .workloads import ( + ExpansionWorkloadCfg, + WorkloadAssignment, + WorkloadExecutionCfg, + WorkloadProgramExecutor, + WorkloadRowResult, +) + +__all__ = ["GymExpansionWorkload", "run_gym_expansion_workloads"] + + +@dataclass(frozen=True) +class GymExpansionWorkload: + """Task-owned declarations for one independently budgeted source group. + + Args: + workload_id: Unique logical source group, independent of its World row. + cfg: Deployment, output, evidence limits and original collection budget. + report: Pinned semantic/execution identities and runtime versions. + source_artifacts: Hashed configuration/source references for each episode. + extensions: Scene, recipe and plan callbacks; rendering and validation + belong to the common extensions passed to the collection function. + + Deployments may select different integration/grasp policies, but must share + exactly the same physical environment, robot, control cadence and program. + This facade supports native Task Programs with row-local scene/plan ports; + configured physical/visual/prototype operators use the cohort job API. + """ + + workload_id: str + cfg: ExpansionRunCfg + report: Mapping[str, object] + source_artifacts: Mapping[str, object] + extensions: GymExpansionHostExtensions = GymExpansionHostExtensions() + + +def _preview_snapshot(env: Any) -> dict[str, object]: + """Freeze actual resolved geometry, including finite JSON parameter encoding.""" + from enum import Enum + import math + + def encode(value: Any) -> Any: + if isinstance(value, Enum): + return encode(value.value) + if isinstance(value, Mapping): + return {str(key): encode(item) for key, item in value.items()} + if isinstance(value, (tuple, list)): + return [encode(item) for item in value] + if isinstance(value, float) and not math.isfinite(value): + if math.isnan(value): + raise ValueError("resolved preview configuration contains NaN") + return "+inf" if value > 0 else "-inf" + if value is None or isinstance(value, (bool, int, float, str)): + return value + if isinstance(value, Path): + return str(value) + if hasattr(value, "tolist"): + return encode(value.tolist()) + return str(value) + + return encode( + { + "robot": env.robot.cfg.to_dict(), + "rigid_object": [ + env.sim.get_rigid_object(uid).cfg.to_dict() + for uid in env.sim.get_rigid_object_uid_list() + ], + "ground_plane_visible": getattr(env.cfg, "ground_plane_visible", True), + } + ) + + +class _GymWorkloadSource: + """Compose existing native admission, frame assembly and persistence owners.""" + + def __init__( + self, owner: Any, groups: Mapping[str, Any], *, reuse_idle_rows: bool = False + ) -> None: + from ._gym_frames import GymFrameCollector + + self.owner, self.groups, self.env = owner, groups, owner.env + self.reuse_idle_rows = reuse_idle_rows + self.collector = GymFrameCollector(owner._context, {}) + self.active: dict[int, dict[str, Any]] = {} + self.activation_snapshot = None + self.nominal = { + uid: self.env.sim.get_rigid_object(uid) + .get_local_pose(to_matrix=True)[0] + .detach() + .cpu() + .tolist() + for uid in self.env.sim.get_rigid_object_uid_list() + } + + def planning_budget(self, assignment): + feedback = self.groups[assignment.workload_id]["host"]._context.feedback + return ( + None + if feedback is None + else getattr(feedback, "planning_budget", feedback.budget) + ) + + def can_admit(self, name): + feedback = self.groups[name]["host"]._context.feedback + if feedback is None: + return True + if self.groups[name]["session"].stop_reason is not None: + return False + from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted + + try: + feedback.budget.check() + except FeasibilityBudgetExhausted: + return False + return True + + def prepare(self, assignments: tuple[WorkloadAssignment, ...]) -> None: + from embodichain.lab.sim.scene import ( + SceneView, + SceneChangeSet, + apply_changes_batch, + ) + from .scene import apply_joint_scene_candidates + + stamp = time.monotonic() + self.activation_snapshot = None + ids = tuple(a.env_id for a in assignments) + view = SceneView(self.env.sim, env_ids=ids) + # Every candidate starts from the same seeded nominal reset. Original + # scene reset events were frozen before rolling admission began. + apply_changes_batch( + view, + {row: SceneChangeSet(self.nominal, tuple(self.nominal)) for row in ids}, + ) + candidates = {} + for assignment in assignments: + group = self.groups[assignment.workload_id] + if group["admitted_at"][0] is None: + group["admitted_at"][0] = time.monotonic() + ctx = group["host"]._context + request = ctx.journal.allocate_recipe(ctx.recipe(assignment.ordinal)) + request.recipe.validate_host(ctx.supported_operations, ctx.compatibility) + for operation in request.recipe.operations: + if operation.name not in { + "physical_parameters", + "joint_scene", + "motion", + }: + raise ValueError( + "rolling workloads only support row-local scene and motion operations" + ) + if operation.name == "physical_parameters" and any( + json.loads(operation.payload_json).values() + ): + raise ValueError( + "physical parameter changes require the cohort job API" + ) + ctx.requests[assignment.env_id] = request + if ctx.scene_source is not None: + candidate = ctx.scene_source.candidate(assignment.ordinal) + if ( + ctx.feedback is None + or ctx.feedback.validate(assignment.ordinal).accepted + ): + candidates[assignment.env_id] = candidate + if candidates: + apply_joint_scene_candidates( + SceneView(self.env.sim, env_ids=tuple(candidates)), candidates + ) + self.owner.timings["prepare"] += time.monotonic() - stamp + + def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: + from embodichain.lab.sim.scene import capture_state + from embodichain.lab.sim.scene.physics import capture_physics_properties + from .contracts import ( + EpisodeCandidate, + SceneCase, + ValidationCheck, + ValidationResult, + ) + from .engine import PreparedEpisodeWorkItem + from ._gym_utils import _hash + + row = assignment.env_id + group = self.groups[assignment.workload_id] + host, engine = group["host"], group["engine"] + ctx = host._context + request = ctx.requests[row] + session = group["session"] + budget_stops = ("wall_time_exhausted", "rollout_budget_exhausted") + stop_reason = session.stop_reason + if stop_reason in budget_stops: + ctx.journal.update(request, status="initial_rejected", reason=stop_reason) + return None + if self.activation_snapshot is None: + self.activation_snapshot = ( + capture_state(ctx.view), + capture_physics_properties(ctx.view), + ) + initial, physics = self.activation_snapshot + ctx.initial, ctx.initial_obs = initial, observation + ctx.physics_before = physics + collision = ctx.collision + if collision is not None: + collision.prepare(initial, env_ids=(row,)) + values = [asdict(entity) for entity in initial.batches[row]] + initial_id = _hash(values) + physical = json.loads( + next( + op.payload_json + for op in request.recipe.operations + if op.name == "physical_parameters" + ) + ) + parameters = { + "ordinal": assignment.ordinal, + "seed": ctx.cfg.seed, + "physics": physical, + "source": ctx.cfg.source, + "recipe_id": request.recipe.recipe_id, + } + for op in request.recipe.operations: + if op.name == "motion": + parameters["motion_recipe"] = json.loads(op.payload_json)[ + "recipe_index" + ] + elif op.name == "joint_scene": + parameters["scene_candidate_id"] = json.loads(op.payload_json)[ + "candidate_id" + ] + case = SceneCase( + _hash( + ( + ctx.compatibility.key, + physical, + parameters.get("scene_candidate_id"), + request.recipe.recipe_id, + ) + ), + initial_id, + ctx.report["semantic_id"], + ctx.program.program_id, + self.env.robot.uid, + ) + session.register_case(case, ctx.limits, joint_names=ctx.names) + try: + identity, _ = session.propose( + case.scene_case_id, + initial_id, + source_id=ctx.program.program_id, + source_revision=ctx.report["recipe_revision"], + template_id=request.recipe.recipe_id, + operator_id="episode", + attempt_id=request.attempt_id, + ) + except RuntimeError: + # The budget can expire during initial-state validation, including + # between the early check and the session's atomic proposal guard. + stop_reason = session.stop_reason + if stop_reason not in budget_stops: + raise + ctx.journal.update(request, status="initial_rejected", reason=stop_reason) + return None + episode_id, commit_id = session.episode_ids(identity) + record = ctx.journal.update( + request, + status="prepared", + recipe=parameters, + case=asdict(case), + identity=asdict(identity), + episode_id=episode_id, + commit_id=commit_id, + joint_names=ctx.names, + initial_state=values, + ) + ctx.prepared[row] = identity, episode_id, commit_id, request.directory, record + valid = collision is None or collision.initial[row] + ctx.preparation_checks[row] = ValidationResult( + ( + ValidationCheck("preparation.source_configuration", "passed"), + ValidationCheck( + "preparation.initial_state", "passed" if valid else "failed" + ), + ValidationCheck( + "preparation.settling_steps", + "passed", + metrics={"steps": ctx.cfg.settling_steps}, + ), + ) + ) + if ctx.feedback is not None: + ctx.preparation_checks[row] = ValidationResult( + ctx.preparation_checks[row].checks + + ctx.feedback.validate(assignment.ordinal).checks + ) + item = PreparedEpisodeWorkItem( + EpisodeCandidate(identity, json.dumps(request.recipe.to_dict())), + ctx.compatibility.key, + row, + ) + native = engine.admit_row(item, speculative=self.reuse_idle_rows) + if native is None: + ctx.journal.update(request, status="initial_rejected") + return None + factory = None + if ctx.extensions.plan_transform_factory is not None: + factory = ctx.extensions.plan_transform_factory( + self.env, + ExpansionRowContext( + row, + identity, + assignment.ordinal, + request.attempt_id, + request.directory, + case, + request.recipe, + ), + ) + if factory is None: + raise ValueError("plan_transform_factory returned no factory") + adapter = group["adapter"].for_env_ids((row,)) + bridge = adapter.create_bridge( + ctx.program, plan_transform_factory=factory, isolate_policy_sessions=True + ) + # The frame owner is shared and observes all active rows once per step. + self.owner._context.initial, self.owner._context.initial_obs = ( + initial, + observation, + ) + self.collector.add({row: ctx.prepared[row]}) + self.active[row] = { + "group": group, + "native": native, + "adapter": adapter, + "factory": factory, + "physics_before": ctx.physics_before, + } + return bridge + + def started(self, assignment: WorkloadAssignment, world_control_step: int) -> None: + row = assignment.env_id + state = self.active[row] + ctx = state["group"]["host"]._context + state["group"]["engine"].mark_row_started(state["native"]) + if ctx.feedback is not None: + ctx.feedback.budget.counts["rollouts"] += 1 + ctx.prepared[row][4]["world_control_start"] = world_control_step + ctx.journal.update( + ctx.requests[row], status="running", world_control_start=world_control_step + ) + + def observe( + self, + assignments: tuple[WorkloadAssignment, ...], + action: Any, + observation: object, + ) -> Sequence[int]: + ids = tuple(a.env_id for a in assignments) + self.activation_snapshot = None + for name in {a.workload_id for a in assignments}: + self.groups[name]["host"]._context.journal.checkpoint() + self.collector.observe(ids, action, observation) + if self.reuse_idle_rows: + row_metadata = action.metadata.get("row_actions", {}) + for row in ids: + # Retain one already-owned metadata view, rather than copying + # command histories on the control hot path. + self.active[row]["last_controller_metadata"] = row_metadata.get( + str(row), {} + ) + checks = self.owner.collision + return tuple( + row + for row in ids + if checks is not None + and row in checks.results + and checks.results[row].status != "passed" + ) + + def complete(self, result: WorkloadRowResult) -> Any: + from embodichain.lab.sim.scene.physics import capture_physics_properties + from ._gym_episode import GymEpisodeAssembler + + row = result.assignment.env_id + state = self.active.pop(row, None) + if state is None: + ctx = self.groups[result.assignment.workload_id]["host"]._context + if result.program.reasons == ("wall_time_exhausted",): + ctx.journal.update( + ctx.requests[row], + status="initial_rejected", + reason="wall_time_exhausted", + ) + if ctx.feedback is not None: + ctx.feedback.finish( + result.assignment.ordinal, None, None, executed=False + ) + return None + group = state["group"] + ctx = group["host"]._context + outcome = result.program + factory = state["factory"] + if factory is not None: + records = tuple(factory.records) + ctx.prepared[row][4]["expansion"] = [r.to_metadata() for r in records] + portable = [] + ctx.source_artifacts = { + key: value + for key, value in ctx.source_artifacts.items() + if not key.startswith(f"plan_{row}_") + } + for index, record in enumerate(records): + trace = record.to_metadata() + if "plan_artifact" in trace: + key = f"plan_{row}_{index}" + ctx.source_artifacts[key] = trace.pop("plan_artifact") + trace["plan_artifact_key"] = key + portable.append(trace) + outcome = replace(outcome, expansion_records=(tuple(portable),)) + if ( + ctx.extensions.require_plan_transform + and outcome.accepted[0] + and not records + ): + outcome = replace( + outcome, + accepted=(False,), + reasons=("requested_operator_not_applied",), + ) + ctx.physics_before = state["physics_before"] + ctx.prepared[row][4].update( + world_control_start=result.world_control_start, + world_control_end=result.world_control_end, + ) + try: + episode = GymEpisodeAssembler(ctx).assemble( + {row: ctx.prepared[row]}, + self.collector, + {}, + outcome, + capture_physics_properties(ctx.view), + )[row] + if episode is not None: + episode = replace( + episode, + metadata={ + **episode.metadata, + "execution": { + **self.execution, + "workload_id": result.assignment.workload_id, + "world_control_start": result.world_control_start, + "world_control_end": result.world_control_end, + }, + }, + ) + receipt = None + try: + receipt = group["engine"].complete_row(state["native"], episode) + return receipt + finally: + if ctx.feedback is not None: + ctx.feedback.finish( + result.assignment.ordinal, + episode, + receipt, + executed=result.world_control_start is not None, + ) + finally: + state["adapter"].close() + self.collector.discard(row) + + def cancel(self, result: WorkloadRowResult, *, reason: str) -> None: + """Retire competing work without changing its canonical program outcome.""" + from ._gym_episode import _save_partial + + row = result.assignment.env_id + state = self.active.pop(row, None) + group = self.groups[result.assignment.workload_id] + ctx = group["host"]._context + request = ctx.requests[row] + if ctx.feedback is not None: + ctx.feedback.finish( + result.assignment.ordinal, + None, + None, + executed=result.world_control_start is not None, + ) + record = { + "env_id": row, + "status": "cancelled", + "reason": reason, + "program_accepted": result.program.accepted[0], + "program_reason": result.program.reasons[0], + "program_segments": result.program.segments[0], + "steps": result.program.lengths[0], + "world_control_start": result.world_control_start, + "world_control_end": result.world_control_end, + } + if state is None: + ctx.journal.update(request, **record) + return + try: + group["engine"].cancel_row(state["native"], reason=reason) + record["last_controller_metadata"] = { + key: value + for key, value in state.get("last_controller_metadata", {}).items() + if value is None or type(value) in (str, int, float, bool) + } + measured = ( + None + if self.owner.collision is None + else self.owner.collision.results.get(row) + ) + if measured is not None: + record["measured_collision"] = { + "status": measured.status, + "reason": measured.reason, + } + if state["factory"] is not None: + record["expansion"] = [ + r.to_metadata() for r in state["factory"].records + ] + record["partial_rollout"] = _save_partial( + self.collector.records[row], + request.directory / "cancelled.npz", + self.env.step_dt, + ) + if row in self.collector.spools: + record["media_manifest"] = str(self.collector.spools[row].finish()) + ctx.journal.update(request, **record) + finally: + state["adapter"].close() + self.collector.discard(row) + + def close(self) -> None: + for group in self.groups.values(): + feedback = group["host"]._context.feedback + if feedback is not None: + feedback.checkpoint() + for state in self.active.values(): + state["adapter"].close() + self.active.clear() + + +def run_gym_expansion_workloads( + workloads: Sequence[GymExpansionWorkload], + execution: WorkloadExecutionCfg, + *, + extensions: GymExpansionHostExtensions | None = None, + output_dir: str | Path, +) -> Mapping[str, object]: + """Collect and export independent groups in one process and one owned World. + + The sole collector uses exactly ``workload_count * batch_size`` physical + environments. It captures the seeded nominal scene in this same World; + preparation never creates a reference World. Original per-group quotas, + attempts, measured acceptance and confirmed receipt accounting are retained. + One bounded writer thread exports frozen groups while other groups collect. + Writers receive only artifact paths and never access the live World. + + Args: + workloads: Nonempty unique source groups, each owning a fresh output. + execution: W, B, planning and original step/settling budgets. + extensions: Common collection renderer and measured validator. Per-group + scene/recipe/plan callbacks belong to each workload's extensions. + output_dir: Fresh root for execution/reference/progress diagnostics. + + Returns: + Owned per-workload results and actual process/World/planning evidence. + Partial quotas remain explicit even when qualified artifacts were saved. + """ + from .host import GymExpansionHost + from ._gym_recipe import GymRecipeBuilder + from ._gym_utils import _hash + from .engine import ExpansionEngine, ExpansionJobCfg + from .journal import ExpansionJournal + from .session import ExpansionSession + from .session_cfg import ExpansionSessionCfg + from .persistence import ArtifactEpisodeSink, load_expansion_episode + from .workflow import _RecordedSink + from .episode_export import export_expansion_episodes + from embodichain.lab.gym.envs.managers.cfg import FunctorCfg + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import REGISTERED_ENVS + from embodichain.utils.utility import load_config + from embodichain.lab.gym.envs._json import json_safe_copy + + if not workloads or len({w.workload_id for w in workloads}) != len(workloads): + raise ValueError("workloads must be nonempty with unique IDs") + execution.__post_init__() + if execution.workload_count > len(workloads): + raise ValueError("workload_count exceeds available groups") + common = extensions or GymExpansionHostExtensions() + if ( + common.scene_source_factory + or common.feedback_factory + or common.recipe_transform + or common.plan_transform_factory + ): + raise ValueError( + "common extensions may only select rendering and measured validation" + ) + root = Path(output_dir) + root.mkdir(parents=True, exist_ok=True) + configs = [] + for workload in workloads: + cfg = workload.cfg + cfg.__post_init__() + for field, digest in workload.report["input_files"].items(): + value = getattr(cfg, field, None) + if ( + value is None + or hashlib.sha256(Path(value).read_bytes()).hexdigest() != digest + ): + raise ValueError(f"workload input {field} differs from preflight") + if any( + getattr(cfg, field) != getattr(workloads[0].cfg, field) + for field in ( + "capture_media", + "episode_byte_budget", + "media_buffer_bytes", + ) + ): + raise ValueError("workloads must share media and evidence buffer limits") + if not workload.workload_id or not isinstance(workload.workload_id, str): + raise ValueError("workload IDs must be nonempty text") + if ( + any( + getattr(cfg, field) + for field in ( + "source_profile", + "collision_profile", + "prototype_validation_profile", + "motion_profile", + "scene_atlas", + "visual_profile", + "render_profiles", + "mass_scale", + "friction", + "center_of_mass", + "joint_stiffness_scale", + "joint_damping_scale", + ) + ) + or cfg.source != "task_program" + ): + raise ValueError( + "configured physical/visual/prototype operators require the cohort job API" + ) + if (cfg.seed, cfg.max_steps, cfg.settling_steps) != ( + execution.seed, + execution.max_steps, + execution.settling_steps, + ): + raise ValueError( + "workload and execution seeds/step/settling budgets must match" + ) + if ( + workload.extensions.validator_factory + or workload.extensions.collection_renderer + ): + raise ValueError("rendering and validation must use common extensions") + raw = load_config(cfg.deployment) + configs.append( + ( + raw, + config_to_cfg( + raw, + source_path=Path(cfg.deployment), + task_program_path_override=cfg.program, + ), + ) + ) + first = configs[0][1] + + def physical_key(env_id: str, cfg: Any) -> str: + robot = deepcopy(cfg.robot.to_dict()) + for binding in robot.get("gripper_bindings", {}).values(): + binding["declaration"].pop("root_from_contact_frame", None) + binding["defaults"].pop("root_from_contact_frame", None) + binding["defaults"].get("binding", {}).pop("root_from_contact_frame", None) + physical = { + name: getattr(cfg, name) + for name in ( + "robot", + "sim_cfg", + "rigid_object", + "background", + "target_control_frequency", + "sim_steps_per_control", + "events", + "sensor", + "expert_trajectory", + "actions", + "control_parts", + "active_joint_ids", + "scene_preparation", + ) + if hasattr(cfg, name) + } + physical["robot"] = robot + # EmbodiedEnv uses the registration default only without an explicit + # hook. Normalize both routes without mutating either declaration. + if physical.get("scene_preparation") is None: + default = REGISTERED_ENVS[env_id].default_kwargs.get("scene_preparation") + if default is not None: + physical["scene_preparation"] = FunctorCfg(**deepcopy(default)) + return _hash( + { + name: value.to_dict() if hasattr(value, "to_dict") else value + for name, value in physical.items() + } + ) + + if any( + physical_key(raw["id"], cfg) != physical_key(configs[0][0]["id"], first) + for raw, cfg in configs[1:] + ): + raise ValueError( + "workloads must share physical environment, robot, sensors, cadence, expert action schema and scene preparation" + ) + + owner = source = None + feedback_closers = [] + groups: dict[str, Any] = {} + adapters: dict[int, Any] = {} + futures: dict[str, Any] = {} + started = time.monotonic() + writer = ThreadPoolExecutor(max_workers=1, thread_name_prefix="expansion-export") + try: + cfg = ExpansionRunCfg( + **{ + **workloads[0].cfg.to_dict(), + "batch_size": execution.num_envs, + "seed": ( + workloads[0].cfg.seed + if execution.nominal_seed is None + else execution.nominal_seed + ), + } + ) + owner = GymExpansionHost( + cfg, + workloads[0].report, + root, + ExpansionJournal(root), + workloads[0].source_artifacts, + extensions=common, + ) + env = owner.env + adapters[ + id(REGISTERED_ENVS[configs[0][0]["id"]].task_program_adapter_factory) + ] = env.task_program_adapter + # Sampling only row zero retains the original B=1 seeded reference + # regardless of the collection capacity. All rows use that same scene. + import inspect + from embodichain.lab.gym.envs.managers.event_manager import EventManager + + events = getattr(env, "event_manager", None) + if events is not None: + for name in events.active_functors.get("reset", ()): + event = deepcopy(events.get_functor_cfg(name)) + if "allow_partial_envs" in inspect.signature(event.func).parameters: + # No trajectories are live during this one nominal reset. + event.params["allow_partial_envs"] = True + events.set_functor_cfg(name, event) + env.reset( + seed=execution.reset_seed, options={"save_data": False, "reset_ids": [0]} + ) + # The reference now owns all reset-time scene randomization/settling. + # Re-running those functors during row refill could advance live peers + # outside the executor and lose measured control boundaries. + if events is not None: + frozen_events = { + name: deepcopy(events.get_functor_cfg(name)) + for mode, names in events.active_functors.items() + if mode != "reset" + for name in names + } + env.cfg.events = frozen_events + env.event_manager = EventManager(frozen_events, env) + snapshot = _preview_snapshot(env) + nominal = { + uid: env.sim.get_rigid_object(uid) + .get_local_pose()[0] + .detach() + .cpu() + .tolist() + for uid in env.sim.get_rigid_object_uid_list() + } + _atomic_json( + root / "reference-scene.json", + { + "seed": execution.reset_seed, + "sampling_seed": execution.seed, + "poses_xyzw": nominal, + }, + ) + for workload, (raw, environment_cfg) in zip(workloads, configs, strict=True): + path = Path(workload.cfg.output_dir) + path.mkdir(parents=True, exist_ok=False) + cfg = ExpansionRunCfg( + **{**workload.cfg.to_dict(), "batch_size": execution.num_envs} + ) + report = { + **deepcopy(dict(workload.report)), + "resolved_deployment": snapshot, + } + journal = ExpansionJournal(path) + journal.open_run(report, cfg.to_dict(), resume=False) + _atomic_json(path / "preflight.json", report) + ctx = replace( + owner._context, + cfg=cfg, + report=report, + root=path, + journal=journal, + source_artifacts=deepcopy(dict(workload.source_artifacts)), + environment_cfg=environment_cfg, + requests={}, + prepared={}, + preparation_checks={}, + services={}, + bridges={}, + extensions=workload.extensions, + scene_source=None, + feedback=None, + ) + if workload.extensions.scene_source_factory is not None: + ctx.scene_source = workload.extensions.scene_source_factory(env) + if workload.extensions.feedback_factory is not None: + ctx.feedback = workload.extensions.feedback_factory( + env, ctx.scene_source, path + ) + close = getattr(ctx.feedback, "close", None) + if close is not None: + feedback_closers.append(close) + builder = GymRecipeBuilder(ctx) + transform = workload.extensions.recipe_transform + ctx.recipe = lambda ordinal, builder=builder, transform=transform: ( + builder.recipe(ordinal) + if transform is None + else transform(builder.recipe(ordinal)) + ) + # Non-owning facade delegates existing admission/acceptance methods; + # only owner above opens/closes the shared environment. + host = GymExpansionHost.__new__(GymExpansionHost) + host._context = ctx + factory = REGISTERED_ENVS[raw["id"]].task_program_adapter_factory + adapter = adapters.get(id(factory)) + if adapter is None: + contact_frames = {} + for instance, binding in environment_cfg.robot.gripper_bindings.items(): + actual = first.robot.gripper_bindings[instance] + frame = binding["defaults"].get("root_from_contact_frame") + if frame != actual["defaults"].get("root_from_contact_frame"): + contact_frames[binding["declaration"]["control_part"]] = frame + adapter = ( + factory.create_adapter(env, grasp_contact_frames=contact_frames) + if contact_frames + else factory.create_adapter(env) + ) + adapters[id(factory)] = adapter + ctx.program = adapter.compile(environment_cfg.task_program) + if ctx.feedback is not None: + bind = getattr(ctx.feedback, "bind_program", None) + if bind is not None: + bind(adapter, ctx.program) + if ( + workload.extensions.plan_transform_factory is not None + and adapter.configured_nominal_sources + ): + raise ValueError( + "a workload transform cannot replace configured nominal sources" + ) + if ( + groups + and ctx.program.program_id + != next(iter(groups.values()))["host"]._context.program.program_id + ): + raise ValueError("workloads must share the same Task Program") + admitted_at = [None] + + def admission_clock(admitted_at=admitted_at): + return ( + 0.0 if admitted_at[0] is None else time.monotonic() - admitted_at[0] + ) + + session = ExpansionSession( + ExpansionSessionCfg.from_mapping( + { + "collection": { + "target_committed_episodes": cfg.target_episodes, + "max_rollout_attempts": ( + cfg.max_attempts + if ctx.feedback is None + else ctx.feedback.budget.cfg.max_rollouts + ), + "max_proposals": cfg.max_attempts, + "max_wall_time_s": cfg.max_wall_time_s, + }, + "execution": { + "ready_high_watermark": execution.num_envs, + "ready_low_watermark": 0, + }, + "persistence": { + "pending_episode_limit": execution.num_envs, + "pending_max_bytes": cfg.episode_byte_budget + * execution.num_envs, + }, + "validation": {"require_path_collision": True}, + } + ), + coverage=None if ctx.feedback is None else ctx.feedback.coverage, + episode_namespace=journal.job_instance_id, + clock=admission_clock, + ) + sink = ArtifactEpisodeSink(path) + engine = ExpansionEngine( + ExpansionJobCfg( + batch_size=execution.num_envs, + candidate_budget=execution.num_envs, + episode_byte_budget=cfg.episode_byte_budget, + ), + session=session, + host=host, + sink=_RecordedSink(sink, journal), + ) + groups[workload.workload_id] = { + "host": host, + "session": session, + "sink": sink, + "engine": engine, + "adapter": adapter, + "admitted_at": admitted_at, + } + journal.stage("collection", "running") + source = _GymWorkloadSource( + owner, groups, reuse_idle_rows=execution.reuse_idle_rows + ) + specs = tuple( + ExpansionWorkloadCfg( + workload_id=w.workload_id, + target_episodes=w.cfg.target_episodes, + max_attempts=w.cfg.max_attempts, + max_wall_time_s=w.cfg.max_wall_time_s, + ) + for w in workloads + ) + + def export(name: str) -> dict[str, Any]: + group = groups[name] + journal = group["host"]._context.journal + stamp = time.monotonic() + journal.stage("export_lerobot", "running") + try: + exported = export_expansion_episodes(journal.root) + journal.stage("export_lerobot", "complete") + return {"lerobot": exported, "export_wall_s": time.monotonic() - stamp} + except Exception as error: + journal.stage("export_lerobot", "failed", error=str(error)) + raise + + def done(name: str, counts: Mapping[str, object]) -> None: + group = groups[name] + journal = group["host"]._context.journal + if group["host"]._context.feedback is not None: + group["host"]._context.feedback.checkpoint() + complete = counts["committed"] == group["host"].cfg.target_episodes + journal.stage("collection", "complete" if complete else "budget_exhausted") + journal.checkpoint(force=True) + futures[name] = writer.submit(export, name) + _atomic_json( + root / "workload-progress.json", + { + key: dict(value["session"].snapshot()["counts"]) + for key, value in groups.items() + }, + ) + + executor = WorkloadProgramExecutor( + env, specs, source, execution, on_workload_done=done + ) + source.execution = { + "mode": "single_process_single_world", + "workload_count": execution.workload_count, + "batch_size": execution.batch_size, + "num_envs": execution.num_envs, + "world": dict(executor.world_info), + } + _atomic_json(root / "execution.json", source.execution) + result = dict(executor.run()) + collection_wall = time.monotonic() - started + source.close() + for adapter in adapters.values(): + adapter.close() + owner.close() + owner = None + results = {} + for name, group in groups.items(): + exported = futures[name].result() + readback = [] + paths = { + item["episode_id"]: str( + Path(exported["lerobot"]["directory"]) / item["dataset_path"] + ) + for item in exported["lerobot"]["datasets"] + } + for receipt in group["sink"].inspect(): + episode = load_expansion_episode(receipt.storage_id) + readback.append( + { + "episode_id": episode.episode_id, + "steps": len(episode.actions), + "expansion": json_safe_copy( + episode.metadata.get("expansion", ()), + field_name="expansion readback", + ), + "lerobot": paths[episode.episode_id], + } + ) + counts = result["counts"][name] + results[name] = { + "complete": counts["committed"] == group["host"].cfg.target_episodes, + "target_episodes": group["host"].cfg.target_episodes, + "committed_episodes": counts["committed"], + "attempts": counts["attempts"], + "readback": readback, + "directory": str(group["host"].root), + **exported, + } + _atomic_json(group["host"].root / "adapter-readback.json", readback) + result.update( + workloads=results, + collection_wall_s=collection_wall, + wall_s=time.monotonic() - started, + ) + _atomic_json(root / "workload-execution.json", result) + return result + except BaseException as error: + for name, group in groups.items(): + if name not in futures: + group["host"]._context.journal.stage( + "collection", "failed", error=str(error) + ) + raise + finally: + writer.shutdown(wait=True, cancel_futures=True) + if source is not None: + source.close() + for adapter in adapters.values(): + adapter.close() + for group in groups.values(): + group["host"]._context.journal.checkpoint(force=True, state="finished") + for close in reversed(feedback_closers): + close() + if owner is not None: + owner.close() diff --git a/embodichain/lab/expansion/host.py b/embodichain/lab/expansion/host.py index 7c705325c7..739a4355c9 100644 --- a/embodichain/lab/expansion/host.py +++ b/embodichain/lab/expansion/host.py @@ -48,6 +48,7 @@ from ._gym_recipe import GymRecipeBuilder from ._gym_prepare import GymCohortPreparer from ._gym_execution import GymCohortExecutor +from .host_extensions import GymExpansionHostExtensions __all__ = ["GymExpansionHost"] @@ -93,12 +94,19 @@ def __init__( root: Path, journal: ExpansionJournal, source_artifacts: Mapping[str, object], + *, + extensions: GymExpansionHostExtensions | None = None, ) -> None: self._context = GymHostContext( cfg, report, root, journal, source_artifacts, cfg.batch_size ) + self._context.extensions = extensions or GymExpansionHostExtensions() + if not isinstance(self._context.extensions, GymExpansionHostExtensions): + raise TypeError("extensions must be GymExpansionHostExtensions") + if self._context.extensions.feedback_factory is not None: + raise ValueError("feedback_factory requires run_gym_expansion_workloads") self._recipe_builder = GymRecipeBuilder(self._context) - self._context.recipe = self._recipe_builder.recipe + self._context.recipe = self.recipe self._preparer = GymCohortPreparer(self._context) self._executor = GymCohortExecutor(self._context) try: @@ -123,6 +131,9 @@ def _open(self) -> None: environment_cfg.record_trajectory = False environment_cfg.sim_cfg.headless = True _configure_sensor_capture(environment_cfg, capture_media=cfg.capture_media) + extensions = self._context.extensions + if extensions.collection_renderer is not None: + environment_cfg.sim_cfg.render_cfg.renderer = extensions.collection_renderer from embodichain.lab.sim.profiler import ProfilerCfg environment_cfg.profiler = ProfilerCfg( @@ -170,6 +181,17 @@ def _open(self) -> None: env, cfg.collision_profile, root / "collision" ) self._context.collision = collision + if extensions.validator_factory is not None: + if collision is not None: + raise ValueError("choose a collision profile or an extension validator") + self._context.collision = collision = extensions.validator_factory( + env, root + ) + if collision is None or not all( + callable(getattr(collision, method, None)) + for method in ("prepare", "sample", "close") + ): + raise TypeError("validator_factory must return a measured validator") profile = None if cfg.source == "real2sim": from embodichain.lab.task_program.integrations.simulation.real2sim_collection import ( @@ -208,6 +230,12 @@ def _open(self) -> None: self._context.collision = collision self._context.profile = profile self._context.scene_source = scene_source + if extensions.scene_source_factory is not None: + if scene_source is not None: + raise ValueError("choose a scene atlas or an extension scene source") + self._context.scene_source = extensions.scene_source_factory(env) + if not callable(getattr(self._context.scene_source, "candidate", None)): + raise TypeError("scene_source_factory must return a candidate source") self._context.motion_profile = motion_profile self._context.motion_recipes = motion_recipes self._context.names = names @@ -448,7 +476,9 @@ def profile(self, value: Any) -> None: def recipe(self, ordinal: int) -> ExpansionRecipe: """Sample a pinned recipe without mutating the World.""" - return self._recipe_builder.recipe(ordinal) + recipe = self._recipe_builder.recipe(ordinal) + transform = self._context.extensions.recipe_transform + return recipe if transform is None else transform(recipe) def prepare_batch( self, requests: tuple[RecipeAttempt, ...], session: ExpansionSession diff --git a/embodichain/lab/expansion/host_extensions.py b/embodichain/lab/expansion/host_extensions.py new file mode 100644 index 0000000000..911e1e67b4 --- /dev/null +++ b/embodichain/lab/expansion/host_extensions.py @@ -0,0 +1,121 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Non-owning public extension ports for the native Gym expansion host.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from collections.abc import Callable, Mapping +from pathlib import Path +from typing import Any, Protocol +from .contracts import CandidateIdentity, SceneCase, ValidationResult +from .recipes import ExpansionRecipe + +__all__ = ["ExpansionValidator", "ExpansionRowContext", "GymExpansionHostExtensions"] + + +class ExpansionValidator(Protocol): + """Measured validation port shared by geometry and native-contact checks. + + Unavailable evidence must reject. Implementations never step/reset the World; + the host calls prepare after settling and sample on every measured boundary. + """ + + initial: Mapping[int, bool] + terminal: Mapping[int, ValidationResult] + results: Mapping[int, Any] + + def prepare( + self, snapshot: Any = None, *, env_ids: tuple[int, ...] | None = None + ) -> None: + """Reset evidence after settling; rolling callers select independent rows.""" + ... + + def sample(self, ids: tuple[int, ...], action: Any, snapshot: Any = None) -> None: + """Observe the original control boundary and its physical substeps.""" + ... + + def close(self) -> None: + """Release only owned observers/resources; the host retains its World.""" + ... + + +@dataclass(frozen=True) +class ExpansionRowContext: + """Immutable identity supplied to a per-row plan-transform factory.""" + + env_id: int + identity: CandidateIdentity + ordinal: int + attempt_id: int + directory: Path + scene_case: SceneCase + recipe: ExpansionRecipe + + +@dataclass(frozen=True) +class GymExpansionHostExtensions: + """Runtime callbacks; construction, collection and persistence stay native. + + Args: + collection_renderer: Explicit headless collection renderer, or None for + the native sensor/media default. + scene_source_factory: Receives the live environment and returns a source + exposing candidate(ordinal); it cannot own another World. + validator_factory: Receives environment and artifact root; returns the + same measured port for either physical geometry or native contacts. + recipe_transform: Receives the native immutable recipe and returns its + task-owned operations. Preparation verifies it again before mutation. + plan_transform_factory: Receives environment and immutable row identity; + returns an ordinary Task Program transform factory. Its record metadata + and hashed plan attachments are archived by the native episode owner. + feedback_factory: Optional rolling-collection feedback owner receiving env, + scene source and artifact directory; exposes coverage, budget, validate, + finish and checkpoint without advancing physics or writing episodes. + require_plan_transform: Reject completed programs without transform records. + """ + + collection_renderer: str | None = None + scene_source_factory: Callable[[Any], Any] | None = None + validator_factory: Callable[[Any, Path], ExpansionValidator] | None = None + recipe_transform: Callable[[ExpansionRecipe], ExpansionRecipe] | None = None + plan_transform_factory: Callable[[Any, ExpansionRowContext], Any] | None = None + feedback_factory: Callable[[Any, Any, Path], Any] | None = field( + default=None, kw_only=True + ) + require_plan_transform: bool = False + + def __post_init__(self) -> None: + if self.collection_renderer is not None and ( + type(self.collection_renderer) is not str + or not self.collection_renderer.strip() + ): + raise ValueError("collection_renderer must be a nonempty renderer name") + for name in ( + "scene_source_factory", + "feedback_factory", + "validator_factory", + "recipe_transform", + "plan_transform_factory", + ): + value = getattr(self, name) + if value is not None and not callable(value): + raise TypeError(f"{name} must be callable") + if type(self.require_plan_transform) is not bool or ( + self.require_plan_transform and self.plan_transform_factory is None + ): + raise ValueError("required plan transforms need an explicit factory") diff --git a/embodichain/lab/expansion/journal.py b/embodichain/lab/expansion/journal.py index 6573986161..3e717c013f 100644 --- a/embodichain/lab/expansion/journal.py +++ b/embodichain/lab/expansion/journal.py @@ -190,6 +190,37 @@ def update(self, attempt: RecipeAttempt | Path, **values: object) -> dict: _atomic_json(path, record) return record + def allocate_recipe(self, recipe: ExpansionRecipe) -> RecipeAttempt: + """Allocate one fresh rolling attempt while independent peers are active. + + The single writer supplies a unique, monotonically increasing ordinal. + This port does not resume interrupted attempts or change collection + budgets; native workload admission remains responsible for those limits. + """ + if self.run_id is None or self.operation != "all": + raise RuntimeError("rolling allocation requires an open collection run") + records = self.attempts() + if records and recipe.context.ordinal <= max(r["ordinal"] for r in records): + raise ValueError("rolling recipe ordinals must increase") + sequence = len(records) + directory = self.root / "attempts" / f"{sequence:08d}" + if directory.exists(): + raise FileExistsError("attempt sequence already exists") + _atomic_json( + directory / "attempt.json", + { + "sequence": sequence, + "ordinal": recipe.context.ordinal, + "recipe_id": recipe.recipe_id, + "recipe_spec": recipe.to_dict(), + "attempt_id": 0, + "run_id": self.run_id, + "status": "proposed", + "steps": 0, + }, + ) + return RecipeAttempt(recipe, 0, sequence, directory) + def reconcile(self, sink: object) -> None: """Repair frozen writes before admitting any physical retry.""" confirmed = {r.commit_id: r for r in sink.inspect()} diff --git a/embodichain/lab/expansion/persistence.py b/embodichain/lab/expansion/persistence.py index 5683a8033a..4d688b8841 100644 --- a/embodichain/lab/expansion/persistence.py +++ b/embodichain/lab/expansion/persistence.py @@ -18,12 +18,14 @@ from __future__ import annotations from dataclasses import asdict, replace +from collections import OrderedDict import hashlib import json import os from pathlib import Path import tempfile import shutil +from threading import RLock import numpy as np from embodichain.lab.gym.envs._json import json_safe_copy from .contracts import ExpertEpisode, CommitReceipt @@ -39,6 +41,10 @@ __all__ = ["ArtifactEpisodeSink", "load_expansion_episode"] +_EPISODE_CACHE = OrderedDict() +_EPISODE_CACHE_LOCK = RLock() +_EPISODE_CACHE_BYTES = 0 + def _source_references(metadata): references = [ @@ -65,6 +71,45 @@ def load_expansion_episode(path: str | Path) -> ExpertEpisode: Legacy staged episodes remain readable in place. New artifacts carry only relative references and can be moved without changing their frozen bytes. """ + global _EPISODE_CACHE_BYTES + path = Path(path).resolve() + # Re-read all evidence bytes on every call. Path/mtime keys alone could + # conceal tampering; this cache only avoids repeated parsing/JSON ownership. + with path.open("rb") as stream: + digest = hashlib.file_digest(stream, "sha256").digest() + with _EPISODE_CACHE_LOCK: + cached = _EPISODE_CACHE.get(path) + if cached is not None and cached[0] == digest: + _EPISODE_CACHE.move_to_end(path) + # Construction retains every numeric check and owns fresh tensors. + return replace(cached[1]) + episode = _load_expansion_episode(path) + with path.open("rb") as stream: + if hashlib.file_digest(stream, "sha256").digest() != digest: + raise ValueError("Evidence changed while loading") + prototype = replace(episode) + size = sum( + value.numel() * value.element_size() + for value in ( + prototype.actions, + prototype.timestamps, + *prototype.observations.values(), + ) + ) + with _EPISODE_CACHE_LOCK: + previous = _EPISODE_CACHE.pop(path, None) + if previous is not None: + _EPISODE_CACHE_BYTES -= previous[2] + if size <= 64 * 1024 * 1024: + _EPISODE_CACHE[path] = digest, prototype, size + _EPISODE_CACHE_BYTES += size + while len(_EPISODE_CACHE) > 16 or _EPISODE_CACHE_BYTES > 128 * 1024 * 1024: + _, (_, _, released) = _EPISODE_CACHE.popitem(last=False) + _EPISODE_CACHE_BYTES -= released + return episode + + +def _load_expansion_episode(path: Path) -> ExpertEpisode: from embodichain.lab.gym.envs.managers.real2sim_collection import ( load_staged_real2sim_episode, ) diff --git a/embodichain/lab/expansion/preview.py b/embodichain/lab/expansion/preview.py new file mode 100644 index 0000000000..b136d85869 --- /dev/null +++ b/embodichain/lab/expansion/preview.py @@ -0,0 +1,310 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Owned launch, wait and stop operations for native result-preview services.""" + +from __future__ import annotations + +import json +import math +import os +from pathlib import Path +import signal +import subprocess +import sys +import time +from uuid import uuid4 + +from ._task_program_config import load_mapping +from embodichain.data_pipeline.recording.media import _atomic_json as atomic_json +from datetime import datetime, timezone + + +def utc_now() -> str: + return datetime.now(timezone.utc).isoformat() + + +__all__ = [ + "launch_expansion_preview", + "preview_expansion_results", + "stop_expansion_preview", +] + + +def _process_command(pid: int) -> bytes: + try: + return Path(f"/proc/{pid}/cmdline").read_bytes() + except FileNotFoundError: + return b"" + + +def _inputs(paths: list[Path]) -> list[Path]: + result = [] + for supplied in paths: + path = supplied.expanduser().resolve() + if (path / "job.json").is_file() or (path / "preview-plan.json").is_file(): + result.append(path) + elif (path / "expansion.json").is_file(): + data = load_mapping(path / "expansion.json") + result.extend( + Path(row["directory"]).resolve() + for row in data.get("operators", {}).values() + if row.get("committed_episodes", 0) > 0 + ) + else: + raise ValueError( + f"Preview input must be an aggregate result or native job: {path}" + ) + if not result or len(set(result)) != len(result): + raise ValueError( + "Preview requires distinct job outputs with confirmed episodes" + ) + return result + + +def _recorded_eef(inputs: list[Path]) -> str | None: + """Resolve the first measured motion endpoint, without robot-name routing.""" + from embodichain.lab.expansion import ArtifactEpisodeSink, load_expansion_episode + + for path in inputs: + if not (path / "job.json").is_file(): + continue + job = load_mapping(path / "job.json") + robot = job.get("resolved_deployment", {}).get("robot", {}) + solvers = robot.get("solver_cfg", {}) + ends = { + item.get("end_link_name") + for item in solvers.values() + if item.get("end_link_name") + } + if len(ends) == 1: + return next(iter(ends)) + receipts = ArtifactEpisodeSink(path).inspect() + if not receipts: + continue + episode = load_expansion_episode(receipts[0].storage_id) + for record in episode.metadata.get("expansion", ()): + joints = set(record.get("controlled_joint_names", ())) + matches = [ + cfg.get("end_link_name") + for cfg in solvers.values() + if joints + and joints == set(cfg.get("joint_names") or ()) + and cfg.get("end_link_name") + ] + if len(set(matches)) == 1: + return matches[0] + for segment in episode.metadata.get("program_segments", ()): + runtime = segment.get( + "runtime", segment.get("metadata", {}).get("runtime", {}) + ) + for call in runtime.get("calls", ()): + for endpoint in call.get("resolved_core_policy", {}).get( + "endpoints", () + ): + if endpoint.get("endpoint_id") == "motion": + end = solvers.get(endpoint.get("target_id"), {}).get( + "end_link_name" + ) + if end: + return end + return None + + +def launch_expansion_preview( + paths: list[Path], + *, + output: Path | None = None, + port: int = 0, + gpu: int = 0, + duration: float = 900, + open_browser: bool = False, + eef_link: str | None = None, + startup_timeout: float = 240, + max_episodes: int = 32, +) -> dict: + """Start a bounded native preview service after collection/export completes. + + Logs/status are separate from immutable run artifacts. Inputs are read-only; + no trajectory is recollected, substituted or credited as another success. + + Args: + paths: Native job, aggregate result or preview-plan directories. + output: New directory for owned service status and logs. + port: Loopback port; zero allocates an available port. + gpu: Visible GPU index used by the read-only renderer. + duration: Service seconds after readiness; zero waits for interruption. + open_browser: Open a browser after native rendering is ready. + eef_link: Optional endpoint; recorded motion supplies it otherwise. + startup_timeout: Bounded wait for the first complete frame. + max_episodes: Maximum recorded episodes to preload, between one and 64. + Returns: + Owned service status, command identity and ready URL. Linux PID ownership + is checked before the stop/wait APIs signal the process. + """ + paths = [Path(path) for path in paths] + if ( + type(port) is not int + or not 0 <= port <= 65535 + or type(gpu) is not int + or gpu < 0 + ): + raise ValueError("Invalid preview port or GPU") + if ( + not math.isfinite(duration) + or duration < 0 + or not math.isfinite(startup_timeout) + or startup_timeout <= 0 + ): + raise ValueError("Invalid preview duration/startup timeout") + if type(max_episodes) is not int or not 1 <= max_episodes <= 64: + raise ValueError("Preview max_episodes must be in [1,64]") + inputs = _inputs(paths) + output = ( + output or (paths[0].resolve().parent / ".previews" / uuid4().hex) + ).resolve() + output.mkdir(parents=True, exist_ok=True) + status_path = output / "status.json" + if status_path.exists(): + raise FileExistsError(f"Preview status already exists: {status_path}") + command = [sys.executable, "-m", "embodichain", "expansion", "preview"] + for path in inputs: + command.extend(["--input", str(path)]) + command.extend( + [ + "--port", + str(port), + "--gpu", + str(gpu), + "--duration", + str(duration), + "--max-episodes", + str(max_episodes), + ] + ) + end = eef_link or _recorded_eef(inputs) + if end: + command.extend(["--eef-link", end]) + if open_browser: + command.append("--open") + log_path = output / "server.log" + with log_path.open("w") as log: + process = subprocess.Popen( + command, stdout=log, stderr=subprocess.STDOUT, start_new_session=True + ) + status = { + "state": "starting", + "pid": process.pid, + "command": command, + "inputs": [str(p) for p in inputs], + "started_at": utc_now(), + "status_file": str(status_path), + "log": str(log_path), + } + atomic_json(status_path, status) + deadline = time.monotonic() + startup_timeout + try: + while time.monotonic() < deadline: + for line in log_path.read_text(errors="replace").splitlines(): + try: + ready = json.loads(line) + except ValueError: + continue + if isinstance(ready, dict) and ready.get("state") == "ready": + status.update(ready, state="ready", eef_link=end) + atomic_json(status_path, status) + return status + if process.poll() is not None: + raise RuntimeError( + f"Native preview exited {process.returncode}: {log_path.read_text(errors='replace')[-3000:]}" + ) + time.sleep(0.1) + raise TimeoutError(f"Native preview startup timed out; see {log_path}") + except BaseException: + if process.poll() is None: + process.send_signal(signal.SIGINT) + try: + process.wait(timeout=30) + except subprocess.TimeoutExpired: + process.kill() + process.wait() + atomic_json( + status_path, {**status, "state": "failed", "returncode": process.returncode} + ) + raise + + +def stop_expansion_preview(path: str | Path) -> dict[str, object]: + """Stop only the native service whose recorded command still owns its PID. + + Args: + path: Service status JSON or its directory. + Returns: + Closed status after the owned process stops; changed ownership rejects. + """ + path = Path(path) + status_path = path / "status.json" if path.is_dir() else path + status = load_mapping(status_path) + pid = status["pid"] + command = _process_command(pid) + if command: + actual = command.rstrip(b"\0").split(b"\0") + if actual != [os.fsencode(arg) for arg in status["command"]]: + raise ValueError("Preview PID is no longer owned by the recorded command") + try: + os.kill(pid, signal.SIGINT) + except ProcessLookupError: + pass + deadline = time.monotonic() + 30 + while _process_command(pid) and time.monotonic() < deadline: + time.sleep(0.1) + if _process_command(pid): + raise TimeoutError("Preview has not stopped; inspect its log") + status.update(state="closed", closed_at=utc_now()) + atomic_json(status_path, status) + return status + + +def preview_expansion_results( + paths: list[Path], **options: object +) -> dict[str, object]: + """Run the native browser viewer until its duration ends or Ctrl+C stops it. + + Args: + paths: Confirmed native job or aggregate result directories. + **options: Launch options accepted by :func:`launch_expansion_preview`. + Returns: + Final service status after bounded playback or interruption. + """ + status = launch_expansion_preview(paths, **options) + sys.stdout.write(json.dumps(status, ensure_ascii=False) + "\n") + sys.stdout.flush() + try: + expected = [os.fsencode(arg) for arg in status["command"]] + while True: + command = _process_command(status["pid"]) + if not command or command.rstrip(b"\0").split(b"\0") != expected: + break + time.sleep(0.2) + except KeyboardInterrupt: + return stop_expansion_preview(Path(status["status_file"])) + try: + os.waitpid(status["pid"], os.WNOHANG) + except ChildProcessError: + pass + status.update(state="closed", closed_at=utc_now()) + atomic_json(Path(status["status_file"]), status) + return status diff --git a/embodichain/lab/expansion/session.py b/embodichain/lab/expansion/session.py index 8087f9d7b9..53d5248e2a 100644 --- a/embodichain/lab/expansion/session.py +++ b/embodichain/lab/expansion/session.py @@ -564,6 +564,12 @@ def _reserved(self) -> int: for attempt in self._attempts.values() ) + def _write_reserved(self) -> int: + return sum( + attempt.state in {"pending_write", "committed"} + for attempt in self._attempts.values() + ) + def take_ready( self, case_id: str, @@ -571,6 +577,7 @@ def take_ready( *, episode_byte_budget: int | None = None, candidate_id: str | None = None, + speculative: bool = False, ) -> CandidateTrajectoryBatch | EpisodeCandidate | None: """Reserve a row, rollout attempt, and pending payload capacity before execution. @@ -586,12 +593,17 @@ def take_ready( episode_byte_budget: Maximum episode payload bytes to reserve for this row. candidate_id: Optional exact ready candidate to reserve. Omission preserves deterministic FIFO selection within the case. + speculative: Permit competing rollouts for unfilled quota. Payload, + attempt and write reservations still bound execution; only one + passed episode can reserve each remaining persistence slot. Returns: One assigned candidate row, or ``None`` when no matching row or capacity is available or the session has stopped. """ self._cases[(case_id, initial_state_id)] + if type(speculative) is not bool: + raise TypeError("speculative must be a bool") if candidate_id is not None and ( type(candidate_id) is not str or not candidate_id.strip() ): @@ -608,7 +620,8 @@ def take_ready( ) if ( self.stop_reason - or self._reserved() >= self._cfg.collection.target_committed_episodes + or (self._write_reserved() if speculative else self._reserved()) + >= self._cfg.collection.target_committed_episodes or self._counts["rollout_attempted"] + assigned >= self._cfg.collection.max_rollout_attempts or pending_count >= self._cfg.persistence.pending_episode_limit @@ -761,6 +774,9 @@ def accept_episode(self, episode: ExpertEpisode) -> bool: if self._episode_bytes(episode) > attempt.episode_byte_budget: self.release(episode.identity, reason="episode_byte_budget_exceeded") raise BufferError("Payload exceeds its reserved episode byte budget.") + if self._write_reserved() >= self._cfg.collection.target_committed_episodes: + self.release(episode.identity, reason="quota_reserved_by_peer") + return False owned = replace(episode) if not self._reserve_coverage(owned): self.release(episode.identity, reason="coverage_rejected") diff --git a/embodichain/lab/expansion/session_cfg.py b/embodichain/lab/expansion/session_cfg.py index a66a3851fb..7808c707ca 100644 --- a/embodichain/lab/expansion/session_cfg.py +++ b/embodichain/lab/expansion/session_cfg.py @@ -125,6 +125,13 @@ def _decode(cls: type, data: Mapping[str, Any], path: str = "") -> Any: ) for item_key, item_value in value.items() } + elif value_type is float: + if any( + type(v) not in (int, float) or not math.isfinite(v) + for v in value.values() + ): + raise ValueError(f"{name} must map strings to finite numbers") + decoded[key] = {k: float(v) for k, v in value.items()} elif value_type is str: if any(type(item_value) is not str for item_value in value.values()): raise ValueError(f"{name} must map strings to strings") diff --git a/embodichain/lab/expansion/task_pose_coverage.py b/embodichain/lab/expansion/task_pose_coverage.py new file mode 100644 index 0000000000..a095c3fcba --- /dev/null +++ b/embodichain/lab/expansion/task_pose_coverage.py @@ -0,0 +1,311 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Receipt-confirmed task pose coverage shared across sampled scene cases.""" + +from __future__ import annotations + +from collections import Counter +from collections.abc import Mapping +from copy import deepcopy +from typing import TYPE_CHECKING + +import numpy as np +import torch + +if TYPE_CHECKING: + from .contracts import ExpertEpisode, SceneCase + +__all__ = ["TaskPoseCoverage"] + + +from ._pose_cells import _grid, _pose_cell, _measurements + + +class TaskPoseCoverage: + """Count measured pick/place cells only after ordinary sink confirmation. + + Args: + domain_id: Host-validated invariant experiment identity, including the + installation and grid, excluding sampled scene poses and RNG seed. + entity_uid: Manipulated rigid object whose measured states are counted. + grids: Explicit ``pick`` and ``place`` XY/yaw grids in arena coordinates. + measurements: Optional named entity/boundary declarations. Initial, + terminal and named segment-end states are measured independently. + Fixed categories never turn tracking noise into pose novelty. + + Scene cases keep their existing identities. This adapter aggregates them + within one declared experiment, never across separate instances/domains. + """ + + def __init__( + self, + domain_id: str, + entity_uid: str, + grids: Mapping, + *, + measurements: Mapping | None = None, + ) -> None: + if not all( + type(s) is str and s.strip() == s and s for s in (domain_id, entity_uid) + ): + raise ValueError("Coverage requires nonempty domain and entity identities") + if not isinstance(grids, Mapping) or not grids: + raise ValueError("Coverage requires explicit grids") + if measurements is None and set(grids) != {"pick", "place"}: + raise ValueError("Legacy coverage requires pick and place grids") + if measurements is not None and set(measurements) != set(grids): + raise ValueError("Coverage measurements must match grid names") + self.domain_id, self.entity_uid = domain_id, entity_uid + self.grids = {k: _grid(v) for k, v in grids.items()} + self.measurements = ( + None if measurements is None else _measurements(measurements, grids) + ) + self._cases: dict[str, tuple[str, str, str]] = {} + self._population = None + self._pending: dict[str, tuple[str, tuple]] = {} + self._confirmed: dict[str, tuple[str, tuple]] = {} + self._counts = Counter() + + def register_case( + self, + case: SceneCase, + joint_limits: torch.Tensor, + joint_names: tuple[str, ...], + reference: float | None, + ) -> None: + """Register a scene without changing its identity or normalization.""" + if reference is not None: + raise ValueError( + "Task pose coverage does not accept a manipulability reference" + ) + population = ( + case.task_id, + case.robot_profile_id, + case.calibration_id, + tuple(joint_names), + ) + if self._population is not None and population != self._population: + raise ValueError( + "Coverage domain cannot mix task/robot/calibration/joint layouts" + ) + self._population = population + identity = (case.task_id, case.robot_profile_id, case.calibration_id) + if ( + case.scene_case_id in self._cases + and self._cases[case.scene_case_id] != identity + ): + raise ValueError("Conflicting scene case registration") + self._cases[case.scene_case_id] = identity + + def reference(self, case: SceneCase) -> None: + """This coverage policy supplies no motion normalization reference.""" + if case.scene_case_id not in self._cases: + raise KeyError(case.scene_case_id) + return None + + def cells( + self, pick_pose: object, place_pose: object + ) -> tuple[tuple[str, int, int, int], ...]: + """Return ordered tagged cells; out-of-domain poses contribute no cell.""" + return tuple( + (phase, *cell) + for phase, pose in (("pick", pick_pose), ("place", place_pose)) + if (cell := _pose_cell(pose, self.grids[phase])) is not None + ) + + def reserve(self, episode: ExpertEpisode) -> bool: + """Own measured cells until the matching persistence receipt arrives.""" + case_id = episode.identity.scene_case_id + if case_id not in self._cases: + raise ValueError("Episode scene is outside this coverage domain") + if not episode.validation.accepted: + return False + if self.measurements is None: + states = self._states(episode, self.entity_uid) + cells = self.cells( + states[0].detach().cpu().numpy(), states[-1].detach().cpu().numpy() + ) + else: + measured = [] + for phase, declaration in self.measurements.items(): + states = self._states(episode, declaration["entity"]) + boundary = declaration["boundary"] + index = 0 if boundary == "initial" else len(states) - 1 + if boundary == "segment_end": + segments = [ + s + for s in episode.metadata.get("program_segments", ()) + if s["name"] == declaration["segment"] + ] + if len(segments) != 1 or not segments[0]["success"]: + raise ValueError( + "Coverage requires one successful named segment" + ) + index = int(segments[0]["end_step"]) + if not 0 <= index < len(states): + raise ValueError( + "Coverage segment boundary is outside T+1 states" + ) + cell = _pose_cell( + states[index].detach().cpu().numpy(), self.grids[phase] + ) + if cell is not None: + measured.append( + ( + phase, + *((0, 0, 0) if declaration.get("fixed", False) else cell), + ) + ) + cells = tuple(measured) + value = (case_id, cells) + previous = self._confirmed.get( + episode.commit_id, self._pending.get(episode.commit_id) + ) + if previous is not None and previous != value: + raise ValueError("Commit identity was reused with different measured cells") + if episode.commit_id not in self._confirmed: + self._pending[episode.commit_id] = value + return True # Duplicates are valid data; they cannot increase unique coverage. + + @staticmethod + def _states(episode: ExpertEpisode, entity: str) -> torch.Tensor: + states = episode.observations.get(f"rigid/{entity}/state") + if ( + states is None + or states.ndim != 2 + or states.shape[0] != len(episode.timestamps) + or states.shape[1] < 7 + or not bool(torch.isfinite(states).all()) + ): + raise ValueError("Task coverage requires finite T+1 measured rigid states") + return states + + def proposal_cells(self, poses: Mapping) -> tuple: + """Predict only declared initial cells; later phases require measurements.""" + if self.measurements is None: + raise ValueError( + "Legacy proposal cells require explicit pick/place references" + ) + return tuple( + (phase, *((0, 0, 0) if declaration.get("fixed", False) else cell)) + for phase, declaration in self.measurements.items() + if declaration["boundary"] == "initial" + and (cell := _pose_cell(poses[declaration["entity"]], self.grids[phase])) + is not None + ) + + def confirm(self, case_id: str, commit_id: str) -> None: + """Idempotently count the reservation qualified by the session's receipt.""" + value = self._confirmed.get(commit_id, self._pending.get(commit_id)) + if value is None or value[0] != case_id: + raise ValueError("Foreign or unreserved coverage confirmation") + if commit_id in self._confirmed: + return + self._pending.pop(commit_id) + self._confirmed[commit_id] = value + self._counts.update(value[1]) + + def release(self, case_id: str, commit_id: str) -> None: + """Release unconfirmed evidence without erasing committed coverage.""" + value = self._pending.get(commit_id) + if value is not None and value[0] != case_id: + raise ValueError("Foreign coverage release") + self._pending.pop(commit_id, None) + + def gain(self, cells: tuple, predicted: Mapping | None = None) -> float: + """Score scarcity using formal counts and explicitly tentative occupancy.""" + pending = Counter( + cell for _, values in self._pending.values() for cell in values + ) + predicted = {} if predicted is None else predicted + return sum( + 1 / (1 + self._counts[cell] + pending[cell] + predicted.get(cell, 0)) + for cell in cells + ) + + def snapshot(self) -> tuple[Mapping[str, int], Mapping[str, Mapping[int, int]]]: + """Return confirmed coverage with no fabricated manipulability bands.""" + return {self.domain_id: len(self._counts)}, {} + + def phase_counts(self) -> Mapping[str, int]: + """Return owned pick/place counts separately from motion quality bands.""" + return { + phase: sum(cell[0] == phase for cell in self._counts) + for phase in self.grids + } + + def state_dict(self) -> dict: + """Return an owned receipt-derived checkpoint, including pending evidence.""" + return deepcopy( + { + "version": 1, + "domain_id": self.domain_id, + "entity_uid": self.entity_uid, + "grids": self.grids, + "measurements": self.measurements, + "population": self._population, + "cases": self._cases, + "pending": self._pending, + "confirmed": self._confirmed, + } + ) + + def load_state_dict(self, state: Mapping) -> None: + """Restore matching-domain counts; pending evidence remains unconfirmed.""" + data = deepcopy(dict(state)) + if data.get("version") != 1 or any( + data.get(k) != getattr(self, k) + for k in ("domain_id", "entity_uid", "grids", "measurements") + ): + raise ValueError("Coverage checkpoint belongs to another domain/grid") + if self._pending or self._confirmed or self._cases: + raise ValueError("Restore coverage only into a new adapter") + + def decode(entries): + return { + key: (value[0], tuple(tuple(cell) for cell in value[1])) + for key, value in entries.items() + } + + pending, confirmed = decode(data["pending"]), decode(data["confirmed"]) + if pending.keys() & confirmed.keys(): + raise ValueError("Coverage checkpoint repeats pending/confirmed identities") + cases = {k: tuple(v) for k, v in data["cases"].items()} + for case, cells in (*pending.values(), *confirmed.values()): + if case not in cases or any( + len(c) != 4 + or c[0] not in self.grids + or any(type(i) is not int or i < 0 for i in c[1:]) + or any( + i >= round((hi - lo) / step) + for i, lo, hi, step in zip( + c[1:], + self.grids[c[0]]["lower"], + self.grids[c[0]]["upper"], + self.grids[c[0]]["cell_size"], + ) + ) + for c in cells + ): + raise ValueError("Malformed coverage checkpoint cells") + pop = data["population"] + self._cases = cases + self._population = None if pop is None else (*pop[:3], tuple(pop[3])) + self._pending, self._confirmed = pending, confirmed + self._counts = Counter( + cell for _, cells in confirmed.values() for cell in cells + ) diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py new file mode 100644 index 0000000000..d62af0d1c1 --- /dev/null +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -0,0 +1,490 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Declarative scene/motion jobs composed through the native workload engine.""" + +from __future__ import annotations +from collections.abc import Mapping, Sequence +from copy import deepcopy +from dataclasses import replace +from pathlib import Path +from typing import Any, TYPE_CHECKING +import hashlib +import json +import os +from embodichain.utils.utility import load_config as load_mapping +from embodichain.data_pipeline.recording.media import ( + _digest as file_digest, + _atomic_json as atomic_json, +) +from ._task_program_config import load_task_program_expansion, load_expansion_viewer + +if TYPE_CHECKING: + from embodichain.lab.sim.scene.feasibility import JointSceneCandidate + +__all__ = [ + "load_task_program_expansion", + "load_expansion_viewer", + "run_task_program_expansion", + "derive_grasp_expansion_deployment", +] + + +def _load_deployment(path: Path) -> None: + from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + + discover_task_packages() + execute_init_hooks() + config_to_cfg(load_mapping(path), source_path=path) + + +from ._pose_sampling import _BoundedPoseSource, _AdaptivePoseSource + + +def _task_expansion_extensions(*, options, operator, seed, report): + """Declare task proposals; upstream owns collection and persistence.""" + from .host_extensions import GymExpansionHostExtensions + + pose_source = None + + def source(env): + nonlocal pose_source + from ._scene_first_sampling import _LayoutPoseSource + + source_type = ( + _LayoutPoseSource + if "layout" in options.get("scene_sampling", {}) + else _AdaptivePoseSource + ) + pose_source = ( + source_type(env, options, seed=seed, revision=report["semantic_id"]) + if "scene_sampling" in options + else _BoundedPoseSource( + env, options["scene"], seed=seed, revision=report["semantic_id"] + ) + ) + return pose_source + + def feedback(env, source, path): + source.output = path + source.checkpoint() + return source + + def recipe_transform(recipe): + operations = [] + for operation in recipe.operations: + payload = json.loads(operation.payload_json) + if operation.name == "motion": + payload.update(operator=operator, options=options["trajectory"]) + elif operation.name == "joint_scene": + candidate = pose_source.candidate(recipe.context.ordinal) + payload["pose_changes"] = { + change.entity_uid: change.arena_pose + for change in candidate.scene.pose_changes + } + operations.append(replace(operation, payload_json=json.dumps(payload))) + return replace(recipe, operations=tuple(operations)) + + def plans(env, row): + from embodichain.lab.task_program.integrations.expansion import ( + TaskProgramSameGridExpansionFactory, + ) + + return TaskProgramSameGridExpansionFactory.from_native_plan( + operator, + options["trajectory"], + seed=seed + row.ordinal, + scene_case=row.scene_case, + revision=report["recipe_revision"], + output=row.directory / "plans", + episode_id=row.ordinal, + ) + + return GymExpansionHostExtensions( + scene_source_factory=source, + feedback_factory=feedback if "scene_sampling" in options else None, + recipe_transform=recipe_transform, + plan_transform_factory=plans, + require_plan_transform=operator != "object_pose", + ) + + +def _prepare_deployment(deployment: Path, output: Path) -> Path: + """Snapshot configuration without opening a reference simulation World.""" + from embodichain.utils.config_paths import resolve_config_path + import yaml + + original = load_mapping(deployment) + + def paths(value): + if isinstance(value, dict): + return {key: paths(item) for key, item in value.items()} + if isinstance(value, list): + return [paths(item) for item in value] + if isinstance(value, str) and value.endswith((".yaml", ".yml", ".json")): + return str(resolve_config_path(value, base_dir=deployment.parent).resolve()) + return value + + snapshot = paths(deepcopy(original)) + owner = "environment" if "environment" in snapshot else "scene" + if "component" in snapshot[owner]: + from embodichain.lab.gym.utils._component_composition import ( + _resolve_local_resource_paths, + ) + + component_path = Path(snapshot[owner]["component"]) + physical = load_mapping(component_path) + _resolve_local_resource_paths(physical, base_dir=component_path.parent) + snapshot[owner] = {"component": str((output / "environment.yaml").resolve())} + else: + from embodichain.lab.gym.utils.composition import resolve_gym_components + + physical = resolve_gym_components( + {owner: snapshot[owner]}, base_dir=deployment.parent + ).config + (output / "environment.yaml").write_text(yaml.safe_dump(physical, sort_keys=False)) + snapshot["id"] += "-Expansion-v1" + path = output / "deployment.json" + atomic_json(path, snapshot) + atomic_json( + output / "source.json", + { + "deployment": str(deployment), + "sha256": file_digest(deployment), + "config": original, + }, + ) + return path + + +def derive_grasp_expansion_deployment( + deployment: str | Path, output: str | Path, settings: Mapping[str, Any] +) -> Path: + """Snapshot a native grasp policy without mutating the source deployment. + + Args: + deployment: Native deployment with integration and embodiment references. + output: Fresh directory for derived component files. + settings: Validated affordance action_options and grasp_generators. + + Returns: + Derived deployment path, usable by native configuration preflight or + ordinary task execution. Physical task criteria remain unchanged. + """ + deployment, output = Path(deployment), Path(output) + from embodichain.utils.config_paths import resolve_config_path + import yaml + + snapshot = load_mapping(deployment) + integration_path = resolve_config_path( + snapshot["task_program"]["integration"], base_dir=deployment.parent + ) + integration = load_mapping(integration_path) + profile = integration["profile"] + presets = profile.get("skill_options", profile.get("action_options", {})) + for name, changes in settings["action_options"].items(): + if name not in presets or "kind" in changes: + raise ValueError( + "Affordance options must retain existing skill preset kinds" + ) + for key, value in changes.items(): + if value is None: + presets[name].pop(key, None) + else: + presets[name][key] = value + output.mkdir(parents=True) + integration_path = output / "integration.yaml" + integration_path.write_text(yaml.safe_dump(integration, sort_keys=False)) + snapshot["task_program"]["integration"] = str(integration_path.resolve()) + generator_overrides = settings.get("grasp_generators", {}) + if generator_overrides: + embodiment_path = resolve_config_path( + snapshot["embodiment"]["component"], base_dir=deployment.parent + ) + embodiment = load_mapping(embodiment_path) + generators = ( + embodiment["skill_profile"] + .setdefault("runtime_services", {}) + .setdefault("grasp_pose_generators", {}) + ) + for name, changes in generator_overrides.items(): + changes = dict(changes) + if "root_from_contact_frame" in changes: + # Bound physical data belongs to the actual gripper declaration, + # not its generator policy. Keep both native views consistent. + bound = [ + item + for item in embodiment.get("grippers", {}).values() + if item.get("control_part") == name + ] + if len(bound) != 1: + raise ValueError( + "A contact frame override requires one bound gripper" + ) + bound[0]["root_from_contact_frame"] = changes.pop( + "root_from_contact_frame" + ) + if name not in generators: + if not {"kind", "gripper"} <= changes.keys(): + raise ValueError(f"Unknown embodiment grasp generator: {name}") + generators[name] = {} + generators[name].update(changes) + embodiment_path = output / "embodiment.yaml" + embodiment_path.write_text(yaml.safe_dump(embodiment, sort_keys=False)) + snapshot["embodiment"]["component"] = str(embodiment_path.resolve()) + snapshot["id"] += "-Affordance-v1" + path = output / "deployment.json" + atomic_json(path, snapshot) + return path + + +def _run_operators( + deployment, output, options, selected, *, seed, sources, viewer, runtime_versions +): + """Call the public upstream collector without operator processes.""" + from embodichain.lab.expansion import ( + ExpansionRunCfg, + GymExpansionHostExtensions, + GymExpansionWorkload, + MeasuredExpansionValidator, + WorkloadExecutionCfg, + run_gym_expansion_workloads, + ) + from embodichain.utils.nms import pose_nms_scope + import torch + + settings = options["engine"] + versions = dict(runtime_versions) + workloads = [] + for name in selected: + operator_deployment = deployment + artifacts = dict(sources) + affordance = options["trajectory"].get("affordance", {}) + if name == "affordance" and ( + affordance["action_options"] or affordance.get("grasp_generators") + ): + operator_deployment = derive_grasp_expansion_deployment( + deployment, output / "inputs" / "affordance", affordance + ) + for path in operator_deployment.parent.iterdir(): + if path.is_file(): + artifacts[f"affordance_{path.name}"] = { + "path": str(path), + "sha256": file_digest(path), + } + _load_deployment(operator_deployment) + identity = hashlib.sha256( + json.dumps( + { + "options": options, + "operator": name, + "seed": seed, + "source_hashes": sorted( + value["sha256"] for value in artifacts.values() + ), + }, + sort_keys=True, + ).encode() + ).hexdigest() + report = { + "job_id": identity, + "semantic_id": identity, + "execution_id": identity, + "recipe_revision": identity, + "input_files": {"deployment": file_digest(operator_deployment)}, + "operator": name, + "adapter": "embodichain.task-program-workloads-v1", + "runtime_versions": versions, + "viewer": viewer, + } + cfg = ExpansionRunCfg( + deployment=str(operator_deployment), + output_dir=str((output / "dataset" / name).resolve()), + batch_size=settings["batch_size"], + target_episodes=settings["episodes_per_operator"], + max_attempts=settings["max_attempts"], + max_steps=settings["max_steps"], + max_wall_time_s=settings["max_wall_time_s"], + settling_steps=settings["settling_steps"], + capture_media=settings["capture_media"], + seed=seed, + ) + workloads.append( + GymExpansionWorkload( + name, + cfg, + report, + artifacts, + _task_expansion_extensions( + options=options, operator=name, seed=seed, report=report + ), + ) + ) + renderer = settings["collection_renderer"] + if renderer == "auto": + renderer = viewer["renderer"] if settings["capture_media"] else None + common = GymExpansionHostExtensions( + collection_renderer=renderer, + validator_factory=lambda env, path: MeasuredExpansionValidator( + env, + options["validation"], + path, + backend=( + "native_contacts" + if settings["collision_backend"] == "native_contacts" + else "geometry" + ), + device=settings["collision_device"], + ), + ) + execution = WorkloadExecutionCfg( + workload_count=min(settings["workload_count"], len(selected)), + batch_size=settings["batch_size"], + max_steps=settings["max_steps"], + settling_steps=settings["settling_steps"], + seed=seed, + reuse_idle_rows=settings.get("reuse_idle_rows", False), + nominal_seed=settings.get("nominal_seed"), + ) + chunks = settings["grasp_nms_chunk_size"] if torch.cuda.is_available() else 2048 + with pose_nms_scope(chunk_size=chunks): + result = run_gym_expansion_workloads( + workloads, execution, extensions=common, output_dir=output + ) + atomic_json(output / "workload-execution.json", result) + return result["workloads"] + + +def run_task_program_expansion( + deployment: str | Path, + configuration: str | Path, + output_dir: str | Path, + *, + seed: int = 0, + operators: Sequence[str] | None = None, + source_artifacts: Mapping[str, object] | None = None, + runtime_versions: Mapping[str, object] | None = None, + viewer: Mapping[str, object] | None = None, + preview_output: str | Path | None = None, +) -> Mapping[str, object]: + """Collect declared scene/motion operators through the single-World engine. + + Args: + deployment: Native Gym deployment; task discovery uses installed providers. + configuration: Closed YAML/JSON scene, trajectory and measured criteria. + output_dir: Fresh collection directory; aggregate results are saved here. + seed: Candidate seed, independent of physical row allocation. + operators: Optional enabled subset in its desired group order. + source_artifacts: Caller-owned hashed inputs retained in native evidence. + runtime_versions: Caller provenance; package versions are read when absent. + viewer: Headless/renderer declarations. A non-headless job opens the native + read-only preview after collection, qualification and export. + preview_output: Optional service status/log directory outside saved data. + + Returns: + Owned operator quotas, confirmed readback, native dataset paths and preview + status. Incomplete groups remain explicit and never earn success credit. + """ + from importlib import import_module, metadata + import torch + + deployment, configuration, output = ( + Path(deployment), + Path(configuration), + Path(output_dir), + ) + options = load_task_program_expansion(configuration) + view = load_expansion_viewer(None if viewer is None else dict(viewer)) + selected = list( + options["trajectory"]["operators"] if operators is None else operators + ) + if ( + not selected + or len(set(selected)) != len(selected) + or set(selected) - set(options["trajectory"]["operators"]) + ): + raise ValueError("Requested operators must be a distinct enabled subset") + import_module("lerobot.datasets.lerobot_dataset") + threads = options["engine"]["cpu_threads"] + for name in ("OMP_NUM_THREADS", "MKL_NUM_THREADS", "OPENBLAS_NUM_THREADS"): + os.environ[name] = str(threads) + torch.set_num_threads(threads) + if runtime_versions is None: + runtime_versions = {} + for name in ( + "embodichain", + "dexsim-engine", + "torch", + "torchvision", + "torchcodec", + "lerobot", + "numpy", + ): + try: + runtime_versions[name] = metadata.version(name) + except metadata.PackageNotFoundError: + runtime_versions[name] = None + output.mkdir(parents=True, exist_ok=True) + inputs = output / "inputs" + inputs.mkdir(exist_ok=True) + sources = dict(source_artifacts or {}) + derived = _prepare_deployment(deployment, inputs) + atomic_json(inputs / "expansion.json", options) + for path in (derived, inputs / "environment.yaml", inputs / "expansion.json"): + sources[f"derived_{path.name}"] = { + "path": str(path), + "sha256": file_digest(path), + } + groups = _run_operators( + derived, + output, + options, + selected, + seed=seed, + sources=sources, + viewer=view, + runtime_versions=runtime_versions, + ) + result = { + "complete": all(value.get("complete", False) for value in groups.values()), + "operators": groups, + "configuration_sha256": file_digest(configuration), + "viewer": view, + } + atomic_json(output / "expansion.json", result) + if not view["headless"]: + from .preview import launch_expansion_preview + + result["preview"] = launch_expansion_preview( + [ + Path(group["directory"]) + for group in groups.values() + if group.get("committed_episodes", 0) + ], + output=( + Path(preview_output) + if preview_output is not None + else output.parent / ".previews" / output.name + ), + open_browser=True, + max_episodes=64, + ) + atomic_json(output / "expansion.json", result) + return result diff --git a/embodichain/lab/expansion/workloads.py b/embodichain/lab/expansion/workloads.py new file mode 100644 index 0000000000..edbc884b2c --- /dev/null +++ b/embodichain/lab/expansion/workloads.py @@ -0,0 +1,787 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""One World, rolling physical rows and receipt-qualified workload quotas.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from contextlib import nullcontext +from dataclasses import dataclass +import math +import os +from threading import get_ident +import time +from typing import Any, Protocol, TYPE_CHECKING + +from embodichain.utils import configclass +from .contracts import CommitReceipt + +if TYPE_CHECKING: + from embodichain.lab.gym.envs.task_program.bridge import TaskProgramDemoBridge + from embodichain.lab.gym.envs.types import ControllerAction + from .program import ProgramBatchResult + +__all__ = [ + "ExpansionWorkloadCfg", + "WorkloadExecutionCfg", + "WorkloadAssignment", + "WorkloadRowResult", + "WorkloadSource", + "WorkloadProgramExecutor", +] + + +@configclass +class ExpansionWorkloadCfg: + """Bound one logical source independently of its physical slot. + + Args: + workload_id: Unique source/recipe group identifier. + target_episodes: Confirmed qualified receipts required for this group. + max_attempts: Physical candidate budget, including rejected preparations. + max_wall_time_s: Admission budget starting with the first candidate. + Already active trajectories retain their ordinary safe-stop path. + """ + + workload_id: str = "default" + target_episodes: int = 1 + max_attempts: int = 8 + max_wall_time_s: float = 600.0 + + def __post_init__(self) -> None: + if ( + type(self.workload_id) is not str + or not self.workload_id + or self.workload_id.strip() != self.workload_id + ): + raise ValueError("workload_id must be a nonempty stripped string") + for name in ("target_episodes", "max_attempts"): + if type(getattr(self, name)) is not int or getattr(self, name) < 1: + raise ValueError(f"{name} must be a positive integer") + if self.max_attempts < self.target_episodes: + raise ValueError("max_attempts must cover target_episodes") + if ( + type(self.max_wall_time_s) not in (int, float) + or not math.isfinite(self.max_wall_time_s) + or self.max_wall_time_s <= 0 + ): + raise ValueError("max_wall_time_s must be finite and positive") + + +@configclass +class WorkloadExecutionCfg: + """Declare physical capacity without allocating processes or Worlds. + + Args: + workload_count: Maximum simultaneous independent source groups, W. + batch_size: Candidate rows reserved for each active group, B. + max_steps: Maximum ordinary controller commands per candidate. + settling_steps: Minimum physical substeps before a row becomes active. + seed: Base seed; each candidate owns a stream keyed by local ordinal. + nominal_seed: Optional independent reset seed for the authored nominal + scene. Omission preserves the historical coupling to ``seed``. + batch_planning: Cooperatively merge compatible native motion/solver calls. + reuse_idle_rows: Let unfinished groups borrow otherwise idle rows. A + rejected attempt permits one additional competing candidate, bounded + by physical capacity and the original admission budgets. Sources must + implement ``cancel(result, *, reason)`` without returning a receipt. + """ + + workload_count: int = 1 + batch_size: int = 1 + max_steps: int = 2000 + settling_steps: int = 5 + seed: int = 0 + batch_planning: bool = True + reuse_idle_rows: bool = False + nominal_seed: int | None = None + + def __post_init__(self) -> None: + for name in ("workload_count", "batch_size", "max_steps"): + if type(getattr(self, name)) is not int or getattr(self, name) < 1: + raise ValueError(f"{name} must be a positive integer") + for name in ("settling_steps", "seed"): + if type(getattr(self, name)) is not int or getattr(self, name) < 0: + raise ValueError(f"{name} must be a nonnegative integer") + if type(self.batch_planning) is not bool: + raise TypeError("batch_planning must be a bool") + if type(self.reuse_idle_rows) is not bool: + raise TypeError("reuse_idle_rows must be a bool") + if self.nominal_seed is not None and ( + type(self.nominal_seed) is not int or self.nominal_seed < 0 + ): + raise ValueError("nominal_seed must be a nonnegative integer or None") + + @property + def reset_seed(self) -> int: + """Return the declared nominal scene seed, with legacy coupling by default.""" + return self.seed if self.nominal_seed is None else self.nominal_seed + + @property + def num_envs(self) -> int: + """Return the required single-World capacity, W multiplied by B.""" + return self.workload_count * self.batch_size + + +@dataclass(frozen=True) +class WorkloadAssignment: + """Logical candidate identity bound to a physical row for one attempt. + + Args: + workload_id: Source group, independent of lane/row allocation. + ordinal: Zero-based candidate index local to this workload. + env_id: Physical environment row in the sole World. + seed: Candidate-owned random stream seed. + """ + + workload_id: str + ordinal: int + env_id: int + seed: int + + +@dataclass(frozen=True) +class WorkloadRowResult: + """Canonical bridge outcome and exact World control-boundary span. + + Args: + assignment: Candidate whose bridge and measured buffer were consumed. + program: Original Task Program result for this one physical row. + world_control_start: First actually consumed command boundary, or None. + world_control_end: Last consumed command boundary, or None. + """ + + assignment: WorkloadAssignment + program: ProgramBatchResult + world_control_start: int | None + world_control_end: int | None + + +class WorkloadSource(Protocol): + """Task-owned scene/plan/evidence ports; the executor alone steps the World. + + Sources retain native ExpansionSession admission, required physical checks, + evidence assembly and sink receipts. ``complete`` may return a receipt only + after all required checks pass and native persistence confirms the write. + Callbacks must not advance physics or create another World. A completed + group's frozen evidence can be exported independently of the live World. + Idle-row reuse additionally requires ``cancel(result, *, reason) -> None``. + That port retires uncommitted native work and preserves the canonical + program result; administrative cancellation never earns quota. + """ + + def prepare(self, assignments: tuple[WorkloadAssignment, ...]) -> None: + """Apply selected-row changes inside the trusted reset callback.""" + ... + + def activate( + self, assignment: WorkloadAssignment, observation: object + ) -> TaskProgramDemoBridge | None: + """Validate measured preparation, capture T+1 initial data and bind a row.""" + ... + + def started(self, assignment: WorkloadAssignment, world_control_step: int) -> None: + """Mark rollout started in the native session after its first actual step.""" + ... + + def observe( + self, + assignments: tuple[WorkloadAssignment, ...], + action: ControllerAction, + observation: object, + ) -> Sequence[int]: + """Record every active boundary; return rows requiring safe rejection.""" + ... + + def complete(self, result: WorkloadRowResult) -> CommitReceipt | None: + """Assemble native evidence and return its confirmed receipt or rejection.""" + ... + + +class WorkloadProgramExecutor: + """Refill independent rows while preserving native bridge acknowledgement. + + Args: + environment: One initialized environment with exactly W times B rows. + workloads: Ordered source groups with independent quotas and budgets. + source: Host-owned scene, Task Program, validation and persistence ports. + cfg: Capacity and control/settling budgets; creates no World itself. + checkpoint: Optional cooperative cancellation/window callback, called at + control boundaries on the owner thread. + on_workload_done: Optional frozen-group notification. It receives an + owned count mapping and must not advance or mutate the live World. + """ + + def __init__( + self, + environment: Any, + workloads: Sequence[ExpansionWorkloadCfg], + source: WorkloadSource, + cfg: WorkloadExecutionCfg, + *, + checkpoint: Callable[[], None] | None = None, + on_workload_done: Callable[[str, Mapping[str, object]], None] | None = None, + ) -> None: + import torch + + if not isinstance(cfg, WorkloadExecutionCfg): + raise TypeError("cfg must be WorkloadExecutionCfg") + cfg.__post_init__() + self.env = getattr(environment, "unwrapped", environment) + if self.env.num_envs != cfg.num_envs: + raise ValueError( + "the single World must contain workload_count * batch_size environments" + ) + events = getattr(self.env, "event_manager", None) + if events is not None and events.active_functors.get("interval"): + raise ValueError("rolling expansion requires fixed episode parameters") + if not workloads or any( + not isinstance(w, ExpansionWorkloadCfg) for w in workloads + ): + raise ValueError("workloads must contain ExpansionWorkloadCfg values") + for workload in workloads: + workload.__post_init__() + self.workloads = { + w.workload_id: ExpansionWorkloadCfg(**w.to_dict()) for w in workloads + } + if len(self.workloads) != len(workloads) or cfg.workload_count > len(workloads): + raise ValueError("workload IDs must be unique and cover every active lane") + self.cfg, self.source = WorkloadExecutionCfg(**cfg.to_dict()), source + if cfg.reuse_idle_rows and not callable(getattr(source, "cancel", None)): + raise TypeError("idle-row reuse requires a native source cancellation port") + self.checkpoint, self.on_workload_done = checkpoint, on_workload_done + self.owner = get_ident() + from embodichain.lab.sim import SimulationManager + + if len(SimulationManager._instances) != 1: + raise ValueError( + "rolling workload collection requires one live World in its process" + ) + state_device = self.env.robot.get_qpos().device + if state_device.type != self.env.sim.device.type: + raise RuntimeError("measured robot state and World devices disagree") + self._world_info = { + "process_id": os.getpid(), + "world_instance_id": self.env.sim.instance_id, + "world_object_id": id(self.env.sim.get_world()), + "world_count": len(SimulationManager._instances), + "physics_backend": self.env.sim.physics_backend, + "physics_device": str(self.env.sim.device), + "state_device": str(state_device), + "gpu_physics": bool(self.env.sim.is_use_gpu_physics), + "solvers": { + name: {"class": type(solver).__name__, "device": str(solver.device)} + for name, solver in self.env.robot._solvers.items() + }, + } + self._used = False + self.lanes: list[str | None] = [None] * cfg.workload_count + self.counts = { + name: {"attempts": 0, "rollouts": 0, "committed": 0} + for name in self.workloads + } + self.started_at: dict[str, float] = {} + self.warming: dict[int, dict[str, Any]] = {} + self.live: dict[int, dict[str, Any]] = {} + self.completed: set[str] = set() + self.receipts: set[tuple[str, str, str]] = set() + self.control_steps = self.active_control_steps = self.refills = 0 + self.hold = self.env.robot.get_qpos(target=True).clone() + self.observation = None + self.planning_stats: dict[str, int] = {} + self.reused_rows = 0 + self.cancelled = {name: 0 for name in self.workloads} + self.torch = torch + + @property + def world_info(self) -> Mapping[str, object]: + """Return owned actual process, World, state and solver device identities.""" + from copy import deepcopy + + return deepcopy(self._world_info) + + def _eligible(self, name: str) -> bool: + workload, counts = self.workloads[name], self.counts[name] + return ( + getattr(self.source, "can_admit", lambda name: True)(name) + and counts["committed"] < workload.target_episodes + and counts["attempts"] < workload.max_attempts + and ( + name not in self.started_at + or time.monotonic() - self.started_at[name] < workload.max_wall_time_s + ) + ) + + def _inflight(self, name: str) -> int: + return sum( + state["assignment"].workload_id == name + for state in (*self.live.values(), *self.warming.values()) + ) + + def _group_done(self, name: str) -> None: + if name not in self.completed: + self.completed.add(name) + if self.on_workload_done is not None: + self.on_workload_done(name, dict(self.counts[name])) + + def _fill(self) -> None: + added = [] + occupied = self.live.keys() | self.warming.keys() + for lane, name in enumerate(self.lanes): + if ( + name is not None + and not self._inflight(name) + and not self._eligible(name) + ): + self._group_done(name) + name = None + if name is None: + name = next( + ( + key + for key in self.workloads + if key not in self.lanes and self._eligible(key) + ), + None, + ) + self.lanes[lane] = name + if name is None: + continue + for row in range( + lane * self.cfg.batch_size, (lane + 1) * self.cfg.batch_size + ): + if row in occupied or not self._eligible(name): + continue + counts, spec = self.counts[name], self.workloads[name] + if counts["committed"] + self._inflight(name) >= spec.target_episodes: + break + assignment = WorkloadAssignment( + name, counts["attempts"], row, self.cfg.seed + counts["attempts"] + ) + counts["attempts"] += 1 + self.started_at.setdefault(name, time.monotonic()) + self.warming[row] = {"assignment": assignment, "settled": 0} + added.append(assignment) + if self.cfg.reuse_idle_rows: + for row in range(self.cfg.num_envs): + if row in self.live or row in self.warming: + continue + candidates = [] + for name in dict.fromkeys(self.lanes): + if name is None or not self._eligible(name): + continue + counts = self.counts[name] + inflight = self._inflight(name) + failed = counts["attempts"] - counts["committed"] - inflight + demand = ( + self.workloads[name].target_episodes + - counts["committed"] + + failed + ) + if inflight < demand: + candidates.append((inflight, counts["attempts"], name)) + if not candidates: + break + name = min(candidates)[2] + counts = self.counts[name] + assignment = WorkloadAssignment( + name, counts["attempts"], row, self.cfg.seed + counts["attempts"] + ) + counts["attempts"] += 1 + self.started_at.setdefault(name, time.monotonic()) + self.warming[row] = {"assignment": assignment, "settled": 0} + added.append(assignment) + self.reused_rows += 1 + if not added: + return + self.refills += bool(self.live) + assignments = tuple(added) + before = self.env.sim.physics_step_index + result = self.env.reset( + seed=self.cfg.reset_seed, + options={ + "save_data": False, + "reset_ids": [a.env_id for a in added], + "episode_prepare": lambda: self.source.prepare(assignments), + }, + ) + if self.env.sim.physics_step_index != before: + raise RuntimeError( + "rolling reset/preparation must not advance World physics" + ) + self.observation = result[0] + rows = [a.env_id for a in added] + self.hold[rows] = self.env.robot.get_qpos(target=True)[rows] + + def _activate(self) -> None: + from ._workload_cursor import _WorkloadCursor + from .program import ProgramBatchResult + + for row in tuple(self.warming): + state = self.warming[row] + assignment = state["assignment"] + expired = ( + time.monotonic() - self.started_at[assignment.workload_id] + >= self.workloads[assignment.workload_id].max_wall_time_s + ) + if state["settled"] < self.cfg.settling_steps and not expired: + continue + if expired: + bridge = None + else: + self.env._elapsed_steps[row] = 0 + self.env._reset_physical_objective((row,)) + bridge = self.source.activate(assignment, self.observation) + del self.warming[row] + if bridge is None: + outcome = ProgramBatchResult( + (row,), + (0,), + (False,), + ("wall_time_exhausted" if expired else "preparation_rejected",), + ((),), + ) + if ( + self.source.complete( + WorkloadRowResult(assignment, outcome, None, None) + ) + is not None + ): + raise ValueError( + "unstarted or rejected preparation cannot earn a receipt" + ) + continue + if ( + tuple(bridge.env_ids.tolist()) != (row,) + or bridge.clock.step_index != 0 + or not math.isclose( + bridge.clock.step_dt, self.env.step_dt, rel_tol=0, abs_tol=1e-12 + ) + ): + raise ValueError( + "bridge must freshly bind exactly the assigned physical row and clock" + ) + self.live[row] = { + **state, + "cursor": _WorkloadCursor( + bridge, + self.env.robot, + assignment.seed, + budget=getattr( + self.source, "planning_budget", lambda assignment: None + )(assignment), + ), + "start": None, + } + + def _finish(self, row: int) -> None: + from .program import ProgramBatchResult + + state = self.live[row] + assignment, cursor = state["assignment"], state["cursor"] + end = self.control_steps if cursor.length else None + if cursor.length and end - state["start"] + 1 != cursor.length: + raise RuntimeError("an active trajectory missed a World control boundary") + result = ProgramBatchResult( + (row,), + (cursor.length,), + (cursor.accepted,), + (cursor.reason,), + (tuple(cursor.results),), + ) + row_result = WorkloadRowResult(assignment, result, state["start"], end) + cancelled = state.get("cancel_reason") + if cancelled: + if getattr(self.source, "cancel")(row_result, reason=cancelled) is not None: + raise ValueError("cancelled work cannot earn a receipt") + self.cancelled[assignment.workload_id] += 1 + receipt = None + else: + receipt = self.source.complete(row_result) + if receipt is not None: + if ( + not isinstance(receipt, CommitReceipt) + or not receipt.confirmed + or not cursor.accepted + or not cursor.length + ): + raise ValueError( + "only confirmed qualified native receipts earn workload quota" + ) + identity = receipt.episode_id, receipt.candidate_id, receipt.commit_id + if any( + identity[i] == known[i] for known in self.receipts for i in range(3) + ): + raise ValueError( + "a repeated episode/candidate/commit cannot earn quota twice" + ) + self.receipts.add(identity) + self.counts[assignment.workload_id]["committed"] += 1 + cursor.close() + del self.live[row] + name = assignment.workload_id + if ( + receipt is not None + and self.cfg.reuse_idle_rows + and self.counts[name]["committed"] == self.workloads[name].target_episodes + ): + self._cancel_peers(name) + + def _cancel_peers(self, name: str) -> None: + from .program import ProgramBatchResult + + reason = "quota_fulfilled_by_peer" + for state in self.live.values(): + if state["assignment"].workload_id == name and not state.get( + "cancel_reason" + ): + state["cancel_reason"] = reason + cursor = state["cursor"] + # An existing rejection/stop already yielded its next real + # hold during gather. Do not resume and acknowledge it twice. + state["replace_command"] = ( + not cursor.finished + and cursor.rejection is None + and cursor.emergency is None + ) + state["cursor"].abort(reason) + for row, state in tuple(self.warming.items()): + if state["assignment"].workload_id != name: + continue + outcome = ProgramBatchResult((row,), (0,), (False,), (reason,), ((),)) + result = WorkloadRowResult(state["assignment"], outcome, None, None) + if getattr(self.source, "cancel")(result, reason=reason) is not None: + raise ValueError("cancelled preparation cannot earn a receipt") + self.cancelled[name] += 1 + del self.warming[row] + + def run(self) -> Mapping[str, object]: + """Collect until every group meets quota or exhausts its original budget. + + Returns: + Owned counts, completeness and actual World control/planning stats. + Failed quotas remain explicit; partial results never imply success. + """ + if get_ident() != self.owner or self._used: + raise RuntimeError( + "a workload executor is single-use on its World owner thread" + ) + self._used = True + from tensordict import TensorDict + from embodichain.lab.gym.envs.types import ControllerAction + from ._planning_batches import batched_planning + from embodichain.lab.sim.objects.backends._numeric_observations import ( + numeric_observation_scope, + ) + + env, torch = self.env, self.torch + previous_auto = getattr(env, "_demo_no_auto_reset", False) + had_mask = hasattr(env, "_demo_active_mask") + previous_mask = getattr(env, "_demo_active_mask", None) + env._demo_no_auto_reset = True + primary = None + planning = ( + batched_planning(env.robot, self.cfg.seed) + if self.cfg.batch_planning + else nullcontext() + ) + try: + with planning as planner: + while True: + if self.checkpoint is not None: + self.checkpoint() + self._fill() + self._activate() + cursors = {row: state["cursor"] for row, state in self.live.items()} + with numeric_observation_scope(env.sim, env.robot): + commands = ( + planner.gather(cursors) + if planner is not None + else { + row: command + for row, cursor in cursors.items() + if (command := cursor.next()) is not None + } + ) + for row in tuple(self.live): + if row not in commands: + self._finish(row) + if not commands and not self.warming: + if any(self._eligible(name) for name in self.workloads): + continue + break + qpos, qvel = self.hold.clone(), torch.zeros_like(self.hold) + velocity = ( + env.expert_action_spec.joint_command_mode == "position_velocity" + ) + for row in tuple(commands): + cursor = self.live[row]["cursor"] + if self.live[row].pop("replace_command", False): + replacement = cursor.next() + if replacement is None: + del commands[row] + self._finish(row) + continue + commands[row] = replacement + if ( + cursor.length >= self.cfg.max_steps + and cursor.emergency is None + ): + cursor.abort("step_budget_exhausted") + replacement = cursor.next() + if replacement is None: + del commands[row] + self._finish(row) + continue + commands[row] = replacement + value = commands[row].value + fields = ( + {"qpos": value} + if isinstance(value, torch.Tensor) + else value + ) + if "qpos" not in fields or set(fields) - {"qpos", "qvel"}: + raise ValueError( + "workload rows require explicit position controller commands" + ) + if velocity and "qvel" not in fields: + raise ValueError( + "position-velocity evidence requires explicit velocity commands" + ) + for name, target in (("qpos", qpos), ("qvel", qvel)): + if name not in fields: + continue + data = fields[name] + if data.ndim != 2 or data.shape[0] != 1: + raise ValueError( + "row commands must retain one physical batch row" + ) + columns = ( + list(env.active_joint_ids) + if data.shape[1] == len(env.active_joint_ids) + else list(range(env.robot.dof)) + ) + if data.shape[1] != len(columns): + raise ValueError("row command joint layout differs") + target[row, columns] = data[0] + velocity |= name == "qvel" + if not commands and not self.warming: + continue + payload = {"qpos": qpos} + if velocity: + payload["qvel"] = qvel + action = ControllerAction( + TensorDict(payload, batch_size=[env.num_envs]), + metadata={ + "row_actions": { + str(row): dict(command.metadata) + for row, command in commands.items() + } + }, + ) + mask = torch.zeros( + env.num_envs, dtype=torch.bool, device=env.device + ) + mask[list(commands)] = True + # Settling rows execute their explicit reset hold. Marking + # them inactive lets Gym replace that target with measured + # qpos at every tick, removing the drive's restoring error. + # They still have no cursor, receipt or acceptance samples. + mask[list(self.warming)] = True + env._demo_active_mask = mask + before = env.sim.physics_step_index + self.observation, _, terminated, truncated, _ = env.step(action) + advanced = env.sim.physics_step_index - before + if advanced <= 0: + raise RuntimeError( + "the owning World did not advance physical time" + ) + self.control_steps += 1 + self.active_control_steps += len(commands) + self.hold = qpos + for state in self.warming.values(): + state["settled"] += advanced + for row in commands: + state, cursor = self.live[row], self.live[row]["cursor"] + if cursor.length == 0: + state["start"] = self.control_steps + self.counts[state["assignment"].workload_id][ + "rollouts" + ] += 1 + self.source.started(state["assignment"], self.control_steps) + cursor.length += 1 + cursor.pending_consumed = True + assignments = tuple( + self.live[row]["assignment"] for row in commands + ) + rejected = ( + tuple( + self.source.observe(assignments, action, self.observation) + ) + if commands + else () + ) + if ( + len(set(rejected)) != len(rejected) + or set(rejected) - commands.keys() + ): + raise ValueError( + "source rejection must select unique active physical rows" + ) + for row in commands: + if row in rejected: + self.live[row]["cursor"].abort("measured_validation_failed") + elif bool(terminated[row]) or bool(truncated[row]): + self.live[row]["cursor"].abort("environment_terminated") + if planner is not None: + self.planning_stats = dict(planner.stats) + for name in self.workloads: + self._group_done(name) + return { + "complete": all( + self.counts[name]["committed"] == spec.target_episodes + for name, spec in self.workloads.items() + ), + "reused_rows": self.reused_rows, + "cancelled": dict(self.cancelled), + "num_envs": self.cfg.num_envs, + "workload_count": self.cfg.workload_count, + "batch_size": self.cfg.batch_size, + "counts": {name: dict(values) for name, values in self.counts.items()}, + "control_steps": self.control_steps, + "active_control_steps": self.active_control_steps, + "refills": self.refills, + "planning": dict(self.planning_stats), + "world": self.world_info, + } + except BaseException as error: + primary = error + raise + finally: + cleanup = None + for state in self.live.values(): + try: + state["cursor"].close() + except BaseException as error: + cleanup = error + env._demo_no_auto_reset = previous_auto + if had_mask: + env._demo_active_mask = previous_mask + elif hasattr(env, "_demo_active_mask"): + delattr(env, "_demo_active_mask") + if primary is None and cleanup is not None: + raise cleanup diff --git a/embodichain/lab/gym/envs/_json.py b/embodichain/lab/gym/envs/_json.py index 660c687214..90ed8e7487 100644 --- a/embodichain/lab/gym/envs/_json.py +++ b/embodichain/lab/gym/envs/_json.py @@ -27,6 +27,10 @@ def json_safe_copy(value: Any, *, field_name: str) -> Any: """Return an owned JSON value without implicit type coercion.""" + from embodichain.utils._owned_json import _FrozenMap + + if type(value) is _FrozenMap and value._strict: + return value.mutable_copy() if value is None or type(value) in {bool, int, str}: return value if type(value) is float: diff --git a/embodichain/lab/gym/envs/task_program/bridge.py b/embodichain/lab/gym/envs/task_program/bridge.py index 9de1c60d4d..ae38d02e77 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -452,14 +452,33 @@ def encode( active_mask: torch.Tensor, ) -> EnvAction: """Write addressed joints while holding every other qpos column.""" + return self._encode_commands( + (command,), base_action=base_action, active_mask=active_mask + ) + + def _encode_commands(self, commands, *, base_action, active_mask): + """Own one output for a validated frame's built-in joint destinations.""" + if not isinstance(base_action, (torch.Tensor, TensorDict)): + raise TypeError( + "The built-in joint-position encoder requires a tensor action or " + "a TensorDict containing qpos." + ) + action = base_action.clone() + for command in commands: + self._write_command(command, action=action, active_mask=active_mask) + return action + + @staticmethod + def _write_command(command, *, action, active_mask): + """Apply one command to an already-owned frame output.""" if not isinstance(command.target, JointPositionTarget): raise TypeError("Joint-position transport requires JointPositionTarget.") if not isinstance(command.payload, JointPositionPayload): raise TypeError("Joint-position transport requires JointPositionPayload.") - if isinstance(base_action, torch.Tensor): - qpos = base_action - elif isinstance(base_action, TensorDict) and "qpos" in base_action: - qpos = base_action["qpos"] + if isinstance(action, torch.Tensor): + qpos = action + elif isinstance(action, TensorDict) and "qpos" in action: + qpos = action["qpos"] else: raise TypeError( "The built-in joint-position encoder requires a tensor action or " @@ -487,38 +506,34 @@ def encode( if not qpos.is_floating_point(): raise TypeError("The full-qpos base action must be floating point.") - action_qpos = qpos.clone() + action_qpos = qpos columns = torch.tensor(joint_ids, dtype=torch.long, device=qpos.device) selected = action_qpos.index_select(1, columns) selected[active_mask] = positions[active_mask].to(dtype=action_qpos.dtype) action_qpos[:, columns] = selected - if isinstance(base_action, torch.Tensor): - return action_qpos + if isinstance(action, torch.Tensor): + return velocities = command.payload.velocities if velocities is None: raise ValueError( "position_velocity joint commands require payload velocities." ) - if "qvel" not in base_action: + if "qvel" not in action: raise TypeError( "Position-velocity joint actions require qpos and qvel base targets." ) - qvel = base_action["qvel"] + qvel = action["qvel"] if qvel.shape != qpos.shape or qvel.device != qpos.device: raise ValueError( "Full qpos and qvel base targets must share shape and device." ) - action_qvel = qvel.clone() + action_qvel = qvel selected_velocity = action_qvel.index_select(1, columns) selected_velocity[active_mask] = velocities[active_mask].to( dtype=action_qvel.dtype ) action_qvel[:, columns] = selected_velocity - action = base_action.clone() - action["qpos"] = action_qpos - action["qvel"] = action_qvel - return action def hold( self, @@ -534,6 +549,9 @@ def hold( return base_action.clone() +_BUILTIN_JOINT_ENCODE = JointPositionGymTransportEncoder.encode + + class RuntimeCommandFrameEncoder: """Encode transport-neutral command frames to controller-ready Gym actions. @@ -742,7 +760,17 @@ def encode(self, frame: RuntimeCommandFrame) -> EnvAction: self._validate_command_types(transport, command) by_transport.setdefault(command.transport_id, []).append(command) for transport_id, transport in self._transports.items(): - for command in by_transport.get(transport_id, ()): + commands = by_transport.get(transport_id, ()) + if ( + len(commands) > 1 + and type(transport) is JointPositionGymTransportEncoder + and getattr(transport.encode, "__func__", None) is _BUILTIN_JOINT_ENCODE + ): + action = transport._encode_commands( + commands, base_action=action, active_mask=frame.active_mask + ) + continue + for command in commands: action = transport.encode( command, base_action=action, @@ -2067,12 +2095,14 @@ def abort( reason: str, *, last_action_consumed: bool, + controller_hold: ControllerAction | None = None, ) -> Iterator[ControllerAction]: return self._abort_segment( segment, lifecycle, reason=reason, last_action_consumed=last_action_consumed, + controller_hold=controller_hold, ) return abort @@ -2084,12 +2114,28 @@ def _abort_segment( *, reason: str, last_action_consumed: bool, + controller_hold: ControllerAction | None = None, ) -> Iterator[ControllerAction]: - """Abort one segment, surfacing a safe hold only after controller activity.""" + """Abort after activity, preferring runtime-proven controller holds. + + A trusted controller owner may supply an explicit labelled hold when + the runtime became terminal before its final yielded motion was + consumed. Pending motion alone is never promoted to a safe stop. + """ if type(reason) is not str or not reason: raise ValueError("abort reason must be a non-empty string.") if type(last_action_consumed) is not bool: raise TypeError("last_action_consumed must be a bool.") + if controller_hold is not None: + if not isinstance(controller_hold, ControllerAction): + raise TypeError("controller_hold must be a ControllerAction or None.") + if ( + controller_hold.metadata.get("bridge_action_kind") + not in _SAFE_HOLD_ACTION_KINDS + ): + raise ValueError( + "controller_hold requires an explicit safe-hold action kind." + ) if lifecycle.abort_started: raise RuntimeError( f"Segment {segment.segment_id!r} abort handshake already started." @@ -2153,6 +2199,15 @@ def _abort_segment( safe_action = self._sink.drain_safe_stop_action( fallback=None if last_action_consumed else pending, ) + if safe_action is None and controller_hold is not None: + # A controller owner can prove a hold even after a runtime has + # become terminal while its final motion is still unconsumed. + # Never treat that pending motion itself as a safe stop. + held = controller_hold.snapshot() + safe_action = ControllerAction( + held.value, + metadata={**held.metadata, "abort_hold_source": "controller_owner"}, + ) if safe_action is None: raise TaskProgramBridgeError( f"Segment {segment.segment_id!r} stopped before exhaustion, but " diff --git a/embodichain/lab/sim/atomic_skills/_command_batches.py b/embodichain/lab/sim/atomic_skills/_command_batches.py new file mode 100644 index 0000000000..492357f672 --- /dev/null +++ b/embodichain/lab/sim/atomic_skills/_command_batches.py @@ -0,0 +1,194 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Validate owned CUDA command snapshots in bounded CPU slabs.""" + +from __future__ import annotations + +from collections import defaultdict +import torch +from .bindings import JointPositionTarget +from .runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + TimedCommandSequence, +) + +__all__: list[str] = [] + + +def _transfer(tensors, device): + """Own one slab per dtype; preserve bits, shapes and independent field spans.""" + import torch + + grouped = defaultdict(dict) + for tensor in tensors: + grouped[tensor.dtype][id(tensor)] = tensor + copied = {} + for group in grouped.values(): + values = tuple(group.values()) + packed = torch.cat([value.detach().reshape(-1) for value in values]).to(device) + for value, part in zip(values, packed.split([v.numel() for v in values])): + copied[id(value)] = part.reshape(value.shape) + return copied + + +def leaves(frame): + if type(frame) is not RuntimeCommandFrame or type(frame.commands) not in ( + list, + tuple, + ): + return None + tensors = [frame.active_mask, frame.env_ids, frame.hold_duration] + for command in frame.commands: + if ( + type(command) is not EndpointCommand + or type(command.target) is not JointPositionTarget + or type(command.payload) is not JointPositionPayload + ): + return None + tensors.append(command.payload.positions) + if command.payload.velocities is not None: + tensors.append(command.payload.velocities) + if any(type(value) is not torch.Tensor or value.requires_grad for value in tensors): + return None + if frame.env_ids.device.type != "cuda" or any( + value.device != frame.env_ids.device for value in tensors + ): + return None + return tensors + + +def cpu_frame(frame, copied): + commands = tuple( + EndpointCommand( + target=command.target, + payload=JointPositionPayload( + positions=copied[id(command.payload.positions)], + velocities=( + copied[id(command.payload.velocities)] + if command.payload.velocities is not None + else None + ), + ), + ) + for command in frame.commands + ) + return RuntimeCommandFrame( + commands=commands, + active_mask=copied[id(frame.active_mask)], + env_ids=copied[id(frame.env_ids)], + hold_duration=copied[id(frame.hold_duration)], + ) + + +def own(cls, **fields): + value = object.__new__(cls) + for name, field in fields.items(): + object.__setattr__(value, name, field) + return value + + +def lift_frame(frame, copied): + return own( + RuntimeCommandFrame, + commands=tuple( + own( + EndpointCommand, + target=command.target, + payload=own( + JointPositionPayload, + positions=copied[id(command.payload.positions)], + velocities=( + copied[id(command.payload.velocities)] + if command.payload.velocities is not None + else None + ), + ), + ) + for command in frame.commands + ), + active_mask=copied[id(frame.active_mask)], + env_ids=copied[id(frame.env_ids)], + hold_duration=copied[id(frame.hold_duration)], + ) + + +def numeric(frame): + # Already passed all native CPU checks; collect owned validated fields. + values = [frame.active_mask, frame.env_ids, frame.hold_duration] + for command in frame.commands: + values.append(command.payload.positions) + if command.payload.velocities is not None: + values.append(command.payload.velocities) + return values + + +def validate_frame(frame): + if ( + type(frame) is not RuntimeCommandFrame + or type(frame.env_ids) is not torch.Tensor + or frame.env_ids.device.type != "cuda" + ): + return False + values = leaves(frame) + if values is None: + return False + checked = cpu_frame(frame, _transfer(values, "cpu")) + result = lift_frame(checked, _transfer(numeric(checked), frame.env_ids.device)) + for name in ("commands", "active_mask", "env_ids", "hold_duration"): + object.__setattr__(frame, name, getattr(result, name)) + return True + + +def validate_sequence(sequence): + if ( + type(sequence.frames) not in (list, tuple) + or type(sequence.env_ids) is not torch.Tensor + or sequence.env_ids.device.type != "cuda" + or sequence.env_ids.requires_grad + ): + return False + tensors = [sequence.env_ids] + for frame in sequence.frames: + values = leaves(frame) + if values is None or frame.env_ids.device != sequence.env_ids.device: + return False + tensors.extend(values) + copied = _transfer(tensors, "cpu") + # Preserve native sequence-before-frame rejection order, including + # malformed mutable IDs in an otherwise previously valid snapshot. + TimedCommandSequence(frames=(), env_ids=copied[id(sequence.env_ids)]) + for index, frame in enumerate(sequence.frames): + if not torch.equal(copied[id(frame.env_ids)], copied[id(sequence.env_ids)]): + raise ValueError(f"Frame {index} env_ids do not match the sequence.") + checked = TimedCommandSequence( + frames=tuple(cpu_frame(frame, copied) for frame in sequence.frames), + env_ids=copied[id(sequence.env_ids)], + ) + tensors = [checked.env_ids] + for frame in checked.frames: + tensors.extend(numeric(frame)) + copied = _transfer(tensors, sequence.env_ids.device) + object.__setattr__( + sequence, + "frames", + tuple(lift_frame(frame, copied) for frame in checked.frames), + ) + object.__setattr__(sequence, "env_ids", copied[id(checked.env_ids)]) + return True diff --git a/embodichain/lab/sim/atomic_skills/bindings.py b/embodichain/lab/sim/atomic_skills/bindings.py index e2bd6cd1e1..3f57133224 100644 --- a/embodichain/lab/sim/atomic_skills/bindings.py +++ b/embodichain/lab/sim/atomic_skills/bindings.py @@ -203,6 +203,24 @@ def __post_init__(self) -> None: raise ValueError("JointPositionTarget.joint_ids must be unique.") object.__setattr__(self, "joint_ids", joint_ids) + def snapshot(self) -> JointPositionTarget: + """Return an independently owned target. + + Standard immutable fields are reconstructed and validated; subclasses + and extended scalar types retain generic deep-snapshot semantics. + + Returns: + A new target of the same concrete type. + """ + if ( + type(self) is not JointPositionTarget + or type(self.control_part) is not str + or type(self.joint_ids) is not tuple + or any(type(joint) is not int for joint in self.joint_ids) + ): + return deepcopy(self) + return JointPositionTarget(self.control_part, self.joint_ids) + @property def transport_id(self) -> str: """Return the built-in joint-position transport identifier.""" diff --git a/embodichain/lab/sim/atomic_skills/core.py b/embodichain/lab/sim/atomic_skills/core.py index 20587e026b..fcebdab5d4 100644 --- a/embodichain/lab/sim/atomic_skills/core.py +++ b/embodichain/lab/sim/atomic_skills/core.py @@ -61,6 +61,8 @@ ) from .requirements import GRASP_CAPABILITY, SkillBindingContract from .runtime_commands import ( + _sequence_from_fresh_frames, + _frame_from_fresh_commands, EndpointCommand, JointPositionPayload, RuntimeCommandFrame, @@ -1358,8 +1360,8 @@ def _authorize_command_targets( EndpointCommand(target=target, payload=command.payload) ) frames.append( - RuntimeCommandFrame( - commands=tuple(endpoint_commands), + _frame_from_fresh_commands( + endpoint_commands, active_mask=( frame.active_mask if active_mask is None @@ -1369,7 +1371,7 @@ def _authorize_command_targets( hold_duration=frame.hold_duration, ) ) - return TimedCommandSequence(frames=tuple(frames), env_ids=commands.env_ids) + return _sequence_from_fresh_frames(frames, commands.env_ids) def _joint_command_sequence( self, @@ -1420,14 +1422,14 @@ def _joint_command_sequence( # dispatching it, wait for the next arrival interval; the terminal # frame reuses its own interval as the action's settling window. frames.append( - RuntimeCommandFrame( - commands=tuple(endpoint_commands), + _frame_from_fresh_commands( + endpoint_commands, active_mask=active_mask, env_ids=trajectory.env_ids, hold_duration=trajectory.dt[:, next_waypoint_index], ) ) - return TimedCommandSequence(frames=tuple(frames), env_ids=trajectory.env_ids) + return _sequence_from_fresh_frames(frames, trajectory.env_ids) @staticmethod def _build_segments( diff --git a/embodichain/lab/sim/atomic_skills/policies.py b/embodichain/lab/sim/atomic_skills/policies.py index 22fb610e65..d868fbbcfa 100644 --- a/embodichain/lab/sim/atomic_skills/policies.py +++ b/embodichain/lab/sim/atomic_skills/policies.py @@ -19,7 +19,7 @@ from __future__ import annotations import math -from collections.abc import Callable +from collections.abc import Callable, Mapping from copy import deepcopy from dataclasses import dataclass, field, fields, is_dataclass from enum import Enum @@ -63,6 +63,16 @@ class MotionPolicy: strategy: Literal["motion_gen", "ik_interp"] = "ik_interp" """Motion strategy: ``motion_gen`` or ``ik_interp``.""" + phase_paths: Mapping[str, Literal["joint", "cartesian"]] = field( + default_factory=dict, kw_only=True + ) + """Explicit primitive phase paths; empty preserves each skill's defaults. + + PickUp reads pre_grasp/grasp/lift (and optional initial_lift); Place reads + pre_place/place/retreat. Other primitives retain their own declared paths. + Placement translation keyframes all retain their original phase's path. + """ + sample_count: int = 50 """Minimum trajectory sample count when the backend does not preserve samples.""" @@ -143,6 +153,22 @@ class MotionPolicy: """ def __post_init__(self) -> None: + if not isinstance(self.phase_paths, Mapping) or any( + type(name) is not str + or not name.strip() + or mode not in ("joint", "cartesian") + for name, mode in self.phase_paths.items() + ): + raise ValueError("phase_paths must map phase names to joint/cartesian") + if self.phase_paths and ( + self.strategy != "ik_interp" + or self.interpolation_mode != "fixed_grid" + or self.ik_seed_mode != "sequential" + ): + raise ValueError("phase_paths requires fixed-grid sequential ik_interp") + from embodichain.utils._owned_json import freeze_metadata + + object.__setattr__(self, "phase_paths", freeze_metadata(self.phase_paths)) if self.final_velocity_rejection not in {"hold", "mask_only"}: raise ValueError("final_velocity_rejection must be hold or mask_only.") segment_retries = self.segment_sample_count_retries diff --git a/embodichain/lab/sim/atomic_skills/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_skills/primitives/coordinated_placement.py index 34a7c42dd4..9ad8e0d16b 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_skills/primitives/coordinated_placement.py @@ -400,8 +400,11 @@ def failed_with_batching(message: str) -> SkillPlan: placing_place_qpos = placing_approach_traj[:, -1] support_place_qpos = support_approach_traj[:, -1] - placing_hand_qpos = state.last_qpos[:, list(resources.placing_hand.joint_ids)] - support_hand_qpos = state.last_qpos[:, list(resources.support_hand.joint_ids)] + # A contact-blocked measured position does not retain closing effort. + # Both bindings already require a grasp command; use it until release, + # just as MoveHeldObject does for a single held object. + placing_hand_qpos = resources.placing_hand_close_qpos + support_hand_qpos = resources.support_hand_close_qpos approach_trajectory = self._assemble_segment( state.last_qpos, placing_approach_traj, diff --git a/embodichain/lab/sim/atomic_skills/primitives/hand_over.py b/embodichain/lab/sim/atomic_skills/primitives/hand_over.py index c868348965..3ba75931b5 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_skills/primitives/hand_over.py @@ -740,6 +740,7 @@ def _plan_existing_hold( qpos=source_start_qpos, name=resources.first.arm.control_part, to_matrix=True, + env_ids=context.env_ids, ) current_object_pose = torch.bmm(source_eef, pose_inv(source_object_to_eef)) @@ -781,6 +782,7 @@ def _plan_existing_hold( qpos=destination_start_qpos, name=resources.second.arm.control_part, to_matrix=True, + env_ids=context.env_ids, ) # Approach diagonally from the receiver's side of the embodiment. A # top-down receiver places two bulky parallel grippers in the same @@ -1268,12 +1270,14 @@ def _plan_direction( qpos=handover_start_qpos, name=handover.arm.control_part, to_matrix=True, + env_ids=context.env_ids, ) receive_start_qpos = start_qpos[:, list(receive.arm.joint_ids)] receive_start_eef = self.robot.compute_fk( qpos=receive_start_qpos, name=receive.arm.control_part, to_matrix=True, + env_ids=context.env_ids, ) vertical_mode = ( @@ -1967,6 +1971,7 @@ def _report_phase_failure( pose=target_poses[:, waypoint_index], name=arm.control_part, joint_seed=joint_seed, + env_ids=context.env_ids, ) ik_success = normalize_success_mask( ik_success, diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 3213f9eb22..ce65f7fb95 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -223,7 +223,10 @@ class PickUpOptions(SkillOptions): """ cartesian_lift: bool = False - """Preserve a straight world-Z lift with IK interpolation and native limits.""" + """Preserve a straight world-Z lift with IK interpolation and native limits. + + Mutually exclusive with explicit ``MotionPolicy.phase_paths``. + """ initial_lift_height: float = field(default=0.0, kw_only=True) """Initial world-Z clearance (m) from the current TCP pose before approach.""" @@ -382,6 +385,8 @@ def _get_full_pickup_trajectory( hand_grasp_qpos: torch.Tensor, interpolation_dt: float, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: + if options.cartesian_lift and motion_policy.phase_paths: + raise ValueError("cartesian_lift and phase_paths are mutually exclusive.") if options.approach_along_grasp_axis: approach_direction = grasp_xpos[:, :3, 2] pre_grasp_xpos = translate_pose_world( @@ -411,6 +416,7 @@ def _get_full_pickup_trajectory( # planner options were supplied. motion_options.sample_count = None waypoints = [pre_grasp_xpos, grasp_xpos, lift_xpos] + phase_names = ["pre_grasp", "grasp", "lift"] if options.initial_lift_height > 0.0: start_xpos = self.robot.compute_fk( qpos=start_arm_qpos, @@ -427,6 +433,12 @@ def _get_full_pickup_trajectory( start_xpos.new_tensor([0.0, 0.0, options.initial_lift_height]), ) waypoints.insert(0, initial_lift) + phase_names.insert(0, "initial_lift") + + if any(name in motion_policy.phase_paths for name in phase_names): + motion_options.waypoint_modes = tuple( + motion_policy.phase_paths.get(name, "joint") for name in phase_names + ) if options.cartesian_lift: if motion_policy.strategy != "ik_interp": diff --git a/embodichain/lab/sim/atomic_skills/primitives/place.py b/embodichain/lab/sim/atomic_skills/primitives/place.py index d65249429f..26f1e0836b 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/place.py +++ b/embodichain/lab/sim/atomic_skills/primitives/place.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import dataclass, field +import math from typing import ClassVar, Literal import torch @@ -138,6 +139,14 @@ class PlaceOptions(SkillOptions): release_settle_steps: int = 0 """Fully open hold frames before retracting the end-effector.""" + release_settle_duration: float = field(default=0.0, kw_only=True) + """Minimum physical seconds to hold open in a newly planned native Place. + + Rounded upward to control intervals and combined with release_settle_steps + using the larger hold. This is actuation time, not an acceptance tolerance. + External nominal sources retain their authored release timing unchanged. + """ + lift_height: float = 0.1 """Height (m) to retract the end-effector after opening the gripper.""" @@ -148,12 +157,22 @@ class PlaceOptions(SkillOptions): """Number of fixed-orientation Cartesian keyframes per translation segment.""" cartesian_approach: bool = field(default=False, kw_only=True) - """Follow the approach with sampled Cartesian poses under joint motion limits.""" + """Follow the approach with sampled Cartesian poses under joint motion limits. + + Mutually exclusive with explicit ``MotionPolicy.phase_paths``. + """ preserve_current_object_orientation: bool = False """Keep the held object's observed world orientation at the place target.""" def __post_init__(self) -> None: + if ( + isinstance(self.release_settle_duration, bool) + or not isinstance(self.release_settle_duration, (int, float)) + or not math.isfinite(self.release_settle_duration) + or self.release_settle_duration < 0 + ): + raise ValueError("release_settle_duration must be finite and nonnegative.") if type(self.cartesian_approach) is not bool: raise TypeError("cartesian_approach must be boolean.") if self.hand_interp_steps < 1: @@ -315,6 +334,10 @@ def _plan( # planner options were supplied. motion_options.sample_count = None if options.cartesian_approach: + if request.motion_policy.phase_paths: + raise ValueError( + "cartesian_approach and phase_paths are mutually exclusive." + ) if request.motion_policy.strategy != "ik_interp": raise ValueError("cartesian_approach requires strategy='ik_interp'.") from embodichain.lab.sim.motion.ik import sample_cartesian_targets @@ -400,6 +423,15 @@ def _plan( back_arm = down_arm[:, -1:].expand(-1, n_back, -1).clone() motion_success = down_success & back_success else: + paths = request.motion_policy.phase_paths + if any(name in paths for name in ("pre_place", "place", "retreat")): + motion_options.waypoint_modes = ( + *(paths.get("pre_place", "joint"),) + * options.cartesian_waypoint_count, + *(paths.get("place", "joint"),) + * (down_xpos.shape[1] - options.cartesian_waypoint_count), + *(paths.get("retreat", "joint"),) * back_xpos.shape[1], + ) motion_result = self.motion_generator.generate( build_pose_plan_states(torch.cat([down_xpos, back_xpos], dim=1)), options=motion_options, @@ -443,7 +475,10 @@ def _plan( hold_hand_qpos, hand_open_qpos, n_waypoints=n_open ) - n_settle = options.release_settle_steps + n_settle = max( + options.release_settle_steps, + math.ceil(options.release_settle_duration / context.require_control_dt()), + ) open_start = n_down settle_start = open_start + n_open back_start = settle_start + n_settle diff --git a/embodichain/lab/sim/atomic_skills/runtime_commands.py b/embodichain/lab/sim/atomic_skills/runtime_commands.py index 606438a584..2a05935f3f 100644 --- a/embodichain/lab/sim/atomic_skills/runtime_commands.py +++ b/embodichain/lab/sim/atomic_skills/runtime_commands.py @@ -70,6 +70,18 @@ def _snapshot_target(target: RuntimeEndpointTarget) -> RuntimeEndpointTarget: return snapshot +def _unique_env_ids(env_ids: torch.Tensor) -> bool: + """Avoid a data-dependent kernel for a provably unique dense CPU singleton.""" + if ( + type(env_ids) is torch.Tensor + and env_ids.device.type == "cpu" + and env_ids.layout == torch.strided + and env_ids.numel() == 1 + ): + return True + return torch.unique(env_ids).numel() == env_ids.numel() + + class RuntimeCommandPayload(ABC): """Immutable-by-ownership payload submitted to one runtime transport.""" @@ -260,6 +272,14 @@ class RuntimeCommandFrame: hold_duration: torch.Tensor def __post_init__(self) -> None: + from ._command_batches import validate_frame + + if validate_frame(self): + return + RuntimeCommandFrame._initialize(self, owned_commands=False) + + def _initialize(self, *, owned_commands: bool) -> None: + """Validate every frame field, optionally transferring private commands.""" if isinstance(self.commands, (str, bytes)): raise TypeError("commands must be an iterable of EndpointCommand values.") try: @@ -280,7 +300,7 @@ def __post_init__(self) -> None: ): raise ValueError("env_ids must be int64 with shape (batch_size,).") batch_size = int(self.env_ids.shape[0]) - if torch.unique(self.env_ids).numel() != batch_size: + if not _unique_env_ids(self.env_ids): raise ValueError("env_ids must be unique.") if not isinstance(self.active_mask, torch.Tensor): raise TypeError("active_mask must be a torch.Tensor.") @@ -302,7 +322,11 @@ def __post_init__(self) -> None: if self.hold_duration.device != self.env_ids.device: raise ValueError("hold_duration and env_ids must share a device.") - snapshots = tuple(command.snapshot() for command in commands) + snapshots = ( + commands + if owned_commands + else tuple(command.snapshot() for command in commands) + ) destinations: set[tuple[str, str]] = set() joint_owners: dict[int, tuple[str, str]] = {} for command in snapshots: @@ -410,6 +434,10 @@ class TimedCommandSequence: env_ids: torch.Tensor def __post_init__(self) -> None: + from ._command_batches import validate_sequence + + if validate_sequence(self): + return if not isinstance(self.env_ids, torch.Tensor): raise TypeError("env_ids must be a torch.Tensor.") if ( @@ -418,7 +446,7 @@ def __post_init__(self) -> None: or self.env_ids.shape[0] < 1 ): raise ValueError("env_ids must be int64 with shape (batch_size,).") - if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + if not _unique_env_ids(self.env_ids): raise ValueError("env_ids must be unique.") if isinstance(self.frames, (str, bytes)): raise TypeError("frames must be an iterable of RuntimeCommandFrame values.") @@ -473,6 +501,93 @@ def snapshot(self) -> TimedCommandSequence: return TimedCommandSequence(frames=self.frames, env_ids=self.env_ids) +def _canonical_cpu_command(command: EndpointCommand) -> bool: + """Identify standard commands without user-defined snapshot callbacks.""" + return ( + type(command) is EndpointCommand + and type(command.target) is JointPositionTarget + and type(command.target.control_part) is str + and type(command.target.joint_ids) is tuple + and all(type(joint) is int for joint in command.target.joint_ids) + and not getattr(command.target, "__dict__", {}) + and type(command.payload) is JointPositionPayload + and type(command.payload.positions) is torch.Tensor + and command.payload.positions.device.type == "cpu" + and ( + command.payload.velocities is None + or type(command.payload.velocities) is torch.Tensor + ) + ) + + +def _frame_from_fresh_commands( + commands: list[EndpointCommand], + *, + active_mask: torch.Tensor, + env_ids: torch.Tensor, + hold_duration: torch.Tensor, +) -> RuntimeCommandFrame: + """Transfer unexposed engine-built commands while retaining frame checks. + + Only fresh commands from the two canonical engine builders may enter this + path. Extended targets, payloads and tensor types use public construction. + Public frame constructors and snapshots never transfer caller ownership. + """ + fields = dict( + commands=tuple(commands), + active_mask=active_mask, + env_ids=env_ids, + hold_duration=hold_duration, + ) + if any( + type(v) is not torch.Tensor or v.device.type != "cpu" + for v in (active_mask, env_ids, hold_duration) + ) or not all(_canonical_cpu_command(c) for c in commands): + return RuntimeCommandFrame(**fields) + frame = object.__new__(RuntimeCommandFrame) + for name, value in fields.items(): + object.__setattr__(frame, name, value) + RuntimeCommandFrame._initialize(frame, owned_commands=True) + commands.clear() + return frame + + +def _sequence_from_fresh_frames( + frames: list[RuntimeCommandFrame], env_ids: torch.Tensor +) -> TimedCommandSequence: + """Transfer private, freshly validated canonical frames into a sequence. + + Only engine builders may pass their newly constructed, unexposed CPU frames. + CUDA retains packed sequence validation rather than synchronizing each ID check. + Public constructors and snapshots always retain defensive revalidation and + copying. Custom targets/payloads retain that path as their callbacks may + carry aliases or side effects. The input list is consumed on transfer. + """ + if ( + type(env_ids) is not torch.Tensor + or env_ids.device.type != "cpu" + or any( + type(frame) is not RuntimeCommandFrame + or any( + type(value) is not torch.Tensor + for value in (frame.active_mask, frame.env_ids, frame.hold_duration) + ) + or not all(_canonical_cpu_command(command) for command in frame.commands) + for frame in frames + ) + ): + return TimedCommandSequence(frames=tuple(frames), env_ids=env_ids) + sequence = TimedCommandSequence(frames=(), env_ids=env_ids) + for index, frame in enumerate(frames): + if frame.device != sequence.device: + raise ValueError(f"Frame {index} must share the sequence device.") + if not torch.equal(frame.env_ids, sequence.env_ids): + raise ValueError(f"Frame {index} env_ids do not match the sequence.") + object.__setattr__(sequence, "frames", tuple(frames)) + frames.clear() + return sequence + + __all__ = [ "EndpointCommand", "JointPositionPayload", diff --git a/embodichain/lab/sim/motion/_phase_paths.py b/embodichain/lab/sim/motion/_phase_paths.py new file mode 100644 index 0000000000..0176d93024 --- /dev/null +++ b/embodichain/lab/sim/motion/_phase_paths.py @@ -0,0 +1,111 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Explicit waypoint phase paths composed through the ordinary motion facade.""" + +from __future__ import annotations + +from dataclasses import replace +from itertools import groupby +from typing import TYPE_CHECKING +import torch +from .planners.utils import MoveType, PlanResult, RejectedMotionCandidate + +if TYPE_CHECKING: + from .motion_generator import MotionGenerator, MotionGenOptions + from .planners.utils import PlanState + +__all__: list[str] = [] + + +def generate_phase_paths( + generator: MotionGenerator, + targets: list[PlanState], + options: MotionGenOptions, + *, + defer_recovery: bool, +) -> PlanResult: + """Retain one declared grid while grouping consecutive path modes.""" + modes = options.waypoint_modes + if len(modes) != len(targets) or not all( + t.move_type is MoveType.EEF_MOVE for t in targets + ): + raise ValueError( + f"waypoint_modes must describe every Cartesian waypoint: modes={modes}, target_types={[state.move_type for state in targets]}" + ) + if ( + options.start_qpos is None + or options.sample_count is None + or options.sample_count <= len(targets) + ): + raise ValueError( + "phase paths require a start and at least one interval per waypoint" + ) + if type(defer_recovery) is not bool or ( + defer_recovery and "cartesian" not in modes + ): + raise ValueError("deferred phase recovery requires a Cartesian phase") + pieces, start, offset = [], options.start_qpos, 0 + for mode, group in groupby(modes): + stop = offset + len(tuple(group)) + intervals = (options.sample_count - 1) * stop // len(targets) - ( + options.sample_count - 1 + ) * offset // len(targets) + result = generator.generate( + targets[offset:stop], + options=replace( + options, + waypoint_modes=(), + start_qpos=start, + sample_count=intervals + 1, + is_linear=mode == "cartesian", + ), + defer_recovery=defer_recovery and mode == "cartesian", + ) + if result.positions is None or result.dt is None: + raise ValueError("phase path returned no timed trajectory") + pieces.append(result) + start = result.positions[:, -1] + offset = stop + success = torch.stack([piece.success for piece in pieces]).all(dim=0) + positions = torch.cat( + [p.positions if i == 0 else p.positions[:, 1:] for i, p in enumerate(pieces)], + dim=1, + ) + dt = torch.cat( + [p.dt if i == 0 else p.dt[:, 1:] for i, p in enumerate(pieces)], dim=1 + ) + rejected = None + if any(p.rejected_candidate is not None for p in pieces): + mask = torch.zeros_like(success) + candidates = [] + for i, p in enumerate(pieces): + points = p.positions + if p.rejected_candidate is not None: + mask |= p.rejected_candidate.env_mask + points = p.rejected_candidate.positions + candidates.append(points if i == 0 else points[:, 1:]) + rejected = RejectedMotionCandidate(torch.cat(candidates, dim=1), dt, mask) + combined = PlanResult( + success=success, + positions=torch.where( + success[:, None, None], positions, options.start_qpos[:, None] + ), + dt=dt, + rejected_candidate=rejected, + recovery=pieces[-1].recovery, + ) + return generator._normalize_plan_result(combined, targets, options) diff --git a/embodichain/lab/sim/motion/expansion/_samples.py b/embodichain/lab/sim/motion/expansion/_samples.py new file mode 100644 index 0000000000..7011c2c35f --- /dev/null +++ b/embodichain/lab/sim/motion/expansion/_samples.py @@ -0,0 +1,43 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Validated, explicit spatial draws for reproducible boundary experiments.""" + +from __future__ import annotations +from collections.abc import Mapping +import math + +__all__: list[str] = [] + + +def validate_samples( + joints: object, knots: object, via_count: int +) -> tuple[dict[str, float], tuple[float, ...]]: + if not isinstance(joints, Mapping) or any( + not isinstance(k, str) or not k for k in joints + ): + raise ValueError("joint_samples must map joint names to signed fractions") + if not isinstance(knots, (tuple, list)): + raise ValueError("knot_samples must be a sequence") + values = tuple(joints.values()) + tuple(knots) + if any( + type(v) not in (int, float) or not math.isfinite(v) or not -1 <= v <= 1 + for v in values + ): + raise ValueError("explicit spatial samples must be finite and in [-1,1]") + if knots and len(knots) != via_count: + raise ValueError("knot_samples must match via_count") + return {k: float(v) for k, v in joints.items()}, tuple(float(v) for v in knots) diff --git a/embodichain/lab/sim/motion/expansion/cfg.py b/embodichain/lab/sim/motion/expansion/cfg.py index 1a56e362fe..6f4f74598b 100644 --- a/embodichain/lab/sim/motion/expansion/cfg.py +++ b/embodichain/lab/sim/motion/expansion/cfg.py @@ -93,8 +93,16 @@ class _SpatialCfg: method: tuple[str, ...] = ("joint_residual",) joint_offset_scale: float = 0.05 via_count: int = 1 + joint_samples: dict[str, float] = {} + knot_samples: tuple[float, ...] = () def __post_init__(self) -> None: + from ._samples import validate_samples + + self.joint_samples, self.knot_samples = validate_samples( + self.joint_samples, self.knot_samples, self.via_count + ) + _boolean(self.enabled, "spatial.enabled") methods = (self.method,) if isinstance(self.method, str) else tuple(self.method) if not methods: diff --git a/embodichain/lab/sim/motion/expansion/combined.py b/embodichain/lab/sim/motion/expansion/combined.py index 0acae6e3b6..5f526a97a2 100644 --- a/embodichain/lab/sim/motion/expansion/combined.py +++ b/embodichain/lab/sim/motion/expansion/combined.py @@ -236,8 +236,16 @@ class _SpatialCfg: joint_offset_scale: float = 0.05 dedup_tolerance: float | None = None via_count: int = 1 + joint_samples: dict[str, float] = {} + knot_samples: tuple[float, ...] = () def __post_init__(self) -> None: + from ._samples import validate_samples + + self.joint_samples, self.knot_samples = validate_samples( + self.joint_samples, self.knot_samples, self.via_count + ) + self.operators = _strings(self.operators, "trajectory.spatial.operators") if any(item not in ("joint_residual", "via_points") for item in self.operators): raise ValueError( @@ -520,6 +528,8 @@ def from_mapping(cls, data: Mapping[str, Any]) -> "CombinedExpansionProfile": "enabled", "operators", "joint_offset_scale", + "joint_samples", + "knot_samples", "dedup_tolerance", "via_count", }, diff --git a/embodichain/lab/sim/motion/expansion/operators.py b/embodichain/lab/sim/motion/expansion/operators.py index f87bceed64..5f9bd20d62 100644 --- a/embodichain/lab/sim/motion/expansion/operators.py +++ b/embodichain/lab/sim/motion/expansion/operators.py @@ -17,6 +17,7 @@ """Grasp-pose and constrained qpos operators requiring downstream validation.""" from __future__ import annotations +from collections.abc import Mapping, Sequence from dataclasses import replace import math @@ -312,6 +313,7 @@ def joint_residual( joint_limits: torch.Tensor, normalized_scale: float, generator: torch.Generator, + joint_samples: Mapping[str, float] | None = None, ) -> TrajectoryTemplate: """Add one smooth, endpoint-preserving residual per allowed free phase. @@ -324,6 +326,10 @@ def joint_residual( joint_limits: Finite lower/upper limits with shape ``(D, 2)``. normalized_scale: Maximum offset as a fraction of each joint range. generator: Explicit local generator; global RNG is never consumed. + joint_samples: Optional signed fractions in [-1,1], keyed by all + controlled joint names. Extra names must belong to the full template. + Uses the same draw in each permitted phase; endpoints and validation + remain unchanged. Omit to retain ordinary random sampling. Returns: A new template requiring path and physical validation. @@ -336,6 +342,14 @@ def joint_residual( q = template.positions.clone() limits = _resolve_joint_limits(q, joint_limits) indices = list(template.controlled_joint_indices) + from ._samples import validate_samples + + samples, _ = validate_samples({} if joint_samples is None else joint_samples, (), 1) + if samples and ( + set(samples) - set(template.joint_names) + or any(template.joint_names[i] not in samples for i in indices) + ): + raise ValueError("joint_samples must cover the controlled joint names") for phase in phases: length = phase.stop_index - phase.start_index if length < 3: @@ -344,7 +358,16 @@ def joint_residual( values = torch.rand( len(indices), generator=generator, device=generator.device, dtype=q.dtype ).to(q.device) - offsets = (2 * values - 1) * scale * (limits[indices, 1] - limits[indices, 0]) + direction = ( + torch.tensor( + [samples[template.joint_names[i]] for i in indices], + device=q.device, + dtype=q.dtype, + ) + if samples + else 2 * values - 1 + ) + offsets = direction * scale * (limits[indices, 1] - limits[indices, 0]) q[phase.start_index : phase.stop_index, indices] += envelope[:, None] * offsets _reject_limit_violations(q, limits, indices) return replace(template, positions=q) @@ -357,6 +380,8 @@ def via_points( via_count: int, normalized_scale: float, generator: torch.Generator, + joint_samples: Mapping[str, float] | None = None, + knot_samples: Sequence[float] = (), ) -> TrajectoryTemplate: """Route each allowed free phase through sampled interior joint knots. @@ -383,6 +408,11 @@ def via_points( via_count: Number of interior knots per phase, at least one. normalized_scale: Maximum knot offset as a fraction of each joint range. generator: Explicit local generator; global RNG is never consumed. + knot_samples: Optional signed knot magnitudes; length must match via_count. + joint_samples: Optional signed fractions in [-1,1], keyed by all + controlled joint names. Extra names must belong to the full template. + Uses the same draw in each permitted phase; endpoints and validation + remain unchanged. Omit to retain ordinary random sampling. Returns: A new template requiring path and physical validation. @@ -398,6 +428,16 @@ def via_points( limits = _resolve_joint_limits(q, joint_limits) indices = list(template.controlled_joint_indices) spans = limits[indices, 1] - limits[indices, 0] + from ._samples import validate_samples + + samples, knots_fixed = validate_samples( + {} if joint_samples is None else joint_samples, knot_samples, via_count + ) + if samples and ( + set(samples) - set(template.joint_names) + or any(template.joint_names[i] not in samples for i in indices) + ): + raise ValueError("joint_samples must cover the controlled joint names") for phase in phases: length = phase.stop_index - phase.start_index if length < via_count + 2: @@ -408,8 +448,20 @@ def via_points( device=generator.device, dtype=q.dtype, ).to(q.device) - magnitudes = 2 * values[:via_count] - 1 - direction = 2 * values[via_count:] - 1 + magnitudes = ( + torch.tensor(knots_fixed, device=q.device, dtype=q.dtype) + if knots_fixed + else 2 * values[:via_count] - 1 + ) + direction = ( + torch.tensor( + [samples[template.joint_names[i]] for i in indices], + device=q.device, + dtype=q.dtype, + ) + if samples + else 2 * values[via_count:] - 1 + ) knots = magnitudes[:, None] * (direction * scale * spans)[None, :] progress = torch.linspace(0, 1, length, device=q.device, dtype=q.dtype) q[phase.start_index : phase.stop_index, indices] += _clamped_hermite( diff --git a/embodichain/lab/sim/motion/expansion/variants.py b/embodichain/lab/sim/motion/expansion/variants.py index 475c8790f8..949ee76ef4 100644 --- a/embodichain/lab/sim/motion/expansion/variants.py +++ b/embodichain/lab/sim/motion/expansion/variants.py @@ -556,6 +556,7 @@ def _apply_variant_once( joint_limits=joint_limits, normalized_scale=spatial.joint_offset_scale, generator=generator, + joint_samples=spatial.joint_samples, ) elif variant.spatial_operator == "via_points": result = via_points( @@ -564,6 +565,8 @@ def _apply_variant_once( via_count=spatial.via_count, normalized_scale=spatial.joint_offset_scale, generator=generator, + joint_samples=spatial.joint_samples, + knot_samples=spatial.knot_samples, ) elif variant.spatial_operator == "nullspace_residual": if target is not None: diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index 88bae56c6a..23fdb63547 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -207,8 +207,32 @@ class MotionGenOptions: retains its own Torch-only fork; no outer action counter is reused. """ + waypoint_modes: tuple[Literal["joint", "cartesian"], ...] = field( + default=(), kw_only=True + ) + """Explicit path mode from the preceding pose to each supplied waypoint. + + Supported for fixed-grid sequential ``ik_interp``. Empty retains ordinary + planning. Consecutive equal modes share a native request; the total sample + count and interval remain unchanged. No robot or target-name inference occurs. + """ + def __post_init__(self) -> None: """Validate backend-neutral motion generation options.""" + if type(self.waypoint_modes) is not tuple or any( + mode not in ("joint", "cartesian") for mode in self.waypoint_modes + ): + raise ValueError("waypoint_modes must be a tuple of joint/cartesian modes") + if self.waypoint_modes and ( + self.strategy != "ik_interp" + or self.interpolation_mode != "fixed_grid" + or self.ik_seed_mode != "sequential" + or self.is_linear + or self.preserve_cartesian_samples + ): + raise ValueError( + "waypoint_modes requires unsampled fixed-grid sequential ik_interp" + ) segment_retries = self.segment_sample_count_retries if ( type(segment_retries) is not tuple @@ -761,11 +785,24 @@ def generate( Raises: ValueError: If targets or options violate the motion contract. """ + # The optional expansion scope collects requests on the World owner. + # It is inert for ordinary callers and preserves native budget scopes. + from embodichain.lab.expansion._planning_batches import _try_motion + + handled, result = _try_motion(self, target_states, options, defer_recovery) + if handled: + return result if not target_states: raise ValueError("target_states must contain at least one waypoint.") options = MotionGenOptions() if options is None else options # Options are mutable config objects; revalidate at the call boundary. options.__post_init__() + if options.waypoint_modes: + from ._phase_paths import generate_phase_paths + + return generate_phase_paths( + self, target_states, options, defer_recovery=defer_recovery + ) if type(defer_recovery) is not bool: raise TypeError("defer_recovery must be a boolean.") linear_keyframes = ( @@ -1233,6 +1270,17 @@ def _solve_seeded_cartesian_waypoints( or poses.shape[1] == 0 ): raise ValueError("Cartesian waypoints must have shape (B, N, 4, 4), N > 0.") + solver = self.robot.get_solver(control_part) + from ._budget import _ACTIVE_BUDGET + + if ( + solver is not None + and getattr(solver, "supports_sequential_batch_ik", False) + and _ACTIVE_BUDGET.get() is None + ): + # The solver owns the sequential dependency inside its bounded + # path hook. Do not re-enter Python/Robot once per Cartesian node. + return self._solve_cartesian_waypoints(poses, seed, control_part) success = torch.ones(batch, dtype=torch.bool, device=seed.device) points = [] for index in range(poses.shape[1]): diff --git a/embodichain/lab/sim/motion/planners/base_planner.py b/embodichain/lab/sim/motion/planners/base_planner.py index 99ff6a1d79..a0daf18f50 100644 --- a/embodichain/lab/sim/motion/planners/base_planner.py +++ b/embodichain/lab/sim/motion/planners/base_planner.py @@ -184,8 +184,10 @@ def wrapper(self, *args, **kwargs): target_states = kwargs.get("target_states", args[0] if args else None) if target_states is not None and hasattr(self, "robot"): robot_num = getattr(self.robot, "num_instances", None) + selected = getattr(self, "_planning_env_ids", None) + expected = len(selected) if selected is not None else robot_num _check_batch_consistency( - target_states, expected_b=robot_num, robot_num_instances=robot_num + target_states, expected_b=expected, robot_num_instances=expected ) return func(self, *args, **kwargs) diff --git a/embodichain/lab/sim/motion/solvers/base_solver.py b/embodichain/lab/sim/motion/solvers/base_solver.py index d80e0979cc..4b5cd78402 100644 --- a/embodichain/lab/sim/motion/solvers/base_solver.py +++ b/embodichain/lab/sim/motion/solvers/base_solver.py @@ -531,6 +531,16 @@ def get_tcp(self) -> np.ndarray: """ return self.tcp_xpos + @property + def supports_sequential_batch_ik(self) -> bool: + """Whether continuous batching uses each preceding point-query seed. + + This stronger contract also preserves seed-dependent candidate search. + Merely supporting continuous selection from a fixed candidate pool is + insufficient. Solvers opt in only when their path hook implements it. + """ + return False + @property def supports_continuous_batch_ik(self) -> bool: """Whether all-candidate IK and temporally continuous selection are supported. @@ -697,9 +707,15 @@ def get_jacobian( # Ensure locations is a tensor if provided if locations is not None: + from pytorch_kinematics import Transform3d + locations = torch.as_tensor( locations, dtype=torch.float32, device=self.device ) + # PK 0.10 constructs a CPU Transform3d for raw offset tensors. + # Supply its declared device instead of mixing CPU homogeneous + # padding with CUDA offsets inside the dependency. + locations = Transform3d(pos=locations, dtype=qpos.dtype, device=qpos.device) # Compute the Jacobian using the kinematics chain J = self.pk_serial_chain.jacobian(th=qpos, locations=locations) diff --git a/embodichain/lab/sim/motion/solvers/opw_solver.py b/embodichain/lab/sim/motion/solvers/opw_solver.py index 7d06e3e561..53e38a1ae1 100644 --- a/embodichain/lab/sim/motion/solvers/opw_solver.py +++ b/embodichain/lab/sim/motion/solvers/opw_solver.py @@ -186,7 +186,8 @@ def get_fk_warp(self, qpos: torch.tensor, **kwargs) -> torch.tensor: qpos_batch = qpos n_sample = qpos_batch.shape[0] # Retimed trajectories retain float64; the OPW kernel consumes float32. - qpos_wp = wp.from_torch(qpos_batch.to(dtype=torch.float32).reshape(-1)) + qpos_batch = qpos_batch.to(device=self.device, dtype=torch.float32).contiguous() + qpos_wp = wp.from_torch(qpos_batch.reshape(-1)) xpos_wp = wp.zeros( n_sample * 16, dtype=float, device=standardize_device_string(self.device) ) diff --git a/embodichain/lab/sim/motion/solvers/ssik_solver.py b/embodichain/lab/sim/motion/solvers/ssik_solver.py index c8befce863..8eeb36445e 100644 --- a/embodichain/lab/sim/motion/solvers/ssik_solver.py +++ b/embodichain/lab/sim/motion/solvers/ssik_solver.py @@ -352,6 +352,15 @@ def prepare_buffers(self, max_batch: int) -> None: if provider is not None: provider.prepare_buffers(capacity) + @property + def supports_sequential_batch_ik(self) -> bool: + """Whether the path hook enumerates the ordinary preceding-seed pool.""" + return ( + self.cfg.backend == "warp" + and self.family == "srs" + and self.cfg.srs_search == "seed_local" + ) + @property def supports_continuous_batch_ik(self) -> bool: """Whether candidate generation and previous-frame selection are supported.""" diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index e6c937aa41..4fac890ac8 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -2054,8 +2054,8 @@ def set_state( ) if ( (root_pose is not None or qpos is not None) - and self.device.type == "cpu" and not self._data.is_newton_backend + and not self._world.get_physics_scene().is_direct_gpu() ): # Direct root/joint writes update native kinematics, but CPU link # transforms are published by World.update. Do not advance physics diff --git a/embodichain/lab/sim/objects/backends/_numeric_observations.py b/embodichain/lab/sim/objects/backends/_numeric_observations.py new file mode 100644 index 0000000000..2c9e3ab47c --- /dev/null +++ b/embodichain/lab/sim/objects/backends/_numeric_observations.py @@ -0,0 +1,165 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Bounded shared native reads during a single World-owner planning boundary. + +Only Default CPU full-batch numeric reads are shared. Each consumer still +receives its own output buffer. Checked native writes invalidate all samples; +physics/topology revisions and scope exit prevent reuse across boundaries. +Raw SDK mutations and concurrent World writers are outside this internal +read-boundary contract. +""" + +from __future__ import annotations + +from collections.abc import Callable +from contextlib import contextmanager +from contextvars import ContextVar +from threading import get_ident +from typing import Any, Iterator, TYPE_CHECKING +import torch + +if TYPE_CHECKING: + from embodichain.lab.sim.sim_manager import SimulationManager + from embodichain.lab.sim.objects.robot import Robot + +__all__: list[str] = [] +_READS = frozenset( + ( + "fetch_pose", + "fetch_root_pose", + "fetch_link_pose", + "fetch_joint_position", + "fetch_joint_velocity", + "fetch_joint_force", + "fetch_linear_velocity", + "fetch_angular_velocity", + "fetch_root_linear_velocity", + "fetch_root_angular_velocity", + ) +) +_ACTIVE: ContextVar[_NumericReads | None] = ContextVar( + "native_numeric_reads", default=None +) + + +class _NumericReads: + def __init__( + self, batches: tuple[object, ...], revision: Callable[[], object] + ) -> None: + self.batches = {id(batch): batch for batch in batches} + self.revision: Callable[[], object] | None = revision + self.stamp = revision() + self.owner = get_ident() + self.closed = False + self.values: dict[tuple, tuple[torch.Tensor, Any]] = {} + + def refresh(self) -> bool: + """Check the owner/revision before accessing protected samples.""" + if self.closed or self.owner != get_ident() or self.revision is None: + return False + stamp = self.revision() + if stamp != self.stamp: + self.values.clear() + self.stamp = stamp + return True + + def key( + self, batch: object, method: str, args: tuple, kwargs: dict + ) -> tuple | None: + if not self.refresh(): + return None + if not method.startswith("fetch_"): + # Invalidate before a write, including partially failing writes. + self.values.clear() + return None + if ( + method not in _READS + or id(batch) not in self.batches + or len(args) != 1 + or kwargs + ): + return None + out = args[0] + if ( + type(out) is not torch.Tensor + or out.device.type != "cpu" + or out.requires_grad + or not out.is_contiguous() + ): + return None + return (id(batch), method, out.shape, out.dtype) + + +def shared_pose( + batch: object, to_matrix: bool, read: Callable[[], torch.Tensor] +) -> torch.Tensor: + """Own one derived pose representation and clone it for every consumer.""" + reads = _ACTIVE.get() + if reads is None or not reads.refresh() or id(batch) not in reads.batches: + return read() + key = ( + id(batch), + "derived_pose", + to_matrix, + torch.is_autocast_enabled("cpu"), + torch.get_autocast_dtype("cpu"), + ) + cached = reads.values.get(key) + if cached is not None: + return cached[0].clone() + value = read() + if ( + type(value) is torch.Tensor + and value.device.type == "cpu" + and not value.requires_grad + ): + reads.values[key] = (value.detach().clone(), None) + return value + + +@contextmanager +def numeric_observation_scope(sim: SimulationManager, robot: Robot) -> Iterator[None]: + """Share numeric samples only while this owner is gathering row commands.""" + if sim.physics_backend != "default" or sim.device.type != "cpu": + yield + return + batches = [robot._data.articulation_view.batch] + for uid in sim.get_rigid_object_uid_list(): + data = sim.get_rigid_object(uid)._data + if data is not None: + batches.append(data.body_view.batch) + reads = _NumericReads( + tuple(batches), + lambda: ( + sim.physics_step_index, + id(robot._data.articulation_view.scene), + robot._data.articulation_view.scene.topology_revision, + ), + ) + previous = _ACTIVE.get() + if previous is not None: + previous.values.clear() + token = _ACTIVE.set(reads) + try: + yield + finally: + reads.values.clear() + reads.closed = True + reads.batches.clear() + reads.revision = None + _ACTIVE.reset(token) + if previous is not None: + previous.values.clear() diff --git a/embodichain/lab/sim/objects/backends/scene.py b/embodichain/lab/sim/objects/backends/scene.py index 50c0e704fd..6332a315fd 100644 --- a/embodichain/lab/sim/objects/backends/scene.py +++ b/embodichain/lab/sim/objects/backends/scene.py @@ -34,6 +34,7 @@ import torch from .base import ArticulationViewBase, RigidBodyViewBase +from ._numeric_observations import _ACTIVE if TYPE_CHECKING: from dexsim.scene import ( @@ -57,12 +58,20 @@ def _checked_batch_call( **kwargs: Any, ) -> Any: """Call one Scene batch operation and reject native failure statuses.""" + reads = _ACTIVE.get() + key = None if reads is None else reads.key(batch, method_name, args, kwargs) + if key is not None and key in reads.values: + saved, status = reads.values[key] + args[0].copy_(saved) + return status status = getattr(batch, method_name)(*args, **kwargs) if isinstance(status, Integral) and status < 0: raise RuntimeError( f"DexSim Scene batch operation {method_name!r} failed with " f"status {status}." ) + if key is not None: + reads.values[key] = (args[0].detach().clone(), status) return status diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index e4aa4c028e..b4f5cdc1b1 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -36,6 +36,10 @@ is_newton_scene, ) from embodichain.lab.sim.objects.backends.base import RigidBodyViewBase +from embodichain.lab.sim.objects.backends._numeric_observations import ( + _ACTIVE as _NUMERIC_READS, + shared_pose, +) from embodichain.lab.sim.objects.backends.rigid_physics import ( _set_default_spawn_friction, apply_legacy_physical_attr, @@ -911,6 +915,21 @@ def get_local_pose_cpu( if self.is_static: return get_local_pose_cpu(self._entities, to_matrix).to(self.device) + if ( + type(self) is RigidObject + and type(to_matrix) is bool + and self._data is not None + and _NUMERIC_READS.get() is not None + ): + return shared_pose( + self._data.body_view.batch, + to_matrix, + lambda: RigidObject._read_dynamic_local_pose(self, to_matrix), + ) + return RigidObject._read_dynamic_local_pose(self, to_matrix) + + def _read_dynamic_local_pose(self, to_matrix: bool) -> torch.Tensor: + """Convert the current native pose without sharing caller-owned storage.""" pose = self.body_data.pose.clone() if to_matrix: xyz = pose[:, :3] diff --git a/embodichain/lab/sim/scene/_support_grid.py b/embodichain/lab/sim/scene/_support_grid.py index 96aaeaeb39..97dfd4d0c9 100644 --- a/embodichain/lab/sim/scene/_support_grid.py +++ b/embodichain/lab/sim/scene/_support_grid.py @@ -24,45 +24,12 @@ import numpy as np import shapely -from shapely.geometry import Polygon, MultiPolygon, mapping, shape +from shapely.geometry import Polygon, MultiPolygon, mapping from shapely.ops import unary_union -__all__: list[str] = [] - +from embodichain.lab._scene_config.policy import _read_support_polygon -def _read_support_polygon(value: object) -> Polygon | MultiPolygon: - """Decode finite XY GeoJSON before accessing any scene assets.""" - if not isinstance(value, dict) or value.get("type") not in ( - "Polygon", - "MultiPolygon", - ): - raise ValueError( - "Support polygon must be a GeoJSON Polygon or MultiPolygon mapping." - ) - try: - polygon = shape(value) - except ( - AttributeError, - KeyError, - IndexError, - TypeError, - ValueError, - OverflowError, - shapely.errors.GEOSException, - ) as exc: - raise ValueError("Support polygon has invalid GeoJSON coordinates.") from exc - if ( - not isinstance(polygon, (Polygon, MultiPolygon)) - or polygon.is_empty - or polygon.has_z - or not np.isfinite(shapely.get_coordinates(polygon)).all() - or not polygon.is_valid - or polygon.area <= 0 - ): - raise ValueError( - "Support polygon must contain a valid nonempty finite XY area." - ) - return polygon +__all__: list[str] = [] def _plane_region( diff --git a/embodichain/lab/sim/scene/_validation.py b/embodichain/lab/sim/scene/_validation.py index 52898f649f..a867003193 100644 --- a/embodichain/lab/sim/scene/_validation.py +++ b/embodichain/lab/sim/scene/_validation.py @@ -18,69 +18,18 @@ from __future__ import annotations -import math -from numbers import Real from typing import TYPE_CHECKING +from embodichain.lab._scene_config.values import ( + _identifier, + _nonnegative_integer, + _rigid_pose, +) + if TYPE_CHECKING: import torch -__all__ = [] - - -def _identifier(value: str, name: str) -> str: - if not isinstance(value, str) or not value or value.strip() != value: - raise ValueError(f"{name} must be a nonempty stripped string") - return value - - -def _nonnegative_integer(value: int, name: str) -> int: - if type(value) is not int or value < 0: - raise ValueError(f"{name} must be a nonnegative integer") - return value - - -def _rigid_pose(value: object) -> tuple[tuple[float, ...], ...]: - try: - rows = tuple(tuple(row) for row in value) # type: ignore[union-attr] - except TypeError as exc: - raise ValueError("arena_pose must be a 4x4 rigid transform") from exc - if len(rows) != 4 or any(len(row) != 4 for row in rows): - raise ValueError("arena_pose must be a 4x4 rigid transform") - if any( - isinstance(item, bool) or not isinstance(item, Real) or not math.isfinite(item) - for row in rows - for item in row - ): - raise ValueError("arena_pose must contain finite real numbers") - pose = tuple( - tuple(float(item) if item != 0 else 0.0 for item in row) for row in rows - ) - tolerance = 1e-6 - if any( - abs(pose[3][index] - target) > tolerance - for index, target in enumerate((0, 0, 0, 1)) - ): - raise ValueError("arena_pose must have homogeneous last row [0, 0, 0, 1]") - rotation = tuple(row[:3] for row in pose[:3]) - if any( - abs(sum(rotation[k][i] * rotation[k][j] for k in range(3)) - (i == j)) - > tolerance - for i in range(3) - for j in range(3) - ): - raise ValueError("arena_pose rotation must be orthonormal") - determinant = ( - rotation[0][0] - * (rotation[1][1] * rotation[2][2] - rotation[1][2] * rotation[2][1]) - - rotation[0][1] - * (rotation[1][0] * rotation[2][2] - rotation[1][2] * rotation[2][0]) - + rotation[0][2] - * (rotation[1][0] * rotation[2][1] - rotation[1][1] * rotation[2][0]) - ) - if abs(determinant - 1.0) > tolerance: - raise ValueError("arena_pose rotation must be proper (determinant +1)") - return (*pose[:3], (0.0, 0.0, 0.0, 1.0)) +__all__: list[str] = [] def _owned_batch_pose(value: torch.Tensor, name: str) -> torch.Tensor: diff --git a/embodichain/lab/sim/scene/expansion/__init__.py b/embodichain/lab/sim/scene/expansion/__init__.py index 32758cf991..35b0707b5c 100644 --- a/embodichain/lab/sim/scene/expansion/__init__.py +++ b/embodichain/lab/sim/scene/expansion/__init__.py @@ -27,10 +27,12 @@ from .cfg import SceneExpansionCfg from .contracts import ScenePoseChange, SceneVariant +from .conditions import SceneLayoutCondition from .source import FixedSceneCandidateSource __all__ = [ "SceneExpansionCfg", + "SceneLayoutCondition", "ScenePoseChange", "SceneVariant", "FixedSceneCandidateSource", diff --git a/embodichain/lab/sim/scene/expansion/_policy.py b/embodichain/lab/sim/scene/expansion/_policy.py index 52567481ca..03a4c4ffee 100644 --- a/embodichain/lab/sim/scene/expansion/_policy.py +++ b/embodichain/lab/sim/scene/expansion/_policy.py @@ -13,15 +13,23 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- - -"""Source-independent policies for bounded scene layout sampling.""" +"""Public scene policy configuration imports.""" from __future__ import annotations -from copy import deepcopy -from dataclasses import MISSING -import math -from typing import Any -from embodichain.utils import configclass + +from embodichain.lab._scene_config.policy import ( + IntraGroupRandomizationCfg, + MemberRandomizationCfg, + SceneRandomizationJobCfg, + SceneRandomizationPolicyCfg, + SupportOverrideCfg, + _decode_intra, + _intra_payload, + _policy_data, + _validate_backend, + _validate_generation_parameters, + _validate_intra, +) __all__ = [ "MemberRandomizationCfg", @@ -30,306 +38,3 @@ "SceneRandomizationPolicyCfg", "SceneRandomizationJobCfg", ] - - -@configclass -class MemberRandomizationCfg: - """One non-root member's displacement bounds in its parent support frame. - - ``xy_offset_bounds_m`` is ``[[min_x, min_y], [max_x, max_y]]`` relative - to the original entity origin; ``None`` samples the complete support area. - Yaw is a continuous increment around the support normal, retaining tilt. - """ - - xy_offset_bounds_m: list[list[float]] | None = None - yaw_delta_range_deg: list[float] = [-180.0, 180.0] - - -@configclass -class SupportOverrideCfg: - """Explicit parent-local support frame and GeoJSON Polygon/MultiPolygon. - - The proper rigid ``parent_from_support`` matrix locates the support plane - at support Z=0 in the parent local Z-up geometry after asset scaling. Its polygon must be covered by actual coplanar mesh faces. - """ - - parent_from_support: list[list[float]] = MISSING - polygon: dict = MISSING - - -@configclass -class IntraGroupRandomizationCfg: - """Explicit member selection and conservative support geometry settings. - - Fixed members follow their parent. Clearances and grid resolution are in - metres. Overrides are keyed by parent UID, independently of target order. - """ - - targets: dict[str, MemberRandomizationCfg] = {} - boundary_margin_m: float = 0.002 - sibling_margin_m: float = 0.002 - support_gap_m: float = 0.001 - grid_resolution_m: float = 0.001 - support_overrides: dict[str, SupportOverrideCfg] = {} - - -def _validate_backend(*, backend: str, device: str, batch_size: int) -> None: - """Validate execution controls without initializing CUDA or reading assets.""" - if backend not in ("slsqp", "batched"): - raise ValueError("backend must be slsqp or batched.") - if not isinstance(device, str) or not ( - device == "cpu" - or device == "cuda" - or ( - device.startswith("cuda:") and device[5:].isascii() and device[5:].isdigit() - ) - ): - raise ValueError("device must be cpu, cuda, or cuda:N.") - if backend == "slsqp" and device != "cpu": - raise ValueError( - "The slsqp backend runs on CPU; use backend: batched for CUDA." - ) - if type(batch_size) is not int or batch_size < 1: - raise ValueError("batch_size must be a positive integer.") - - -def _validate_generation_parameters( - *, seed: int, candidate_count: int, max_attempts: int, collision_margin_m: float -) -> None: - """Validate YAML values and Python overrides before accessing scene files.""" - for name, value, minimum in ( - ("seed", seed, 0), - ("candidate_count", candidate_count, 1), - ("max_attempts", max_attempts, 1), - ): - if type(value) is not int or value < minimum: - raise ValueError(f"{name} must be an integer >= {minimum}.") - if candidate_count > max_attempts: - raise ValueError("max_attempts must be at least candidate_count.") - if type(collision_margin_m) not in (int, float) or collision_margin_m < 0: - raise ValueError("collision_margin_m must be finite and nonnegative.") - try: - finite = math.isfinite(collision_margin_m) - except OverflowError: - finite = False - if not finite: - raise ValueError("collision_margin_m must be finite and nonnegative.") - - -def _mapping(value: Any, *, name: str, allowed: set[str]) -> dict[str, Any]: - if not isinstance(value, dict): - raise ValueError(f"{name} must be a mapping.") - unknown = set(value) - allowed - if unknown: - raise ValueError(f"Unknown {name} fields: {', '.join(sorted(unknown))}.") - return value - - -def _decode_intra(value: Any) -> IntraGroupRandomizationCfg | None: - """Strictly decode nested YAML mappings, also used for persisted policies.""" - if value is None: - return None - root = _mapping( - value, - name="intra_group", - allowed={ - "targets", - "boundary_margin_m", - "sibling_margin_m", - "support_gap_m", - "grid_resolution_m", - "support_overrides", - }, - ) - targets = root.get("targets", {}) - if not isinstance(targets, dict): - raise ValueError("targets must be a mapping.") - decoded = {} - for uid, target in targets.items(): - if not isinstance(uid, str) or not uid.strip(): - raise ValueError("Target UIDs must be nonempty strings.") - item = _mapping( - target, - name=f"target {uid}", - allowed={"xy_offset_bounds_m", "yaw_delta_range_deg"}, - ) - decoded[uid] = MemberRandomizationCfg(**item) - overrides = root.get("support_overrides", {}) - if not isinstance(overrides, dict): - raise ValueError("support_overrides must be a mapping.") - override_cfg = {} - for uid, override in overrides.items(): - item = _mapping( - override, - name=f"support override {uid}", - allowed={"parent_from_support", "polygon"}, - ) - if set(item) != {"parent_from_support", "polygon"}: - raise ValueError("Support overrides require a transform and polygon.") - override_cfg[uid] = SupportOverrideCfg(**item) - cfg = IntraGroupRandomizationCfg( - **{k: v for k, v in root.items() if k not in ("targets", "support_overrides")}, - targets=decoded, - support_overrides=override_cfg, - ) - _validate_intra(cfg) - return cfg - - -def _validate_intra(cfg: IntraGroupRandomizationCfg) -> None: - import numpy as np - - if not isinstance(cfg, IntraGroupRandomizationCfg): - raise ValueError("intra_group must be an IntraGroupRandomizationCfg.") - cfg.validate() - if not isinstance(cfg.targets, dict) or not isinstance(cfg.support_overrides, dict): - raise ValueError("targets and support_overrides must be mappings.") - for field in ( - "boundary_margin_m", - "sibling_margin_m", - "support_gap_m", - "grid_resolution_m", - ): - value = getattr(cfg, field) - try: - finite = type(value) in (int, float) and math.isfinite(value) - except OverflowError: - finite = False - if not finite or value < 0 or (field == "grid_resolution_m" and value == 0): - raise ValueError( - f"{field} must be finite and {'positive' if field == 'grid_resolution_m' else 'nonnegative'}." - ) - for uid, item in cfg.targets.items(): - if ( - not isinstance(uid, str) - or not uid - or not isinstance(item, MemberRandomizationCfg) - ): - raise ValueError("targets must map UIDs to MemberRandomizationCfg.") - yaw = np.asarray(item.yaw_delta_range_deg) - if ( - yaw.shape != (2,) - or yaw.dtype.kind not in "fi" - or not np.isfinite(yaw).all() - or yaw[0] > yaw[1] - or not np.isfinite(yaw[1] - yaw[0]) - ): - raise ValueError( - "yaw_delta_range_deg requires finite ordered bounds and width." - ) - if item.xy_offset_bounds_m is not None: - xy = np.asarray(item.xy_offset_bounds_m) - if ( - xy.shape != (2, 2) - or xy.dtype.kind not in "fi" - or not np.isfinite(xy).all() - or np.any(xy[0] > xy[1]) - ): - raise ValueError( - "xy_offset_bounds_m requires finite ordered lower/upper XY corners." - ) - for uid, override in cfg.support_overrides.items(): - if ( - not isinstance(uid, str) - or not uid - or not isinstance(override, SupportOverrideCfg) - ): - raise ValueError("support_overrides must map UIDs to SupportOverrideCfg.") - override.validate() - from embodichain.lab.sim.scene._support_grid import _read_support_polygon - - try: - _read_support_polygon(override.polygon) - except ValueError as exc: - raise ValueError(f"Support override {uid!r}: {exc}") from exc - try: - matrix = np.asarray(override.parent_from_support, dtype=float) - except (TypeError, ValueError, OverflowError) as exc: - raise ValueError( - "parent_from_support must contain finite numeric values." - ) from exc - if ( - matrix.shape != (4, 4) - or not np.isfinite(matrix).all() - or not np.array_equal(matrix[3], [0, 0, 0, 1]) - or not np.allclose( - matrix[:3, :3].T @ matrix[:3, :3], np.eye(3), atol=1e-8, rtol=0 - ) - or not np.isclose(np.linalg.det(matrix[:3, :3]), 1, atol=1e-8, rtol=0) - ): - raise ValueError("parent_from_support must be a proper rigid transform.") - - -def _intra_payload(cfg: IntraGroupRandomizationCfg) -> dict: - """Serialize only documented settings, independent of configclass internals.""" - return deepcopy( - { - "targets": { - uid: { - "xy_offset_bounds_m": item.xy_offset_bounds_m, - "yaw_delta_range_deg": list(item.yaw_delta_range_deg), - } - for uid, item in sorted(cfg.targets.items()) - }, - **{ - key: float(getattr(cfg, key)) - for key in ( - "boundary_margin_m", - "sibling_margin_m", - "support_gap_m", - "grid_resolution_m", - ) - }, - "support_overrides": { - uid: { - "parent_from_support": item.parent_from_support, - "polygon": item.polygon, - } - for uid, item in sorted(cfg.support_overrides.items()) - }, - } - ) - - -@configclass -class SceneRandomizationPolicyCfg: - """Geometric policy independent of scene files and runtime lifecycles. - - Args: - backend: Explicit numeric backend; the shared interface uses batched sampling. - device: CPU or CUDA tensor device. - batch_size: Maximum logical proposals evaluated together. - collision_margin_m: Minimum group AABB separation in metres. - randomize_groups: Sample permitted group-root XY translations. - group_targets: Root UIDs to move; None selects all permitted roots. - group_xy_offset_bounds_m: Optional per-root arena XY displacement bounds. - intra_group: Selected non-root XY/yaw changes and support settings. - """ - - backend: str = "batched" - device: str = "cpu" - batch_size: int = 4096 - collision_margin_m: float = 0.02 - randomize_groups: bool = True - group_targets: tuple[str, ...] | None = None - group_xy_offset_bounds_m: dict[str, list[list[float]]] = {} - intra_group: IntraGroupRandomizationCfg | None = None - - -@configclass -class SceneRandomizationJobCfg: - """Bounded caller-owned generation settings and output representation. - - Args: - policy: Source-independent geometric sampling policy. - seed: Addressed generation seed. - candidate_count: Requested valid layouts. - max_attempts: Whole-layout proposal budget. - layout_storage: JSON or packed candidate storage. - """ - - policy: SceneRandomizationPolicyCfg = SceneRandomizationPolicyCfg() - seed: int = 0 - candidate_count: int = 10 - max_attempts: int = 1000 - layout_storage: str = "packed" diff --git a/embodichain/lab/sim/scene/expansion/conditions.py b/embodichain/lab/sim/scene/expansion/conditions.py new file mode 100644 index 0000000000..8d9390a6f0 --- /dev/null +++ b/embodichain/lab/sim/scene/expansion/conditions.py @@ -0,0 +1,197 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Owned arena-space anchors that restrict an existing prepared layout domain.""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass, replace +import math +from types import MappingProxyType +from typing import TYPE_CHECKING + +import numpy as np + +from .._validation import _identifier, _rigid_pose + +if TYPE_CHECKING: + from .layout import PreparedSceneRandomization + +__all__ = ["SceneLayoutCondition"] + + +class _LayoutConditionDomainError(ValueError): + """A well-formed anchor conflicts with its prepared layout domain.""" + + +@dataclass(frozen=True) +class SceneLayoutCondition: + """Fix declared entities to arena poses without enlarging their motion domain. + + Args: + poses: Native UID to rigid arena-space pose. Inputs are independently + owned immutable tuples. Group roots retain their authored rotation + and height; selected members retain their declared support-frame + XY/yaw constraints. A moving ancestor of a member anchor must also + be anchored, so an absolute anchor cannot drift with its parent. + + Conditions do not authorize new motion, topology changes or physics steps. + Geometric infeasibility yields an empty/budget-exhausted generation result; + malformed or unsupported conditions reject before candidate generation. + """ + + poses: Mapping[str, object] + + def __post_init__(self) -> None: + if not isinstance(self.poses, Mapping): + raise TypeError("Condition poses must be a UID-to-pose mapping") + owned = {} + for uid, pose in self.poses.items(): + _identifier(uid, "condition UID") + owned[uid] = _rigid_pose(pose) + object.__setattr__(self, "poses", MappingProxyType(dict(sorted(owned.items())))) + + def to_dict(self) -> dict[str, list[list[float]]]: + """Return a detached JSON-compatible projection of the anchors.""" + return {uid: [list(row) for row in pose] for uid, pose in self.poses.items()} + + +def _condition_prepared( + prepared: PreparedSceneRandomization, condition: SceneLayoutCondition +) -> PreparedSceneRandomization: + """Compile anchors to zero-width bounds, keeping existing geometry checks.""" + if not isinstance(condition, SceneLayoutCondition): + raise TypeError("conditions must be SceneLayoutCondition") + if not condition.poses: + return prepared + source, policy = prepared.source, prepared.policy + unknown = set(condition.poses) - set(source.poses) + if unknown: + raise ValueError(f"Unknown condition entities: {sorted(unknown)}") + nodes = prepared._geometry["nodes"] + index = {n["uid"]: i for i, n in enumerate(nodes)} + roots = {nodes[i]["uid"] for i in prepared._movable_roots} + targets = policy["intra_group"]["targets"] + variable = roots | set(targets) + world = {source.support_uid: np.asarray(source.poses[source.support_uid])} + for n in nodes: + uid = n["uid"] + original = np.asarray(source.poses[uid]) + parent_uid = source.parent_by_uid[uid] + anchored = uid in condition.poses + if anchored: + parent = parent_uid + while parent != source.support_uid: + if parent in variable and parent not in condition.poses: + raise _LayoutConditionDomainError( + f"Anchor {uid!r} requires its moving ancestor {parent!r} to be anchored" + ) + parent = source.parent_by_uid[parent] + parent_world = world[parent_uid] + if n["parent"] < 0: + if not anchored: + world[uid] = original + continue + wanted = np.asarray(condition.poses[uid]) + if ( + not np.allclose(wanted[:3, :3], original[:3, :3], rtol=0, atol=1e-9) + or abs(wanted[2, 3] - original[2, 3]) > 1e-9 + ): + raise _LayoutConditionDomainError( + "Root anchors support declared XY translation only; rotation/height must remain authored" + ) + delta = wanted[:2, 3] - original[:2, 3] + if uid not in roots: + if not np.allclose(delta, 0, rtol=0, atol=1e-9): + raise _LayoutConditionDomainError( + f"Condition cannot move fixed group {uid!r}" + ) + else: + bounds = policy["group_xy_offset_bounds_m"].get(uid) + if bounds is not None and ( + np.any(delta < np.asarray(bounds)[0] - 1e-9) + or np.any(delta > np.asarray(bounds)[1] + 1e-9) + ): + raise _LayoutConditionDomainError( + "Anchor lies outside declared root XY bounds" + ) + policy["group_xy_offset_bounds_m"][uid] = [ + delta.tolist(), + delta.tolist(), + ] + world[uid] = wanted + continue + relative = np.asarray(n["relative_pose"]) + if not anchored: + world[uid] = parent_world @ relative + continue + wanted = np.asarray(condition.poses[uid]) + if uid not in targets: + if not np.allclose(wanted, parent_world @ relative, rtol=0, atol=1e-9): + raise _LayoutConditionDomainError( + f"Condition cannot move undeclared member {uid!r}" + ) + world[uid] = wanted + continue + spec = targets[uid] + support = prepared._geometry["supports"][parent_uid] + inv_frame = np.linalg.inv(np.asarray(support["parent_from_support"])) + baseline = inv_frame @ relative + local = inv_frame @ np.linalg.inv(parent_world) @ wanted + rotation = local[:3, :3] @ np.linalg.inv(baseline[:3, :3]) + angle = math.degrees(math.atan2(rotation[1, 0], rotation[0, 0])) + c, s = math.cos(math.radians(angle)), math.sin(math.radians(angle)) + yaw_rotation = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + low, high = spec["yaw_delta_range_deg"] + angle += 360 * math.floor(((low + high) / 2 - angle) / 360 + 0.5) + height = ( + policy["intra_group"]["support_gap_m"] + + support.get("height_ceiling_m", 0) + - support["root_min_z"][uid] + ) + if ( + not np.allclose(rotation, yaw_rotation, rtol=0, atol=1e-9) + or abs(local[2, 3] - height) > 1e-9 + ): + raise _LayoutConditionDomainError( + "Member anchor violates its authored tilt or support height" + ) + if angle < low - 1e-8 or angle > high + 1e-8: + raise _LayoutConditionDomainError( + "Member anchor lies outside declared yaw bounds" + ) + offset = local[:2, 3] - baseline[:2, 3] + bounds = spec["xy_offset_bounds_m"] + if bounds is not None and ( + np.any(offset < np.asarray(bounds)[0] - 1e-9) + or np.any(offset > np.asarray(bounds)[1] + 1e-9) + ): + raise _LayoutConditionDomainError( + "Member anchor lies outside declared XY bounds" + ) + spec["xy_offset_bounds_m"] = [offset.tolist(), offset.tolist()] + spec["yaw_delta_range_deg"] = [angle, angle] + world[uid] = wanted + if source.support_uid in condition.poses and not np.allclose( + condition.poses[source.support_uid], + source.poses[source.support_uid], + rtol=0, + atol=1e-9, + ): + raise ValueError("The main support is fixed") + return replace(prepared, _policy=deepcopy(policy)) diff --git a/embodichain/lab/sim/scene/expansion/contracts.py b/embodichain/lab/sim/scene/expansion/contracts.py index 2b103efa11..a1bc4fb78a 100644 --- a/embodichain/lab/sim/scene/expansion/contracts.py +++ b/embodichain/lab/sim/scene/expansion/contracts.py @@ -13,145 +13,19 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- - -"""Immutable scene proposals, independent of task and execution lifecycles.""" +"""Public scene proposal contracts shared with simulator-free configuration.""" from __future__ import annotations -from dataclasses import dataclass -import hashlib -import json +from embodichain.lab._scene_config.scene import ( + ScenePoseChange, + SceneVariant, + _variant_id, +) +from embodichain.lab._scene_config.values import ( + _identifier, + _nonnegative_integer, + _rigid_pose, +) __all__ = ["ScenePoseChange", "SceneVariant"] - - -from .._validation import _identifier, _nonnegative_integer, _rigid_pose - - -def _variant_id( - parent_scene_id: str, - seed: int, - ordinal: int, - pose_changes: tuple, - metadata: tuple, -) -> str: - """Hash already validated, canonically ordered proposal values.""" - payload = { - "schema": "scene-variant/v1", - "parent_scene_id": parent_scene_id, - "seed": seed, - "ordinal": ordinal, - "pose_changes": pose_changes, - "metadata": metadata, - } - encoded = json.dumps( - payload, sort_keys=True, separators=(",", ":"), allow_nan=False - ).encode("utf-8") - return "scene-variant:" + hashlib.sha256(encoded).hexdigest() - - -@dataclass(frozen=True) -class ScenePoseChange: - """A proposed pose for one existing physical entity. - - Args: - entity_uid: Stable physical entity UID, independent of environment slots. - arena_pose: Homogeneous 4x4 transform from the entity frame into its - local Z-up arena frame. Inputs are copied into immutable tuples. - - .. attention:: - An arena pose excludes replication offsets. The host converts it to - backend coordinates when applying the candidate. This contract does - not establish support, collision freedom, or manipulation feasibility. - """ - - entity_uid: str - arena_pose: tuple[tuple[float, ...], ...] - - def __post_init__(self) -> None: - _identifier(self.entity_uid, "entity_uid") - object.__setattr__(self, "arena_pose", _rigid_pose(self.arena_pose)) - - -@dataclass(frozen=True) -class SceneVariant: - """An immutable scene proposal with reproducible proposal identity. - - Args: - parent_scene_id: Content/revision identity of the original scene. - seed: Nonnegative proposal-generation seed. - ordinal: Nonnegative position in the candidate source. - pose_changes: Unique existing entities and their proposed arena poses. - metadata: Unique string key/value provenance entries. Entries and pose - changes are sorted so equivalent input ordering has one identity. - - .. attention:: - ``variant_id`` identifies a proposal, not a settled initial state or a - successful execution. Hosts record those identities and evidence - separately. Metadata describes provenance, not executable operations. - Since ``ordinal`` participates in the hash, consuming the same layout - at a different source position changes ``variant_id``. File-backed - consumers should retain the original layout identity and proposal - ordinal in metadata for provenance across selection orders; this ID - is not a geometry-only deduplication key. - """ - - parent_scene_id: str - seed: int - ordinal: int - pose_changes: tuple[ScenePoseChange, ...] - metadata: tuple[tuple[str, str], ...] = () - - def __post_init__(self) -> None: - _identifier(self.parent_scene_id, "parent_scene_id") - _nonnegative_integer(self.seed, "seed") - _nonnegative_integer(self.ordinal, "ordinal") - try: - changes = tuple(self.pose_changes) - except TypeError as exc: - raise ValueError( - "pose_changes must contain ScenePoseChange values" - ) from exc - if any(not isinstance(change, ScenePoseChange) for change in changes): - raise ValueError("pose_changes must contain ScenePoseChange values") - if len({change.entity_uid for change in changes}) != len(changes): - raise ValueError("pose_changes must contain unique entity UIDs") - object.__setattr__( - self, - "pose_changes", - tuple(sorted(changes, key=lambda change: change.entity_uid)), - ) - try: - entries = tuple(self.metadata) - if any(isinstance(entry, str) for entry in entries): - raise ValueError("metadata must contain string key/value pairs") - metadata = tuple(tuple(entry) for entry in entries) - except TypeError as exc: - raise ValueError("metadata must contain string key/value pairs") from exc - if any(len(entry) != 2 for entry in metadata): - raise ValueError("metadata must contain string key/value pairs") - for key, value in metadata: - _identifier(key, "metadata key") - if not isinstance(value, str): - raise ValueError("metadata values must be strings") - if len({key for key, _ in metadata}) != len(metadata): - raise ValueError("metadata must contain unique keys") - object.__setattr__(self, "metadata", tuple(sorted(metadata))) - - @property - def variant_id(self) -> str: - """Return a stable identity including geometry, source, seed, and ordinal. - - Returns: - SHA-256 identity independent of Python hash randomization and - temporary physical environment assignments. - """ - return _variant_id( - self.parent_scene_id, - self.seed, - self.ordinal, - tuple( - (change.entity_uid, change.arena_pose) for change in self.pose_changes - ), - self.metadata, - ) diff --git a/embodichain/lab/sim/scene/expansion/layout.py b/embodichain/lab/sim/scene/expansion/layout.py index 479946ce70..17763a96f3 100644 --- a/embodichain/lab/sim/scene/expansion/layout.py +++ b/embodichain/lab/sim/scene/expansion/layout.py @@ -37,6 +37,7 @@ from ..geometry import SupportSurfaceConfig, detect_support_region from ..query import _backend_tensor from .contracts import ScenePoseChange, SceneVariant +from .conditions import SceneLayoutCondition, _condition_prepared from ._layout_numeric import ( _hierarchy_batch_limit, _numeric_geometry, @@ -44,13 +45,10 @@ _specification, ) from ._policy import ( - IntraGroupRandomizationCfg, SceneRandomizationPolicyCfg, - _decode_intra, - _intra_payload, + _policy_data, _validate_backend, _validate_generation_parameters, - _validate_intra, ) if TYPE_CHECKING: @@ -252,66 +250,6 @@ def scene_layout_input_from_sim( ) -def _policy_data(cfg: SceneRandomizationPolicyCfg | Mapping) -> dict: - if isinstance(cfg, Mapping): - raw = dict(cfg) - cfg = SceneRandomizationPolicyCfg(**raw) - if not isinstance(cfg, SceneRandomizationPolicyCfg): - raise TypeError("policy must be a SceneRandomizationPolicyCfg or mapping") - cfg = cfg.copy() - cfg.validate() - _validate_backend(backend=cfg.backend, device=cfg.device, batch_size=cfg.batch_size) - if cfg.backend != "batched": - raise ValueError( - "The source-independent layout API supports backend='batched'." - ) - _validate_generation_parameters( - seed=0, - candidate_count=1, - max_attempts=1, - collision_margin_m=cfg.collision_margin_m, - ) - if type(cfg.randomize_groups) is not bool: - raise ValueError("randomize_groups must be a boolean.") - intra = cfg.intra_group - if isinstance(intra, dict) or intra is None: - intra = _decode_intra(intra) - if intra is None: - intra = IntraGroupRandomizationCfg() - _validate_intra(intra) - if not isinstance(cfg.group_xy_offset_bounds_m, dict): - raise ValueError("group_xy_offset_bounds_m must map root UIDs to XY bounds.") - for uid, bounds in cfg.group_xy_offset_bounds_m.items(): - if bounds is None: - raise ValueError( - "Root XY bounds must be explicit lower/upper corners; omit unbounded roots." - ) - _decode_intra( - { - "targets": { - uid: {"xy_offset_bounds_m": bounds, "yaw_delta_range_deg": [0, 0]} - } - } - ) - if cfg.group_xy_offset_bounds_m and not cfg.randomize_groups: - raise ValueError("Root XY bounds require randomize_groups=True.") - if cfg.group_targets is not None and ( - isinstance(cfg.group_targets, str) - or len(set(cfg.group_targets)) != len(cfg.group_targets) - ): - raise ValueError("group_targets must contain unique root UIDs.") - return { - "backend": cfg.backend, - "device": cfg.device, - "batch_size": cfg.batch_size, - "collision_margin_m": float(cfg.collision_margin_m), - "randomize_groups": cfg.randomize_groups, - "group_targets": None if cfg.group_targets is None else list(cfg.group_targets), - "group_xy_offset_bounds_m": deepcopy(cfg.group_xy_offset_bounds_m), - "intra_group": _intra_payload(intra), - } - - @dataclass(frozen=True) class PreparedSceneRandomization: """Own reusable numeric geometry, policy, permissions and source identity.""" @@ -571,6 +509,7 @@ def generate_scene_variants( candidate_count: int = 10, max_attempts: int = 1000, start_ordinal: int = 0, + conditions: SceneLayoutCondition | None = None, ) -> SceneRandomizationBatch: """Generate a batch without loading meshes or constructing simulation objects. @@ -580,10 +519,17 @@ def generate_scene_variants( candidate_count: Requested valid layouts. max_attempts: Whole-layout proposal budget. start_ordinal: First logical proposal; allows bounded continuation. + conditions: Optional owned arena-pose anchors. They restrict existing + root XY/member XY-yaw domains and never authorize extra motion. + Moving ancestors must be anchored explicitly. Empty conditions + preserve the ordinary sequence. Infeasible geometry exhausts the + declared proposal budget without moving or repairing anchors. Returns: Complete or budget-exhausted proposals with exact continuation ordinal. """ + if conditions is not None: + prepared = _condition_prepared(prepared, conditions) _validate_generation_parameters( seed=seed, candidate_count=candidate_count, @@ -600,6 +546,15 @@ def generate_scene_variants( ): raise ValueError("Requested CUDA device is unavailable.") spec = _specification(prepared._geometry, policy, device) + anchored_roots = ( + tuple( + i + for i in prepared._movable_roots + if prepared._geometry["nodes"][i]["uid"] in conditions.poses + ) + if conditions is not None + else () + ) limit = ( _hierarchy_batch_limit(policy["batch_size"], len(spec[-1])) if policy["randomize_groups"] @@ -619,6 +574,7 @@ def generate_scene_variants( group_margin=policy["collision_margin_m"], support_gap=policy["intra_group"]["support_gap_m"], movable_roots=prepared._movable_roots, + anchored_roots=anchored_roots, root_offset_bounds={ i: torch.tensor( policy["group_xy_offset_bounds_m"][node["uid"]], @@ -656,6 +612,11 @@ def generate_scene_variants( ("generator", _ALGORITHM), ("source_sha256", prepared.source.fingerprint), ("geometry_sha256", prepared.geometry_sha256), + *( + (("conditions_sha256", _digest(conditions.to_dict())),) + if conditions is not None and conditions.poses + else () + ), ), ) return SceneRandomizationBatch( diff --git a/embodichain/lab/sim/scene/feasibility/_collision_batches.py b/embodichain/lab/sim/scene/feasibility/_collision_batches.py new file mode 100644 index 0000000000..6ffe907fc9 --- /dev/null +++ b/embodichain/lab/sim/scene/feasibility/_collision_batches.py @@ -0,0 +1,335 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + + +"""Batch the pinned Newton convex query without touching the live World.""" + +from __future__ import annotations + +import math + +import numpy as np + +__all__: list[str] = [] + + +class _CollisionBatches: + """Keep native shapes, precision, pair exemptions and halfspace supports. + + The query owns one numerical model containing independent replicas. It is + never stepped and creates no simulation World. Explicit pairs prohibit + cross-environment contacts; only the small per-row summaries are read back. + """ + + def __init__(self, checker, count, allowed, moving, all_pairs=False): + import newton + import warp as wp + + self.checker, self.count, self.wp = checker, count, wp + self.graph = None + self.queries = 0 + self.mask, self.planes, self.has_native = checker._mask( + allowed, moving, all_pairs + ) + self.locals = np.asarray([body.local_pose for body in checker.bodies]) + self.indices = [ + checker._frame_uids.index(body.frame_uid) for body in checker.bodies + ] + self.model = self.state = self.pipeline = self.contacts = None + self.meshes = [] + if not self.has_native: + return + builder = newton.ModelBuilder() + cfg = newton.ModelBuilder.ShapeConfig( + gap=checker.detection_gap_m / 2, + margin=0.0, + density=0.0, + collision_filter_parent=False, + ) + for index in checker._native: + body = checker.bodies[index] + owner = builder.add_body( + mass=1.0, + inertia=wp.mat33(np.eye(3, dtype=np.float32)), + is_kinematic=True, + ) + options = {"body": owner, "cfg": cfg} + if body.kind == "box": + shape = builder.add_shape_box( + hx=body.dimensions[0] / 2, + hy=body.dimensions[1] / 2, + hz=body.dimensions[2] / 2, + **options, + ) + elif body.kind == "sphere": + shape = builder.add_shape_sphere(radius=body.dimensions[0], **options) + elif body.kind in ("capsule", "cylinder"): + shape = getattr(builder, "add_shape_" + body.kind)( + radius=body.dimensions[0], + half_height=body.dimensions[1] / 2, + **options, + ) + else: + mesh = newton.Mesh( + np.asarray(body.vertices, dtype=np.float32), + np.asarray(body.triangles, dtype=np.int32).reshape(-1), + compute_inertia=False, + maxhullvert=len(body.vertices), + ) + self.meshes.append(mesh) + shape = builder.add_shape_convex_hull(mesh=mesh, **options) + if shape != owner: + raise RuntimeError("Collision query requires one shape per owned body") + batch = newton.ModelBuilder() + batch.replicate(builder, count) + self.model = batch.finalize(device=checker.device) + self.state = self.model.state() + width = len(checker._native) + pairs = [ + (checker._native_lookup[i], checker._native_lookup[j]) + for i, j in checker._pairs + if i in checker._native_lookup and j in checker._native_lookup + ] + explicit = np.asarray( + [ + (a + row * width, b + row * width) + for row in range(count) + for a, b in pairs + ], + dtype=np.int32, + ) + self.explicit = wp.array(explicit, dtype=wp.vec2i, device=checker.device) + self.capacity = checker._capacity * count + self.pipeline = newton.CollisionPipeline( + self.model, + broad_phase="explicit", + shape_pairs_filtered=self.explicit, + include_static_kinematic_pairs=True, + rigid_contact_max=self.capacity, + soft_contact_max=0, + ) + if self.pipeline.narrow_phase.global_contact_reducer is not None: + raise RuntimeError("Unsupported batched Newton narrow-phase route") + self.contacts = self.pipeline.contacts() + self.active = wp.array(self.mask, dtype=wp.int32, device=checker.device) + self.tolerance = wp.empty(count, dtype=float, device=checker.device) + self.output = wp.empty((count, 3), dtype=float, device=checker.device) + self.empty_count = wp.zeros(1, dtype=wp.int32, device=checker.device) + self.stream = ( + wp.Stream(checker.device) if checker.device.startswith("cuda") else None + ) + if self.stream is not None: + wp.synchronize_device(checker.device) + + def _run(self): + from embodichain.compute.geometry._warp.batch_contact_query import ( + contact_summary, + ) + + checker, wp = self.checker, self.wp + self.output.fill_(math.inf) + self.pipeline.collide(self.state, self.contacts) + phase, contacts = self.pipeline.narrow_phase, self.contacts + counters = [] + for count_name, storage_name in ( + ("gjk_candidate_pairs_count", "gjk_candidate_pairs"), + ("split_gjk_work_count", "split_gjk_work_items"), + ("split_manifold_work_count", "split_manifold_work_items"), + ): + value, storage = getattr(phase, count_name), getattr(phase, storage_name) + counters.extend( + ( + value if value is not None else self.empty_count, + storage.shape[0] if storage is not None else 0, + ) + ) + wp.launch( + contact_summary, + self.capacity, + inputs=[ + contacts.rigid_contact_count, + contacts.rigid_contact_shape0, + contacts.rigid_contact_shape1, + contacts.rigid_contact_point0, + contacts.rigid_contact_point1, + contacts.rigid_contact_normal, + contacts.rigid_contact_margin0, + contacts.rigid_contact_margin1, + self.model.shape_body, + self.state.body_q, + self.active, + len(checker._native), + self.count, + checker.clearance_m, + self.tolerance, + self.capacity, + self.pipeline.broad_phase_pair_count, + len(self.explicit), + *counters, + self.output, + ], + device=checker.device, + ) + + def check(self, frame_poses, rows): + from scipy.spatial.transform import Rotation + from embodichain.lab.sim.scene.feasibility import UnsupportedFeasibilityError + from embodichain.lab.sim.scene.feasibility.contracts import FeasibilityCheck + from embodichain.lab.sim.scene.feasibility.collision import _query_float32 + + checker = self.checker + if checker._closed: + raise RuntimeError("collision checker is closed") + frames = np.stack([frame_poses[uid] for uid in checker._frame_uids], axis=1) + if frames.shape != (self.count, len(checker._frame_uids), 4, 4): + raise ValueError("Collision frames must preserve every physical row") + if frames.dtype.kind not in "fiu" or not np.isfinite(frames).all(): + raise ValueError("arena_pose must contain finite real numbers") + frames = frames.astype(np.float64) + rotation = frames[..., :3, :3] + if ( + np.any(np.abs(frames[..., 3, :] - (0, 0, 0, 1)) > 1e-6) + or np.any( + np.abs(np.swapaxes(rotation, -1, -2) @ rotation - np.eye(3)) > 1e-6 + ) + or np.any(np.abs(np.linalg.det(rotation) - 1) > 1e-6) + ): + raise ValueError("arena_pose must be a proper rigid transform") + frames[..., 3, :] = (0, 0, 0, 1) + frames[..., :3, 3] -= frames[:, :1, :3, 3].copy() + poses = frames[:, self.indices] @ self.locals + if not np.isfinite(poses).all(): + raise UnsupportedFeasibilityError( + "Nonfinite composed physical collision pose" + ) + failures = {} + for i, j in self.planes: + plane, shape = (i, j) if checker.bodies[i].kind == "halfspace" else (j, i) + a, b = checker.bodies[plane], checker.bodies[shape] + normal = poses[:, plane, :3, :3] @ np.asarray(a.dimensions[:3]) + local = np.einsum("bji,bj->bi", poses[:, shape, :3, :3], normal) + if b.kind == "convex": + support = (local @ np.asarray(b.vertices).T).min(axis=-1) + elif b.kind == "sphere": + support = -b.dimensions[0] + elif b.kind == "capsule": + support = -b.dimensions[0] - np.abs(local[:, 2]) * b.dimensions[1] / 2 + elif b.kind == "cylinder": + support = ( + -b.dimensions[0] * np.linalg.norm(local[:, :2], axis=-1) + - np.abs(local[:, 2]) * b.dimensions[1] / 2 + ) + elif b.kind == "box": + support = -np.abs(local) @ (np.asarray(b.dimensions) / 2) + else: + raise UnsupportedFeasibilityError("Unsupported analytic halfspace pair") + distance = (normal * (poses[:, shape, :3, 3] - poses[:, plane, :3, 3])).sum( + -1 + ) + distance += support - a.dimensions[3] + if not np.isfinite(distance).all(): + raise UnsupportedFeasibilityError( + "Nonfinite analytic halfspace separation" + ) + for row in rows: + if distance[row] < checker.clearance_m - 1e-8 and row not in failures: + failures[row] = (i, j) + if self.has_native: + matrices = poses[:, checker._native] + relative = matrices[..., :3, 3] - matrices[:, :1, :3, 3] + rotations = Rotation.from_matrix(matrices[..., :3, :3].reshape(-1, 3, 3)) + quaternions = rotations.as_quat().reshape(self.count, -1, 4) + packed = _query_float32( + np.concatenate((relative, quaternions), axis=-1), + "Packed physical collision pose", + ) + recovered = rotations.as_matrix().reshape(matrices[..., :3, :3].shape) + loss = np.linalg.norm(recovered - matrices[..., :3, :3], axis=(-2, -1)).max( + -1 + ) + tolerance = ( + checker.numeric_tolerance_m + + 2 * (checker._geometry_error + loss * checker._geometry_radius) + + 32 + * np.finfo(np.float32).eps + * (np.abs(relative).max(axis=(-2, -1)) + checker._geometry_radius) + ) + if not np.isfinite(tolerance).all() or np.any( + tolerance >= checker.detection_gap_m - checker.clearance_m + ): + raise UnsupportedFeasibilityError( + "Candidate exceeds the contact-band precision budget" + ) + checker._max_effective_tolerance = max( + checker._max_effective_tolerance, float(tolerance.max()) + ) + # Solvers may temporarily borrow external CUDA streams. Own this + # query's stream through assignment, capture, launch and readback. + # Inputs have already crossed a measured CPU boundary above. + with self.wp.ScopedStream(self.stream, sync_enter=False): + self.state.body_q.assign(packed.reshape(-1, 7)) + self.tolerance.assign(tolerance.astype(np.float32)) + if self.graph is None: + self._run() + if self.stream is not None and checker._use_cuda_graph: + self.wp.synchronize_stream(self.stream) + with self.wp.ScopedCapture(stream=self.stream) as capture: + self._run() + self.graph = capture.graph + self.wp.capture_launch(self.graph, stream=self.stream) + else: + self.wp.capture_launch(self.graph, stream=self.stream) + summary = self.output.numpy() + for row in rows: + if ( + math.isfinite(summary[row, 2]) + or math.isnan(summary[row, 0]) + or summary[row, 0] == -math.inf + ): + raise UnsupportedFeasibilityError( + "Batched Newton query overflowed or returned invalid evidence" + ) + if row not in failures and math.isfinite(summary[row, 1]): + a, b = divmod(int(summary[row, 1]), len(checker._native)) + failures[row] = (checker._native[a], checker._native[b]) + self.queries += 1 + result = [] + for row in rows: + if row in failures: + i, j = failures[row] + pair = checker.bodies[i].collision_id, checker.bodies[j].collision_id + report = FeasibilityCheck( + "collision", + "failed", + f"{pair[0]} / {pair[1]}: forbidden collision or insufficient clearance", + ) + else: + pair = None + report = FeasibilityCheck( + "collision", + "passed", + "Discrete Newton convex/physical-halfspace query; batched physical rows", + ) + result.append((row, report, pair)) + return result + + def close(self): + with self.wp.ScopedStream(getattr(self, "stream", None), sync_enter=False): + self.graph = self.contacts = self.pipeline = self.state = self.model = None + for name in ("explicit", "active", "tolerance", "output", "empty_count"): + setattr(self, name, None) + self.meshes.clear() + self.stream = None diff --git a/embodichain/lab/sim/scene/feasibility/_values.py b/embodichain/lab/sim/scene/feasibility/_values.py index 1d3f6a1ea2..0c86962f9d 100644 --- a/embodichain/lab/sim/scene/feasibility/_values.py +++ b/embodichain/lab/sim/scene/feasibility/_values.py @@ -13,126 +13,19 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- -"""Private canonical value validation for feasibility artifacts.""" +"""Internal compatibility imports for canonical feasibility value validation.""" from __future__ import annotations -from collections.abc import Mapping -from dataclasses import fields, is_dataclass -import hashlib -import json -import math -from numbers import Real -from enum import Enum -import numpy as np -import torch -from pathlib import Path -from types import MappingProxyType - -from .._validation import _identifier, _rigid_pose - -__all__ = [] - - -def plain(value: object) -> object: - if isinstance(value, np.ndarray): - return plain(value.tolist()) - if isinstance(value, np.generic): - return plain(value.item()) - if isinstance(value, torch.Tensor): - return plain(value.detach().cpu().tolist()) - if isinstance(value, (Path, torch.device)): - return str(value) - if isinstance(value, slice): - return {"slice": [value.start, value.stop, value.step]} - if isinstance(value, Enum): - return plain(value.value) - if isinstance(getattr(value, "name", None), str) and isinstance( - getattr(value, "value", None), Real - ): - return { - "enum_type": type(value).__module__ + "." + type(value).__qualname__, - "name": value.name, - "value": plain(value.value), - } - if isinstance(value, (set, frozenset)): - return sorted( - (plain(v) for v in value), key=lambda v: json.dumps(v, sort_keys=True) - ) - if is_dataclass(value) and not isinstance(value, type): - return {f.name: plain(getattr(value, f.name)) for f in fields(value)} - if isinstance(value, Mapping): - return {str(k): plain(v) for k, v in value.items()} - if isinstance(value, (tuple, list)): - return [plain(v) for v in value] - return value - - -def digest(value: object) -> str: - return hashlib.sha256( - json.dumps( - plain(value), sort_keys=True, separators=(",", ":"), allow_nan=False - ).encode() - ).hexdigest() - - -def owned_json(value: Mapping[str, object]) -> Mapping[str, object]: - copied = json.loads(json.dumps(plain(value), allow_nan=False)) - - def freeze(item: object) -> object: - if isinstance(item, dict): - return MappingProxyType({k: freeze(v) for k, v in item.items()}) - if isinstance(item, list): - return tuple(freeze(v) for v in item) - return item - - return freeze(copied) - - -def finite(values: object, name: str) -> tuple[float, ...]: - result = tuple(values) - if any( - isinstance(v, bool) or not isinstance(v, Real) or not math.isfinite(v) - for v in result - ): - raise ValueError(f"{name} requires finite real values") - return tuple(float(v) for v in result) - - -def count(value: int, name: str, minimum: int = 1) -> None: - if type(value) is not int or value < minimum: - raise ValueError(f"{name} must be an integer >= {minimum}") - - -def names(values: object, name: str) -> tuple[str, ...]: - if isinstance(values, str): - raise ValueError(f"{name} requires a sequence") - result = tuple(values) - for value in result: - _identifier(value, name) - if len(set(result)) != len(result): - raise ValueError(f"{name} must be unique") - return result - - -def poses(values: Mapping[str, object]) -> Mapping[str, tuple]: - return MappingProxyType( - { - _identifier(k, "entity UID"): _rigid_pose(v) - for k, v in sorted(values.items()) - } - ) - - -def atomic_json(path: Path, value: object) -> None: - import os - import tempfile - - path.parent.mkdir(parents=True, exist_ok=True) - fd, temporary = tempfile.mkstemp(prefix=path.name + ".", dir=path.parent) - try: - with os.fdopen(fd, "w", encoding="utf-8") as stream: - json.dump(plain(value), stream, allow_nan=False, sort_keys=True, indent=2) - os.replace(temporary, path) - finally: - Path(temporary).unlink(missing_ok=True) +from embodichain.lab._scene_config.values import ( + atomic_json, + count, + digest, + finite, + names, + owned_json, + plain, + poses, +) + +__all__: list[str] = [] diff --git a/embodichain/lab/sim/scene/feasibility/collision.py b/embodichain/lab/sim/scene/feasibility/collision.py index 38159c4f20..f45478e3e0 100644 --- a/embodichain/lab/sim/scene/feasibility/collision.py +++ b/embodichain/lab/sim/scene/feasibility/collision.py @@ -296,6 +296,8 @@ def __init__( raise UnsupportedFeasibilityError("Physical collision pair budget exceeded") self._capacity = min(max_contacts, requested_capacity) self._mask_cache = OrderedDict() + self._batch_queries = OrderedDict() + self.last_batch_pairs: tuple[tuple[str, str] | None, ...] = () self._model = self._state = self._pipeline = self._contacts = None self._graph = self._active = self._output = self._empty_count = ( self._explicit @@ -703,6 +705,86 @@ def check( "Discrete Newton convex/physical-halfspace query; clearance is capped by the detection band", ), (0.0 if math.isinf(clearance) else min(self.detection_gap_m, clearance)) + def check_batch( + self, + frame_poses: Mapping[str, object], + *, + allowed_contacts: Sequence[tuple[str, str]] = (), + moving_objects: Sequence[str] = (), + all_pairs: bool = False, + env_ids: Sequence[int] | None = None, + ) -> tuple[FeasibilityCheck, ...]: + """Check independent measured rows with one bounded numerical query. + + Args: + frame_poses: Every declared frame with arena poses ``(B, 4, 4)``. + Tensor inputs are copied to the query-owned CPU boundary. + allowed_contacts: Shared explicit contact exemptions for these rows. + moving_objects: Dynamic frames requiring environment checks. + all_pairs: Also inspect static layout pairs. + env_ids: Ordered selected rows; None selects all B rows. + + Returns: + Discrete checks in selected-row order. ``last_batch_pairs`` records + each failure's physical IDs. This API does not report a distance or + overwrite the scalar check's clearance interval. It never advances + or changes the live simulation. Geometry, precision and saturation + rejection follow :meth:`check`. + """ + if self._closed: + raise RuntimeError("collision checker is closed") + from ._collision_batches import _CollisionBatches + + self.last_batch_pairs = () + frames = {} + size = None + for uid in self._frame_uids: + if uid not in frame_poses: + raise UnsupportedFeasibilityError( + f"Missing physical collision frame {uid}" + ) + value = frame_poses[uid] + if hasattr(value, "detach"): + value = value.detach().cpu().numpy() + array = np.asarray(value) + if array.ndim != 3 or array.shape[1:] != (4, 4) or not array.shape[0]: + raise ValueError("batched arena poses must have shape (B, 4, 4), B > 0") + if size is not None and size != len(array): + raise ValueError("batched collision frames must share their row count") + size = len(array) + frames[uid] = array + rows = tuple(range(size)) if env_ids is None else tuple(env_ids) + if ( + not rows + or len(set(rows)) != len(rows) + or any(type(row) is not int or not 0 <= row < size for row in rows) + ): + raise ValueError("env_ids must select unique physical rows") + if type(all_pairs) is not bool: + raise TypeError("all_pairs must be a bool") + # Resolve permissions with the same native validation before caching. + allowed = frozenset(self._expand_pairs(allowed_contacts)) + self._mask(allowed_contacts, moving_objects, all_pairs) + key = size, allowed, frozenset(moving_objects), all_pairs + query = self._batch_queries.get(key) + if query is None: + if size * self._capacity > 1048576: + raise UnsupportedFeasibilityError( + "Batched collision contact budget exceeded" + ) + query = _CollisionBatches( + self, size, allowed_contacts, moving_objects, all_pairs + ) + self._batch_queries[key] = query + while len(self._batch_queries) > 2: + _, old = self._batch_queries.popitem(last=False) + old.close() + else: + self._batch_queries.move_to_end(key) + result = query.check(frames, rows) + self.last_batch_pairs = tuple(pair for _, _, pair in result) + return tuple(report for _, report, _ in result) + def _failure(self, i: int, j: int) -> FeasibilityCheck: self.last_pair = self.bodies[i].collision_id, self.bodies[j].collision_id return FeasibilityCheck( @@ -713,6 +795,29 @@ def _failure(self, i: int, j: int) -> FeasibilityCheck: def close(self) -> None: """Release owned query arrays/graphs; repeated cleanup is harmless.""" + stream = next( + ( + query.stream + for query in self._batch_queries.values() + if getattr(query, "stream", None) is not None + ), + None, + ) + if stream is not None: + import warp as wp + + # A robot solver may have borrowed a temporary Warp stream. Keep + # query-owned arrays on a live stream through their final release. + with wp.ScopedStream(stream, sync_enter=False): + self._close_arrays() + else: + self._close_arrays() + + def _close_arrays(self) -> None: + for query in self._batch_queries.values(): + query.close() + self._batch_queries.clear() + self.last_batch_pairs = () self._graph = self._pipeline = self._contacts = self._state = self._model = None self._active = self._output = self._empty_count = None self._tolerance = None diff --git a/embodichain/lab/sim/scene/feasibility/contracts.py b/embodichain/lab/sim/scene/feasibility/contracts.py index c38fdcfa09..51069c3e43 100644 --- a/embodichain/lab/sim/scene/feasibility/contracts.py +++ b/embodichain/lab/sim/scene/feasibility/contracts.py @@ -22,12 +22,19 @@ from dataclasses import dataclass, field from functools import cached_property from types import MappingProxyType -import math from typing import Literal -from .._validation import _identifier, _rigid_pose -from ..expansion import ScenePoseChange, SceneVariant -from ._values import count, digest, finite, names, owned_json, poses +from embodichain.lab._scene_config.feasibility import ( + HeldObject, + PhaseTarget, + PoseSearchVariable, + TaskFeasibilityRequest, + TaskOperationMode, + TaskPhase, +) +from .._validation import _identifier +from ..expansion import SceneVariant +from ._values import digest, finite, names, owned_json, poses __all__ = [ "PoseSearchVariable", @@ -42,230 +49,6 @@ "CandidateEvaluation", ] -_IDENTITY = ( - (1.0, 0.0, 0.0, 0.0), - (0.0, 1.0, 0.0, 0.0), - (0.0, 0.0, 1.0, 0.0), - (0.0, 0.0, 0.0, 1.0), -) - - -@dataclass(frozen=True) -class PoseSearchVariable: - """One XY/yaw displacement applied to a scene group or robot installation. - - Args: - name: Unique variable name. - kind: ``scene`` or ``robot``; robot members are physical robot UIDs. - reference_pose: Group-to-arena Z-up transform, including mounting height. - lower: Lower XY metre / yaw radian displacement bounds. - upper: Upper displacement bounds; equal endpoints fix a parameter. - members: Member UID to member-to-group transforms. All members move - together, preserving dual-arm mounting and support-group relations. - """ - - name: str - kind: Literal["scene", "robot"] - reference_pose: tuple - lower: tuple[float, float, float] - upper: tuple[float, float, float] - members: Mapping[str, tuple] - - def __post_init__(self) -> None: - _identifier(self.name, "variable name") - if self.kind not in ("scene", "robot"): - raise ValueError("variable kind must be scene or robot") - object.__setattr__(self, "reference_pose", _rigid_pose(self.reference_pose)) - lo, hi = finite(self.lower, "lower"), finite(self.upper, "upper") - if len(lo) != 3 or len(hi) != 3 or any(a > b for a, b in zip(lo, hi)): - raise ValueError("bounds require three ordered XY/yaw coordinates") - if hi[2] - lo[2] > 2 * math.pi + 1e-10: - raise ValueError("yaw interval must span at most one turn") - object.__setattr__(self, "lower", lo) - object.__setattr__(self, "upper", hi) - object.__setattr__(self, "members", poses(self.members)) - if not self.members: - raise ValueError("variables require explicit member UIDs") - - -@dataclass(frozen=True) -class PhaseTarget: - """One simultaneous TCP target in an explicit arena or entity frame. - - Args: - control_part: Explicit solver name, preserving custom names. - pose: TCP-to-frame rigid transform. - frame_uid: Entity frame UID, or ``None`` for the arena frame. - """ - - control_part: str - pose: tuple - frame_uid: str | None = None - - def __post_init__(self) -> None: - _identifier(self.control_part, "control_part") - object.__setattr__(self, "pose", _rigid_pose(self.pose)) - if self.frame_uid is not None: - _identifier(self.frame_uid, "frame_uid") - - -@dataclass(frozen=True) -class HeldObject: - """Rigid attachment during travel; never a physical-success assertion. - - Args: - entity_uid: Held object UID. - control_part: Owner TCP/control part. - tcp_from_object: Object-to-TCP transform used by collision FK. - """ - - entity_uid: str - control_part: str - tcp_from_object: tuple = _IDENTITY - - def __post_init__(self) -> None: - _identifier(self.entity_uid, "held entity") - _identifier(self.control_part, "held owner") - object.__setattr__(self, "tcp_from_object", _rigid_pose(self.tcp_from_object)) - - -@dataclass(frozen=True) -class TaskPhase: - """A synchronized stage and its explicit planning-state transition. - - Args: - phase_id: Stable stage identity. - targets: Simultaneous targets, at most one per control part. - dependencies: Predecessors; input order must be a topological order. - held_objects: Attachments during incoming motion, one owner per object. - allowed_contacts: Physical collision IDs allowed to touch in this stage. - resulting_poses: Projected object states after this stage, used only for - subsequent planning; runtime must still read fresh physical state. - joint_targets: Explicit joint-name commands, e.g. open/closed fingers. - """ - - phase_id: str - targets: tuple[PhaseTarget, ...] = () - dependencies: tuple[str, ...] = () - held_objects: tuple[HeldObject, ...] = () - allowed_contacts: tuple[tuple[str, str], ...] = () - resulting_poses: tuple[ScenePoseChange, ...] = () - joint_targets: tuple[tuple[str, float], ...] = () - - def __post_init__(self) -> None: - _identifier(self.phase_id, "phase_id") - for attr, cls in ( - ("targets", PhaseTarget), - ("held_objects", HeldObject), - ("resulting_poses", ScenePoseChange), - ): - values = tuple(getattr(self, attr)) - if any(not isinstance(v, cls) for v in values): - raise TypeError(f"{attr} contains an invalid contract") - object.__setattr__(self, attr, values) - names([v.control_part for v in self.targets], "phase target parts") - names([v.entity_uid for v in self.held_objects], "held objects") - names([v.entity_uid for v in self.resulting_poses], "resulting poses") - object.__setattr__( - self, "dependencies", names(self.dependencies, "dependencies") - ) - pairs = tuple( - tuple(sorted(names(pair, "contact pair"))) for pair in self.allowed_contacts - ) - if any(len(p) != 2 for p in pairs) or len(set(pairs)) != len(pairs): - raise ValueError("allowed contacts require unique distinct pairs") - object.__setattr__(self, "allowed_contacts", pairs) - joints = tuple( - (_identifier(k, "joint target"), finite((v,), "joint target")[0]) - for k, v in self.joint_targets - ) - names([k for k, _ in joints], "joint targets") - object.__setattr__(self, "joint_targets", joints) - - -@dataclass(frozen=True) -class TaskOperationMode: - """One permitted grasp/assignment/handover branch of the original task.""" - - mode_id: str - phases: tuple[TaskPhase, ...] - - def __post_init__(self) -> None: - _identifier(self.mode_id, "mode_id") - values = tuple(self.phases) - if not values or any(not isinstance(v, TaskPhase) for v in values): - raise ValueError("modes require explicit task phases") - names([v.phase_id for v in values], "phase IDs") - seen = set() - for phase in values: - if not set(phase.dependencies) <= seen: - raise ValueError("phases must be in dependency order without cycles") - seen.add(phase.phase_id) - object.__setattr__(self, "phases", values) - - -@dataclass(frozen=True) -class TaskFeasibilityRequest: - """Versioned fixed semantics, robot, scene, and allowed joint search domain. - - Args: - task_id: Semantic identity including original roles/goals/invariants. - robot_profile_id: Robot, tool and calibrated configuration identity. - policy_id: Planner/runtime policy version. - validator_id: Measured acceptance version. - parent_scene_id: Base scene revision/content identity. - entity_poses: Authored arena poses, including every variable member. - variables: Permitted scene and mounting changes. - modes: Allowed operation modes, preserving resource constraints. - """ - - task_id: str - robot_profile_id: str - policy_id: str - validator_id: str - parent_scene_id: str - entity_poses: Mapping[str, tuple] - variables: tuple[PoseSearchVariable, ...] - modes: tuple[TaskOperationMode, ...] - - def __post_init__(self) -> None: - for key in ( - "task_id", - "robot_profile_id", - "policy_id", - "validator_id", - "parent_scene_id", - ): - _identifier(getattr(self, key), key) - object.__setattr__(self, "entity_poses", poses(self.entity_poses)) - variables, modes = tuple(self.variables), tuple(self.modes) - if any(not isinstance(v, PoseSearchVariable) for v in variables): - raise TypeError("variables require PoseSearchVariable values") - if not modes or any(not isinstance(v, TaskOperationMode) for v in modes): - raise ValueError("request requires explicit operation modes") - names([v.name for v in variables], "variable names") - members = [k for v in variables for k in v.members] - names(members, "variable members") - if not set(members) <= self.entity_poses.keys(): - raise ValueError("variable members must exist in the authored scene") - names([v.mode_id for v in modes], "mode IDs") - for mode in modes: - for phase in mode.phases: - referenced = { - t.frame_uid for t in phase.targets if t.frame_uid is not None - } - referenced |= {h.entity_uid for h in phase.held_objects} - referenced |= {p.entity_uid for p in phase.resulting_poses} - if not referenced <= self.entity_poses.keys(): - raise ValueError("phase refers to an undeclared entity") - object.__setattr__(self, "variables", variables) - object.__setattr__(self, "modes", modes) - - @cached_property - def request_id(self) -> str: - """Return the content identity of all fixed conditions and permissions.""" - return digest(self) - @dataclass(frozen=True) class RobotDeploymentCandidate: diff --git a/embodichain/lab/sim/scene/feasibility/persistence.py b/embodichain/lab/sim/scene/feasibility/persistence.py index b8d3e5b135..1e47b696e8 100644 --- a/embodichain/lab/sim/scene/feasibility/persistence.py +++ b/embodichain/lab/sim/scene/feasibility/persistence.py @@ -19,21 +19,15 @@ from __future__ import annotations from collections.abc import Mapping -from dataclasses import fields from pathlib import Path import json import numpy as np +from embodichain.lab._scene_config.feasibility import _closed, request_from_mapping from ..expansion import ScenePoseChange, SceneVariant from ._values import atomic_json, digest, plain from .cfg import SceneRobotFeasibilityCfg from .contracts import ( - PoseSearchVariable, - PhaseTarget, - HeldObject, - TaskPhase, - TaskOperationMode, - TaskFeasibilityRequest, RobotDeploymentCandidate, JointSceneCandidate, CandidateEvaluation, @@ -49,46 +43,6 @@ __all__ = ["request_from_mapping", "save_feasibility_atlas", "load_feasibility_atlas"] -def _closed(cls: type, value: Mapping[str, object]) -> dict: - if not isinstance(value, Mapping): - raise ValueError(f"{cls.__name__} requires a mapping") - unknown = set(value) - {f.name for f in fields(cls)} - if unknown: - raise ValueError(f"Unknown {cls.__name__} fields: {sorted(unknown)}") - return dict(value) - - -def request_from_mapping(value: Mapping[str, object]) -> TaskFeasibilityRequest: - """Decode a provider-independent request through closed production contracts. - - Args: - value: JSON/YAML task, scene, mounting and phase declarations. - Returns: - Validated immutable request. No dotted import or model client is loaded. - """ - data = _closed(TaskFeasibilityRequest, value) - data["variables"] = tuple( - PoseSearchVariable(**_closed(PoseSearchVariable, v)) for v in data["variables"] - ) - modes = [] - for mode in data["modes"]: - m = _closed(TaskOperationMode, mode) - phases = [] - for phase in m["phases"]: - p = _closed(TaskPhase, phase) - for key, cls in ( - ("targets", PhaseTarget), - ("held_objects", HeldObject), - ("resulting_poses", ScenePoseChange), - ): - p[key] = tuple(cls(**_closed(cls, v)) for v in p.get(key, ())) - phases.append(TaskPhase(**p)) - m["phases"] = tuple(phases) - modes.append(TaskOperationMode(**m)) - data["modes"] = tuple(modes) - return TaskFeasibilityRequest(**data) - - def save_feasibility_atlas(atlas: FeasibilityAtlas, path: str | Path) -> None: """Atomically publish a frozen, content-addressed evidence snapshot. diff --git a/embodichain/lab/sim/scene/feasibility/planning.py b/embodichain/lab/sim/scene/feasibility/planning.py index c871153968..d07d7dec51 100644 --- a/embodichain/lab/sim/scene/feasibility/planning.py +++ b/embodichain/lab/sim/scene/feasibility/planning.py @@ -227,7 +227,7 @@ def plan_task_mode( class RobotFeasibilityBackend: - """Live B=1 Robot adapter with multi-seed IK and full-joint RRT fallback. + """Selected-row Robot adapter with multi-seed IK and full-joint RRT fallback. Args: robot: Prepared robot at the candidate's actual installation. @@ -235,6 +235,9 @@ class RobotFeasibilityBackend: cfg: Bounded branch and discretization settings. entity_poses: Settled arena poses for static and movable scene entities. seed: Candidate-local planner randomness. + env_id: Explicit prototype row for numeric solver and geometry bindings. + initial_qpos: Optional owned nominal full-joint planning start. This avoids + using another rollout's current state during rolling admission. The fallback plans in full joint coordinates, with the same collision checks for straight connections and sampled detours. It does not replace runtime @@ -249,19 +252,37 @@ def __init__( entity_poses: Mapping[str, object], *, seed: int = 0, + env_id: int = 0, + initial_qpos: np.ndarray | None = None, ) -> None: - if robot.get_qpos().shape[0] != 1 or robot.pk_chain is None: + if ( + type(env_id) is not int + or not 0 <= env_id < robot.get_qpos().shape[0] + or robot.pk_chain is None + ): raise UnsupportedFeasibilityError( - "Robot planning requires B=1 and full-link kinematics" + "Robot planning requires a valid selected row and full-link kinematics" ) self.robot, self.collision, self.cfg = robot, collision, cfg + self.env_id = env_id self.joint_names = tuple(robot.joint_names) - self.initial_qpos = robot.get_qpos()[0].detach().cpu().double().numpy().copy() + self.initial_qpos = ( + robot.get_qpos()[env_id].detach().cpu().double().numpy().copy() + if initial_qpos is None + else np.array(initial_qpos, dtype=float, copy=True) + ) + if ( + self.initial_qpos.shape != (len(self.joint_names),) + or not np.isfinite(self.initial_qpos).all() + ): + raise ValueError( + "Planning start must contain finite full-joint coordinates" + ) self.joint_limits = ( - robot.get_qpos_limits()[0].detach().cpu().double().numpy().copy() + robot.get_qpos_limits()[env_id].detach().cpu().double().numpy().copy() ) self._root = ( - robot.get_local_pose(to_matrix=True)[0].detach().cpu().double().numpy() + robot.get_local_pose(to_matrix=True)[env_id].detach().cpu().double().numpy() ) self._entities = {k: np.asarray(v) for k, v in entity_poses.items()} self._rng = np.random.default_rng(seed) @@ -380,7 +401,9 @@ def solutions( solver = self.robot.get_solver(name=target.control_part) live_root = ( self.robot.get_link_pose( - link_name=solver.root_link_name, env_ids=[0], to_matrix=True + link_name=solver.root_link_name, + env_ids=[self.env_id], + to_matrix=True, )[0] .detach() .cpu() @@ -397,7 +420,7 @@ def solutions( budget.check() samples = self.robot.sample_reachable_pose( name=target.control_part, - env_ids=[0], + env_ids=[self.env_id], num_samples=self.cfg.ik_seeds, max_attempts=self.cfg.ik_seeds, generator=torch.Generator(device=self.robot.device).manual_seed( @@ -434,7 +457,7 @@ def solutions( device=self.robot.device, ), name=target.control_part, - env_ids=[0], + env_ids=[self.env_id], return_all_solutions=True, ) budget.check() @@ -484,7 +507,7 @@ def solutions( q[None], dtype=torch.float32, device=self.robot.device ), name=target.control_part, - env_ids=[0], + env_ids=[self.env_id], to_matrix=True, )[0] .detach() diff --git a/embodichain/lab/sim/scene/feasibility/proposals.py b/embodichain/lab/sim/scene/feasibility/proposals.py index 06c01176da..5bf8281d82 100644 --- a/embodichain/lab/sim/scene/feasibility/proposals.py +++ b/embodichain/lab/sim/scene/feasibility/proposals.py @@ -37,6 +37,83 @@ __all__ = ["RobotWorkspaceProposalSource"] +def _workspace_fk_samples( + robot: Robot, + budget: FeasibilityBudget, + *, + control_part: str, + source: str, + sample_count: int, + seed: int, +) -> np.ndarray: + """Return owned current-arena FK poses from a bounded workspace population.""" + generator = torch.Generator(device=robot.device).manual_seed(seed) + budget.check() + if source == "cache": + workspace = robot.get_workspace(control_part) + settings = robot.cfg.workspace_cfg[control_part] + indices = workspace.sample_indices( + sample_count, + strategy=settings.strategy, + min_score=settings.min_score, + generator=generator, + ) + if ( + indices.shape != (sample_count,) + or indices.dtype != torch.int64 + or bool(((indices < 0) | (indices >= workspace.num_samples)).any()) + ): + raise ValueError("workspace sampler returned invalid cache indices") + qpos = workspace.qpos[indices].to(device=robot.device) + else: + limits = robot.get_qpos_limits(name=control_part)[0] + if ( + not bool(torch.isfinite(limits).all()) + or bool((limits[:, 1] < limits[:, 0]).any()) + or bool((limits.abs() > 1e6).any()) + ): + raise UnsupportedFeasibilityError( + "Joint-uniform workspace preparation requires explicit finite joint bounds" + ) + qpos = limits[:, 0] + torch.rand( + (sample_count, len(limits)), device=robot.device, generator=generator + ) * (limits[:, 1] - limits[:, 0]) + if ( + qpos.ndim != 2 + or len(qpos) != sample_count + or not bool(torch.isfinite(qpos).all()) + ): + raise ValueError( + "workspace proposals require finite selected joint configurations" + ) + poses = np.empty((sample_count, 4, 4), dtype=np.float64) + for start in range(0, sample_count, budget.cfg.batch_size): + budget.check() + stop = min(start + budget.cfg.batch_size, sample_count) + batch = robot.compute_batch_fk( + qpos=qpos[None, start:stop], + name=control_part, + env_ids=[0], + to_matrix=True, + ) + if batch.shape != (1, stop - start, 4, 4): + raise ValueError("workspace FK proposals must match the requested batch") + poses[start:stop] = batch[0].detach().cpu().numpy() + if not np.isfinite(poses).all(): + raise ValueError("workspace FK proposals must be finite rigid matrices") + rotations = poses[:, :3, :3] + if ( + not np.allclose(poses[:, 3, :], [0.0, 0.0, 0.0, 1.0], atol=1e-5, rtol=0) + or not np.allclose( + rotations.transpose(0, 2, 1) @ rotations, np.eye(3), atol=1e-5, rtol=0 + ) + or not np.allclose(np.linalg.det(rotations), 1.0, atol=1e-5, rtol=0) + ): + raise ValueError("workspace FK proposals must be proper rigid transforms") + budget.check() + return poses + + class RobotWorkspaceProposalSource: """Copy current-model FK samples into an independent numerical proposal prior. @@ -112,76 +189,16 @@ def __init__( np.linalg.inv(np.array(target.pose)), np.linalg.inv(np.array(variable.members[target.frame_uid])), ) - generator = torch.Generator(device=robot.device).manual_seed(budget.cfg.seed) - budget.check() - if control_part in (robot.cfg.workspace_cfg or {}): - workspace = robot.get_workspace(control_part) - settings = robot.cfg.workspace_cfg[control_part] - indices = workspace.sample_indices( - sample_count, - strategy=settings.strategy, - min_score=settings.min_score, - generator=generator, - ) - if ( - indices.shape != (sample_count,) - or indices.dtype != torch.int64 - or bool(((indices < 0) | (indices >= workspace.num_samples)).any()) - ): - raise ValueError("workspace sampler returned invalid cache indices") - qpos = workspace.qpos[indices].to(device=robot.device) - origin = "configured_qpos_cache" - else: - limits = robot.get_qpos_limits(name=control_part)[0] - if ( - not bool(torch.isfinite(limits).all()) - or bool((limits[:, 1] < limits[:, 0]).any()) - or bool((limits.abs() > 1e6).any()) - ): - raise UnsupportedFeasibilityError( - "Joint-uniform workspace preparation requires explicit finite joint bounds" - ) - qpos = limits[:, 0] + torch.rand( - (sample_count, len(limits)), device=robot.device, generator=generator - ) * (limits[:, 1] - limits[:, 0]) - origin = "joint_uniform_fk" - if ( - qpos.ndim != 2 - or len(qpos) != sample_count - or not bool(torch.isfinite(qpos).all()) - ): - raise ValueError( - "workspace proposals require finite selected joint configurations" - ) - batches = [] - for start in range(0, sample_count, budget.cfg.batch_size): - budget.check() - batch = ( - robot.compute_batch_fk( - qpos=qpos[None, start : start + budget.cfg.batch_size], - name=control_part, - env_ids=[0], - to_matrix=True, - )[0] - .detach() - .cpu() - .double() - .numpy() - .copy() - ) - batches.append(batch) - poses = np.concatenate(batches) - if poses.ndim != 3 or poses.shape[1:] != (4, 4) or not np.isfinite(poses).all(): - raise ValueError("workspace FK proposals must be finite rigid matrices") - rotations = poses[:, :3, :3] - if ( - not np.allclose(poses[:, 3, :], [0.0, 0.0, 0.0, 1.0], atol=1e-5, rtol=0) - or not np.allclose( - rotations.transpose(0, 2, 1) @ rotations, np.eye(3), atol=1e-5, rtol=0 - ) - or not np.allclose(np.linalg.det(rotations), 1.0, atol=1e-5, rtol=0) - ): - raise ValueError("workspace FK proposals must be proper rigid transforms") + has_cache = control_part in (robot.cfg.workspace_cfg or {}) + poses = _workspace_fk_samples( + robot, + budget, + control_part=control_part, + source="cache" if has_cache else "joint_uniform_fk", + sample_count=sample_count, + seed=budget.cfg.seed, + ) + origin = "configured_qpos_cache" if has_cache else "joint_uniform_fk" self._local = np.linalg.inv(self._root)[None] @ poses self._uniform_fraction = uniform_fraction self._rng = np.random.default_rng(budget.cfg.seed) diff --git a/embodichain/lab/sim/sensors/__init__.py b/embodichain/lab/sim/sensors/__init__.py index 94fc2e49a9..405e8b2bb4 100644 --- a/embodichain/lab/sim/sensors/__init__.py +++ b/embodichain/lab/sim/sensors/__init__.py @@ -32,6 +32,7 @@ ContactSensor, ContactSensorCfg, ArticulationContactFilterCfg, + ContactPermissionChecker, ) from .contact_grid import ContactGrid, ContactGridCfg from .contact_resultant import ContactResultant, ContactResultantCfg diff --git a/embodichain/lab/sim/sensors/contact/__init__.py b/embodichain/lab/sim/sensors/contact/__init__.py index 80a6e493ce..ed6121e479 100644 --- a/embodichain/lab/sim/sensors/contact/__init__.py +++ b/embodichain/lab/sim/sensors/contact/__init__.py @@ -48,10 +48,12 @@ from .history import ( ContactHistory, ) +from .permissions import ContactPermissionChecker __all__ = [ "ArticulationContactFilterCfg", "ContactSensor", + "ContactPermissionChecker", "ContactSensorCfg", "ContactSamplingObserver", "CONTACT_ACTOR_IDS", diff --git a/embodichain/lab/sim/sensors/contact/permissions.py b/embodichain/lab/sim/sensors/contact/permissions.py new file mode 100644 index 0000000000..5270daac22 --- /dev/null +++ b/embodichain/lab/sim/sensors/contact/permissions.py @@ -0,0 +1,209 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Device-side permission checks over complete native contact samples.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from typing import TYPE_CHECKING +import torch + +if TYPE_CHECKING: + from .sensor import ContactSensor + +__all__ = ["ContactPermissionChecker"] + + +class ContactPermissionChecker: + """Classify every valid measured contact without constructing Python pairs. + + Args: + sensor: Initialized native sensor with applied-impulse validity flags. + actor_frames: Explicit actor ID to frame name mapping for every row. + A global static actor may appear in several rows with the same name. + allowed_contacts: Structural and task-owned unordered frame exemptions. + moving_frames: Robot and dynamic frames requiring permission checks. + + This checker does not infer adjacency, change filters, step physics or select + a sampling frequency. A host requiring substep validation must invoke it at + every original physical sample. Unknown actors and unavailable impulses fail + closed; cumulative native/scatter loss invalidates every row conservatively. + """ + + def __init__( + self, + sensor: ContactSensor, + actor_frames: Sequence[Mapping[int, str]], + *, + allowed_contacts: Sequence[tuple[str, str]], + moving_frames: Sequence[str], + ) -> None: + self.sensor = sensor + self.device = torch.device(sensor.device) + self.rows = sensor.num_instances + if len(actor_frames) != self.rows or any(not row for row in actor_frames): + raise ValueError("actor_frames must declare every nonempty sensor row") + if any( + type(actor) is not int + or actor < 0 + or type(name) is not str + or not name.strip() + for row in actor_frames + for actor, name in row.items() + ): + raise ValueError( + "contact actors require nonnegative IDs and explicit frame names" + ) + names = sorted({name for row in actor_frames for name in row.values()}) + if any( + len(pair) != 2 or any(name not in names for name in pair) + for pair in allowed_contacts + ): + raise ValueError("contact permissions reference an absent frame") + if set(moving_frames) - set(names): + raise ValueError("moving_frames references an absent contact frame") + codes = {name: index for index, name in enumerate(names)} + actors = {} + for row, frames in enumerate(actor_frames): + for actor, name in frames.items(): + code, rows = actors.setdefault(actor, (codes[name], set())) + if code != codes[name]: + raise ValueError("contact actor has ambiguous frame identities") + rows.add(row) + actors = sorted(actors.items()) + allowed = {frozenset(pair) for pair in allowed_contacts} + self._actors = torch.tensor( + [actor for actor, _ in actors], dtype=torch.int32, device=self.device + ) + self._memberships = torch.tensor( + [ + [row in members for row in range(self.rows)] + for _, (_, members) in actors + ], + dtype=torch.bool, + device=self.device, + ) + self._codes = torch.tensor( + [code for _, (code, _) in actors], dtype=torch.long, device=self.device + ) + self._moving = torch.tensor( + [name in moving_frames for name in names], + dtype=torch.bool, + device=self.device, + ) + self._allowed = torch.tensor( + [[frozenset((a, b)) in allowed for b in names] for a in names], + dtype=torch.bool, + device=self.device, + ) + self._first = torch.empty(self.rows, dtype=torch.int32, device=self.device) + self._drop = torch.zeros(1, dtype=torch.int32, device=self.device) + self._output = torch.empty( + (self.rows, 3), dtype=torch.int32, device=self.device + ) + self._ready = torch.cuda.Event() if self.device.type == "cuda" else None + if self._ready is not None: + self._ready.record(torch.cuda.current_stream(self.device)) + self._actors_token = id(sensor._query.actors) + + def check(self) -> torch.Tensor: + """Return owned per-row ``(reason, actor0, actor1)`` integer summaries. + + Eager CUDA checks wait for the latest sensor sample on the caller's + stream without refreshing contacts or advancing histories. + + Returns: + Tensor ``(B, 3)`` on the sensor device. Reason zero passes; one means + unavailable applied impulse, two unknown/cross-row actor, three + forbidden moving contact, four dropped evidence. The earliest bad + buffer slot supplies actor IDs. No valid contact is subsampled. + + Raises: + RuntimeError: Sensor topology changed after permission binding. + """ + import warp as wp + from embodichain.compute.geometry._warp.contact_permissions import ( + classify_contacts, + contact_failures, + ) + + if self._ready is not None: + torch.cuda.current_stream(self.device).wait_event(self._ready) + # The checker event orders its own buffers, not the sensor's producer. + self.sensor._wait_for_sample() + data = self.sensor.get_data() + if id(self.sensor._query.actors) != self._actors_token: + raise RuntimeError( + "contact topology changed; bind a new permission checker" + ) + pairs = data["user_ids"] + self._first.fill_(pairs.shape[1] * 8) + self._drop.zero_() + # Use the current Torch stream on every call, rather than assuming that + # the stream used at construction is still the caller's producer. + stream = ( + wp.stream_from_torch(torch.cuda.current_stream(self.device)) + if self.device.type == "cuda" + else None + ) + wp.launch( + classify_contacts, + pairs.shape[:2], + inputs=[ + wp.from_torch(pairs), + wp.from_torch(data["is_valid"]), + wp.from_torch(data["impulse_valid"]), + wp.from_torch(self._actors), + wp.from_torch(self._memberships), + wp.from_torch(self._codes), + wp.from_torch(self._moving), + wp.from_torch(self._allowed), + wp.from_torch(self.sensor.dropped_contacts_mask), + wp.from_torch(self._first), + wp.from_torch(self._drop), + ], + device=str(self.device), + stream=stream, + ) + wp.launch( + contact_failures, + self.rows, + inputs=[ + wp.from_torch(pairs), + wp.from_torch(self._first), + wp.from_torch(self._drop), + wp.from_torch(self._output), + ], + device=str(self.device), + stream=stream, + ) + result = self._output.clone() + if self._ready is not None: + current = torch.cuda.current_stream(self.device) + self._ready.record(current) + for value in ( + self._actors, + self._memberships, + self._codes, + self._moving, + self._allowed, + self._first, + self._drop, + self._output, + ): + value.record_stream(current) + return result diff --git a/embodichain/lab/task_program/compiler/lowering.py b/embodichain/lab/task_program/compiler/lowering.py index c2d01e751f..da685143b0 100644 --- a/embodichain/lab/task_program/compiler/lowering.py +++ b/embodichain/lab/task_program/compiler/lowering.py @@ -2061,7 +2061,7 @@ def _lower_place( option_template = self._skill_option_template(analyzed, PlaceOptions) if call.at is not None: object_target = self._broadcast_pose( - call.at.to_matrix(), + self._ground_object_target(SemanticObjectTarget(pose=call.at), context), context, name="Place.at", ) diff --git a/embodichain/lab/task_program/integrations/_configured_services.py b/embodichain/lab/task_program/integrations/_configured_services.py index 7178646a50..cc290549c7 100644 --- a/embodichain/lab/task_program/integrations/_configured_services.py +++ b/embodichain/lab/task_program/integrations/_configured_services.py @@ -363,7 +363,9 @@ class _AntipodalGraspPoseGeneratorFactory: filter_ground_collision: bool | None = None force_refresh: bool | None = None - def validate_binding(self, robot: object, target_id: str) -> None: + def validate_binding( + self, robot: object, target_id: str, *, contact_frame_override: object = None + ) -> None: """Reject a static service snapshot bound to different physical hardware.""" import json @@ -381,7 +383,17 @@ def validate_binding(self, robot: object, target_id: str) -> None: expected_binding = { key: value for key, value in declaration.items() if key != "instance" } - if json.dumps(selected["declaration"], sort_keys=True) != json.dumps( + actual_binding = dict(selected["declaration"]) + if contact_frame_override is not None: + # Explicit workload-local sampling frames change the proposal + # convention, never the installed TCP, mesh or joint commands. + if json.dumps(contact_frame_override, sort_keys=True) != json.dumps( + expected.get("root_from_contact_frame"), sort_keys=True + ): + raise ValueError("Contact frame must match the declared grasp service.") + expected_binding.pop("root_from_contact_frame", None) + actual_binding.pop("root_from_contact_frame", None) + if json.dumps(actual_binding, sort_keys=True) != json.dumps( expected_binding, sort_keys=True ): raise ValueError( @@ -396,11 +408,19 @@ def validate_binding(self, robot: object, target_id: str) -> None: ) def physical_model(values): - return { + result = { key: value for key, value in values.items() if key not in {"min_opening_width", "max_opening_width"} } + if contact_frame_override is not None: + result.pop("root_from_contact_frame", None) + result["binding"] = { + key: value + for key, value in result.get("binding", {}).items() + if key != "root_from_contact_frame" + } + return result if json.dumps(physical_model(actual), sort_keys=True) != json.dumps( physical_model(expected), sort_keys=True diff --git a/embodichain/lab/task_program/integrations/_measured_batch.py b/embodichain/lab/task_program/integrations/_measured_batch.py new file mode 100644 index 0000000000..a63057a10f --- /dev/null +++ b/embodichain/lab/task_program/integrations/_measured_batch.py @@ -0,0 +1,133 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + + +"""Batch measured predicate inputs while retaining native event state machines.""" + +from __future__ import annotations + +from contextlib import contextmanager +import json + +import numpy as np + +__all__: list[str] = [] + + +class _MeasuredChecks: + """Read one packed physical boundary instead of synchronizing per predicate. + + Native monitors still own ordered events, consecutive counts, receipts and + terminal reports. Only their predicate arithmetic is shared across rows; + unsupported fast paths continue through the original native predicate. + The cache exists solely inside an explicit measured-boundary scope. + """ + + def __init__(self, env, configuration, monitors): + self.env, self.monitors = env, monitors + conditions = [*configuration["initial_checks"], *configuration["final_checks"]] + for event in configuration["events"]: + conditions.extend(event["conditions"]) + self.conditions = {self.key(c): c for c in conditions} + self.entities = tuple( + dict.fromkeys( + c[name] for c in conditions for name in ("entity", "other") if name in c + ) + ) + self.values = None + self.reads = 0 + + @staticmethod + def key(condition): + return json.dumps(condition, sort_keys=True, separators=(",", ":")) + + @contextmanager + def boundary(self): + import torch + + env = self.env + parts = [] + for uid in self.entities: + obj = env.sim.get_rigid_object(uid) + if obj is None: + raise ValueError("Predicate entity is absent from the physical scene") + parts.append( + torch.cat( + ( + obj.get_local_pose()[:, :3], + obj.body_state[:, 7:10].norm(dim=-1, keepdim=True), + obj.body_state[:, 10:13].norm(dim=-1, keepdim=True), + ), + dim=-1, + ) + ) + parts.append(env.robot.get_qpos()) + # One D2H boundary, independent of the number of predicates and rows. + packed = torch.cat(parts, dim=-1).detach().cpu().numpy() + self.reads += 1 + states = { + uid: packed[:, index * 5 : (index + 1) * 5] + for index, uid in enumerate(self.entities) + } + joints = packed[:, len(self.entities) * 5 :] + values = {} + for key, condition in self.conditions.items(): + kind = condition["kind"] + if kind == "joint_range": + index = env.robot.joint_names.index(condition["joint"]) + value = joints[:, index].astype(np.float64) + result = (value >= condition["lower"]) & (value <= condition["upper"]) + elif kind in ("pose_near", "lifted", "speed_below", "axis_relation"): + state = states[condition["entity"]] + position = state[:, :3].astype(np.float64) + if kind == "pose_near": + result = ( + np.linalg.norm(position - condition["position"], axis=-1) + <= condition["tolerance_m"] + ) + elif kind == "lifted": + initial = np.full(env.num_envs, np.nan) + for row, monitor in self.monitors.items(): + initial[row] = monitor.initial_heights.get( + condition["entity"], np.nan + ) + result = position[:, 2] - initial >= condition["height_m"] + elif kind == "axis_relation": + axis = condition["axis"] + delta = position[:, axis] - states[condition["other"]][:, axis] + result = (delta >= condition["lower"]) & ( + delta <= condition["upper"] + ) + else: + # Native scalar comparisons convert the float32 norms to + # Python floats before comparing authored thresholds. + result = ( + state[:, 3].astype(np.float64) <= condition["linear_m_s"] + ) & (state[:, 4].astype(np.float64) <= condition["angular_rad_s"]) + else: + continue + values[key] = ~result if condition.get("negate", False) else result + previous = { + row: getattr(monitor, "_batch_predicates", None) + for row, monitor in self.monitors.items() + } + for monitor in self.monitors.values(): + monitor._batch_predicates = values + try: + yield + finally: + for row, monitor in self.monitors.items(): + monitor._batch_predicates = previous[row] diff --git a/embodichain/lab/task_program/integrations/_native_plan_expansion.py b/embodichain/lab/task_program/integrations/_native_plan_expansion.py new file mode 100644 index 0000000000..66e3a72e27 --- /dev/null +++ b/embodichain/lab/task_program/integrations/_native_plan_expansion.py @@ -0,0 +1,455 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Native measured-plan branch of the shared same-grid expansion factory.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import hashlib +from pathlib import Path +from typing import Any + +__all__: list[str] = [] + + +@dataclass(frozen=True) +class TaskProgramNativeExpansionRecord: + """Owned measured-plan expansion provenance and hashed numeric attachment.""" + + env_id: int + payload: dict[str, Any] + + def __post_init__(self) -> None: + from embodichain.utils._owned_json import freeze_metadata + + object.__setattr__(self, "payload", freeze_metadata(self.payload)) + + def to_metadata(self) -> dict[str, Any]: + from embodichain.lab.gym.envs._json import json_safe_copy + + return { + "env_id": self.env_id, + **json_safe_copy(self.payload, field_name="expansion record"), + } + + +class _NativePlanExpansion: + """Select native grasp branches or adapt explicitly editable free phases.""" + + def __init__( + self, + operator: str, + options: dict, + *, + seed: int, + scene_case, + revision: str, + output: Path | None = None, + episode_id: int = 0, + ): + if operator not in ( + "object_pose", + "affordance", + "joint_residual", + "via_points", + "nullspace_residual", + ): + raise ValueError("unsupported native-plan expansion operator") + self.operator = operator + from embodichain.lab.gym.envs._json import json_safe_copy + + self.options = json_safe_copy(options, field_name="same-grid options") + self.seed = seed + self.scene_case, self.revision = scene_case, revision + self.output = output + self.episode_id = episode_id + self._records: list[TaskProgramNativeExpansionRecord] = [] + + @property + def records(self) -> tuple[TaskProgramNativeExpansionRecord, ...]: + return tuple(self._records) + + def create_plan_transform(self, request, *, engine): + if self.operator == "object_pose": + return None + if self.operator == "affordance": + if ( + request.analysis_call_index + not in self.options["affordance"]["call_indices"] + ): + return None + + def record(resolved, context, plan): + return self._record_affordance(request, context, plan) + + return record + + calls = self.options.get("call_indices") + if calls is not None and request.analysis_call_index not in calls: + return None + + def transform(resolved, context, plan): + return self._transform(request, engine, resolved, context, plan) + + return transform + + def prepare_planning_context(self, request, context, *, engine): + """Select a non-nominal native Affordance branch before planning.""" + if self.operator != "affordance": + return context + settings = self.options["affordance"] + if request.analysis_call_index not in settings["call_indices"]: + return context + if context.batch_size != 1: + raise ValueError("Native affordance expansion requires batch_size=1") + from embodichain.lab.sim.atomic_actions.affordance_sampling import ( + AffordanceSamplingContext, + ) + + return replace( + context, + affordance_sampling=AffordanceSamplingContext( + count=settings["branches"], + seed=self.seed, + episode_id=self.episode_id, + branch_override=1 + self.episode_id % (settings["branches"] - 1), + ), + ) + + def _record_affordance(self, call, context, plan): + """Require distinct native grasp geometry, retaining physical validation.""" + if not bool(plan.plan_success.all()): + return plan + samples = plan.diagnostics.metadata.get("affordance_sample", {}) + sampling = context.affordance_sampling + branch = sampling.branch_override + distinct = any( + sample.get("success") == [True] + and sample.get("unique_candidate_counts", [0])[0] > branch + and sample.get("reused") == [False] + for sample in samples.values() + ) + if not distinct: + raise ValueError("affordance produced no distinct feasible grasp branch") + evidence = { + "operator": "affordance", + "skill": plan.skill_id, + "workflow_call_index": call.analysis_call_index, + "sampling": sampling.metadata(), + "samples": samples, + "mode": "native_affordance_replanning", + } + if self.output is not None: + import numpy as np + from embodichain.lab.expansion.persistence import _digest as file_digest + + self.output.mkdir(parents=True, exist_ok=True) + path = ( + self.output + / f"call-{call.analysis_call_index}-{len(self._records)}.npz" + ) + with path.open("wb") as stream: + np.savez_compressed( + stream, + candidate=plan.joint_trajectory.positions[0].detach().cpu().numpy(), + dt=plan.joint_trajectory.dt[0].detach().cpu().numpy(), + ) + evidence["plan_artifact"] = {"path": str(path), "sha256": file_digest(path)} + metadata = dict(plan.diagnostics.metadata, expansion=evidence) + self._records.append( + TaskProgramNativeExpansionRecord(int(context.env_ids[0]), evidence) + ) + return replace(plan, diagnostics=replace(plan.diagnostics, metadata=metadata)) + + def _transform(self, call, engine, request, context, plan): + import torch + from embodichain.lab.sim.atomic_actions.plans import TimedTrajectory + from embodichain.lab.sim.atomic_actions.trajectory_adapter import ( + ActionPlanTemplateAdapter, + ) + from embodichain.lab.sim.motion.expansion import SourceContext + from embodichain.lab.sim.motion.expansion.operators import ( + ProposalRejected, + joint_residual, + nullspace_residual, + validate_motion_samples, + via_points, + ) + + free = set(self.options["free_phases"].get(plan.skill_id, ())) + if ( + not free + or plan.joint_trajectory is None + or not bool(plan.plan_success.all()) + ): + return plan + if context.batch_size != 1: + raise ValueError("Native same-grid transforms require batch_size=1") + endpoints = tuple( + e for e in request.binding.endpoints if e.endpoint_id == "motion" + ) + motion_ids = tuple(dict.fromkeys(i for e in endpoints for i in e.joint_ids)) + permissions = { + segment.name: (self.operator,) if segment.name in free else () + for segment in plan.segments + } + adapter = ActionPlanTemplateAdapter( + tuple(engine.robot.joint_names), + permissions, + { + name: "free" if value else "contact" + for name, value in permissions.items() + }, + controlled_joint_indices=motion_ids, + ) + source = SourceContext( + self.scene_case.task_id, + self.revision, + f"{call.workflow_id}:{call.analysis_call_index}", + self.scene_case, + context.require_control_dt(), + ) + template = adapter.export_template(plan, context=source) + reference_phases = template.phases + # Keep the first/last two samples, including discrete derivative stencils. + phases = tuple( + ( + replace(p, start_index=p.start_index + 1, stop_index=p.stop_index - 1) + if p.allowed_operators and p.stop_index - p.start_index >= 5 + else replace(p, allowed_operators=()) + ) + for p in template.phases + ) + if not any(p.allowed_operators for p in phases): + return plan + template = replace(template, phases=phases) + limits = engine.robot.get_qpos_limits().detach().cpu() + if limits.ndim == 3: + limits = limits[int(context.env_ids[0])] + active = list(template.controlled_joint_indices) + velocity = engine.robot.get_qvel_limits().detach().cpu() + if velocity.ndim == 2: + velocity = velocity[int(context.env_ids[0])] + velocity = velocity[active].clamp(max=self.options["velocity_limit"]) + acceleration = torch.full_like(velocity, self.options["acceleration_limit"]) + seed = int.from_bytes( + hashlib.sha256( + f"{self.seed}/{source.unit_id}/{self.operator}".encode() + ).digest()[:8], + "little", + ) % (2**63) + generator = torch.Generator().manual_seed(seed) + jacobians, kinematics = self._kinematics(engine.robot, endpoints, template) + + def motion_checks(trajectory): + checks = [] + for phase in phases: + if not phase.allowed_operators: + continue + # Operators keep their endpoints exact. The narrowed phase + # leaves two original samples at each boundary, covering every + # second difference affected by an edit. + start, stop = max(0, phase.start_index - 1), min( + len(trajectory.dt), phase.stop_index + 1 + ) + intervals = trajectory.dt[start:stop].clone() + intervals[0] = 0 + checks.append( + validate_motion_samples( + trajectory.positions[start:stop, active], + intervals, + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + ) + return checks + + reference_checks = motion_checks(template) + if any(not check.accepted for check in reference_checks): + raise ValueError( + f"Reference free phases already exceed sampled motion bounds: " + f"{reference_checks}" + ) + reason = "no distinct candidate" + for attempt in range(self.options["max_proposals"]): + arguments = dict( + joint_limits=limits, + normalized_scale=self.options["joint_offset_scale"], + generator=generator, + ) + try: + if self.operator == "joint_residual": + candidate = joint_residual(template, **arguments) + elif self.operator == "via_points": + candidate = via_points( + template, via_count=self.options["via_count"], **arguments + ) + elif self.operator == "nullspace_residual": + candidate = nullspace_residual( + template, task_jacobians=jacobians, **arguments + ) + else: + raise ValueError(f"Unsupported operator: {self.operator}") + except ProposalRejected as exc: + reason = str(exc) + continue + delta = float((candidate.positions - template.positions).abs().max()) + if delta < self.options["minimum_joint_delta"]: + continue + checks = motion_checks(candidate) + if any(not check.accepted for check in checks): + reason = f"edited-phase motion limits: {checks}" + continue + fk = self._check_fk(template, candidate, kinematics) + if self.operator == "nullspace_residual" and ( + fk["max_translation_m"] + > self.options["nullspace_translation_tolerance_m"] + or fk["max_rotation_rad"] + > self.options["nullspace_rotation_tolerance_rad"] + ): + reason = f"nullspace FK error: {fk}" + continue + positions = adapter.restore_positions(plan, candidate.positions).unsqueeze( + 0 + ) + metadata = dict(plan.diagnostics.metadata) + evidence = { + "operator": self.operator, + "skill": plan.skill_id, + "workflow_call_index": call.analysis_call_index, + "phases": [p.phase_id for p in phases if p.allowed_operators], + "reference_phases": [ + {"name": p.phase_id, "start": p.start_index, "stop": p.stop_index} + for p in reference_phases + ], + "controlled_joint_names": [template.joint_names[i] for i in active], + "task_rows": self.options["nullspace_task_rows"], + "max_joint_delta": delta, + "proposal": attempt, + "same_time_grid": True, + "motion_validation_scope": "edited_phases_and_boundary_stencils", + "motion": { + "reference": [ + dict(c.metrics) for r in reference_checks for c in r.checks + ], + "candidate": [dict(c.metrics) for r in checks for c in r.checks], + }, + "fk": fk, + } + if self.output is not None: + import numpy as np + from embodichain.lab.expansion.persistence import _digest as file_digest + + self.output.mkdir(parents=True, exist_ok=True) + path = ( + self.output + / f"call-{call.analysis_call_index}-{len(self._records)}.npz" + ) + editable = torch.zeros_like(template.positions, dtype=torch.bool) + for phase in phases: + if phase.allowed_operators: + editable[phase.start_index : phase.stop_index, active] = True + with path.open("wb") as stream: + np.savez_compressed( + stream, + reference=template.positions.numpy(), + candidate=candidate.positions.numpy(), + dt=template.dt.numpy(), + editable=editable.numpy(), + ) + evidence["plan_artifact"] = { + "path": str(path), + "sha256": file_digest(path), + } + metadata.update( + expansion=evidence, + collision_evidence="unverified", + collision_scope="not_checked", + ) + rebuilt = engine.rebuild_plan_from_trajectory( + request, + context, + replace(plan, diagnostics=replace(plan.diagnostics, metadata=metadata)), + TimedTrajectory.from_positions( + positions, + env_ids=context.env_ids, + dt=plan.joint_trajectory.dt.clone(), + ), + ) + self._records.append( + TaskProgramNativeExpansionRecord(int(context.env_ids[0]), evidence) + ) + return rebuilt + raise ValueError( + f"{self.operator} produced no qualified {plan.skill_id} variant: {reason}" + ) + + def _kinematics(self, robot, endpoints, template): + import torch + + active = list(template.controlled_joint_indices) + blocks, models = [], [] + for endpoint in endpoints: + indices = list(endpoint.joint_ids) + if not set(indices) & set(active): + continue + solver = robot.get_solver(endpoint.target.control_part) + if solver is None or solver.pk_serial_chain is None: + raise ValueError( + "Trajectory augmentation requires the actual robot kinematic chain" + ) + chain_ids = [ + robot.joint_names.index(n) + for n in solver.pk_serial_chain.get_joint_parameter_names() + ] + qpos = template.positions[:, chain_ids] + rows = self.options["nullspace_task_rows"] + jacobian = ( + solver.get_jacobian( + qpos, + locations=torch.as_tensor(solver.tcp_xpos)[:3, 3], + ) + .detach() + .cpu()[:, rows, :] + ) + block = torch.zeros(len(qpos), len(rows), len(active), dtype=jacobian.dtype) + for column, joint in enumerate(chain_ids): + if joint in active: + block[:, :, active.index(joint)] = jacobian[:, :, column] + blocks.append(block) + models.append((solver, chain_ids)) + if not blocks: + raise ValueError("No kinematic chain covers the edited motion joints") + return torch.cat(blocks, dim=1), models + + @staticmethod + def _check_fk(reference, candidate, models): + import torch + + translation, rotation = 0.0, 0.0 + for solver, indices in models: + a = solver.get_fk(reference.positions[:, indices]).detach().cpu().double() + b = solver.get_fk(candidate.positions[:, indices]).detach().cpu().double() + translation = max( + translation, float((a[:, :3, 3] - b[:, :3, 3]).norm(dim=-1).max()) + ) + relative = a[:, :3, :3].transpose(-1, -2) @ b[:, :3, :3] + cosine = ((relative.diagonal(dim1=-2, dim2=-1).sum(-1) - 1) / 2).clamp( + -1, 1 + ) + rotation = max(rotation, float(torch.acos(cosine).max())) + return {"max_translation_m": translation, "max_rotation_rad": rotation} diff --git a/embodichain/lab/task_program/integrations/catalog.py b/embodichain/lab/task_program/integrations/catalog.py index aa27214dbe..5ddea4c5cc 100644 --- a/embodichain/lab/task_program/integrations/catalog.py +++ b/embodichain/lab/task_program/integrations/catalog.py @@ -236,8 +236,12 @@ def _canonical_value(value: object) -> object: and not value.supports_segment_motion_recovery ): metadata.pop("supports_segment_motion_recovery") - if type(value) is MotionPolicy and value.final_velocity_rejection == "hold": - metadata.pop("final_velocity_rejection") + if type(value) is MotionPolicy: + if value.final_velocity_rejection == "hold": + metadata.pop("final_velocity_rejection") + # An inactive phase-path extension preserves existing registration seals. + if not value.phase_paths: + metadata.pop("phase_paths") if type(value) is RecoveryPolicy and value.retry_all_failed_plans is True: metadata.pop("retry_all_failed_plans") if isinstance(value, PickUpOptions) and value.initial_lift_height == 0.0: @@ -248,6 +252,8 @@ def _canonical_value(value: object) -> object: metadata.pop("grasp_retract_distance") if isinstance(value, PlaceOptions) and value.cartesian_approach is False: metadata.pop("cartesian_approach") + if isinstance(value, PlaceOptions) and value.release_settle_duration == 0.0: + metadata.pop("release_settle_duration") if isinstance(value, SlideOptions) and value.push_approach_offset is None: metadata.pop("push_approach_offset") return {"type": _qualified_name(value), "fields": metadata} diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index f65c2691b3..c5afef5008 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -1088,6 +1088,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: "object_motion_keyframes", "grasp_settle_steps", "release_settle_steps", + "release_settle_duration", "pick_object_part", "lift_height", "pre_grasp_distance", @@ -1288,6 +1289,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: { "hand_interp_steps", "release_settle_steps", + "release_settle_duration", "lift_height", "max_approach_retract_z", "cartesian_waypoint_count", @@ -1313,6 +1315,12 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: path=f"{path}.release_settle_steps", minimum=0, ) + if "release_settle_duration" in config: + kwargs["release_settle_duration"] = _real( + config["release_settle_duration"], + path=f"{path}.release_settle_duration", + minimum=0.0, + ) if "lift_height" in config: kwargs["lift_height"] = _real( config["lift_height"], @@ -1792,6 +1800,7 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: "plan_opts", "interpolation_mode", "ik_seed_mode", + "phase_paths", "sample_count_retries", "segment_sample_count_retries", "segment_continuous_ik_recovery", @@ -1804,6 +1813,7 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: config["sample_count"], path=f"{path}.sample_count", minimum=2 ) kwargs: dict[str, object] = { + "phase_paths": dict(config.get("phase_paths", {})), "strategy": _identifier( config.get("strategy", "ik_interp"), path=f"{path}.strategy", @@ -3503,6 +3513,8 @@ def registration(self) -> SimulationTaskProgramRegistration: def create_adapter( self, environment: object, + *, + grasp_contact_frames: Mapping[str, object] | None = None, ) -> TaskProgramEnvironmentAdapter: """Create a live adapter for an initialized environment.""" from .demo_skill import _ConfiguredDemoSkillSource @@ -3513,7 +3525,9 @@ def create_adapter( ): if getattr(environment, "num_envs", None) != 1: raise ValueError("configured Demo Skills require num_envs == 1") - adapter = self.delegate.create_adapter(environment) + adapter = self.delegate.create_adapter( + environment, grasp_contact_frames=grasp_contact_frames + ) adapter._set_configured_nominal_sources(self.nominal_motion_sources) return adapter diff --git a/embodichain/lab/task_program/integrations/expansion.py b/embodichain/lab/task_program/integrations/expansion.py index 543c680ff6..7c62629306 100644 --- a/embodichain/lab/task_program/integrations/expansion.py +++ b/embodichain/lab/task_program/integrations/expansion.py @@ -24,6 +24,7 @@ import hashlib from collections.abc import Callable, Mapping, Sequence import math +from pathlib import Path from typing import ClassVar import torch @@ -54,6 +55,7 @@ ) from embodichain.lab.sim.atomic_skills.state import PlanningContext from embodichain.lab.task_program.runtime.expansion import TaskProgramPlanRequest +from ._native_plan_expansion import TaskProgramNativeExpansionRecord __all__ = [ "TaskProgramSourceAdapter", @@ -63,6 +65,7 @@ "TaskProgramMappedRetimeRecord", "TaskProgramSameGridExpansionFactory", "TaskProgramSameGridExpansionRecord", + "TaskProgramNativeExpansionRecord", ] @@ -193,6 +196,63 @@ class TaskProgramSameGridExpansionFactory: still pass the unchanged full-path motion and collision checks. """ + @classmethod + def from_native_plan( + cls, + operator: str, + options: Mapping[str, object], + *, + seed: int, + scene_case: SceneCase, + revision: str, + output: Path | None = None, + episode_id: int = 0, + ) -> TaskProgramSameGridExpansionFactory: + """Expand live Atomic plans on their existing measured control grid. + + Args: + operator: object_pose, affordance, joint_residual, via_points or nullspace_residual. + options: Task-owned phase allowlists, call selection, native operator + magnitudes, sampled motion/FK limits and proposal budget. + seed: Logical candidate seed, independent of physical row. + scene_case: Measured scene and initial-state identity. + revision: Pinned task/source revision. + output: Optional directory for hashed reference/candidate attachments. + episode_id: Logical affordance branch identity. + + This source path retains native operators and Atomic reconstruction, + protects phase-boundary derivative stencils and never claims physical + success. The host must still perform measured collision/task acceptance. + """ + from ._native_plan_expansion import _NativePlanExpansion + + instance = object.__new__(cls) + instance._native_delegate = _NativePlanExpansion( + operator, + options, + seed=seed, + scene_case=scene_case, + revision=revision, + output=output, + episode_id=episode_id, + ) + return instance + + def prepare_planning_context( + self, + request: TaskProgramPlanRequest, + context: PlanningContext, + *, + engine: AtomicSkillEngine, + ) -> PlanningContext: + """Bind optional native affordance sampling before the ordinary planner.""" + delegate = getattr(self, "_native_delegate", None) + return ( + context + if delegate is None + else delegate.prepare_planning_context(request, context, engine=engine) + ) + def __init__( self, *, @@ -306,8 +366,15 @@ def __init__( self._records: list[TaskProgramSameGridExpansionRecord] = [] @property - def records(self) -> tuple[TaskProgramSameGridExpansionRecord, ...]: + def records( + self, + ) -> tuple[ + TaskProgramSameGridExpansionRecord | TaskProgramNativeExpansionRecord, ... + ]: """Return successfully rebuilt call-level candidates.""" + delegate = getattr(self, "_native_delegate", None) + if delegate is not None: + return delegate.records return tuple( replace(record, template=replace(record.template)) for record in self._records @@ -317,6 +384,9 @@ def create_plan_transform( self, request: TaskProgramPlanRequest, *, engine: AtomicSkillEngine ) -> PlanTransform | None: """Select the spatial transform for one compiled call occurrence.""" + delegate = getattr(self, "_native_delegate", None) + if delegate is not None: + return delegate.create_plan_transform(request, engine=engine) if not isinstance(request, TaskProgramPlanRequest) or not isinstance( engine, AtomicSkillEngine ): @@ -867,6 +937,10 @@ def _combined_augmentation_payload( } }, } + if spatial.joint_samples: + payload["factors"]["spatial"]["joint_samples"] = spatial.joint_samples + if spatial.knot_samples: + payload["factors"]["spatial"]["knot_samples"] = spatial.knot_samples if spatial.dedup_tolerance is not None: payload["coverage"] = { "joint_dedup_normalized_tol": spatial.dedup_tolerance, diff --git a/embodichain/lab/task_program/integrations/feasibility.py b/embodichain/lab/task_program/integrations/feasibility.py index f8887fe207..279b2e5f3b 100644 --- a/embodichain/lab/task_program/integrations/feasibility.py +++ b/embodichain/lab/task_program/integrations/feasibility.py @@ -23,6 +23,8 @@ from pathlib import Path from typing import Any +import math +import json import numpy as np from scipy.spatial.transform import Rotation @@ -91,6 +93,14 @@ def __init__( self.events.append((data["event_id"], conditions, steps)) if len({e[0] for e in self.events}) != len(self.events): raise ValueError("event IDs must be unique") + event_conditions = ( + condition for _, conditions, _ in self.events for condition in conditions + ) + self._transport_guards = { + json.dumps(condition, sort_keys=True): condition + for condition in (*self.final_checks, *event_conditions) + if condition["kind"] == "transport_guard" + } self.initial_heights = {} self._index = 0 self._steps = 0 @@ -98,12 +108,28 @@ def __init__( self._previous = [] self._order_ok = True self.receipts = [] + self._transport = {} @staticmethod def _validate(value: Mapping[str, object]) -> dict: kinds = { "pose_near": {"entity", "position", "tolerance_m"}, + "pose_relative": { + "entity", + "other", + "position_tolerance", + "yaw_tolerance_rad", + "yaw_period_rad", + "tilt_tolerance_rad", + }, "lifted": {"entity", "height_m"}, + "transport_guard": { + "entity", + "other", + "lift_height_m", + "minimum_transit_height_m", + "goal_tolerance_m", + }, "joint_range": {"joint", "lower", "upper"}, "speed_below": {"entity", "linear_m_s", "angular_rad_s"}, "axis_relation": {"entity", "other", "axis", "lower", "upper"}, @@ -114,7 +140,13 @@ def _validate(value: Mapping[str, object]) -> dict: "Unsupported measured rigid-task predicate" ) kind = value["kind"] - data = _closed(value, kinds[kind] | {"kind", "negate"}, "predicate") + data = _closed( + value, + kinds[kind] + | {"kind", "negate"} + | ({"position_offset"} if kind == "pose_relative" else set()), + "predicate", + ) if ( not kinds[kind] <= data.keys() or type(data.get("negate", False)) is not bool @@ -153,6 +185,51 @@ def _validate(value: Mapping[str, object]) -> dict: raise ValueError( "Axis relation requires an arena axis and finite ordered bounds" ) + if kind == "pose_relative": + offset = np.asarray(data.get("position_offset", (0, 0, 0)), dtype=float) + if offset.shape != (3,) or not np.isfinite(offset).all(): + raise ValueError("position_offset must have three finite values") + tolerance = np.asarray(data["position_tolerance"], dtype=float) + if ( + tolerance.shape != (3,) + or not np.isfinite(tolerance).all() + or np.any(tolerance <= 0) + ): + raise ValueError( + "position_tolerance must contain three positive finite values" + ) + for key in ("yaw_tolerance_rad", "yaw_period_rad", "tilt_tolerance_rad"): + if ( + type(data[key]) not in (int, float) + or not math.isfinite(data[key]) + or data[key] <= 0 + ): + raise ValueError(f"{key} must be finite and positive") + if data["yaw_tolerance_rad"] > data[ + "yaw_period_rad" + ] / 2 or not math.isclose( + 2 * math.pi / data["yaw_period_rad"], + round(2 * math.pi / data["yaw_period_rad"]), + ): + raise ValueError( + "Yaw symmetry must divide a full turn and tolerance cannot exceed half a period" + ) + if kind == "transport_guard": + for key in ( + "lift_height_m", + "minimum_transit_height_m", + "goal_tolerance_m", + ): + if ( + type(data[key]) not in (int, float) + or not math.isfinite(data[key]) + or data[key] <= 0 + ): + raise ValueError(f"{key} must be finite and positive") + if data["minimum_transit_height_m"] >= data["lift_height_m"]: + raise ValueError( + "Transit threshold must be below the verified lift height" + ) return data def reset(self, env: Any) -> None: @@ -166,6 +243,7 @@ def reset(self, env: Any) -> None: self._previous = [False] * len(self.events) self._order_ok = True self.receipts = [] + self._transport = {} def predicate(self, env: Any, condition: Mapping[str, object]) -> bool: """Read one explicitly declared condition from current physical state. @@ -176,7 +254,20 @@ def predicate(self, env: Any, condition: Mapping[str, object]) -> bool: Returns: Physical truth value; unknown entities/joints are configuration errors. """ + cached = getattr(self, "_batch_predicates", None) + if cached is not None: + from ._measured_batch import _MeasuredChecks + + key = _MeasuredChecks.key(condition) + if key in cached: + return bool(cached[key][self.env_id]) kind = condition["kind"] + if kind == "transport_guard": + lifted, dropped = self._transport.get( + json.dumps(dict(condition), sort_keys=True), (False, False) + ) + passed = lifted and not dropped + return bool(not passed if condition.get("negate", False) else passed) if kind == "joint_range": index = env.robot.joint_names.index(condition["joint"]) value = float(env.robot.get_qpos()[self.env_id, index]) @@ -188,7 +279,40 @@ def predicate(self, env: Any, condition: Mapping[str, object]) -> bool: position = ( obj.get_local_pose()[self.env_id, :3].detach().cpu().double().numpy() ) - if kind == "pose_near": + if kind == "pose_relative": + other = env.sim.get_rigid_object(condition["other"]) + if other is None: + raise ValueError("Relative target entity is absent") + actual = ( + obj.get_local_pose(to_matrix=True)[self.env_id] + .detach() + .cpu() + .double() + .numpy() + ) + target = ( + other.get_local_pose(to_matrix=True)[self.env_id] + .detach() + .cpu() + .double() + .numpy() + ) + delta = actual[:3, 3] - ( + target[:3, 3] + + target[:3, :3] + @ np.asarray(condition.get("position_offset", (0, 0, 0))) + ) + relative = target[:3, :3].T @ actual[:3, :3] + yaw = math.atan2(relative[1, 0], relative[0, 0]) + period = condition["yaw_period_rad"] + yaw_error = abs((yaw + period / 2) % period - period / 2) + tilt = math.acos(float(np.clip(relative[2, 2], -1, 1))) + passed = ( + bool(np.all(np.abs(delta) <= condition["position_tolerance"])) + and yaw_error <= condition["yaw_tolerance_rad"] + and tilt <= condition["tilt_tolerance_rad"] + ) + elif kind == "pose_near": passed = ( np.linalg.norm(position - condition["position"]) <= condition["tolerance_m"] @@ -274,9 +398,72 @@ def predicate(self, env: Any, condition: Mapping[str, object]) -> bool: ) return bool(not passed if condition.get("negate", False) else passed) + @staticmethod + def batch_scope( + env: Any, + monitors: Sequence[MeasuredTaskMonitor], + *, + extra_conditions: Sequence[Mapping[str, object]] = (), + ) -> object: + """Share one measured numeric boundary across independent event monitors. + + Args: + env: Live environment or complete measured-state facade. + monitors: Distinct physical-row monitors, already reset for this episode. + extra_conditions: Additional validated initial checks to read in this scope. + + Returns: + Context manager. Inside it, predicate arithmetic uses one packed + readback; ordered events, counters and reports still belong to each + native monitor. Unsupported fast paths keep the scalar predicate. + No cached physical values survive the scope. + """ + from ._measured_batch import _MeasuredChecks + + selected = {monitor.env_id: monitor for monitor in monitors} + if not selected or len(selected) != len(monitors): + raise ValueError( + "batch_scope requires unique nonempty physical-row monitors" + ) + if any(not 0 <= row < env.num_envs for row in selected): + raise ValueError("monitor row is outside the measured environment") + first = next(iter(selected.values())) + conditions = [first._validate(c) for c in extra_conditions] + final, events = [], [] + for monitor in selected.values(): + final.extend(monitor.final_checks) + events.extend({"conditions": values} for _, values, _ in monitor.events) + return _MeasuredChecks( + env, + {"initial_checks": conditions, "final_checks": final, "events": events}, + selected, + ).boundary() + def observe(self, env: Any, action: Any) -> None: """Record consecutive event predicates at actual control boundaries.""" self._steps += 1 + for key, condition in self._transport_guards.items(): + lifted, dropped = self._transport.get(key, (False, False)) + pose = env.sim.get_rigid_object(condition["entity"]).get_local_pose()[ + self.env_id, :3 + ] + goal = env.sim.get_rigid_object(condition["other"]).get_local_pose()[ + self.env_id, :3 + ] + if ( + not np.isfinite(pose.detach().cpu().numpy()).all() + or not np.isfinite(goal.detach().cpu().numpy()).all() + ): + self._transport[key] = (lifted, True) + continue + height = float(pose[2]) - self.initial_heights[condition["entity"]] + lifted = lifted or height >= condition["lift_height_m"] + dropped = dropped or ( + lifted + and height < condition["minimum_transit_height_m"] + and float((pose[:2] - goal[:2]).norm()) > condition["goal_tolerance_m"] + ) + self._transport[key] = (lifted, dropped) expected = self._index for i, (name, conditions, required) in enumerate(self.events): current = all(self.predicate(env, c) for c in conditions) diff --git a/embodichain/lab/task_program/integrations/simulation/environment.py b/embodichain/lab/task_program/integrations/simulation/environment.py index dfee0f64d9..064b4e2b2d 100644 --- a/embodichain/lab/task_program/integrations/simulation/environment.py +++ b/embodichain/lab/task_program/integrations/simulation/environment.py @@ -1529,16 +1529,44 @@ def registration(self) -> SimulationTaskProgramRegistration: def create_adapter( self, environment: object, + *, + grasp_contact_frames: Mapping[str, object] | None = None, ) -> TaskProgramEnvironmentAdapter: - """Create fresh grasp services and bind the initialized environment.""" + """Create fresh grasp services and bind the initialized environment. + + Args: + environment: Sole owner of the actual robot and physical World. + grasp_contact_frames: Explicit per-control-part sampling conventions + matching this factory's declared grasp services. Only these + contact frames may differ from the installed binding; TCP, + physical meshes, joint commands and all other calibration stay + strictly checked. The live robot configuration is never mutated. + + Returns: + Fresh adapter with independent grasp and execution services. + """ from embodichain.toolkits.graspkit import GraspPoseGenerator self._registration.assert_unchanged() + frames = dict(grasp_contact_frames or {}) + if set(frames) - self._grasp_pose_generator_factories.keys(): + raise ValueError("Contact frame overrides require declared grasp services.") generators: dict[str, GraspPoseGenerator] = {} for target_id, factory in self._grasp_pose_generator_factories.items(): validator = getattr(factory, "validate_binding", None) if validator is not None: - validator(environment.robot, target_id) + if target_id in frames: + validator( + environment.robot, + target_id, + contact_frame_override=frames[target_id], + ) + else: + validator(environment.robot, target_id) + elif target_id in frames: + raise ValueError( + "Grasp service cannot validate a contact frame override." + ) generator = factory() if not isinstance(generator, GraspPoseGenerator): raise TypeError( diff --git a/embodichain/lab/task_program/integrations/simulation/policies.py b/embodichain/lab/task_program/integrations/simulation/policies.py index 66ec1937b7..3e92e7ce45 100644 --- a/embodichain/lab/task_program/integrations/simulation/policies.py +++ b/embodichain/lab/task_program/integrations/simulation/policies.py @@ -735,6 +735,7 @@ def _hold_actions(self, policy, *, segment, active_mask): context, attached, previous_target = self._hold_observer( cfg.object, accepted ) + attached = attached.to(device=active_mask.device) self.validate_observation_scope(clock) if clock.step_index != start + tick: raise ValueError("Hold observation changed its execution clock.") @@ -760,6 +761,7 @@ def _hold_actions(self, policy, *, segment, active_mask): name="hold.at", ) ) + target = target.to(device=active_mask.device) pose = context.scene.entities[cfg.object].pose.to(target) if pose.ndim == 2: pose = pose.unsqueeze(0).expand(active_mask.numel(), -1, -1) @@ -1912,7 +1914,14 @@ def _measure_pose_delta_settle_target( target.native_entity, entity_id=target.canonical_id, ) - pose = pose.index_select(0, row_indices.to(pose.device)) + # _read_pose already selects this port's bound physical rows. Convert + # the requested World IDs to local offsets instead of selecting twice + # with the original IDs (for example row 7 in a B=1 policy port). + matches = row_indices[:, None] == self._row_indices[None, :] + if not bool((matches.sum(dim=1) == 1).all()): + raise ValueError("Pose-delta settling requested an unbound physical row") + local = matches.long().argmax(dim=1).to(pose.device) + pose = pose.index_select(0, local) row_count = row_indices.numel() if previous_pose is None: speed = torch.full( diff --git a/embodichain/lab/task_program/runtime/results.py b/embodichain/lab/task_program/runtime/results.py index 61d288c669..4851fc16b3 100644 --- a/embodichain/lab/task_program/runtime/results.py +++ b/embodichain/lab/task_program/runtime/results.py @@ -423,6 +423,7 @@ def _motion_policy_to_metadata(policy: MotionPolicy) -> dict[str, object]: "velocity_targets": policy.velocity_targets, "interpolation_mode": policy.interpolation_mode, "ik_seed_mode": policy.ik_seed_mode, + **({"phase_paths": dict(policy.phase_paths)} if policy.phase_paths else {}), "sample_count_retries": list(policy.sample_count_retries), "require_final_velocity": policy.require_final_velocity, "final_velocity_rejection": policy.final_velocity_rejection, diff --git a/embodichain/lab/visualization/markers/__init__.py b/embodichain/lab/visualization/markers/__init__.py index 2f65927085..4172846aaf 100644 --- a/embodichain/lab/visualization/markers/__init__.py +++ b/embodichain/lab/visualization/markers/__init__.py @@ -22,6 +22,7 @@ from .group import MarkerGroup from .visualization_markers import VisualizationMarkers, VisualizationMarkersCfg from .robot_markers import RobotMarkers, RobotMarkersCfg, RobotMarkersAppearanceCfg +from .frame import MarkerFramePublisher __all__ = [ "RobotMarkers", @@ -30,6 +31,7 @@ "MarkerGroupCfg", "MarkerPrototypeCfg", "MarkerGroup", + "MarkerFramePublisher", "VisualizationMarkers", "VisualizationMarkersCfg", ] diff --git a/embodichain/lab/visualization/markers/_native.py b/embodichain/lab/visualization/markers/_native.py index 49dfd66940..a5ff8df9c6 100644 --- a/embodichain/lab/visualization/markers/_native.py +++ b/embodichain/lab/visualization/markers/_native.py @@ -86,7 +86,9 @@ class NativeMarkerRenderer: isolate them with the camera's render-category filter. """ - def __init__(self, scene: Scene, *, use_scene_for_overlays: bool = False) -> None: + def __init__( + self, scene: Scene, *, use_scene_for_overlays: bool = False, xray: bool = False + ) -> None: try: spawn = import_module("dexsim.spawn") required = ( @@ -111,6 +113,7 @@ def __init__(self, scene: Scene, *, use_scene_for_overlays: bool = False) -> Non self._spawn = spawn self._scene = scene self._use_scene_for_overlays = use_scene_for_overlays + self._xray = xray self._groups: dict[str, dict[str, _Entry]] = {} self._texture_directory = TemporaryDirectory( prefix="embodichain-marker-textures-" @@ -124,7 +127,10 @@ def _create( """Declare one render-only object through its lifetime-owning Scene.""" name = f"__embodichain_marker_{uuid4().hex}" if template is not None and hasattr(self._scene, "clone_mesh_object"): - return self._scene.clone_mesh_object(template.handle.path, name) + handle = self._scene.clone_mesh_object(template.handle.path, name) + if self._xray: + handle.get_material().set_depth_culling(False) + return handle scene_lighting = self._scene_lit(marker) use_scene = scene_lighting or self._use_scene_for_overlays parameters = ( @@ -235,7 +241,10 @@ def _create( physics=None, per_env=False, ) - return self._scene.add_mesh_object(desc, arena_name="default") + handle = self._scene.add_mesh_object(desc, arena_name="default") + if self._xray: + handle.get_material().set_depth_culling(False) + return handle def _discard(self, handle: SpawnedRigidBody) -> None: # Scene.close() invalidates the selected handles. @@ -288,7 +297,13 @@ def _apply( if previous is None or previous.visible != marker.visible: handle.set_visible(marker.visible) - def publish(self, name: str, markers: tuple[MeshMarkerOverlay, ...]) -> None: + def publish( + self, + name: str, + markers: tuple[MeshMarkerOverlay, ...], + *, + retain_omitted: bool = False, + ) -> None: """Apply a complete group, staging replacements before releasing old meshes. Args: @@ -296,7 +311,7 @@ def publish(self, name: str, markers: tuple[MeshMarkerOverlay, ...]) -> None: markers: Complete detached group snapshots in world coordinates. """ previous = self._groups.get(name, {}) - pending: dict[str, _Entry] = {} + pending: dict[str, _Entry] = dict(previous) if retain_omitted else {} created = [] touched = [] batch_enabled = hasattr(self._scene, "create_visual_object_batch") @@ -304,6 +319,20 @@ def publish(self, name: str, markers: tuple[MeshMarkerOverlay, ...]) -> None: for entry in previous.values(): templates.setdefault(id(entry.snapshot.vertices), entry) try: + if retain_omitted: + selected = {marker.overlay_id for marker in markers} + for key, entry in previous.items(): + if key not in selected and entry.snapshot.visible: + touched.append(entry) + hidden = replace( + entry.snapshot, + visible=False, + _shared_geometry=entry.snapshot._geometry, + ) + self._apply( + entry.handle, hidden, entry.snapshot, transform=False + ) + pending[key] = _Entry(entry.handle, hidden) for marker in markers: entry = previous.get(marker.overlay_id) if entry is not None and self._same_geometry(entry.snapshot, marker): @@ -329,9 +358,9 @@ def publish(self, name: str, markers: tuple[MeshMarkerOverlay, ...]) -> None: ) pending[marker.overlay_id] = _Entry(handle, marker) templates.setdefault(id(marker.vertices), pending[marker.overlay_id]) - if batch_enabled and pending: + if batch_enabled and markers: transforms = _transforms(markers) - handles = tuple(entry.handle for entry in pending.values()) + handles = tuple(pending[marker.overlay_id].handle for marker in markers) cached = self._batches.get(name) if ( cached is None diff --git a/embodichain/lab/visualization/markers/frame.py b/embodichain/lab/visualization/markers/frame.py new file mode 100644 index 0000000000..95093135d4 --- /dev/null +++ b/embodichain/lab/visualization/markers/frame.py @@ -0,0 +1,86 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Resident marker resources and one cross-group native frame publication.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING +from .group import MarkerGroup +from .cfg import MarkerGroupCfg +from ._native import NativeMarkerRenderer + +if TYPE_CHECKING: + from embodichain.lab.sim import SimulationManager + +__all__ = ["MarkerFramePublisher"] + + +class MarkerFramePublisher: + """Stage robot/object groups and publish one coherent render-only frame. + + Args: + simulation: Prepared simulation owning the native render resources. + xray: Disable depth culling for translucent comparison overlays. + + Groups belong to this publisher, independently of automatic simulation + markers. Initial publication uploads all declared resources, including hidden + instances. Later publications transform only visible instances, retain hidden + resources, and use actual native visibility rather than moving/scaling objects. + Publishing neither prepares nor steps the simulation. Close before the World. + """ + + def __init__(self, simulation: SimulationManager, *, xray: bool = False) -> None: + self.sim = simulation + self.groups: dict[str, MarkerGroup] = {} + self.native = NativeMarkerRenderer(simulation.spawn_result, xray=xray) + self.publications = 0 + self._closed = False + + def add_marker_group(self, cfg: MarkerGroupCfg) -> MarkerGroup: + """Declare a group compatible with RobotMarkers/VisualizationMarkers.""" + if self._closed: + raise RuntimeError("marker frame publisher is closed") + if cfg.name in self.groups: + raise ValueError(f"duplicate marker group: {cfg.name}") + group = MarkerGroup( + cfg, num_envs=self.sim.num_envs, origins=self.sim.arena_offsets + ) + self.groups[group.name] = group + return group + + def publish(self, *, preload: bool = False) -> None: + """Submit all staged groups together, retaining hidden GPU resources.""" + if self._closed: + raise RuntimeError("marker frame publisher is closed") + initial = self.publications == 0 or preload + meshes = tuple( + mesh + for group in self.groups.values() + for mesh in group.snapshot(visible_only=not initial) + ) + self.native.publish("frame", meshes, retain_omitted=True) + self.publications += 1 + + def close(self) -> None: + """Release resident batches, meshes and groups; repeated close is safe.""" + if self._closed: + return + self._closed = True + self.native.close() + for group in self.groups.values(): + group.remove() + self.groups.clear() diff --git a/embodichain/lab/visualization/markers/group.py b/embodichain/lab/visualization/markers/group.py index c499d2d5be..03dbaa7033 100644 --- a/embodichain/lab/visualization/markers/group.py +++ b/embodichain/lab/visualization/markers/group.py @@ -391,7 +391,7 @@ def _poses(self, env_id: int) -> tuple[np.ndarray, np.ndarray]: xyzw = (rotation * Rotation.from_quat(xyzw)).as_quat() return xyz + self._origins[env_id], xyzw - def snapshot(self) -> tuple[MeshMarkerOverlay, ...]: + def snapshot(self, *, visible_only: bool = False) -> tuple[MeshMarkerOverlay, ...]: """Return detached world-space snapshots without querying parent assets. Returns: @@ -408,6 +408,10 @@ def snapshot(self) -> tuple[MeshMarkerOverlay, ...]: ): raise ValueError("World positions must be finite float32 values.") for index, prototype_index in enumerate(state["prototype_indices"]): + if visible_only and not ( + self._visible[env_id] and state["visible"][index] + ): + continue prototype = prototypes[prototype_index] rgba = ( prototype.color diff --git a/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py b/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py index 7c2f41a69a..2dd6230673 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py +++ b/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py @@ -16,6 +16,9 @@ from __future__ import annotations +from contextlib import contextmanager, nullcontext +from collections.abc import Iterator, Callable + import numpy as np import open3d as o3d import torch @@ -28,6 +31,60 @@ __all__ = ["GripperCollisionCfg", "GripperCollisionChecker"] +_NATIVE_POINT_QUERY = ConvexCollisionChecker.query_batch_points + + +@contextmanager +def _cpu_query_device( + checker: ConvexCollisionChecker, +) -> Iterator[tuple[torch.device, Callable]]: + """Keep a query's existing CUDA computation resident with bounded scratch. + + CPU transforms, every surface sample and the signed-distance kernel are + unchanged. Inputs are copied afresh per query; no mutable geometry is cached + across calls. Scratch is released on its owning stream before scope exit. + """ + import warp as wp + from embodichain.compute.geometry._warp.convex_query import ( + convex_signed_distance_kernel, + ) + + device = torch.device("cuda", torch.cuda.current_device()) + planes = checker.plane_equations["plane_equations"].to(device).clone() + counts = checker.plane_equations["plane_equation_counts"].to(device).clone() + workspace = None + + def query( + points: torch.Tensor, *, collision_threshold: float + ) -> tuple[torch.Tensor, torch.Tensor]: + nonlocal workspace + gpu_points = points.to(device) + shape = (*points.shape[:2], planes.shape[0]) + if workspace is None or workspace.shape != shape: + workspace = None + workspace = wp.empty(shape, dtype=wp.float32, device=str(device)) + workspace.fill_(-float("inf")) + wp.launch( + convex_signed_distance_kernel, + dim=shape, + inputs=( + wp.from_torch(gpu_points), + wp.from_torch(planes), + wp.from_torch(counts), + ), + outputs=(workspace,), + device=str(device), + ) + distance = wp.to_torch(workspace).min(dim=-1).values + return distance <= collision_threshold, distance + + with wp.ScopedStream(wp.stream_from_torch(torch.cuda.current_stream(device))): + try: + yield device, query + finally: + workspace = None + + @configclass class GripperCollisionCfg: """Target-object decomposition settings, independent of gripper geometry.""" @@ -76,6 +133,10 @@ def query( ) -> tuple[torch.Tensor, torch.Tensor]: """Reduce finite mesh-surface queries at canonical grasp poses. + CPU inputs retain CPU transforms and the existing CUDA classification. + Native multi-chunk queries keep geometry and reductions on that device + for one call; every original sample and chunk bound is preserved. + Args: obj_pose: World pose of the target mesh, shape ``(4, 4)``. grasp_poses: World canonical contact poses, shape ``(B, 4, 4)``. @@ -111,44 +172,62 @@ def _query_chunks( """Reduce all samples with bounded candidate/point/hull intermediates.""" relative = torch.linalg.inv(obj_pose).unsqueeze(0) @ grasp_poses count = len(grasp_poses) - collided = torch.zeros(count, dtype=torch.bool, device=self.device) - distance = torch.full( - (count,), float("inf"), device=self.device, dtype=grasp_poses.dtype - ) ground = ( self.get_ground_height(obj_pose) if is_filter_ground_collision else None ) # Visualization holds at most one pose's surface and colors. batch_size = 1 if is_visual else candidate_chunk_size samples = self.surface_points.to(dtype=grasp_poses.dtype) - for start in range(0, count, batch_size): - end = min(start + batch_size, count) - display_points, display_flags = [], [] - for points in samples.split(point_chunk_size): - local = transform_points_mat(points, relative[start:end]) - flags, distances = self._checker.query_batch_points( - local, collision_threshold=collision_threshold - ) - if ground is not None: - world = transform_points_mat(points, grasp_poses[start:end]) - ground_distance = world[:, :, 2] - ground - flags = flags | (ground_distance < collision_threshold) - distances = torch.minimum(distances, ground_distance) - collided[start:end] |= flags.any(dim=1) - distance[start:end] = torch.minimum( - distance[start:end], distances.min(dim=1).values - ) + native_cpu = ( + count > 0 + and ( + count > candidate_chunk_size + or len(self.surface_points) > point_chunk_size + ) + and self.device.type == "cpu" + and not is_visual + and type(self._checker) is ConvexCollisionChecker + and getattr(self._checker.query_batch_points, "__func__", None) + is _NATIVE_POINT_QUERY + ) + scope = ( + _cpu_query_device(self._checker) + if native_cpu + else nullcontext((self.device, self._checker.query_batch_points)) + ) + with scope as (query_device, point_query): + collided = torch.zeros(count, dtype=torch.bool, device=query_device) + distance = torch.full( + (count,), float("inf"), device=query_device, dtype=grasp_poses.dtype + ) + for start in range(0, count, batch_size): + end = min(start + batch_size, count) + display_points, display_flags = [], [] + for points in samples.split(point_chunk_size): + local = transform_points_mat(points, relative[start:end]) + flags, distances = point_query( + local, collision_threshold=collision_threshold + ) + if ground is not None: + world = transform_points_mat(points, grasp_poses[start:end]) + ground_distance = (world[:, :, 2] - ground).to(query_device) + flags = flags | (ground_distance < collision_threshold) + distances = torch.minimum(distances, ground_distance) + collided[start:end] |= flags.any(dim=1) + distance[start:end] = torch.minimum( + distance[start:end], distances.min(dim=1).values + ) + if is_visual: + display_points.append(local[0].cpu().numpy()) + display_flags.append(flags[0].cpu().numpy()) if is_visual: - display_points.append(local[0].cpu().numpy()) - display_flags.append(flags[0].cpu().numpy()) - if is_visual: - cloud = o3d.geometry.PointCloud() - displayed = np.concatenate(display_points) - flags_np = np.concatenate(display_flags) - cloud.points = o3d.utility.Vector3dVector(displayed) - colors = np.zeros_like(displayed) - colors[flags_np, 0] = 1 - colors[~flags_np, 1] = 1 - cloud.colors = o3d.utility.Vector3dVector(colors) - o3d.visualization.draw_geometries([self._checker.mesh, cloud]) - return collided, distance + cloud = o3d.geometry.PointCloud() + displayed = np.concatenate(display_points) + flags_np = np.concatenate(display_flags) + cloud.points = o3d.utility.Vector3dVector(displayed) + colors = np.zeros_like(displayed) + colors[flags_np, 0] = 1 + colors[~flags_np, 1] = 1 + cloud.colors = o3d.utility.Vector3dVector(colors) + o3d.visualization.draw_geometries([self._checker.mesh, cloud]) + return collided.to(self.device), distance.to(self.device) diff --git a/embodichain/utils/_owned_json.py b/embodichain/utils/_owned_json.py new file mode 100644 index 0000000000..12460f8477 --- /dev/null +++ b/embodichain/utils/_owned_json.py @@ -0,0 +1,120 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + + +"""Owned immutable finite JSON nodes and defensive mutable projections.""" + +from __future__ import annotations + +from collections.abc import Mapping +import json +import math +from types import MappingProxyType + +__all__: list[str] = [] + +_ATOMS = frozenset((str, bool, int, type(None))) + + +class _FrozenTuple(tuple): + __slots__ = () + + def __new__(cls, values): + return super().__new__(cls, (freeze_metadata(value) for value in values)) + + +class _FrozenMap(Mapping): + """Own validated immutable children; expose no mutable backing dictionary.""" + + __slots__ = ("_data", "_strict", "_encoded") + + def __init__(self, values): + if not isinstance(values, Mapping): + raise ValueError("metadata must contain only finite JSON values") + owned, strict = {}, True + for key, value in values.items(): + if not isinstance(key, str) or not key.strip(): + raise ValueError("metadata key must be a nonempty string") + item = freeze_metadata(value) + owned[key] = item + strict &= _key_valid(key) and _strict_keys(item) + # Validate and detach even direct construction of a node's public type. + # Reusing a supposedly immutable wrapper over a caller-owned dictionary + # would permit later mutation to invalidate its finite-JSON proof. + object.__setattr__(self, "_data", MappingProxyType(owned)) + object.__setattr__(self, "_strict", strict) + object.__setattr__(self, "_encoded", None) + + def __setattr__(self, name, value): + raise TypeError("Frozen metadata cannot be modified") + + def __getitem__(self, key): + return self._data[key] + + def __iter__(self): + return iter(self._data) + + def __len__(self): + return len(self._data) + + def __deepcopy__(self, memo): + return self + + def __repr__(self): + return repr(self._data) + + def __str__(self): + return str(self._data) + + def mutable_copy(self): + if self._encoded is None: + encoded = json.dumps( + self._data, + default=dict, + ensure_ascii=False, + allow_nan=False, + ) + object.__setattr__(self, "_encoded", encoded) + # Every call returns new mutable containers. No caller can modify the + # validated immutable source through the returned tree. + return json.loads(self._encoded) + + +def _key_valid(key): + return type(key) is str and bool(key) and key == key.strip() + + +def _strict_keys(value): + if type(value) is _FrozenMap: + return value._strict + if type(value) is _FrozenTuple: + return all(_strict_keys(item) for item in value) + return True + + +def freeze_metadata(value: object) -> object: + kind = type(value) + if kind in _ATOMS or kind in (_FrozenMap, _FrozenTuple): + return value + if kind is float: + if not math.isfinite(value): + raise ValueError("metadata must contain only finite JSON values") + return value + if isinstance(value, (list, tuple)): + return _FrozenTuple(value) + if isinstance(value, Mapping): + return _FrozenMap(value) + raise ValueError("metadata must contain only finite JSON values") diff --git a/embodichain/utils/nms.py b/embodichain/utils/nms.py index a43a5be3c8..8befa8028d 100644 --- a/embodichain/utils/nms.py +++ b/embodichain/utils/nms.py @@ -17,6 +17,9 @@ from __future__ import annotations import math +from contextlib import contextmanager +from contextvars import ContextVar +from collections.abc import Iterator import numpy as np import torch @@ -24,9 +27,35 @@ from embodichain.utils import logger from embodichain.utils.math import quat_from_matrix -__all__ = ["pose_nms", "pose_nms_indices"] +__all__ = ["pose_nms", "pose_nms_indices", "pose_nms_scope"] _POSE_NMS_CHUNK_SIZE = 2048 +_POSE_NMS_SCOPE: ContextVar[int] = ContextVar( + "pose_nms_chunk_size", default=_POSE_NMS_CHUNK_SIZE +) + + +@contextmanager +def pose_nms_scope(*, chunk_size: int | None = None) -> Iterator[int]: + """Select a task-local NMS memory budget without replacing any function. + + Args: + chunk_size: Positive pairwise tile bound; None inherits the active scope + (2048 outside a scope). Explicit NMS call arguments take precedence. + + Yields: + Active tile bound. Nested scopes restore their parent on success/failure; + threads and copied cooperative contexts retain their own selection. + Threshold arithmetic, candidate ordering and output values are unchanged. + """ + size = _POSE_NMS_SCOPE.get() if chunk_size is None else chunk_size + if type(size) is not int or size < 1: + raise ValueError("NMS chunk_size must be a positive integer") + token = _POSE_NMS_SCOPE.set(size) + try: + yield size + finally: + _POSE_NMS_SCOPE.reset(token) def _poses_to_components(poses: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: @@ -219,7 +248,7 @@ def pose_nms_indices( angle_th: float = np.pi / 36, dist_th: float = 0.003, preserve_order: bool = False, - chunk_size: int = _POSE_NMS_CHUNK_SIZE, + chunk_size: int | None = None, ) -> torch.Tensor: """Return pose indices after removing poses that are too close. @@ -237,7 +266,7 @@ def pose_nms_indices( ``False``, poses with fewer close neighbors are selected first. Defaults to ``False``. chunk_size: Maximum size of either dimension of a pairwise tile. - Defaults to 2048. + None uses the active pose_nms_scope, defaulting to 2048. Returns: Indices of selected poses. Shape is ``(M,)``, where ``M <= N``. @@ -246,6 +275,7 @@ def pose_nms_indices( ValueError: If ``poses`` is not shaped as ``(N, 4, 4)``, is not on a CPU or CUDA device, or ``chunk_size`` is not positive. """ + chunk_size = _POSE_NMS_SCOPE.get() if chunk_size is None else chunk_size if poses.ndim != 3 or poses.shape[-2:] != (4, 4): raise ValueError(f"Invalid input shape {poses.shape}, expected (N, 4, 4).") if poses.device.type not in {"cpu", "cuda"}: @@ -316,7 +346,7 @@ def pose_nms( poses: torch.Tensor, angle_th: float = np.pi / 36, dist_th: float = 0.003, - chunk_size: int = _POSE_NMS_CHUNK_SIZE, + chunk_size: int | None = None, ) -> tuple[torch.Tensor, torch.Tensor]: """Remove poses that are too close in translation and rotation. @@ -325,7 +355,7 @@ def pose_nms( angle_th: Rotation threshold in radians. Defaults to pi / 36. dist_th: Translation threshold. Defaults to 0.003. chunk_size: Maximum size of either dimension of a pairwise tile. - Defaults to 2048. + None uses the active pose_nms_scope, defaulting to 2048. Returns: A tuple containing the filtered pose matrices in input order and their diff --git a/examples/expansion/engine/README.md b/examples/expansion/engine/README.md index 1a01cc0565..e1165c7648 100644 --- a/examples/expansion/engine/README.md +++ b/examples/expansion/engine/README.md @@ -24,6 +24,29 @@ for measured 3D/RGB-D comparisons, per-capability checks, recovery diagnostics a portable dataset verification. `--mode quick` samples a smaller subset; unrun capabilities remain explicit. `--reuse` rebuilds the offline page without a World. +## Native ghost preview + +Compute Task Program expansion at both parameter bounds and stratified interior +points, validate the real results, then automatically open the native viewer: + +```bash +embodichain expansion preview --job examples/expansion/engine/job.preview.yaml +``` + +Preview existing results without any collection: + +```bash +embodichain expansion preview --input /path/to/job-output --gpu 0 --open +``` + +Repeat `--input` for groups, or point it at a generated preview-plan directory. +`--inpu` is an alias for `--input`. In job mode `--output` selects a **new** output +folder; it is no longer the existing-result input flag. `--plan-only` freezes and +validates samples without executing, and `--no-open` suppresses automatic opening. +The [preview guide](ghost-preview.md) explains budgets, failed-boundary evidence, +scene-atlas domains, finite-sampling limitations and the WebRTC TODO. Rendering is +native; browser transport remains HTTP JPEG, not WebRTC. + ## Run a configured job From the repository root, with the simulation dependencies installed: diff --git a/examples/expansion/engine/ghost-preview.md b/examples/expansion/engine/ghost-preview.md new file mode 100644 index 0000000000..376d3ca9d8 --- /dev/null +++ b/examples/expansion/engine/ghost-preview.md @@ -0,0 +1,262 @@ +# 扩增分布的原生虚影预览 + +本文记录空间、affordance 与轨迹分布的交互预览方案,以及 2026-11-15 完成的 +小规模原型验收,并说明基于已有结果启动的仓库入口。它补充 [Expansion Engine](README.md) 的结果查看流程。 + +## 当前能力与边界 + +| 路径 | 渲染与交互 | 状态 | +| --- | --- | --- | +| SDK 原生窗口对照 | `preview_expansion_comparison` 加载已验收轨迹,显示参考与选中虚影;N 切换、Space 暂停、Q 关闭 | 仓库已有能力,见 [comparison.py](../../../embodichain/lab/expansion/comparison.py) | +| 浏览器结果预览 | EmbodiChain 原生离屏渲染完整模型;HTTP JPEG 传图,HTTP 控制筛选与回放 | `embodichain expansion preview`,直接读取已确认结果 | +| 原生 WebRTC 推流 | 复用 EmbodiChain 的原生推流生命周期与控制接口 | TODO,尚未接入本预览 | + +“原生渲染”指场景由 `SimulationManager`、`RobotMarkers`、 +`VisualizationMarkers` 和 DexSim Hybrid 渲染器生成,**不表示采用 WebRTC**。 +浏览器预览没有自行绘制机器人骨架,也不使用 Viser 重建场景;它显示原生离屏 +相机输出的 JPEG。本次使用 OptiX 去噪。展示灯光、视角和透明材质与采集时可以不同, +不能把预览图片当作采集传感器的原始观测。 + +最初实验使用采集时冻结的源码 `ab04d972b5b75042434498db9a889b4edd03b6fe`。 +仓库入口直接读取原始结果,不需要该源码快照、本地预处理 JSON 或实验脚本。 +[showcase](showcase/README.md) 的离线三维/RGB-D 页面也是另一条查看路径。 + +## 基于 Task Program 重新计算并预览 + +```bash +# 一体化入口:检查配置 → 固定采样计划 → 实测扩增 → 验收 → 原生预览。 +embodichain expansion preview --job examples/expansion/engine/job.preview.yaml + +# 先查看计划。--output 必须是新目录,不会覆盖原 job 的数据。 +embodichain expansion preview --job examples/expansion/engine/job.preview.yaml \ + --output /tmp/preview-plan-check --plan-only + +# 减少区间内采样点,用于快速验证;仍包含中心和两个端点。 +embodichain expansion preview --job examples/expansion/engine/job.preview.yaml \ + --output /tmp/preview-run --interior-samples 1 --no-open --duration 10 + +# 重放整个结果分布及失败/未执行记录,不重新计算。 +embodichain expansion preview --input /tmp/preview-run --open +``` + +`--job` 使用原生 Task Program source、原始动作/物理时钟和验收配置;不支持把 +Real2Sim 或视觉 profile 静默转换为另一种扩增。计算结束后默认尝试打开浏览器, +`--no-open` 可关闭此行为。`--input` 是已有结果的只读入口,兼容别名 `--inpu`; +`--job` 与 `--input` 互斥。`--output` 只在 job 模式指定新产物目录,省略时在原 +job 输出目录旁创建唯一目录。原 job 文件和原产物不修改,也不隐式 resume。 + +### 参数空间与采样含义 + +- 连续物理轴取自 job 的质量、摩擦、关节刚度/阻尼倍率及 CoM 区间。 +- 若配置 `scene_atlas`,读取其原始 request 中物体布局与机器人安装 XY/yaw 的 + lower/upper,通过已有联合场景参数化转换为完整场景/安装姿态。保留组内关系, + 不拼接不同可行区域的边界。采样的是**原始声明范围**,不是历史成功区域证明; + 每个新点都必须重新执行并接受原来的实测验证。 +- 轨迹 `joint_residual` 的受控关节使用独立的有符号比例轴 `[-1,1]`,实际残差 + 幅值由原 `joint_offset_scale × joint range` 决定;`via_points` 另外包含节点 + 幅值轴。`joint_samples` / `knot_samples` 是可选的显式采样值,不设置时原随机 + 算法保持不变。受保护阶段、端点、非受控关节和关节限位检查均保留。 +- 每个参数点遍历配置的 affordance 分支和轨迹 recipe。全零关节残差在数学上 + 等于名义轨迹,因此该点只执行唯一名义变体,避免重复计成功或降低去重阈值。 + 轨迹数量不足以覆盖声明的全部算子时会提前拒绝,并要求补足配置。 +- 未声明范围的维度保持固定;不会把碰撞检查配置里的搜索范围自动当作启用的 + 场景扩增。需要空间扩增时,在 job 中显式设置已有 `scene_atlas`。 + +默认采样包含全下界、全上界、中心和 **5 个 Latin-hypercube 分层区间内点**。 +每个变化轴都包含其两个端点,区间内各轴独立分层。`--interior-samples N` 可调整 +区间内点数。它覆盖的是有限参数样本,**不是所有混合角点的 Cartesian 穷举**, +不是连续空间成功率估计;同一关节的显式偏移复用于各允许自由阶段,也不穷举 +独立分段方向组合。物理约束与 FK 非线性意味着参数端点不等于世界坐标轨迹包络端点。 + +示例 `job.preview.yaml` 声明 2 个抓取分支 × 2 个轨迹变体,默认 8 个参数点; +中心零残差折叠后计划 30 次实测,位于其 `max_attempts: 32` 预算内。 +所有点的总次数受原 job `max_attempts`、`--max-episodes` 和 64 条预览上限共同约束。 +超过预算在执行前报错,不静默截断端点或改窄范围;全部子作业共用原 job 的 +`max_wall_time_s` 总时间预算。`--duration` 仅控制之后的查看时长。 +每个参数点在独立子进程中运行原生 Engine,以明确释放 World;预览进程只在 +采集结束后创建渲染资源。`--gpu` 是当前 `CUDA_VISIBLE_DEVICES` 内的可见编号。 + +### 覆盖报告与失败处理 + +新目录包含 `preview-plan.json`、冻结的 `configs/`、逐点 `logs/` 和原生 `jobs/` +结果。计划记录源文件哈希、上下界、归一化坐标、请求值、目标 recipe 数和后续 +实测结果;执行前重新核对语义身份,源配置发生变化则停止。 + +不会为了填满预览而重新采样、替换失败端点、放宽验收或将轨迹回退算作边界覆盖。 +报告区分已确认、部分通过、拒绝、错误、预算耗尽和未执行。`complete` 表示计划 +尝试完成,`coverage_complete` 仅在全部唯一 recipe 被原样执行并确认时成立;抓取分支复用或轨迹回退不计作完整覆盖。 +存在有效结果时仍打开预览,最终返回 2 表示不完整覆盖;没有有效轨迹时返回 2, +保留报告和日志,不伪造成功回放。 + +页面显示所有计划点的参数和状态,候选虚影只来自确认结果;初始位置图层中, +红色计划点表示未完全确认的参数点,不代表一条失败轨迹。筛选后没有匹配样本时 +显示空结果,不用别的候选替代。WebRTC 接入仍为 TODO。 + +## 从已有结果启动 + +在安装了兼容 EmbodiChain、DexSim 原生渲染依赖的环境中运行: + +```bash +# 单个已完成或部分完成的作业:只加载已有确认回执,不继续采集。 +embodichain expansion preview --input /path/to/job-output --gpu 0 --open + +# 复用本次配方的两个空间组,叠加一条独立参考。 +embodichain expansion preview \ + --input /path/to/results/space-a \ + --input /path/to/results/space-b \ + --reference /path/to/results/reference \ + --gpu 0 --port 8769 --open +``` + +每个输入是含 `job.json` 和 `episodes/` 的原始作业输出目录;不是导出的 LeRobot +数据集或单个 NPZ。可重复传入 `--input`,空间筛选以输入结果目录分组。 +已有结果直接读取仍要求固定布局;`--job` 会将 scene-atlas 的每个采样点冻结为 +独立布局子作业,再作为一个参数组加载。 +`--reference` 可省略;指定时必须恰好含一个已确认参考 episode,且不能与候选重复。 + +模型从 `job.json.resolved_deployment.robot` 的 URDF 路径恢复,场景刚体几何从 +同一快照恢复。方块、球和文件 Mesh/USD 资产通过原生纯视觉对象加载,保留原生 +坐标轴转换与模型缩放;不会用方块替代网格。位置恒定的静态背景只显示一份。 +不加载原任务配置,也不重新实例化任务/规划器。原模型及引用的视觉 +文件必须仍可读;若资产移动,使用 `--robot-asset /new/path/robot.urdf` 显式提供 +匹配模型。入口检查完整关节名称集合和跨输入模型一致性,但旧结果没有完整视觉 +资产包,不能仅凭关节名证明替换模型几何完全一致;调用者负责提供原模型。 + +默认从唯一的 solver `end_link_name` 推导末端路径,存在多个端点时必须提供 +`--eef-link LINK_NAME`。这里的路径是该 URDF link 的 FK 原点,不隐式附加工具 TCP。 +缺失 affordance/trajectory 注释显示“未记录”,不猜测为名义分支;新记录保存 +`source_env_id`,旧批量记录无法绑定诊断行时省略抓取目标而不猜用第 0 行。只有所有轨迹 +具有一致、有效的动作阶段时才提供阶段对齐。 + +服务默认仅监听 `127.0.0.1:8769`,打印就绪地址后才能访问;`--port 0` 自动选空闲端口。 +`--open` 在首帧完成后尝试打开服务所在机器的浏览器。远程服务器需要端口转发后 +在客户端打开对应地址。Ctrl+C 释放服务和渲染资源;`--duration 60` 在就绪后 +运行 60 秒自动关闭。启动端口冲突、资产缺失和不支持的场景会明确失败。 + +入口不会写入输入目录、更新成功计数或重新采集。它默认最多加载 32 条候选, +`--max-episodes` 可设为 1–64,超出则报错而非静默抽样。还限制总状态帧不超过 +100000、预计算 link FK 不超过 256 MiB;这些是显式的预览规模限制,不是性能保证。 + +## 配方与真实结果 + +使用已有 `task.default.yaml` 的单轮 UR5 + DH PGI 抓取放置任务和 default 物理后端。 +原型关闭传感器媒体采集与数据集导出,保留原有 lift/place/release 等独立校验。 + +| 维度 | 小规模设置 | +| --- | --- | +| 参考 | 方块初始位置 `[-0.42, -0.08, 0.025]` m,1 条轨迹 | +| 空间 S-A | `[-0.435, -0.08, 0.025]` m | +| 空间 S-B | `[-0.405, -0.08, 0.025]` m | +| Affordance | 每个空间 2 个抓取配方分支;`max_proposals: 8` | +| 轨迹 | 每个分支 2 个变体:名义规划和 `joint_residual`,扰动尺度 `0.002` | +| 扩增总数 | 2 × 2 × 2 = 8 条,另加 1 条参考 | +| 执行 | 扩增批大小 2,参考批大小 1,随机种子 7 | + +两个空间位置通过分别冻结的环境配置定义;本次没有启用场景随机化算子, +也没有验证场景扩增引擎的分布采样。2 个位置不能代表连续空间覆盖率。 + +9 条轨迹均通过原有校验并确认回执,共保留 3642 个原始状态帧。 +该结果只代表这个固定小配方,不代表更大规模、其他任务或其他后端的资格结论。 + +## 数据到画面的运行逻辑 + +1. 读取 `ArtifactEpisodeSink` 的确认回执,并通过 `load_expansion_episode` + 加载已验收的真实结果;不重新规划或执行扩增。 +2. 核对 episode 身份、机器人模型、关节名称与顺序,以及动作和状态的 T+1 对齐。 + 加载逐帧完整关节位置、机器人根位姿、物体位姿和时间戳。 +3. 使用与记录匹配的 URDF 和实测关节计算 FK,驱动 `RobotMarkers` 的完整视觉网格。 + 选中样本显示原始模型外观,其余可见候选显示半透明虚影;参考单独开关。 +4. 用真实物体位姿驱动物体模型,并通过 `VisualizationMarkers` 显示初始位置、 + 抓取目标坐标轴和末端路径。 +5. 启动时分块预计算 FK,使用 `MarkerFramePublisher` 发布一致的跨组帧; + 在仿真/渲染所有者线程应用筛选与回放状态,调用原生相机渲染。 + HTTP 线程只排队控制消息和读取已发布画面,不直接访问仿真对象。 +6. 页面轮询同一次发布中的 JPEG 与记录信息,避免分开读取图像和状态造成帧号错配。关闭服务时释放相机、场景和监听端口; + 已提交轨迹与校验结果保持不变。 + +原型回放不调用物理推进,验收记录的物理步数为 0。回放不是新物理成功样本, +也不生成新的成功回执。 + +## 联动筛选与时间语义 + +空间、affordance、轨迹三个筛选器取交集,同时作用于候选列表、虚影、物体、 +初始位置、抓取目标和轨迹线。选中样本被筛掉时,选择交集内的一个有效样本。 +没有匹配候选时显示空结果、停止播放并隐藏候选图层;参考对照仍可独立显示。 +参考轨迹是独立对照,不计入 8 条候选数量,也不受候选筛选限制。 + +- **空间**:显示实际初始位置,不给样本添加人为排列偏移。 +- **Affordance**:A0/A1 是配方分支编号,不是底层候选 ID。读取规划诊断时按实际 + `env_id` 选择批内对应行,再取得抓取目标与候选 ID,不能默认使用第 0 行。 + 先用 `inverse(reference_pose) @ selected_pose` 得到物体局部抓取目标,再用 + 实际初始物体位姿转换到世界坐标。坐标轴展示规划目标,不是实测接触点; + 原型把它固定在初始抓取位置,不随搬运中的方块移动。 +- **轨迹**:V0/V1 来自执行元数据中的实际选择。末端路径由实测关节 FK 推导, + 轨迹线每 4 帧取点并保留末帧;机器人和物体回放仍可访问全部原始状态帧。 +- **时间**:支持公共真实时间或六个记录动作阶段的归一化对齐。每个样本取目标 + 时刻之前最近的已记录帧,不插值生成新姿态;短轨迹到末帧后保持。 + 阶段对齐只改变浏览进度映射,不改变原始时间戳。页面保留实际秒数和源帧号。 + +## 验收与当前限制 + +本地浏览器验收覆盖:筛选数量 8 → 4 → 2 → 1,筛选后原生画面改变, +时间拖动与播放、虚影开关、相机视角切换、窄屏布局和非法控制值拒绝。 +浏览器无脚本错误,回放全过程未推进物理。验收后服务已关闭,结果保留。 + +仓库 CLI 已使用相同的 8 条候选和 1 条参考复测:筛选后的实际虚影/目标数量同步变化, +播放与拖动正常;无需 `--robot-asset` 即可从 `job.json` 恢复资产,`--duration` +结束后端口关闭。结果读取、批内目标绑定、空交集、时间对齐、HTTP 控制/快照、 +CLI 不触发采集和安装包页面资源由生产模块的聚焦测试覆盖。 + +原型脚本及结果保存在本次实验的 `real-demo` 目录: + +| 文件 | 用途 | +| --- | --- | +| `recipe.yaml`、`configs/`、`results/` | 固定配方、环境设置、真实产物与回执 | +| `build_data.py`、`preview-data.json` | 从产物提取显示数据并验证对应关系 | +| `native_preview.py`、`native-preview.html` | 原生渲染服务和浏览器交互页面 | +| `native-assets/` | 对应采集模型的 URDF 与视觉资产副本 | +| `native-acceptance.json`、`native-preview-acceptance.png` | 浏览器验收结果和原生渲染截图 | + +上述文件是原型的历史实验产物;仓库入口不再依赖它们。结果读取与控制语义由 +`embodichain/lab/expansion/_preview_data.py` 负责,原生渲染与资源释放由 +`_preview_render.py` 负责,HTTP 服务与页面由 `_preview_server.py`、`_preview.html` +负责,CLI 由 `embodichain/cli/expansion.py` 分发。 + +当前渲染支持单 URDF 机器人、单位机器人缩放,以及有完整实测位姿的刚体场景。 +文件 Mesh 和 USD 通过原生视觉资产加载,选中物体保留源模型外观,虚影复用实测几何。 +其他 articulated +objects、软体、流体、线缆、显式 visual replacement 和直接 scene-atlas 输入会被拒绝; +不会用方块代替不支持的几何。预览保持记录中的运动与尺寸,虚影材质与灯光用于展示, +不声称精确恢复采集传感器的原始图像。新任务支持应扩展场景恢复契约并验证实测坐标。 + +HTTP JPEG 轮询不提供 WebRTC 的编码、拥塞控制、连接恢复或低延迟保证。 +页面是单个服务的共享回放会话,多人操作会改变同一状态;尚未提供独立会话、 +认证或通用资产包恢复。当前不应对外公开监听。 + +## 一体化入口的实测验收 + +使用已有 UR5 抓取放置任务、default 后端和已保存的联合场景 atlas,覆盖物体 +XY、安装 XY/yaw、质量、摩擦与六个受控关节残差,共 13 个变化轴。 +中心、全下界、全上界和一个区间内点共计划 14 条非重复轨迹(中心 2 条, +其余各 4 条);14 条均完成原有验收并确认,没有轨迹回退或抓取分支复用。 +这是固定小配置的实测结果,不是对任意参数范围或总体成功率的保证。 + +浏览器复测了覆盖报告和 14 → 4 → 2 → 1 → 0 的筛选,包括无结果时隐藏候选 +图层;回放物理推进为 0,自动关闭后端口释放。另行验证了无 motion profile 的 +普通 Task Program 作业也能重新计算并进入预览;批内诊断由来源行号绑定。 + +## TODO:接入 EmbodiChain 原生 WebRTC 推流 + +后续直接复用 EmbodiChain 原生 WebRTC 能力,替换 HTTP JPEG 轮询;先核对实际 +推流入口、信令与关闭接口,不在本示例另建一套独立 WebRTC 栈。 +保留真实结果加载、原生场景、三类筛选联动、播放、时间轴及视角控制。 +控制通道的实现需按原生接口确定,不能预先宣称已经通过 DataChannel 接入。 + +验收条件: + +- 浏览器建立真实 WebRTC 连接并接收原生相机视频,确认画面不再依赖 JPEG 轮询。 +- 沿用这 8 条扩增结果和参考轨迹,复测筛选、拖动、暂停、切换视角及重新连接。 +- 控制状态、源帧和视频画面的对应关系可检查;显示延迟不冒充记录时间。 +- 回放物理步数仍为 0,不触发重采集、不修改原始证据或成功计数。 +- 关闭预览后释放推流、相机、端口及场景资源;记录端到端延迟与实际帧率, + 再据此报告改善,不能用传输协议名称推断性能。 diff --git a/examples/expansion/engine/job.preview.yaml b/examples/expansion/engine/job.preview.yaml new file mode 100644 index 0000000000..1e40c920d6 --- /dev/null +++ b/examples/expansion/engine/job.preview.yaml @@ -0,0 +1,20 @@ +deployment: task.default.yaml +source: task_program +collision_profile: ../scene_robot_feasibility/search.ur5.yaml +output_dir: /tmp/embodichain-expansion/distribution +batch_size: 2 +target_episodes: 4 +max_attempts: 32 +max_steps: 2000 +seed: 7 +settling_steps: 5 +mass_scale: + cube: + - 0.98 + - 1.02 +friction: + cube: + - 0.95 + - 1.0 +motion_profile: motion.preview.yaml +max_wall_time_s: 1800 diff --git a/examples/expansion/engine/motion.preview.yaml b/examples/expansion/engine/motion.preview.yaml new file mode 100644 index 0000000000..88e807158d --- /dev/null +++ b/examples/expansion/engine/motion.preview.yaml @@ -0,0 +1,55 @@ +schema_version: 1 +source: + kind: task_program + source_id: repeated_cube_pick_place + source_revision: one-cycle-v1 + unit_scope: episode + template_id: task_program_call + call_count: 2 + affordance_call_indices: + - 0 + trajectory_call_indices: + - 1 +scene_randomization: + enabled: false + reference_family_count: 1 + profile: none + change_scope: none + keep_drop_targets: true +affordance: + enabled: true + branches_per_family: 2 + max_proposals: 8 + fallback: next_surviving_branch +trajectory: + variants_per_family: 2 + spatial: + enabled: true + operators: + - joint_residual + joint_offset_scale: 0.002 + via_count: 1 + dedup_tolerance: 0.0001 + timing: + enabled: false +visual: + enabled: false + profile_file: extensions/embodichain_tasks/configs/components/randomization_profiles/rgb_visual.yaml + profiles: [] + apply_per_rollout: true + balance: round_robin + restore_before_next_candidate: true +observation: + profiles: [] + capture_rgb: false + capture_video: false +scheduling: + policy: round_robin_fifo + candidate_budget: 4 + reference_family_budget: 1 +execution: + max_inflight: 1 + compatibility: strict +persistence: + sink: lerobot + output_dir: /tmp/embodichain-expansion diff --git a/examples/gensim/scripted_pick_place/README.md b/examples/gensim/scripted_pick_place/README.md index 4c766aa754..17e6985147 100644 --- a/examples/gensim/scripted_pick_place/README.md +++ b/examples/gensim/scripted_pick_place/README.md @@ -18,9 +18,13 @@ This example prepares fixed inputs, runs the shared GenSim workflow, and measure wall-clock time through native execution and cleanup. It demonstrates the previous dual-UR5 table/cube case without a live model or reconstruction service. -The recorded outcome is **pickup succeeds, placement release verification fails**. -It is a reproducible failure investigation, not a successful expert dataset -collection or qualification of real photograph-to-scene generation. +The current defaults release 0.02 m above the resting target and hold open for +0.4 physical seconds before release verification. This avoids the previously +recorded finger/support interference while retaining measured opening, +separation, stability and landing requirements. Explicit zero clearance can +still reproduce a physically rejected contact placement. The historical failure +measurements below remain tied to their original revision; this scripted case +does not qualify real photograph-to-scene generation or model quality. ## Requirements and inputs diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/README.md new file mode 100644 index 0000000000..898a52473c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/README.md @@ -0,0 +1,20 @@ +# Dual-arm RGB sorting acceptance + +This scene-first fixture reuses the original RGB block-sorting program unchanged: +right arm moves red, left arm moves blue, green stays stationary. It selects the +existing dual-UR5 profile and open-loop dual-arm execution policy. The two arms +act sequentially; this case does not demonstrate simultaneous manipulation. + +Default physics, sensors disabled, B=1. The supported primitive tabletop has its +top at 0.835 m, with the 5 cm blocks resting at 0.86 m. Red/blue nominal poses +are the midpoints of the original reset domains, with the same ±2.5 cm X / ±3 cm +Y ranges. Green is +a fixed reference at its original domain midpoint. Geometry/robot topology stays +fixed. Both source strategies share the same layout and task-path screens. + +All original lift (5 cm), XY placement (3.5 / 3 cm), and wait-stable validators +remain unchanged. Independent measured acceptance additionally requires both +objects at table height, both hands released, green within 5 mm of its initial +reference, and stable terminal speeds. Coverage records both manipulated blocks. + +See [the comparison guide](../../../../../../../../../scripts/benchmark/expansion/README.scene_first.md). diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/env.yaml new file mode 100644 index 0000000000..bd36d9f6c1 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/env.yaml @@ -0,0 +1,125 @@ +environment_id: conditional_dual_arm_sort +physics: default +max_episodes: 1 +max_episode_steps: 2400 +simulation: + light: + direct: + - uid: light_1 + light_type: sun + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + background: + - uid: support_surface + shape: + shape_type: Cube + size: + - 1.0 + - 1.0 + - 0.1 + body_type: static + init_pos: + - 0.725 + - 0.0 + - 0.785 + rigid_object: + - uid: block_1 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + attrs: + mass_props: + mass: 0.05 + rigid_props: + max_depenetration_velocity: 10.0 + min_position_iters: 32 + min_velocity_iters: 8 + collision_props: + contact_offset: 0.003 + rest_offset: 0.001 + material_props: + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + init_pos: + - 0.625 + - -0.15 + - 0.86 + body_scale: + - 1 + - 1 + - 1 + - uid: block_2 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + attrs: + mass_props: + mass: 0.05 + rigid_props: + max_depenetration_velocity: 10.0 + min_position_iters: 32 + min_velocity_iters: 8 + collision_props: + contact_offset: 0.003 + rest_offset: 0.001 + material_props: + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + init_pos: + - 0.675 + - 0.0 + - 0.86 + body_scale: + - 1 + - 1 + - 1 + - uid: block_3 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + attrs: + mass_props: + mass: 0.05 + rigid_props: + max_depenetration_velocity: 10.0 + min_position_iters: 32 + min_velocity_iters: 8 + collision_props: + contact_offset: 0.003 + rest_offset: 0.001 + material_props: + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + init_pos: + - 0.725 + - 0.15 + - 0.86 + body_scale: + - 1 + - 1 + - 1 +env: + sim_steps_per_control: 4 + events: {} + dataset: {} +num_envs: 1 +arena_space: 3.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/expansion.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/expansion.yaml new file mode 100644 index 0000000000..1d4d02c893 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/expansion.yaml @@ -0,0 +1,555 @@ +schema_version: 1 +engine: + episodes_per_operator: 4 + max_attempts: 64 + max_steps: 3200 + max_wall_time_s: 600.0 + settling_steps: 30 + capture_media: false + collision_backend: newton + collision_device: cpu +scene: + block_1: + translation_m: + - - -0.025 + - 0.025 + - - -0.03 + - 0.03 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 + block_3: + translation_m: + - - -0.025 + - 0.025 + - - -0.03 + - 0.03 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 +trajectory: + operators: + - object_pose + joint_offset_scale: 0.005 + via_count: 1 + max_proposals: 64 + minimum_joint_delta: 0.002 + velocity_limit: 0.5 + acceleration_limit: 0.5 + nullspace_task_rows: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + place: + - retract +validation: + initial_checks: + - kind: support_bounds + entity: block_1 + lower: + - 0.2 + - -0.5 + - 0.82 + upper: + - 1.2 + - 0.5 + - 0.95 + - kind: support_bounds + entity: block_2 + lower: + - 0.2 + - -0.5 + - 0.82 + upper: + - 1.2 + - 0.5 + - 0.95 + - kind: support_bounds + entity: block_3 + lower: + - 0.2 + - -0.5 + - 0.82 + upper: + - 1.2 + - 0.5 + - 0.95 + final_checks: &id001 + - kind: axis_relation + entity: block_1 + other: block_2 + axis: 0 + lower: -0.115 + upper: -0.045 + - kind: axis_relation + entity: block_1 + other: block_2 + axis: 1 + lower: -0.03 + upper: 0.03 + - kind: axis_relation + entity: block_1 + other: block_2 + axis: 2 + lower: -0.012 + upper: 0.012 + - kind: speed_below + entity: block_1 + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: axis_relation + entity: block_3 + other: block_2 + axis: 0 + lower: 0.045 + upper: 0.115 + - kind: axis_relation + entity: block_3 + other: block_2 + axis: 1 + lower: -0.03 + upper: 0.03 + - kind: axis_relation + entity: block_3 + other: block_2 + axis: 2 + lower: -0.012 + upper: 0.012 + - kind: speed_below + entity: block_3 + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: joint_range + joint: left_gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + - kind: joint_range + joint: right_gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + - kind: pose_near + entity: block_2 + position: + - 0.675 + - 0.0 + - 0.86 + tolerance_m: 0.005 + - kind: speed_below + entity: block_2 + linear_m_s: 0.03 + angular_rad_s: 0.2 + events: + - event_id: block_1_lifted + conditions: + - kind: lifted + entity: block_1 + height_m: 0.05 + - event_id: block_3_lifted + conditions: + - kind: lifted + entity: block_3 + height_m: 0.05 + - event_id: both_sorted_released_stable + conditions: *id001 + consecutive_steps: 3 + allowed_contacts: &id002 + - - block_1 + - support_surface + - - block_1 + - default_plane + - - block_2 + - support_surface + - - block_2 + - default_plane + - - block_3 + - support_surface + - - block_3 + - default_plane + - - block_1 + - DualUR5/right_gripper_finger1_link_1 + - - block_1 + - DualUR5/right_gripper_finger2_link_1 + - - block_3 + - DualUR5/left_gripper_finger1_link_1 + - - block_3 + - DualUR5/left_gripper_finger2_link_1 + mounting_contacts: + - - '@root' + - default_plane + - - '@root' + - support_surface +scene_sampling: + strategy: scene_first + workspace_source: joint_uniform_fk + control_part: right_arm + sample_count: 16384 + robot_anchors: + - phase_id: block_1_grasp + control_part: right_arm + - phase_id: block_3_grasp + control_part: left_arm + uniform_fraction: 0.25 + max_candidates: 256 + max_rollouts: 4 + max_ik_calls: 16384 + max_planning_calls: 2048 + targets: {} + layout: + support_uid: support_surface + parent_by_uid: + block_1: support_surface + block_2: support_surface + block_3: support_surface + policy: + group_targets: + - block_1 + - block_3 + batch_size: 1 + collision_margin_m: 0.002 + group_xy_offset_bounds_m: + block_1: + - - -0.025 + - -0.03 + - - 0.025 + - 0.03 + block_3: + - - -0.025 + - -0.03 + - - 0.025 + - 0.03 + task_mode: + mode_id: sort_with_both_arms + phases: + - phase_id: block_1_approach + dependencies: [] + targets: + - control_part: right_arm + frame_uid: block_1 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.11 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_1_grasp + dependencies: + - block_1_approach + targets: + - control_part: right_arm + frame_uid: block_1 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_1_lift + dependencies: + - block_1_grasp + targets: + - control_part: right_arm + frame_uid: block_1 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.14 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_1 + control_part: right_arm + tcp_from_object: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + - phase_id: block_1_place + dependencies: + - block_1_lift + targets: + - control_part: right_arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - -0.08 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.07 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_1 + control_part: right_arm + tcp_from_object: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + - phase_id: block_1_retreat + dependencies: + - block_1_place + targets: + - control_part: right_arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - -0.08 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.22 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_3_approach + dependencies: + - block_1_retreat + targets: + - control_part: left_arm + frame_uid: block_3 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.11 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_3_grasp + dependencies: + - block_3_approach + targets: + - control_part: left_arm + frame_uid: block_3 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_3_lift + dependencies: + - block_3_grasp + targets: + - control_part: left_arm + frame_uid: block_3 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.14 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_3 + control_part: left_arm + tcp_from_object: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + - phase_id: block_3_place + dependencies: + - block_3_lift + targets: + - control_part: left_arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.08 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.07 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_3 + control_part: left_arm + tcp_from_object: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + - phase_id: block_3_retreat + dependencies: + - block_3_place + targets: + - control_part: left_arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.08 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.22 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 +scheduling: + policy: fifo + pool_size: 8 + exploration_fraction: 0.25 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/integration.yaml new file mode 100644 index 0000000000..63e3ac866d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/integration.yaml @@ -0,0 +1,82 @@ +integration_id: blocks_ranking_rgb_scene_first_dual_ur5 +program_id: rank_blocks_rgb +requires: + scene_contract: blocks_ranking_rgb_scene_v1 + embodiment_contract: dual_arm_parallel_gripper +scene_binding: + contract_id: blocks_ranking_rgb_scene_v1 + registry_id: task_program_blocks_ranking_rgb + rigid_objects: + - entity_id: block_1 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: block_1_grasp + kind: antipodal_grasp + - entity_id: block_2 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: red_placement + kind: support_surface + native_name: red_neighbor + object_target_frame: world_axes + object_target_pose: + position: + - -0.08 + - 0.0 + - 0.08 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - entity_id: blue_placement + kind: support_surface + native_name: blue_neighbor + object_target_frame: world_axes + object_target_pose: + position: + - 0.08 + - 0.0 + - 0.08 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - entity_id: block_3 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: block_3_grasp + kind: antipodal_grasp +profile: + defaults: + pick_up: + primary: primary_manipulator + place: + primary: primary_manipulator + skill_options: + pick: + kind: pick_up + pre_grasp_distance: 0.12 + lift_height: 0.15 + hand_interp_steps: 10 + grasp_settle_steps: 45 + fixed_object_to_eef: + position: + - 0.0 + - 0.0 + - -0.01 + quaternion_xyzw: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + place: + kind: place + lift_height: 0.15 + hand_interp_steps: 10 + preserve_current_object_orientation: true + effect_monitors: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/program.yaml new file mode 100644 index 0000000000..9f8ecd558b --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/program.yaml @@ -0,0 +1,86 @@ +program_id: rank_blocks_rgb +instruction: Arrange the red, green, and blue blocks from left to right while keeping the green block + stationary. +targets: {} +program: +- segment: pick_red_block + instruction: Grasp and lift red block clear of the table. + steps: + pick: + object: block_1 + grasp: block_1_grasp + resources: + primary: primary_manipulator + validators: + - kind: object_lifted + object: block_1 + minimum_lift: 0.05 +- segment: place_red_block + instruction: Place the red block left of the green block, release it, and let it settle. + steps: + place: + object: block_1 + 'on': red_placement + resources: + primary: primary_manipulator + post: + - kind: wait_stable + entity: block_1 + validators: + - kind: object_relative_position + object: block_1 + reference: block_2 + position_offset: + - -0.08 + - 0.0 + - 0.0 + position_tolerance: + - 0.035 + - 0.03 + - null +- segment: pick_blue_block + instruction: Grasp and lift blue block clear of the table. + steps: + pick: + object: block_3 + grasp: block_3_grasp + resources: + primary: secondary_manipulator + validators: + - kind: object_lifted + object: block_3 + minimum_lift: 0.05 +- segment: place_blue_block + instruction: Place the blue block right of the green block, release it, and let it settle. + steps: + place: + object: block_3 + 'on': blue_placement + resources: + primary: secondary_manipulator + post: + - kind: wait_stable + entity: block_3 + validators: + - kind: object_relative_position + object: block_3 + reference: block_2 + position_offset: + - 0.08 + - 0.0 + - 0.0 + position_tolerance: + - 0.035 + - 0.03 + - null + - kind: object_relative_position + object: block_1 + reference: block_2 + position_offset: + - -0.08 + - 0.0 + - 0.0 + position_tolerance: + - 0.035 + - 0.03 + - null diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/task.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/task.yaml new file mode 100644 index 0000000000..f63099d29e --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/task.yaml @@ -0,0 +1,18 @@ +id: ConditionalSceneBlocksRankingRGB-DualUR5-v1 +environment: + component: env.yaml +task_program: + program: program.yaml + integration: integration.yaml + execution_policy: ../../../../../../components/execution_policies/trajectory_open_loop_dual_arm_manipulation.yaml +embodiment: + component: ../../../../../../components/embodiments/dual_ur5_dh_pgi_140_80.yaml + overrides: + init_pos: + - 1.683 + - 0.0 + - 0.5275 + init_rot: + - 0.0 + - 0.0 + - -90.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/README.md new file mode 100644 index 0000000000..458e189193 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/README.md @@ -0,0 +1,9 @@ +# Conditional scene-first task fixture + +This deployment keeps the task's declared controller, physical acceptance and +confirmed persistence gates while comparing bounded versus scene-first XY layouts. +`expansion.yaml` owns explicit support parents, sampling ranges, the task phase +graph and screening effort. It does not change the ordinary deployment defaults. + +See the [cross-task acceptance guide](../../../../../../../../../scripts/benchmark/expansion/README.scene_first.md) for commands, conditional +anchor semantics, scope restrictions and result interpretation. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/env.yaml new file mode 100644 index 0000000000..a2ca89425d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/env.yaml @@ -0,0 +1,99 @@ +environment_id: conditional_stack +physics: default +max_episodes: 1 +max_episode_steps: 2400 +num_envs: 1 +arena_space: 2.5 +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: + - 0.6 + - 0.6 + - 0.6 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + - uid: rect_light + light_type: rect + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 8.0 + init_pos: + - -0.42 + - 0.08 + - 0.8 + direction: + - 0.0 + - 0.0 + - -1.0 + rect_width: 0.5 + rect_height: 0.5 + background: + - uid: support_surface + shape: + shape_type: Cube + size: + - 2.0 + - 2.0 + - 0.1 + body_type: static + init_pos: + - 0.0 + - 0.0 + - -0.05 + rigid_object: + - uid: block_1 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + body_type: dynamic + init_pos: + - -0.4 + - 0.48 + - 0.025 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + - uid: block_2 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + body_type: dynamic + init_pos: + - -0.42 + - -0.08 + - 0.025 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + rigid_object_group: [] + articulation: [] +env: + sim_steps_per_control: 4 + events: {} + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/expansion.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/expansion.yaml new file mode 100644 index 0000000000..f42b1e302c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/expansion.yaml @@ -0,0 +1,296 @@ +schema_version: 1 +engine: + episodes_per_operator: 4 + max_attempts: 64 + max_steps: 2400 + max_wall_time_s: 600.0 + settling_steps: 30 + capture_media: false + collision_backend: newton + collision_device: cpu +scene: + block_1: + translation_m: + - - -0.06 + - 0.06 + - - -0.06 + - 0.06 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 + block_2: + translation_m: + - - -0.06 + - 0.06 + - - -0.06 + - 0.06 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 +trajectory: + operators: + - object_pose + joint_offset_scale: 0.005 + via_count: 1 + max_proposals: 64 + minimum_joint_delta: 0.002 + velocity_limit: 0.5 + acceleration_limit: 0.5 + nullspace_task_rows: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + place: + - retract +validation: + initial_checks: + - kind: support_bounds + entity: block_2 + lower: + - -1 + - -1 + - -0.003 + upper: + - 1 + - 1 + - 0.1 + final_checks: &id001 + - kind: axis_relation + entity: block_2 + other: block_1 + axis: 2 + lower: 0.04 + upper: 0.062 + - kind: speed_below + entity: block_2 + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: speed_below + entity: block_1 + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: joint_range + joint: gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + events: + - event_id: lift + conditions: + - kind: lifted + entity: block_2 + height_m: 0.05 + - event_id: stacked + conditions: *id001 + consecutive_steps: 3 + allowed_contacts: &id002 + - - block_1 + - default_plane + - - block_1 + - support_surface + - - block_1 + - UR5/gripper_finger1_link_1 + - - block_1 + - UR5/gripper_finger2_link_1 + - - block_2 + - default_plane + - - block_2 + - support_surface + - - block_2 + - UR5/gripper_finger1_link_1 + - - block_2 + - UR5/gripper_finger2_link_1 + - - block_1 + - block_2 + mounting_contacts: + - - '@root' + - default_plane + - - '@root' + - support_surface +scene_sampling: + strategy: scene_first + workspace_source: joint_uniform_fk + control_part: arm + sample_count: 16384 + robot_anchors: + - phase_id: grasp + control_part: arm + uniform_fraction: 0.25 + max_candidates: 256 + max_rollouts: 4 + max_ik_calls: 16384 + max_planning_calls: 2048 + targets: {} + layout: + support_uid: support_surface + parent_by_uid: + block_1: support_surface + block_2: support_surface + policy: + group_targets: + - block_1 + - block_2 + batch_size: 1 + collision_margin_m: 0.02 + group_xy_offset_bounds_m: + block_1: + - - -0.06 + - -0.06 + - - 0.06 + - 0.06 + block_2: + - - -0.06 + - -0.06 + - - 0.06 + - 0.06 + task_mode: + mode_id: runtime + phases: + - phase_id: approach + dependencies: [] + targets: + - control_part: arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.1 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: grasp + dependencies: + - approach + targets: + - control_part: arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: lift + dependencies: + - grasp + targets: + - control_part: arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.1 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_2 + control_part: arm + tcp_from_object: + - - 1 + - 0 + - 0 + - 0 + - - 0 + - -1 + - 0 + - 0 + - - 0 + - 0 + - -1 + - 0 + - - 0 + - 0 + - 0 + - 1 + - phase_id: place + dependencies: + - lift + targets: + - control_part: arm + frame_uid: block_1 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.05 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_2 + control_part: arm + tcp_from_object: + - - 1 + - 0 + - 0 + - 0 + - - 0 + - -1 + - 0 + - 0 + - - 0 + - 0 + - -1 + - 0 + - - 0 + - 0 + - 0 + - 1 +scheduling: + policy: fifo + pool_size: 8 + exploration_fraction: 0.25 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/integration.yaml new file mode 100644 index 0000000000..fb0de39013 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/integration.yaml @@ -0,0 +1,69 @@ +integration_id: stack_blocks_two_v1 +program_id: stack_two_blocks +requires: + scene_contract: stack_blocks_two_scene_v1 + embodiment_contract: single_arm_parallel_gripper +scene_binding: + contract_id: stack_blocks_two_scene_v1 + registry_id: task_program_stack_blocks_two + rigid_objects: + - entity_id: block_1 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: block_1_top + kind: support_surface + native_name: top + object_target_frame: world_axes + object_target_pose: + position: + - 0.0 + - 0.0 + - 0.05 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - entity_id: block_2 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: block_2_grasp + kind: antipodal_grasp +profile: + defaults: + pick_up: + primary: primary_manipulator + place: + primary: primary_manipulator + skill_options: + pick: + kind: pick_up + pre_grasp_distance: 0.12 + lift_height: 0.15 + hand_interp_steps: 10 + grasp_settle_steps: 45 + fixed_object_to_eef: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + place: + kind: place + lift_height: 0.1 + hand_interp_steps: 10 + preserve_current_object_orientation: false + effect_monitors: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/program.yaml new file mode 100644 index 0000000000..8ef78b03e3 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/program.yaml @@ -0,0 +1,44 @@ +program_id: stack_two_blocks +instruction: Stack the two blocks into a stable tower with block 2 on top of block + 1. +targets: {} +program: +- segment: pick_block_2 + instruction: Grasp and lift block 2 clear of the table. + steps: + pick: + object: block_2 + grasp: block_2_grasp + validators: + - kind: object_lifted + object: block_2 + minimum_lift: 0.05 +- segment: place_block_2_on_block_1 + instruction: Place block 2 on top of block 1, release it, and let the stack settle. + steps: + place: + object: block_2 + 'on': block_1_top + post: + - kind: wait_stable + entity: block_2 + - kind: wait_stable + entity: block_1 + validators: + - kind: object_relative_position + object: block_2 + reference: block_1 + position_offset: + - 0.0 + - 0.0 + - 0.05 + position_tolerance: + - 0.025 + - 0.025 + - 0.012 + - kind: object_upright + object: block_1 + maximum_tilt_degrees: 45.0 + - kind: object_upright + object: block_2 + maximum_tilt_degrees: 45.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/task.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/task.yaml new file mode 100644 index 0000000000..a371b22ba4 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/task.yaml @@ -0,0 +1,19 @@ +id: ConditionalSceneStack-UR5-v1 +environment: + component: env.yaml +task_program: + program: program.yaml + integration: integration.yaml + execution_policy: ../../../../../../components/execution_policies/trajectory_open_loop.yaml +embodiment: + component: ../../../../../../components/embodiments/ur5_dh_pgi_140_80.yaml + overrides: + init_qpos: + - 0 + - -1.57 + - 1.57 + - -1.57 + - -1.57 + - 0 + - 0 + - 0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/README.md new file mode 100644 index 0000000000..dc771fbc51 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/README.md @@ -0,0 +1,109 @@ +# Workspace-conditioned task pose coverage + +This acceptance task picks a 6×4×4 cm cuboid, lifts it, places it at a live +scene-relative XY/yaw target, releases it and waits for stability. Default CPU +physics, the UR5/DH gripper embodiment and the original controller/validation +owners are retained. The nominal robot installation is fixed. The non-colliding +goal reference is below the support plane; its declared +0.12 m offset identifies +the intended object centre. The two opposing physical gripper fingers have +explicit contact permissions, including the mimic finger. + +`expansion.yaml` selects two independent dimensions: + +```yaml +scene_sampling: + strategy: bounded_pose # bounded_pose | workspace_joint +scheduling: + policy: fifo # fifo | task_pose_coverage +``` + +Existing configurations without these settings retain the original sampler and +sequence. `workspace_joint` requires explicit object-to-TCP target matrices and +selects `joint_uniform_fk` or `cache` as its prior source. It does not silently +substitute a cache when none is configured. Both sources retain the same task +execution, measured validation and sink receipt path. Coverage is optional for +FIFO; feedback scheduling requires explicit `pose_coverage` grids. + +The task uses one declared fixed object-to-EEF grasp for all four comparisons. +The joint source verifies that its grasp transform matches the executed policy. +Workspace samples are only proposals: projected XY/yaw is checked again at the +actual height and orientation using IK, joint limits and FK residuals. Runtime +planning and the measured collision/effect gates remain authoritative. A prior +miss retains a labelled uniform fallback; it is not an infeasibility certificate. + +The joint candidate remains immutable for its logical ordinal. Feedback affects +only future candidates. Candidate RNG is independent of physical slot assignment; +adaptive selection also depends on the recorded feedback order, so different +parallel schedules need not produce identical future candidate sequences. + +## Coverage and accounting + +Each fully accepted and persisted episode contributes the measured initial and +terminal cuboid XY/yaw to separate pick/place grids. The cuboid's pi yaw symmetry +is canonicalized. Pick has 144 declared cells; place has 54. These are declared +domain cells, not a certified continuous feasible-volume denominator. Trajectory +variants of the same measured task poses do not increase unique task-pose coverage. + +A measured transport guard latches a failure if, after the first 5 cm lift, +the object falls below 2.5 cm above its initial height while more than 3 cm +from the placement goal in XY. Reaching the target later cannot erase that +failure. This control-boundary guard complements collision/effect validation; +it is not a continuous-contact or substep retention certificate. + +The domain fingerprint includes robot/installation, physical object configuration, +validation settings, ranges and grids. It excludes seed, sampling strategy and +individual candidate poses. SceneCase identities remain distinct. Existing session +receipts own acceptance: pending or failed writes do not increase confirmed counts. +Success-rate feedback records actual executions; preflight failures remain a +separate class. Observed TCP poses are auxiliary measurements, not extra points in +the primary score. + +`pose-sampling.json` retains raw proposals, provenance, selections, outcome order, +resource counts, confirmed coverage and a source checkpoint. Checkpoint loading +reproduces proposal state but is **not** a full live-World or job-resume facility; +ordinary session/sink reconciliation must resolve pending writes before continuing. +The collector still requires a fresh output directory. + +The source scores 75% of eligible choices by Beta(1,1) success estimate times +scarcity, with coarse paired pick/place regions; 25% use the underlying proposal +stream. The workspace prior independently reserves 25% uniform proposals. These +are different probabilities and are recorded separately. Pools are bounded at 64; +all generated candidates and decorated IK/planning calls consume budgets, including +unselected candidates. Physical rollouts have a separate cap from preparations. +Calls are cooperatively bounded, not forcibly preempted inside native code. + +## Acceptance runs + +From the repository root: + +```bash +python -m scripts.benchmark.expansion.task_pose_coverage --suite pilot --output outputs/pose-pilot +python -m scripts.benchmark.expansion.task_pose_coverage --suite smoke --output outputs/pose-smoke +python -m scripts.benchmark.expansion.task_pose_coverage --suite formal --output outputs/pose-formal +python -m scripts.benchmark.expansion.task_pose_coverage --suite installation --output outputs/pose-installations +python -m scripts.benchmark.expansion.task_pose_coverage --suite parallel --output outputs/pose-b4 +python -m scripts.benchmark.expansion.task_pose_coverage --suite trajectory --output outputs/pose-trajectories +python -m scripts.benchmark.expansion.task_pose_coverage --output outputs/pose-formal --rebuild +``` + +A/B/C/D are bounded+FIFO, bounded+feedback, workspace+FIFO, workspace+feedback. +Pilot uses fixed nominal poses and must pass physical acceptance before broad-domain +claims. Smoke uses 12 rollouts per group. Formal runs use both open/obstacle scenes, +five paired seeds, 4096 raw candidates, 128 rollouts, 32768 IK calls, 4096 planning +calls and 1800 seconds per cell. Initialization and prior preparation are included +in end-to-end wall time; pose budgets begin with source construction. Process +startup/import is recorded by the orchestrator log but excluded from worker wall +windows. No warmup is subtracted from these cold measurements. + +The target is a median D/C unique-cell gain >=15%, positive in >=4/5 seeds, +without reduced median pick or place coverage or more than 5 percentage points +acceptance-rate loss. A failed target remains failed; no thresholds are relaxed. +Installation experiments are separate domains; they cannot inflate the nominal +installation score. Obstacle placement and physical criteria are fixed before +formal runs. Run `--media` only for separate illustrative replay; numerical T+1 +evidence and confirmed artifact/LeRobot export are always retained. + +`--suite parallel` verifies real B=4 operation but is not an assertion of identical +adaptive sample order to B=1. Deterministic source checkpoint tests cover feedback +replay and owned candidate reuse. Report wall time, physics outcomes, failures and +coverage separately; never infer physical success from program completion alone. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/README.md new file mode 100644 index 0000000000..de725d51be --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/README.md @@ -0,0 +1,9 @@ +# Conditional scene-first task fixture + +This deployment keeps the task's declared controller, physical acceptance and +confirmed persistence gates while comparing bounded versus scene-first XY layouts. +`expansion.yaml` owns explicit support parents, sampling ranges, the task phase +graph and screening effort. It does not change the ordinary deployment defaults. + +See the [cross-task acceptance guide](../../../../../../../../scripts/benchmark/expansion/README.scene_first.md) for commands, conditional +anchor semantics, scope restrictions and result interpretation. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/env.yaml new file mode 100644 index 0000000000..212bf07c3a --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/env.yaml @@ -0,0 +1,93 @@ +environment_id: conditional_pick_place +physics: default +max_episodes: 1 +max_episode_steps: 2400 +num_envs: 1 +arena_space: 2.5 +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: + - 0.6 + - 0.6 + - 0.6 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + - uid: rect_light + light_type: rect + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 8.0 + init_pos: + - -0.42 + - 0.08 + - 0.8 + direction: + - 0.0 + - 0.0 + - -1.0 + rect_width: 0.5 + rect_height: 0.5 + background: + - uid: goal + shape: + shape_type: Cube + size: + - 0.01 + - 0.01 + - 0.001 + body_type: static + init_pos: + - -0.4 + - 0.48 + - -0.1 + attrs: + collision_props: + collision_enabled: false + - uid: support_surface + shape: + shape_type: Cube + size: + - 2.0 + - 2.0 + - 0.1 + body_type: static + init_pos: + - 0.0 + - 0.0 + - -0.05 + rigid_object: + - uid: cube + shape: + shape_type: Cube + size: + - 0.06 + - 0.04 + - 0.04 + body_type: dynamic + init_pos: + - -0.42 + - -0.08 + - 0.02 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + rigid_object_group: [] + articulation: [] +env: + sim_steps_per_control: 4 + events: {} + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml new file mode 100644 index 0000000000..73e8e9df09 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml @@ -0,0 +1,295 @@ +schema_version: 1 +engine: + episodes_per_operator: 4 + max_attempts: 64 + max_steps: 2400 + max_wall_time_s: 600.0 + settling_steps: 30 + capture_media: false + collision_backend: newton + collision_device: cpu +scene: + cube: + translation_m: + - - -0.06 + - 0.06 + - - -0.06 + - 0.06 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 + goal: + translation_m: + - - -0.06 + - 0.06 + - - -0.06 + - 0.06 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 +trajectory: + operators: + - object_pose + joint_offset_scale: 0.005 + via_count: 1 + max_proposals: 64 + minimum_joint_delta: 0.002 + velocity_limit: 0.5 + acceleration_limit: 0.5 + nullspace_task_rows: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + place: + - retract +validation: + initial_checks: + - kind: support_bounds + entity: cube + lower: + - -1 + - -0.4 + - -0.003 + upper: + - -0.2 + - 0.7 + - 0.05 + final_checks: + - &id001 + kind: pose_relative + entity: cube + other: goal + position_tolerance: + - 0.02 + - 0.02 + - 0.01 + yaw_tolerance_rad: 0.17453292519943295 + yaw_period_rad: 3.141592653589793 + tilt_tolerance_rad: 0.17453292519943295 + position_offset: + - 0 + - 0 + - 0.12 + - &id002 + kind: joint_range + joint: gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + - &id003 + kind: speed_below + entity: cube + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: transport_guard + entity: cube + other: goal + lift_height_m: 0.05 + minimum_transit_height_m: 0.025 + goal_tolerance_m: 0.03 + events: + - event_id: lift + conditions: + - kind: lifted + entity: cube + height_m: 0.05 + - event_id: placed_and_released + conditions: + - *id001 + - *id002 + - *id003 + consecutive_steps: 3 + allowed_contacts: &id004 + - - cube + - default_plane + - - cube + - support_surface + - - cube + - UR5/gripper_finger1_link_1 + - - cube + - UR5/gripper_finger2_link_1 + mounting_contacts: + - - '@root' + - default_plane + - - '@root' + - support_surface +scene_sampling: + strategy: scene_first + workspace_source: joint_uniform_fk + control_part: arm + sample_count: 16384 + robot_anchors: + - phase_id: grasp + control_part: arm + uniform_fraction: 0.25 + max_candidates: 256 + max_rollouts: 4 + max_ik_calls: 16384 + max_planning_calls: 2048 + targets: {} + layout: + support_uid: support_surface + parent_by_uid: + cube: support_surface + policy: + group_targets: + - cube + batch_size: 1 + collision_margin_m: 0.02 + group_xy_offset_bounds_m: + cube: + - - -0.06 + - -0.06 + - - 0.06 + - 0.06 + task_mode: + mode_id: runtime + phases: + - phase_id: approach + dependencies: [] + targets: + - control_part: arm + frame_uid: cube + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.1 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id004 + - phase_id: grasp + dependencies: + - approach + targets: + - control_part: arm + frame_uid: cube + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id004 + - phase_id: lift + dependencies: + - grasp + targets: + - control_part: arm + frame_uid: cube + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.1 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id004 + held_objects: + - entity_uid: cube + control_part: arm + tcp_from_object: + - - 1 + - 0 + - 0 + - 0 + - - 0 + - -1 + - 0 + - 0 + - - 0 + - 0 + - -1 + - 0 + - - 0 + - 0 + - 0 + - 1 + - phase_id: place + dependencies: + - lift + targets: + - control_part: arm + frame_uid: goal + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.12 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id004 + held_objects: + - entity_uid: cube + control_part: arm + tcp_from_object: + - - 1 + - 0 + - 0 + - 0 + - - 0 + - -1 + - 0 + - 0 + - - 0 + - 0 + - -1 + - 0 + - - 0 + - 0 + - 0 + - 1 +scheduling: + policy: fifo + pool_size: 8 + exploration_fraction: 0.25 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/integration.yaml new file mode 100644 index 0000000000..baa11842f9 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/integration.yaml @@ -0,0 +1,47 @@ +integration_id: workspace_pose_pick_place_v1 +program_id: workspace_pose_pick_place +requires: + scene_contract: workspace_pose_pick_place_scene_v1 + embodiment_contract: single_arm_parallel_gripper +scene_binding: + contract_id: workspace_pose_pick_place_scene_v1 + registry_id: task_program_workspace_pose_pick_place + rigid_objects: + - entity_id: cube + dynamics: dynamic + semantic_type: cube + affordances: + - entity_id: cube_grasp + kind: antipodal_grasp + - entity_id: goal + dynamics: static + semantic_type: placement_reference +profile: + defaults: + pick_up: + primary: primary_manipulator + place: + primary: primary_manipulator + skill_options: + pick: + kind: pick_up + fixed_object_to_eef: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + place: + kind: place + effect_monitors: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/program.yaml new file mode 100644 index 0000000000..610056960c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/program.yaml @@ -0,0 +1,44 @@ +program_id: workspace_pose_pick_place +instruction: Pick up the cuboid and place it at the scene-relative goal pose. +targets: + drop_pose: + kind: scene_relative + entity: goal + frame: parent + position: + - 0 + - 0 + - 0.12 + quaternion_xyzw: + - 0 + - 0 + - 0 + - 1 +program: + kind: repeat + count: 1 + body: + kind: sequence + items: + - kind: segment + name: pick_cube + instruction: Grasp and lift the cube. + steps: + kind: invoke + call: + kind: pick + object: cube + - kind: segment + name: place_cube + instruction: Place the cube at the next target position and release it. + steps: + kind: invoke + call: + kind: place + object: cube + at: + kind: target_ref + target: drop_pose + post: + - kind: wait_stable + entity: cube diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/task.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/task.yaml new file mode 100644 index 0000000000..73df857927 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/task.yaml @@ -0,0 +1,19 @@ +id: ConditionalScenePickPlace-UR5-v1 +environment: + component: env.yaml +task_program: + program: program.yaml + integration: integration.yaml + execution_policy: ../../../../../components/execution_policies/trajectory_open_loop.yaml +embodiment: + component: ../../../../../components/embodiments/ur5_dh_pgi_140_80.yaml + overrides: + init_qpos: + - 0 + - -1.57 + - 1.57 + - -1.57 + - -1.57 + - 0 + - 0 + - 0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.obstacle.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.obstacle.yaml new file mode 100644 index 0000000000..3c71af7a46 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.obstacle.yaml @@ -0,0 +1,93 @@ +environment_id: workspace_pose_pick_place-obstacle +physics: default +max_episodes: 1 +max_episode_steps: 1200 +num_envs: 1 +arena_space: 2.5 +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: + - 0.6 + - 0.6 + - 0.6 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + - uid: rect_light + light_type: rect + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 8.0 + init_pos: + - -0.42 + - 0.08 + - 0.8 + direction: + - 0.0 + - 0.0 + - -1.0 + rect_width: 0.5 + rect_height: 0.5 + background: + - uid: goal + shape: + shape_type: Cube + size: + - 0.01 + - 0.01 + - 0.001 + body_type: static + init_pos: + - -0.4 + - 0.48 + - -0.1 + attrs: + collision_props: + collision_enabled: false + - uid: obstacle + shape: + shape_type: Cube + size: + - 0.08 + - 0.08 + - 0.12 + body_type: static + init_pos: + - -0.75 + - -0.2 + - 0.06 + rigid_object: + - uid: cube + shape: + shape_type: Cube + size: + - 0.06 + - 0.04 + - 0.04 + body_type: dynamic + init_pos: + - -0.42 + - -0.08 + - 0.02 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + rigid_object_group: [] + articulation: [] +env: + sim_steps_per_control: 4 + events: {} + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.yaml new file mode 100644 index 0000000000..3639166932 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.yaml @@ -0,0 +1,81 @@ +environment_id: workspace_pose_pick_place +physics: default +max_episodes: 1 +max_episode_steps: 1200 +num_envs: 1 +arena_space: 2.5 +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: + - 0.6 + - 0.6 + - 0.6 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + - uid: rect_light + light_type: rect + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 8.0 + init_pos: + - -0.42 + - 0.08 + - 0.8 + direction: + - 0.0 + - 0.0 + - -1.0 + rect_width: 0.5 + rect_height: 0.5 + background: + - uid: goal + shape: + shape_type: Cube + size: + - 0.01 + - 0.01 + - 0.001 + body_type: static + init_pos: + - -0.4 + - 0.48 + - -0.1 + attrs: + collision_props: + collision_enabled: false + rigid_object: + - uid: cube + shape: + shape_type: Cube + size: + - 0.06 + - 0.04 + - 0.04 + body_type: dynamic + init_pos: + - -0.42 + - -0.08 + - 0.02 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + rigid_object_group: [] + articulation: [] +env: + sim_steps_per_control: 4 + events: {} + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/expansion.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/expansion.yaml new file mode 100644 index 0000000000..74fa2ef3fe --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/expansion.yaml @@ -0,0 +1,201 @@ +schema_version: 1 +engine: + episodes_per_operator: 128 + max_attempts: 4096 + max_steps: 1200 + max_wall_time_s: 1800.0 + settling_steps: 30 + capture_media: false + collision_backend: newton + collision_device: cpu +scene: + cube: + translation_m: + - - -0.48 + - 0.12 + - - -0.22 + - 0.18 + - - 0 + - 0 + yaw_rad: + - -1.5707963267948966 + - 1.5707963267948966 + goal: + translation_m: + - - -0.2 + - 0.1 + - - -0.18 + - 0.12 + - - 0 + - 0 + yaw_rad: + - -1.5707963267948966 + - 1.5707963267948966 +trajectory: + operators: + - object_pose + joint_offset_scale: 0.005 + via_count: 1 + max_proposals: 64 + minimum_joint_delta: 0.002 + velocity_limit: 0.5 + acceleration_limit: 0.5 + nullspace_task_rows: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + place: + - retract +validation: + initial_checks: + - kind: support_bounds + entity: cube + lower: + - -1 + - -0.4 + - -0.003 + upper: + - -0.2 + - 0.7 + - 0.05 + final_checks: + - &id001 + kind: pose_relative + entity: cube + other: goal + position_tolerance: + - 0.02 + - 0.02 + - 0.01 + yaw_tolerance_rad: 0.17453292519943295 + yaw_period_rad: 3.141592653589793 + tilt_tolerance_rad: 0.17453292519943295 + position_offset: + - 0 + - 0 + - 0.12 + - &id002 + kind: joint_range + joint: gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + - &id003 + kind: speed_below + entity: cube + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: transport_guard + entity: cube + other: goal + lift_height_m: 0.05 + minimum_transit_height_m: 0.025 + goal_tolerance_m: 0.03 + events: + - event_id: lift + conditions: + - kind: lifted + entity: cube + height_m: 0.05 + - event_id: placed_and_released + conditions: + - *id001 + - *id002 + - *id003 + consecutive_steps: 3 + allowed_contacts: + - - cube + - UR5/gripper_finger1_link_1 + - - cube + - UR5/gripper_finger2_link_1 + - - cube + - default_plane + mounting_contacts: + - - '@root' + - default_plane +scene_sampling: + strategy: bounded_pose + workspace_source: joint_uniform_fk + control_part: arm + sample_count: 4096 + uniform_fraction: 0.25 + max_candidates: 4096 + max_rollouts: 128 + max_ik_calls: 32768 + max_planning_calls: 4096 + targets: + cube: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + goal: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.12 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 +scheduling: + policy: fifo + pool_size: 64 + exploration_fraction: 0.25 +pose_coverage: + entity: cube + place_reference: goal + grids: + pick: + lower: + - -0.9 + - -0.3 + - -1.5707963267948966 + upper: + - -0.3 + - 0.1 + - 1.5707963267948966 + cell_size: + - 0.1 + - 0.1 + - 0.5235987755982988 + yaw_period: 3.141592653589793 + place: + lower: + - -0.6 + - 0.3 + - -1.5707963267948966 + upper: + - -0.3 + - 0.6 + - 1.5707963267948966 + cell_size: + - 0.1 + - 0.1 + - 0.5235987755982988 + yaw_period: 3.141592653589793 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.obstacle.ur5.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.obstacle.ur5.yaml new file mode 100644 index 0000000000..c9ba26d82f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.obstacle.ur5.yaml @@ -0,0 +1,19 @@ +id: TaskProgramWorkspacePosePickPlaceObstacle-v1 +environment: + component: env.obstacle.yaml +task_program: + program: task_program/program.yaml + integration: task_program/integration.yaml + execution_policy: ../../../components/execution_policies/trajectory_open_loop.yaml +embodiment: + component: ../../../components/embodiments/ur5_dh_pgi_140_80.yaml + overrides: + init_qpos: + - 0 + - -1.57 + - 1.57 + - -1.57 + - -1.57 + - 0 + - 0 + - 0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.ur5.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.ur5.yaml new file mode 100644 index 0000000000..a70cff410f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.ur5.yaml @@ -0,0 +1,19 @@ +id: TaskProgramWorkspacePosePickPlace-v1 +environment: + component: env.yaml +task_program: + program: task_program/program.yaml + integration: task_program/integration.yaml + execution_policy: ../../../components/execution_policies/trajectory_open_loop.yaml +embodiment: + component: ../../../components/embodiments/ur5_dh_pgi_140_80.yaml + overrides: + init_qpos: + - 0 + - -1.57 + - 1.57 + - -1.57 + - -1.57 + - 0 + - 0 + - 0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/integration.yaml new file mode 100644 index 0000000000..baa11842f9 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/integration.yaml @@ -0,0 +1,47 @@ +integration_id: workspace_pose_pick_place_v1 +program_id: workspace_pose_pick_place +requires: + scene_contract: workspace_pose_pick_place_scene_v1 + embodiment_contract: single_arm_parallel_gripper +scene_binding: + contract_id: workspace_pose_pick_place_scene_v1 + registry_id: task_program_workspace_pose_pick_place + rigid_objects: + - entity_id: cube + dynamics: dynamic + semantic_type: cube + affordances: + - entity_id: cube_grasp + kind: antipodal_grasp + - entity_id: goal + dynamics: static + semantic_type: placement_reference +profile: + defaults: + pick_up: + primary: primary_manipulator + place: + primary: primary_manipulator + skill_options: + pick: + kind: pick_up + fixed_object_to_eef: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + place: + kind: place + effect_monitors: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/program.yaml new file mode 100644 index 0000000000..610056960c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/program.yaml @@ -0,0 +1,44 @@ +program_id: workspace_pose_pick_place +instruction: Pick up the cuboid and place it at the scene-relative goal pose. +targets: + drop_pose: + kind: scene_relative + entity: goal + frame: parent + position: + - 0 + - 0 + - 0.12 + quaternion_xyzw: + - 0 + - 0 + - 0 + - 1 +program: + kind: repeat + count: 1 + body: + kind: sequence + items: + - kind: segment + name: pick_cube + instruction: Grasp and lift the cube. + steps: + kind: invoke + call: + kind: pick + object: cube + - kind: segment + name: place_cube + instruction: Place the cube at the next target position and release it. + steps: + kind: invoke + call: + kind: place + object: cube + at: + kind: target_ref + target: drop_pose + post: + - kind: wait_stable + entity: cube diff --git a/pyproject.toml b/pyproject.toml index 0ff92ba08c..16c1e56e98 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -41,6 +41,7 @@ dependencies = [ "ortools", "prettytable", "psutil>=5.9", + "greenlet>=3.2,<4", "black==26.10.1", "fvcore", "h5py", diff --git a/scripts/benchmark/expansion/README.scene_first.md b/scripts/benchmark/expansion/README.scene_first.md new file mode 100644 index 0000000000..02651c1bf1 --- /dev/null +++ b/scripts/benchmark/expansion/README.scene_first.md @@ -0,0 +1,103 @@ +# Conditional layout sampling acceptance + +Run the three native task fixtures with: + +```bash +python -m scripts.benchmark.expansion.scene_first --pilot --seeds 7 --rollouts 2 --candidates 8 --output outputs/layout-pilot +python -m scripts.benchmark.expansion.scene_first --seeds 7,18,29,40 --rollouts 6 --candidates 48 --seconds 300 --output outputs/layout-comparison +python -m scripts.benchmark.expansion.scene_first --output outputs/layout-comparison --rebuild +``` + +The collector accepts four strategies for layout comparisons. + +| `scene_sampling.strategy` | Proposal | +| --- | --- | +| `bounded_pose` | Draw each declared independent pose in its bounds. A comparison opts into the same layout and task-path screening by providing `layout` and `task_mode`. | +| `scene_first` | Generate the declared layout before checking the robot's task path. | +| `robot_first` | Use a workspace sample to anchor one support group, then generate the remaining layout. | +| `bidirectional` | Alternate scene-first and robot-first by raw ordinal under the same candidate budget. Seed parity chooses the first direction. | + +The legacy default stays `bounded_pose`. Use `--cases` and `--strategies` to +select subsets. The four-seed run balances the order within each task and uses +one raw draw per attempt. The report merges coverage cells across seeds and +retains per-seed timings, rejected proposals and source counts in `results.json`. + +`layout.support_uid` and `parent_by_uid` select real prepared rigid entities and +an acyclic support tree. The shared scene generator prepares scaled geometry and +conservative support/clearance data once. A proposal fixes optional arena-space +`layout.conditions`, then samples the rest of the allowed layout. It does not +step physics, resize a World or mutate runtime asset topology. It preserves +complete support groups rather than independently mixing their coordinates. + +Current group roots support XY translation with authored rotation/height; +member XY/yaw requires an explicitly declared intra-group policy. Root yaw or +Z expansion is rejected rather than silently ignored. Exact member anchors +require their moving ancestors to be anchored. Anchors restrict an existing +domain, not create a new placement permission. No-condition layouts retain the +old addressed RNG stream and continuation behavior. `SceneLayoutCondition` +is also available directly in `embodichain.lab.sim.scene.expansion`. + +Robot-first and bidirectional jobs declare `scene_sampling.robot_anchors`. Each +entry selects a `phase_id` and `control_part` from `task_mode`. The target must +refer to an existing, movable layout root or to a member rigidly attached to one. +Earlier task phases must leave that relation intact. For example, the sorting +fixture alternates the right-arm red pickup and left-arm blue pickup as anchors. + +The proposal source prepares `sample_count` FK samples per selected control +part, from the declared `workspace_source` (`cache` or `joint_uniform_fk`). +It projects each TCP's XY through the target's authored offset to the initial +group, intersects task and layout bounds, and balances selection across +occupied XY cells. The room occupied by the anchor is reserved during layout +completion. An empty prior spends one raw proposal and rejects that draw. The +next draw in the bidirectional stream still follows its assigned direction. +Height and orientation come from the authored root. The projected pose goes +through full-pose IK and task-path collision screening. + +`task_mode` uses existing TaskOperationMode/TaskPhase declarations. Targets name +control parts explicitly and can describe multiple simultaneous arms. Held-object +transforms belong to each incoming phase and must match the intended task; they +are never inferred from a name. The existing feasibility backend selects current +or cached workspace seeds, checks IK, current joint limits and full-pose FK +residuals, and searches collision-checked full-state phase connections. A cache +miss is not a reachability rejection; bounded direct seeds remain available. +A no-solution result means this attempt exhausted its declared search, not proof +that the complete region is unreachable. Every actual Task Program still grounds +and plans again from the measured prepared scene. + +`screening` bounds beam_width, ik_seeds, max_rrt_nodes and planning_timeout_s; +it changes preflight effort, not acceptance thresholds. All raw proposals, +discarded candidates and IK/planning calls consume the declared budgets. Failed +layout or task screening remains preparation rejection and never earns physical +success or a sink receipt. Homogeneous W times B replicas share one prepared +prototype and retain a frozen nominal planning start; eight-row collection is +covered by `tests/lab/expansion/test_layout_sampling_sim.py`. Each proposal still +uses the complete task-path screen before normal measured execution. Collision queries own no physical World and are released on +normal completion and startup failures. A topology revision invalidates the +prepared source instead of silently retaining stale geometry. + +The fixtures reuse native Task Program execution, physical geometry, controllers, +segment validators and measured terminal/event acceptance. The grab/place and +stack fixtures use a rectangular primitive support with the existing UR5 +embodiment. The dual-arm RGB sorting fixture reuses its original program and +dual-UR5 profile: right arm moves red, left arm moves blue, with the green +reference fixed. The arms act sequentially; this does not measure simultaneous +bimanual execution. Its original 5 cm lift and 3.5 / 3 cm XY placement bounds +remain unchanged. Independent acceptance additionally checks both release +states, terminal heights/speeds and the stationary green reference. All +movable topology and physical parameters stay fixed. Only declared XY layouts +vary in the controlled run. Both source strategies share the same task ranges, +path checks, physical thresholds and persistence gate. An initial geometry/path +pass never substitutes physical task completion. + +Reports keep proposed/prepared/executed/accepted/persisted counts separately, +record full failed attempts and numerical T+1 evidence, and audit read-back from +confirmed episode artifacts. Declared scene-grid coverage is not a certified +continuous feasible-volume fraction. Task pose diversity is counted separately; +each fixed sorting placement contributes one category rather than novelty from +tracking noise. Both manipulated blocks contribute initial/placement coverage. +For each strategy the report shows cold wall time per seed and per receipt, +including World preparation, screening, execution and export. The separate +screening figure helps identify where time was spent. + +The acceptance path rejects unsupported phases or geometry and uses the original +task validation thresholds. RGB sorting exercises both arms in the comparison. diff --git a/scripts/benchmark/expansion/runtime_comparison.py b/scripts/benchmark/expansion/runtime_comparison.py new file mode 100644 index 0000000000..8d8c00d0cd --- /dev/null +++ b/scripts/benchmark/expansion/runtime_comparison.py @@ -0,0 +1,449 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Compare sensor-free Task Program execution on pinned source trees. + +Run with --help. Results measure execution, not qualified dataset production. +""" + +from __future__ import annotations +import argparse +import json +import os +from pathlib import Path +import sys +import time +import resource +import subprocess +import cProfile +import pstats + + +def write_report(path: Path, rows: list[dict]) -> None: + """Write three tables, explicitly separating program acceptance from quality.""" + text = [ + "# Sensor-free execution diagnostic", + "", + ( + "Profiler enabled: these times diagnose hotspots and must not be compared as benchmark latency." + if any(row.get("profiled") for row in rows) + else "Profiler disabled." + ), + "", + "Cold execution; CPU Default physics and solvers, with legacy CUDA grasp classification included. Program acceptance is not physical qualification. No sensors, recording, external collision checks or persistence. Process RSS peaks include startup; their sum is an upper bound, not a simultaneous sampled peak.", + "", + "## Time & Memory", + "", + "| Scale | Mode | cost_time_ms | Action rows/s | Sum env.step s | Sum sim.update s | Sum peak RSS MiB | cpu_delta_mb | gpu_delta_mb | peak_gpu_mb |", + "|---|---|---:|---:|---:|---:|---:|---|---|---|", + ] + for row in rows: + text.append( + f"| {row['scale']} | {row['mode']} | {row['execution_s']*1000:.3f} | {row['action_rows']/row['execution_s']:.1f} | {row['env_step_s']:.3f} | {row['sim_update_s']:.3f} | {row['peak_rss_mib']:.1f} | N/A | N/A | N/A |" + ) + text += [ + "", + "## Success & Other Metrics", + "", + "| Scale | Mode | Attempts | Program accepted | Action rows |", + "|---|---|---:|---:|---:|", + ] + for row in rows: + text.append( + f"| {row['scale']} | {row['mode']} | {row['attempts']} | {row['accepted']} | {row['action_rows']} |" + ) + text += [ + "", + "## Leaderboard", + "", + "Ranked by program acceptance fraction only; modes with different action counts are not equal-work latency comparisons.", + "", + "| Rank | Scale | Mode | Program acceptance |", + "|---:|---|---|---:|", + ] + for i, row in enumerate( + sorted(rows, key=lambda r: r["accepted"] / max(r["attempts"], 1), reverse=True), + 1, + ): + text.append( + f"| {i} | {row['scale']} | {row['mode']} | {row['accepted']/max(row['attempts'],1):.3f} |" + ) + path.write_text("\n".join(text) + "\n") + + +def matrix_main() -> None: + """Run variants sequentially, with concurrent main workers per scale.""" + p = argparse.ArgumentParser() + p.add_argument("--matrix", action="store_true") + p.add_argument("--baseline-root", type=Path, required=True) + p.add_argument("--pr-root", type=Path, required=True) + p.add_argument("--output-dir", type=Path, required=True) + p.add_argument("--waves", type=int, default=2) + p.add_argument("--seeds", default="7") + p.add_argument("--scales", default="small,medium,large") + p.add_argument("--order", choices=("forward", "reverse"), default="forward") + a = p.parse_args() + a.output_dir.mkdir(parents=True, exist_ok=False) + cpus = sorted(os.sched_getaffinity(0))[:8] + os.sched_setaffinity(0, cpus) + settings = {"small": (1, 1), "medium": (2, 4), "large": (4, 8)} + rows = [] + for seed_index, seed in enumerate(map(int, a.seeds.split(","))): + for scale in a.scales.split(","): + w, b = settings[scale] + modes = ["cohort", "rolling-off", "rolling-on"] + if (seed_index % 2) ^ (a.order == "reverse"): + modes.reverse() + for mode in modes: + processes = [] + for worker in range(w if mode == "cohort" else 1): + name = f"{scale}-{mode}-seed{seed}-worker{worker}" + output = a.output_dir / (name + ".json") + log = (a.output_dir / (name + ".log")).open("w") + command = [ + sys.executable, + str(Path(__file__).resolve()), + "--root", + str(a.baseline_root if mode == "cohort" else a.pr_root), + "--mode", + mode, + "--rows", + str(b if mode == "cohort" else w * b), + "--groups", + str(1 if mode == "cohort" else w), + "--episodes", + str(a.waves * b if mode == "cohort" else a.waves * w * b), + "--seed", + str(seed), + "--output", + str(output), + ] + process = subprocess.Popen( + command, + stdout=log, + stderr=subprocess.STDOUT, + env={ + **os.environ, + "OMP_NUM_THREADS": "1", + "MKL_NUM_THREADS": "1", + "PYTHONHASHSEED": "0", + "SDL_VIDEODRIVER": "offscreen", + }, + ) + processes.append((process, log, output)) + failed = [] + for process, log, output in processes: + code = process.wait() + log.close() + if code: + failed.append(str(output.with_suffix(".log"))) + if failed: + raise RuntimeError(f"Workers failed: {failed}") + reports = [json.loads(output.read_text()) for _, _, output in processes] + episodes = [e for report in reports for e in report["episodes"]] + row = { + "scale": scale, + "mode": mode, + "seed": seed, + "execution_s": max(r["execution_end"] for r in reports) + - min(r["execution_start"] for r in reports), + "env_step_s": sum(r["env_step_s"] for r in reports), + "sim_update_s": sum(r["sim_update_s"] for r in reports), + "peak_rss_mib": sum(r["peak_rss_mib"] for r in reports), + "attempts": len(episodes), + "accepted": sum(e["program_accepted"] for e in episodes), + "action_rows": sum(e["steps"] for e in episodes), + } + rows.append(row) + (a.output_dir / "results.json").write_text( + json.dumps( + { + "cpu_affinity": cpus, + "initial_order": a.order, + "waves": a.waves, + "baseline_root": str(a.baseline_root), + "pr_root": str(a.pr_root), + "rows": rows, + }, + indent=2, + ) + ) + write_report(a.output_dir / "report.md", rows) + print(json.dumps(row), flush=True) + + +def main() -> None: + """Execute a bounded real-physics diagnostic in one isolated process.""" + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--root", type=Path, required=True) + p.add_argument( + "--mode", choices=["cohort", "rolling-off", "rolling-on"], required=True + ) + p.add_argument("--rows", type=int, default=1) + p.add_argument("--groups", type=int, default=1) + p.add_argument("--episodes", type=int, default=1) + p.add_argument("--seed", type=int, default=7) + p.add_argument("--output", type=Path, required=True) + p.add_argument( + "--profile", + action="store_true", + help="Profile execution only; these times are not benchmark results.", + ) + a = p.parse_args() + root = a.root.resolve() + sys.path[:0] = [ + str(root), + str(root / "extensions/embodichain_tasks"), + str(root / "extensions/embodichain_robots"), + ] + import torch + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.utils.utility import load_config + from embodichain.lab.expansion.program import collect_program_batch + + torch.set_num_threads(1) + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + discover_task_packages() + execute_init_hooks() + cfg = config_to_cfg(load_config(path), source_path=path) + cfg.num_envs = cfg.sim_cfg.num_envs = a.rows + cfg.sim_cfg.device = "cpu" + cfg.sim_cfg.headless = True + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sensor = [] + cfg.enable_sensor = False + cfg.dataset = None + cfg.init_rollout_buffer = cfg.record_trajectory = False + timings = {"env_step_s": 0.0, "reset_s": 0.0} + sim_time = [0.0] + episodes = [] + adapters = {} + env = None + start = time.perf_counter() + try: + env = REGISTERED_ENVS[load_config(path)["id"]].make(cfg=cfg) + assert env.sim.physics_backend == "default" + assert not env.sim.get_sensor_uid_list(), env.sim.get_sensor_uid_list() + env.reset(seed=a.seed) + program = env.compile_task_program(cfg.task_program) + step = env.step + reset = env.reset + + def timed_step(*args, **kwargs): + tick = time.perf_counter() + try: + return step(*args, **kwargs) + finally: + timings["env_step_s"] += time.perf_counter() - tick + + def timed_reset(*args, **kwargs): + tick = time.perf_counter() + try: + return reset(*args, **kwargs) + finally: + timings["reset_s"] += time.perf_counter() - tick + + env.step = timed_step + env.reset = timed_reset + update = env.sim.update + + def timed_update(*args, **kwargs): + tick = time.perf_counter() + try: + return update(*args, **kwargs) + finally: + sim_time[0] += time.perf_counter() - tick + + env.sim.update = timed_update + setup = time.perf_counter() - start + + def bridge(row): + adapter = env.task_program_adapter.for_env_ids((row,)) + adapters[row] = adapter + return adapter.create_bridge(program, isolate_policy_sessions=True) + + def completed(result): + for row, length, accepted, reason in zip( + result.env_ids, result.lengths, result.accepted, result.reasons + ): + episodes.append( + { + "row": row, + "steps": length, + "program_accepted": accepted, + "reason": reason, + } + ) + adapters.pop(row).close() + + profiler = cProfile.Profile() if a.profile else None + cpu_before = resource.getrusage(resource.RUSAGE_SELF) + if profiler is not None: + profiler.enable() + begin = time.perf_counter() + before = env.sim.physics_step_index + planning = {} + if a.mode == "cohort": + remaining = a.episodes + while remaining: + n = min(a.rows, remaining) + env.reset(seed=a.seed, options={"save_data": False}) + # Same minimum settling, at ordinary environment cadence. + from embodichain.lab.gym.envs.types import ControllerAction + + physical = env.sim.physics_step_index + while env.sim.physics_step_index - physical < 5: + env.step(ControllerAction(env.robot.get_qpos(target=True).clone())) + env._elapsed_steps[:] = 0 + env._reset_physical_objective(tuple(range(n))) + result = collect_program_batch( + env, + {r: bridge(r) for r in range(n)}, + max_steps=1200, + observe_step=lambda *args: None, + ) + completed(result) + remaining -= n + else: + from embodichain.lab.expansion.workloads import ( + WorkloadProgramExecutor, + WorkloadExecutionCfg, + ExpansionWorkloadCfg, + ) + + class Source: + def prepare(self, assignments): + pass + + def activate(self, assignment, observation): + return bridge(assignment.env_id) + + def started(self, assignment, boundary): + pass + + def observe(self, assignments, action, observation): + return () + + def complete(self, result): + completed(result.program) + # No persistence in this diagnostic: never fabricate receipts. + return None + + assert a.rows % a.groups == 0 and a.episodes % a.groups == 0 + count = a.episodes // a.groups + executor = WorkloadProgramExecutor( + env, + [ + ExpansionWorkloadCfg(f"g{i}", count, count, 3600) + for i in range(a.groups) + ], + Source(), + WorkloadExecutionCfg( + workload_count=a.groups, + batch_size=a.rows // a.groups, + max_steps=1200, + settling_steps=5, + seed=a.seed, + batch_planning=a.mode == "rolling-on", + ), + ) + outcome = executor.run() + planning = outcome["planning"] + elapsed = time.perf_counter() - begin + if profiler is not None: + profiler.disable() + cpu_after = resource.getrusage(resource.RUSAGE_SELF) + report = { + "mode": a.mode, + "rows": a.rows, + "seed": a.seed, + "setup_s": setup, + "execution_s": elapsed, + "profiled": a.profile, + "physics_backend": env.sim.physics_backend, + "physics_device": str(env.sim.device), + "solver_devices": { + name: str(solver.device) for name, solver in env.robot._solvers.items() + }, + "grasp_collision_device_policy": "legacy current CUDA device", + "cuda_visible_devices": os.environ.get("CUDA_VISIBLE_DEVICES"), + "cuda_device_after_execution": ( + torch.cuda.current_device() if torch.cuda.is_initialized() else None + ), + "process_cpu_s": cpu_after.ru_utime + + cpu_after.ru_stime + - cpu_before.ru_utime + - cpu_before.ru_stime, + "execution_start": begin, + "execution_end": begin + elapsed, + "sim_update_s": sim_time[0], + "peak_rss_mib": resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024, + **timings, + "remaining_execution_s": elapsed - sum(timings.values()), + "physics_substeps": env.sim.physics_step_index - before, + "episodes": episodes, + "planning": planning, + "sensors": env.sim.get_sensor_uid_list(), + "scope": "cold execution including planning, preparation and env.step; no sensors, external collision qualification, media, artifact persistence or export; program acceptance is not physical qualification", + } + a.output.parent.mkdir(parents=True, exist_ok=True) + if profiler is not None: + profiler.dump_stats(str(a.output.with_suffix(".pstats"))) + with a.output.with_suffix(".profile.txt").open("w") as stream: + stream.write( + "Profiled execution only. Cumulative times overlap; greenlet switching can distort caller attribution. Use leaf self-times and call counts, not sums of cumulative times.\n" + ) + stats = pstats.Stats(profiler, stream=stream) + stats.sort_stats("tottime").print_stats(70) + stats.sort_stats("cumulative").print_stats(70) + a.output.write_text(json.dumps(report, indent=2)) + write_report( + a.output.with_suffix(".md"), + [ + { + **report, + "scale": str(a.rows), + "attempts": len(episodes), + "accepted": sum(e["program_accepted"] for e in episodes), + "action_rows": sum(e["steps"] for e in episodes), + } + ], + ) + print( + json.dumps( + {k: v for k, v in report.items() if k not in ("episodes", "planning")} + ), + flush=True, + ) + finally: + for adapter in adapters.values(): + adapter.close() + if env is not None: + env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + matrix_main() if "--matrix" in sys.argv else main() diff --git a/scripts/benchmark/expansion/scene_first.py b/scripts/benchmark/expansion/scene_first.py new file mode 100644 index 0000000000..e63342f3b8 --- /dev/null +++ b/scripts/benchmark/expansion/scene_first.py @@ -0,0 +1,635 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Compare four layout proposal strategies on three native Task Programs. + +Run: python -m scripts.benchmark.expansion.scene_first --output DIR +Use --pilot first. Each case uses a fresh process, Default CPU physics, the +same full-state geometric/task-path screens and measured acceptance/receipts. +""" + +from __future__ import annotations + +import argparse +from collections import Counter, defaultdict +import hashlib +import importlib.metadata +import json +import os +from pathlib import Path +import resource +from statistics import median +import subprocess +import sys +import time +import numpy as np +import yaml + +ROOT = Path(__file__).resolve().parents[3] +TASKS = ROOT / "extensions/embodichain_tasks/configs/tasks/manipulation" +CASES = { + "pick_place": TASKS / "workspace_pose_pick_place/acceptance/scene_first", + "stack": TASKS / "tableware/stack_blocks_two/acceptance/scene_first", + "dual_arm_sort": TASKS / "tableware/blocks_ranking_rgb/acceptance/scene_first", +} +STRATEGIES = ("bounded_pose", "scene_first", "robot_first", "bidirectional") +PLACEMENTS = { + "pick_place": {"cube": ("goal", (0.0, 0.0))}, + "stack": {"block_2": ("block_1", (0.0, 0.0))}, + "dual_arm_sort": { + "block_1": ("block_2", (-0.08, 0.0)), + "block_3": ("block_2", (0.08, 0.0)), + }, +} + + +def _write(path, value): + path.write_text(json.dumps(value, indent=2, default=str) + "\n") + + +def _xy_cell(pose, origin, bounds): + """Four fixed XY bins per authored nonzero axis; never fit bins to results.""" + delta = np.asarray(pose)[:2] - np.asarray(origin)[:2] + lo, hi = np.array(bounds)[:2, 0], np.array(bounds)[:2, 1] + if np.any(delta < lo - 1e-5) or np.any(delta > hi + 1e-5): + return None + return tuple( + min(3, max(0, int(np.floor(4 * (v - a) / (b - a))))) if b > a else 0 + for v, a, b in zip(delta, lo, hi) + ) + + +def _confirmed_episodes(root): + """Read only receipt-confirmed evidence; a staged NPZ is not a commit.""" + from embodichain.lab.expansion.persistence import ( + ArtifactEpisodeSink, + load_expansion_episode, + ) + + if not root.exists(): + return () + return tuple( + load_expansion_episode(receipt.storage_id) + for receipt in ArtifactEpisodeSink(root).inspect() + if receipt.confirmed + ) + + +def _task_pose_cells(episode, case, cfg, nominal): + """Count each manipulated object on frozen initial and placement domains.""" + cells = set() + for uid, (goal, offset) in PLACEMENTS[case].items(): + values = episode.observations[f"rigid/{uid}/state"].cpu().numpy() + initial_uid = cfg["scene"][uid].get("follow", uid) + initial = _xy_cell( + values[0], nominal[uid], cfg["scene"][initial_uid]["translation_m"] + ) + if initial is not None: + cells.add((uid, "initial", *initial)) + bounds = cfg["scene"].get(goal, {}).get("translation_m", [[0, 0]] * 3) + if all(pair[0] == pair[1] for pair in bounds[:2]): + # A fixed target contributes one category after measured acceptance; + # tracking error inside its tolerance cannot create pose diversity. + cells.add((uid, "placed-fixed")) + else: + origin = np.asarray(nominal[goal], dtype=float).copy() + origin[:2] += offset + placed = _xy_cell(values[-1], origin, bounds) + if placed is not None: + cells.add((uid, "placed", *placed)) + return cells + + +def _strategy_order(strategies: list[str], seed_index: int) -> list[str]: + """Balance position and preceding strategy over four seeds in the full comparison.""" + width = len(strategies) + order = [0, *(i // 2 + 1 if i % 2 else width - i // 2 for i in range(1, width))] + return [strategies[(index + seed_index) % width] for index in order] + + +def _source_breakdown(events, checks): + """Attribute screened and executed outcomes to the actual raw source.""" + origins = defaultdict(Counter) + selections = {} + rejected = Counter() + for e in events: + if e["event"] == "proposal": + source = origins[e["origin"]] + source["proposed"] += 1 + source["screened"] += int(e["screened"]) + if not e["screened"]: + rejected[e["reason"]] += 1 + elif e["origin"] == "robot_first": + anchor = checks["workspace_anchor_checks"][str(e["raw_ordinal"])] + actual = e["pose_changes"][anchor["root_uid"]] + if not np.allclose(actual, anchor["pose"], atol=1e-9, rtol=0): + raise ValueError( + "Conditioned layout did not preserve the workspace anchor" + ) + elif e["event"] == "selection": + selections[e["ordinal"]] = e["origin"] + elif e["event"] == "outcome": + source = origins[selections[e["ordinal"]]] + for key in ("executed", "physical_accepted", "committed"): + source[key] += int(e[key]) + return {k: dict(v) for k, v in origins.items()}, dict(rejected) + + +def worker(args): + """Execute one isolated source/case/seed with complete failure denominators.""" + sys.path[:0] = [ + str(ROOT), + str(ROOT / "extensions/embodichain_tasks"), + str(ROOT / "extensions/embodichain_robots"), + ] + from embodichain.lab.expansion.task_program_jobs import run_task_program_expansion + + directory = args.output + directory.mkdir(parents=True, exist_ok=False) + cfg = yaml.safe_load((CASES[args.case] / "expansion.yaml").read_text()) + cfg["scene_sampling"]["strategy"] = args.strategy + cfg["engine"].update( + episodes_per_operator=args.rollouts, + max_attempts=args.candidates, + max_wall_time_s=args.seconds, + ) + cfg["scene_sampling"].update( + max_candidates=args.candidates, max_rollouts=args.rollouts + ) + # One raw draw per attempt across all four variants. The screening budgets + # and measured acceptance remain those declared by the fixture. + cfg["scheduling"]["pool_size"] = 1 + if args.pilot: + for value in cfg["scene"].values(): + if "follow" not in value: + value["translation_m"] = [[0, 0]] * 3 + for key in cfg["scene_sampling"]["layout"]["policy"][ + "group_xy_offset_bounds_m" + ]: + cfg["scene_sampling"]["layout"]["policy"]["group_xy_offset_bounds_m"][ + key + ] = [[0, 0], [0, 0]] + config = directory / "expansion.yaml" + config.write_text(yaml.safe_dump(cfg, sort_keys=False)) + files = subprocess.check_output( + ["git", "ls-files", "--cached", "--others", "--exclude-standard"], + cwd=ROOT, + text=True, + ).splitlines() + paths = [ + p + for p in files + if p.startswith( + ( + "embodichain/lab/expansion/", + "embodichain/lab/sim/scene/", + "embodichain/compute/geometry/", + "extensions/embodichain_tasks/configs/tasks/manipulation/", + ) + ) + and (ROOT / p).is_file() + ] + paths.append(str(Path(__file__).resolve().relative_to(ROOT))) + _write( + directory / "source-manifest.json", + {p: hashlib.sha256((ROOT / p).read_bytes()).hexdigest() for p in paths}, + ) + started = time.perf_counter() + result = None + error = None + try: + result = run_task_program_expansion( + CASES[args.case] / "task.yaml", + config, + directory / "collection", + seed=args.seed, + ) + except Exception as exc: + error = f"{type(exc).__name__}: {exc}" + import traceback + + traceback.print_exc() + run_wall_s = time.perf_counter() - started + root = directory / "collection/dataset/object_pose" + source = root / "pose-sampling.json" + data = json.loads(source.read_text()) if source.exists() else {} + events = data.get("events", []) + outcomes = [e for e in events if e["event"] == "outcome"] + screen_path = root / "scene-first-checks.json" + screens = json.loads(screen_path.read_text()) if screen_path.exists() else {} + origins, proposal_rejections = _source_breakdown(events, screens) + failures = {} + for path in root.glob("attempts/*/attempt.json"): + d = json.loads(path.read_text()) + if d.get("status") != "committed": + key = d.get("reason", d.get("status", "unavailable")) + failures[key] = failures.get(key, 0) + 1 + nominal_path = directory / "collection/reference-scene.json" + nominal = ( + json.loads(nominal_path.read_text())["poses_xyzw"] + if nominal_path.exists() + else {} + ) + independent = [uid for uid, opts in cfg["scene"].items() if "follow" not in opts] + joint = set() + marginal = set() + task = set() + audits = [] + committed = 0 + for episode in _confirmed_episodes(root): + committed += 1 + row = [] + for uid in independent: + states = episode.observations[f"rigid/{uid}/state"] + cell = _xy_cell( + states[0].cpu().numpy(), + nominal[uid], + cfg["scene"][uid]["translation_m"], + ) + if cell is not None: + marginal.add((uid, *cell)) + row.append((uid, cell)) + if all(cell is not None for _, cell in row): + joint.add(tuple((uid, *cell) for uid, cell in row)) + task.update(_task_pose_cells(episode, args.case, cfg, nominal)) + steps = len(episode.actions) + if ( + not episode.validation.accepted + or len(episode.timestamps) != steps + 1 + or any(len(v) != steps + 1 for v in episode.observations.values()) + or episode.metadata["source_env_id"] != 0 + ): + raise ValueError("Invalid measured episode evidence at confirmed readback") + arm_motion = {} + if args.case == "dual_arm_sort": + q = episode.observations["full_joint_positions"].cpu().numpy() + names = episode.metadata["joint_names"] + for arm in ("left", "right"): + columns = [ + i for i, name in enumerate(names) if name.startswith(f"{arm}_joint") + ] + arm_motion[arm] = float(np.ptp(q[:, columns], axis=0).max()) + if min(arm_motion.values()) <= 0.1: + raise ValueError( + "Dual-arm sorting requires measured motion in both arms" + ) + audits.append( + { + "episode_id": episode.episode_id, + "T": len(episode.actions), + "observations": len(episode.timestamps), + "source_row": episode.metadata["source_env_id"], + "validation": episode.validation.accepted, + "arm_joint_range_rad": arm_motion, + } + ) + slots_per_uid = { + uid: int( + np.prod( + [ + 4 if pair[1] > pair[0] else 1 + for pair in cfg["scene"][uid]["translation_m"][:2] + ] + ) + ) + for uid in independent + } + metric = { + "case": args.case, + "strategy": args.strategy, + "seed": args.seed, + "pilot": args.pilot, + "budget": data.get("budget", {}), + "declared_budget": { + "candidates": args.candidates, + "rollouts": args.rollouts, + "seconds": args.seconds, + }, + "cpu_affinity": sorted(os.sched_getaffinity(0)), + "cuda_visible_devices": os.environ.get("CUDA_VISIBLE_DEVICES"), + "execution": ( + json.loads((directory / "collection/execution.json").read_text()) + if (directory / "collection/execution.json").exists() + else None + ), + "error": error, + "wall_s": run_wall_s, + "peak_rss_mib": resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024, + "executed": sum(bool(e["executed"]) for e in outcomes), + "physically_accepted": sum(bool(e["physical_accepted"]) for e in outcomes), + "committed": committed, + "scene_joint_cells": len(joint), + "scene_joint_declared_cells": int(np.prod(list(slots_per_uid.values()))), + "scene_marginal_cells": len(marginal), + "scene_marginal_declared_cells": sum(slots_per_uid.values()), + "task_pose_cells": len(task), + "coverage_cells": { + "scene_joint": sorted(joint), + "scene_marginal": sorted(marginal), + "task": sorted(task), + }, + "preparation_rejected": sum(not e["executed"] for e in outcomes), + "proposal_rejections": proposal_rejections, + "origins": origins, + "workspace_prior": screens.get("workspace_prior", []), + "workspace_prepare_wall_s": screens.get("workspace_prepare_wall_s"), + "proposal_screen_wall_s": screens.get("proposal_screen_wall_s"), + "failures": failures, + "episode_audit": audits, + "versions": { + name: importlib.metadata.version(name) + for name in ["torch", "dexsim-engine"] + }, + "result": result, + } + _write(directory / "metrics.json", metric) + print(json.dumps(metric), flush=True) + if error: + raise SystemExit(1) + + +def _aggregate(rows): + """Combine unique receipt-confirmed coverage across seeds in a fixed domain.""" + groups = defaultdict(list) + for row in rows: + groups[row["case"], row["strategy"]].append(row) + result = [] + for (case, strategy), runs in sorted(groups.items()): + origins = defaultdict(Counter) + for run in runs: + for origin, values in run.get("origins", {}).items(): + origins[origin].update(values) + coverage = {} + for kind in ("scene_joint", "scene_marginal", "task"): + if all("coverage_cells" in run for run in runs): + coverage[kind] = len( + { + json.dumps(cell) + for run in runs + for cell in run["coverage_cells"][kind] + } + ) + else: + coverage[kind] = None + totals = { + k: sum(run[k] for run in runs) + for k in ( + "executed", + "physically_accepted", + "committed", + "preparation_rejected", + ) + } + proposed = [run["budget"].get("candidates") for run in runs] + result.append( + dict( + case=case, + strategy=strategy, + seeds=[r["seed"] for r in runs], + raw_proposals=( + None if any(v is None for v in proposed) else sum(proposed) + ), + median_wall_s=median(run["wall_s"] for run in runs), + total_wall_s=sum(run["wall_s"] for run in runs), + median_screen_s=median( + [ + run["proposal_screen_wall_s"] + for run in runs + if run.get("proposal_screen_wall_s") is not None + ] + or [0.0] + ), + peak_rss_mib=max(run["peak_rss_mib"] for run in runs), + scene_joint_declared=max( + run["scene_joint_declared_cells"] for run in runs + ), + scene_marginal_declared=max( + run["scene_marginal_declared_cells"] for run in runs + ), + coverage=coverage, + origins={key: dict(val) for key, val in origins.items()}, + failed_runs=[ + f'{run["seed"]}: {run["error"]}' for run in runs if run["error"] + ], + **totals, + ) + ) + return result + + +def report(output, rows): + """Write aggregated comparisons and preserve seed-level results separately.""" + aggregate = _aggregate(rows) + _write(output / "results.json", rows) + _write(output / "aggregate.json", aggregate) + lines = [ + "# Conditional layout sampling across three tasks", + "", + "Default CPU physics, B=1, sensors disabled. Each strategy uses the same", + "task domains, layout and path checks, runtime replanning, physical", + "acceptance and artifact/LeRobot export. CUDA grasp classification may be", + "used. Cold wall time covers the expansion call, including initialization", + "and export, excluding process startup and benchmark readback. RSS is the", + "process high-water mark; incremental CPU/GPU allocations are unmeasured.", + "", + "## Time & Memory", + "", + "|Task|Strategy|Seeds|Median cost_time_ms|Median screen s|Wall s / receipt|Peak RSS MiB|cpu_delta_mb|gpu_delta_mb|peak_gpu_mb|", + "|---|---|---:|---:|---:|---:|---:|---|---|---|", + ] + for r in aggregate: + per_episode = ( + f'{r["total_wall_s"]/r["committed"]:.2f}' if r["committed"] else "N/A" + ) + lines.append( + f"|{r['case']}|{r['strategy']}|{len(r['seeds'])}|{r['median_wall_s']*1000:.1f}|" + f"{r['median_screen_s']:.2f}|{per_episode}|{r['peak_rss_mib']:.1f}|N/A|N/A|N/A|" + ) + lines += [ + "", + "## Success & Other Metrics", + "", + "|Task|Strategy|Raw proposals|Prep rejected|Accepted / executed|Committed|Marginal XY cells|Joint XY cells|Task pose cells|Failed runs|", + "|---|---|---:|---:|---:|---:|---:|---:|---:|---:|", + ] + for r in aggregate: + coverage = r["coverage"] + number = lambda x: "N/A" if x is None else str(x) + lines.append( + f"|{r['case']}|{r['strategy']}|{number(r['raw_proposals'])}|{r['preparation_rejected']}|" + f"{r['physically_accepted']}/{r['executed']}|{r['committed']}|" + f"{number(coverage['scene_marginal'])}/{r['scene_marginal_declared']}|" + f"{number(coverage['scene_joint'])}/{r['scene_joint_declared']}|" + f"{number(coverage['task'])}|{len(r['failed_runs'])}|" + ) + lines += [ + "", + "## Leaderboard", + "", + "Order within each task by physical acceptance per executed rollout.", + "", + "|Task|Strategy|Physical acceptance|Confirmed episodes|", + "|---|---|---:|---:|", + ] + for r in sorted( + aggregate, + key=lambda r: ( + r["case"], + -r["physically_accepted"] / r["executed"] if r["executed"] else 1.0, + ), + ): + rate = ( + f"{r['physically_accepted']/r['executed']:.3f}" + if r["executed"] + else "N/A (no rollout)" + ) + lines.append(f"|{r['case']}|{r['strategy']}|{rate}|{r['committed']}|") + lines += [ + "", + "Scene coverage is the union of fixed XY grid cells from receipt-confirmed", + "measured initial layouts. Task cells use measured initial/terminal object", + "positions; each fixed placement is a single category. The counted cells", + "describe observed sample diversity in the fixture domains.", + "See results.json for each seed, proposal failures, source usage, recorded", + "budgets, hardware affinity, and readback audit. Aggregated wall times", + "include all recorded runs, including errors.", + ] + (output / "report.md").write_text("\n".join(lines) + "\n") + + +def main(): + """Run fresh worker processes sequentially, alternating source order by seed.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--worker", action="store_true") + parser.add_argument("--pilot", action="store_true") + parser.add_argument("--rebuild", action="store_true") + parser.add_argument("--cases", default=",".join(CASES)) + parser.add_argument("--strategies", default=",".join(STRATEGIES)) + parser.add_argument("--case", choices=tuple(CASES), default="pick_place") + parser.add_argument("--strategy", choices=STRATEGIES, default="scene_first") + parser.add_argument("--seeds", default="7,17") + parser.add_argument("--seed", type=int, default=7) + parser.add_argument("--rollouts", type=int, default=4) + parser.add_argument("--candidates", type=int, default=64) + parser.add_argument("--seconds", type=float, default=600) + args = parser.parse_args() + args.output = args.output.resolve() + if args.worker: + worker(args) + return + if args.rebuild: + report( + args.output, + [ + json.loads(p.read_text()) + for p in sorted(args.output.rglob("metrics.json")) + ], + ) + return + cases = args.cases.split(",") + strategies = args.strategies.split(",") + if any(case not in CASES for case in cases) or len(set(cases)) != len(cases): + parser.error("Select distinct task cases from " + ",".join(CASES)) + if any(s not in STRATEGIES for s in strategies) or len(set(strategies)) != len( + strategies + ): + parser.error("Select distinct strategies from " + ",".join(STRATEGIES)) + args.output.mkdir(parents=True, exist_ok=False) + rows = [] + for case in cases: + for index, seed in enumerate(map(int, args.seeds.split(","))): + for strategy in _strategy_order(strategies, index): + directory = args.output / f"{case}-{strategy}-{seed}" + command = [ + sys.executable, + str(Path(__file__).resolve()), + "--worker", + "--output", + str(directory), + "--case", + case, + "--strategy", + strategy, + "--seed", + str(seed), + "--rollouts", + str(args.rollouts), + "--candidates", + str(args.candidates), + "--seconds", + str(args.seconds), + ] + if args.pilot: + command += ["--pilot"] + attempt_started = time.perf_counter() + env = { + **os.environ, + "OMP_NUM_THREADS": "1", + "MKL_NUM_THREADS": "1", + "SDL_VIDEODRIVER": "offscreen", + } + with (args.output / (directory.name + ".log")).open("w") as log: + try: + proc = subprocess.run( + command, + cwd=ROOT, + env=env, + stdout=log, + stderr=subprocess.STDOUT, + timeout=args.seconds + 180, + ) + worker_error = f"Worker exited with status {proc.returncode}" + except subprocess.TimeoutExpired: + worker_error = "Worker exceeded the hard process timeout" + metrics = directory / "metrics.json" + if not metrics.exists(): + directory.mkdir(exist_ok=True) + _write( + metrics, + { + "case": case, + "strategy": strategy, + "seed": seed, + "budget": {}, + "declared_budget": { + "candidates": args.candidates, + "rollouts": args.rollouts, + "seconds": args.seconds, + }, + "error": worker_error, + "wall_s": time.perf_counter() - attempt_started, + "peak_rss_mib": 0, + "executed": 0, + "physically_accepted": 0, + "committed": 0, + "scene_joint_declared_cells": 0, + "scene_marginal_declared_cells": 0, + "preparation_rejected": 0, + }, + ) + row = json.loads(metrics.read_text()) + rows.append(row) + report(args.output, rows) + if row["error"]: + print( + f"Worker failed: {directory}; diagnostics retained", flush=True + ) + print(args.output / "report.md", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/benchmark/expansion/task_pose_coverage.py b/scripts/benchmark/expansion/task_pose_coverage.py new file mode 100644 index 0000000000..154f438866 --- /dev/null +++ b/scripts/benchmark/expansion/task_pose_coverage.py @@ -0,0 +1,540 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Compare bounded/workspace sampling and FIFO/coverage scheduling. + +Run: python -m scripts.benchmark.expansion.task_pose_coverage --output DIR +Use --suite formal only after the smoke scene qualification succeeds. +The worker uses real physics, measured acceptance and native persistence. +""" + +from __future__ import annotations + +import argparse +from copy import deepcopy +import importlib.metadata +import json +import math +import os +from pathlib import Path +import resource +import subprocess +import sys +import time +import statistics +import yaml + +ROOT = Path(__file__).resolve().parents[3] +TASK = ( + ROOT + / "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place" +) +GROUPS = { + "A": ("bounded_pose", "fifo"), + "B": ("bounded_pose", "task_pose_coverage"), + "C": ("workspace_joint", "fifo"), + "D": ("workspace_joint", "task_pose_coverage"), +} + + +def _write(path: Path, value: object) -> None: + path.write_text(json.dumps(value, indent=2, default=str) + "\n") + + +def _worker(args) -> None: + sys.path[:0] = [ + str(ROOT), + str(ROOT / "extensions/embodichain_tasks"), + str(ROOT / "extensions/embodichain_robots"), + ] + from embodichain.lab.expansion.task_program_jobs import run_task_program_expansion + + args.output.mkdir(parents=True, exist_ok=False) + cfg = yaml.safe_load((TASK / "expansion.yaml").read_text()) + cfg["scene_sampling"]["strategy"], cfg["scheduling"]["policy"] = GROUPS[args.group] + cfg["engine"].update( + episodes_per_operator=args.rollouts, + max_attempts=args.candidates, + batch_size=args.rows, + max_wall_time_s=args.seconds, + ) + cfg["scene_sampling"].update( + max_candidates=args.candidates, + max_rollouts=args.rollouts, + max_ik_calls=args.ik_calls, + max_planning_calls=args.planning_calls, + ) + if args.nominal: + for v in cfg["scene"].values(): + v.update(translation_m=[[0, 0], [0, 0], [0, 0]], yaw_rad=[0, 0]) + cfg["trajectory"]["operators"] = [args.operator] + config = args.output / "configuration.yaml" + config.write_text(yaml.safe_dump(cfg, sort_keys=False)) + deployment = TASK / ( + "task.obstacle.ur5.yaml" if args.scene == "obstacle" else "task.ur5.yaml" + ) + raw = yaml.safe_load(deployment.read_text()) + # Resolve every component before writing an installation-specific deployment. + for key in ("environment", "embodiment"): + raw[key]["component"] = str((TASK / raw[key]["component"]).resolve()) + for key in ("program", "integration", "execution_policy"): + raw["task_program"][key] = str((TASK / raw["task_program"][key]).resolve()) + if args.installation != "nominal": + sign = 1 if args.installation == "positive" else -1 + raw["embodiment"]["overrides"].update( + init_pos=[sign * 0.05, 0, 0], init_rot=[0, 0, sign * 10] + ) + if args.media: + cfg["engine"]["capture_media"] = True + config.write_text(yaml.safe_dump(cfg, sort_keys=False)) + dep = args.output / "deployment.yaml" + dep.write_text(yaml.safe_dump(raw, sort_keys=False)) + import hashlib + + source_paths = subprocess.check_output( + ["git", "ls-files", "--cached", "--others", "--exclude-standard"], + cwd=ROOT, + text=True, + ).splitlines() + selected = [ + ROOT / path + for path in source_paths + if path.startswith( + ( + "embodichain/lab/expansion/", + "embodichain/lab/task_program/", + "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/", + "scripts/benchmark/expansion/task_pose_coverage.py", + ) + ) + and (ROOT / path).is_file() + ] + _write( + args.output / "source-manifest.json", + { + str(p.relative_to(ROOT)): hashlib.sha256(p.read_bytes()).hexdigest() + for p in selected + }, + ) + start = time.perf_counter() + result = None + error = None + try: + result = run_task_program_expansion( + dep, config, args.output / "collection", seed=args.seed + ) + except Exception as exc: + error = f"{type(exc).__name__}: {exc}" + import traceback + + traceback.print_exc() + samples = list((args.output / "collection").rglob("pose-sampling.json")) + data = json.loads(samples[0].read_text()) if samples else {} + events = data.get("events", []) + outcomes = [e for e in events if e["event"] == "outcome"] + confirmed = data.get("coverage", {}).get("confirmed", {}) + cells = {tuple(cell) for _, values in confirmed.values() for cell in values} + counts = {p: sum(c[0] == p for c in cells) for p in ("pick", "place")} + failure = {} + for record in (args.output / "collection").rglob("attempt.json"): + d = json.loads(record.read_text()) + if d.get("status") != "committed": + reason = d.get("reason", d.get("status", "unknown")) + failure[reason] = failure.get(reason, 0) + 1 + versions = { + name: importlib.metadata.version(name) for name in ("torch", "dexsim-engine") + } + summary = { + "source_digest": hashlib.sha256( + (args.output / "source-manifest.json").read_bytes() + ).hexdigest(), + "suite": args.suite, + "declared_budget": { + "rollouts": args.rollouts, + "candidates": args.candidates, + "ik_calls": args.ik_calls, + "planning_calls": args.planning_calls, + "seconds": args.seconds, + }, + "group": args.group, + "scene": args.scene, + "installation": args.installation, + "seed": args.seed, + "rows": args.rows, + "operator": args.operator, + "nominal": args.nominal, + "media": args.media, + "execution_status": "error" if error else "completed", + "error": error, + "wall_s": time.perf_counter() - start, + "peak_rss_mib": resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024, + "committed": len(confirmed), + "physical_accepted": sum(e["physical_accepted"] for e in outcomes), + "executed": sum(e["executed"] for e in outcomes), + "coverage": counts, + "unique_cells": len(cells), + "budget": data.get("budget", {}), + "failures": failure, + "versions": versions, + "result": result, + "coverage_domain": data.get("domain_id"), + "path": str(args.output), + } + _write(args.output / "metrics.json", summary) + print(json.dumps(summary), flush=True) + if error: + raise SystemExit(1) + + +def _report(output: Path, rows: list[dict]) -> None: + _write(output / "results.json", rows) + lines = [ + "# Task pose coverage acceptance", + "", + "Default CPU physics; cold end-to-end preparation, planning, rollout, validation and export. " + "Grasp classification may use CUDA. RSS is a process high-water mark; GPU memory unmeasured. " + "Only receipt-confirmed, physically validated episodes increase coverage.", + "", + "## Time & Memory", + "", + "|Case|Group|Seed|cost_time_ms|peak RSS MiB|cpu_delta_mb|gpu_delta_mb|peak_gpu_mb|", + "|---|---|---:|---:|---:|---|---|---|", + ] + for r in rows: + lines.append( + f"|{r['scene']}/{r['installation']}/B={r['rows']}|{r['group']}|{r['seed']}|{r['wall_s']*1000:.1f}|{r['peak_rss_mib']:.1f}|N/A|N/A|N/A|" + ) + lines += [ + "", + "## Success & Other Metrics", + "", + "|Case|Group|Executed|Physically accepted|Committed|Pick cells|Place cells|Outcome|", + "|---|---|---:|---:|---:|---:|---:|---|", + ] + for r in rows: + lines.append( + f"|{r['scene']}/{r['installation']}|{r['group']}|{r['executed']}|{r['physical_accepted']}|{r['committed']}|{r['coverage']['pick']}|{r['coverage']['place']}|{r['error'] or 'completed'}|" + ) + lines += [ + "", + "## Leaderboard", + "", + "Ranked by measured acceptance rate; coverage gates are evaluated separately.", + "", + "|Group|Accepted / executed|Acceptance rate|Confirmed unique cells (sum across separate runs)|", + "|---|---|---:|---:|", + ] + aggregates = [] + for group in GROUPS: + selected = [r for r in rows if r["group"] == group] + if selected: + n = sum(r["executed"] for r in selected) + k = sum(r["physical_accepted"] for r in selected) + aggregates.append((group, k, n, sum(r["unique_cells"] for r in selected))) + for g, k, n, u in sorted( + aggregates, key=lambda v: v[1] / max(v[2], 1), reverse=True + ): + lines.append(f"|{g}|{k}/{n}|{k/max(n,1):.3f}|{u}|") + gates = {} + for scene in ("open", "obstacle"): + pairs = [] + for seed in (7, 17, 27, 37, 47): + matched = { + r["group"]: r + for r in rows + if r["scene"] == scene + and r["seed"] == seed + and r["installation"] == "nominal" + and not r["nominal"] + and not r["media"] + and r["rows"] == 1 + and r["operator"] == "object_pose" + and r.get("suite") == "formal" + and r.get("execution_status") == "completed" + and r.get("error") is None + and r.get("declared_budget") + == { + "rollouts": 128, + "candidates": 4096, + "ik_calls": 32768, + "planning_calls": 4096, + "seconds": 1800, + } + } + if ( + set(matched) >= {"C", "D"} + and matched["C"].get("coverage_domain") + == matched["D"].get("coverage_domain") + and matched["C"].get("versions") == matched["D"].get("versions") + and matched["C"].get("source_digest") + == matched["D"].get("source_digest") + and bool(matched["C"].get("source_digest")) + ): + pairs.append((matched["C"], matched["D"])) + if len(pairs) != 5 or any(c["unique_cells"] == 0 for c, d in pairs): + gates[scene] = { + "status": "unqualified", + "reason": "Need five complete paired seeds with nonzero baseline coverage", + } + continue + gains = [d["unique_cells"] / c["unique_cells"] - 1 for c, d in pairs] + phase_ok = all( + statistics.median(d["coverage"][p] for c, d in pairs) + >= statistics.median(c["coverage"][p] for c, d in pairs) + for p in ("pick", "place") + ) + rate = lambda r: r["physical_accepted"] / max(1, r["executed"]) + passed = ( + statistics.median(gains) >= 0.15 + and sum(g > 0 for g in gains) >= 4 + and phase_ok + and statistics.median(rate(d) - rate(c) for c, d in pairs) >= -0.05 + ) + gates[scene] = { + "status": "passed" if passed else "failed", + "median_gain": statistics.median(gains), + "positive_seeds": sum(g > 0 for g in gains), + } + _write(output / "acceptance.json", gates) + lines += [ + "", + "## Acceptance gates", + "", + json.dumps(gates, indent=2), + "", + "Raw proposal/outcome logs and receipt checkpoints remain in each run directory. Single-point nominal pilots do not qualify broad-domain improvement.", + ] + (output / "report.md").write_text("\n".join(lines) + "\n") + # Offline plot, without importing simulation during report rebuild. + try: + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + fig, axes = plt.subplots(1, 3, figsize=(15, 4)) + for r in rows: + logs = list(Path(r["path"]).rglob("pose-sampling.json")) + if not logs: + continue + events = [ + e + for e in json.loads(logs[0].read_text())["events"] + if e["event"] == "outcome" + ] + for ax, key in zip(axes, ("proposals", "rollouts", "elapsed_s")): + ax.step( + [e.get(key, 0) for e in events], + [sum(e["coverage"].values()) for e in events], + where="post", + label=f"{r['group']}/{r['scene']}/{r['seed']}", + ) + ax.set_xlabel(key) + ax.set_ylabel("Confirmed task pose cells") + if axes[-1].lines: + axes[-1].legend(fontsize=6) + fig.tight_layout() + fig.savefig(output / "coverage.svg") + plt.close(fig) + except ImportError: + pass + + +def main() -> None: + """Run isolated comparable trials or rebuild reports from their artifacts.""" + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--output", type=Path, required=True) + p.add_argument("--worker", action="store_true") + p.add_argument( + "--seeds", + default=None, + help="Comma-separated seed subset; omitted uses suite defaults", + ) + p.add_argument( + "--scenes", default=None, help="Comma-separated open/obstacle subset" + ) + p.add_argument( + "--suite", + choices=("pilot", "smoke", "formal", "installation", "parallel", "trajectory"), + default="smoke", + ) + p.add_argument("--rebuild", action="store_true") + p.add_argument("--group", choices=tuple(GROUPS), default="A") + p.add_argument("--scene", choices=("open", "obstacle"), default="open") + p.add_argument( + "--installation", choices=("nominal", "positive", "negative"), default="nominal" + ) + p.add_argument("--seed", type=int, default=7) + p.add_argument("--rows", type=int, default=1) + p.add_argument("--rollouts", type=int, default=12) + p.add_argument("--candidates", type=int, default=384) + p.add_argument("--ik-calls", type=int, default=32768) + p.add_argument("--planning-calls", type=int, default=4096) + p.add_argument("--seconds", type=float, default=300) + p.add_argument("--nominal", action="store_true") + p.add_argument("--media", action="store_true") + p.add_argument( + "--operator", choices=("object_pose", "joint_residual"), default="object_pose" + ) + args = p.parse_args() + args.output = args.output.resolve() + if args.worker: + _worker(args) + return + if args.rebuild: + _report( + args.output, + [ + json.loads(f.read_text()) + for f in sorted(args.output.glob("*/metrics.json")) + ], + ) + return + args.output.mkdir(parents=True, exist_ok=False) + seeds = ( + [7, 17, 27, 37, 47] + if args.suite == "formal" + else [7, 17, 27] if args.suite == "installation" else [7] + ) + scenes = ["open", "obstacle"] if args.suite in ("formal", "pilot") else [args.scene] + if args.seeds is not None: + seeds = [int(value) for value in args.seeds.split(",")] + if args.scenes is not None: + scenes = args.scenes.split(",") + if not set(scenes) <= {"open", "obstacle"}: + p.error("Unknown scene") + installations = ( + ["positive", "negative"] if args.suite == "installation" else ["nominal"] + ) + groups = ["C", "D"] if args.suite == "installation" else list(GROUPS) + rows = [] + _write( + args.output / "provenance.json", + { + "head": subprocess.check_output( + ["git", "rev-parse", "HEAD"], cwd=ROOT, text=True + ).strip(), + "diff": subprocess.check_output(["git", "diff"], cwd=ROOT, text=True), + "scope": vars(args), + "cpu_affinity": sorted(os.sched_getaffinity(0)), + }, + ) + for scene in scenes: + for installation in installations: + for i, seed in enumerate(seeds): + for group in ( + groups + if ( + (7, 17, 27, 37, 47).index(seed) + if seed in (7, 17, 27, 37, 47) + else i + ) + % 2 + == 0 + else groups[::-1] + ): + destination = args.output / f"{scene}-{installation}-{group}-{seed}" + n = ( + 128 + if args.suite == "formal" + else ( + 64 + if args.suite == "installation" + else 1 if args.suite == "pilot" else args.rollouts + ) + ) + cap = ( + 4096 + if args.suite == "formal" + else 2048 if args.suite == "installation" else args.candidates + ) + seconds = ( + 1800 + if args.suite == "formal" + else 900 if args.suite == "installation" else args.seconds + ) + command = [ + sys.executable, + str(Path(__file__).resolve()), + "--worker", + "--suite", + args.suite, + "--output", + str(destination), + "--group", + group, + "--scene", + scene, + "--seed", + str(seed), + "--installation", + installation, + "--rows", + str(4 if args.suite == "parallel" else args.rows), + "--rollouts", + str(n), + "--candidates", + str(cap), + "--seconds", + str(seconds), + "--operator", + ( + "joint_residual" + if args.suite == "trajectory" + else args.operator + ), + ] + if args.suite == "installation": + command += ["--ik-calls", "16384", "--planning-calls", "2048"] + if args.suite == "pilot" or args.nominal: + command += ["--nominal"] + if args.media: + command += ["--media"] + with (args.output / (destination.name + ".log")).open("w") as log: + try: + result = subprocess.run( + command, + cwd=ROOT, + env={ + **os.environ, + "OMP_NUM_THREADS": "1", + "MKL_NUM_THREADS": "1", + "SDL_VIDEODRIVER": "offscreen", + }, + stdout=log, + stderr=subprocess.STDOUT, + timeout=seconds + 180, + ) + except subprocess.TimeoutExpired: + raise RuntimeError( + f"Worker timeout; preserve log {log.name}" + ) + if (destination / "metrics.json").exists(): + rows.append( + json.loads((destination / "metrics.json").read_text()) + ) + _report(args.output, rows) + if result.returncode: + raise RuntimeError(f"Worker failed; inspect {destination}") + if args.suite == "pilot" and not rows[-1]["committed"]: + raise RuntimeError( + "Nominal scene qualification failed; do not relax acceptance or narrow the declared domain" + ) + print(args.output / "report.md") + + +if __name__ == "__main__": + main() diff --git a/setup.py b/setup.py index b18acb621a..fdd3eb1f79 100644 --- a/setup.py +++ b/setup.py @@ -153,6 +153,7 @@ def get_package_data() -> dict[str, list[str]]: ] return { "embodichain": ["VERSION"], + "embodichain.lab.expansion": ["_preview.html", "_distribution.html"], "embodichain.lab.visualization": [ "web_assets/*.html", "web_assets/*.js", diff --git a/tests/benchmark/expansion/test_scene_first_report.py b/tests/benchmark/expansion/test_scene_first_report.py new file mode 100644 index 0000000000..5bf5e69be2 --- /dev/null +++ b/tests/benchmark/expansion/test_scene_first_report.py @@ -0,0 +1,161 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Receipt and frozen-domain semantics for cross-task scene acceptance reports.""" + +from __future__ import annotations +from types import SimpleNamespace +import numpy as np +import torch +from scripts.benchmark.expansion.scene_first import ( + STRATEGIES, + _aggregate, + _confirmed_episodes, + _strategy_order, + _task_pose_cells, + _xy_cell, +) + + +def test_each_strategy_runs_once_at_each_position_and_after_each_peer(): + strategies = list(STRATEGIES) + orders = [_strategy_order(strategies, i) for i in range(4)] + assert all(set(column) == set(strategies) for column in zip(*orders)) + assert len({pair for order in orders for pair in zip(order, order[1:])}) == 12 + + +def test_cross_seed_coverage_counts_each_measured_cell_once(): + run = dict( + case="stack", + strategy="bidirectional", + executed=2, + physically_accepted=2, + committed=2, + preparation_rejected=1, + budget={"candidates": 3}, + wall_s=30.0, + peak_rss_mib=2000, + scene_joint_declared_cells=256, + scene_marginal_declared_cells=32, + error=None, + ) + shared = ["a", 0, 0] + rows = [ + dict( + run, + seed=seed, + coverage_cells={ + "scene_joint": [[shared, ["b", seed, 0]]], + "scene_marginal": [shared, ["b", seed, 0]], + "task": [["a", "initial", 0, 0], ["a", "placed", seed, 0]], + }, + ) + for seed in (1, 2) + ] + (result,) = _aggregate(rows) + assert result["coverage"] == {"scene_joint": 2, "scene_marginal": 3, "task": 3} + assert (result["committed"], result["raw_proposals"]) == (4, 6) + + +def test_staged_numerical_file_without_receipt_has_no_coverage_credit(tmp_path): + folder = tmp_path / "episodes" / "staged" + folder.mkdir(parents=True) + (folder / "episode.npz").write_bytes(b"not committed") + assert _confirmed_episodes(tmp_path) == () + + +def test_confirmed_readback_uses_verified_receipt_storage(monkeypatch, tmp_path): + from unittest.mock import Mock + import embodichain.lab.expansion.persistence as persistence + + episode = object() + load = Mock(return_value=episode) + monkeypatch.setattr(persistence, "load_expansion_episode", load) + monkeypatch.setattr( + persistence.ArtifactEpisodeSink, + "inspect", + lambda self: ( + SimpleNamespace(confirmed=True, storage_id="confirmed.npz"), + SimpleNamespace(confirmed=False, storage_id="pending.npz"), + ), + ) + assert _confirmed_episodes(tmp_path) == (episode,) + load.assert_called_once_with("confirmed.npz") + + +def test_xy_bins_are_fixed_and_reject_outside_domain(): + bounds = [[-0.1, 0.1], [-0.1, 0.1], [0, 0]] + assert _xy_cell([-0.1, -0.1, 0], [0, 0, 0], bounds) == (0, 0) + assert _xy_cell([0.1, 0.1, 0], [0, 0, 0], bounds) == (3, 3) + assert _xy_cell([0.11, 0, 0], [0, 0, 0], bounds) is None + assert _xy_cell([0, 0, 0], [0, 0, 0], [[0, 0], [0, 0], [0, 0]]) == (0, 0) + + +def test_both_arms_count_but_fixed_goal_tracking_noise_adds_no_diversity(): + nominal = { + "block_1": [0.625, -0.15, 0.86], + "block_2": [0.675, 0, 0.86], + "block_3": [0.725, 0.15, 0.86], + } + cfg = { + "scene": { + uid: {"translation_m": [[-0.025, 0.025], [-0.03, 0.03], [0, 0]]} + for uid in ("block_1", "block_3") + } + } + episode = SimpleNamespace( + observations={ + "rigid/block_1/state": torch.tensor( + [[0.62, -0.155, 0.86], [0.59, -0.005, 0.86]] + ), + "rigid/block_3/state": torch.tensor( + [[0.72, 0.145, 0.86], [0.76, 0.005, 0.86]] + ), + } + ) + cells = _task_pose_cells(episode, "dual_arm_sort", cfg, nominal) + assert cells == { + ("block_1", "initial", 1, 1), + ("block_3", "initial", 1, 1), + ("block_1", "placed-fixed"), + ("block_3", "placed-fixed"), + } + for value in episode.observations.values(): + value[-1, :2] += 0.01 + assert _task_pose_cells(episode, "dual_arm_sort", cfg, nominal) == cells + + +def test_relative_placement_offsets_use_the_reference_sampling_domain(): + nominal = { + "block_1": [0.625, -0.15, 0.86], + "block_2": [0.675, 0, 0.86], + "block_3": [0.725, 0.15, 0.86], + } + bounds = [[-0.025, 0.025], [-0.03, 0.03], [0, 0]] + cfg = {"scene": {uid: {"translation_m": bounds} for uid in nominal}} + episode = SimpleNamespace( + observations={ + "rigid/block_1/state": torch.tensor( + [nominal["block_1"], [0.575, -0.02, 0.86]] + ), + "rigid/block_3/state": torch.tensor( + [nominal["block_3"], [0.775, 0.02, 0.86]] + ), + } + ) + cells = _task_pose_cells(episode, "dual_arm_sort", cfg, nominal) + assert ("block_1", "placed", 0, 0) in cells + assert ("block_3", "placed", 3, 3) in cells diff --git a/tests/benchmark/expansion/test_task_pose_report.py b/tests/benchmark/expansion/test_task_pose_report.py new file mode 100644 index 0000000000..46ef649d19 --- /dev/null +++ b/tests/benchmark/expansion/test_task_pose_report.py @@ -0,0 +1,78 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Formal evidence gates cannot be satisfied by smoke or incompatible results.""" + +from __future__ import annotations +import json +import pytest +from scripts.benchmark.expansion.task_pose_coverage import _report + + +@pytest.mark.parametrize( + "change", + [ + {"suite": "smoke"}, + {"execution_status": "error", "error": "crash"}, + {"declared_budget": {"rollouts": 12}}, + {"coverage_domain": "other"}, + {"source_digest": "other"}, + ], +) +def test_formal_gate_rejects_incomplete_or_incompatible_records(tmp_path, change): + rows = [] + for seed in (7, 17, 27, 37, 47): + for group in ("C", "D"): + count = 10 if group == "C" else 20 + r = { + "suite": "formal", + "declared_budget": { + "rollouts": 128, + "candidates": 4096, + "ik_calls": 32768, + "planning_calls": 4096, + "seconds": 1800, + }, + "scene": "open", + "installation": "nominal", + "seed": seed, + "group": group, + "rows": 1, + "operator": "object_pose", + "nominal": False, + "media": False, + "execution_status": "completed", + "error": None, + "coverage_domain": "domain", + "versions": {"sdk": "v1"}, + "source_digest": "same", + "coverage": {"pick": count, "place": count}, + "unique_cells": 2 * count, + "executed": 128, + "physical_accepted": 128, + "committed": 128, + "wall_s": 100, + "peak_rss_mib": 100, + "path": str(tmp_path / "missing"), + } + if group == "D": + r.update(change) + rows.append(r) + _report(tmp_path, rows) + assert ( + json.loads((tmp_path / "acceptance.json").read_text())["open"]["status"] + == "unqualified" + ) diff --git a/tests/gen_sim/test_workflow.py b/tests/gen_sim/test_workflow.py index acf1d97d4d..70f68c54dd 100644 --- a/tests/gen_sim/test_workflow.py +++ b/tests/gen_sim/test_workflow.py @@ -259,6 +259,19 @@ def run_cfg(tmp_path: Path) -> _RunCfg: ) +@pytest.mark.parametrize( + "binding,expected", + [({}, 0.02), ({"profile": {}}, 0.02), ({"release_clearance": 0.0}, 0.0)], +) +def test_release_clearance_default_preserves_explicit_contact_placement( + run_cfg, binding, expected +): + original = json.loads(json.dumps(binding)) + cfg = _RunCfg(**{**run_cfg.to_dict(), "binding": binding}) + assert cfg.binding["release_clearance"] == expected + assert binding == original + + @pytest.mark.parametrize("success", [True, False]) def test_workflow_generates_binds_and_preserves_actual_execution_verdict( run_cfg: _RunCfg, monkeypatch: pytest.MonkeyPatch, success: bool @@ -274,7 +287,7 @@ def test_workflow_generates_binds_and_preserves_actual_execution_verdict( run_cfg.instruction += " \n" run_cfg.task_id = " gensim_task " - # Releasing above the resting pose is opt-in; cover both settings. + # Exercise raised release and an explicit zero-clearance contact placement. clearance = 0.03 if success else 0.0 run_cfg = _RunCfg( **{ @@ -1112,8 +1125,12 @@ def _node(node_id: str, call: dict, depends_on: list[str], **semantic) -> dict: @pytest.mark.parametrize("configured", [False, True]) +@pytest.mark.parametrize("clearance", [0.0, 0.02]) def test_relative_placement_validators_follow_the_selected_lowerer( - run_cfg: _RunCfg, monkeypatch: pytest.MonkeyPatch, configured: bool + run_cfg: _RunCfg, + monkeypatch: pytest.MonkeyPatch, + configured: bool, + clearance: float, ) -> None: instruction = "Put the cube right of the table" semantic = dict( @@ -1158,7 +1175,7 @@ def test_relative_placement_validators_follow_the_selected_lowerer( **{ **run_cfg.to_dict(), "instruction": instruction, - "binding": {"runtime_services": services}, + "binding": {"runtime_services": services, "release_clearance": clearance}, } ) @@ -1176,7 +1193,13 @@ def test_relative_placement_validators_follow_the_selected_lowerer( assert validators == [] else: (validator,) = validators - assert lowerer["routes"][0]["world_displacement"] == validator["displacement"] + # The route is the release pose; acceptance still measures the resting + # landing pose. Explicit zero clearance retains contact placement. + landing = validator["displacement"] + release = lowerer["routes"][0]["world_displacement"] + assert release[:2] == landing[:2] + assert landing[2] == pytest.approx(0.005) + assert release[2] == pytest.approx(0.005 + clearance) @pytest.mark.parametrize("lift_height_m", [None, 0.10]) @@ -2059,7 +2082,7 @@ def test_legacy_configured_coordinated_target_survives_binding(run_cfg, monkeypa route = dict( object_id="source", target_id="tray_target", world_displacement=[0.1, 0.0, 0.0] ) - run_cfg.binding = { + binding = { "runtime_services": { "registered_semantic_lowerers": [ dict(kind="coordinated_transport", routes=[route]) @@ -2073,6 +2096,7 @@ def test_legacy_configured_coordinated_target_survives_binding(run_cfg, monkeypa } }, } + run_cfg = _RunCfg(**{**run_cfg.to_dict(), "binding": binding}) run_cfg.output.mkdir() with _services(step=step, plan=proposed) as services: task = plan_image_task( diff --git a/tests/gym/envs/task_program/test_bridge.py b/tests/gym/envs/task_program/test_bridge.py index 1746c21e81..a461f3e8d9 100644 --- a/tests/gym/envs/task_program/test_bridge.py +++ b/tests/gym/envs/task_program/test_bridge.py @@ -1148,6 +1148,39 @@ def test_joint_encoder_emits_full_qpos_and_holds_inactive_rows() -> None: assert torch.equal(action[1], qpos[1]) +@pytest.mark.parametrize("mode", ["position", "position_velocity"]) +def test_joint_encoder_groups_destinations_without_aliasing_source(mode) -> None: + """A complete frame preserves independent destinations and inactive rows.""" + qpos = torch.arange(BATCH_SIZE * ROBOT_DOF, dtype=torch.float32).reshape( + BATCH_SIZE, ROBOT_DOF + ) + source = qpos.clone() + encoder = RuntimeCommandFrameEncoder(_QposProvider(qpos), joint_command_mode=mode) + first = _joint_frame( + duration=STEP_DT, + active_mask=torch.tensor([True, False]), + velocities=torch.tensor([[1.0, 3.0], [11.0, 13.0]]), + ) + extra = EndpointCommand( + target=JointPositionTarget(control_part="second", joint_ids=(0,)), + payload=JointPositionPayload( + positions=torch.tensor([[90.0], [91.0]]), + velocities=torch.tensor([[9.0], [9.1]]), + ), + ) + frame = replace(first, commands=(*first.commands, extra)) + action = encoder.encode(frame) + positions = action if mode == "position" else action["qpos"] + assert torch.equal(positions[0], torch.tensor([90.0, 10.0, 2.0, 30.0, 4.0])) + assert torch.equal(positions[1], source[1]) + if mode == "position_velocity": + assert torch.equal(action["qvel"][0], torch.tensor([9.0, 1.0, 0.0, 3.0, 0.0])) + assert torch.equal(action["qvel"][1], torch.zeros(ROBOT_DOF)) + positions.zero_() + assert torch.equal(qpos, source) + assert torch.equal(extra.payload.positions, torch.tensor([[90.0], [91.0]])) + + def test_joint_encoder_holds_unaddressed_joints_at_controller_targets() -> None: """A scoped command must not chase measured drift on another resource.""" current = torch.zeros(BATCH_SIZE, ROBOT_DOF) diff --git a/tests/gym/envs/task_program/test_catalog.py b/tests/gym/envs/task_program/test_catalog.py index e6af80e2ce..09357122d1 100644 --- a/tests/gym/envs/task_program/test_catalog.py +++ b/tests/gym/envs/task_program/test_catalog.py @@ -1176,6 +1176,32 @@ def registration_for(motion_policy): velocity_limit=None, ) ) + fixed_sequential_policy = replace( + policy, + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + acceleration_limit=None, + velocity_limit=None, + ) + no_phases = registration_for(fixed_sequential_policy) + explicit_empty = registration_for(replace(fixed_sequential_policy, phase_paths={})) + joint_phase = registration_for( + replace(fixed_sequential_policy, phase_paths={"grasp": "joint"}) + ) + cartesian_phase = registration_for( + replace(fixed_sequential_policy, phase_paths={"grasp": "cartesian"}) + ) + assert no_phases.fingerprint == explicit_empty.fingerprint + assert ( + len( + { + no_phases.fingerprint, + joint_phase.fingerprint, + cartesian_phase.fingerprint, + } + ) + == 3 + ) assert default.fingerprint == explicit_default.fingerprint assert len({default.fingerprint, sequential.fingerprint, fixed.fingerprint}) == 3 for registration in (default, explicit_default, sequential, fixed): diff --git a/tests/gym/envs/task_program/test_feasibility.py b/tests/gym/envs/task_program/test_feasibility.py index 9a62f0a910..e61087f2d2 100644 --- a/tests/gym/envs/task_program/test_feasibility.py +++ b/tests/gym/envs/task_program/test_feasibility.py @@ -99,6 +99,44 @@ def test_monitor_requires_measured_lift_then_stable_terminal_and_rejects_reverse assert not monitor.validation(env).accepted +@pytest.mark.parametrize("drop", [False, True]) +def test_event_only_transport_guard_uses_earlier_measured_boundaries(drop): + env, cube = environment() + goal = Cube() + goal.position[0, 0] = 0.5 + objects = {"cube": cube, "goal": goal} + env.sim.get_rigid_object_uid_list = lambda: tuple(objects) + env.sim.get_rigid_object = objects.get + arrived = { + "kind": "pose_near", + "entity": "cube", + "position": [0.5, 0.0, 0.025], + "tolerance_m": 0.01, + } + transport = { + "kind": "transport_guard", + "entity": "cube", + "other": "goal", + "lift_height_m": 0.05, + "minimum_transit_height_m": 0.025, + "goal_tolerance_m": 0.03, + } + monitor = MeasuredTaskMonitor( + (arrived,), + ({"event_id": "transported", "conditions": [arrived, transport]},), + ) + monitor.reset(env) + cube.position[0, 2] = 0.1 + monitor.observe(env, None) + assert not monitor.receipts + if drop: + cube.position[0, 2] = 0.025 + monitor.observe(env, None) + cube.position[:] = goal.position + monitor.observe(env, None) + assert monitor.validation(env).accepted is not drop + + def test_monitor_reads_physical_velocity_and_closed_schema(): env, cube = environment() monitor = MeasuredTaskMonitor( diff --git a/tests/lab/expansion/fixtures/task_program_job.yaml b/tests/lab/expansion/fixtures/task_program_job.yaml new file mode 100644 index 0000000000..bd11a2db29 --- /dev/null +++ b/tests/lab/expansion/fixtures/task_program_job.yaml @@ -0,0 +1,48 @@ +schema_version: 1 +engine: + workload_count: 2 + batch_size: 1 + episodes_per_operator: 1 + max_attempts: 4 + max_steps: 3000 + max_wall_time_s: 240 + settling_steps: 5 + capture_media: false +scene: + cube: + translation_m: [[0.0, 0.0], [0.0, 0.0], [0.0, 0.0]] + yaw_rad: [0.0, 0.0] +trajectory: + operators: [object_pose, joint_residual] + joint_offset_scale: 0.0001 + via_count: 2 + max_proposals: 8 + minimum_joint_delta: 0.0000001 + velocity_limit: 10.0 + acceleration_limit: 100.0 + nullspace_task_rows: [0, 1, 2] + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + pick_up: [approach] + place: [retract] +validation: + initial_checks: + - {kind: pose_near, entity: cube, position: [-0.42, -0.08, 0.025], tolerance_m: 0.08} + final_checks: &final + - {kind: pose_near, entity: cube, position: [-0.4, 0.48, 0.025], tolerance_m: 0.04} + - {kind: joint_range, joint: gripper_finger1_joint_1, lower: -0.001, upper: 0.005} + - {kind: speed_below, entity: cube, linear_m_s: 0.03, angular_rad_s: 0.2} + events: + - event_id: lift + conditions: + - {kind: lifted, entity: cube, height_m: 0.05} + - event_id: placed_and_released + conditions: *final + consecutive_steps: 3 + allowed_contacts: + - [cube, UR5/gripper_finger1_link_1] + - [cube, UR5/gripper_finger2_link_1] + - [cube, default_plane] + mounting_contacts: + - ['@root', default_plane] diff --git a/tests/lab/expansion/test_acceptance.py b/tests/lab/expansion/test_acceptance.py new file mode 100644 index 0000000000..cfd5507e56 --- /dev/null +++ b/tests/lab/expansion/test_acceptance.py @@ -0,0 +1,228 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Measured acceptance boundaries without native simulation resources.""" + +from __future__ import annotations + +from types import SimpleNamespace +import pytest +import torch +from embodichain.lab.expansion.acceptance import _ContactRow + +pytestmark = pytest.mark.no_sim + + +@pytest.fixture +def contact_row(tmp_path): + def build(duration=0.25, ground=False, frequency=10): + asset = tmp_path / "robot.urdf" + asset.write_text( + '' + ) + actors = {"table": 10, "book": 11} + if ground: + actors["default_plane"] = 12 + + def actor_ids(uid, links=None): + if uid == "robot": + ids = {"root": 1, "left": 2, "right": 3} + return torch.tensor([[ids[link] for link in links]]) + return torch.tensor([[actors[uid]]]) + + sensor = SimpleNamespace(get_actor_ids=actor_ids) + env = SimpleNamespace( + control_frequency=frequency, + robot=SimpleNamespace( + uid="robot", + link_names=["root", "left", "right"], + cfg=SimpleNamespace( + fpath=str(asset), + root_props=SimpleNamespace(self_collision_enabled=True), + ), + ), + sim=SimpleNamespace( + sim_config=SimpleNamespace(enable_ground_plane=ground), + get_rigid_object_uid_list=lambda: ["table", "book"], + get_rigid_object=lambda uid: SimpleNamespace( + body_type="dynamic" if uid == "book" else "static" + ), + ), + ) + cfg = { + "initial_checks": [], + "mounting_contacts": [], + "allowed_contacts": [], + "events": [], + "final_checks": [ + {"kind": "joint_range", "joint": "j", "lower": -1, "upper": 1} + ], + "contact_hold": { + "entity": "book", + "links": ["left", "right"], + "duration_s": duration, + "max_translation_drift_m": 0.001, + "max_rotation_drift_rad": 0.001, + }, + } + return _ContactRow(env, cfg, tmp_path, sensor=sensor) + + return build + + +@pytest.fixture +def sample_contacts(): + """Feed the production observer/sample boundary using explicit physics intervals.""" + from embodichain.lab.expansion.acceptance import MeasuredExpansionValidator + + def build(row): + row.env.num_envs = 1 + row.env.robot.joint_names = ["j"] + row.env.robot.get_qpos = lambda: torch.zeros((1, 1)) + row.active = True + state = SimpleNamespace( + pose=torch.tensor([[0.0, 0, 0, 0, 0, 0, 1]]), + contacts=torch.ones((1, 2), dtype=torch.bool), + ) + row.env.sim.get_rigid_object = lambda uid: SimpleNamespace( + get_local_pose=lambda: state.pose + ) + row.env.sim.simulation_time = 0.0 + row.env.sim.physics_step_index = 0 + row.sensor.get_data = lambda: { + "user_ids": torch.tensor([[[11, 2], [11, 3]]]), + "is_valid": state.contacts, + "impulse_valid": torch.ones((1, 2), dtype=torch.bool), + } + row.monitor.reset(row.env) + validator = object.__new__(MeasuredExpansionValidator) + validator.env = row.env + validator.rows = {0: row} + validator.sensor = row.sensor + validator.checker = SimpleNamespace( + check=lambda: torch.zeros((1, 3), dtype=torch.int64) + ) + validator.terminal, validator.results = {}, {} + + def sample(dts=None): + validator.begin_control_step() + for dt in ((1 / row.env.control_frequency,) if dts is None else dts): + row.env.sim.simulation_time += dt + row.env.sim.physics_step_index += 1 + validator.after_sample(dt, None) + validator.sample((0,), None) + return row.terminal[0] + + return sample, state + + return build + + +@pytest.mark.parametrize( + "duration,frequency,required", + [ + (0.25, 10, 4), + (0.025, 10, 2), + (0.3, 10, 4), + (0.14, 50, 8), + (0.07, 100, 8), + (0.1400000001, 50, 9), + (0.0700000001, 100, 9), + ], +) +def test_contact_hold_requires_full_duration_and_distinct_measured_times( + contact_row, sample_contacts, duration, frequency, required +): + row = contact_row(duration, ground=True, frequency=frequency) + sample, state = sample_contacts(row) + for _ in range(required - 1): + assert not sample().accepted + assert sample().accepted + report = row._hold_report() + assert report["elapsed_duration_s"] >= duration - 1e-15 + assert report["required_samples"] == required + # Repeated observations without physics cannot establish a hold. + row.reset() + row.active = True + for _ in range(required): + assert not sample(()).accepted + assert not row._hold_report()["passed"] + row.reset() + row.active = True + for _ in range(required - 1): + sample() + state.contacts[0, 0] = False + assert not sample().accepted + row.reset() + row.active = True + state.contacts.fill_(True) + for _ in range(required - 1): + sample() + state.pose[0, 0] = 0.1 + assert not sample().accepted + assert row._hold_report()["max_translation_drift_m"] == pytest.approx(0.1) + + +@pytest.mark.parametrize("ground", [False, True]) +def test_native_contacts_only_resolve_present_default_plane(contact_row, ground): + row = contact_row(ground=ground) + assert {"table", "book", "robot/root", "robot/left", "robot/right"} <= set( + row.frames.values() + ) + assert ("default_plane" in row.frames.values()) is ground + + +def test_contact_sampling_uses_measured_physics_intervals(contact_row, sample_contacts): + row = contact_row(ground=True) + sample, _ = sample_contacts(row) + for accepted in (False, False, False, True): + assert sample().accepted is accepted + assert row._hold_report()["elapsed_duration_s"] == pytest.approx(0.3) + + +@pytest.mark.parametrize("prior_steps", [500_000, 5_000_000]) +@pytest.mark.parametrize("physics_dt,passed", [(0.001, True), (0.0009999, False)]) +def test_hold_clock_is_independent_of_long_running_world_roundoff( + contact_row, sample_contacts, prior_steps, physics_dt, passed +): + row = contact_row(0.7, ground=True, frequency=100) + sample, _ = sample_contacts(row) + clock = 0.0 + for _ in range(prior_steps): + clock += 0.001 + row.env.sim.simulation_time = clock + row.env.sim.physics_step_index = prior_steps + # 71 samples span 700 steps; verify additional rolling windows as well. + for index in range(80): + result = sample((physics_dt,) * 10) + assert result.accepted is (passed and index >= 70) + report = row._hold_report() + assert report["elapsed_duration_s"] == pytest.approx(700 * physics_dt, abs=1e-15) + assert row.env.sim.simulation_time > clock + assert report["passed"] is passed + + +def test_hold_clock_follows_variable_physics_intervals(contact_row, sample_contacts): + row = contact_row(0.25, ground=True, frequency=10) + sample, _ = sample_contacts(row) + for intervals, passed in ( + ((0.03, 0.07), False), + ((0.02, 0.08), False), + ((0.01, 0.04, 0.05), False), + ((0.04, 0.06), True), + ): + assert sample(intervals).accepted is passed + assert row._hold_report()["elapsed_duration_s"] == pytest.approx(0.3) diff --git a/tests/lab/expansion/test_comparison.py b/tests/lab/expansion/test_comparison.py new file mode 100644 index 0000000000..016cd793c9 --- /dev/null +++ b/tests/lab/expansion/test_comparison.py @@ -0,0 +1,76 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import numpy as np +import pytest +from embodichain.lab.expansion import MeasuredTrajectory +from embodichain.lab.visualization.markers import ( + MarkerGroup, + MarkerGroupCfg, + MarkerPrototypeCfg, +) + + +def test_measured_comparison_owns_boundaries_and_preserves_marker_identity(): + times = np.array([0.0, 0.1, 0.3]) + poses = np.tile([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (3, 1)) + clip = MeasuredTrajectory( + "original", + "robot", + ("joint",), + times, + np.array([[1.0], [2.0], [3.0]]), + poses, + {"object": poses}, + ) + times[1] = 99 + poses[0, 0] = 99 + assert [clip.frame_at(t) for t in (-1.0, 0.0, 0.1, 0.2, 0.3, 5.0)] == [ + 0, + 0, + 1, + 1, + 2, + 2, + ] + assert clip.root_poses[0, 0] == clip.object_poses["object"][0, 0] == 0 + with pytest.raises(ValueError, match="ordered"): + MeasuredTrajectory( + "bad", + "robot", + ("joint",), + np.array([0.0, 0.0]), + np.zeros((2, 1)), + np.zeros((2, 7)), + {}, + ) + group = MarkerGroup( + MarkerGroupCfg( + name="ghost", prototypes={"box": MarkerPrototypeCfg(shape="box")} + ), + num_envs=1, + origins=np.zeros((1, 3)), + ) + group.update(translations=np.zeros((3, 3)), visible=np.array([False, True, False])) + complete = group.snapshot() + selected = group.snapshot(visible_only=True) + assert len(complete) == 3 and len(selected) == 1 + assert selected[0].overlay_id == complete[1].overlay_id + group.update(visible=np.array([False, False, True])) + assert group.snapshot(visible_only=True)[0].overlay_id == complete[2].overlay_id + group.remove() diff --git a/tests/lab/expansion/test_configured.py b/tests/lab/expansion/test_configured.py index fa77556963..1da22d2625 100644 --- a/tests/lab/expansion/test_configured.py +++ b/tests/lab/expansion/test_configured.py @@ -22,6 +22,76 @@ from embodichain.lab.expansion import ExpansionEngine, load_expansion_job +def test_task_program_declarations_are_pure_and_protect_contact_phases(tmp_path): + import json + import os + import subprocess + import sys + import yaml + from embodichain.lab.expansion import load_task_program_expansion + + path = Path(__file__).parent / "fixtures/task_program_job.yaml" + code = "from embodichain.lab.expansion import load_task_program_expansion; import json,sys; value=load_task_program_expansion(sys.argv[1]); assert not any(k=='dexsim' or k.startswith('dexsim.') for k in sys.modules); print(json.dumps(value))" + process = subprocess.run( + [sys.executable, "-c", code, str(path)], + capture_output=True, + text=True, + check=True, + env={**os.environ}, + ) + value = json.loads(process.stdout) + assert (value["engine"]["workload_count"], value["engine"]["batch_size"]) == (2, 1) + assert value["trajectory"]["operators"] == ["object_pose", "joint_residual"] + value["engine"]["nominal_seed"] = 7 + nominal = tmp_path / "nominal.yaml" + nominal.write_text(yaml.safe_dump(value)) + assert load_task_program_expansion(nominal)["engine"]["nominal_seed"] == 7 + value["engine"]["nominal_seed"] = True + nominal.write_text(yaml.safe_dump(value)) + with pytest.raises(ValueError, match="nominal_seed"): + load_task_program_expansion(nominal) + value["engine"]["nominal_seed"] = 7 + value["trajectory"]["free_phases"]["pick_up"] = ["approach", "lift"] + invalid = tmp_path / "protected.yaml" + invalid.write_text(yaml.safe_dump(value)) + with pytest.raises(ValueError, match="protected free phase"): + load_task_program_expansion(invalid) + + +def test_official_layout_jobs_load_without_simulation_imports(): + import subprocess + import sys + + task_root = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation" + ) + paths = sorted(task_root.glob("**/acceptance/scene_first/expansion.yaml")) + assert paths + code = """ +import importlib.abc +import sys + +class NoSimulator(importlib.abc.MetaPathFinder): + def find_spec(self, fullname, path=None, target=None): + if fullname.startswith(("dexsim", "embodichain.lab.sim", "embodichain.lab.gym")): + raise AssertionError(f"configuration loader imported {fullname}") + +sys.meta_path.insert(0, NoSimulator()) +from embodichain.lab.expansion import load_task_program_expansion +for path in sys.argv[1:]: + sampling = load_task_program_expansion(path)["scene_sampling"] + assert sampling["layout"]["policy"] and sampling["task_mode"]["phases"] + assert sampling["robot_anchors"] +""" + result = subprocess.run( + [sys.executable, "-c", code, *map(str, paths)], + capture_output=True, + text=True, + ) + assert result.returncode == 0, result.stderr + + @pytest.mark.parametrize("name", ["default", "newton", "rgbd", "motion"]) def test_example_preflight_resolves_owned_backend_and_budget(name, tmp_path): path = ( @@ -69,3 +139,43 @@ def test_resource_changes_preserve_semantics_but_record_new_execution(tmp_path): assert a["execution_id"] != b["execution_id"] cfg.mass_scale = {"cube": (2.0, 3.0)} assert preflight_expansion_job(cfg)["semantic_id"] != a["semantic_id"] + + +@pytest.mark.parametrize("owner", ["environment", "scene"]) +def test_deployment_snapshot_preserves_component_relative_assets(tmp_path, owner): + import json + import yaml + from embodichain.lab.expansion.task_program_jobs import _prepare_deployment + from embodichain.lab.gym.utils.composition import resolve_gym_components + + source = tmp_path / "source" + source.mkdir() + mesh = source / "mesh.obj" + mesh.write_text("v 0 0 0\nv 1 0 0\nv 0 1 0\nf 1 2 3\n") + simulation = { + "rigid_object": [ + {"uid": "mesh", "shape": {"shape_type": "Mesh", "fpath": "./mesh.obj"}} + ] + } + component = ( + { + "environment_id": "mesh-test", + "physics": "default", + "simulation": simulation, + "env": {}, + } + if owner == "environment" + else {"scene_id": "mesh-test", "simulation": simulation} + ) + (source / "component.yaml").write_text(yaml.safe_dump(component)) + declaration = {"id": "SnapshotTest-v1", owner: {"component": "component.yaml"}} + deployment = source / "deployment.yaml" + deployment.write_text(yaml.safe_dump(declaration)) + before = {p.name: p.read_bytes() for p in source.iterdir()} + output = tmp_path / "output" + output.mkdir() + derived = _prepare_deployment(deployment, output) + snapshot = json.loads(derived.read_text()) + resolved = resolve_gym_components(snapshot, base_dir=derived.parent).config + assert resolved["rigid_object"][0]["shape"]["fpath"] == str(mesh) + assert {p.name: p.read_bytes() for p in source.iterdir()} == before diff --git a/tests/lab/expansion/test_distribution.py b/tests/lab/expansion/test_distribution.py new file mode 100644 index 0000000000..0675800ce9 --- /dev/null +++ b/tests/lab/expansion/test_distribution.py @@ -0,0 +1,229 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations +import numpy as np +import pytest +from embodichain.lab.expansion._distribution import _motion, _summarize + + +def test_installation_noise_does_not_split_domain_or_modify_raw_poses(): + from embodichain.lab.expansion._distribution import _installation_pose + + measured = [2.3283064365386963e-10, -3.725290298461914e-9, 0, 0, 0, 0, -1] + raw = list(measured) + assert _installation_pose(measured) == _installation_pose([0, 0, 0, 0, 0, 0, 1]) + assert measured == raw + assert _installation_pose([0.01, 0, 0, 0, 0, 0, 1]) != _installation_pose(measured) + assert _installation_pose([0, 0, 0, 0, 0, -1, -1e-10]) == _installation_pose( + [0, 0, 0, 0, 0, 1, 1e-10] + ) + with pytest.raises(ValueError, match="nonzero"): + _installation_pose([0] * 7) + + +def test_motion_density_and_time_are_separate(): + a = _motion(np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]), np.array([0.0, 2.0])) + b = _motion( + np.array([[0.0, 0.0, 0.0], [0.5, 0.0, 0.0], [1.0, 0.0, 0.0]]), + np.array([0.0, 2.0, 4.0]), + ) + assert a["path_length"] == b["path_length"] == 1 + assert a["duration_s"] == 2 and b["duration_s"] == 4 + np.testing.assert_allclose(a["path"], b["path"]) + with pytest.raises(ValueError): + _motion(np.zeros((2, 3)), np.array([1.0, 1.0])) + + +def test_joint_distribution_and_idempotency(): + rows = [ + dict( + commit_id=str(i), + initial_pose=[x, 0, 0], + final_pose=[x, 0, 0], + object_motion={}, + geometry_bucket=None, + ) + for i, x in enumerate([-0.15, 0.15]) + ] + r = _summarize(rows + rows, 0.1, 1) + assert r["count"] == 2 + assert r["joint"] == {"-2,0|-2,0": 1, "1,0|1,0": 1} + assert r["joint_at_minimum"] == 2 + with pytest.raises(ValueError): + _summarize([rows[0], dict(rows[0], final_pose=[9, 0, 0])], 0.1, 1) + + +def test_missing_layout_is_not_zero(): + r = _summarize( + [dict(commit_id="a", object_motion=None, geometry_bucket=None)], 0.1, 5 + ) + assert r["layout_missing"] == 1 and r["layout_valid"] == 0 + assert r["joint"] == {} + + +@pytest.mark.parametrize( + ("tasks", "source_uuids", "matches"), + [ + (("program-a", "program-b"), ("source-a", "source-b"), False), + (("program-a", "program-a"), ("source-a", "source-b"), True), + ((None, None), ("source-a", "source-b"), False), + ((None, None), ("source-a", "source-a"), True), + ], +) +def test_report_requires_matching_recorded_task_identity( + tmp_path, monkeypatch, tasks, source_uuids, matches +): + from types import SimpleNamespace + import torch + from embodichain.lab.expansion import report_expansion_distribution + from embodichain.lab.expansion import persistence + + jobs = [tmp_path / name for name in ("before", "after")] + episodes = {} + for job, task_id, source_uuid in zip(jobs, tasks, source_uuids, strict=True): + job.mkdir() + (job / "job.json").write_text("{}") + metadata = {"instruction": "Pick and place", "source_episode_uuid": source_uuid} + if task_id is not None: + metadata["scene_case"] = {"task_id": task_id} + episodes[job.name] = SimpleNamespace( + episode_id=f"episode-{job.name}", + commit_id=f"commit-{job.name}", + metadata=metadata, + timestamps=torch.tensor([0.0, 1.0]), + observations={}, + validation=SimpleNamespace(accepted=True), + ) + monkeypatch.setattr( + persistence.ArtifactEpisodeSink, + "inspect", + lambda self: (SimpleNamespace(confirmed=True, storage_id=self.directory.name),), + ) + monkeypatch.setattr(persistence, "load_expansion_episode", episodes.__getitem__) + output = tmp_path / "report" + if matches: + report = report_expansion_distribution(jobs[:1], jobs[1:], output, entity="mug") + assert (report["before"], report["after"]) == (1, 1) + else: + with pytest.raises(ValueError, match="matching domain"): + report_expansion_distribution(jobs[:1], jobs[1:], output, entity="mug") + assert not output.exists() + + +def test_orientation_sign_and_out_of_plane_rotation(): + from embodichain.lab.expansion._distribution_pose import orientation + + poses = np.array([[0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0]], dtype=float) + a = orientation(poses, np.array([0.0, 2.0])) + poses[:, 3:] *= -1 + b = orientation(poses, np.array([0.0, 2.0])) + assert a == b + assert a["angular_path_rad"] == pytest.approx(np.pi) + assert a["angular_speed_p50_p95"] == pytest.approx([np.pi / 2] * 2) + assert a["initial_rotvec_rad"] == [0.0, 0.0, 0.0] + assert abs(a["final_rotvec_rad"][0]) == pytest.approx(np.pi) + poses[:, 3:] = 0 + with pytest.raises(ValueError): + orientation(poses, np.array([0.0, 2.0])) + + +def test_tcp_root_transform_and_calibration(tmp_path): + import torch + from types import SimpleNamespace + from embodichain.lab.expansion._distribution_pose import RecordedTCP + + asset = tmp_path / "test.urdf" + asset.write_text( + """""" + ) + offset = np.eye(4) + offset[2, 3] = 0.15 + tcp = RecordedTCP( + { + "robot": { + "fpath": str(asset), + "gripper_bindings": { + "eef": { + "root_link": "tool", + "defaults": {"default_root_from_tcp": offset.tolist()}, + } + }, + } + } + ) + e = SimpleNamespace( + metadata={ + "initial_state": [{"kind": "robot", "uid": "r", "joint_names": ["slide"]}], + "scene_state_fields": [ + {"entity_uid": "r", "field": "pose", "key": "root"}, + {"entity_uid": "r", "field": "qpos", "key": "q"}, + ], + }, + observations={ + "root": torch.tensor([[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]] * 2), + "q": torch.tensor([[0.0], [1.0]]), + }, + ) + p = tcp.poses(e) + np.testing.assert_allclose(p[:, :3], [[1, 2, 3.15], [2, 2, 3.15]]) + np.testing.assert_allclose(p[:, 3:], [[0, 0, 0, 1]] * 2) + + +def test_detail_pages_match_agent_records_and_summary(tmp_path, monkeypatch): + import json + from embodichain.lab.expansion import _distribution as module + + rows = [] + for i in range(101): + rows.append( + dict( + episode_id=f"e{i}", + commit_id=f"c{i}", + initial_pose=[0, 0, 0, 0, 0, 0, 1], + final_pose=[1, 0, 0, 0, 0, 0, 1], + object_motion={ + "path_length": 1.0, + "duration_s": 2.0, + "path": [[0, 0, 0], [1, 0, 0]], + }, + joint_motion=None, + tcp_motion=None, + orientation={ + "rotation_bin_initial": "6,6,6", + "rotation_bin_final": "6,6,6", + }, + geometry_bucket=None, + ) + ) + monkeypatch.setattr(module, "_read", lambda paths, entity: (rows, ["domain"])) + output = tmp_path / "result" + module._run([], [], output, "cube", 0.1, 5) + summary = json.loads((output / "summary.json").read_text()) + detail = [ + json.loads(line) + for line in (output / "after-episodes.jsonl").read_text().splitlines() + ] + pages = [ + json.loads((output / "pages" / f"after-{n}.json").read_text()) for n in range(2) + ] + assert [len(p) for p in pages] == [100, 1] + assert pages[0] + pages[1] == detail + assert summary["after"]["orientation"]["initial"] == {"6,6,6": 101} + assert summary["after"]["joint"] == {"0,0|10,0": 101} + assert summary["after"]["episodes"][100]["page"] == "after-1" + assert summary["after"]["episodes"][0]["object_motion"] is True + assert rows[0].get("pick_cell") is None diff --git a/tests/lab/expansion/test_episode_assembly.py b/tests/lab/expansion/test_episode_assembly.py index d2ff7d3f23..15e65eaf8a 100644 --- a/tests/lab/expansion/test_episode_assembly.py +++ b/tests/lab/expansion/test_episode_assembly.py @@ -44,6 +44,7 @@ def test_source_rejection_preserves_successful_peer(tmp_path, failed_row): assert episodes[failed_row] is None peer = episodes[1 - failed_row] assert peer is not None and peer.validation.accepted + assert peer.metadata["source_env_id"] == 1 - failed_row assert len(peer.actions) == 1 and len(peer.timestamps) == 2 record = json.loads((tmp_path / str(failed_row) / "attempt.json").read_text()) assert ( diff --git a/tests/lab/expansion/test_export.py b/tests/lab/expansion/test_export.py index ce9b2e5046..eddf3b32e1 100644 --- a/tests/lab/expansion/test_export.py +++ b/tests/lab/expansion/test_export.py @@ -29,6 +29,61 @@ from embodichain.lab.expansion import export as module +def test_per_episode_export_uses_real_sdk_and_preserves_portable_boundaries(tmp_path): + from embodichain.lab.expansion import ( + export_expansion_episodes, + load_expansion_episode, + ) + from lerobot.datasets.lerobot_dataset import LeRobotDataset + + sink = ArtifactEpisodeSink(tmp_path) + for ordinal in range(2): + parent = episode() + parent = replace( + parent, + identity=replace( + parent.identity, + candidate_id=f"candidate-{ordinal}", + geometry_family_id=f"family-{ordinal}", + ), + episode_id=f"episode-{ordinal}", + commit_id=f"commit-{ordinal}", + observations={ + "joint_positions": torch.cat( + ( + parent.observations["joint_positions"], + parent.observations["joint_positions"] + 0.03, + ), + dim=1, + ), + "joint_velocities": torch.zeros(3, 2), + "joint_efforts": torch.zeros(3, 2), + }, + actions=torch.cat((parent.actions, parent.actions + 0.03), dim=1), + metadata={ + "expert_action_spec": { + "joint_names": ["joint_0", "joint_1"], + "joint_command_mode": "position", + "step_dt": 0.1, + }, + "source_episode_uuids": ["source"], + }, + ) + sink.submit(parent, submission_id=0) + result = export_expansion_episodes(tmp_path) + assert result["episodes"] == 2 + assert export_expansion_episodes(tmp_path) == result + for row in result["datasets"]: + path = Path(result["directory"]) / row["dataset_path"] + data = LeRobotDataset(path.name, root=path, download_videos=False) + assert data.num_episodes == 1 and len(data) == 2 + receipts = ArtifactEpisodeSink(path / "embodichain_artifacts").inspect() + assert len(receipts) == 1 and receipts[0].episode_id == row["episode_id"] + saved = load_expansion_episode(receipts[0].storage_id) + assert len(saved.timestamps) == len(saved.actions) + 1 == 3 + torch.testing.assert_close(data[0]["action"], saved.actions[0], rtol=0, atol=0) + + def test_export_includes_visual_children_without_new_physical_receipts( tmp_path, monkeypatch ): diff --git a/tests/lab/expansion/test_extension_ports_sim.py b/tests/lab/expansion/test_extension_ports_sim.py new file mode 100644 index 0000000000..0b4304d017 --- /dev/null +++ b/tests/lab/expansion/test_extension_ports_sim.py @@ -0,0 +1,627 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import os +from pathlib import Path +import subprocess +import sys +import pytest + + +def _check_place_keyframes(env, generator): + from dataclasses import replace + from embodichain.lab.sim.atomic_skills import ( + AtomicSkillEngine, + ControlPartCommandProfile, + MotionPolicy, + SkillInvocation, + ) + from embodichain.lab.sim.atomic_skills.primitives.place import ( + PlaceGoal, + PlaceOptions, + ) + from embodichain.lab.sim.atomic_skills.primitives._helpers import ( + split_joint_trajectory_at_pose, + ) + from embodichain.lab.sim.atomic_skills.trajectory_ops import split_three_segments + from embodichain.lab.sim.motion.planners.utils import PlanState + import torch + + hand_limits = env.robot.get_qpos_limits(name="eef")[0] + engine = AtomicSkillEngine( + generator, + env_ids=(0,), + control_profiles={ + "eef": ControlPartCommandProfile.joint_positions( + open=hand_limits[:, 0], grasp=hand_limits[:, 1] + ) + }, + ) + binding = engine.bind_control_parts( + "place", {"primary": {"motion": "arm", "grasp": "eef"}} + ) + context = engine.initial_context(control_dt=0.04) + arm_ids = env.robot.get_joint_ids("arm") + start = context.robot.qpos[:, arm_ids] + start_pose = env.robot.compute_fk( + qpos=start, name="arm", to_matrix=True, env_ids=(0,) + ) + release = start_pose.clone() + release[:, 0, 3] += 0.025 + approach = release.clone() + approach[:, 2, 3] += 0.03 + # Four explicit quarter-points belong to each original translation segment. + points = [] + for first, last in ( + (start_pose, approach), + (approach, release), + (release, approach), + ): + for alpha in (0.25, 0.5, 0.75, 1.0): + pose = last.clone() + pose[:, :3, 3] = first[:, :3, 3] + alpha * ( + last[:, :3, 3] - first[:, :3, 3] + ) + points.append(PlanState.from_xpos(pose)) + options = PlaceOptions(cartesian_waypoint_count=4, lift_height=0.03) + for before, after in (("joint", "cartesian"), ("cartesian", "joint")): + policy = MotionPolicy( + strategy="ik_interp", + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + sample_count=125, + velocity_limit=None, + acceleration_limit=None, + phase_paths={"pre_place": before, "place": after, "retreat": after}, + ) + invocation = SkillInvocation( + skill_id="place", + goal=PlaceGoal(release), + binding=binding, + motion_policy=policy, + skill_options=options, + ) + plan = engine.plan(invocation, context) + down_count, _, back_count = split_three_segments( + policy.sample_count, + options.hand_interp_steps, + first_segment_name="approach", + third_segment_name="back", + ) + reference_options = policy.to_motion_gen_options( + start_qpos=start, + control_part="arm", + sample_count=down_count + back_count, + interpolation_dt=context.control_dt, + ) + reference_options.waypoint_modes = (before,) * 4 + (after,) * 8 + reference = generator.generate(points, options=reference_options) + assert plan.success_all and reference.success.all() + down, back = split_joint_trajectory_at_pose( + reference.positions, + release, + robot=env.robot, + control_part="arm", + first_sample_count=down_count, + second_sample_count=back_count, + dt=reference.dt, + control_dt=context.control_dt, + arrival_indices=reference.waypoint_indices, + success=reference.success, + split_target_index=7, + env_ids=(0,), + ) + trajectory = plan.joint_trajectory + for segment, expected in zip( + (plan.segments[0], plan.segments[-1]), (down, back) + ): + torch.testing.assert_close( + trajectory.positions[:, segment.start : segment.stop, arm_ids], + expected, + rtol=0, + atol=1e-6, + ) + with pytest.raises(ValueError, match="cartesian_approach and phase_paths"): + engine.plan( + replace( + invocation, skill_options=replace(options, cartesian_approach=True) + ), + context, + ) + for period, steps, expected in ((0.04, 0, 3), (0.02, 0, 6), (0.04, 5, 5)): + timed = engine.plan( + replace( + invocation, + skill_options=replace( + options, release_settle_duration=0.11, release_settle_steps=steps + ), + ), + engine.initial_context(control_dt=period), + ) + release = timed.segment("release") + assert release.stop - release.start == options.hand_interp_steps + expected + hand_ids = env.robot.get_joint_ids("eef") + torch.testing.assert_close( + timed.joint_trajectory.positions[ + :, release.stop - expected : release.stop, hand_ids + ], + hand_limits[None, :, 0].expand(1, expected, -1), + rtol=0, + atol=0, + ) + + +def _probe(output): + import torch + from embodichain.lab.expansion import MeasuredExpansionValidator + from embodichain.lab.expansion.program import TaskProgramBatchExecutor + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenCfg, + MotionGenOptions, + ) + from embodichain.lab.sim.motion.planners.trapezoidal_planner import ( + TrapezoidalPlannerCfg, + ) + from embodichain.lab.sim.motion.planners.utils import PlanState + from embodichain.lab.task_program.integrations.simulation.feasibility_geometry import ( + collision_bodies_from_simulation, + ) + from embodichain.utils.utility import load_config + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + discover_task_packages() + execute_init_hooks() + cfg = config_to_cfg(load_config(path), source_path=path) + cfg.num_envs = cfg.sim_cfg.num_envs = 2 + cfg.sim_cfg.device, cfg.sim_cfg.headless = "cpu", True + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sensor, cfg.enable_sensor, cfg.dataset = [], False, None + cfg.init_rollout_buffer = cfg.record_trajectory = False + cfg.task_program.program.count = 1 + env = REGISTERED_ENVS[load_config(path)["id"]].make(cfg=cfg) + try: + env.reset(seed=0) + generator = MotionGenerator( + MotionGenCfg( + planner_cfg=TrapezoidalPlannerCfg( + robot_uid=env.robot.uid, sim_instance_id=env.sim.instance_id + ) + ), + env_ids=(0,), + ) + start = env.robot.get_qpos(name="arm")[0:1].clone() + states = [] + for index in range(3): + goal = start.clone() + goal[:, 0] += 0.001 * (index + 1) + pose = env.robot.compute_fk( + qpos=goal, name="arm", to_matrix=True, env_ids=(0,) + ) + states.append(PlanState.from_xpos(pose)) + base = dict( + strategy="ik_interp", + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + interpolation_dt=0.04, + start_qpos=start, + control_part="arm", + ) + ordinary = generator.generate(states, MotionGenOptions(sample_count=25, **base)) + first = generator.generate(states[:1], MotionGenOptions(sample_count=9, **base)) + tail = generator.generate( + states[1:], + MotionGenOptions( + sample_count=17, + **{**base, "start_qpos": first.positions[:, -1], "is_linear": True}, + ), + ) + before = env.sim.physics_step_index + mixed = generator.generate( + states, + MotionGenOptions( + sample_count=25, + waypoint_modes=("joint", "cartesian", "cartesian"), + **base, + ), + ) + assert mixed.success.all() and first.success.all() and tail.success.all() + torch.testing.assert_close( + mixed.positions, + torch.cat((first.positions, tail.positions[:, 1:]), dim=1), + rtol=0, + atol=1e-6, + ) + torch.testing.assert_close( + mixed.dt, torch.cat((first.dt, tail.dt[:, 1:]), dim=1), rtol=0, atol=0 + ) + after = generator.generate(states, MotionGenOptions(sample_count=25, **base)) + torch.testing.assert_close( + ordinary.positions, after.positions, rtol=0, atol=1e-6 + ) + assert env.sim.physics_step_index == before + _check_place_keyframes(env, generator) + assert env.sim.physics_step_index == before + program = env.compile_task_program(cfg.task_program) + frames = sorted({b.frame_uid for b in collision_bodies_from_simulation(env)}) + value = float(env.robot.get_qpos()[0, 0]) + name = env.robot.joint_names[0] + configuration = { + "initial_checks": [ + { + "kind": "joint_range", + "joint": name, + "lower": value - 0.1, + "upper": value + 0.1, + } + ], + "final_checks": [ + { + "kind": "joint_range", + "joint": name, + "lower": value + 1, + "upper": value + 2, + } + ], + "events": [], + "mounting_contacts": [], + "allowed_contacts": [ + (a, b) for i, a in enumerate(frames) for b in frames[i + 1 :] + ], + } + for backend in ("geometry", "native_contacts"): + selected = dict(configuration) + if backend == "native_contacts": + selected["contact_hold"] = { + "entity": "cube", + "links": ["gripper_finger1_link_1", "gripper_finger2_link_1"], + "duration_s": env.step_dt * 1.4, + "max_translation_drift_m": 0.001, + "max_rotation_drift_rad": 0.001, + } + check = MeasuredExpansionValidator( + env, selected, Path(output) / backend, backend=backend + ) + try: + check.prepare() + TaskProgramBatchExecutor(env, program).run( + (0, 1), max_steps=3, observe_step=check.sample + ) + assert set(check.results) == set(check.terminal) == {0, 1} + assert all( + row.status == "passed" for row in check.results.values() + ), check.results + peer = check.terminal[1] + check.prepare(env_ids=(0,)) + assert check.terminal[1] is peer and 0 not in check.terminal + if backend == "geometry": + from embodichain.lab.gym.envs.types import ControllerAction + + # A real collider overlap in only one selected arena must + # not reject the safe peer, including reordered selection. + forbidden = { + **configuration, + "allowed_contacts": [ + pair + for pair in configuration["allowed_contacts"] + if not ( + "cube" in pair + and any( + name.startswith(env.robot.uid + "/") + for name in pair + ) + ) + ], + } + measured = MeasuredExpansionValidator( + env, + forbidden, + Path(output) / "geometry-selected", + backend="geometry", + ) + cube = env.sim.get_rigid_object("cube") + original = cube.get_local_pose(to_matrix=True)[1:2].clone() + try: + measured.prepare() + body = next( + b + for b in collision_bodies_from_simulation(env, env_id=1) + if b.robot and b.kind != "halfspace" + ) + link = body.frame_uid.split("/", 1)[1] + pose = env.robot.get_link_pose(link, to_matrix=True)[1:2] + pose = pose @ torch.tensor(body.local_pose, dtype=pose.dtype) + before_query = env.sim.physics_step_index + cube.set_local_pose(pose, env_ids=[1]) + measured.sample((1, 0), ControllerAction(env.robot.get_qpos())) + assert measured.results[0].status == "passed" + assert measured.results[1].status == "failed" + assert env.sim.physics_step_index == before_query + finally: + cube.set_local_pose(original, env_ids=[1]) + measured.close() + if backend == "native_contacts": + assert check.rows[1].substeps > 0 and check.rows[0].substeps == 0 + # The clock uses the same native physics intervals that + # supplied contact evidence, independently of World time. + report = check.rows[1]._hold_report() + assert report["samples"] == report["required_samples"] == 3 + assert report["elapsed_duration_s"] == pytest.approx( + 2 * env.step_dt + ) + finally: + check.close() + finally: + env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +@pytest.mark.subprocess_sim +def test_explicit_phase_paths_and_selected_row_acceptance(tmp_path): + result = subprocess.run( + [sys.executable, __file__, str(tmp_path)], + cwd=Path(__file__).resolve().parents[3], + env={**os.environ, "SDL_VIDEODRIVER": "offscreen", "OMP_NUM_THREADS": "1"}, + capture_output=True, + text=True, + timeout=300, + ) + assert result.returncode == 0, result.stdout[-3000:] + result.stderr[-5000:] + + +def _check_coordinated_grasp_targets(env): + """Real selected-row IK keeps commanded preload on contact-blocked hands.""" + import torch + from embodichain.lab.sim.atomic_skills import ( + AntipodalAffordance, + AtomicSkillEngine, + ControlPartCommandProfile, + CoordinatedPlacementGoal, + CoordinatedPlacementOptions, + HeldObjectState, + MotionPolicy, + ObjectSemantics, + SkillInvocation, + TaskState, + ) + from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenCfg, + ) + from embodichain.lab.sim.motion.planners.trapezoidal_planner import ( + TrapezoidalPlannerCfg, + ) + + rows = (3, 1) + generator_cfg = MotionGenCfg( + planner_cfg=TrapezoidalPlannerCfg( + robot_uid=env.robot.uid, sim_instance_id=env.sim.instance_id + ) + ) + generator = MotionGenerator(generator_cfg, env_ids=rows) + profiles = {} + for part in ("left_eef", "right_eef"): + limits = env.robot.get_qpos_limits(name=part)[0] + profiles[part] = ControlPartCommandProfile.joint_positions( + open=limits[:, 0], grasp=limits[:, 1] + ) + engine = AtomicSkillEngine(generator, env_ids=rows, control_profiles=profiles) + binding = engine.bind_control_parts( + "coordinated_placement", + { + "placing": {"motion": "left_arm", "grasp": "left_eef"}, + "support": {"motion": "right_arm", "grasp": "right_eef"}, + }, + ) + poses, held = {}, {} + for slot, arm in (("placing", "left_arm"), ("support", "right_arm")): + poses[slot] = env.robot.compute_fk( + qpos=env.robot.get_qpos(name=arm)[list(rows)], + name=arm, + to_matrix=True, + env_ids=rows, + ) + held[binding.endpoint(slot, "motion").task_state_key] = HeldObjectState( + semantics=ObjectSemantics( + entity_id=slot, + label=slot, + geometry={}, + affordance=AntipodalAffordance(), + ), + object_to_eef=torch.eye(4).repeat(2, 1, 1), + grasp_xpos=poses[slot], + ) + context = engine.initial_context( + task=TaskState(batch_size=2, device="cpu", held_objects=held), control_dt=0.04 + ) + before = env.sim.physics_step_index + # The stronger solver capability must preserve actual preceding-seed IK, + # including a failed peer, without losing non-contiguous physical rows. + start = context.robot.qpos[:, env.robot.get_joint_ids("right_arm")] + nodes = start[:, None].repeat(1, 4, 1) + nodes[:, :, 0] += torch.tensor([0.0, 0.01, 0.02, 0.03]) + targets = torch.stack( + [ + env.robot.compute_fk( + qpos=node, name="right_arm", to_matrix=True, env_ids=rows + ) + for node in nodes.unbind(1) + ], + dim=1, + ) + for failed_peer in (False, True): + requested = targets.clone() + if failed_peer: + requested[1, 2, 0, 3] += 100 + seed, points = start, [] + accepted = torch.ones(2, dtype=torch.bool) + for pose in requested.unbind(1): + valid, point = env.robot.compute_ik( + pose=pose, name="right_arm", joint_seed=seed, env_ids=rows + ) + seed = torch.where(valid[:, None], point, seed) + points.append(seed) + accepted &= valid + expected = torch.where( + accepted[:, None, None], torch.stack(points, 1), start[:, None] + ) + valid, actual = generator._solve_seeded_cartesian_waypoints( + requested, start, "right_arm" + ) + assert torch.equal(valid, accepted) + torch.testing.assert_close(actual, expected, rtol=0, atol=1e-6) + from embodichain.lab.expansion._planning_batches import batched_planning + + class PathQuery: + # A numerical request producer, with the real Robot/SSIK implementation. + def __init__(self, index): + self.index = index + + def next(self): + index = self.index + single = MotionGenerator(generator_cfg, env_ids=(rows[index],)) + return single._solve_seeded_cartesian_waypoints( + requested[index : index + 1], start[index : index + 1], "right_arm" + ) + + with batched_planning(env.robot, seed=2) as broker: + answers = broker.gather({row: PathQuery(i) for i, row in enumerate(rows)}) + assert broker.stats["max_group"] == 2 + for index, row in enumerate(rows): + assert torch.equal(answers[row][0], accepted[index : index + 1]) + torch.testing.assert_close( + answers[row][1], expected[index : index + 1], rtol=0, atol=1e-6 + ) + for release in (False, True): + plan = engine.plan( + SkillInvocation( + skill_id="coordinated_placement", + binding=binding, + goal=CoordinatedPlacementGoal( + placing_object_target_pose=poses["placing"], + support_object_target_pose=poses["support"], + ), + motion_policy=MotionPolicy(sample_count=30, strategy="ik_interp"), + skill_options=CoordinatedPlacementOptions( + release=release, lift_height=0, hand_interp_steps=4, retreat_steps=4 + ), + ), + context, + ) + assert plan.success_all, plan.diagnostics + for part in ("left_eef", "right_eef"): + ids = env.robot.get_joint_ids(part) + expected = env.robot.get_qpos_limits(name=part)[0, :, 1] + actual = plan.joint_trajectory.positions[:, :, ids] + if part == "left_eef" and release: + actual = actual[:, : plan.segments[0].stop] + assert not torch.equal(context.robot.qpos[:, ids], expected.expand(2, -1)) + torch.testing.assert_close( + actual, expected.expand_as(actual), rtol=0, atol=0 + ) + assert env.sim.physics_step_index == before + + +def _probe_handover_rows(): + """Plan and consume selected rows in a larger real handover World.""" + import torch + from embodichain.lab.expansion.program import collect_program_batch + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.utils.utility import load_config + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/hand_over/task.tianji_marvin.yaml" + ) + discover_task_packages() + execute_init_hooks() + cfg = config_to_cfg(load_config(path), source_path=path) + cfg.num_envs = cfg.sim_cfg.num_envs = 4 + cfg.sim_cfg.device, cfg.sim_cfg.headless = "cpu", True + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sensor, cfg.enable_sensor, cfg.dataset = [], False, None + cfg.init_rollout_buffer = cfg.record_trajectory = False + env = REGISTERED_ENVS[load_config(path)["id"]].make(cfg=cfg) + adapters = [] + try: + env.reset(seed=2) + _check_coordinated_grasp_targets(env) + program = env.task_program_adapter.compile(cfg.task_program) + bridges = {} + for row in (3, 1): + adapter = env.task_program_adapter.for_env_ids((row,)) + adapters.append(adapter) + bridges[row] = adapter.create_bridge(program, isolate_policy_sessions=True) + observed = [] + result = collect_program_batch( + env, + bridges, + max_steps=2, + observe_step=lambda ids, action, observation: observed.append(ids), + ) + # The original FK omitted selected IDs and rejected both plans at zero + # commands. Budget cancellation must instead follow real commands. + assert result.env_ids == (3, 1) + assert all(length >= 1 for length in result.lengths), result.lengths + assert observed and set(observed[0]) == {3, 1} + assert not any(result.accepted) + finally: + for adapter in adapters: + adapter.close() + env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +@pytest.mark.subprocess_sim +def test_selected_handover_rows_preserve_world_batch(tmp_path): + result = subprocess.run( + [sys.executable, __file__, "handover"], + cwd=Path(__file__).resolve().parents[3], + env={**os.environ, "SDL_VIDEODRIVER": "offscreen", "OMP_NUM_THREADS": "1"}, + capture_output=True, + text=True, + timeout=300, + ) + assert result.returncode == 0, result.stdout[-3000:] + result.stderr[-5000:] + + +if __name__ == "__main__": + if sys.argv[1] == "handover": + _probe_handover_rows() + else: + _probe(sys.argv[1]) diff --git a/tests/lab/expansion/test_gym_workloads.py b/tests/lab/expansion/test_gym_workloads.py new file mode 100644 index 0000000000..820bdd5ee2 --- /dev/null +++ b/tests/lab/expansion/test_gym_workloads.py @@ -0,0 +1,467 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Workload admission and action-schema compatibility boundaries.""" + +from __future__ import annotations + +from types import SimpleNamespace +from unittest.mock import Mock +import hashlib +import json +import pytest +import torch +from embodichain.lab.expansion.gym_workloads import ( + _GymWorkloadSource, + GymExpansionWorkload, + run_gym_expansion_workloads, +) +from embodichain.lab.expansion.workloads import ( + ExpansionWorkloadCfg, + WorkloadExecutionCfg, + WorkloadAssignment, + WorkloadProgramExecutor, +) + +pytestmark = pytest.mark.no_sim + + +def _prepare_cube_scene(cfg, *, x): + """A valid pre-World hook whose parameters alter the prepared scene.""" + from copy import deepcopy + + prepared = deepcopy(cfg) + cube = next(obj for obj in prepared.rigid_object if obj.uid == "cube") + cube.init_pos = (x, *cube.init_pos[1:]) + return prepared + + +def _prepare_cube_scene_offset(cfg, *, x): + """A distinct callable declaration with different scene preparation.""" + return _prepare_cube_scene(cfg, x=x + 0.1) + + +@pytest.mark.parametrize("settled", [0, 5]) +def test_expired_warming_row_finishes_group_without_disturbing_live_peer( + monkeypatch, settled +): + monkeypatch.setattr( + "embodichain.lab.expansion.workloads.time.monotonic", lambda: 1.0 + ) + executor = object.__new__(WorkloadProgramExecutor) + executor.cfg = WorkloadExecutionCfg( + workload_count=2, batch_size=1, settling_steps=5 + ) + executor.workloads = { + name: ExpansionWorkloadCfg(name, 1, 1, budget) + for name, budget in (("expired", 0.01), ("peer", 10)) + } + executor.started_at = {"expired": 0.0, "peer": 0.0} + executor.counts = { + name: {"attempts": 1, "rollouts": 0, "committed": 0} + for name in executor.workloads + } + executor.warming = { + 0: {"assignment": WorkloadAssignment("expired", 0, 0, 0), "settled": settled} + } + peer = {"assignment": WorkloadAssignment("peer", 0, 1, 0)} + executor.live = {1: peer} + executor.lanes, executor.completed = ["expired", "peer"], set() + executor.on_workload_done = Mock() + executor.source = SimpleNamespace(activate=Mock(), complete=Mock(return_value=None)) + executor._activate() + assert executor.warming == {} + assert executor.live[1] is peer + executor.source.activate.assert_not_called() + result = executor.source.complete.call_args.args[0] + assert result.program.reasons == ("wall_time_exhausted",) + assert result.world_control_start is result.world_control_end is None + executor._fill() + executor.on_workload_done.assert_called_once_with( + "expired", executor.counts["expired"] + ) + assert executor.live[1] is peer + + +@pytest.mark.parametrize("stop", ["wall_time_exhausted", "rollout_budget_exhausted"]) +def test_native_source_retires_exhausted_attempt_before_reading_world(stop): + from embodichain.lab.expansion.contracts import ( + EpisodeCandidate, + SceneCase, + ValidationCheck, + ValidationResult, + ) + from embodichain.lab.expansion.session import ExpansionSession + from embodichain.lab.expansion.session_cfg import ExpansionSessionCfg + + clock = [0.0] + session = ExpansionSession( + ExpansionSessionCfg.from_mapping( + { + "collection": { + "max_wall_time_s": 0.01, + "target_committed_episodes": 2, + "max_proposals": 3, + "max_rollout_attempts": 2, + }, + "execution": {"ready_high_watermark": 3, "ready_low_watermark": 0}, + "persistence": {"pending_episode_limit": 2, "pending_max_bytes": 4096}, + } + ), + clock=lambda: clock[0], + ) + if stop == "wall_time_exhausted": + clock[0] = 1.0 + else: + session.register_case( + SceneCase("case", "initial", "scene", "task", "robot"), + torch.tensor([[-1.0, 1.0]]), + joint_names=("joint",), + ) + passed = ValidationResult( + ( + ValidationCheck("path_collision", "passed"), + ValidationCheck("task_success", "passed"), + ) + ) + identities = [] + for index in range(2): + identity, _ = session.propose( + "case", + "initial", + source_id="source", + source_revision="revision", + template_id=f"recipe-{index}", + operator_id="episode", + ) + session.add_prepared_episode(EpisodeCandidate(identity, "{}"), passed) + assert ( + session.take_ready("case", "initial", episode_byte_budget=1024) + is not None + ) + session.mark_rollout_started(identity) + identities.append(identity) + session.release(identities[0], reason="rejected") + assert session.stop_reason is None + session.release(identities[1], reason="rejected") + assert session.stop_reason == stop + proposed = session.snapshot()["counts"]["proposed"] + request = object() + journal = Mock() + ctx = SimpleNamespace(requests={0: request}, journal=journal) + source = object.__new__(_GymWorkloadSource) + source.groups = { + "expired": { + "host": SimpleNamespace(_context=ctx), + "engine": object(), + "session": session, + } + } + assert source.activate(WorkloadAssignment("expired", 0, 0, 0), None) is None + journal.update.assert_called_once_with( + request, status="initial_rejected", reason=stop + ) + assert session.snapshot()["counts"]["proposed"] == proposed + + +@pytest.mark.parametrize( + "stop", ["wall_time_exhausted", "rollout_budget_exhausted", "unexpected"] +) +def test_admission_expiry_race_only_suppresses_budget_exhaustion(stop): + session = SimpleNamespace(stop_reason=None, register_case=Mock()) + + def propose(*args, **kwargs): + session.stop_reason = stop + raise RuntimeError("proposal failed") + + session.propose = propose + request = SimpleNamespace( + recipe=SimpleNamespace( + recipe_id="recipe", + operations=[SimpleNamespace(name="physical_parameters", payload_json="{}")], + ), + attempt_id=0, + ) + ctx = SimpleNamespace( + requests={0: request}, + journal=Mock(), + collision=None, + report={"semantic_id": "semantic", "recipe_revision": "revision"}, + cfg=SimpleNamespace(seed=0, source="task_program"), + compatibility=SimpleNamespace(key="compatible"), + program=SimpleNamespace(program_id="program"), + limits=object(), + names=("joint",), + ) + source = object.__new__(_GymWorkloadSource) + source.groups = { + "group": { + "host": SimpleNamespace(_context=ctx), + "engine": object(), + "session": session, + } + } + source.activation_snapshot = (SimpleNamespace(batches={0: ()}), {}) + source.env = SimpleNamespace(robot=SimpleNamespace(uid="robot")) + assignment = WorkloadAssignment("group", 0, 0, 0) + if stop != "unexpected": + assert source.activate(assignment, None) is None + assert ctx.journal.update.call_args.kwargs["reason"] == stop + else: + with pytest.raises(RuntimeError, match="proposal failed"): + source.activate(assignment, None) + ctx.journal.update.assert_not_called() + + +@pytest.mark.parametrize( + "difference", + [ + None, + "position_velocity", + "use_action_manager", + "active_joint_ids", + "joint_order", + "preparation_params", + "preparation_func", + "preparation_same", + ], +) +def test_workload_action_schema_checked_before_constructing_world( + tmp_path, monkeypatch, difference +): + from embodichain.lab.expansion import ExpansionRunCfg + from embodichain.lab.gym.envs.expert_trajectory import ExpertTrajectoryCfg + from embodichain.lab.gym.utils.registration import REGISTERED_ENVS, EnvSpec + import embodichain.lab.gym.utils.gym_utils as utils + import embodichain.lab.expansion.host as host + + class ReachedWorld(Exception): + pass + + factory = Mock(side_effect=ReachedWorld) + monkeypatch.setattr(host, "GymExpansionHost", factory) + from copy import deepcopy + from pathlib import Path + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.utils.utility import load_config + + deployment = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + base = utils.config_to_cfg(load_config(deployment), source_path=deployment) + workloads, configs = [], {} + for i, name in enumerate(("first", "second")): + deployment = tmp_path / f"{name}.json" + deployment.write_text(json.dumps({"id": name})) + expert = ExpertTrajectoryCfg( + joint_command_mode=( + "position_velocity" + if i and difference == "position_velocity" + else "position" + ), + use_action_manager=bool(i and difference == "use_action_manager"), + ) + config = deepcopy(base) + config.expert_trajectory = expert + config.control_parts = ["arm"] + config.active_joint_ids = [] + if difference in ("active_joint_ids", "joint_order"): + config.control_parts = None + config.active_joint_ids = ( + [i] if difference == "active_joint_ids" else ([1, 0] if i else [0, 1]) + ) + if difference in {"preparation_params", "preparation_func", "preparation_same"}: + from embodichain.lab.gym.envs.managers.cfg import FunctorCfg + + function = ( + "_prepare_cube_scene_offset" + if i and difference == "preparation_func" + else "_prepare_cube_scene" + ) + config.scene_preparation = FunctorCfg( + func=f"{__name__}:{function}", + params={"x": 0.8 if i and difference == "preparation_params" else 0.2}, + ) + configs[name] = config + # Configured tasks share this implementation class. Selection belongs + # to the resolved configs above, never to artificial per-task classes. + monkeypatch.setitem(REGISTERED_ENVS, name, EnvSpec(name, EmbodiedEnv)) + cfg = ExpansionRunCfg( + deployment=str(deployment), + output_dir=str(tmp_path / name), + max_steps=20, + settling_steps=1, + ) + workloads.append( + GymExpansionWorkload( + name, + cfg, + { + "input_files": { + "deployment": hashlib.sha256( + deployment.read_bytes() + ).hexdigest() + } + }, + {}, + ) + ) + monkeypatch.setattr( + utils, "config_to_cfg", lambda raw, **kwargs: configs[raw["id"]] + ) + execution = WorkloadExecutionCfg( + workload_count=2, batch_size=1, max_steps=20, settling_steps=1 + ) + if difference in (None, "preparation_same"): + with pytest.raises(ReachedWorld): + run_gym_expansion_workloads( + workloads, execution, output_dir=tmp_path / "run" + ) + factory.assert_called_once() + else: + with pytest.raises(ValueError, match="schema"): + run_gym_expansion_workloads( + workloads, execution, output_dir=tmp_path / "run" + ) + factory.assert_not_called() + + +@pytest.mark.parametrize( + "field,registered_x,explicit_x,compatible", + [ + ("control_parts", (None, None), (None, None), False), + ("scene_preparation", (None, None), (0.2, 0.8), False), + ("scene_preparation", (0.2, 0.8), (None, None), False), + ("scene_preparation", (0.2, 0.2), (None, None), True), + ("scene_preparation", (0.2, 0.8), (0.4, 0.4), True), + ("scene_preparation", (None, 0.2), (0.2, None), True), + ], + ids=[ + "control_parts", + "explicit_preparation_mismatch", + "registered_preparation_mismatch", + "registered_preparation_same", + "explicit_preparation_overrides_defaults", + "explicit_matches_registered_preparation", + ], +) +def test_real_deployments_validate_shared_world_setup( + tmp_path, monkeypatch, field, registered_x, explicit_x, compatible +): + from pathlib import Path + from embodichain.lab.expansion import ExpansionRunCfg + from embodichain.lab.expansion.task_program_jobs import _prepare_deployment + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + register_env_function, + ) + from embodichain.utils.utility import load_config + import embodichain.lab.expansion.host as host + + import yaml + + task = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + inputs = tmp_path / "inputs" + inputs.mkdir() + base = load_config(_prepare_deployment(task, inputs)) + workloads = [] + + def preparation(x): + return {"func": f"{__name__}:_prepare_cube_scene", "params": {"x": x}} + + for i, name in enumerate(("first", "second")): + component = load_config(task.parent / "envs/default.yaml") + if field == "control_parts": + component["env"][field] = ["arm"] if i == 0 else ["arm", "eef"] + elif explicit_x[i] is not None: + component["env"][field] = preparation(explicit_x[i]) + physical = inputs / f"{name}.environment.yaml" + physical.write_text(yaml.safe_dump(component)) + declaration = { + **base, + "id": f"WorkloadControlSchema-{name}-v1", + "environment": {"component": str(physical)}, + } + path = inputs / f"{name}.json" + path.write_text(json.dumps(declaration)) + config = config_to_cfg(declaration, source_path=path) + if field == "control_parts": + assert config.control_parts == component["env"][field] + elif explicit_x[i] is None: + assert config.scene_preparation is None + else: + assert config.scene_preparation.func == preparation(explicit_x[i])["func"] + assert config.scene_preparation.params == {"x": explicit_x[i]} + spec = REGISTERED_ENVS[declaration["id"]] + # Keep the real configured integration and ordinary environment class, + # but register task-owned defaults through the supported entry point. + register_env_function( + EmbodiedEnv, + declaration["id"], + override=True, + max_episode_steps=spec.max_episode_steps, + task_program_adapter_factory=spec.task_program_adapter_factory, + **( + {"scene_preparation": preparation(registered_x[i])} + if registered_x[i] is not None + else {} + ), + ) + cfg = ExpansionRunCfg( + deployment=str(path), + output_dir=str(tmp_path / name), + max_steps=20, + settling_steps=1, + ) + workloads.append( + GymExpansionWorkload( + name, + cfg, + { + "input_files": { + "deployment": hashlib.sha256(path.read_bytes()).hexdigest() + } + }, + {}, + ) + ) + + class ReachedWorld(Exception): + pass + + constructor = Mock(side_effect=ReachedWorld) + monkeypatch.setattr(host, "GymExpansionHost", constructor) + with pytest.raises( + ReachedWorld if compatible else ValueError, + match=None if compatible else "workloads must share", + ): + run_gym_expansion_workloads( + workloads, + WorkloadExecutionCfg( + workload_count=2, batch_size=1, max_steps=20, settling_steps=1 + ), + output_dir=tmp_path / "run", + ) + if compatible: + constructor.assert_called_once() + else: + constructor.assert_not_called() diff --git a/tests/lab/expansion/test_journal.py b/tests/lab/expansion/test_journal.py index e50bd796b4..d2d67f3e91 100644 --- a/tests/lab/expansion/test_journal.py +++ b/tests/lab/expansion/test_journal.py @@ -91,6 +91,21 @@ def test_interrupted_recipe_retries_with_new_attempt_after_capacity_change(tmp_p next_run.checkpoint(force=True, state="closed") +def test_rolling_allocation_preserves_live_peers_and_ordinal_identity(tmp_path): + ledger = ExpansionJournal(tmp_path) + ledger.open_run( + {"job_id": "a", "semantic_id": "a", "execution_id": "r"}, {}, resume=False + ) + first = ledger.allocate_recipe(recipe(0)) + ledger.update(first, status="running") + second = ledger.allocate_recipe(recipe(1)) + assert [r["status"] for r in ledger.attempts()] == ["running", "proposed"] + assert (second.sequence, second.recipe.context.ordinal) == (1, 1) + with pytest.raises(ValueError, match="ordinals"): + ledger.allocate_recipe(recipe(1)) + ledger.checkpoint(force=True, state="finished") + + def test_changed_semantics_cannot_reuse_job(tmp_path): ledger = ExpansionJournal(tmp_path) ledger.open_run( diff --git a/tests/lab/expansion/test_layout_sampling_sim.py b/tests/lab/expansion/test_layout_sampling_sim.py new file mode 100644 index 0000000000..1c92261536 --- /dev/null +++ b/tests/lab/expansion/test_layout_sampling_sim.py @@ -0,0 +1,103 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Real eight-row conditional layout collection and immutable readback.""" + +from __future__ import annotations + +import os +from pathlib import Path +import subprocess +import sys +import pytest + + +def _probe(output: Path, strategy: str) -> None: + import json + import yaml + import torch + from embodichain.lab.expansion import ( + ArtifactEpisodeSink, + load_expansion_episode, + run_task_program_expansion, + report_expansion_distribution, + ) + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + fixture = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first" + ) + cfg = yaml.safe_load((fixture / "expansion.yaml").read_text()) + cfg["engine"].update( + workload_count=1, + batch_size=8, + episodes_per_operator=8, + max_attempts=32, + nominal_seed=7, + ) + cfg["scene_sampling"].update(strategy=strategy, max_rollouts=32) + path = output / "expansion.yaml" + path.write_text(yaml.safe_dump(cfg)) + result = run_task_program_expansion( + fixture / "task.yaml", path, output / "collection", seed=11 + ) + execution = json.loads((output / "collection/workload-execution.json").read_text()) + reference = json.loads((output / "collection/reference-scene.json").read_text()) + assert (reference["seed"], reference["sampling_seed"]) == (7, 11) + assert execution["num_envs"] == 8 and execution["world"]["world_count"] == 1 + group = result["operators"]["object_pose"] + receipts = ArtifactEpisodeSink(group["directory"]).inspect() + assert len(receipts) == group["committed_episodes"] == 8 + selected = set() + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + assert episode.validation.accepted + assert len(episode.timestamps) == len(episode.actions) + 1 + selected.add(episode.metadata["source_env_id"]) + assert selected == set(range(8)) + source = Path(group["directory"]) / "scene-first-checks.json" + checks = json.loads(source.read_text())["phase_checks"] + assert checks and all( + any(c["stage"] == "screening.clock" and c["status"] == "passed" for c in rows) + for rows in checks.values() + ) + report = report_expansion_distribution( + [output / "collection"], + [output / "collection"], + output / "distribution", + entity="cube", + cell_size_m=0.01, + ) + assert report["before"] == report["after"] == 8 + assert report["comparison_kind"] == "cumulative" + + +@pytest.mark.parametrize("strategy", ["scene_first", "robot_first", "bidirectional"]) +def test_conditional_layout_uses_one_world_and_eight_real_rows(tmp_path, strategy): + result = subprocess.run( + [sys.executable, __file__, str(tmp_path), strategy], + env={**os.environ, "EMBODICHAIN_SIM_EXIT_PROCESS": "0"}, + capture_output=True, + text=True, + timeout=600, + ) + assert result.returncode == 0, result.stdout + result.stderr + + +if __name__ == "__main__": + _probe(Path(sys.argv[1]), sys.argv[2]) diff --git a/tests/lab/expansion/test_persistence.py b/tests/lab/expansion/test_persistence.py index 074f02cfa6..cb8e043b04 100644 --- a/tests/lab/expansion/test_persistence.py +++ b/tests/lab/expansion/test_persistence.py @@ -30,7 +30,10 @@ ValidationCheck, ValidationResult, ) -from embodichain.lab.expansion.persistence import ArtifactEpisodeSink +from embodichain.lab.expansion.persistence import ( + ArtifactEpisodeSink, + load_expansion_episode, +) from embodichain.data_pipeline.recording.media import MediaSpool @@ -63,6 +66,42 @@ def test_staging_has_no_credit_and_recovery_never_reruns_physics(tmp_path): assert len(sink.inspect()) == 1 +def test_verified_episode_reads_own_tensors_and_invalidate_changed_bytes(tmp_path): + sink = ArtifactEpisodeSink(tmp_path) + data = replace(episode(), metadata={"nested": {"values": [1, 2]}}) + receipt = sink.submit(data, submission_id=0) + first = load_expansion_episode(receipt.storage_id) + second = load_expansion_episode(receipt.storage_id) + first.actions.fill_(99) + first.observations["joint_positions"].fill_(88) + third = load_expansion_episode(receipt.storage_id) + torch.testing.assert_close(second.actions, data.actions, rtol=0, atol=0) + torch.testing.assert_close(third.actions, data.actions, rtol=0, atol=0) + torch.testing.assert_close( + third.observations["joint_positions"], + data.observations["joint_positions"], + rtol=0, + atol=0, + ) + from embodichain.lab.gym.envs._json import json_safe_copy + + projected = json_safe_copy(third.metadata, field_name="metadata") + projected["nested"]["values"][0] = 55 + assert third.metadata["nested"]["values"][0] == 1 + external = {"nested": [3]} + owned = type(third.metadata)(external) + external["nested"][0] = 99 + assert owned["nested"][0] == 3 + with pytest.raises(ValueError, match="finite"): + type(third.metadata)({"invalid": float("nan")}) + with pytest.raises(ValueError, match="finite"): + type(third.metadata["nested"]["values"])((float("inf"),)) + path = Path(receipt.storage_id) + path.write_bytes(b"invalid replaced evidence") + with pytest.raises((ValueError, OSError)): + load_expansion_episode(path) + + def test_media_alignment_is_required_before_receipt(tmp_path): sink = ArtifactEpisodeSink(tmp_path) data = episode() diff --git a/tests/lab/expansion/test_planning_batches.py b/tests/lab/expansion/test_planning_batches.py new file mode 100644 index 0000000000..a1272f2fd0 --- /dev/null +++ b/tests/lab/expansion/test_planning_batches.py @@ -0,0 +1,149 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + + +"""Regression coverage for cooperative planning execution contexts.""" + +from __future__ import annotations + +from types import SimpleNamespace +import pytest +import torch +from embodichain.lab.expansion._planning_batches import batched_planning + + +@pytest.mark.parametrize("fail", [False, True]) +def test_autocast_rows_preserve_precision_and_restore_owner(fail): + class Solver: + def get_fk(self, qpos): + if fail and qpos[0, 0] < 0: + raise ValueError("solver failure") + return qpos @ torch.eye(2) + + solver = Solver() + robot = SimpleNamespace(device=torch.device("cpu"), _solvers={"arm": solver}) + inputs = torch.tensor([[1.1234, 2.3456]]) + + class Cursor: + def __init__(self, enabled, value): + self.enabled, self.value = enabled, value + + def next(self): + with torch.autocast("cpu", dtype=torch.bfloat16, enabled=self.enabled): + first = solver.get_fk(self.value) + # Resuming a row must restore the same context for subsequent calls. + second = solver.get_fk(self.value) + return first + second + + cursors = { + 0: Cursor(True, inputs), + 1: Cursor(False, inputs), + 2: Cursor(True, inputs), + } + expected = {row: cursor.next() for row, cursor in cursors.items()} + cpu_rng = torch.get_rng_state().clone() + with torch.autocast("cpu", enabled=False): + with batched_planning(robot, 7) as planner: + if fail: + cursors[1].value = -inputs + with pytest.raises(ValueError, match="solver failure"): + planner.gather(cursors) + else: + actual = planner.gather(cursors) + for row in actual: + torch.testing.assert_close( + actual[row], expected[row], rtol=0, atol=0 + ) + saved = actual[2].clone() + actual[0].zero_() + torch.testing.assert_close(actual[2], saved) + assert not torch.is_autocast_enabled("cpu") + assert not torch.is_autocast_enabled("cpu") + assert torch.equal(cpu_rng, torch.get_rng_state()) + assert "get_fk" not in solver.__dict__ + + +@pytest.mark.gpu +@pytest.mark.parametrize("robot_device", ["cpu", "cuda:0"]) +def test_planning_scope_leaves_external_rng_generators_unchanged(robot_device): + robot = SimpleNamespace(device=torch.device(robot_device), _solvers={}) + with torch.random.fork_rng(devices=list(range(torch.cuda.device_count()))): + torch.manual_seed(123) + cpu = torch.get_rng_state().clone() + cuda = [state.clone() for state in torch.cuda.get_rng_state_all()] + with batched_planning(robot, 7): + assert torch.equal(cpu, torch.get_rng_state()) + for before, after in zip(cuda, torch.cuda.get_rng_state_all()): + assert torch.equal(before, after) + assert torch.equal(cpu, torch.get_rng_state()) + for before, after in zip(cuda, torch.cuda.get_rng_state_all()): + assert torch.equal(before, after) + + +def test_full_motion_grouping_preserves_autocast(monkeypatch): + from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenOptions, + ) + from embodichain.lab.sim.motion.planners.utils import PlanState, PlanResult + + robot = SimpleNamespace(device=torch.device("cpu"), _solvers={}) + + def plan(self, targets, options): + positions = (options.start_qpos @ torch.eye(2))[:, None] + return PlanResult( + success=torch.ones(len(positions), dtype=torch.bool), + positions=positions, + dt=torch.zeros(len(positions), 1), + ) + + monkeypatch.setattr(MotionGenerator, "_generate_once", plan) + + class Cursor: + def __init__(self, row, enabled): + self.enabled = enabled + self.generator = object.__new__(MotionGenerator) + self.generator.robot = robot + self.generator.planner = SimpleNamespace( + cfg=None, collision_world_info=None + ) + self.generator._env_ids = (row,) + + def next(self): + with torch.autocast("cpu", dtype=torch.bfloat16, enabled=self.enabled): + start = torch.tensor([[1.1234, 2.3456]]) + return self.generator.generate( + [PlanState.from_qpos(start)], + MotionGenOptions( + strategy="ik_interp", + interpolation_mode="fixed_grid", + sample_count=2, + interpolation_dt=0.1, + start_qpos=start, + ), + ) + + cursors = {0: Cursor(0, True), 1: Cursor(1, False), 2: Cursor(2, True)} + expected = {row: cursor.next() for row, cursor in cursors.items()} + with batched_planning(robot, 7) as planner: + actual = planner.gather(cursors) + for row in actual: + torch.testing.assert_close( + actual[row].positions, expected[row].positions, rtol=0, atol=0 + ) + saved = actual[2].positions.clone() + actual[0].positions.zero_() + torch.testing.assert_close(actual[2].positions, saved) diff --git a/tests/lab/expansion/test_preview.py b/tests/lab/expansion/test_preview.py new file mode 100644 index 0000000000..1c9054d10a --- /dev/null +++ b/tests/lab/expansion/test_preview.py @@ -0,0 +1,458 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Result-only preview: provenance, row selection, controls and HTTP lifecycle.""" + +from __future__ import annotations + +import base64 +from dataclasses import replace +import hashlib +import json +from pathlib import Path +import threading +import urllib.error +import urllib.request +import numpy as np +import pytest +import torch + +from embodichain.lab.expansion.contracts import ( + CandidateIdentity, + ExpertEpisode, + ValidationResult, + ValidationCheck, +) +from embodichain.lab.expansion.persistence import ArtifactEpisodeSink +from embodichain.lab.expansion._preview_data import load_preview, PreviewState +from embodichain.lab.expansion._preview_render import prepare_geometry +from embodichain.lab.expansion._preview_server import PreviewTransport + + +@pytest.fixture +def recorded_job(tmp_path): + asset = tmp_path / "robot.urdf" + asset.write_text( + '' + '' + '' + '' + '' + ) + output = tmp_path / "result" + output.mkdir() + job = { + "resolved_deployment": { + "robot": { + "uid": "robot", + "fpath": str(asset), + "solver_cfg": {"arm": {"end_link_name": "tip"}}, + }, + "rigid_object": [ + { + "uid": "box", + "shape": {"shape_type": "Cube", "size": [0.05, 0.05, 0.05]}, + } + ], + } + } + (output / "job.json").write_text(json.dumps(job)) + poses = torch.tensor([[0.0, 0, 0, 0, 0, 0, 1]] * 3) + objects = poses.clone() + objects[:, :2] = torch.tensor([10.0, 20.0]) + transforms = np.tile(np.eye(4), (2, 1, 1)) + transforms[:, 0, 3] = [100, 200] + selected = transforms.copy() + selected[:, 0, 3] += [1, 2] + sample = { + "env_ids": [3, 7], + "success": [True, True], + "sampling": {"branch_overrides": [0, 1]}, + "candidate_ids": [23, 31], + "reference_poses": transforms.tolist(), + "selected_poses": selected.tolist(), + } + metadata = { + "initial_state": [ + {"kind": "robot", "uid": "robot", "joint_names": ["joint"]}, + {"kind": "rigid_object", "uid": "box"}, + ], + "scene_state_fields": [ + {"entity_uid": "robot", "field": "pose", "key": "robot_pose"} + ], + "expansion": [ + {"env_id": 7, "trajectory_requested": 1, "trajectory_selected": 1} + ], + "program_segments": [ + { + "metadata": { + "runtime": { + "calls": [ + { + "call": { + "call_kind": "pick", + "arguments": {"object": {"entity_id": "box"}}, + }, + "plan_attempts": [ + { + "planner_diagnostics": { + "metadata": { + "affordance_sample": {"grasp": sample} + } + } + } + ], + "events": [ + { + "kind": "trajectory_segment_entered", + "timestamp": 0.0, + "segment_name": "approach", + }, + { + "kind": "trajectory_segment_entered", + "timestamp": 0.4, + "segment_name": "lift", + }, + ], + } + ] + } + } + } + ], + } + episode = ExpertEpisode( + CandidateIdentity( + "scene", "initial", "candidate", "family", "source", "v1", "template" + ), + { + "full_joint_positions": torch.tensor([[0.0], [np.pi / 2], [np.pi]]), + "robot_pose": poses, + "rigid/box/state": objects, + }, + torch.zeros((2, 1)), + torch.tensor([0.0, 0.5, 1.0], dtype=torch.float64), + "qpos", + ValidationResult((ValidationCheck("task_success", "passed"),)), + "episode", + "commit", + metadata=metadata, + ) + ArtifactEpisodeSink(output).submit(episode, submission_id=0) + return output, job, episode + + +def test_result_loading_binds_grasp_row_and_fk_without_writing_results(recorded_job): + path, _, _ = recorded_job + before = { + p.relative_to(path): hashlib.sha256(p.read_bytes()).hexdigest() + for p in path.rglob("*") + if p.is_file() + } + data = load_preview([path]) + clip = data.clips[0] + assert ( + clip.affordance == "1" and clip.candidate_ids == ("31",) and clip.variant == "1" + ) + np.testing.assert_allclose(clip.targets[0, :3, 3], [12, 20, 0]) + names, _ = prepare_geometry(data) + np.testing.assert_allclose(clip.tips, [[1, 0, 0], [0, 1, 0], [-1, 0, 0]], atol=1e-6) + assert "tip" in names and clip.links.shape[0] == 3 + after = { + p.relative_to(path): hashlib.sha256(p.read_bytes()).hexdigest() + for p in path.rglob("*") + if p.is_file() + } + assert before == after + + +def test_result_loader_rejects_incomplete_scene_and_budget(recorded_job, tmp_path): + path, job, episode = recorded_job + with pytest.raises(ValueError, match="aggregate result or native job"): + load_preview([tmp_path / "missing"]) + assert not (tmp_path / "missing").exists() + ArtifactEpisodeSink(path).submit( + replace(episode, episode_id="episode2", commit_id="commit2"), submission_id=1 + ) + with pytest.raises(ValueError, match="budget"): + load_preview([path], max_episodes=1) + with pytest.raises(ValueError, match="reference"): + load_preview([path], reference=path) + job["resolved_deployment"]["rigid_object"][0]["shape"]["shape_type"] = "Mesh" + (path / "job.json").write_text(json.dumps(job)) + with pytest.raises(ValueError, match="asset is unavailable"): + load_preview([path]) + + +def test_result_loader_keeps_real_mesh_spec_and_native_operator_labels( + recorded_job, tmp_path +): + path, job, episode = recorded_job + mesh = tmp_path / "object.ply" + mesh.write_text( + "ply\nformat ascii 1.0\nelement vertex 3\nproperty float x\nproperty float y\nproperty float z\nelement face 1\nproperty list uchar int vertex_indices\nend_header\n0 0 0\n1 0 0\n0 1 0\n3 0 1 2\n" + ) + job["resolved_deployment"]["rigid_object"][0]["shape"] = { + "shape_type": "Mesh", + "fpath": str(mesh), + } + (path / "job.json").write_text(json.dumps(job)) + data = load_preview([path]) + assert data.boxes["box"] is None + assert data.object_specs["box"]["shape"]["fpath"] == str(mesh) + from embodichain.lab.expansion._preview_data import annotations + + result = annotations( + { + "source_env_id": 0, + "expansion": [{"env_id": 0, "operator": "joint_residual", "proposal": 3}], + }, + data.clips[0].measured, + ) + assert result["variant"] == "joint_residual" + assert result["affordance"] == "unrecorded" + + +def test_filter_empty_intersection_and_phase_alignment(recorded_job): + path, _, _ = recorded_job + data = load_preview([path]) + first = data.clips[0] + data.clips = [ + replace(first, space="A", affordance="0", variant="0"), + replace(first, space="B", affordance="1", variant="1"), + ] + state = PreviewState(data) + state.apply({"space": "B"}) + assert state.visible() == [1] and state.values["selected"] == 1 + state.apply({"aff": "0", "playing": True}) + assert state.visible() == [] and not state.values["playing"] + assert state.snapshot()["source_frame"] is None + state.apply({"space": "all", "aff": "all", "progress": 0.5}) + assert state.frame_at(first) == 0 # phase boundary .4 s -> recorded frame at 0 s + state.apply({"align": "time"}) + assert state.frame_at(first) == 1 # exactly .5 s + state.apply({"progress": 1}) + assert state.frame_at(first) == 2 + for update in ( + {"progress": float("nan")}, + {"selected": True}, + {"playing": "true"}, + {"unknown": 0}, + {"aff": "missing"}, + ): + before = dict(state.values) + with pytest.raises(ValueError): + state.apply(update) + assert state.values == before + + +def test_http_snapshot_control_and_shutdown(recorded_job): + path, _, _ = recorded_job + state = PreviewState(load_preview([path])) + transport = PreviewTransport(state, 0) + transport.publish({"frame": 7, "source_frame": 2}, b"jpeg-frame-7") + thread = threading.Thread(target=transport.server.serve_forever) + thread.start() + try: + snapshot = json.load(urllib.request.urlopen(transport.url + "snapshot")) + assert snapshot["frame"] == 7 and snapshot["source_frame"] == 2 + assert base64.b64decode(snapshot["image"].split(",")[1]) == b"jpeg-frame-7" + req = urllib.request.Request( + transport.url + "control", + data=b'{"progress":0.5}', + headers={"Content-Type": "application/json"}, + method="POST", + ) + assert urllib.request.urlopen(req).status == 204 + assert ( + state.values["progress"] == 0 + ) # only the render owner applies queued commands + state.apply(transport.commands.get_nowait()) + assert state.values["progress"] == 0.5 + req.add_header("Origin", "https://untrusted.example") + with pytest.raises(urllib.error.HTTPError) as error: + urllib.request.urlopen(req) + assert error.value.code == 403 + finally: + transport.server.shutdown() + thread.join(timeout=5) + transport.server.server_close() + assert not thread.is_alive() + + +def test_preview_cli_dispatch_does_not_collect(monkeypatch, recorded_job): + from embodichain.cli.expansion import main + import embodichain.lab.expansion._preview_server as server + from embodichain.lab.expansion import ExpansionEngine + + path, _, _ = recorded_job + called = [] + monkeypatch.setattr( + server, "serve_preview", lambda data, **kw: called.append((len(data.clips), kw)) + ) + monkeypatch.setattr( + ExpansionEngine, + "from_job", + lambda *a, **kw: pytest.fail("preview must not collect"), + ) + assert main(["preview", "--input", str(path), "--gpu", "2", "--open"]) == 0 + assert ( + called[0][0] == 1 and called[0][1]["gpu"] == 2 and called[0][1]["open_browser"] + ) + + +def test_viewer_html_is_in_package_data(tmp_path): + from setup import get_package_data + from setuptools import Distribution + from setuptools.command.build_py import build_py + import shutil + + package = "embodichain.lab.expansion" + source = tmp_path / "source" + source.mkdir() + original = ( + Path(__file__).resolve().parents[3] / "embodichain/lab/expansion/_preview.html" + ) + shutil.copyfile(original, source / "_preview.html") + (source / "__init__.py").write_text("") + distribution = Distribution( + { + "packages": [package], + "package_dir": {package: str(source)}, + "package_data": get_package_data(), + "include_package_data": False, + } + ) + distribution.script_name = "setup.py" + command = build_py(distribution) + command.build_lib = str(tmp_path / "build") + command.ensure_finalized() + command.run() + packaged = Path(command.build_lib) / "embodichain/lab/expansion/_preview.html" + assert packaged.read_bytes() == original.read_bytes() + + +def test_manifest_keeps_rejected_points_visible_in_coverage(recorded_job): + path, _, _ = recorded_job + root = path.parent + manifest = { + "records": [ + {"result": path.name, "confirmed": 1, "status": "confirmed"}, + {"result": "unqualified", "confirmed": 0, "status": "rejected"}, + ], + "coverage_complete": False, + } + (root / "preview-plan.json").write_text(json.dumps(manifest)) + data = load_preview([root]) + assert len(data.clips) == 1 + assert ( + PreviewState(data).snapshot()["coverage"]["records"][1]["status"] == "rejected" + ) + manifest["records"][1]["result"] = "../escape" + (root / "preview-plan.json").write_text(json.dumps(manifest)) + with pytest.raises(ValueError, match="escapes"): + load_preview([root]) + + +def test_job_preview_collects_then_opens_and_input_alias_stays_read_only( + monkeypatch, recorded_job +): + from embodichain.cli.expansion import main + import embodichain.lab.expansion._preview_job as job + import embodichain.lab.expansion._preview_server as server + import torch + + path, _, _ = recorded_job + calls = [] + monkeypatch.setattr( + job, + "plan_preview", + lambda *a, **kw: {"output": str(path), "records": [{}], "planned_attempts": 1}, + ) + + def collect(plan): + calls.append("collect") + return {**plan, "confirmed": 1, "coverage_complete": True} + + monkeypatch.setattr(job, "collect_preview", collect) + monkeypatch.setattr(torch.cuda, "device_count", lambda: 1) + monkeypatch.setattr( + server, + "serve_preview", + lambda data, **kw: calls.append(("serve", kw["open_browser"])), + ) + assert main(["preview", "--job", "job.yaml"]) == 0 + assert calls == ["collect", ("serve", True)] + calls.clear() + assert main(["preview", "--inpu", str(path)]) == 0 + assert calls == [("serve", False)] + + +def test_row_identity_without_motion_and_legacy_ambiguity(recorded_job): + from embodichain.lab.expansion._preview_data import annotations + from embodichain.lab.gym.envs._json import json_safe_copy + + path, _, episode = recorded_job + measured = load_preview([path]).clips[0].measured + metadata = json_safe_copy(episode.metadata, field_name="metadata") + metadata["expansion"] = [] + assert annotations(metadata, measured)["targets"].shape == (0, 4, 4) + metadata["source_env_id"] = 7 + result = annotations(metadata, measured) + np.testing.assert_allclose(result["targets"][0, :3, 3], [12, 20, 0]) + assert result["candidate_ids"] == ("31",) + + +@pytest.mark.parametrize("second_count", [1, 2]) +def test_aggregate_input_keeps_explicit_reference_separate(recorded_job, second_count): + path, job, episode = recorded_job + root = path.parent + directories = [] + for name, count in (("first", 1), ("second", second_count), ("reference", 1)): + directory = root / name + directory.mkdir() + (directory / "job.json").write_text(json.dumps(job)) + for index in range(count): + ArtifactEpisodeSink(directory).submit( + replace( + episode, + episode_id=f"{name}-{index}", + commit_id=f"{name}-commit-{index}", + ), + submission_id=index, + ) + directories.append(directory) + aggregate = root / "aggregate" + aggregate.mkdir() + (aggregate / "expansion.json").write_text( + json.dumps( + { + "operators": { + str(i): { + "directory": str(p), + "committed_episodes": 1 if i == 0 else second_count, + } + for i, p in enumerate(directories[:2]) + } + } + ) + ) + data = load_preview( + [aggregate], reference=directories[2], max_episodes=1 + second_count + ) + assert len(data.clips) == 1 + second_count + assert data.reference.space.endswith("reference") + assert all(not clip.space.endswith("reference") for clip in data.clips) diff --git a/tests/lab/expansion/test_preview_job.py b/tests/lab/expansion/test_preview_job.py new file mode 100644 index 0000000000..fd995017b2 --- /dev/null +++ b/tests/lab/expansion/test_preview_job.py @@ -0,0 +1,202 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Boundary plans cover declared domains without resampling failed endpoints.""" + +from __future__ import annotations + +import json +from pathlib import Path +import numpy as np +import pytest +import yaml +from embodichain.lab.expansion._preview_job import ( + boundary_samples, + plan_preview, + collect_preview, +) + + +def test_endpoint_and_stratified_interior_sampling_is_deterministic(): + points, roles = boundary_samples(7, 5, 17) + assert roles[:3] == ["center", "lower", "upper"] + np.testing.assert_array_equal(points[1], np.zeros(7)) + np.testing.assert_array_equal(points[2], np.ones(7)) + assert np.all((points[3:] > 0) & (points[3:] < 1)) + for column in points[3:].T: + assert sorted((column * 5).astype(int)) == list(range(5)) + np.testing.assert_array_equal(points, boundary_samples(7, 5, 17)[0]) + assert boundary_samples(0, 5, 17)[0].shape == (1, 0) + + +def test_plan_keeps_job_bounds_and_produces_explicit_motion_draws(tmp_path): + from embodichain.lab.expansion.configured import load_expansion_job + + root = Path(__file__).resolve().parents[3] + original = root / "examples/expansion/engine/job.motion.yaml" + before = original.read_bytes() + plan = plan_preview(original, tmp_path / "preview", interior=1) + assert len(plan["records"]) == 4 + assert ( + plan["planned_attempts"] == 7 + ) # exact-zero center is one nominal, not two copies + low = yaml.safe_load((tmp_path / "preview" / plan["records"][1]["job"]).read_text()) + high = yaml.safe_load( + (tmp_path / "preview" / plan["records"][2]["job"]).read_text() + ) + assert low["mass_scale"]["cube"] == [0.98, 0.98] and high["mass_scale"]["cube"] == [ + 1.02, + 1.02, + ] + low_motion = yaml.safe_load(Path(low["motion_profile"]).read_text()) + assert set(low_motion["trajectory"]["spatial"]["joint_samples"].values()) == {-1.0} + assert low["collision_profile"] == load_expansion_job(original).collision_profile + assert original.read_bytes() == before + with pytest.raises(FileExistsError): + plan_preview(original, tmp_path / "preview", interior=1) + with pytest.raises(ValueError, match="Nothing executed"): + plan_preview(original, tmp_path / "too-large", max_episodes=1) + assert not (tmp_path / "too-large").exists() + + +def test_collection_retains_failed_and_unrun_points_without_retry( + monkeypatch, tmp_path +): + import embodichain.lab.expansion._preview_job as module + + (tmp_path / "configs").mkdir() + (tmp_path / "logs").mkdir() + for i in range(2): + (tmp_path / f"configs/{i}.yaml").write_text("max_wall_time_s: 10\n") + plan = dict( + output=str(tmp_path), + collection_budget_s=10, + recipes_per_point=1, + planned_attempts=2, + records=[ + dict( + semantic_id="test", + index=i, + role=role, + job=f"configs/{i}.yaml", + result=f"jobs/{i}", + confirmed=0, + attempts=0, + planned=1, + status="unrun", + ) + for i, role in enumerate(["lower", "upper"]) + ], + ) + monkeypatch.setattr(module, "load_expansion_job", lambda path: None) + monkeypatch.setattr( + module, "preflight_expansion_job", lambda cfg: {"semantic_id": "test"} + ) + calls = [] + + class FailedProcess: + returncode = 2 + + def __init__(self, args, **kw): + calls.append(args) + + def wait(self, timeout): + return 2 + + monkeypatch.setattr(module.subprocess, "Popen", FailedProcess) + result = collect_preview(plan) + assert len(calls) == 2 and all(r["status"] == "rejected" for r in result["records"]) + assert result["confirmed"] == 0 and not result["coverage_complete"] + assert ( + json.loads((tmp_path / "preview-plan.json").read_text())["records"][0]["role"] + == "lower" + ) + + +def test_source_change_stops_before_collecting(monkeypatch, tmp_path): + import embodichain.lab.expansion._preview_job as module + + (tmp_path / "configs").mkdir() + (tmp_path / "configs/job.yaml").write_text("max_wall_time_s: 10\n") + plan = { + "output": str(tmp_path), + "collection_budget_s": 10, + "records": [ + { + "index": 0, + "job": "configs/job.yaml", + "semantic_id": "pinned", + "status": "unrun", + } + ], + } + monkeypatch.setattr(module, "load_expansion_job", lambda path: None) + monkeypatch.setattr( + module, "preflight_expansion_job", lambda cfg: {"semantic_id": "changed"} + ) + monkeypatch.setattr( + module.subprocess, + "Popen", + lambda *a, **k: pytest.fail("changed source must not execute"), + ) + with pytest.raises(ValueError, match="changed after planning"): + collect_preview(plan) + assert ( + json.loads((tmp_path / "preview-plan.json").read_text())["records"][0]["status"] + == "source_changed" + ) + + +def test_reused_affordance_is_not_full_recipe_coverage(): + from embodichain.lab.expansion._preview_job import _exact_recipe + + sample = {"env_ids": [3, 7], "success": [True, True], "reused": [False, True]} + metadata = { + "expansion": [ + { + "env_id": 7, + "affordance_requested": 1, + "trajectory_requested": 1, + "trajectory_selected": 1, + } + ], + "program_segments": [ + { + "metadata": { + "runtime": { + "calls": [ + { + "plan_attempts": [ + { + "planner_diagnostics": { + "metadata": { + "affordance_sample": {"grasp": sample} + } + } + } + ] + } + ] + } + } + } + ], + } + assert not _exact_recipe(metadata) + sample["reused"][1] = False + assert _exact_recipe(metadata) + metadata["expansion"][0]["trajectory_selected"] = 0 + assert not _exact_recipe(metadata) diff --git a/tests/lab/expansion/test_scene_first_sampling.py b/tests/lab/expansion/test_scene_first_sampling.py new file mode 100644 index 0000000000..85f8b6ba5b --- /dev/null +++ b/tests/lab/expansion/test_scene_first_sampling.py @@ -0,0 +1,473 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""All layout sources share conservative screening and own selected poses.""" + +from __future__ import annotations +from types import SimpleNamespace +from copy import deepcopy +import json +import numpy as np +import pytest +import torch + +from scene_layout_support import make_source +from embodichain.lab.expansion._scene_first_sampling import _LayoutPoseSource +from embodichain.lab.expansion._pose_sampling_cfg import validate_sampling +from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted + + +def setup_source( + monkeypatch, + strategy="scene_first", + accepted=True, + member_yaw=False, + workspace=None, + bounded_fixed_root=False, + configure=None, +): + import embodichain.lab.expansion._scene_first_sampling as module + + source = make_source() + + class Object: + def __init__(self, uid): + self.uid = uid + self.cfg = SimpleNamespace(to_dict=lambda: {"uid": uid}) + + def get_local_pose(self, to_matrix=True): + return torch.tensor(np.array(source.poses[self.uid]))[None] + + objects = {uid: Object(uid) for uid in source.poses} + robot = SimpleNamespace( + uid="robot", + device=torch.device("cpu"), + control_parts={"custom_arm": [0]}, + cfg=SimpleNamespace(to_dict=lambda: {"uid": "robot"}), + get_local_pose=lambda to_matrix=True: torch.eye(4)[None], + get_solver=lambda name: object(), + get_qpos_limits=lambda name: torch.tensor([[[0.0, 1.0]]]), + compute_batch_fk=lambda **kw: workspace[None], + ) + env = SimpleNamespace( + num_envs=1, + robot=robot, + sim=SimpleNamespace( + num_envs=1, + get_rigid_object_uid_list=lambda: tuple(objects), + get_rigid_object=objects.get, + sim_config=SimpleNamespace( + physics_cfg=SimpleNamespace(to_dict=lambda: {"backend": "default"}) + ), + ), + ) + checked = [] + closed = [] + monkeypatch.setattr(module, "scene_layout_input_from_sim", lambda *a, **kw: source) + monkeypatch.setattr( + "embodichain.lab.task_program.integrations.simulation.feasibility_geometry.collision_bodies_from_simulation", + lambda env: (), + ) + monkeypatch.setattr( + "embodichain.lab.expansion.acceptance._GeometryRow.contact_permissions", + lambda *a: ((), ()), + ) + monkeypatch.setattr( + "embodichain.lab.sim.scene.feasibility.NewtonCollisionChecker", + lambda *a, **kw: SimpleNamespace(close=lambda: closed.append(True)), + ) + monkeypatch.setattr( + "embodichain.lab.sim.scene.feasibility.RobotFeasibilityBackend", + lambda *a, **kw: None, + ) + + def screen(mode, backend, poses, budget): + checked.append(deepcopy(poses)) + return SimpleNamespace( + accepted=accepted, + checks=( + SimpleNamespace( + stage="motion", + status="passed" if accepted else "failed", + reason="checked task path", + phase_id="reach", + ), + ), + ) + + monkeypatch.setattr("embodichain.lab.sim.scene.feasibility.plan_task_mode", screen) + opts = { + "engine": { + "max_attempts": 8, + "max_wall_time_s": 120.0, + "episodes_per_operator": 1, + }, + "scene": { + "tray": { + "translation_m": [[-0.1, 0.1], [-0.1, 0.1], [0, 0]], + "yaw_rad": [0, 0], + }, + "mug": {"follow": "tray"}, + }, + "validation": {}, + "scene_sampling": { + "strategy": strategy, + "control_part": "custom_arm", + "max_candidates": 64, + "max_rollouts": 8, + "layout": { + "support_uid": "workbench", + "parent_by_uid": dict(source.parent_by_uid), + "policy": { + "group_targets": ["tray"], + "group_xy_offset_bounds_m": {"tray": [[-0.1, -0.1], [0.1, 0.1]]}, + }, + "conditions": {"tray": np.array(source.poses["tray"]).tolist()}, + }, + "task_mode": { + "mode_id": "runtime", + "phases": [ + { + "phase_id": "reach", + "targets": [ + { + "control_part": "custom_arm", + "frame_uid": "mug", + "pose": np.eye(4).tolist(), + } + ], + } + ], + }, + }, + "scheduling": {"policy": "fifo", "pool_size": 4}, + } + if strategy in ("robot_first", "bidirectional"): + sampling = opts["scene_sampling"] + sampling["sample_count"] = len(workspace) + sampling["robot_anchors"] = [ + {"phase_id": "reach", "control_part": "custom_arm"} + ] + sampling["layout"]["conditions"] = { + "workbench": source.poses["workbench"], + } + sampling["task_mode"]["phases"][0]["targets"][0]["pose"][0][3] = 0.04 + opts["scene"]["z_target"] = { + "translation_m": [[-0.1, 0.1], [-0.1, 0.1], [0, 0]], + "yaw_rad": [0, 0], + } + sampling["layout"]["policy"]["group_targets"].append("z_target") + sampling["layout"]["policy"]["group_xy_offset_bounds_m"]["z_target"] = [ + [-0.1, -0.1], + [0.1, 0.1], + ] + if bounded_fixed_root: + opts["scene_sampling"]["layout"]["conditions"] = {} + opts["scene"]["z_target"] = { + "translation_m": [[-0.1, 0.1], [-0.1, 0.1], [0, 0]], + "yaw_rad": [0, 0], + } + if member_yaw: + opts["scene"]["mug"] = { + "translation_m": [[-0.15, 0.15], [-0.15, 0.15], [0, 0]], + "yaw_rad": [0, 0], + } + opts["scene_sampling"]["layout"]["policy"]["intra_group"] = { + "targets": {"mug": {"yaw_delta_range_deg": [30, 30]}} + } + if configure is not None: + configure(opts, objects) + validate_sampling(opts) + return ( + _LayoutPoseSource(env, opts, seed=7, revision="test-source"), + checked, + closed, + ) + + +def test_scene_first_anchors_and_workspace_task_checks_are_owned_and_cached( + monkeypatch, tmp_path +): + source, checked, closed = setup_source(monkeypatch) + source.output = tmp_path + candidate = source.candidate(0) + assert source.validate(0).accepted and len(checked) == 1 + poses = {c.entity_uid: np.array(c.arena_pose) for c in candidate.scene.pose_changes} + np.testing.assert_allclose(poses["tray"], source._conditions.poses["tray"]) + np.testing.assert_allclose(poses["mug"], checked[0]["mug"]) + checked[0]["mug"][0, 3] = 99 + assert candidate is source.candidate(0) + assert source.budget.counts["candidates"] == 1 + assert ( + json.loads((tmp_path / "scene-first-checks.json").read_text())["phase_checks"][ + "0" + ][0]["stage"] + == "motion" + ) + source.close() + source.close() + assert closed == [True] + + +@pytest.mark.parametrize("strategy", ["scene_first", "robot_first", "bidirectional"]) +def test_failed_task_screening_never_becomes_a_successful_fallback( + monkeypatch, strategy +): + source, checked, _ = setup_source( + monkeypatch, strategy=strategy, accepted=False, workspace=_workspace() + ) + source.candidate(0) + assert not source.validate(0).accepted and len(checked) == 4 + assert all(not e["screened"] for e in source._events if e["event"] == "proposal") + + +def _workspace() -> torch.Tensor: + # Tool XY is +4 cm along the support group's X. The first three TCP samples + # share one cell, the fourth is in a different cell, and the last is illegal. + poses = torch.eye(4, dtype=torch.float64).repeat(5, 1, 1) + poses[:, :2, 3] = torch.tensor( + [[-0.309, -0.04], [-0.310, -0.04], [-0.311, -0.04], [-0.19, 0.05], [0.9, 0.0]], + dtype=poses.dtype, + ) + # Keep authored root height and orientation despite the projected FK pose. + poses[:, 2, 3] = 0.7 + poses[:, 1, 1] = poses[:, 2, 2] = -1 + return poses + + +def test_robot_first_balances_prior_cells_and_completes_other_groups(monkeypatch): + workspace = _workspace() + source, checked, _ = setup_source(monkeypatch, "robot_first", workspace=workspace) + workspace.fill_(77) + first = source.candidate(0) + second = source.candidate(1) + assert source.validate(0).accepted and source.validate(1).accepted + assert source.candidate(0) is first + x_coords, other_poses = [], [] + for index, candidate in enumerate((first, second)): + poses = { + c.entity_uid: np.array(c.arena_pose) for c in candidate.scene.pose_changes + } + x_coords.append(poses["tray"][0, 3]) + other_poses.append(poses["z_target"]) + np.testing.assert_allclose(poses["tray"][:3, :3], np.eye(3)) + assert poses["tray"][2, 3] == pytest.approx(0.061) + np.testing.assert_allclose(poses["mug"][:2, 3], poses["tray"][:2, 3]) + np.testing.assert_allclose(checked[index]["tray"], poses["tray"]) + assert min(x_coords) == pytest.approx(-0.35, abs=0.0011) + assert max(x_coords) == pytest.approx(-0.23) + assert not np.allclose(*other_poses) + + +@pytest.mark.parametrize("strategy", ["robot_first", "bidirectional"]) +def test_empty_workspace_draw_costs_a_candidate_and_never_runs_task_screen( + monkeypatch, strategy +): + workspace = _workspace() + workspace[:, 0, 3] = 99 + source, checked, _ = setup_source(monkeypatch, strategy, workspace=workspace) + source.candidate(0) + proposals = [e for e in source._events if e["event"] == "proposal"] + assert not proposals[0]["screened"] + assert proposals[0]["reason"] == "no_workspace_anchor" + if strategy == "robot_first": + assert not source.validate(0).accepted and checked == [] + else: + assert source.validate(0).accepted and len(checked) == 1 + assert [p["origin"] for p in proposals] == ["robot_first", "scene_first"] + assert source._events[-1]["raw_ordinal"] == 1 + assert source.budget.counts["candidates"] == len(proposals) + + +def test_mixed_draws_keep_direction_and_layout_after_checkpoint_restore(monkeypatch): + source, _, _ = setup_source(monkeypatch, "bidirectional", workspace=_workspace()) + first = source.candidate(0) + restored, _, _ = setup_source(monkeypatch, "bidirectional", workspace=_workspace()) + restored.load_state_dict(source.state_dict()) + assert restored.candidate(0) == first + assert restored.candidate(1) == source.candidate(1) + proposals = [e for e in restored._events if e["event"] == "proposal"] + assert [(e["raw_ordinal"], e["origin"]) for e in proposals] == [ + (0, "robot_first"), + (1, "scene_first"), + ] + + +def test_simple_baseline_keeps_identical_task_screening_and_conditions(monkeypatch): + source, checked, _ = setup_source(monkeypatch, strategy="bounded_pose") + # Its simple random draw does not satisfy the exact anchor, and must be + # rejected before any workspace/path computation. + source.candidate(0) + assert not source.validate(0).accepted and checked == [] + assert any(e.get("reason") == "condition_mismatch" for e in source._events) + + +def test_source_obeys_hard_raw_candidate_budget(monkeypatch): + source, checked, _ = setup_source(monkeypatch) + source.budget.charge("candidates", source.cfg["max_candidates"]) + source.candidate(0) + assert not source.validate(0).accepted and checked == [] + assert source.budget.counts["candidates"] == 64 + + +def test_scene_first_config_accepts_batch_and_rejects_unknown_phase_fields(): + # Reuse a valid source's owned config, without constructing a new World. + import yaml + from pathlib import Path + + root = Path(__file__).resolve().parents[3] + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml" + ) + cfg = yaml.safe_load(path.read_text()) + cfg["engine"]["batch_size"] = 8 + validate_sampling(cfg) + cfg["scene_sampling"]["task_mode"]["phases"][0]["unknown"] = True + with pytest.raises(ValueError, match="Unknown TaskPhase"): + validate_sampling(cfg) + + +@pytest.mark.parametrize( + "invalid, error", + [ + ("unknown_stage", "declared phase targets"), + ("moved_frame", "initial frame"), + ("pinned_root", "authored root condition"), + ], +) +def test_robot_anchors_reject_invalid_frame_provenance(invalid, error): + import yaml + from pathlib import Path + + root = Path(__file__).resolve().parents[3] + path = root / ( + "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/" + "acceptance/scene_first/expansion.yaml" + ) + cfg = yaml.safe_load(path.read_text()) + sampling = cfg["scene_sampling"] + sampling["strategy"] = "robot_first" + if invalid == "unknown_stage": + sampling["robot_anchors"][0]["phase_id"] = "unknown" + elif invalid == "moved_frame": + sampling["task_mode"]["phases"][0]["held_objects"] = [ + {"entity_uid": "cube", "control_part": "arm"} + ] + else: + sampling["layout"]["conditions"] = {"cube": np.eye(4).tolist()} + with pytest.raises(ValueError, match=error): + validate_sampling(cfg) + + +def test_source_rejects_stale_geometry_and_does_not_refresh_silently(monkeypatch): + source, checked, _ = setup_source(monkeypatch) + source.env.sim.spawn_result = SimpleNamespace(topology_revision=2) + source._scene_identity = (id(source.env.sim.spawn_result), 1) + with pytest.raises(RuntimeError, match="stale"): + source.candidate(0) + assert checked == [] + + +def test_fixed_layout_geometry_cannot_be_moved_by_a_bounded_draw(monkeypatch): + source, checked, _ = setup_source( + monkeypatch, strategy="bounded_pose", bounded_fixed_root=True + ) + source.candidate(0) + assert not source.validate(0).accepted and not checked + assert source._events[0]["reason"] == "layout_moves_fixed_entity" + + +def test_external_follower_uses_final_layout_parent_pose(monkeypatch): + def configure(opts, objects): + opts["scene"]["reference"] = {"follow": "tray"} + objects["reference"] = SimpleNamespace( + cfg=SimpleNamespace(to_dict=lambda: {"uid": "reference"}), + get_local_pose=lambda to_matrix=True: torch.eye(4)[None], + ) + opts["scene_sampling"]["layout"]["conditions"]["tray"][0][3] += 0.05 + + source, checked, _ = setup_source(monkeypatch, configure=configure) + candidate = source.candidate(0) + assert source.validate(0).accepted + poses = { + c.entity_uid: np.asarray(c.arena_pose) for c in candidate.scene.pose_changes + } + np.testing.assert_allclose(poses["reference"][:3, 3], [0.05, 0, 0], atol=1e-9) + np.testing.assert_allclose(checked[0]["reference"], poses["reference"]) + + +@pytest.mark.parametrize("authored", [False, True]) +def test_outside_layout_bounds_rejects_sample_but_invalidates_authored_anchor( + monkeypatch, authored +): + def configure(opts, objects): + opts["scene"]["tray"]["translation_m"][0] = [0.075, 0.075] + layout = opts["scene_sampling"]["layout"] + layout["policy"]["group_xy_offset_bounds_m"]["tray"][1][0] = 0.06 + if authored: + layout["conditions"]["tray"][0][3] += 0.075 + else: + layout["conditions"] = {} + + if authored: + with pytest.raises(ValueError, match="outside declared root XY bounds"): + setup_source(monkeypatch, strategy="bounded_pose", configure=configure) + else: + source, checked, _ = setup_source( + monkeypatch, strategy="bounded_pose", configure=configure + ) + source.candidate(0) + assert not source.validate(0).accepted and not checked + assert source._events[0]["reason"] == "layout_outside_sampled_domain" + + +def test_member_layout_yaw_cannot_expand_the_task_domain(monkeypatch): + source, checked, _ = setup_source(monkeypatch, member_yaw=True) + source.candidate(0) + assert not source.validate(0).accepted and checked == [] + reasons = [e["reason"] for e in source._events if e["event"] == "proposal"] + assert reasons and set(reasons) == {"layout_outside_task_domain"} + + +def test_screening_must_not_step_the_owning_world(monkeypatch): + source, _, _ = setup_source(monkeypatch) + source.env.sim.physics_step_index = 0 + + def bad_query(*args): + source.env.sim.physics_step_index += 1 + return SimpleNamespace(accepted=True, checks=()) + + monkeypatch.setattr( + "embodichain.lab.sim.scene.feasibility.plan_task_mode", bad_query + ) + with pytest.raises(RuntimeError, match="advanced"): + source.candidate(0) + + +def test_legacy_workspace_strategy_cannot_silently_ignore_new_layout(): + import yaml + from pathlib import Path + + path = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml" + ) + value = yaml.safe_load(path.read_text()) + value["scene_sampling"]["strategy"] = "workspace_joint" + value["scene_sampling"]["targets"] = {"cube": np.eye(4).tolist()} + with pytest.raises(ValueError, match="does not consume layout"): + validate_sampling(value) diff --git a/tests/lab/expansion/test_session.py b/tests/lab/expansion/test_session.py index d8c802989d..c3301850dc 100644 --- a/tests/lab/expansion/test_session.py +++ b/tests/lab/expansion/test_session.py @@ -59,6 +59,95 @@ def test_old_and_new_session_paths_export_the_identical_class(): assert ExpansionSession.__module__ == "embodichain.lab.expansion.session" +def test_competing_rollouts_keep_one_pending_write_and_confirmed_quota(): + session = ExpansionSession( + ExpansionSessionCfg.from_mapping( + { + "collection": { + "target_committed_episodes": 1, + "max_proposals": 3, + "max_rollout_attempts": 3, + }, + "execution": {"ready_high_watermark": 3, "ready_low_watermark": 0}, + "persistence": {"pending_episode_limit": 2, "pending_max_bytes": 4096}, + } + ) + ) + session.register_case( + SceneCase("case", "initial", "scene", "task", "robot"), + torch.tensor([[-1.0, 1.0]]), + joint_names=("joint",), + ) + passed = ValidationResult( + ( + ValidationCheck("path_collision", "passed"), + ValidationCheck("task_success", "passed"), + ) + ) + episodes = [] + for index in range(2): + identity, _ = session.propose( + "case", + "initial", + source_id="source", + source_revision="v1", + template_id=f"recipe{index}", + operator_id="episode", + ) + session.add_prepared_episode(EpisodeCandidate(identity, "{}"), passed) + if index: + assert ( + session.take_ready( + "case", + "initial", + episode_byte_budget=2048, + candidate_id=identity.candidate_id, + ) + is None + ) + assert ( + session.take_ready( + "case", + "initial", + episode_byte_budget=2048, + candidate_id=identity.candidate_id, + speculative=True, + ) + is not None + ) + session.mark_rollout_started(identity) + episode_id, commit_id = session.episode_ids(identity) + episodes.append( + ExpertEpisode( + identity, + {"joint_positions": torch.tensor([[0.0], [0.1]])}, + torch.tensor([[0.1]]), + torch.tensor([0.0, 0.1]), + "qpos", + passed, + episode_id, + commit_id, + ) + ) + assert session.accept_episode(episodes[0]) + # Even before the writer confirms its receipt, the second qualified row + # cannot reserve or write the only quota slot. + assert not session.accept_episode(episodes[1]) + assert session.snapshot()["counts"]["validated_accepted"] == 1 + episode = episodes[0] + receipt = CommitReceipt( + episode.episode_id, + episode.identity.candidate_id, + episode.identity.attempt_id, + "stored", + episode.commit_id, + "case", + ) + assert session.apply_receipt(receipt) + assert not session.apply_receipt(receipt) + assert session.snapshot()["counts"]["committed"] == 1 + + @pytest.mark.parametrize("legacy", [False, True]) def test_explicit_policy_keeps_legacy_trajectory_dedup_and_generic_episode_credit( legacy, diff --git a/tests/lab/expansion/test_task_pose_coverage.py b/tests/lab/expansion/test_task_pose_coverage.py new file mode 100644 index 0000000000..23a8adf3ae --- /dev/null +++ b/tests/lab/expansion/test_task_pose_coverage.py @@ -0,0 +1,733 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Measured-cell accounting, sampling compatibility and feedback regressions.""" + +from __future__ import annotations +from copy import deepcopy +from types import SimpleNamespace +import json +import math +import numpy as np +import pytest +import torch +from embodichain.lab.expansion.task_pose_coverage import TaskPoseCoverage +from embodichain.lab.expansion.contracts import SceneCase +from embodichain.lab.expansion._pose_sampling import ( + _BoundedPoseSource, + _AdaptivePoseSource, +) +from embodichain.lab.expansion._pose_sampling_cfg import validate_sampling +from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted + + +def test_admitted_planning_retains_count_and_cancel_limits_after_admission_time(): + from threading import Event + import time + from embodichain.lab.expansion._pose_sampling import ( + _PoseBudget, + _AdmittedPoseBudget, + ) + from embodichain.lab.sim.scene.feasibility import SceneRobotFeasibilityCfg + + cancellation = Event() + budget = _PoseBudget( + SceneRobotFeasibilityCfg(max_wall_time_s=0.01, max_ik_calls=2), + cancelled=cancellation, + ) + budget.validation = True + admitted = _AdmittedPoseBudget(budget) + time.sleep(0.03) + with pytest.raises(FeasibilityBudgetExhausted, match="wall_time"): + budget.charge("candidates") + admitted.check() + admitted.charge("ik") + assert budget.counts["ik"] == 1 and budget.counts["candidates"] == 0 + cancellation.set() + with pytest.raises(FeasibilityBudgetExhausted, match="cancelled"): + admitted.check() + cancellation.clear() + with pytest.raises(FeasibilityBudgetExhausted, match="ik_budget"): + admitted.charge("ik", 2) + assert budget.counts["ik"] == 1 + + +def grids(): + return { + phase: { + "lower": [-1, -1, -math.pi / 2], + "upper": [1, 1, math.pi / 2], + "cell_size": [0.5, 0.5, math.pi / 2], + "yaw_period": math.pi, + } + for phase in ("pick", "place") + } + + +def coverage(): + cov = TaskPoseCoverage("domain", "object", grids()) + for name in ("scene-a", "scene-b"): + cov.register_case( + SceneCase(name, "initial", "signature", "task", "robot"), + torch.tensor([[-1.0, 1.0]]), + ("joint",), + None, + ) + return cov + + +def episode(commit="c1", case="scene-a", x=0.1, yaw=0.0, accepted=True): + state = torch.tensor( + [ + [x, 0.1, 0.02, 0, 0, math.sin(yaw / 2), math.cos(yaw / 2)], + [x + 0.1, 0.1, 0.02, 0, 0, math.sin(yaw / 2), math.cos(yaw / 2)], + ] + ) + return SimpleNamespace( + commit_id=commit, + identity=SimpleNamespace(scene_case_id=case), + validation=SimpleNamespace(accepted=accepted), + timestamps=torch.tensor([0.0, 0.1]), + observations={"rigid/object/state": state}, + ) + + +def test_receipts_aggregate_cases_without_double_credit_and_own_cells(): + cov = coverage() + e = episode() + assert cov.reserve(e) + assert cov.snapshot()[0] == {"domain": 0} + e.observations["rigid/object/state"].fill_(99) + cov.confirm("scene-a", "c1") + cov.confirm("scene-a", "c1") + assert cov.phase_counts() == {"pick": 1, "place": 1} + assert cov.snapshot()[1] == {} + assert cov.reserve(episode("c2", "scene-b", yaw=math.pi)) + cov.confirm("scene-b", "c2") + assert cov.snapshot()[0] == { + "domain": 2 + } # Symmetry and scene changes aren't novelty. + assert not cov.reserve(episode("bad", accepted=False)) + cov.reserve(episode("c3", x=-0.9)) + cov.release("scene-a", "c3") + with pytest.raises(ValueError): + cov.confirm("scene-a", "c3") + assert cov.snapshot()[0] == {"domain": 2} + + +def test_checkpoint_keeps_pending_unconfirmed_and_rejects_foreign_domain(): + cov = coverage() + cov.reserve(episode()) + cov.confirm("scene-a", "c1") + cov.reserve(episode("pending", x=-0.9)) + state = json.loads(json.dumps(cov.state_dict())) + restored = TaskPoseCoverage("domain", "object", grids()) + restored.load_state_dict(state) + assert restored.snapshot() == cov.snapshot() + restored.confirm("scene-a", "pending") + assert restored.snapshot()[0] == {"domain": 4} + assert cov.snapshot()[0] == {"domain": 2} + with pytest.raises(ValueError): + TaskPoseCoverage("another", "object", grids()).load_state_dict(state) + with pytest.raises(ValueError): + restored.confirm("scene-b", "pending") + + +def test_cells_boundaries_periodicity_and_measured_not_requested_pose(): + cov = coverage() + assert cov.cells([-1, -1, 0, 0, 0, 0, 1], [1, 1, 0, 0, 0, 0, 1]) == ( + ("pick", 0, 0, 1), + ("place", 3, 3, 1), + ) + assert cov.cells([-1.1, 0, 0, 0, 0, 0, 1], [1.1, 0, 0, 0, 0, 0, 1]) == () + with pytest.raises(ValueError): + cov.cells([0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1]) + e = episode(x=-0.9) + e.metadata = {"requested_pose": [0.9, 0.9, 0]} + cov.reserve(e) + cov.confirm("scene-a", "c1") + assert cov.gain((("pick", 0, 2, 1),)) == 0.5 + + +def test_named_boundaries_use_real_artifacts_and_fixed_categories(tmp_path): + from embodichain.lab.expansion import ArtifactEpisodeSink, load_expansion_episode + from embodichain.lab.expansion.contracts import ( + CandidateIdentity, + ExpertEpisode, + ValidationCheck, + ValidationResult, + ) + + measurements = { + "initial": {"entity": "object", "boundary": "initial"}, + "handover": { + "entity": "object", + "boundary": "segment_end", + "segment": "handover", + "fixed": True, + }, + "handover_pose": { + "entity": "object", + "boundary": "segment_end", + "segment": "handover", + }, + "held": {"entity": "object", "boundary": "terminal", "fixed": True}, + } + grid = grids()["pick"] + cov = TaskPoseCoverage( + "domain", + "object", + {name: grid for name in measurements}, + measurements=measurements, + ) + cov.register_case( + SceneCase("case", "initial", "signature", "task", "robot"), + torch.tensor([[-1.0, 1.0]]), + ("joint",), + None, + ) + sink = ArtifactEpisodeSink(tmp_path / "episodes") + for ordinal, x in enumerate((-0.7, 0.7)): + state = torch.tensor( + [ + [x, 0.1, 0.1, 0, 0, 0, 1], + [0.1, 0.1, 0.1, 0, 0, 0, 1], + [0.1 + ordinal * 0.6, 0.1, 0.1, 0, 0, 0, 1], + ] + ) + episode = ExpertEpisode( + CandidateIdentity( + "case", + "initial", + f"candidate{ordinal}", + "family", + "source", + "v1", + "plan", + ), + {"rigid/object/state": state}, + torch.zeros(2, 1), + torch.tensor([0.0, 0.1, 0.2]), + "qpos", + ValidationResult((ValidationCheck("task", "passed"),)), + f"episode{ordinal}", + f"commit{ordinal}", + metadata={ + "program_segments": [ + { + "name": "handover", + "start_step": 0, + "end_step": 1, + "success": True, + } + ] + }, + ) + assert cov.reserve(episode) + assert cov.phase_counts()["initial"] == ordinal + receipt = sink.submit(episode, submission_id=ordinal) + restored = load_expansion_episode(receipt.storage_id) + assert cov.reserve(restored) + cov.confirm("case", episode.commit_id) + cov.confirm("case", episode.commit_id) + assert cov.phase_counts() == { + "initial": 2, + "handover": 1, + "handover_pose": 1, + "held": 1, + } + assert ("handover_pose", 2, 2, 1) in cov.state_dict()["confirmed"]["commit1"][1] + assert cov.proposal_cells({"object": np.eye(4)}) == (("initial", 2, 2, 1),) + snapshot = cov.state_dict() + restored = TaskPoseCoverage( + "domain", + "object", + {name: grid for name in measurements}, + measurements=measurements, + ) + restored.load_state_dict(snapshot) + assert restored.phase_counts() == cov.phase_counts() + with pytest.raises(ValueError, match="segment name"): + TaskPoseCoverage( + "domain", + "object", + {"bad": grid}, + measurements={"bad": {"entity": "object", "boundary": "segment_end"}}, + ) + + +class _Robot: + control_parts = {"arm": [0, 1, 2]} + device = torch.device("cpu") + uid = "robot" + cfg = SimpleNamespace(to_dict=lambda: {"uid": "robot"}) + + def get_local_pose(self, to_matrix=False): + return torch.eye(4)[None] + + def get_qpos(self, **kwargs): + return torch.tensor([[0.0, 0.0, 0.02]]) + + def get_qpos_limits(self, **kwargs): + return torch.tensor([[[-1.0, 1.0], [-1.0, 1.0], [0.02, 0.02]]]) + + def compute_batch_fk(self, qpos, **kwargs): + pose = torch.eye(4).expand(*qpos.shape[:2], 4, 4).clone() + pose[..., :3, 3] = qpos + return pose + + def compute_ik(self, pose, **kwargs): + return torch.ones(len(pose), dtype=torch.bool), pose[:, :3, 3].clone() + + def compute_fk(self, qpos, **kwargs): + pose = torch.eye(4).repeat(len(qpos), 1, 1) + pose[:, :3, 3] = qpos + return pose + + +def environment(): + pose = torch.eye(4)[None] + pose[:, 2, 3] = 0.02 + obj = SimpleNamespace(get_local_pose=lambda to_matrix=True: pose.clone()) + return SimpleNamespace( + robot=_Robot(), + sim=SimpleNamespace( + get_rigid_object=lambda uid: obj, + sim_config=SimpleNamespace( + physics_cfg=SimpleNamespace(to_dict=lambda: {"physics": "default"}) + ), + ), + ) + + +def options(strategy="bounded_pose", policy="fifo"): + value = { + "engine": {"max_attempts": 8, "max_wall_time_s": 120}, + "validation": {}, + "scene": { + uid: { + "translation_m": [[-0.9, 0.9], [-0.9, 0.9], [0, 0]], + "yaw_rad": [0, 0], + } + for uid in ("object", "goal") + }, + "scene_sampling": { + "strategy": strategy, + "sample_count": 64, + "max_candidates": 64, + "targets": ( + {uid: np.eye(4).tolist() for uid in ("object", "goal")} + if strategy == "workspace_joint" + else {} + ), + }, + "scheduling": {"policy": policy, "pool_size": 8}, + "pose_coverage": { + "entity": "object", + "place_reference": "goal", + "grids": grids(), + }, + } + validate_sampling(value) + return value + + +def test_legacy_config_and_simple_fifo_keep_identical_poses(): + old = {"engine": {}, "scene": {}} + expected = deepcopy(old) + validate_sampling(old) + assert old == expected + env = environment() + opts = options() + simple = _BoundedPoseSource(env, opts["scene"], seed=7, revision="revision") + new = _AdaptivePoseSource(env, opts, seed=7, revision="revision") + for ordinal in range(4): + assert ( + new.candidate(ordinal).scene.pose_changes + == simple.candidate(ordinal).scene.pose_changes + ) + + +@pytest.mark.parametrize("strategy", ["bounded_pose", "workspace_joint"]) +@pytest.mark.parametrize("policy", ["fifo", "task_pose_coverage"]) +def test_candidate_is_frozen_after_feedback_and_source_is_seeded(strategy, policy): + opts = options(strategy, policy) + a = _AdaptivePoseSource(environment(), opts, seed=7, revision="revision") + b = _AdaptivePoseSource(environment(), opts, seed=7, revision="revision") + first = a.candidate(0) + assert first == b.candidate(0) + assert a.validate(0).accepted + a.finish(0, None, None, executed=True) + assert a.candidate(0) is first + assert a.budget.counts["candidates"] <= 64 + assert len(a._predicted) == 0 + + +def test_bounded_budget_never_bypasses_candidate_quota(): + a = _AdaptivePoseSource(environment(), options(), seed=7, revision="r") + a.budget.charge("candidates", 64) + with pytest.raises(FeasibilityBudgetExhausted): + a.budget.charge("candidates") + assert a.budget.counts["candidates"] == 64 + + +def test_invalid_joint_config_does_not_fall_back(): + opt = options() + opt["scene_sampling"]["strategy"] = "workspace_joint" + with pytest.raises(ValueError, match="target transforms"): + validate_sampling(opt) + + +def test_sampler_checkpoint_replays_choices_after_feedback(): + opts = options("workspace_joint", "task_pose_coverage") + a = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + first = a.candidate(0) + a.finish(0, None, None, executed=True) + state = json.loads(json.dumps(a.state_dict())) + b = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + b.load_state_dict(state) + assert b.candidate(0) == first + assert a.candidate(1) == b.candidate(1) + assert a.budget.counts == b.budget.counts + + +def test_relative_pose_validation_preserves_yaw_symmetry_and_tilt(): + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + actual = torch.eye(4).repeat(2, 1, 1) + target = actual.clone() + target[:, 2, 3] = -0.1 + actual[:, 2, 3] = 0.02 + actual[0, 0, 0] = actual[0, 1, 1] = -1 + actual[1, 0, 0] = actual[1, 1, 1] = 0 + actual[1, 0, 1] = -1 + actual[1, 1, 0] = 1 + + class Entity: + def __init__(self, poses): + self.poses = poses + + def get_local_pose(self, to_matrix=False): + if to_matrix: + return self.poses.clone() + return self.poses[:, :3, 3].clone() + + objects = {"object": Entity(actual), "target": Entity(target)} + env = SimpleNamespace(sim=SimpleNamespace(get_rigid_object=objects.get)) + condition = { + "kind": "pose_relative", + "entity": "object", + "other": "target", + "position_offset": [0, 0, 0.12], + "position_tolerance": [0.02, 0.02, 0.01], + "yaw_period_rad": math.pi, + "yaw_tolerance_rad": 0.18, + "tilt_tolerance_rad": 0.18, + } + monitor = MeasuredTaskMonitor([condition], [], env_id=0) + assert monitor.predicate(env, condition) + monitor.env_id = 1 + assert not monitor.predicate(env, condition) + actual[1] = actual[0] + actual[1, 2, 3] += 0.02 + assert not monitor.predicate(env, condition) + with pytest.raises(ValueError): + MeasuredTaskMonitor([{**condition, "yaw_period_rad": 0}], []) + + +def test_workspace_fifo_does_not_require_coverage_policy(): + opts = options("workspace_joint", "fifo") + del opts["pose_coverage"] + validate_sampling(opts) + source = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + assert source.coverage is None + source.candidate(0) + assert source.validate(0).accepted + source.finish(0, None, None, executed=False) + opts["scheduling"]["policy"] = "task_pose_coverage" + with pytest.raises(ValueError, match="grids"): + validate_sampling(opts) + + +def test_checkpoint_rejects_out_of_grid_cell_without_partial_restore(): + cov = coverage() + cov.reserve(episode()) + state = cov.state_dict() + state["pending"]["c1"] = ("scene-a", (("pick", 99, 0, 0),)) + restored = TaskPoseCoverage("domain", "object", grids()) + with pytest.raises(ValueError): + restored.load_state_dict(state) + assert not restored._cases and not restored._pending + + +def test_sampling_proposal_budget_counts_discarded_pool_candidates(): + opts = options("bounded_pose", "task_pose_coverage") + source = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + source.candidate(0) + assert source.budget.counts["candidates"] == 8 + assert len(source._candidates) == 1 + # Re-reading the chosen immutable candidate consumes no extra budget. + source.candidate(0) + assert source.budget.counts["candidates"] == 8 + + +def test_coverage_gain_uses_confirmed_pending_and_predicted_separately(): + cov = coverage() + e = episode() + cells = cov.cells( + e.observations["rigid/object/state"][0], + e.observations["rigid/object/state"][-1], + ) + assert cov.gain(cells) == 2 + cov.reserve(e) + assert cov.gain(cells) == 1 + cov.release("scene-a", "c1") + assert cov.gain(cells) == 2 + assert cov.gain(cells, {c: 1 for c in cells}) == 1 + + +def test_workspace_branch_must_match_execution_policy(): + source = _AdaptivePoseSource( + environment(), options("workspace_joint"), seed=7, revision="r" + ) + source.env.cfg = SimpleNamespace( + task_program=SimpleNamespace( + integration=SimpleNamespace(runtime_preset="trajectory") + ) + ) + pick = SimpleNamespace(fixed_object_to_eef=torch.eye(4)) + profile = SimpleNamespace( + presets={ + "trajectory": SimpleNamespace(skill_option_templates={"pick_up": pick}) + } + ) + adapter = SimpleNamespace( + _factory=SimpleNamespace(create_robot_skill_profile=lambda: profile) + ) + from embodichain.lab.task_program.semantics.calls import Pick + from embodichain.lab.task_program.semantics.scene import SceneObjectRef + + program = SimpleNamespace( + iter_segments=lambda: [ + SimpleNamespace( + calls=[SimpleNamespace(call=Pick(object=SceneObjectRef("object")))] + ) + ] + ) + source.bind_program(adapter, program) + pick.fixed_object_to_eef[0, 3] = 0.1 + with pytest.raises(ValueError, match="match"): + source.bind_program(adapter, program) + pick.fixed_object_to_eef = None + with pytest.raises(ValueError, match="fixed_object"): + source.bind_program(adapter, program) + + +def test_budget_expiry_before_first_command_has_no_fabricated_hold(): + from embodichain.lab.expansion._workload_cursor import _WorkloadCursor + + class Bridge: + def __iter__(self): + raise AssertionError("Expired planning must not enter the bridge") + yield + + class Budget: + def check(self): + raise FeasibilityBudgetExhausted("ik_budget_exhausted") + + cursor = _WorkloadCursor( + Bridge(), SimpleNamespace(device=torch.device("cpu")), 7, budget=Budget() + ) + assert cursor.next() is None + assert cursor.finished and not cursor.accepted and cursor.length == 0 + assert cursor.reason == "ik_budget_exhausted" and cursor.last_command is None + + +def test_coverage_rejects_nonphysical_yaw_period_and_identifies_branch_cut(): + from embodichain.lab.expansion._pose_cells import _grid + + with pytest.raises(ValueError, match="complete turn"): + _grid( + { + "lower": [-1, -1, -0.5], + "upper": [1, 1, 0.5], + "cell_size": [0.5, 0.5, 0.1], + "yaw_period": 1, + } + ) + cov = coverage() + a = [0, 0, 0, 0, 0, 1, 0] + b = [0, 0, 0, 0, 0, -1, 0] + assert cov.cells(a, a) == cov.cells(b, b) + + +def test_cache_configuration_is_used_and_empty_qualified_cache_fails(): + from unittest.mock import Mock + + env = environment() + opts = options("workspace_joint") + opts["scene_sampling"]["workspace_source"] = "cache" + cache = SimpleNamespace( + sample_indices=Mock(side_effect=ValueError("No samples meet min_score")) + ) + env.robot.cfg = SimpleNamespace( + to_dict=lambda: {"uid": "robot"}, + workspace_cfg={"arm": SimpleNamespace(strategy="point_uniform", min_score=0.8)}, + ) + env.robot.get_workspace = lambda part: cache + with pytest.raises(ValueError, match="min_score"): + _AdaptivePoseSource(env, opts, seed=7, revision="r") + assert cache.sample_indices.call_args.kwargs["strategy"] == "point_uniform" + assert cache.sample_indices.call_args.kwargs["min_score"] == 0.8 + + +def test_fifo_without_coverage_still_checks_grasp_correspondence(): + from embodichain.lab.task_program.semantics.calls import Pick + from embodichain.lab.task_program.semantics.scene import SceneObjectRef + + opts = options("workspace_joint") + del opts["pose_coverage"] + source = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + source.env.cfg = SimpleNamespace( + task_program=SimpleNamespace( + integration=SimpleNamespace(runtime_preset="trajectory") + ) + ) + fixed = torch.eye(4) + fixed[2, 3] = 0.2 + profile = SimpleNamespace( + presets={ + "trajectory": SimpleNamespace( + skill_option_templates={ + "pick_up": SimpleNamespace(fixed_object_to_eef=fixed) + } + ) + } + ) + adapter = SimpleNamespace( + _factory=SimpleNamespace(create_robot_skill_profile=lambda: profile) + ) + program = SimpleNamespace( + iter_segments=lambda: [ + SimpleNamespace( + calls=[SimpleNamespace(call=Pick(object=SceneObjectRef("object")))] + ) + ] + ) + with pytest.raises(ValueError, match="match"): + source.bind_program(adapter, program) + + +def test_transport_guard_remembers_a_drop_and_resets_per_episode(): + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + state = torch.tensor([[0.0, 0.0, 0.02]]) + goal = torch.tensor([[0.5, 0.0, 0.02]]) + objects = { + "object": SimpleNamespace(get_local_pose=lambda: state.clone()), + "goal": SimpleNamespace(get_local_pose=lambda: goal.clone()), + } + env = SimpleNamespace( + sim=SimpleNamespace( + get_rigid_object=objects.get, + get_rigid_object_uid_list=lambda: tuple(objects), + ) + ) + condition = { + "kind": "transport_guard", + "entity": "object", + "other": "goal", + "lift_height_m": 0.05, + "minimum_transit_height_m": 0.025, + "goal_tolerance_m": 0.03, + } + monitor = MeasuredTaskMonitor([condition], []) + monitor.reset(env) + assert not monitor.validation(env).accepted + state[0, 2] = 0.08 + monitor.observe(env, None) + assert monitor.validation(env).accepted + state[0, 2] = 0.02 + monitor.observe(env, None) + state[0] = goal[0] + monitor.observe(env, None) + assert not monitor.validation(env).accepted + state[0] = torch.tensor([0.0, 0.0, 0.02]) + monitor.reset(env) + state[0, 2] = 0.08 + monitor.observe(env, None) + state[0] = goal[0] + monitor.observe(env, None) + assert monitor.validation(env).accepted + + +def test_new_feedback_hook_preserves_legacy_positional_extension_constructor(): + from embodichain.lab.expansion.host_extensions import GymExpansionHostExtensions + + factory = lambda env, row: None + extensions = GymExpansionHostExtensions(None, None, None, None, factory, True) + assert ( + extensions.require_plan_transform + and extensions.plan_transform_factory is factory + ) + assert extensions.feedback_factory is None + + +def test_bounded_source_owns_input_ranges(): + cfg = options()["scene"] + source = _BoundedPoseSource(environment(), cfg, seed=7, revision="r") + expected = source.candidate(3) + cfg["object"]["translation_m"][0][:] = [0.5, 0.5] + assert source.candidate(3) == expected + + +def test_transport_guard_rejects_nonfinite_measured_state(): + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + state = torch.tensor([[0.0, 0.0, 0.02]]) + goal = torch.tensor([[0.5, 0.0, 0.02]]) + objects = { + "object": SimpleNamespace(get_local_pose=lambda: state.clone()), + "goal": SimpleNamespace(get_local_pose=lambda: goal.clone()), + } + env = SimpleNamespace( + sim=SimpleNamespace( + get_rigid_object=objects.get, + get_rigid_object_uid_list=lambda: tuple(objects), + ) + ) + condition = { + "kind": "transport_guard", + "entity": "object", + "other": "goal", + "lift_height_m": 0.05, + "minimum_transit_height_m": 0.025, + "goal_tolerance_m": 0.03, + } + monitor = MeasuredTaskMonitor([condition], []) + monitor.reset(env) + state[0, 2] = 0.08 + monitor.observe(env, None) + assert monitor.validation(env).accepted + state[0, 2] = float("nan") + monitor.observe(env, None) + assert not monitor.validation(env).accepted diff --git a/tests/lab/expansion/test_workloads_sim.py b/tests/lab/expansion/test_workloads_sim.py new file mode 100644 index 0000000000..95b79fc8a7 --- /dev/null +++ b/tests/lab/expansion/test_workloads_sim.py @@ -0,0 +1,853 @@ +# ---------------------------------------------------------------------------- +# 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 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import os +from pathlib import Path +import subprocess +import sys + +import pytest + + +def _probe(output: str) -> None: + import hashlib + import json + import torch + from embodichain.lab.expansion import ( + ArtifactEpisodeSink, + ExpansionEngine, + ExpansionJobCfg, + ExpansionSession, + ExpansionSessionCfg, + EpisodeCandidate, + PreparedEpisodeWorkItem, + ExpansionWorkloadCfg, + WorkloadExecutionCfg, + WorkloadProgramExecutor, + load_expansion_episode, + ) + from embodichain.lab.expansion.contracts import ( + ExpertEpisode, + SceneCase, + ValidationCheck, + ValidationResult, + ) + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.utils.utility import load_config + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + discover_task_packages() + execute_init_hooks() + cfg = config_to_cfg(load_config(path), source_path=path) + cfg.num_envs = cfg.sim_cfg.num_envs = 4 + cfg.sim_cfg.device = "cpu" + cfg.sim_cfg.headless = True + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sensor, cfg.enable_sensor, cfg.dataset = [], False, None + cfg.init_rollout_buffer = cfg.record_trajectory = False + cfg.task_program.program.count = 1 + env = None + try: + env = REGISTERED_ENVS[load_config(path)["id"]].make(cfg=cfg) + env.reset(seed=0) + robot = env.robot + program = env.compile_task_program(cfg.task_program) + from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenCfg, + MotionGenOptions, + ) + from embodichain.lab.sim.motion.planners.trapezoidal_planner import ( + TrapezoidalPlannerCfg, + ) + from embodichain.lab.sim.motion.planners.utils import PlanState + from embodichain.lab.expansion._planning_batches import batched_planning + + requests = {} + for row in (0, 3): + generator = MotionGenerator( + MotionGenCfg( + planner_cfg=TrapezoidalPlannerCfg( + robot_uid=robot.uid, sim_instance_id=env.sim.instance_id + ) + ), + env_ids=(row,), + ) + start = robot.get_qpos(name="arm")[row : row + 1].clone() + goal = start.clone() + goal[:, 0] += 0.01 * (row + 1) + states = [PlanState.from_qpos(goal)] + options = MotionGenOptions( + strategy="ik_interp", + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + sample_count=12, + interpolation_dt=0.04, + start_qpos=start, + control_part="arm", + ) + requests[row] = (generator, states, options) + reference = { + row: generator.generate(states, options) + for row, (generator, states, options) in requests.items() + } + + class Request: + def __init__(self, row): + self.row = row + + def next(self): + generator, states, options = requests[self.row] + return generator.generate(states, options) + + before = env.sim.physics_step_index + with batched_planning(robot, 0) as planning: + planned = planning.gather({row: Request(row) for row in requests}) + assert ( + planning.stats["motion_calls"] == 1 + and planning.stats["max_motion_group"] == 2 + ), planning.stats + for row, original in reference.items(): + torch.testing.assert_close( + planned[row].success, original.success, rtol=0, atol=0 + ) + torch.testing.assert_close( + planned[row].positions, original.positions, rtol=0, atol=1e-6 + ) + torch.testing.assert_close(planned[row].dt, original.dt, rtol=0, atol=0) + assert env.sim.physics_step_index == before + from embodichain.lab.expansion.collision import _MeasuredCollision + + checks = _MeasuredCollision( + env, + str(root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml"), + Path(output) / "checks", + ) + + class Host: + capacity = env.num_envs + + def preflight(self, items): + assert all(item.compatibility_key == "one-world" for item in items) + + def validate_plan(self, item): + assert torch.isfinite(robot.get_qpos()[item.env_id]).all() + return ValidationResult( + ( + ValidationCheck( + "initial_geometry", + "passed" if checks.initial[item.env_id] else "failed", + ), + ) + ) + + sink = ArtifactEpisodeSink(output) + session = ExpansionSession( + ExpansionSessionCfg.from_mapping( + { + "collection": { + "target_committed_episodes": 4, + "max_proposals": 8, + "max_rollout_attempts": 8, + }, + "execution": {"ready_high_watermark": 4, "ready_low_watermark": 0}, + "persistence": { + "pending_episode_limit": 4, + "pending_max_bytes": 32 * 1024 * 1024, + }, + } + ) + ) + engine = ExpansionEngine( + ExpansionJobCfg(batch_size=4), session=session, host=Host(), sink=sink + ) + + class Source: + def __init__(self): + self.buffers, self.assignments, self.adapters = {}, {}, [] + self.finished = [] + + def prepare(self, assignments): + assert len(SimulationManager._instances) == 1 + self.reset_holds = getattr(self, "reset_holds", {}) + for assignment in assignments: + self.reset_holds[assignment.env_id] = robot.get_qpos(target=True)[ + assignment.env_id + ].clone() + + def activate(self, assignment, observation): + row = assignment.env_id + # The physical settling ticks must keep the authored drive + # target, rather than repeatedly holding gravity-displaced qpos. + torch.testing.assert_close( + robot.get_qpos(target=True)[row], + self.reset_holds[row], + rtol=0, + atol=0, + ) + checks.prepare(env_ids=(row,)) + from copy import deepcopy + + self.scalar_monitors = getattr(self, "scalar_monitors", {}) + self.scalar_monitors[row] = deepcopy(checks.monitors[row]) + key = f"{assignment.workload_id}-{assignment.ordinal}" + state = robot.get_qpos()[row].detach().cpu().clone() + initial = hashlib.sha256(state.numpy().tobytes()).hexdigest() + session.register_case( + SceneCase(key, initial, "scene", program.program_id, robot.uid), + robot.get_qpos_limits()[row].cpu(), + joint_names=tuple(robot.joint_names), + ) + identity, _ = session.propose( + key, + initial, + source_id=program.program_id, + source_revision="v1", + template_id=key, + operator_id="episode", + ) + item = PreparedEpisodeWorkItem( + EpisodeCandidate(identity, "{}"), "one-world", row + ) + native = engine.admit_row(item) + assert native is not None + self.assignments[row] = native + self.buffers[row] = {"actions": [], "states": [state]} + adapter = env.task_program_adapter.for_env_ids((row,)) + self.adapters.append(adapter) + return adapter.create_bridge(program, isolate_policy_sessions=True) + + def started(self, assignment, boundary): + engine.mark_row_started(self.assignments[assignment.env_id]) + + def observe(self, assignments, action, observation): + rejected = [] + checks.sample(tuple(a.env_id for a in assignments), action) + for assignment in assignments: + scalar = self.scalar_monitors[assignment.env_id] + scalar.observe(env, action) + assert scalar.validation(env) == checks.terminal[assignment.env_id] + for assignment in assignments: + row = assignment.env_id + buffer = self.buffers[row] + values = ( + action.value + if isinstance(action.value, torch.Tensor) + else action.value["qpos"] + ) + buffer["actions"].append(values[row].detach().cpu().clone()) + buffer["states"].append( + robot.get_qpos()[row].detach().cpu().clone() + ) + if ( + assignment.workload_id == "first" + and assignment.ordinal == 0 + and len(buffer["actions"]) == 12 + ): + rejected.append(row) + return rejected + + def complete(self, result): + row = result.assignment.env_id + native = self.assignments.pop(row) + data = self.buffers.pop(row) + count = result.program.lengths[0] + assert ( + len(data["actions"]) == count and len(data["states"]) == count + 1 + ) + assert ( + result.world_control_end - result.world_control_start + 1 == count + ) + self.finished.append( + ( + result.assignment.workload_id, + result.assignment.ordinal, + count, + result.program.accepted[0], + ) + ) + if not result.program.accepted[0]: + return engine.complete_row(native, None) + episode = ExpertEpisode( + native.item.candidate.identity, + {"joint_positions": torch.stack(data["states"])}, + torch.stack(data["actions"]), + torch.arange(count + 1, dtype=torch.float64) * env.step_dt, + "qpos", + ValidationResult( + ( + ValidationCheck("task_success", "passed"), + ValidationCheck( + "path_collision", checks.results[row].status + ), + *checks.terminal[row].checks, + ) + ), + native.episode_id, + native.commit_id, + ) + return engine.complete_row(native, episode) + + source = Source() + owner = id(env.sim.get_world()) + done = [] + executor = WorkloadProgramExecutor( + env, + [ + ExpansionWorkloadCfg("first", 2, 4), + ExpansionWorkloadCfg("second", 1, 2), + ExpansionWorkloadCfg("queued", 1, 2), + ], + source, + WorkloadExecutionCfg( + workload_count=2, batch_size=2, max_steps=2500, settling_steps=5 + ), + on_workload_done=lambda name, counts: done.append((name, counts)), + ) + result = executor.run() + assert result["complete"], (result, source.finished) + assert result["num_envs"] == 4 and result["refills"] > 0 + assert result["counts"]["first"] == { + "attempts": 3, + "rollouts": 3, + "committed": 2, + } + assert len(done) == 3 and len(sink.inspect()) == 4 + assert session.snapshot()["counts"]["committed"] == 4 + assert ( + id(env.sim.get_world()) == owner and len(SimulationManager._instances) == 1 + ) + assert next(entry for entry in source.finished if entry[:2] == ("first", 0))[ + 2: + ] == (13, False) + for receipt in sink.inspect(): + episode = load_expansion_episode(receipt.storage_id) + assert len(episode.timestamps) == len(episode.actions) + 1 + assert episode.validation.accepted + with pytest.raises(RuntimeError, match="single-use"): + executor.run() + for adapter in source.adapters: + adapter.close() + checks.close() + (Path(output) / "workload-result.json").write_text(json.dumps(result, indent=2)) + finally: + if env is not None: + env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +def _facade_probe(output: str) -> None: + from copy import deepcopy + import hashlib + import json + import torch + import yaml + from embodichain.lab.expansion import ( + ArtifactEpisodeSink, + ExpansionRunCfg, + GymExpansionHostExtensions, + GymExpansionWorkload, + WorkloadExecutionCfg, + run_gym_expansion_workloads, + load_expansion_episode, + ) + from embodichain.lab.expansion.collision import _MeasuredCollision + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + task = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place" + ) + deployment = task / "task.ur5.yaml" + path = Path(output) + path.mkdir(exist_ok=True) + program = yaml.safe_load((task / "task_program/program.yaml").read_text()) + program["program"]["repeat"]["count"] = 1 + override = path / "program.yaml" + override.write_text(yaml.safe_dump(program)) + + def absolute_paths(value, base): + if isinstance(value, dict): + return {key: absolute_paths(item, base) for key, item in value.items()} + if isinstance(value, list): + return [absolute_paths(item, base) for item in value] + if isinstance(value, str) and value.endswith((".yaml", ".yml", ".json")): + return str((base / value).resolve()) + return value + + raw = absolute_paths(yaml.safe_load(deployment.read_text()), deployment.parent) + component = Path(raw["embodiment"]["component"]) + embodiment = absolute_paths(yaml.safe_load(component.read_text()), component.parent) + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + + base_cfg = config_to_cfg( + yaml.safe_load(deployment.read_text()), source_path=deployment + ) + contact = deepcopy( + base_cfg.robot.gripper_bindings["eef"]["defaults"]["root_from_contact_frame"] + ) + contact[0][3] += 0.001 + embodiment["grippers"]["eef"]["root_from_contact_frame"] = contact + component_override = path / "embodiment.yaml" + component_override.write_text(yaml.safe_dump(embodiment)) + raw["id"] = "TaskProgramRepeatedPickPlace-ContactFrame-v1" + raw["embodiment"]["component"] = str(component_override) + policy_deployment = path / "contact-frame.yaml" + policy_deployment.write_text(yaml.safe_dump(raw)) + config_to_cfg(raw, source_path=policy_deployment) + discover_task_packages() + execute_init_hooks() + workloads = [] + for name, target in (("pair", 2), ("queued", 1)): + selected_deployment = deployment if name == "pair" else policy_deployment + digest = hashlib.sha256(selected_deployment.read_bytes()).hexdigest() + cfg = ExpansionRunCfg( + deployment=str(selected_deployment), + program=str(override), + output_dir=str(path / name), + target_episodes=target, + max_attempts=4, + max_steps=2500, + max_wall_time_s=240, + settling_steps=5, + ) + report = { + "job_id": name, + "semantic_id": name, + "execution_id": name, + "recipe_revision": name, + "input_files": {"deployment": digest}, + "runtime_versions": {}, + } + workloads.append(GymExpansionWorkload(name, cfg, report, {})) + + observed_bindings = [] + + def validator(env, output): + binding = deepcopy(env.robot.cfg.gripper_bindings) + observed_bindings.append((env.robot.cfg, binding)) + factory = REGISTERED_ENVS[raw["id"]].task_program_adapter_factory + with pytest.raises(ValueError, match="actual hand installation"): + factory.create_adapter(env) + assert env.robot.cfg.gripper_bindings == binding + return _MeasuredCollision( + env, + str(root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml"), + output, + ) + + result = run_gym_expansion_workloads( + workloads, + WorkloadExecutionCfg( + workload_count=1, batch_size=2, max_steps=2500, settling_steps=5 + ), + extensions=GymExpansionHostExtensions(validator_factory=validator), + output_dir=path / "execution", + ) + assert result["complete"], result + assert result["num_envs"] == 2 and result["world"]["world_count"] == 1 + assert not SimulationManager._instances + assert all(cfg.gripper_bindings == binding for cfg, binding in observed_bindings) + for name, expected in (("pair", 2), ("queued", 1)): + group = result["workloads"][name] + assert group["complete"] and group["committed_episodes"] == expected + receipts = ArtifactEpisodeSink(path / name).inspect() + assert len(receipts) == len(group["lerobot"]["datasets"]) == expected + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + assert episode.validation.accepted + assert len(episode.timestamps) == len(episode.actions) + 1 + execution = episode.metadata["execution"] + assert execution["world_control_end"] - execution[ + "world_control_start" + ] + 1 == len(episode.actions) + assert execution["world"] == result["world"] + (path / "facade-result.json").write_text(json.dumps(result, indent=2)) + # A real drive change remains incompatible even though contact conventions + # above are allowed. Reject before constructing another World. + embodiment["simulation"]["joint_drive_props"]["stiffness"][ + "gripper_finger1_joint_1" + ] += 1 + hardware_component = path / "different-hardware.yaml" + hardware_component.write_text(yaml.safe_dump(embodiment)) + raw["id"] += "-Hardware" + raw["embodiment"]["component"] = str(hardware_component) + hardware_deployment = path / "different-hardware-task.yaml" + hardware_deployment.write_text(yaml.safe_dump(raw)) + rejected = GymExpansionWorkload( + "different", + ExpansionRunCfg( + **{ + **workloads[-1].cfg.to_dict(), + "deployment": str(hardware_deployment), + "output_dir": str(path / "different"), + } + ), + { + **workloads[-1].report, + "input_files": { + "deployment": hashlib.sha256( + hardware_deployment.read_bytes() + ).hexdigest() + }, + }, + {}, + ) + with pytest.raises(ValueError, match="physical environment"): + run_gym_expansion_workloads( + [workloads[0], rejected], + WorkloadExecutionCfg( + workload_count=1, batch_size=2, max_steps=2500, settling_steps=5 + ), + output_dir=path / "rejected", + ) + assert not SimulationManager._instances + + # Expiry during settling retires only the unstarted group. A peer must + # still collect/export a real receipt, and the aggregate stays incomplete. + from dataclasses import replace + + budget_groups = [ + replace( + workloads[0], + workload_id=name, + cfg=ExpansionRunCfg( + **{ + **workloads[0].cfg.to_dict(), + "output_dir": str(path / name), + "target_episodes": 1, + "max_attempts": 1, + "max_wall_time_s": budget, + } + ), + ) + for name, budget in (("expired", 1e-9), ("peer", 240.0)) + ] + budget_result = run_gym_expansion_workloads( + budget_groups, + WorkloadExecutionCfg( + workload_count=2, batch_size=1, max_steps=2500, settling_steps=5 + ), + extensions=GymExpansionHostExtensions( + validator_factory=lambda env, output: _MeasuredCollision( + env, + str( + root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml" + ), + output, + ) + ), + output_dir=path / "budget-execution", + ) + assert not budget_result["complete"] + assert budget_result["counts"]["expired"] == { + "attempts": 1, + "rollouts": 0, + "committed": 0, + } + assert budget_result["workloads"]["peer"]["complete"] + assert len(ArtifactEpisodeSink(path / "peer").inspect()) == 1 + assert len(budget_result["workloads"]["peer"]["lerobot"]["datasets"]) == 1 + assert not ArtifactEpisodeSink(path / "expired").inspect() + records = list((path / "expired" / "attempts").glob("*/attempt.json")) + assert len(records) == 1 + expired_record = json.loads(records[0].read_text()) + assert expired_record["status"] == "initial_rejected" + assert expired_record["reason"] == "wall_time_exhausted" + assert not SimulationManager._instances + + +def _reuse_probe(output: str) -> None: + """A real rejected installation opens reuse, then only one receipt wins.""" + import json + import numpy as np + import torch + from embodichain.lab.expansion import ( + ArtifactEpisodeSink, + ExpansionRunCfg, + GymExpansionHostExtensions, + GymExpansionWorkload, + WorkloadExecutionCfg, + run_gym_expansion_workloads, + load_expansion_episode, + ) + from embodichain.lab.expansion.collision import _MeasuredCollision + from embodichain.lab.sim.scene.expansion import SceneVariant + from embodichain.lab.sim.scene.feasibility import ( + JointSceneCandidate, + RobotDeploymentCandidate, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.lab.expansion._planning_batches import PlanningBatches + from embodichain.utils.utility import load_config + import yaml + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + task = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place" + ) + path = Path(output) + path.mkdir(exist_ok=True) + raw = load_config(task / "task.ur5.yaml") + + def absolute(value, base): + if isinstance(value, dict): + return {k: absolute(v, base) for k, v in value.items()} + if isinstance(value, list): + return [absolute(v, base) for v in value] + if isinstance(value, str) and value.endswith((".yaml", ".yml", ".json")): + return str((base / value).resolve()) + return value + + raw = absolute(raw, task) + program = yaml.safe_load((task / "task_program/program.yaml").read_text()) + program["program"]["repeat"]["count"] = 1 + program_path = path / "program.yaml" + program_path.write_text(yaml.safe_dump(program)) + raw["task_program"]["program"] = str(program_path) + deployment = path / "deployment.yaml" + deployment.write_text(yaml.safe_dump(raw)) + + def scenes(env): + nominal = env.robot.get_local_pose(to_matrix=True)[0].cpu().tolist() + + class Source: + def candidate(self, ordinal): + pose = np.array(nominal) + if ordinal == 0: + pose[2, 3] -= 0.6 + elif ordinal == 2: + pose[0, 3] += 0.02 + return JointSceneCandidate( + "retry", + SceneVariant("retry", 2, ordinal, ()), + RobotDeploymentCandidate({env.robot.uid: pose.tolist()}), + "installation", + (), + ) + + return Source() + + cfg = ExpansionRunCfg( + deployment=str(deployment), + output_dir=str(path / "group"), + batch_size=1, + target_episodes=1, + max_attempts=6, + max_steps=2500, + max_wall_time_s=180, + settling_steps=5, + seed=2, + ) + workload = GymExpansionWorkload( + "retry", + cfg, + { + "job_id": "retry", + "semantic_id": "retry", + "execution_id": "retry", + "recipe_revision": "v1", + "input_files": {}, + "runtime_versions": {}, + }, + {}, + GymExpansionHostExtensions(scene_source_factory=scenes), + ) + original_gather = PlanningBatches.gather + boundary = {"injected": False, "executed": False} + + def gather(broker, cursors): + actions = original_gather(broker, cursors) + if not boundary["injected"] and any( + c.finished and c.accepted for c in cursors.values() + ): + for row, cursor in cursors.items(): + if row not in actions or cursor.finished: + continue + # Exercise the real host-stop path before quota completion; + # the controlled administrative stop is not fake physics. + cursor.abort("test_stop_before_peer_quota") + hold = cursor.next() + assert hold is not None + actions[row] = hold + boundary.update( + injected=True, + row=row, + cursor=cursor, + clock=cursor.bridge.clock.step_index, + hold=hold, + ) + break + return actions + + def validator(env, output): + original_step = env.step + + def step(action, *args, **kwargs): + pending = boundary["injected"] and not boundary["executed"] + if pending: + assert boundary["cursor"].bridge.clock.step_index == boundary["clock"] + actual = ( + action.value + if isinstance(action.value, torch.Tensor) + else action.value["qpos"] + ) + expected = boundary["hold"].value + expected = ( + expected if isinstance(expected, torch.Tensor) else expected["qpos"] + ) + torch.testing.assert_close( + actual[boundary["row"]], expected[0], rtol=0, atol=0 + ) + result = original_step(action, *args, **kwargs) + if pending: + boundary["executed"] = True + return result + + env.step = step + return _MeasuredCollision( + env, + str(root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml"), + output, + ) + + PlanningBatches.gather = gather + try: + result = run_gym_expansion_workloads( + (workload,), + WorkloadExecutionCfg( + workload_count=1, + batch_size=2, + max_steps=2500, + settling_steps=5, + seed=2, + reuse_idle_rows=True, + ), + extensions=GymExpansionHostExtensions(validator_factory=validator), + output_dir=path / "execution", + ) + finally: + PlanningBatches.gather = original_gather + assert boundary["injected"] and boundary["executed"] + assert result["complete"] and result["reused_rows"] > 0, result + assert result["counts"]["retry"]["committed"] == 1 + assert result["cancelled"]["retry"] > 0 + assert result["num_envs"] == 2 and result["world"]["world_count"] == 1 + receipts = ArtifactEpisodeSink(path / "group").inspect() + assert len(receipts) == 1 + episode = load_expansion_episode(receipts[0].storage_id) + assert ( + episode.validation.accepted + and len(episode.timestamps) == len(episode.actions) + 1 + ) + records = [ + json.loads(p.read_text()) + for p in (path / "group/attempts").glob("*/attempt.json") + ] + assert next(r for r in records if r["ordinal"] == 0)["status"] in { + "initial_rejected", + "rejected", + } + cancelled = [ + p + for p in (path / "group/attempts").glob("*/attempt.json") + if json.loads(p.read_text())["status"] == "cancelled" + ] + assert cancelled + for entry in cancelled: + record = json.loads(entry.read_text()) + assert record["reason"] == "quota_fulfilled_by_peer" + if "partial_rollout" in record: + assert record["env_id"] in (0, 1) + if record["steps"]: + assert record["last_controller_metadata"].get("bridge_action_kind") + with np.load(entry.parent / record["partial_rollout"]["path"]) as data: + assert len(data["qpos"]) == record["steps"] + 1 + assert len(data.get("actions", [])) == record["steps"] + assert not SimulationManager._instances + + +def _declarative_probe(output: str) -> None: + import json + from embodichain.lab.expansion import ( + run_task_program_expansion, + ArtifactEpisodeSink, + load_expansion_episode, + ) + from embodichain.lab.sim import SimulationManager + + root = Path(__file__).resolve().parents[3] + result = run_task_program_expansion( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml", + Path(__file__).parent / "fixtures/task_program_job.yaml", + output, + ) + assert result["complete"], result + execution = json.loads((Path(output) / "workload-execution.json").read_text()) + assert execution["world"]["world_count"] == 1 and execution["num_envs"] == 2 + assert not SimulationManager._instances + for operator, group in result["operators"].items(): + receipts = ArtifactEpisodeSink(group["directory"]).inspect() + assert len(receipts) == len(group["lerobot"]["datasets"]) == 1 + episode = load_expansion_episode(receipts[0].storage_id) + assert episode.validation.accepted + assert len(episode.timestamps) == len(episode.actions) + 1 + if operator == "joint_residual": + assert episode.metadata["expansion"] + + +@pytest.mark.requires_sim +@pytest.mark.parametrize("mode", ["executor", "facade", "declarative", "reuse"]) +def test_rolling_workloads_keep_native_receipts_and_world_boundaries(tmp_path, mode): + result = subprocess.run( + [sys.executable, __file__, str(tmp_path), mode], + cwd=Path(__file__).resolve().parents[3], + env={**os.environ, "SDL_VIDEODRIVER": "offscreen", "OMP_NUM_THREADS": "1"}, + capture_output=True, + text=True, + timeout=300, + ) + assert result.returncode == 0, result.stdout[-4000:] + result.stderr[-5000:] + + +if __name__ == "__main__": + { + "executor": _probe, + "facade": _facade_probe, + "declarative": _declarative_probe, + "reuse": _reuse_probe, + }[sys.argv[2] if len(sys.argv) > 2 else "executor"](sys.argv[1]) diff --git a/tests/lab/task_program/test_semantic_compiler.py b/tests/lab/task_program/test_semantic_compiler.py index 3f36af3c17..786919f89d 100644 --- a/tests/lab/task_program/test_semantic_compiler.py +++ b/tests/lab/task_program/test_semantic_compiler.py @@ -2617,3 +2617,30 @@ def test_occurrence_policy_rejects_unknown_preset_and_assurance_changes( (Pick(object=SceneObjectRef("cube")),), preset_ids=(preset_id,) ) assert all(provider.calls == 0 for provider in providers) + + +def test_place_grounds_scene_relative_pose_from_current_observation() -> None: + from embodichain.lab.task_program.semantics.calls import SemanticRelativePose + + registry, (_, table) = _scene_registry() + compiler, _ = _compiler(registry) + pick = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = compiler.ground(pick, 0, _context(registry)).invocation.goal.semantics + object_to_eef = torch.eye(4).repeat(2, 1, 1) + object_to_eef[:, 2, 3] = 0.12 + target = SemanticRelativePose("table", (0.1, 0.0, 0.02), (0, 0, 0, 1)) + workflow = compiler.analyze((Place(object=SceneObjectRef("cube"), at=target),)) + first = compiler.ground( + workflow, 0, _held_context(registry, semantics, object_to_eef) + ) + expected = table.pose.clone() + expected[:, 0, 3] += 0.1 + expected[:, 2, 3] += 0.14 + torch.testing.assert_close(first.invocation.goal.xpos, expected) + table.pose[:, 1, 3] += 0.2 + second = compiler.ground( + workflow, 0, _held_context(registry, semantics, object_to_eef) + ) + expected[:, 1, 3] += 0.2 + torch.testing.assert_close(second.invocation.goal.xpos, expected) + assert not torch.equal(first.invocation.goal.xpos, second.invocation.goal.xpos) diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index c2d3391ad9..fc3defc09c 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -677,8 +677,9 @@ def get_link_pose( env_ids: list[int] | None = None, to_matrix: bool = True, ) -> torch.Tensor: - del env_ids, to_matrix - pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + del to_matrix + count = NUM_ENVS if env_ids is None else len(env_ids) + pose = torch.eye(4).repeat(count, 1, 1) pose[:, 0, 3] = -1.0 if link_name == "left_root" else 1.0 return pose @@ -745,8 +746,15 @@ def compute_fk( qpos: torch.Tensor | None = None, name: str | None = None, to_matrix: bool = True, + env_ids: list[int] | torch.Tensor | None = None, ) -> torch.Tensor: - count = NUM_ENVS if qpos is None else qpos.shape[0] + count = ( + (NUM_ENVS if env_ids is None else len(env_ids)) + if qpos is None + else qpos.shape[0] + ) + if env_ids is not None: + assert len(env_ids) == count return torch.eye(4).repeat(count, 1, 1) robot.get_qpos.side_effect = get_qpos @@ -2339,6 +2347,58 @@ def generate(states, options=None): ) +@pytest.mark.parametrize("cartesian_lift", [False, True]) +def test_pick_phase_paths_and_cartesian_lift_require_explicit_choice( + cartesian_lift, tmp_path +): + generator = _motion_generator() + # Fixed-grid planning retains the real authored-velocity-limit check. + urdf = tmp_path / "arm.urdf" + urdf.write_text( + '' + + "".join(f'' for i in range(ROBOT_DOF + 1)) + + "".join( + f'' + f'' + '' + for i in range(ROBOT_DOF) + ) + + "" + ) + generator.robot.cfg.fpath = str(urdf) + generator.generate = Mock(wraps=generator.generate) + action = _bind_action(generator, PickUp()) + pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + invocation = SkillInvocation( + skill_id="pick_up", + goal=GraspGoal(semantics=_semantics(entity_id="target"), grasp_xpos=pose), + binding=_binding(action), + motion_policy=MotionPolicy( + strategy="ik_interp", + sample_count=20, + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + acceleration_limit=None, + phase_paths={"pre_grasp": "cartesian"}, + ), + skill_options=PickUpOptions(cartesian_lift=cartesian_lift, lift_height=0.1), + ) + context = _context(scene=_target_scene(pose, timestamp=0.0, version=0)) + if cartesian_lift: + with pytest.raises(ValueError, match="cartesian_lift and phase_paths"): + _plan_action(action, invocation, context) + generator.generate.assert_not_called() + else: + _plan_action(action, invocation, context) + assert generator.generate.call_args_list[0].kwargs[ + "options" + ].waypoint_modes == ( + "cartesian", + "joint", + "joint", + ) + + def test_pick_holds_fully_closed_before_lifting_when_configured() -> None: generator = _motion_generator() generator.generate = Mock(wraps=generator.generate) diff --git a/tests/sim/atomic_skills/test_core.py b/tests/sim/atomic_skills/test_core.py index 26cc228128..7c8f36ebc2 100644 --- a/tests/sim/atomic_skills/test_core.py +++ b/tests/sim/atomic_skills/test_core.py @@ -1039,8 +1039,12 @@ def test_public_plan_authorizes_raw_action_plan_destinations() -> None: action.plan(request, context) -def test_command_target_authorization_rejects_altered_joint_claims() -> None: +@pytest.mark.parametrize( + "device", ["cpu", pytest.param("cuda:0", marks=pytest.mark.gpu)] +) +def test_command_target_authorization_rejects_altered_joint_claims(device: str) -> None: context = _context() + env_ids = context.env_ids.to(device) request = ResolvedSkillRequest( skill_id="dependency_test", goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), @@ -1065,20 +1069,42 @@ def test_command_target_authorization_rejects_altered_joint_claims() -> None: commands=( EndpointCommand( target=JointPositionTarget("arm", (2, 3)), - payload=JointPositionPayload(torch.ones(2, 2)), + payload=JointPositionPayload(torch.ones(2, 2, device=device)), ), ), - active_mask=torch.ones(2, dtype=torch.bool), - env_ids=context.env_ids, - hold_duration=torch.full((2,), 0.1), + active_mask=torch.ones(2, dtype=torch.bool, device=device), + env_ids=env_ids, + hold_duration=torch.full((2,), 0.1, device=device), ) with pytest.raises(ValueError, match="bound joint-position target"): _DependencyAction._authorize_command_targets( request, - TimedCommandSequence(frames=(frame,), env_ids=context.env_ids), + TimedCommandSequence(frames=(frame,), env_ids=env_ids), ) + commands = _command_sequence(env_ids=env_ids, frame_count=2) + owned = _DependencyAction._authorize_command_targets(request, commands) + commands.frames[0].commands[0].payload.positions.fill_(99) + torch.testing.assert_close( + owned.frames[0].commands[0].payload.positions, torch.ones(2, 2, device=device) + ) + owned.frames[0].commands[0].payload.positions.fill_(55) + torch.testing.assert_close( + owned.frames[1].commands[0].payload.positions, + torch.full((2, 2), 2.0, device=device), + ) + owned.frames[0].commands[0].payload.positions[0, 0] = float("nan") + with pytest.raises(ValueError, match="finite"): + owned.snapshot() + commands.frames[1].commands[0].payload.positions[0, 0] = float("nan") + with pytest.raises(ValueError, match="finite"): + _DependencyAction._authorize_command_targets(request, commands) + commands.frames[1].commands[0].payload.positions.fill_(2) + commands.frames[0].hold_duration.fill_(-1) + with pytest.raises(ValueError, match="non-negative"): + _DependencyAction._authorize_command_targets(request, commands) + def test_command_target_authorization_rejects_custom_claim_conflicts() -> None: context = _context() diff --git a/tests/sim/atomic_skills/test_runtime_commands.py b/tests/sim/atomic_skills/test_runtime_commands.py index 6bb50b4768..45c3ed4b95 100644 --- a/tests/sim/atomic_skills/test_runtime_commands.py +++ b/tests/sim/atomic_skills/test_runtime_commands.py @@ -141,6 +141,40 @@ def _frame( ) +@pytest.mark.gpu +def test_cuda_sequence_snapshots_keep_ownership_and_revalidate_mutated_values() -> None: + target = JointPositionTarget("arm", (0, 1)) + ids = torch.tensor([1, 5], device="cuda:0") + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target, + JointPositionPayload( + torch.full((2, 2), 0.1 * (i + 1), device="cuda:0") + ), + ), + ), + active_mask=torch.tensor([True, False], device="cuda:0"), + env_ids=ids, + hold_duration=torch.full((2,), 0.04, device="cuda:0"), + ) + for i in range(3) + ) + original = TimedCommandSequence(frames, ids) + copied = original.snapshot() + original.frames[0].commands[0].payload.positions.fill_(99) + assert copied.frames[0].commands[0].payload.positions[0, 0] == 0.1 + copied.frames[0].commands[0].payload.positions.fill_(55) + assert copied.frames[1].commands[0].payload.positions[0, 0] == 0.2 + original.frames[0].commands[0].payload.positions[0, 0] = float("nan") + with pytest.raises(ValueError, match="finite"): + original.snapshot() + copied.frames[0].hold_duration[0] = -0.1 + with pytest.raises(ValueError, match="non-negative"): + copied.snapshot() + + def test_runtime_command_payload_is_abstract() -> None: with pytest.raises(TypeError): RuntimeCommandPayload() # type: ignore[abstract] @@ -377,3 +411,32 @@ def test_timed_command_sequence_requires_nonempty_int64_batch() -> None: TimedCommandSequence(frames=(), env_ids=torch.tensor([0.0])) with pytest.raises(ValueError, match="unique"): TimedCommandSequence(frames=(), env_ids=torch.tensor([2, 2])) + + +def test_joint_target_snapshot_revalidates_and_preserves_custom_ownership() -> None: + original = JointPositionTarget("arm", (0, 1)) + copied = original.snapshot() + assert copied == original and copied is not original + object.__setattr__(original, "joint_ids", (0, 0)) + with pytest.raises(ValueError, match="unique"): + original.snapshot() + + @dataclass(frozen=True, slots=True) + class ExtendedTarget(JointPositionTarget): + metadata: list[int] + + custom = ExtendedTarget("arm", (0, 1), [3]) + snapshot = custom.snapshot() + assert type(snapshot) is ExtendedTarget + snapshot.metadata[0] = 7 + assert custom.metadata == [3] + + class AnnotatedName(str): + pass + + name = AnnotatedName("arm") + name.annotations = [1] + named = JointPositionTarget(name, (0, 1)) + named_copy = named.snapshot() + named_copy.control_part.annotations.append(2) + assert named.control_part.annotations == [1] diff --git a/tests/sim/motion/_cartesian_segment_fixture.py b/tests/sim/motion/_cartesian_segment_fixture.py index e448a779b3..9360717571 100644 --- a/tests/sim/motion/_cartesian_segment_fixture.py +++ b/tests/sim/motion/_cartesian_segment_fixture.py @@ -27,7 +27,7 @@ def segment_fixture(case): calls = [] - robot = NS(device=torch.device("cpu")) + robot = NS(device=torch.device("cpu"), get_solver=lambda name=None: None) def fk(qpos, **kwargs): result = torch.eye(4).expand(qpos.shape[0], 4, 4).clone() diff --git a/tests/sim/motion/expansion/test_operators.py b/tests/sim/motion/expansion/test_operators.py index 814593d003..838c4d6e5f 100644 --- a/tests/sim/motion/expansion/test_operators.py +++ b/tests/sim/motion/expansion/test_operators.py @@ -616,3 +616,97 @@ def test_approach_cone_rejects_malformed_inputs() -> None: polar_angles=torch.tensor([0.0]), azimuth_angles=torch.tensor([0.0]), ) + + +@pytest.mark.parametrize("fraction, expected", [(-1.0, 0.05), (0.0, 0.25), (1.0, 0.45)]) +def test_explicit_residual_draws_reach_bounds_and_preserve_protected_rows( + fraction, expected +): + source = template() + result = joint_residual( + source, + joint_limits=torch.tensor([[-1.0, 1.0], [-1.0, 1.0]]), + normalized_scale=0.1, + generator=torch.Generator().manual_seed(17), + joint_samples={"arm": fraction}, + ) + assert float(result.positions[1, 0]) == pytest.approx(expected, abs=1e-6) + torch.testing.assert_close( + result.positions[[0, 2, 3, 4, 5]], source.positions[[0, 2, 3, 4, 5]] + ) + torch.testing.assert_close(result.positions[:, 1], source.positions[:, 1]) + torch.testing.assert_close(source.positions, template().positions) + + +def test_explicit_draws_do_not_bypass_joint_limits_and_require_named_controls(): + limits = torch.tensor([[-1.0, 1.0], [-1.0, 1.0]]) + with pytest.raises(ProposalRejected): + joint_residual( + template(), + joint_limits=limits, + normalized_scale=1.0, + generator=torch.Generator(), + joint_samples={"arm": 1.0}, + ) + for samples in ( + {"missing": 0.0}, + {"arm": float("nan")}, + {"arm": True}, + {"arm": 1.1}, + [], + ): + with pytest.raises(ValueError): + joint_residual( + template(), + joint_limits=limits, + normalized_scale=0.1, + generator=torch.Generator(), + joint_samples=samples, + ) + + +def test_explicit_via_knots_and_configured_payload_are_owned(): + from embodichain.lab.sim.motion.expansion.cfg import TrajectoryAugmentationCfg + + source = template() + source = replace( + source, + allowed_operators=("via_points",), + phases=( + replace(source.phases[0], allowed_operators=("via_points",)), + source.phases[1], + ), + ) + samples = {"arm": -1.0} + cfg = TrajectoryAugmentationCfg.from_mapping( + { + "factors": { + "spatial": { + "enabled": True, + "method": ["via_points"], + "joint_samples": samples, + "knot_samples": [1.0], + } + } + } + ) + samples["arm"] = 1.0 + result = via_points( + source, + joint_limits=torch.tensor([[-1.0, 1.0], [-1.0, 1.0]]), + via_count=1, + normalized_scale=0.1, + generator=torch.Generator(), + joint_samples=cfg.factors.spatial.joint_samples, + knot_samples=cfg.factors.spatial.knot_samples, + ) + assert float(result.positions[1, 0]) == pytest.approx(0.05, abs=1e-6) + + +def test_explicit_sample_mapping_rejects_nonfinite_config_values(): + from embodichain.lab.sim.motion.expansion.cfg import TrajectoryAugmentationCfg + + with pytest.raises(ValueError, match="finite"): + TrajectoryAugmentationCfg.from_mapping( + {"factors": {"spatial": {"joint_samples": {"arm": float("nan")}}}} + ) diff --git a/tests/sim/motion/solvers/test_base_solver.py b/tests/sim/motion/solvers/test_base_solver.py index c5f260493e..745cfde5f4 100644 --- a/tests/sim/motion/solvers/test_base_solver.py +++ b/tests/sim/motion/solvers/test_base_solver.py @@ -45,6 +45,38 @@ } +@pytest.mark.parametrize( + "device", ["cpu", pytest.param("cuda:0", marks=pytest.mark.gpu)] +) +def test_tcp_offset_jacobian_uses_the_solver_device(tmp_path, device): + path = tmp_path / "one_joint.urdf" + path.write_text(""" + + + + + + + + """) + solver = PytorchSolverCfg( + urdf_path=str(path), + root_link_name="base", + end_link_name="tool", + joint_names=["joint"], + ).init_solver(device=device) + expected = torch.tensor( + [[[-0.2], [1.0], [0.0], [0.0], [0.0], [1.0]]], device=device + ) + for locations in ( + torch.tensor([0.0, 0.2, 0.0]), + torch.tensor([[0.0, 0.2, 0.0]], device=device), + ): + actual = solver.get_jacobian(torch.zeros(1, 1), locations=locations) + torch.testing.assert_close(actual, expected, rtol=0, atol=1e-6) + assert actual.device.type == torch.device(device).type + + @pytest.mark.parametrize("dtype", [np.float16, np.float32, np.float64]) def test_nearest_weight_numpy_dtypes_update_tensor_in_place(dtype) -> None: solver = object.__new__(OPWSolver) diff --git a/tests/sim/motion/test_explicit_interpolation.py b/tests/sim/motion/test_explicit_interpolation.py index 807b26271a..f32bfa2436 100644 --- a/tests/sim/motion/test_explicit_interpolation.py +++ b/tests/sim/motion/test_explicit_interpolation.py @@ -77,6 +77,7 @@ def solve(*, pose, joint_seed, name): get_qpos_limits=lambda **kwargs: torch.tensor([[[-1.0, 1.0]]]).expand( batch, -1, -1 ), + get_solver=lambda name=None: None, compute_ik=solve, ) return generator, seeds diff --git a/tests/sim/motion/test_motion_generator_batched.py b/tests/sim/motion/test_motion_generator_batched.py index c4886a743d..e1bd0a66d1 100644 --- a/tests/sim/motion/test_motion_generator_batched.py +++ b/tests/sim/motion/test_motion_generator_batched.py @@ -2371,3 +2371,58 @@ def test_cartesian_timing_budget_exhaustion_returns_failed_safe_hold(monkeypatch ) assert success.tolist() == [False] assert torch.equal(positions, q[:, :1].repeat(1, 2, 1)) + + +@pytest.mark.parametrize("modes", [("joint",), ("joint", "cartesian")]) +def test_phase_paths_materialize_owned_velocity_and_hold_failed_rows( + modes, monkeypatch +): + generator = _mock_generator() + start = torch.zeros(BATCH_SIZE, CONTROLLED_DOF) + states = [PlanState.from_xpos(torch.eye(4).repeat(BATCH_SIZE, 1, 1)) for _ in modes] + pieces = [] + + def segment(_self, targets, options=None, *, defer_recovery=False): + positions = ( + options.start_qpos[:, None] + + torch.arange(options.sample_count, dtype=torch.float32)[ + None, :, None + ].expand(BATCH_SIZE, -1, CONTROLLED_DOF) + * 0.25 + ) + result = _timed_result( + positions, success=torch.tensor([True, False]), step_dt=0.25 + ) + pieces.append(result) + return result + + # Exercise the public composition and real normalization with deterministic + # segment outputs, independent of IK convergence or simulation resources. + original = MotionGenerator.generate + monkeypatch.setattr(MotionGenerator, "generate", segment) + result = original( + generator, + states, + MotionGenOptions( + strategy="ik_interp", + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + waypoint_modes=modes, + sample_count=5, + interpolation_dt=0.25, + start_qpos=start, + control_part="arm", + ), + ) + expected = torch.ones_like(result.positions) + expected[:, 0] = expected[:, -1] = 0 + expected[1] = 0 + torch.testing.assert_close(result.velocities, expected) + torch.testing.assert_close( + result.positions[1], torch.zeros_like(result.positions[1]) + ) + assert result.positions.shape[1] == 5 + assert result.dt.sum(dim=1).tolist() == [1.0, 1.0] + saved = result.positions.clone() + pieces[0].positions.fill_(99) + torch.testing.assert_close(result.positions, saved) diff --git a/tests/sim/objects/test_articulation_state.py b/tests/sim/objects/test_articulation_state.py index 3aeb6e6311..1fcbbad5b9 100644 --- a/tests/sim/objects/test_articulation_state.py +++ b/tests/sim/objects/test_articulation_state.py @@ -40,7 +40,10 @@ def test_combined_state_position_limits(clamp_targets: bool, clamp_qpos: bool) - qpos_limits=torch.tensor([[[-1.0, 1.0], [-2.0, 2.0]]] * 3), articulation_view=SimpleNamespace(apply_state=Mock()), ) - art._world = SimpleNamespace(update=Mock()) + art._world = SimpleNamespace( + update=Mock(), + get_physics_scene=lambda: SimpleNamespace(is_direct_gpu=lambda: False), + ) art._stabilize_newton_mimic_target_write = Mock() art.set_state( env_ids=[2, 0], @@ -89,8 +92,9 @@ def test_combined_state_position_limits(clamp_targets: bool, clamp_qpos: bool) - @pytest.mark.parametrize( "field", ["qpos", "target_qpos", "qvel", "target_qvel", "root_pose"] ) +@pytest.mark.parametrize("direct_gpu", [False, True]) def test_cpu_transform_publication_only_for_direct_pose_writes( - newton: bool, field: str + newton: bool, field: str, direct_gpu: bool ) -> None: art = object.__new__(Articulation) art.device = torch.device("cpu") @@ -101,13 +105,16 @@ def test_cpu_transform_publication_only_for_direct_pose_writes( qpos_limits=torch.tensor([[[-1.0, 1.0], [-2.0, 2.0]]]), articulation_view=SimpleNamespace(apply_state=Mock()), ) - art._world = SimpleNamespace(update=Mock()) + art._world = SimpleNamespace( + update=Mock(), + get_physics_scene=lambda: SimpleNamespace(is_direct_gpu=lambda: direct_gpu), + ) art._stabilize_newton_mimic_target_write = Mock() value = torch.tensor([[0.1, 0.2]]) if field == "root_pose": value = torch.tensor([[0.1, 0.2, 0.3, 0.0, 0.0, 0.0, 1.0]]) art.set_state(**{field: value}) - if not newton and field in {"qpos", "root_pose"}: + if not newton and not direct_gpu and field in {"qpos", "root_pose"}: art._world.update.assert_called_once_with(0.0) else: art._world.update.assert_not_called() diff --git a/tests/sim/objects/test_scene_backend.py b/tests/sim/objects/test_scene_backend.py index ede7d95d4e..52697d00b4 100644 --- a/tests/sim/objects/test_scene_backend.py +++ b/tests/sim/objects/test_scene_backend.py @@ -1400,3 +1400,105 @@ class RebuiltScene: rigid.get_friction([0]) assert factory.call_count == 2 assert factory.call_args.args[0] is rebuilt + + +def test_numeric_boundary_owns_samples_and_invalidates_writes_and_revisions() -> None: + from contextvars import copy_context + from embodichain.lab.sim.objects.backends._numeric_observations import ( + numeric_observation_scope, + shared_pose, + ) + from embodichain.lab.sim.objects.backends.scene import _checked_batch_call + + class Batch: + def __init__(self): + self.value = torch.ones(2, 3) + self.reads = 0 + + def fetch_joint_position(self, out): + self.reads += 1 + out.copy_(self.value) + return 2 + + def apply_joint_position(self, value): + self.value.copy_(value) + return 2 + + def apply_failed(self): + self.value.fill_(9) + raise RuntimeError("partial native write") + + batch = Batch() + sim = SimpleNamespace( + physics_backend="default", + device=torch.device("cpu"), + physics_step_index=0, + _spawn_scene=SimpleNamespace(topology_revision=0), + get_rigid_object_uid_list=lambda: (), + ) + robot = SimpleNamespace( + _data=SimpleNamespace( + articulation_view=SimpleNamespace(batch=batch, scene=sim._spawn_scene) + ) + ) + + def read(): + out = torch.empty(2, 3) + assert _checked_batch_call(batch, "fetch_joint_position", out) == 2 + return out + + with numeric_observation_scope(sim, robot): + escaped = copy_context() + first = read() + first.fill_(77) + torch.testing.assert_close(copy_context().run(read), torch.ones(2, 3)) + assert batch.reads == 1 + derived_calls = [] + + def derive(): + derived_calls.append(True) + return batch.value @ torch.eye(3) + 10 + + derived = shared_pose(batch, True, derive) + derived.fill_(88) + torch.testing.assert_close( + shared_pose(batch, True, derive), torch.full((2, 3), 11.0) + ) + assert len(derived_calls) == 1 + torch.testing.assert_close( + shared_pose(batch, False, lambda: batch.value.clone()), torch.ones(2, 3) + ) + _checked_batch_call(batch, "apply_joint_position", torch.full((2, 3), 2.0)) + torch.testing.assert_close(read(), torch.full((2, 3), 2.0)) + assert batch.reads == 2 + torch.testing.assert_close( + shared_pose(batch, True, derive), torch.full((2, 3), 12.0) + ) + assert len(derived_calls) == 2 + with torch.autocast("cpu", dtype=torch.bfloat16): + torch.testing.assert_close( + shared_pose(batch, True, derive), + torch.full((2, 3), 12.0, dtype=torch.bfloat16), + ) + assert len(derived_calls) == 3 + sim.physics_step_index += 1 + batch.value.fill_(3) + assert read()[0, 0] == 3 and batch.reads == 3 + sim._spawn_scene.topology_revision += 1 + batch.value.fill_(4) + assert read()[0, 0] == 4 and batch.reads == 4 + with pytest.raises(RuntimeError, match="partial native"): + _checked_batch_call(batch, "apply_failed") + assert read()[0, 0] == 9 and batch.reads == 5 + with numeric_observation_scope(sim, robot): + _checked_batch_call(batch, "apply_joint_position", torch.full((2, 3), 6.0)) + assert read()[0, 0] == 6 + assert read()[0, 0] == 6 and batch.reads == 7 + # Same physics tick, new boundary: reset/prepare writes are never reused. + batch.value.fill_(8) + with numeric_observation_scope(sim, robot): + assert read()[0, 0] == 8 and batch.reads == 8 + batch.value.fill_(10) + assert escaped.run(read)[0, 0] == 10 and batch.reads == 9 + batch.value.fill_(11) + assert escaped.run(read)[0, 0] == 11 and batch.reads == 10 diff --git a/tests/sim/scene/expansion/test_conditions.py b/tests/sim/scene/expansion/test_conditions.py new file mode 100644 index 0000000000..2496b2bb2a --- /dev/null +++ b/tests/sim/scene/expansion/test_conditions.py @@ -0,0 +1,238 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Anchored layout restrictions preserve geometry, ownership and addressed RNG.""" + +from __future__ import annotations +from dataclasses import replace +import numpy as np +import pytest + +from embodichain.lab.sim.scene.expansion import ( + SceneLayoutCondition, + prepare_scene_randomization, + generate_scene_variants, + save_scene_variants, + load_scene_variants, +) +from scene_layout_support import make_source, make_policy + + +def _poses(variant): + return {c.entity_uid: np.asarray(c.arena_pose) for c in variant.pose_changes} + + +def test_condition_owns_anchors_and_leaves_prepared_and_other_samples_independent(): + prepared = prepare_scene_randomization(make_source(), make_policy()) + anchor = np.array(prepared.source.poses["tray"]) + anchor[0, 3] += 0.1 + condition = SceneLayoutCondition({"tray": anchor}) + anchor[0, 3] = 99 + original_policy = prepared.policy + result = generate_scene_variants( + prepared, conditions=condition, seed=7, candidate_count=4 + ) + assert result.status == "completed" + for variant in result.variants: + np.testing.assert_allclose( + _poses(variant)["tray"], condition.poses["tray"], rtol=0, atol=1e-12 + ) + assert not np.allclose( + _poses(result.variants[0])["mug"], _poses(result.variants[-1])["mug"] + ) + assert prepared.policy == original_policy + projection = condition.to_dict() + projection["tray"][0][3] = 77 + assert condition.to_dict()["tray"][0][3] == pytest.approx(-0.2) + with pytest.raises(TypeError): + condition.poses["tray"] = np.eye(4) + + +def test_member_anchor_requires_pinned_ancestors_and_preserves_full_pose(): + prepared = prepare_scene_randomization(make_source(), make_policy()) + first = generate_scene_variants(prepared, seed=9, candidate_count=1).variants[0] + poses = _poses(first) + with pytest.raises(ValueError, match="ancestor"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"mug": poses["mug"]}) + ) + condition = SceneLayoutCondition(poses) + batch = generate_scene_variants( + prepared, conditions=condition, candidate_count=2, max_attempts=10 + ) + assert batch.status == "completed" + for variant in batch.variants: + for uid, pose in poses.items(): + np.testing.assert_allclose(_poses(variant)[uid], pose, rtol=0, atol=1e-10) + + +@pytest.mark.parametrize("size", [1, 7, 64]) +def test_condition_prefix_continuation_and_persistence(size, tmp_path): + source = make_source() + condition = SceneLayoutCondition({"tray": source.poses["tray"]}) + prepared = prepare_scene_randomization(source, make_policy(batch_size=size)) + one = generate_scene_variants( + prepared, conditions=condition, seed=13, candidate_count=3 + ) + two = generate_scene_variants( + prepared, + conditions=condition, + seed=13, + candidate_count=2, + start_ordinal=one.next_ordinal, + ) + together = generate_scene_variants( + prepared, conditions=condition, seed=13, candidate_count=5 + ) + assert tuple(one.variants) + tuple(two.variants) == tuple(together.variants) + reference = generate_scene_variants( + prepare_scene_randomization(source, make_policy(batch_size=1)), + conditions=condition, + seed=13, + candidate_count=5, + ) + assert tuple(reference.variants) == tuple(together.variants) + saved = save_scene_variants(together, tmp_path / "layouts.json") + loaded = load_scene_variants(saved) + assert tuple(loaded.variants) == tuple(together.variants) + + +def test_bad_anchor_rejects_without_relaxing_domain_or_permissions(): + source = make_source() + prepared = prepare_scene_randomization( + source, + make_policy(group_xy_offset_bounds_m={"tray": [[-0.1, -0.1], [0.1, 0.1]]}), + ) + outside = np.array(source.poses["tray"]) + outside[0, 3] += 0.2 + with pytest.raises(ValueError, match="bounds"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"tray": outside}) + ) + tilted = np.array(source.poses["tray"]) + tilted[0, 0] = -1 + tilted[1, 1] = -1 + with pytest.raises(ValueError, match="rotation"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"tray": tilted}) + ) + fixed = np.array(source.poses["z_target"]) + fixed[0, 3] += 0.01 + with pytest.raises(ValueError, match="fixed"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"z_target": fixed}) + ) + with pytest.raises(ValueError, match="Unknown"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"missing": np.eye(4)}) + ) + + +def test_geometrically_impossible_anchor_returns_exact_exhaustion_without_repair(): + source = make_source() + prepared = prepare_scene_randomization(source, make_policy()) + overlap = np.array(source.poses["tray"]) + overlap[:2, 3] = np.array(source.poses["z_target"])[:2, 3] + result = generate_scene_variants( + prepared, + conditions=SceneLayoutCondition({"tray": overlap}), + candidate_count=1, + max_attempts=9, + ) + assert result.status == "budget_exhausted" and not result.variants + assert result.attempts == 9 and result.next_ordinal == 9 + + +def test_anchor_reserves_room_before_sampling_an_earlier_sibling(): + source = make_source() + prepared = prepare_scene_randomization( + source, + make_policy( + group_targets=["tray", "z_target"], + intra_group={}, + collision_margin_m=0.02, + group_xy_offset_bounds_m={ + "tray": [[0, -0.04], [0.35, 0.04]], + "z_target": [[-0.8, -0.4], [0.4, 0.4]], + }, + ), + ) + target = np.array(source.poses["z_target"]) + target[:2, 3] = [-0.3, 0.0] + result = generate_scene_variants( + prepared, + conditions=SceneLayoutCondition({"z_target": target}), + seed=11, + candidate_count=8, + max_attempts=8, + ) + assert result.status == "completed" and len(result.variants) == 8 + for variant in result.variants: + poses = _poses(variant) + np.testing.assert_allclose(poses["z_target"], target, rtol=0, atol=1e-10) + # Their Y ranges always overlap. Clearance is possible only in X: + # 0.15 m tray half-width + 0.05 m block half-width + 0.02 m margin. + assert poses["tray"][0, 3] - target[0, 3] >= 0.22 - 1e-10 + + +def test_no_condition_and_empty_condition_preserve_legacy_exact_sequence(): + prepared = prepare_scene_randomization(make_source(), make_policy()) + a = generate_scene_variants(prepared, seed=41, candidate_count=4) + b = generate_scene_variants( + prepared, seed=41, candidate_count=4, conditions=SceneLayoutCondition({}) + ) + assert tuple(a.variants) == tuple(b.variants) and a.attempts == b.attempts + + +@pytest.mark.gpu +def test_cuda_anchors_keep_cpu_geometry_certificate_and_selected_pose(): + import torch + + if not torch.cuda.is_available(): + pytest.skip("CUDA unavailable") + source = make_source() + condition = SceneLayoutCondition({"tray": source.poses["tray"]}) + prepared = prepare_scene_randomization( + source, make_policy(device="cuda:0", batch_size=7) + ) + result = generate_scene_variants( + prepared, conditions=condition, seed=13, candidate_count=3 + ) + assert result.status == "completed" + for candidate in result.variants: + np.testing.assert_allclose( + _poses(candidate)["tray"], source.poses["tray"], rtol=0, atol=1e-10 + ) + + +def test_member_anchor_under_fixed_parent_and_invalid_height_are_explicit(): + prepared = prepare_scene_randomization( + make_source(), make_policy(randomize_groups=False) + ) + initial = generate_scene_variants(prepared, seed=19, candidate_count=1).variants[0] + pose = _poses(initial)["mug"] + result = generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"mug": pose}), candidate_count=1 + ) + assert result.status == "completed" + np.testing.assert_allclose( + _poses(result.variants[0])["mug"], pose, rtol=0, atol=1e-10 + ) + pose[2, 3] += 0.01 + with pytest.raises(ValueError, match="height"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"mug": pose}) + ) diff --git a/tests/sim/scene/feasibility/test_collision_planning.py b/tests/sim/scene/feasibility/test_collision_planning.py index 899164d3aa..12b028b1ee 100644 --- a/tests/sim/scene/feasibility/test_collision_planning.py +++ b/tests/sim/scene/feasibility/test_collision_planning.py @@ -69,6 +69,67 @@ def pose(x): checker.check({"left": pose(0)}) +@pytest.mark.parametrize( + "device", ["cpu", pytest.param("cuda:0", marks=pytest.mark.gpu)] +) +def test_batched_geometry_matches_scalar_rows_and_rebinds_permissions(device): + bodies = ( + CollisionBody("arm", "arm", "box", (0.2, 0.2, 0.2), robot=True), + CollisionBody("payload", "payload", "sphere", (0.1,)), + CollisionBody("obstacle", "obstacle", "box", (0.2, 0.2, 0.2)), + CollisionBody("ground", "ground", "halfspace", (0, 0, 1, 0)), + ) + frames = { + name: np.repeat(np.eye(4)[None], 3, axis=0) + for name in ("arm", "payload", "obstacle", "ground") + } + for name, x in (("arm", 0.0), ("payload", 1.0), ("obstacle", 2.0)): + frames[name][:, 0, 3] = x + frames[name][:, 2, 3] = 0.5 + frames["payload"][1, 0, 3] = 0.1 + frames["payload"][2, 2, 3] = -0.1 + checker = NewtonCollisionChecker(bodies, device=device) + try: + for allowed in ((), (("arm", "payload"),)): + expected, pairs = [], [] + for row in range(3): + report, _ = checker.check( + {key: value[row] for key, value in frames.items()}, + allowed_contacts=allowed, + moving_objects=("payload",), + ) + expected.append(report.status) + pairs.append(checker.last_pair) + result = checker.check_batch( + frames, allowed_contacts=allowed, moving_objects=("payload",) + ) + assert [report.status for report in result] == expected + assert checker.last_batch_pairs == tuple(pairs) + selected = checker.check_batch( + frames, + allowed_contacts=allowed, + moving_objects=("payload",), + env_ids=(2, 0), + ) + assert [report.status for report in selected] == [expected[2], expected[0]] + assert expected == ["passed", "passed", "failed"] + # Changing permissions cannot reuse the previous mask, and invalid row + # IDs or malformed physical transforms fail before querying geometry. + assert ( + checker.check_batch(frames, moving_objects=("payload",))[1].status + == "failed" + ) + with pytest.raises(ValueError, match="unique"): + checker.check_batch(frames, env_ids=(1, 1)) + assert checker.last_batch_pairs == () + malformed = {key: value.copy() for key, value in frames.items()} + malformed["arm"][0, 3, 0] = 1 + with pytest.raises(ValueError, match="rigid"): + checker.check_batch(malformed) + finally: + checker.close() + + def test_phase_branches_must_connect_and_dual_targets_stay_synchronized(): identity = np.eye(4).tolist() mode = TaskOperationMode( diff --git a/tests/sim/scene/test_state.py b/tests/sim/scene/test_state.py index a6a7ccebc9..9f0368f5f7 100644 --- a/tests/sim/scene/test_state.py +++ b/tests/sim/scene/test_state.py @@ -175,7 +175,10 @@ def test_boundary_positions_round_trip_through_production_articulation_setter( articulation._resolve_env_ids = lambda ids: torch.tensor([0]) articulation._resolve_joint_ids = lambda ids: torch.tensor([0]) articulation._stabilize_newton_mimic_target_write = lambda *args, **kwargs: None - articulation._world = SimpleNamespace(update=lambda dt: None) + articulation._world = SimpleNamespace( + update=lambda dt: None, + get_physics_scene=lambda: SimpleNamespace(is_direct_gpu=lambda: False), + ) articulation.body_data.is_newton_backend = False port_write = articulation.set_state articulation.body_data.articulation_view = SimpleNamespace( diff --git a/tests/sim/sensors/test_contact_default_e2e.py b/tests/sim/sensors/test_contact_default_e2e.py index 348c44394e..591ceedd6f 100644 --- a/tests/sim/sensors/test_contact_default_e2e.py +++ b/tests/sim/sensors/test_contact_default_e2e.py @@ -19,6 +19,7 @@ import pytest import warp as wp +import torch from embodichain.lab.sim.cfg import ( DefaultPhysicsCfg, @@ -26,7 +27,7 @@ RigidBodyPhysicsCfg, RigidObjectCfg, ) -from embodichain.lab.sim.sensors import ContactSensorCfg +from embodichain.lab.sim.sensors import ContactSensorCfg, ContactPermissionChecker from embodichain.lab.sim.shapes import CubeCfg from embodichain.lab.sim.sim_manager import SimulationManager, SimulationManagerCfg @@ -110,6 +111,51 @@ def test_default_contact_sensor_preserves_contact_multiplicity( sensor.get_actor_info(actor_id).path.endswith("/cube") for actor_id in cube_actor_ids ) + assert sim.is_use_gpu_physics == device.startswith("cuda") + assert sim.get_world().get_physics_scene().is_direct_gpu() == ( + device.startswith("cuda") + ) + if device == "cpu": + maps = [{} for _ in range(2)] + for uid in ("cube", "ground", "default_plane"): + values = sensor.get_actor_ids(uid)[:, 0].cpu().tolist() + for row, actor in enumerate(values): + maps[row][actor] = uid + checker = ContactPermissionChecker( + sensor, + maps, + allowed_contacts=(("cube", "ground"),), + moving_frames=("cube",), + ) + accepted = checker.check() + assert accepted[:, 0].cpu().tolist() == [0, 0] + refused = ContactPermissionChecker( + sensor, maps, allowed_contacts=(), moving_frames=("cube",) + ) + assert refused.check()[:, 0].cpu().tolist() == [3, 3] + assert accepted[:, 0].cpu().tolist() == [0, 0] + if device.startswith("cuda"): + other = torch.cuda.Stream() + with torch.cuda.stream(other): + repeated = checker.check() + other.synchronize() + assert repeated[:, 0].cpu().tolist() == [0, 0] + bounded = sim.add_sensor( + ContactSensorCfg( + uid="bounded", + rigid_uid_list=["cube"], + filter_need_both_actor=False, + max_contacts_per_env=1, + ) + ) + bounded.update() + overflow = ContactPermissionChecker( + bounded, + maps, + allowed_contacts=(("cube", "ground"),), + moving_frames=("cube",), + ) + assert overflow.check()[:, 0].cpu().tolist() == [4, 4] finally: sim.destroy(exit_process=False) SimulationManager.flush_cleanup_queue() diff --git a/tests/sim/sensors/test_contact_sensor_graph.py b/tests/sim/sensors/test_contact_sensor_graph.py index 2241692cbb..297039b4e0 100644 --- a/tests/sim/sensors/test_contact_sensor_graph.py +++ b/tests/sim/sensors/test_contact_sensor_graph.py @@ -23,7 +23,11 @@ import warp as wp from dexsim.scene import ContactActorInfo, ContactBuffer, ContactQueryCapabilities -from embodichain.lab.sim.sensors import ContactSensor, ContactSensorCfg +from embodichain.lab.sim.sensors import ( + ContactPermissionChecker, + ContactSensor, + ContactSensorCfg, +) pytestmark = [pytest.mark.gpu, pytest.mark.no_sim] @@ -209,8 +213,14 @@ def assert_air_time(value: float) -> None: @pytest.mark.parametrize("history_sample", [False, True]) -def test_cached_sample_is_ready_on_another_cuda_stream(history_sample: bool) -> None: - """A cached update must wait for the producer's outstanding device work.""" +@pytest.mark.parametrize( + "dropped_count", + [pytest.param(0, id="forbidden-contact"), pytest.param(3, id="dropped-evidence")], +) +def test_cached_sample_is_ready_on_another_cuda_stream( + history_sample: bool, dropped_count: int +) -> None: + """Permission checks and cached updates wait for the pending sensor sample.""" wp.init() device = torch.device("cuda:0") query = _CudaQuery(device) @@ -238,6 +248,12 @@ def test_cached_sample_is_ready_on_another_cuda_stream(history_sample: bool) -> if history_sample else None ) + checker = ContactPermissionChecker( + sensor, + ({0: "ground", 10: "cube"}, {0: "ground", 20: "cube"}), + allowed_contacts=(), + moving_frames=("cube",), + ) def sample() -> None: if history_sample: @@ -246,7 +262,10 @@ def sample() -> None: sensor.update() sample() # Compile kernels before arranging the delayed producer. + checker.check() sensor.reset() + query.buffer.device_counts[0] = 0 if dropped_count else 2 + query.buffer.device_counts[1] = dropped_count torch.cuda.synchronize(device) owner.physics_step_index += 1 producer = torch.cuda.Stream(device=device) @@ -255,14 +274,21 @@ def sample() -> None: torch.cuda._sleep(200_000_000) sample() with torch.cuda.stream(consumer): + # Check before a cached update can independently wait for the producer. + permissions = checker.check() sample() observed = sensor.get_data()["is_valid"].clone() force = history.force.clone() if history is not None else None consumer.synchronize() - assert observed.tolist() == [[True, False], [True, False]] + assert permissions.tolist() == ( + [[4, 0, 0], [4, 0, 0]] if dropped_count else [[3, 0, 10], [3, 0, 20]] + ) + assert observed.tolist() == [[not dropped_count, False]] * 2 if force is not None: torch.testing.assert_close( force, - torch.tensor([[[0.0, 0.0, 2.0]], [[0.0, 0.0, 2.0]]], device=device), + torch.tensor( + [[[0.0, 0.0, 0.0 if dropped_count else 2.0]]] * 2, device=device + ), ) producer.synchronize() diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index a77a9bdcb3..1ea0b69519 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -20,6 +20,7 @@ import gc import queue import threading +import weakref from pathlib import Path @@ -601,10 +602,7 @@ def test_deferred_destroy_prepares_backend_before_releasing_world( class CyclicView: def __init__(self) -> None: - self.cycle = self - - def __del__(self) -> None: - events.append("view_release") + self.retained_view = self sim = object.__new__(SimulationManager) spawn_scene = MagicMock() @@ -614,9 +612,14 @@ def __del__(self) -> None: ) sim._gizmos = {} sim._markers = {} + view = CyclicView() + released = weakref.finalize(view, events.append, "view_released") sim._rigid_objects = { - "body": SimpleNamespace(_data=CyclicView(), _entities=[], _spawn_result=None) + "asset": SimpleNamespace( + _data=view, _entities=[view], _spawn_result=spawn_scene + ) } + del view sim._constraints = {} sim._rigid_object_groups = {} sim._fluid_objects = {} @@ -658,9 +661,12 @@ def collect_views() -> int: sim._deferred_destroy() + # Clearing a facade can make its cyclic native views collectible. Their + # finalizers must run before the owning native Scene/World are closed. + assert not released.alive + assert events.index("backend_prepare") < events.index("view_released") + assert events.index("view_released") < events.index("spawn_close") assert events.index("backend_prepare") < events.index("gc_collect") - assert "view_release" in events - assert events.index("view_release") < events.index("spawn_close") assert events.index("backend_prepare") < events.index("spawn_close") assert events.index("backend_prepare") < events.index("world_quit") assert instances == ({} if owns_registry_entry else {0: replacement}) diff --git a/tests/toolkits/test_fixed_gripper_geometry.py b/tests/toolkits/test_fixed_gripper_geometry.py index 27f1d5b1c4..260da1014e 100644 --- a/tests/toolkits/test_fixed_gripper_geometry.py +++ b/tests/toolkits/test_fixed_gripper_geometry.py @@ -232,3 +232,64 @@ def test_authored_collision_box_preserves_physical_pad_dimensions( assert geometry.provenance["parts"][0]["geometry_kind"] == "urdf_box" assert geometry.provenance["parts"][0]["mesh_path"] is None assert len(geometry.provenance["files_sha256"]) == 1 + + +@pytest.mark.gpu +def test_native_cpu_chunks_keep_all_samples_and_owned_results_across_streams() -> None: + import warp as wp + + wp.init() + from types import SimpleNamespace + from embodichain.toolkits.graspkit.pg_grasp.collision_checker import ( + ConvexCollisionChecker, + ) + + convex = object.__new__(ConvexCollisionChecker) + convex.device = torch.device("cpu") + normals = torch.cat((torch.eye(3), -torch.eye(3))) + convex.plane_equations = { + "plane_equations": torch.cat((normals, torch.full((6, 1), -0.1)), dim=1)[None], + "plane_equation_counts": torch.tensor([6], dtype=torch.int32), + } + checker = object.__new__(GripperCollisionChecker) + checker.device = torch.device("cpu") + checker._checker = convex + checker.obj_mesh_verts = torch.zeros(1, 3, dtype=torch.float64) + x = torch.linspace(-0.3, 0.3, 1025) + checker.surface_points = torch.stack((x, x.flip(0), x * 0.5), dim=-1) + poses = torch.eye(4).repeat(258, 1, 1)[::2] + poses[:, 0, 3] = torch.linspace(0, 0.5, len(poses)) + stream = torch.cuda.Stream() + for ground in (False, True): + for threshold in (0.0, -0.02, 0.02): + checker._checker = SimpleNamespace( + query_batch_points=convex.query_batch_points + ) + expected = checker.query( + torch.eye(4), + poses, + collision_threshold=threshold, + is_filter_ground_collision=ground, + ) + checker._checker = convex + with torch.cuda.stream(stream): + actual = checker.query( + torch.eye(4), + poses, + collision_threshold=threshold, + is_filter_ground_collision=ground, + ) + for value, reference in zip(actual, expected): + assert value.device.type == "cpu" + torch.testing.assert_close(value, reference, rtol=0, atol=0) + # Each query observes new geometry, and later scratch reuse cannot alter + # already returned results. Expand the box to encompass a distant pose. + old = checker.query(torch.eye(4), poses, is_filter_ground_collision=False) + saved = tuple(value.clone() for value in old) + convex.plane_equations["plane_equations"][..., 3] = -1.0 + changed = checker.query(torch.eye(4), poses, is_filter_ground_collision=False) + assert changed[0].all() and not old[0].all() + for value, reference in zip(old, saved): + torch.testing.assert_close(value, reference, rtol=0, atol=0) + empty = checker.query(torch.eye(4), poses[:0], is_filter_ground_collision=False) + assert empty[0].numel() == empty[1].numel() == 0 diff --git a/tests/utils/test_nms_scope.py b/tests/utils/test_nms_scope.py new file mode 100644 index 0000000000..412b75cbef --- /dev/null +++ b/tests/utils/test_nms_scope.py @@ -0,0 +1,52 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from concurrent.futures import ThreadPoolExecutor +from contextvars import copy_context +import pytest +import torch +from embodichain.utils.nms import pose_nms, pose_nms_scope + + +def test_nms_memory_scope_is_owned_and_preserves_selection(): + def current(): + with pose_nms_scope() as size: + return size + + poses = torch.eye(4).repeat(8, 1, 1) + poses[:, 0, 3] = torch.tensor([0.0, 0.001, 0.002, 0.01, 0.012, 0.02, 0.021, 0.03]) + expected = pose_nms(poses, chunk_size=2) + with pose_nms_scope(chunk_size=2): + inherited = copy_context() + with pytest.raises(RuntimeError): + with pose_nms_scope(chunk_size=3): + assert current() == 3 + raise RuntimeError("scope interrupted") + assert current() == 2 + actual = pose_nms(poses) + assert torch.equal(actual[0], expected[0]) and torch.equal( + actual[1], expected[1] + ) + explicit = pose_nms(poses, chunk_size=1) + assert torch.equal(explicit[1], expected[1]) + with ThreadPoolExecutor(max_workers=1) as pool: + assert pool.submit(current).result() == 2048 + assert current() == 2048 and inherited.run(current) == 2 + with pytest.raises(ValueError): + with pose_nms_scope(chunk_size=0): + pass